SLAM 공부했다 - (2)

 코드로 넘어가기 전에 이전 내용을 복습하는 차원에서 matrix를 fill-in하는 실습을 다시 한 번 해보자. 문제를 풀면서 이런 생각이 들 수가 있다. "그럼 계속해서 움직이고, Landmark들이 계속 늘어나는 상황이면 행렬이 엄청나게 커질 텐데 프로세서가 버틸 수 있을까??" 그래서 이번에는 이에 대한 해결책을 제시한다. Online-SLAM!!! 

 

 상황에 따라 다르지만, 사실상 계속해서 motion/ measurement를 시행하면서 이 모든 data가 필요한 것은 아니다.

로봇에 달린 프로세서에게 부담을 주기도 싫고, 사실상 내가 필요한 것은 가장 최신의 정보들과만 관련이 있다!! 라고 한다면 이 Online SLAM이 적합하다. 

 Landmark들은 언제 또 지날지 모르니까 버릴 수 없고, 가장 최신의 위치 정보를 제외한 나머지는 버린다고 하자.

다시 최신의 위치 정보가 들어오면, 이 정보들이 들어올 자리를 만들어줘야 한다. 행렬의 차원을 키워서 중간에 삽입을 하기 때문에 이 과정을 expand라고 부른다. 지금까지 해왔던 것처럼 모든 element들과의 상호 관계를 고려해서 숫자들을 삽입하고,

이젠, 옛것이 되어 버린 바로 전 정보들을 버려줘야 하는데, 그냥 버리면 당연히 큰일난다. 위와 같이 4개의 행렬 조각들을 잘라낼 것이고, 이들을 순서대로 A B C D라고 부를 것이다. 그리고 잘라내고 남은 행렬들에 어떠한 연산을 해 줄 것이다. 이 연산을 통해서 이전 결과값들을 버리지 않고 반영할 수가 있다. 

 아래 보이는 파란 식의 연산을 해준다. 수학적으로 왜 이렇게 되는지 보이지 않아서 주입식 교육 같긴 하지만... 일단 이렇다!! 이제 코드를 보자! 기존의 엄청난 연산 필요했던 Graph SLAM은 아래와 같다.

 

def slam(data, N, num_landmarks, motion_noise, measurement_noise):
 
    # Set the dimension of the filter
    dim = 2 * (N + num_landmarks) 
 
    # make the constraint information matrix and vector
    Omega = matrix()
    Omega.zero(dim, dim)
    Omega.value[0][0] = 1.0
    Omega.value[1][1] = 1.0
 
    Xi = matrix()
    Xi.zero(dim, 1)
    Xi.value[0][0] = world_size / 2.0
    Xi.value[1][0] = world_size / 2.0
    
    # process the data
 
    for k in range(len(data)):
 
        # n is the index of the robot pose in the matrix/vector
        n = k * 2 
    
        measurement = data[k][0]
        motion      = data[k][1]
    
        # integrate the measurements
        for i in range(len(measurement)):
    
            # m is the index of the landmark coordinate in the matrix/vector
            m = 2 * (N + measurement[i][0])
    
            # update the information maxtrix/vector based on the measurement
            for b in range(2):
                Omega.value[n+b][n+b] +=  1.0 / measurement_noise
                Omega.value[m+b][m+b] +=  1.0 / measurement_noise
                Omega.value[n+b][m+b] += -1.0 / measurement_noise
                Omega.value[m+b][n+b] += -1.0 / measurement_noise
                Xi.value[n+b][0]      += -measurement[i][1+b] / measurement_noise
                Xi.value[m+b][0]      +=  measurement[i][1+b] / measurement_noise
 
 
        # update the information maxtrix/vector based on the robot motion
        for b in range(4):
            Omega.value[n+b][n+b] +=  1.0 / motion_noise
        for b in range(2):
            Omega.value[n+b  ][n+b+2] += -1.0 / motion_noise
            Omega.value[n+b+2][n+b  ] += -1.0 / motion_noise
            Xi.value[n+b  ][0]        += -motion[b] / motion_noise
            Xi.value[n+b+2][0]        +=  motion[b] / motion_noise
 
    # compute best estimate
    mu = Omega.inverse() * Xi
 
    # return the result
    return mu

 이번에는 연산의 양이 거의 지수적으로 절반이 된 Online SLAM 코드~

def online_slam(data, N, num_landmarks, motion_noise, measurement_noise):
    dim = 2 * (1 + num_landmarks)
 
    Omega = matrix()
    Omega.zero(dim, dim)
    Omega.value[0][0] = 1.0
    Omega.value[1][1] = 1.0
    
    Xi = matrix()
    Xi.zero(dim, 1)
    Xi.value[0][0] = world_size / 2.0
    Xi.value[1][0] = world_size / 2.0
 
    for k in range(len(data)):
        measurement = data[k][0]
        motion      = data[k][1]
        
        for i in range(len(measurement)):
            m = 2 * (1 + measurement[i][0])
 
            for b in range(2):
                Omega.value[b][b] += 1.0 / measurement_noise
                Omega.value[b][m+b] += -1.0 / measurement_noise
                Omega.value[m+b][b] += -1.0 / measurement_noise
                Omega.value[m+b][m+b] += 1.0 / measurement_noise
                Xi.value[b][0]      += -measurement[i][b+1] / measurement_noise
                Xi.value[m+b][0]      +=  measurement[i][b+1] / measurement_noise
 
 
        list = [0, 1] + [i for i in range(4, dim + 2)]
        Omega = Omega.expand(dim + 2, dim + 2, list, list)
        Xi = Xi.expand(dim + 2, 1, list, [0])
        
        for b in range(4):
            Omega.value[b][b] += 1.0 / motion_noise
        for b in range(2):
            Omega.value[b][b+2] += -1.0 / motion_noise
            Omega.value[b+2][b] += -1.0 / motion_noise
            Xi.value[b][0]           += -motion[b] / motion_noise
            Xi.value[b+2][0]         += motion[b] / motion_noise
 
        #print(len(Omega.value), Omega.dimx)
        newlist = [i for i in range(2, Omega.dimx)]
        A = Omega.take([0, 1], newlist)
        B = Omega.take([0, 1])
        C = Xi.take([0, 1], [0])       
 
        Omega = Omega.take(newlist) - (A.transpose() * B.inverse() * A)
        Xi = Xi.take(newlist, [0]) - (A.transpose() * B.inverse() * C)
 
    mu = Omega.inverse() * Xi
    return mu, Omega # make sure you return both of these matrices to be marked correct.

 이렇게 어영부영 SLAM 강좌가 끝이 났다. 사실 SLAM의 종류는 매우 많고 실질적으로 사용하려면 가장 중요한 visual-odometry라는 것을 공부해야 한다. 카메라와 IMU를 이용해서 SLAM을 하는 것이 가장 일반적이기 때문이다. 더불어 DOF(degree of freedom)을 고려해야 한다는데, 나는 아직 한참 멀었다. 후. 후.