Visual Odometry, RGB-D 3D Visual SLAM

[신민서]

[  WSL  ]  Ubuntu 24.04 Jazzy ROS2

Carla에서 Depth camera로 RTAB-MAP을 이용하여 Visual SLAM 수행

RTAB-Map as an Open-Source Lidar and Visual SLAM Library for Large-Scale and Long-Term Online Operation

---

▶ rtab-map visual odometry / RGB-D

그림 2는 RAB-Map의 visual odometry를 보여준다는 뜻, F2F(녹색), F2M(빨간색),
RGB-D나 스테레오 카메라 입력으로 사용할 수 있음, 로봇 상에서 카메라가 어디에 위치해 있는지 알기 위해 tf가 필요함.
이를 통해 오도메트리 정보를 로봇 베이스 프레임으로 변환할 수 있음

> F2F (Frame-To_Frame) : 현재 들어온 영상 프레임을 바로 직전의 프레임과 비교

> F2M (Frame-To_Map) : 현재 프레임을 직전 프레임이 아닌, 지금까지 생성된 Local Map(특징점들의 집합)과 비교

그림 2: RGB-D odometry와 Stereo odometry Ros 노드의 다이어그램.
TF는 로봇 베이스에 대한 카메라의 위치를 정의, 동시에 오도메트리 변환을 발행하기 위한 출력으로도 사용함
전체적인 파이프라인은 둘이 동일한데, stereo의 경우만 대응점 계산이 필요하다는 점만 다름
두 가지 오도메트리 방식인 F2F, F2M을 사용할 수 있음

Feature Detection: 프레임이 캡쳐되면, Vis/MaxFeatures 파라미터로 정해진 최대 개수만큼
GFTT(GoodFeaturesToTrack) 특징점들이 감지됨.
GFTT가 파라미터 튜닝이나 다양한 이미지 크기, 조명 세기에서 균일하게 특징점 잡아서 선택했음
RGB-D 이미지 경우, 유효하지 않은 깊이 값을 가진 특징점을 추출하는 걸 피하기 위해
Depth를 GFTT를 위한 마스크로 사용한다는 뜻

> GFTT(GoodFeaturesToTrack) : 영상 처리에서 추적하기 좋은 코너(Corner) 점들을 찾아내는 알고리즘

https://bkshin.tistory.com/entry/OpenCV-26-%EC%9D%B4%EB%AF%B8%EC%A7%80%EC%9D%98-%ED%8A%B9%EC%A7%95%EA%B3%BC-%ED%82%A4-%ED%8F%AC%EC%9D%B8%ED%8A%B8

OpenCV - 26. 이미지의 특징점(Keypoints)과 특징점 검출기(Keypoints detector)

Feature Matching: F2F는 GFTT 특징점에 대해 직접 Optical Flow을 적용하여
키 프레임과의 빠른 특징점 대응 관계를 제공함

> Optical Flow: 영상의 픽셀 밝기 변화 패턴을 따라가며 추적하는 기술 (어디로 갔는지)

계산 속도가 매우 빠름

Motion Frediction: 현재 프레임의 어디에 위치해야 하는지 예측하기 위해 Motion Model이 사용됨
특징점 매칭을 위한 Search Window을 제한하여, 반복적인 텍스처나 동적 물체가 있는 환경에서 나음
탐색 창의 반경은 Vis/CorGuessWinSize 파라미터로 정의됨
등속 운동 모델 ( 방금 전 속도 그대로 움직일 것이라고 가정하는 모델)이 사용됨

Motion Estimation: 특징점 간의 대응 관계가 계산되면 PnP RANSAN을 사용하여 특징점들에 맞춰 변환 계산

> PnP (Perspective-n-Point) : 2D image 상의 점들과 그에 해당하는 3D 공간 상의 점들을 알 때,

카메라 위치와 방향을 알아내는 알고리즘

https://wikidocs.net/305886

2-4. PnP 3D-2D(Perspective-n-Points)

> RANSAC (Random Sample Consensus) : 데이터 속에 노이즈가 많을 때, 무작위로 샘플 뽑아서 모델 찾는 기법

> 변환 계산? : 이전 키프레임 또는 지도에 비해 상대적으로 어떻게 이동하고 회전했는지

Local Bundel Adjustment: 계산된 변환 결과는 마지막 키 프레임의 특징점들만 사용하여
Local Bundle Adjustment를 통해 정교하게 다듬어짐
Pose Update: 추정된 변환 값을 바탕으로 출력 오도메트리가 업데이트 됨. 동시에 tf도 업데이트

> Local Bundle Adjustment : 카메라의 위치와 3D의 특징점의 위치를 동시에 미세 조정하여 오차 최소화하는 방법

---

▶ converter.py

converter.py 작성 이유

1. 깊이(Depth) 데이터 자료형 차이

: CARLA에서는 Depth를 그림(bgra8) 형태로 보냄 

RTAB-Map에서는 Depth를 실제 거리 숫자(32FC1, 실수형)으로 받아야 하기 때문에 맞춰야 함

converter.py에서 CARLA가 보낸 그림의 색깔(RGB)을 공식에 대입해서

실제 미터(m) 단위 거리로 계산한 뒤 RTAB-Map에게 넘겨줌

2. 좌표계(TF) 방향 문제

: CARLA camera는 Z축이 앞방향인데, 로봇 몸통은 X축이 앞방향

= 그대로 받으면 땅바닥이 90도 서서 벽처럼 나오게 됨

converter.py에서 TF 변환 정보를 계속 넘겨줌

CARLA에서 센서 토픽을 쏠 때 converter.py도 실행 후

RTAB-Map을 실행하면 됨

$ python3 converter.py

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
import numpy as np
import math

# 오일러 각도(Roll, Pitch, Yaw)를 쿼터니언으로 바꾸는 함수
def get_quaternion_from_euler(roll, pitch, yaw):
    qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
    qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
    qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
    qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
    return [qx, qy, qz, qw]

class CarlaAutoFixer(Node):
    def __init__(self):
        super().__init__('carla_auto_fixer')
       
        # ROS 표준 카메라 좌표 변환 (앞->앞, 아래->아래)
        # -90도 = -1.57079
        self.tf_roll = -1.57079
        self.tf_pitch = 0.0
        self.tf_yaw = -1.57079
       
        # FOV 및 카메라 설정
        self.fov_deg = 110.0
        self.current_width = 0
        self.current_height = 0

        # TF 브로드캐스터
        self.tf_broadcaster = StaticTransformBroadcaster(self)
        self.publish_static_tf() # 시작하자마자 TF 쏨

        # [구독]
        self.sub_rgb = self.create_subscription(
            Image, '/carla/hero/rgb/image', self.rgb_callback, qos_profile_sensor_data)
        self.sub_depth = self.create_subscription(
            Image, '/carla/hero/depth/image', self.depth_callback, qos_profile_sensor_data)
        self.sub_info = self.create_subscription(
            CameraInfo, '/carla/hero/rgb/camera_info', self.info_callback, qos_profile_sensor_data)
       
        # [발행]
        self.pub_rgb = self.create_publisher(Image, '/carla/hero/rgb/image_converted', 10)
        self.pub_depth = self.create_publisher(Image, '/carla/hero/depth/image_converted', 10)
        self.pub_info = self.create_publisher(CameraInfo, '/carla/hero/rgb/camera_info_converted', 10)
       
        self.bridge = CvBridge()
        self.get_logger().info('>>> CARLA Rotation Fixer Started (Roll-90, Yaw-90) <<<')

    def publish_static_tf(self):
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = "hero"    # 몸통 (기준)
        t.child_frame_id = "hero_optical"   # 카메라 (회전시킬 대상)

        t.transform.translation.x = 0.5  # 로봇 중심보다 0.5m 앞에 설치됐다고 가정
        t.transform.translation.y = 0.0
        t.transform.translation.z = 1.5  # 로봇 바닥에서 1.5m 높이에 설치됐다고 가정

        # 오일러 -> 쿼터니언 변환
        q = get_quaternion_from_euler(self.tf_roll, self.tf_pitch, self.tf_yaw)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        self.tf_broadcaster.sendTransform(t)

    def rgb_callback(self, msg):
        self.current_width = msg.width
        self.current_height = msg.height
        msg.header.frame_id = "hero_optical"
        self.pub_rgb.publish(msg)

    def info_callback(self, msg):
        if self.current_width == 0: return
       
        # TF를 주기적으로 쏴줘서 끊김 방지
        self.publish_static_tf()

        w = self.current_width
        h = self.current_height
        fx = w / (2.0 * math.tan(math.radians(self.fov_deg) / 2.0))
        fy = fx
        cx = w / 2.0
        cy = h / 2.0

        msg.header.frame_id = "hero_optical"
        msg.width = w
        msg.height = h
        msg.distortion_model = "plumb_bob"
        msg.k = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0]
        msg.p = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0]
        msg.d = [0.0, 0.0, 0.0, 0.0, 0.0]
        self.pub_info.publish(msg)

    def depth_callback(self, msg):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgra8')
            bgra = cv_image.astype(np.float32)
            normalized = (bgra[:,:,2] + bgra[:,:,1] * 256.0 + bgra[:,:,0] * 256.0 * 256.0) / (256.0**3 - 1)
            depth_meters = normalized * 1000.0
           
            new_msg = self.bridge.cv2_to_imgmsg(depth_meters, encoding='32FC1')
            new_msg.header = msg.header
            new_msg.header.frame_id = "hero_optical"
            self.pub_depth.publish(new_msg)
        except Exception:
            pass

def main():
    rclpy.init()
    node = CarlaAutoFixer()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

---

▶ rtabmap.launch.py 분석

rtabmap_ros/rtabmap_launch/launch/rtabmap.launch.py

위 코드에는 rgb-d 관련 코드 말고도 다른 센서로 실행 가능한 파라미터 값들이 많아서

현재 사용한 RGB-D SLAM 관련 코드만 우선 분석함

RGB-D odometry

https://kind-slip-86b.notion.site/rtabmap-launch-py-RGB-D-odometry-2f38a2c2bfdb8001b65de408b4e0e0cb?source=copy_link

rtabmap.launch.py RGB-D odometry | Notion

RTAB-Map SLAM

https://kind-slip-86b.notion.site/rtabmap-launch-py-RTAB-Map-SLAM-2f38a2c2bfdb80e58b15fcc455d81626?source=copy_link

rtabmap.launch.py RTAB-Map SLAM | Notion

RTAM-Map Viz

https://kind-slip-86b.notion.site/rtabmap-launch-py-RTAB-Map-Viz-2f38a2c2bfdb80858b29fa8c79cc39a2?source=copy_link

rtabmap.launch.py RTAB-Map Viz | Notion

사용한 명령어

$ ros2 launch rtabmap_launch rtabmap.launch.py \
    use_sim_time:=true \
    \
    args:="-d --DeleteDbxxxxOnStart \
           --Grid/CellSize 0.05 \
           --Grid/RangeMax 100.0 \
           --Vis/MaxDepth 100.0 \
           --Mem/ImagePreDecimation 1 \
           --Mem/ImagePostDecimation 1" \
    \
    visual_odometry:=true \
    icp_odometry:=false \
    \
    subscribe_rgb:=true \
    subscribe_depth:=true \
    subscribe_scan:=false \
    subscribe_imu:=true \
    \
    frame_id:=hero \
    \
    rgb_topic:=/carla/hero/rgb/image_converted \
    depth_topic:=/carla/hero/depth/image_converted \
    camera_info_topic:=/carla/hero/rgb/camera_info_converted \
    \
    imu_topic:=/carla/hero/imu \
    wait_imu_to_init:=false \
    \
    approx_sync:=true \
    queue_size:=20 \
    qos:=1

1. 기본 실행 및 시간 설정 (Basic Launch)
RTAB-Map 패키지를 불러오고 시뮬레이션 시간을 동기화하는 부분임.

ros2 launch rtabmap_launch rtabmap.launch.py
rtabmap_launch 패키지에 있는 메인 실행 파일인 rtabmap.launch.py를 실행함.
use_sim_time:=true
로봇의 실제 시간이 아니라, CARLA 시뮬레이터가 보내주는 가상의 시간(/clock)을 사용하도록 설정함.

2. 내부 알고리즘 인자 설정 (Internal Args)
RTAB-Map 코어 노드(rtabmap)에 직접 전달되는 세부 파라미터들임.

-d
기존에 저장된 데이터베이스를 삭제함.
--DeleteDbxxxxOnStart
시작할 때 rtabmap.db 파일을 초기화하여 매번 깨끗한 상태에서 맵핑을 시작하도록 함.
--Grid/CellSize 0.05
2D 점유 격자 지도(Occupancy Grid)의 해상도를 5cm(0.05m)로 설정함.
--Grid/RangeMax 100.0
로봇 위치로부터 반경 100m 이내의 장애물만 2D 지도에 표시함.
--Vis/MaxDepth 100.0
비주얼 오도메트리에서 특징점을 추출할 때, 카메라로부터 100m 이내에 있는 물체만 사용함.
--Mem/ImagePreDecimation 1
이미지를 처리하기 전 해상도를 줄이는 비율임. 1은 줄이지 않고 원본 그대로 쓴다는 뜻임.
--Mem/ImagePostDecimation 1
특징점 추출 등을 위해 이미지를 줄이는 비율임. 

3. 오도메트리 모드 설정 (Odometry Mode)
위치 추정 방식을 결정하는 중요한 부분임.

visual_odometry:=true
카메라 영상을 이용해 위치를 추정하는 비주얼 오도메트리 노드를 실행함.
icp_odometry:=false
라이다 데이터를 이용한 ICP(Iterative Closest Point) 오도메트리는 사용하지 않음.

4. 센서 구독 설정 (Sensor Subscription)
어떤 센서 데이터를 사용할지 켜고 끄는 스위치임.

subscribe_rgb:=true
컬러(RGB) 이미지 토픽을 구독함.
subscribe_depth:=true
깊이(Depth) 이미지 토픽을 구독함.
subscribe_scan:=false
2D 라이다 스캔 데이터는 구독하지 않음.
subscribe_imu:=true
IMU(관성 측정 장치) 데이터를 구독함. (기울기 보정 등에 사용)

5. 좌표계 및 토픽 연결 (Frames & Topics)
로봇의 기준과 데이터가 들어오는 통로를 지정함.

frame_id:=hero
로봇의 몸통(Base Link) 역할을 하는 프레임 이름을 hero로 설정함.
rgb_topic:=/carla/hero/rgb/image_converted
실제 RGB 이미지가 들어오는 토픽 경로를 연결함.
depth_topic:=/carla/hero/depth/image_converted
실제 Depth 이미지가 들어오는 토픽 경로를 연결함.
camera_info_topic:=/carla/hero/rgb/camera_info_converted
카메라 렌즈 정보가 들어오는 토픽 경로를 연결함.
imu_topic:=/carla/hero/imu
IMU 데이터가 들어오는 토픽 경로를 연결함.

6. 동기화 및 품질 설정 (Sync & QoS)
데이터 처리 방식과 통신 품질을 조율함.

wait_imu_to_init:=false
IMU 데이터가 들어와서 수평을 잡을 때까지 기다리지 않고, 바로 SLAM을 시작하도록 설정함.
approx_sync:=true
RGB와 Depth 이미지의 시간이 정확히 일치하지 않아도(CARLA 특성), 비슷한 시간대면 묶어서 처리하는 근사 동기화를 사용함.
queue_size:=20
데이터가 들어올 때 버퍼 크기를 20개로 설정하여, 순간적인 렉에도 데이터가 끊기지 않게 함.
qos:=1
통신 품질(Quality of Service)을 1로 설정함. Reliable 모드

---

▶ 실행

[CARLA]에서 맵의 일부분만 돌아봄

위와 같이 계속 돌면서 SLAM하게 됨

3바퀴 돈 후 Visual odometry

보기 좋게 띄우려면 저장된 맵을 불러와서 설정하면 됨

우선 명령어의 인자값에 -d --DeleteDbxxxxOnStart 인자값을 빼고 실행하면

위와 같이 실행되는데 빨간색 박스를 누르면 이전에 저장했던 맵을 로드함

맵 db는 보통 여기에 저장되니 따로 백업해두는 편이 좋음

맵을 로드하기 전에 Map을 보기 좋게 띄워야 하기 때문에

위 설정에서 Window를 누르면 preferences... 를 누르면 아래 설정이 뜸

위와 같은 설정값으로 바꿈

그 다음 똑같이 맵 로드를 하면

이전보단 보기 좋게 변경됨

여기에서 저장하는 방법은

기다리면 위와 같은 창이 뜸. 원하는 곳에 저장

---

▶ view.py

Point Cloud로 저장한 map을 확인하기 위해 작성

> python view.py

import open3d as o3d

# 파일 읽기
pcd = o3d.io.read_point_cloud("cloud_final.ply")

# 시각화 객체 생성
vis = o3d.visualization.Visualizer()
vis.create_window(window_name='My Point Cloud', width=800, height=600)

# 포인트 클라우드 추가
vis.add_geometry(pcd)

# 렌더링 옵션 가져와서 점 크기 설정
opt = vis.get_render_option()
opt.point_size = 2.0  # 여기 숫자를 3.0 ~ 10.0 사이로 조절하셈
opt.background_color = [0, 0, 0]  # 배경색 (0,0,0: 검정 / 1,1,1: 흰색)

# 실행 및 종료
vis.run()
vis.destroy_window()

L키를 누르면 조명 관련된 설정 꺼져서 보기 편함

---

▶ rtabmap-database Viewer 

이전 프레임들이랑 특징점이 맞는지, point cloud 저장을 위해 어떤 형식을 쓸 건지 보는 코드

$ rtabmap-databaseViewer ~/.ros/rtabmap.db

---

▶ save pgm

rtab-map 런치 파일 실행할 때 아래 옵션 체크

이후 rtabmap-databaseViewer가 켜진 상태에서 실행하면 됨

누르면 Export optimized 2D map... 이 보임 그걸 눌러서 pgm 형태로 저장하면 됨