[Android] Android bluetooth connect and receive data(안드로이드 블루투스 연결 및 데이터 수신)

How to connect bluetooth device and receive data in android device


이 글은 일반적으로 많이 사용되는 HC-06 블루투스 모듈과 통신하기 위한 방법을 설명한다. HC-06은 블루투스 2.0을 이용하며 최근 많이 사용되고 있는 저전력 블루투스 모듈인 BLE와는 다르다.

1. 블루투스 관련 권한 설정

안드로이드 디바이스에서 블루투스 관련 기능을 사용하기 위해서는 블루투스 관련 권한 설정을 해야한다.

<uses-permission android:name="android.permission.BLUETOOTH_ADMIN" />
<uses-permission android:name="android.permission.BLUETOOTH" />

안드로이드 디바이스에서 블루투스 권한을 얻기 위해서는 AndroidManifest.xml 파일에 위의 코드 두줄을 추가해 줘야 한다.

이 글에서는 다루지 않고 따로 정리하겠지만 블루투스 연결뿐만 아니라 주변에 있는 블루투스 기기 검색을 위해서는 다음과 같이 ACCESS_COARSE_LOCATIONACCESS_FINE_LOCATION 에 대한 권한도 필요하다.

<uses-permission android:name="android.permission.ACCESS_COARSE_LOCATION" />
<uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />

2. 블루투스 활성화 및 연결

블루투스 연결 관련 함수들을 class 로 작성해서 사용하는 것이 좋은 방법이며 다음과 같은 맴버 변수를 선언하였다.

private static final int REQUEST_ENABLE_BT = 3;
public BluetoothAdapter mBluetoothAdapter = null;
Set<BluetoothDevice> mDevices;
int mPairedDeviceCount;
BluetoothDevice mRemoteDevice;
BluetoothSocket mSocket;
InputStream mInputStream;
OutputStream mOutputStream;
Thread mWorkerThread;
int readBufferPositon;      //버퍼 내 수신 문자 저장 위치
byte[] readBuffer;      //수신 버퍼
byte mDelimiter = 10;

가장 먼저 사용하는 기기가 블루투스를 지원하는지, 지원한다면 블루투스가 켜져있는지 확인해야 한다.

public void CheckBluetooth(){

    mBluetoothAdapter = BluetoothAdapter.getDefaultAdapter();
	if (mBluetoothAdapter == null) {
		// 장치가 블루투스 지원하지 않는 경우
		Toast.makeText(activity, "Bluetooth no available", Toast.LENGTH_SHORT).show();
	} else {
		// 장치가 블루투스 지원하는 경우
		if (!mBluetoothAdapter.isEnabled()) {
			// 블루투스를 지원하지만 비활성 상태인 경우
			// 블루투스를 활성 상태로 바꾸기 위해 사용자 동의 요첨
			Intent enableBtIntent = new Intent( BluetoothAdapter.ACTION_REQUEST_ENABLE);
			activity.startActivityForResult(enableBtIntent, REQUEST_ENABLE_BT);
		} else {
			// 블루투스를 지원하며 활성 상태인 경우
			// 페어링된 기기 목록을 보여주고 연결할 장치를 선택.
			selectDevice();
		}
	}
}

이상이 없는 경우에 pairing 목록에서 연결하고자 하는 기기를 검색해서 사용자에게 선택하도록 한다. 이때 bluetooth 모듈에 연결하기 위해서는 기기와 paring 한 기록이 있어야 한다. paring이 되어 있다는 의미는 기기의 정보가 핸드폰에 등록이 되어 있으며 서로 존재를 알고 있다는 의미이다. pairing은 핸드폰의 블루투스 설정에서 기기 검색을 통해 가능하다. (코드를 통해서 pairing 과정을 수행할 수도 있다. 하지만 이 글에서는 페어링이 되어 있는 기기에 연결하는 과정만을 설명한다)

selectDevice() 함수는 현재 핸드폰에 등록되어 있는 pairing목록을 화면의 띄워서 사용자에게 연결하고자 하는 기기를 선택할 수 있도록 화면을 띄워준다.

private void selectDevice() {
    //페어링되었던 기기 목록 획득
    mDevices = mBluetoothAdapter.getBondedDevices();
    //페어링되었던 기기 갯수
    mPairedDeviceCount = mDevices.size();
    //Alertdialog 생성(activity에는 context입력)
    AlertDialog.Builder builder = new AlertDialog.Builder(activity);
    //AlertDialog 제목 설정
    builder.setTitle("Select device");


    // 페어링 된 블루투스 장치의 이름 목록 작성
    final List<String> listItems = new ArrayList<String>();
    for (BluetoothDevice device : mDevices) {
        listItems.add(device.getName());
    }
    if(listItems.size() == 0){
        //no bonded device => searching
        Log.d("Bluetooth", "No bonded device");
    }else{
        Log.d("Bluetooth", "Find bonded device");
        // 취소 항목 추가
        listItems.add("Cancel");

        final CharSequence[] items = listItems.toArray(new CharSequence[listItems.size()]);
        builder.setItems(items, new DialogInterface.OnClickListener() {
            //각 아이템의 click에 따른 listener를 설정
            @Override
            public void onClick(DialogInterface dialog, int which) {
                Dialog dialog_ = (Dialog) dialog;
                // 연결할 장치를 선택하지 않고 '취소'를 누른 경우
                if (which == listItems.size()-1) {
                    Toast.makeText(dialog_.getContext(), "Choose cancel", Toast.LENGTH_SHORT).show();
                
                } else {
                    //취소가 아닌 디바이스를 선택한 경우 해당 기기에 연결
                    connectToSelectedDevice(items[which].toString());
                }

            }
        });

        builder.setCancelable(false);    // 뒤로 가기 버튼 사용 금지
        AlertDialog alert = builder.create();
        alert.show();   //alert 시작
    }
}

connectToSelectedDevice() 함수는 선택된 기기로 연결하는 과정을 시작하는 함수이다.

private void connectToSelectedDevice(final String selectedDeviceName) {
    //블루투스 기기에 연결하는 과정이 시간이 걸리기 때문에 그냥 함수로 수행을 하면 GUI에 영향을 미친다
    //따라서 연결 과정을 thread로 수행하고 thread의 수행 결과를 받아 다음 과정으로 넘어간다.
    
    //handler는 thread에서 던지는 메세지를 보고 다음 동작을 수행시킨다.
    final Handler mHandler = new Handler() {
        public void handleMessage(Message msg) {
            if (msg.what == 1) // 연결 완료
            {
                try {
                    //연결이 완료되면 소켓에서 outstream과 inputstream을 얻는다. 블루투스를 통해
                    //데이터를 주고 받는 통로가 된다.
                    mOutputStream = mSocket.getOutputStream();
                    mInputStream = mSocket.getInputStream();
                    // 데이터 수신 준비
                    beginListenForData();
                    
                } catch (IOException e) {
                    e.printStackTrace();
                }
            } else {    //연결 실패
                Toast.makeText(activity,"Please check the device", Toast.LENGTH_SHORT).show();
                try {
                    mSocket.close();
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }
        }
    };

    //연결과정을 수행할 thread 생성
    Thread thread = new Thread(new Runnable() {
        public void run() {
            //선택된 기기의 이름을 갖는 bluetooth device의 object
            mRemoteDevice = getDeviceFromBondedList(selectedDeviceName);
            UUID uuid = UUID.fromString("00001101-0000-1000-8000-00805f9b34fb");

            try {
                // 소켓 생성
                mSocket = mRemoteDevice.createRfcommSocketToServiceRecord(uuid);
                // RFCOMM 채널을 통한 연결, socket에 connect하는데 시간이 걸린다. 따라서 ui에 영향을 주지 않기 위해서는
                // Thread로 연결 과정을 수행해야 한다.
                mSocket.connect();
                mHandler.sendEmptyMessage(1);
            } catch (Exception e) {
                // 블루투스 연결 중 오류 발생
                mHandler.sendEmptyMessage(-1);
            }
        }
    });
    
    //연결 thread를 수행한다
    thread.start();
}

//기기에 저장되어 있는 해당 이름을 갖는 블루투스 디바이스의 bluetoothdevice 객채를 출력하는 함수
//bluetoothdevice객채는 기기의 맥주소뿐만 아니라 다양한 정보를 저장하고 있다.

BluetoothDevice getDeviceFromBondedList(String name) {
    BluetoothDevice selectedDevice = null;
    mDevices = mBluetoothAdapter.getBondedDevices();
    //pair 목록에서 해당 이름을 갖는 기기 검색, 찾으면 해당 device 출력
    for (BluetoothDevice device : mDevices) {
        if (name.equals(device.getName())) {
            selectedDevice = device;
            break;
        }
    }
    return selectedDevice;
}

socket에 연결이 완료되면 데이터를 수신할 thread (beginListenForDat()) 를 실행한다.

//블루투스 데이터 수신 Listener
protected void beginListenForData() {
    final Handler handler = new Handler();
    readBuffer = new byte[1024];  //  수신 버퍼
    readBufferPositon = 0;        //   버퍼 내 수신 문자 저장 위치
    
    // 문자열 수신 쓰레드
    mWorkerThread = new Thread(new Runnable() {
        @Override
        public void run() {

            while (!Thread.currentThread().isInterrupted()) {

                try {

                    int bytesAvailable = mInputStream.available();
                    if (bytesAvailable > 0) { //데이터가 수신된 경우
                        byte[] packetBytes = new byte[bytesAvailable];
                        mInputStream.read(packetBytes);
                        for (int i = 0; i < bytesAvailable; i++) {
                            byte b = packetBytes[i];
                            if (b == mDelimiter) {
                                byte[] encodedBytes = new byte[readBufferPositon];
                                System.arraycopy(readBuffer, 0, encodedBytes, 0, encodedBytes.length);
                                final String data = new String(encodedBytes, "US-ASCII");
                                readBufferPositon = 0;
                                handler.post(new Runnable() {
                                    public void run() {
                                        //수신된 데이터는 data 변수에 string으로 저장!! 이 데이터를 이용하면 된다.
                                    
                                        char[] c_arr = data.toCharArray(); // char 배열로 변환
                                        if (c_arr[0] == 'a') {
                                            if (c_arr[1] == '1') {
                                            
                                            //a1이라는 데이터가 수신되었을 때
                                            
                                            }
                                            if (c_arr[1] == '2') {
                                            
                                            //a2라는 데이터가 수신 되었을 때
                                            }
                                        }
                                    }
                                });
                            } else {
                            readBuffer[readBufferPositon++] = b;
                            }
                        }
                    }
                } catch (UnsupportedEncodingException e) {
                    e.printStackTrace();
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }
        }
    });
    //데이터 수신 thread 시작
    mWorkerThread.start();
}

마지막으로 데이터를 전송하는 방법은 다음과 같다.

public void SendResetSignal(){
    String msg = "bs00000";
    try {
        mOutputStream.write(msg.getBytes());
    } catch (IOException e) {
        e.printStackTrace();
    }
}

이번 글에서는 pairing목록에서 기기 정보를 가져와서 블루투스 통신을 수행하는 방법에 대해서 다루었다. 다음에는 기기가 pairing목록에 없을 때

기기를 검색해서 pairing과정까지 한번에 수행하는 방법을 알아볼 것이다.


[Ubuntu] tensorflow 1.0 설치 (cuda, cudnn 호환, Ubuntu)

Tensorflow 1.0 설치 방법


Tensorflow 1.0 버전과 조합에 따른 설치 방법 정리

##### 파이썬 2.7
# Ubuntu/Linux 64-bit, CPU only, Python 2.7
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/linux/cpu/tensorflow-1.0.0rc0-cp27-none-linux_x86_64.whl

# Ubuntu/Linux 64-bit, GPU enabled, Python 2.7
# Requires CUDA toolkit 8.0 and CuDNN v5.
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/linux/gpu/tensorflow_gpu-1.0.0rc0-cp27-none-linux_x86_64.whl

# Mac OS X, CPU only, Python 2.7:
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/mac/cpu/tensorflow-1.0.0rc0-py2-none-any.whl

# Mac OS X, GPU enabled, Python 2.7:
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/mac/gpu/tensorflow_gpu-1.0.0rc0-py2-none-any.whl

##### 파이썬 3.x 
# Ubuntu/Linux 64-bit, CPU only, Python 3.3
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/linux/cpu/tensorflow-1.0.0rc0-cp33-cp33m-linux_x86_64.whl

# Ubuntu/Linux 64-bit, GPU enabled, Python 3.3
# Requires CUDA toolkit 8.0 and CuDNN v5.
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/linux/gpu/tensorflow_gpu-1.0.0rc0-cp33-cp33m-linux_x86_64.whl

# Ubuntu/Linux 64-bit, CPU only, Python 3.4
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/linux/cpu/tensorflow-1.0.0rc0-cp34-cp34m-linux_x86_64.whl

# Ubuntu/Linux 64-bit, GPU enabled, Python 3.4
# Requires CUDA toolkit 8.0 and CuDNN v5.
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/linux/gpu/tensorflow_gpu-1.0.0rc0-cp34-cp34m-linux_x86_64.whl

# Ubuntu/Linux 64-bit, CPU only, Python 3.5
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/linux/cpu/tensorflow-1.0.0rc0-cp35-cp35m-linux_x86_64.whl

# Ubuntu/Linux 64-bit, GPU enabled, Python 3.5
# Requires CUDA toolkit 8.0 and CuDNN v5.
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/linux/gpu/tensorflow_gpu-1.0.0rc0-cp35-cp35m-linux_x86_64.whl

# Ubuntu/Linux 64-bit, CPU only, Python 3.6
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/linux/cpu/tensorflow-1.0.0rc0-cp36-cp36m-linux_x86_64.whl

# Ubuntu/Linux 64-bit, GPU enabled, Python 3.6
# Requires CUDA toolkit 8.0 and CuDNN v5.
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/linux/gpu/tensorflow_gpu-1.0.0rc0-cp36-cp36m-linux_x86_64.whl

# Mac OS X, CPU only, Python 3.4 or 3.5:
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/mac/cpu/tensorflow-1.0.0rc0-py3-none-any.whl

# Mac OS X, GPU enabled, Python 3.4 or 3.5:
$ export TF_BINARY_URL=https://storage.googleapis.com/tensorflow/mac/gpu/tensorflow_gpu-1.0.0rc0-py3-none-any.whl

#### 설치
$ sudo pip install --upgrade $TF_BINARY_URL



[Ubuntu] Ubuntu 16.04.2 bluetooth load fail 문제

최신 intel wireless chipset을 사용하는 노트북에서 ubuntu 부팅시 bluetooth를 인식하지 못하는 문제 해결방법


인텔 최신 칩셋인

Intel® Dual Band Wireless-AC 8265

Intel® Dual Band Wireless-AC 8260

과 같은 무선칩셋을 사용하는 노트북의 경우 ubuntu 16.04가 부팅할 때 다음과 같은 오류 메세지를 띄우면서 부팅이 안되는 경우가 발생한다.

Intel 8265 Dual-band Wireless Bluetooth Drivers Failing to Load

위와 같은 최신 인텔 칩셋은 16.04.1에서는 지원하지 않으며 16.04.02의 경우 wifi는 잘 연결된다. 16.10은 문제없이 모두 지원하는 것 같으나 LTS버전이 아니기 때문에 본인은 16.04버전을 선호한다.

위와 같이 오류 메세지가 뜨면서 부팅이 안되는 경우 아래의 Ubuntu forum글을 참고하면된다.

참고 url

해결방법:

sudo apt-get install linux-generic-hwe-16.04-edge xserver-xorg-input-libinput-hwe-16.04 && wget http://mirrors.kernel.org/ubuntu/pool/main/l/linux-firmware/linux-firmware_1.161.1_all.deb && sudo dpkg -i linux-firmware_1.161.1_all.deb

위의 명령어를 sudo 권한으로 실행시킨다. 본인의 경우 dual 부팅을 사용하고 있는데 바로 ubuntu를 실행하는 경우에는 오류가 뜨고, 윈도우를 한번 실행시켰다가 다시 ubuntu로 부팅하는 경우는 정상적으로 부팅이 되는 상황이라 위의 해결방법으로 해결하였다.


[SLAM] Robust Graph SLAM

Outlier에 영향을 덜 받는 Robust Graph SLAM에 대해서 알아본다.


본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.

이번 글에서는 이전에 설명했던 Graph based SLAM이 outlier(잘못된 정보)에 Robust하게 만드는 방법에 대해서 설명한다.

Graph-based SLAM은 least square방법을 사용하여 로봇과 landmark의 위치를 최적화 시킨다. 즉, 현재의 graph상의 로봇의 위치에서 얻어질 것으로 예상되는 측정값과 실제 측정값과의 차이를 최소화 시키는 방향으로 graph가 최적화 된다.

만약 그래프가 최적화 되는 과정에서 아래 그림과 같이 잘못된 연결(edge)이 생성되면 어떻게 될까?

위 그림과 같이 전혀 다른 두 위치를 같은 위치로 인식하여 edge를 발생시켰을 경우 아래와 같이 그래프 전체에 왜곡이 발생하게 된다.

그리고 이렇게 잘못 발생된 edge가 많아질 수록 왜곡은 심해지게 된다. 이러한 문제는 다음과 같은 상황에서 주로 발생한다.

  1. 특징이 없는 환경(아무런 특징이 없는 복도 등)
  2. 같은 빌딩내의 비슷한 환경의 방
  3. GPS의 multi-path(GPS 신호가 다른 건물에 반사된 후 수신되어 오차가 발생하는 현상)

따라서 실제 graph-based SLAM을 수행할 때 위의 상황 뿐만아니라 다양한 상황에서 왜곡이 발생하게 된다. 이러한 잘못된 정보를 outlier라고 하며, 이러한 outlier에 덜 영향을 받는 최적화 방법이 필요하다. 이번 글에서는 최적화 과정을 Robust하게 만드는 방법에 대해서 설명하도록 한다.

Robust M-estimator

이전 글인 Least Square관련 글에서는 noise를 Gaussina 분포로 가정하고, least square방법으로 최적해를 구하는 방법에 대해서 설명하였다. 또한 least square와 Gaussian 분포의 관계에 대해서도 언급하였었다. Least square방법은 M-estimator의 한 종류이며, Gaussian noise를 가정한 model이다. M-estimator는 noise의 형태를 Gaussian으로 가정하지 않는다. M-estimator의 PDF(Probability density function)은 아래와 같이 정의된다. \[p(e)= exp(-\rho(e))\]

그렇다면 least square에서 최적해를 계산하는 것과 같이 negative log likelihood form으로 표기하면 PDF의 최대값을 찾는 문제는 log likelihood의 최소값을 찾는 문제가 된다. \[\mathbf{x}^* = argmin_{\mathbf{x}} \sum_i \rho(e_i(\mathbf{x}))\]

즉 위의 log likelihood식에서 $\rho(e)$가 $\rho(e) = e^2$ 처럼 제곱의 형태인 것이 Least square문제가 되는 것이다. 그렇다면 $\rho$ fucntion에는 어떤 종류들이 있는지 살펴보도록 하자.

위 그림은 각 함수의 형태를 보여준다. 최적화를 하기 위한 cost function의 형태에 변화를 줌으로써 outlier에 강인한 특성을 주기 위한 노력들이다. 이렇게 cost function의 형태를 바꿈으로써 outlier에 Robust한 특성을 어떻게 갖게 되는 것인지 살펴보자. 최적화는 error들의 합을 최소화 하는 방향으로 입력을 변화시키며 해를 찾아가는 방법이다. 이러한 과정에서 error가 매우 큰(Outlier라고 생각 할 수 있는) term들이 최적화 과정에서 큰 weight로 작용하게 된다(영향을 크게 준다). 따라서 위의 함수들은 ourlier라고 생각 할 수 있는, 즉 error가 매우 큰 term에 대해서 weight를 다소 줄이는 방향으로 cost function의 형태를 구성한 것이다. 다양한 형태의 function중에서 가장 많이 사용되는 형태는 Huber M-estimator이다.

앞으로 설명할 Max-mixture와 dynamic covariance scaling 방법도 m-estimator와 비슷한 개념을 이용하여 최적화의 robust함을 높이려는 방법이다.

Max-mixture

Max-mixture는 M-estimator와 마찬가지로 graph-based optimization을 outlier에 robust하게 만들기 위한 방법중에 하나이다. Max-mixture는 이름에서 알 수 있듯이 여러개의 Gaussian 분포를 합하는 과정에서 수학적인 이점을 얻기 위해서 각 Gaussian 분포의 최대값을 이용한다. 왜 이러한 방법을 사용하는지 아래 수식을 보도록 하자.

세상에 존재하는 다앙햔 분포를 1개의 Gaussian만을 이용하여 표현하기엔 부족한 경우가 많이 발생한다. 따라서 다양한 분포를 표현하기 위해서 여러개의 Gaussian을 더하는 형태로 분포를 표현할 수 있다. \[p(\mathbf{z}\mid\mathbf{x}) = \sum_k w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k\]

위의 분포를 최적화 시키기 위한 cost function을 얻기 위해 negative log likelihood을 취하면 다음과 같은 형태가 된다. \[-log p(\mathbf{z}\mid\mathbf{x}) = -log \sum_k w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k\]

1개의 Gaussian분포일 경우에는 sum($\sum$)이 없기 떄문에 log와 exponential이 계산되어 우리가 앞에서 계산한 cost function의 형태로 계산되어 최적화 과정을 진행할 수 있게 된다. 하지만 Gaussian mixture 모델의 경우 Sum의 형태이기 때문에 log가 sum의 안으로 들어갈 수 없다. 따라서 mixture 모델을 최적화 시키기 위해서 최대값을 이용하는 max-mixture approximation 방법을 이용한다. \[\begin{aligned} p(\mathbf{z}\mid\mathbf{x}) &= \sum_k w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k\\ &\approx max_k \ \ w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k \end{aligned}\]

즉 위의 근사화과정을 그림으로 표현하면 아래와 같다.

즉 두개의 분포의 값을 더하는 것 대신 최대값만을 추출하여 분포를 표현하는 것이다. 이는 두 분포의 평균값이 어느정도 거리로 떨어져 있을 경우에는 오차가 작지만 두 분포의 평균값이 매우 가까운 경우에는 큰 오차를 발생시킨다. 근사화된 식의 negative log likelihood는 다음과 같다. \[\begin{aligned} -log p(\mathbf{z}\mid\mathbf{x}) &= -log \ \ max_k \ \ w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k\\ &= min_k (\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k - log(w_k \eta_k) \end{aligned}\]

즉 max-mixture는 Gaussian 분포의 max값을 취하는 형태로 근사화 함으로써 우리가 풀 수 있는 최적화의 Cost function을 구할 수 있게 되었다.

위 그림에서 빨간색 그래프는 같은 mean값을 갖는(다른 variance) 두 gaussian의 합을 max-mixture방법을 통해 얻은 분포의 cost-function을 보여준다. 앞에서 설명한 M-estimator의 cost function들과 유사함을 알 수 있다. 따라서 여러개의 Gaussian 분포를 더함으로써 graph optimization과정을 Robust하게 만들 수 있다.

Dynamic Covariance Scaling(DCS)

Graph의 최적화를 Robust하게 만드는 다른 방법은 DCS(Dynamic Covariance Scaling)방법이다. DCS는 각 Error값에 해당하는 information matrix의 크기를 조절함으로써 robust하게 만든다. 원래 graph optimization식은 다음과 같다. \[\mathbf{x}^* = argmin_{\mathbf{x}} \sum_{ij} \mathbf{e}_{ij}(\mathbf{x})^T \Omega_{ij}\mathbf{e}_{ij}(\mathbf{x})\]

DCS는 cost function의 information matrix의 크기를 조절하는 scaling factor($s_{ij}^2$)를 추가한다. \[\mathbf{x}^* = argmin_{\mathbf{x}} \sum_{ij} \mathbf{e}_{ij}(\mathbf{x})^T (s_{ij}^2 \Omega_{ij})\mathbf{e}_{ij}(\mathbf{x})\]

scaling factor인 $s_{ij}^2$는 다음과 같이 정의된다. \[s_{ij} = min(1, \frac{2\Phi}{\Phi+\chi_{ij}^2})\]

즉 두개의 파라미터 $\Phi$와 $\chi$에 의해 scaling factor가 조절되며, 이 두 파라미터에 의해서 cost function이 변하게 된다.

위 그림은 DCS의 원래 cost function(검정색)과 scaling factor(파란색)그리고 최종 scaling이 된 cost function(빨간색)을 보여준다. Error가 적은 영역에서 scaling factor는 1이지만 error가 큰 영역으로 갈 수록 scaling factor가 작아지며, error가 큰 영역의 함수 출력값을 줄여준다. 이는 m-estimator가 error가 큰 영역의 값을 줄이는 것과 비슷한 효과로 볼 수 있다. 원래 DCS가 이러한 개념을 처음 사용한 것은 아니다. DCS가 나오기 전인 2012년 IROS에 error가 큰 term의 영향을 없애는 “Switchable Constraint for Robust Pose Graph SLAM”이 발표되었다. 이는 획기적으로 graph 최적화를 robust하게 만들었으나 과정이 매우 복잡한 단점이 있었다. 그 이후에 “Switchable Constraint” 논문을 위와같이 close form으로 정리하여 ICRA 2013년도에 발표한 논문이 “Robust Map Optimization using Dynamic Covariance Scaling”이다.

이 글에서 DCS에 대해서 너무 깊이 논하지는 않을 것이다. 조금 더 자세히 내용이 필요한 경우 위 논문을 찾아보기 바란다.

정리

이번 글에서는 graph-based SLAM을 outlier에 robust하게 만드는 방법에 대해서 살펴보았다. 이러한 목적으로 여러가지 방법이 사용되고 있으나 결과적으로는 최적화의 cost function에서 error가 큰 영역의 출력값을 줄여주는 방법이라는 점에서는 유사함을 알 수 있다.

본 글을 참조하실 때에는 출처 명시 부탁드립니다.


[SLAM] Graph-based SLAM with Landmark

Landmark가 있는 상황에서의 graph-based SLAM에 대해서 설명한다.


본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.

graph-based SLAM 포스트에서는 landmark가 없는 환경에서 로봇의 위치 간의 관계 만을 이용하여 graph를 최적화 시키는 방법에 대해서 설명하였다. 이번 글에서는 landmark가 있는 환경에서 이 landmark들을 이용하여 로봇 간의 위치 정보를 알 수 있을 때 graph-based SLAM이 어떻게 달라지는지 설명할 것이다.

Graph-based SLAM with Landmark

아래 그림은 Global 좌표계에서 로봇의 위치와 landmark의 위치를 보여주는 그림이다.

Landmark가 존재하는 실제 환경은 아래와 같다. 아래 그림과 같은 공원의 경우 공원에 있는 나무들이 landmark가 되고 나무들의 위치정보를 이용하여 SLAM을 적용한다.

로봇의 위치와 landmark와의 관계를 도식적으로 그려보면 다음과 같다.

이제 graph에서 node는 로봇의 위치뿐만 아니라 landmark의 위치도 포함하게 된다. 이때 landmark는 방향없이 x,y좌표로만 구성된다.

위의 정보들로 구성된 graph의 error를 최소화 하는 landmark와 로봇의 위치를 계산함으로써 로봇의 위치를 구할 수 있다. Landmark가 없는 환경에서 “virtual measurement”라는 것을 언급했었다. “virtual measurement”는 non-successive한 로봇의 위치에서 얻어진 센서 데이터를 이용하여 계산된 두 로봇간의 상대 위치(relative pose)를 의미한다. 이렇게 계산된 “virtual measurement”는 non-successive한 노드 사이의 constraint가 된다. Landmark가 있는 환경에서는 위 그림과 같이 non-succesive한 로봇간의 direct한 edge는 발생하지 않으며, landmark를 통해서만 연결된다.

Rank of Information Matrix

Landmark를 통한 graph SLAM의 경우 센서 특성에 따른 information matrix의 rank를 살펴보아야 한다. 여기서는 bearing only observation 센서모델로 로봇의 위치에서 landmark까지의 각도만을 측정할 수 있는 센서의 경우를 생각한다.

Bearing observation의 경우 observation function은 다음과 같다.

이 observation function을 이용한 error term은 다음과 같다.

위 error term으로 구성되는 information matrix의 rank는 어떻게 될까? \[\mathbf{H}_{ij} = \mathbf{J}_{ij}^T \mathbf{\Omega}_{ij} \mathbf{J}_{ij}\]

위 식에서 $\mathbf{J}{ij}$는 error term을 편미분한 행렬인데, bearing only 센서의 경우 error term의 dimension은 1이므로 Jacobian의 rank는 1이다. 따라서 $\mathbf{H}{ij}$의 rank도 1이 된다.

rank가 1이라는 것은 무엇을 의미할까? rank가 1이라는 것은 위 그림과 같이 어떠한 landmark가 있을 때 로봇은 x-y plane에 어디든 존재할 수 있으며 단지 그 x-y좌표에 해당하는 heading 각도만 한정된다는 의미이다. 따라서 bearing only sensor의 경우 로봇의 위치를 정확히 알기 위해서는 3개 이상의 observation이 필요하다. 이러한 시스템을 “under-determined” 시스템이라고 부른다.

따라서 Information matrix의 rank는 로봇의 위치 중(x,y,heading) 몇가지의 정보를 한정할 수 있는지를 의미하며, 로봇의 위치에 대한 unique solusion을 계산하기 위해서는 full lank가 되어야 한다.

Under-determined System

이러한 under-determined system, 즉 information matrix의 rank가 full lank가 아닌 경우 이러한 상황을 해결하기 위한 방법이 “Levenberge Marquardt” method 이다. 즉 “damping factor”를 더함으로써 full rank의 matrix를 만들어 unique한 해를 찾는다. 원래의 식은 아래와 같다. \[\mathbf{H} \triangle \mathbf{x} = -\mathbf{b}\]

damping factor가 추가된 식은 아래와 같다. \[(\mathbf{H} + \lambda \mathbf{I})\triangle \mathbf{x} = -\mathbf{b}\]

damping factor는 error의 증감에 따라 크기를 변화시킨다. 전체적인 알고리즘은 다음과 같다.

정리

이 글에서는 Landmark가 있는 환경에서의 Graph-based SLAM에 대해서 알아보았다. Landmark가 없는 환경에서는 센서 데이터를 이용하여 non-successive node간의 edge정보를 계산하였다. 이러한 edge는 node간의 직접적인 연결이다. 반면, landmark가 있는 환경에서는 각 node에서 바라보는 landmark의 위치가 observation 정보가 되며, non-successive한 node간의 연결은 landmark를 통해서 이루어진다. 이때 센서의 종류에 따라 information matrix의 rank가 결정되고 under-determined system이 되므로 이를 풀기위한 기법이 필요하게 된다(LM method).

본 글을 참조하실 때에는 출처 명시 부탁드립니다.


Pagination