S-PTAM 리뷰 - 코드(1) : Visual Odometry 1

S-PTAM에서 사용하는, 스테레오 카메라를 이용한 visual odometry pipeline에 대해 지난 3개의 포스트에서 알아보았습니다. 이번 포스트에서는 S-PTAM 코드에서 visual odometry에 관련된 부분을 살펴보며, 그러한 논리적 흐름과 수학적 방법론들이 코드로는 어떻게 구현되어 있는지 구경해 보도록 하겠습니다.

 

일단 이 글에서 설명한 대로 코드를 내려받았다고 가정합니다. 이번 포스트와 함께 열어놓고 참조하며 읽는 것이 도움이 될 것이라고 생각합니다.

 

처음에는 코드를 line-by-line으로 설명해 볼까 시도도 했는데, 함수 안에 함수 안에 함수가 있는 식으로 복잡한 코드인데다 데이터를 여기저기서 상호참조해 오기 때문에 흐름을 따라가기가 쉽지 않습니다. 따라서 모든 함수를 설명하지 않고, 주요 함수가 어떤 역할을 수행하는지에 대해 정리하며 진행하도록 하겠습니다.

 

순차적인 흐름을 파악하는 것이 목적이므로, 코드에 쓰여진 순서가 아닌 실행되는 순서를 따라가 보겠습니다. 일단 메인 파일인 sptam.py의 main 함수입니다.

 

if __name__ == '__main__':
    import cv2
    import g2o

    import argparse

    from threading import Thread
    
    from components import Camera
    from components import StereoFrame
    from feature import ImageFeature
    from params import ParamsKITTI, ParamsEuroc
    from dataset import KITTIOdometry, EuRoCDataset
    

    parser = argparse.ArgumentParser()
    parser.add_argument('--no-viz', action='store_true', help='do not visualize')
    parser.add_argument('--dataset', type=str, help='dataset (KITTI/EuRoC)', 
        default='KITTI')
    parser.add_argument('--path', type=str, help='dataset path', 
        default='path/to/your/KITTI_odometry/sequences/00')
    args = parser.parse_args()

    if args.dataset.lower() == 'kitti':
        params = ParamsKITTI()
        dataset = KITTIOdometry(args.path)
    elif args.dataset.lower() == 'euroc':
        params = ParamsEuroc()
        dataset = EuRoCDataset(args.path)

    sptam = SPTAM(params)

    visualize = not args.no_viz
    if visualize:
        from viewer import MapViewer
        viewer = MapViewer(sptam, params)


    cam = Camera(
        dataset.cam.fx, dataset.cam.fy, dataset.cam.cx, dataset.cam.cy, 
        dataset.cam.width, dataset.cam.height, 
        params.frustum_near, params.frustum_far, 
        dataset.cam.baseline)

 

필요한 모듈과 클래스들을 import해 오고, argparse를 사용해 사용할 dataset의 종류를 사용자로부터 입력받은 후 각 데이터셋에 필요한 파라미터들을 설정합니다. cam 객체가 카메라 파라미터들을 담고 있습니다.

 

    durations = []
    for i in range(len(dataset)):
        featurel = ImageFeature(dataset.left[i], params)
        featurer = ImageFeature(dataset.right[i], params)
        timestamp = dataset.timestamps[i]

 

왼쪽/오른쪽 카메라 이미지를 받아 featurel과 featurer 객체를 생성합니다. 이 객체들은 feature.py에 있는 ImageFeature 클래스의 객체로써, 내부에 feature extraction과 matching 등을 위한 여러 함수가 구현되어 있습니다. timestamp객체는 stereo frame이 생성된 시각을 담고 있습니다.

 

        time_start = time.time()  

        t = Thread(target=featurer.extract_feature_points)
        t.start()
        featurel.extract_feature_points()
        t.join()

 

프로세싱이 시작된 시각을 time_start에 저장해 두고, featurel과 featurer 객체의 extract_feature_points 함수를 이용하여 왼쪽/오른쪽 이미지에서 feature들을 추출합니다. 이 keypoints와 descriptors들은 ImageFeature 객체 내부의 self.keypoints와 self.descriptors 리스트에 저장됩니다.

 

        frame = StereoFrame(i, g2o.Isometry3d(), featurel, featurer, cam, timestamp=timestamp)

 

이제 featurel과 featurer 객체를 포함하는 StereoFrame클래스의 객체를 만듭니다. 이 클래스는 components.py에 존재하는 클래스로, 최대한 쉽게 설명하자면 하나의 stereo frame에 대한 모든 정보와 기능을 가지고 있는 객체를 생성하는 역할을 맡습니다. 왼쪽/오른쪽 2D image, feature들과 그 descriptor들, frame의 고유한 index, pose정보, timestamp등을 저장하고 있으며, 내부에는 3D-2D point correspondency를 계산하고 삼각측량을 수행하는 등 visual odometry의 핵심 기능들을 수행하는 함수들이 구현되어 있습니다. 이 함수들을 지금 자세히 설명하려면 너무 길어지고, 나중에 호출될 때 구경해 보도록 하겠습니다.

 

        if not sptam.is_initialized():
            sptam.initialize(frame)

 

이제 첫 번째 stereo frame이 생성되었으니, visual odometry 이론편 3번째에서 설명한 대로 initialization을 수행하러 갑니다. sptam은 위에서 생성된 SPTAM 클래스(sptam.py에 있습니다)의 객체로, 내부에 initialization 수행을 위한 initialize 함수가 구현되어 있습니다. 해당 클래스로 이동해서 함수를 살펴보겠습니다.

 

# main class
class SPTAM(object):
    def __init__(self, params):
        # slam 구동에 필요한 파라미터
        self.params = params

        # tracker
        self.tracker = Tracker(params)
        # 매 frame마다의 위치 변화를 대강 계산하기 위한 motion model
        # pnp의 수렴 속도를 빠르게 하고 더 나은 수렴성을 보장하기 위해서는 local minimum과 가까운 곳에서 최적화를 시작할 필요가 있음
        # 즉 이전 frame의 위치를 initial guess로 놓고 iterative한 연산을 시작하기보다는,
        # 부정확하더라도 대강의 위치 예측을 수행해 true pose에 조금이라도 가까운 점에서 연산을 시작하는 것이 나음
        # 직전의 속도와 각속도를 이용해 pose를 예측함
        self.motion_model = MotionModel()

        # graph(논문에서의 map에 해당 - keyframe과 mappoint등을 담고 있는 지도 자체에 해당함)
        self.graph = CovisibilityGraph()
        # mapping 작업을 수행하기 위한 객체
        # 이 객체가 생성됨과 동시에 백그라운드에서 별도의 thread로 bundle adjustment 작업이 지속적으로 수행됨 - mapping.py 참조
        self.mapping = MappingThread(self.graph, params)

        # loop closing 작업을 수행하기 위한 객체
        # 이 객체가 생성됨과 동시에 백그라운드에서 별도의 thread로 loop closing 작업이 지속적으로 수행됨 - loopclosing.py 참조
        self.loop_closing = LoopClosing(self, params)
        # loop closing이 수행되었을 때 keyframe과 mappoint들을 조정하기 위한 보정 데이터를 담는 변수
        # Rotation, translation이라고 생각하면 됨
        self.loop_correction = None
        
        # reference keyframe - 직전 keyframe과 같을 수도 다를 수도 있음
        # 쉽게 말하면, 매 frame마다의 pose 역추정을 위해 필요한 3d point(mappoint)들이 가장 많이 들어 있는 keyframe이라고 할 수 있음
        self.reference = None
        # 직전 keyframe
        self.preceding = None
        # 현재 keyframe
        self.current = None
        # slam 알고리즘의 각종 작동 상태를 기록하는 변수
        self.status = defaultdict(bool)
        
    # slam 구동 중지
    def stop(self):
        self.mapping.stop()
        if self.loop_closing is not None:
            self.loop_closing.stop()

    # 첫 번째 frame에서만 구동되는 initialization 과정
    def initialize(self, frame):
        # mappoint는 3d point + descriptor
        # measurement는 위의 3d point와 매칭되는 2d point + 각종 정보
        # 삼각측량을 이용해 최초의 mappoint들을 생성해냄
        mappoints, measurements = frame.triangulate_and_create_mappoints()

 

위쪽은 SPTAM 클래스의 객체 생성에 관련된 부분들로, 여러 가지 계산을 수행하는 모듈들로 보시면 됩니다. Visual odometry에서 중요한 부분은 Tracker와 MotionModel 객체로, 각각 $ (R, T) $ 계산과 $ (R, T) $의 initial estimate 생성 부분을 담당합니다.

 

initialization 과정은 initialize 함수로 구현되어 있습니다. StereoFrame 객체(즉 위에서 생성된 frame 객체)를 입력받아, 일단 그 객체의 메소드인 triangulate_and_create_mappoints()를 구동합니다. 조금 복잡해지지만, components.py의 StereoFrame 클래스로 이동해 해당 함수를 살펴보겠습니다.

 

    def triangulate_and_create_mappoints(self):
        # 왼쪽 오른쪽에서 unmatched상태인 keypoint, descriptor, index들을 데려옴
        kps_left, desps_left, idx_left = self.left.get_unmatched_keypoints()
        kps_right, desps_right, idx_right = self.right.get_unmatched_keypoints()

        # mappoints : MapPoint의 리스트
        # matches : 매칭된 index들의 list of tuples(왼쪽/오른쪽)
        mappoints, matches = self.triangulate_from_LR_kpts_and_dsts(
            kps_left, desps_left, kps_right, desps_right)

        measurements = []
        for mappoint, (i, j) in zip(mappoints, matches):
            meas = Measurement(
                Measurement.Type.STEREO,
                Measurement.Source.TRIANGULATION,
                [kps_left[i], kps_right[j]],
                [desps_left[i], desps_right[j]])
            meas.mappoint = mappoint
            # meas.view는 camera cs 기준 3차원 좌표
            meas.view = self.transform_wcs_to_ccs(mappoint.position)
            measurements.append(meas)

            self.left.set_matched(idx_left[i])
            self.right.set_matched(idx_right[j])

        return mappoints, measurements

 

일단 get_unmatched_keypoints 함수를 이용해 왼쪽/오른쪽 이미지에서 3D-2D matching이 수행되지 않은 feature들을 가져옵니다(최초의 프레임이므로 당연히 매칭된 적이 없고, 모든 점들을 갖고 오게 됩니다). Keypoints와 그 descriptors, 그리고 각 point의 index들을 가지고 옵니다.

 

그 후는 triangulate_from_LR_kpts_and_dsts 함수를 이용해서 3차원 MapPoint들을 생성합니다. 이 함수 내에도 또다른 함수가 사용되기 때문에 순차적으로 설명하려면 너무 복잡하고, 결과만 말하자면 이 함수는 MapPoint들과 그 descriptor들을 담고 있는 MapPoint객체의 리스트인 mappoints와 왼쪽/오른쪽 이미지에서 매칭된 feature들의 index 쌍의 리스트인 matches를 반환합니다. 

 

그 후에는 이 mappoints와 matches를 사용해서 각각의 MapPoint에 대해 Measurement 객체를 생성합니다. Measurement 객체는 이 글에서 설명했듯 하나의 MapPoint에 대한 모든 정보를 담고 있는 객체라고 할 수 있습니다. 나중에 어디서든 MapPoint들에 대한 정보가 필요하다면, 이 객체에 들어 있는 정보를 참조하면 됩니다. 이 Measurement들을 모아서 measurements 리스트로 반환합니다(MapPoint들의 리스트인 mappoints도 같이 반환합니다). 이제 다시 sptam.py의 initialize 함수로 돌아가겠습니다.

 

        # mappoint가 너무 적으면 오류 발생
        assert len(mappoints) >= self.params.init_min_points, (
            'Not enough points to initialize map.')

        # 최초의 frame은 keyframe으로 설정
        keyframe = frame.to_keyframe()
        # 최초의 frame은 원점으로 사용되기 때문에 BA나 loop closure때 움직여서는 안됨.
        # 고정된 frame임을 표시
        keyframe.set_fixed(True)
        # map에 keyframe을 추가
        self.graph.add_keyframe(keyframe)

 

생성된 MapPoint들의 개수가 너무 적으면 에러를 발생시키는 부분이 있고, 그 다음은 최초의 keyframe을 생성하는 부분입니다. 이 글에서 언급했듯 최초의 stereo frame은 keyframe으로 선정됩니다. 또한 최초의 위치는 무조건 원점이기 때문에 그래프 최적화나 bundle adjustment 수행 시에도 움직이면 안 되므로, set_fixed 함수로 위치를 고정시킵니다. 이러면 loop closure가 일어나거나 bundle adjustment가 수행되어도 이 keyframe의 위치는 변하지 않습니다. 그 다음 keyframe을 graph에 추가합니다(S-PTAM에서 graph와 map은 같은 거라고 생각하셔도 됩니다).

 

        # map에 mappoint와 각종 측정치를 추가
        # 복잡하므로 mapping.py 참조
        # 설명 :
            # map 에 키프레임, mappoints, measurements를 추가한 다음
            # keyframe, mappoint, measurement가 서로를 상호참조할 수 있도록 서로의 데이터를 서로 안에 넣어둠
            # 예를 들어 하나의 mappoint가 어느 keyframe에서 관측되었는지 알 필요가 있을 때... 등등
            # 1. 일단 graph에 mappoint 하나를 추가
            # 2. 그다음에 graph내부의 add_measurement_and_do_covisibility_tasks 함수를 사용해서 measurement 하나에 keyframe과 mappoint 정보를 추가함
            # 3. 그리고 keyframe과 mappoint에 각각 measurement를 append 및 추가함(keyframe 하나에는 meas 여러개가 들어가지만 mappoint에는 상응하는 meas 하나만 들어감)
            # 4. mappoint의 measurement count를 1 증가시킴(관측된 횟수를 의미)
            # keyframe, mappoint, measurement들이 서로를 상호 참조하는 형태
            # 하여튼 이걸 mappoints와 measurements 내부의 모든 mappoint와 measurement에 대해서 수행
        self.mapping.add_mappoints_and_handle_covisibility(keyframe, mappoints, measurements)
        # loop closure용으로 키프레임을 저장해둠
        if self.loop_closing is not None:
            self.loop_closing.add_keyframe(keyframe)

 

다음에는 add_mappoints_and_handle_covisibility라는 함수가 나오는데, 이게 좀 복잡합니다. 일단 함수 자체는 mapping.py의 Mapping 클래스 안에 구현되어 있습니다. 이 함수 안에도 또 다른 함수가 있는데, 이 안에도 또 다른 함수가 있고... 인셉션 영화 같습니다. 그렇기 때문에 이 함수만의 역할을 설명하도록 하겠습니다.

 

이 함수의 주 기능은 현재 keyframe과 mappoints, measurements를 받아 graph(map)에 MapPoint들과 Measurement들을 추가하는 것입니다. 거기에 더해 covisibility 관련 작업을 수행하는데, covisibility 작업이란 서로 다른 키프레임 사이에서 공통적으로 관측된 MapPoint들을 처리하는 작업입니다. 예를 들어, 어떤 MapPoint가 7번째 keyframe에서 처음 생성되었는데, 8번째 keyframe에서도 관측되었다고 생각해 봅시다. 이는 keyframe 사이를 이어 주는 중요한 구속 조건으로, 나중에 그래프 최적화 등에서 쓰일 수 있습니다. 그렇기에 일단 measurement에 keyframe과 MapPoint 데이터를 넣어 주고, 이 measurement를 다시 keyframe과 MapPoint에 추가하는 등의 covisibility 관련 작업을 수행합니다. 이 작업이 끝난 후 정리를 해 보면 다음과 같습니다.

• 7번째 keyframe의 covisibility keyframe에 8번째 keyframe이 등록됩니다.
• 8번째 keyframe의 covisibility keyframe에도 7번째 keyframe이 등록됩니다.
• measurement 객체(현재 8번째 keyframe에서 생성된 객체)에 공통적으로 관측된 MapPoint와 현재 keyframe이 등록됩니다.
• 현재 8번째 keyframe에 이 measurement가 등록됩니다.
• 공통적으로 관측된 MapPoint에도 이 measurement가 등록됩니다.

즉 하나의 Measurement 객체에는 그 객체가 생성되었을 때의 keyframe과 MapPoint만이 들어가 있지만, keyframe과 MapPoint는 여러 개의 Measurement를 가지고 있을 수 있습니다. 즉 keyframe 사이의 구속 조건(연관성)이 표현된 것이라고 볼 수 있습니다. 직관적으로 잘 와닿지 않을 수도 있는데, 구체적으로 알고 싶으시다면 직접 코드를 파헤쳐 보시는 것이 좋겠습니다. 제가 설명하려면 너무 장황해질 것 같습니다.

 

하여튼 이 함수가 수행된 후에는 self.loop_closing.add_keyframe 함수를 통해 loop closure 담당 모듈에 keyframe을 추가해 둡니다. Loop closing에 대해서는 추후에 설명하겠습니다.

 

        # 최초의 keyframe이므로 reference, preceding, current keyframe은 모두 현재 keyframe으로 설정
        self.reference = keyframe
        self.preceding = keyframe
        self.current = keyframe
        # initialization이 마무리되었음을 표시
        self.status['initialized'] = True

        # 대강의 pose estimation을 위한 motion model도 update(최초위치 [0, 0, 0])
        self.motion_model.update_pose(
            frame.timestamp, frame.position, frame.orientation)

 

그 다음에는 reference/preceding/current keyframe을 모두 현재 keyframe으로 설정합니다(최초의 keyframe이므로). Preceding keyframe은 현재 keyframe 이전의 keyframe, current keyframe은 가장 최신의 keyframe입니다. Reference keyframe은 복잡한 개념이기 때문에 다음 편에서 설명하겠습니다.

 

그 후 initialization이 끝났음을 내부 변수에 표시해 두고, motion model(decaying velocity model)에 현재 시각의 위치를 원점으로 설정합니다.

 

자, 여기까지가 첫 번째 stereo frame을 받아 initialization을 수행하는 과정입니다. 다음 포스트에는 새로운 keyframe이 들어올 때마다 visual odometry를 수행하는 과정을 살펴보겠습니다.