ICP Odometry, LiDAR 3D SLAM

[신민서]

[  WSL  ]  Ubuntu 24.04 Jazzy ROS2

Carla에서 3D LiDAR RTAB-MAP을 이용하여 SLAM 수행

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

---

▶ rtab-map icp odometry (Lidar Odometry)

Fig 3: S2S(녹색), S2F(빨간색) 구분, LiDAR Odometry의 다이어그램임
visual odometry와 유사한 용어를 사용한다는 뜻, Key frame = point cloud or laser scan

S2S (Scan to Scan) : 바로 직전의 scan data frame과 현재 scan data만 비교

S2M (Scan to Map) : 현재 scan data를 지금까지 누적된 Map과 비교

RTAB-Map의 기본 설정 (visual odometry도 포함)

= S2M (F2M)으로 설정되어 있음

Odometry 설정할 수 있는 값 확인 - 467 line

https://github.com/introlab/rtabmap/blob/master/corelib/include/rtabmap/core/Parameters.h

rtabmap/corelib/include/rtabmap/core/Parameters.h at master · introlab/rtabmap

RTABMAP_PARAM(Odom, Strategy,               int, 0,      
"0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");

Fig 3: icp odometry 블록 다이어그램, TF는 로봇 베이스를 기준으로 한 라이다의 위치를 정의함
로봇 베이스의 odometry 변환을 출력함
등속 운동 모델(분홍색)을 사용할지, 다른 소스를 사용할지 선택할 수 있음

Laser Scan을 사용할지, Point Cloud를 사용할지 선택 가능

input으로 TF와 함께 센서 값들을 받음

Point Cloud Filtering: input으로 받은 point cloud를 다운샘플링 되고 법선 벡터(normals)가 계산됨
point cloud를 로봇 베이스 프레임으로 변환하기 위해 tf가 사용됨
이를 통해 odometry가 그에 맞춰(로봇 중심 기준으로) 계산될 수 있도록 함

Normals: 각 점(point)이 어느 방향을 바라보고 있는지를 나타내는 벡터.

ICP Registration: 새로운 point cloud를 S2M 방식이나 S2S 방식에 registration 시키기 위해 icp 알고리즘 수행
point cloud map은 과거의 point cloud들이 모여서 조립된 형태
점-대-점 (P2P, Point to Point) 또는 점-대-면 (P2N, Point to Plane) 관계를 사용하여 수행
평평한 표면 (인공적인 환경)에서는 P2N 방식이 더 선호됨

ICP (Iterative Closest Point): 두 개의 3D point cloud가 있을 때, 겹쳐지는 위치를 찾아내는 알고리즘

P2P (Point to Point): 두 점이 있으면 가장 가까운 점의 거리를 줄이는 방식
> 단순하지면, 벽 같은 평면을 만나면 잘못 매칭됨

P2N (Point to Plane): 점 하나를 다른 점 하나가 속한 평면(면)까지 거리를 줄이는 방식
> 여기에서 Normals가 사용됨, 실내 환경에서 더 좋은 성능을 보임

https://alida.tistory.com/105

[SLAM] Iterative Closest Point(ICP) 개념 정리 (+ point-to-plane, GICP)

Motion Prediction: ICP는 미지의 대응 관계를 다루기 때문에, transform을 추정하기 전에 유효한 동작 예측 값을 필요로 함
registration 결과에서 얻거나, tf를 통해 외부 odometry로부터 얻을 수 있음 (wheels)
외부 오도메트리가 사용되지 않는 경우는, 등속 운동 모델에 따라 동작 예측이 수행됨

문제점은 복도같은 환경에서는 odometry 오차가 계속 쌓임, icp가 회전은 보정할 수 있는데
속도 변화는 감지할 수 없기 때문임 = 이런 경우 외부 odometry를 사용

현재 point cloud의 구조적 복잡도가 고정된 임계값 (Icp/PointToPlaneMinComplexity)보다 낮은 경우
외부 오도메트리에서 가져와야 함

ICP 알고리즘은 답을 한 번에 찾는 게 아니라, 예측 후 조금씩 맞춰가는 방식임.

처음에 엉뚱한 곳을 찍으면 엉뚱한 결과가 나옴 (초기값이 중요)

Pose Update: ICP registration에 성공하면, odometry pose가 업데이트됨.
외부 odometry가 사용될 때, TF 출력은 외부 odometry tf에 대한 보정값이 됨
이를 통해 두 변환이 tf 트리 내에 존재함 (/odom_icp -> /odom -> /base_link)
visual odometry와 마찬가지로 공분산은 3D 점 대응 관계들 사이의 MAD 방식 사용

MAD (Median Absolute Deviation): 평균 대신 중간값을 기준으로 편차를 계산하는 방식

ICP가 변환을 찾지 못하는 경우, odometry는 lost 상태가 됨
LiDAR Odometry는 동작 예측값이 null일 때 lost 상태에서 스스로 복구할 수 없음

따라서 LiDAR Odometry는 반드시 초기화 되어야 함

LiDAR가 구조물을 감지할 수 있는 한, 로봇이 길을 잃는 경우는 드물다고 함..
잃어도 외부 odometry가 사용되는 경우에는 lost 상태에서 스스로 복구 가능함

Odometry Lost: 주변에 벽이 없거나, 너무 빨리 움직여서 겹치는 부분이 없을 때 odometry 동작을 멈춤

LiDAR/ICP 특징은 바로 직전의 모양과 비교하는 성능이 강해서 한 번 길을 잃으면, 구분하기 어려워 함
> 엉뚱한 곳에 매칭되어 지도가 망가지는 것을 방지하기 위해 처음부터 그냥 reset해버림

LOAM (Lidar Odometry and Mapping)

Ji_LidarMapping_RSS2014_v8.pdf

3.48MB

---

▶ ros2 bag

1. ros2 bag?

ROS 2 시스템에서 센서 데이터, 메시지 통신 등을 기록하거나 재생하는 데 사용되는 도구

= 현재 받고 있는 토픽들을 기록해놨다가 다시 꺼내서 사용할 수 있음

simulator에서 같은 상황을 실험할 때마다 계속 같은 상황을 유지할 수 없으니 사용

2. ros2 bag install

https://github.com/ros2/rosbag2

GitHub - ros2/rosbag2

위 깃허브에 잘 나와있음

$ sudo apt-get install ros-jazzy-rosbag2

3. ros2 bag record & play

예제 - gazebo

https://docs.ros.org/en/jazzy/Tutorials/Beginner-CLI-Tools/Recording-And-Playing-Back-Data/Recording-And-Playing-Back-Data.html

Recording and playing back data — ROS 2 Documentation: Jazzy documentation

CARLA에서 주는 topic들을 받아서 나중에 쓰기 위해 저장

$ mkdir bag_files
$ cd bag_files 

CARLA에서 topic이 오기 시작하면 record로 기록 시작하면 됨

$ ros2 bag record -a -o carla_topics

-a : 현재 모든 topic을 기록에 넣겠다는 뜻

-o : 경로 안에 저장하겠다는 뜻

기록 시작하면 받고 있는 topic의 목록들을 보여주고 기록을 끝낼 때는 ctrl + c로 끝내면 됨

잘 저장되었는지 확인

$ ros2 bag info carla_topics/

저장된 걸 실행하고 싶을 때

$ ros2 bag play carla_topics/

마찬가지로 실행을 끝내고 싶다면 ctrl + c로 끝내면 됨

---

▶ ERROR & WARNNING

http://official-rtab-map-forum.206.s1.nabble.com/Errors-on-icp-odometry-td7546.html#a7643

Official RTAB-Map Forum - Errors on icp odometry

http://official-rtab-map-forum.206.s1.nabble.com/Icp-Odometry-td8396.html

Official RTAB-Map Forum - Icp Odometry

Official RTAB-Map Forum에서 같은 경우를 찾음

결론적으로 현재 상황에서는 Carla가 보내주는 라이다 데이터의 시간(Timestamp)과 ROS 시스템의 시간을 맞춰줘야 해결

---

▶ 실행 명령 및 결과

파라미터 분석

https://kind-slip-86b.notion.site/rtabmap-parameter-2fb8a2c2bfdb80bc86bbef9591807603?source=copy_link

rtabmap parameter | Notion

icp odometry로 실행 명령어

launch로 작성해서 한 줄로 실행하도록 정리함

carla_slam.launch.py

from launch import LaunchDescription 
from launch_ros.actions import Node



def generate_launch_description():
    return LaunchDescription([



        Node(
            package='tf2_ros', executable='static_transform_publisher',  # tf2_ros 패키지의 정적 변환 실행 파일임
            arguments=['0', '0', '2.4', '0', '0', '0', 'hero', 'lidar']  # hero 프레임 위 2.4m에 lidar 프레임 고정함
        ),



        Node(
            package='rtabmap_odom', executable='icp_odometry', output='screen',
            parameters=[{  
                'frame_id': 'hero',  # 로봇의 기준 프레임 이름
                'odom_frame_id': 'odom',  # 생성될 오도메트리 프레임 이름
                'wait_for_transform': 3.0,   # TF 변환 기다리는 시간 1초
                'use_sim_time': True,  # 시뮬레이션 시간(ROS Time) 사용
                'subscribe_imu': False,  # IMU 데이터는 사용 안 함
                'subscribe_scan_cloud': True,  # 3D 포인트 클라우드 토픽 구독
                'scan_cloud_max_points': 0,  # 처리할 포인트 클라우드 개수 제한 없음
                'queue_size': 500,  # 메시지 큐 크기
                'approx_sync': True,  # 메시지 타임스탬프


                'Odom/Strategy': '0',   # (F2M: Frame-to-Map) 사용
                'Odom/GuessMotion': 'true',  # 이전 움직임을 바탕으로 다음 위치 예측
                'Odom/ResetCountdown': '1',  # 추적 실패 시 1 프레임 뒤에 오도메트리 리셋
                'Odom/ScanKeyFrameThr': '0.8',  # 키프레임 생성 임계값 0.8
                'OdomF2M/ScanSubtractRadius': '0.5',  # 스캔 데이터 뺄 때 반경 0.5m
                'OdomF2M/ScanMaxSize': '10000',  # 스캔 맵 최대 포인트 개수 10000개    




                'Icp/MaxTranslation': '0',  # ICP 매칭 시 최대 이동 거리 제한 없음
                'Icp/MaxRotation': '0',  # ICP 매칭 시 최대 회전 각도 제한 없음
                'Icp/CCMaxFinalRMS': '1', # 오차 임계값 허용.. 최대로
                'Icp/VoxelSize': '0.5',    # 포인트 클라우드를 0.5m 간격으로 다운샘플링함
                'Icp/CorrespondenceRatio': '0.01',  # 대응점 비율이 1%만 되어도 매칭 성공으로 침
                'Icp/PointToPlane': 'true',  # 점-대-면(Point-to-Plane) 매칭 방식 사용함
                'Icp/PointToPlaneK': '20',  # 법선 벡터 계산할 때 주변 점 20개 참조함
                'Icp/MaxCorrespondenceDistance': '1.5',  # 대응점 찾을 때 최대 1.5m 거리까지 허용함
                'Icp/PMOutlierRatio': '1',  # 다 쓰도록 변경 (버리는 포인트 없이)
               
                'Reg/Force3DoF': 'true',


            }],
            remappings=[
                ('scan_cloud', '/carla/hero/lidar')  
            ]
        ),
       



        Node(
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=[{  
                'frame_id': 'hero',  
                'map_frame_id': 'map',
                'use_sim_time': True,  # 시뮬레이션 시간 사용
                'subscribe_depth': False,  # 깊이 카메라
                'subscribe_rgb': 'true',  # RGB 카메라
                'subscribe_scan_cloud': True,  # 포인트 클라우드 사용함
                'approx_sync': True,
                'queue_size': 500,
                'qos': 2,  
               


                # 'Rtabmap/StartNewMapOnLoopClosure': 'false',  # 루프 클로저 발생해도 새 맵 시작 안 함
                'Rtabmap/LoopThr': '0.05',
                'Rtabmap/PublishingRate': '1.0',  # 맵 데이터 발행 주기 1Hz
                'Rtabmap/AlwaysUpdate': 'true',  # 로봇이 멈춰있어도 맵 업데이트
                'Rtabmap/MaxRepublished': '0',  # 데이터 재발행 제한 없음(무한)



                'Icp/CorrespondenceRatio': '0.2',  # 매칭 비율 1%면 루프 클로저 인정함
                'Icp/VoxelSize': '0.2',  # 복셀 크기 설정함
                'Icp/MaxCorrespondenceDistance': '3.0',   # 매칭 거리 3m까지 허용
                'Icp/MaxRotation': '0',
                'Icp/MaxTranslation': '0',
                'Icp/PointToPlane': 'true',  # 점-대-면 방식
                'Icp/PointToPlaneK': '20',  # 법선 계산용 이웃 점 개수
                # 'Icp/PMOutlierRatio': '0.7',  # 이상치 비율 설정


               
                # 'Reg/VarianceFromInliers': '999.0',  # 인라이어 분산값을 매우 높게 잡음
                'Reg/Strategy': '1',  # 0번(visual) 1번(ICP) 2번 융합
                'Reg/Force3DoF': 'true',  # 3자유도(2D)
                # 'RGBD/OptimizeMaxError': '0',  # 최적화 에러 무시하고 강제로 합침
                'RGBD/ProximityMaxGraphDepth' : '0',




                'Grid/3D': 'true',  # 3D 그리드 맵 생성
                'Grid/RayTracing': 'true',   # 레이트레이싱으로 빈 공간 채움
                'Grid/FromDepth': 'false',  # 깊이 이미지 말고 라이다로 그리드 만듦
                'Grid/RangeMax': '100.0',     # 그리드 맵 최대 범위
                'Grid/CellSize': '0.5',  # 그리드 셀 크기 0.5m



                'Map/CloudFromScan': 'true',  # 스캔 데이터로부터 클라우드 지도 생성
               
                'Mem/IncrementalMemory': 'true', # 끄면 localization
                'Mem/UseOdomFeatures': 'false',
                'Mem/BinDataKept': 'true',



                'Optimizer/VarianceIgnored': 'true',
               
                'wait_for_transform': 3.0,  # TF 대기 시간 1초
                'Odom/AlwaysProcessMostRecentFrame': 'false',  # 항상 최신 프레임만 처리하는 건 아니게 설정..
            }],
            remappings=[  
                ('scan_cloud', '/carla/hero/lidar'),
                ('odom', '/odom'),
                ('rgb/image', '/carla/hero/rgb/image'),      
                ('rgb/camera_info', '/carla/hero/rgb/camera_info')  
            ],
            arguments=['-d']  # 실행 시 기존 데이터베이스 삭제하고 새로 시작
        ),
       



        Node(  
            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
            parameters=[{  
                'frame_id': 'hero',  
                'use_sim_time': True,  # 시뮬레이션 시간 씀
                'subscribe_odom_info': True,  
                'subscribe_scan_cloud': True,  
                'approx_sync': True,
                'queue_size': 500,
                'qos': 2  
            }],
            remappings=[  
                ('scan_cloud', '/carla/hero/lidar'),
                ('odom', '/odom')  
            ]
        )
    ])

$ ros2 launch carla_slam.launch.py

결합 실패 시

결합 성공

이런 식으로 Proximity Detection을 한 노란색 선이 떠야 성공임

정답과 비교

Proximity Detection이 보정한 모습

위와 같이 너무 거미줄처럼 쳐버리면 원래 맵이 찌그러져 결론적으로는 localization 성능에 문제가 있게 됨

이런식으로 겹쳐보여야 맵 생성이 잘 된 것

---

▶ 로그 메세지 분석

실행 시 정상적으로 동작한다면 위와 같은 로그 메세지가 뜸

rtabmap

• Rate=1.00s : 1초마다 업데이트 중
• WM: 메모리에 몇 프레임까지 잡고 있다는 뜻
• local map: 지도 데이터 생성된 프레임
• Limit: 시간 제한
• RTAB-MAP: 지도를 갱신하는데 걸린 시간

icp odometry

• ratio: ICP 매칭 점수
• std dev: 위치 추정의 오차 범위 (cm)
• update time: 오도메트리 계산하는 데 걸린 시간

---

▶ Localization

---

▶ 2D MAP

[Sungryul Lee]

slam관련 파라미터를 모두 설명할것, 특히 loop closure detection관련 파라미터를 자세히 설명할것
현재까지 나온 로그메시지를 모두 분석하고 개선방법을 제안할것

[Sungryul Lee]

최종성능평가는 로컬라이제이션 모드로 실행해서 자동차의 정답자세과 비교해볼것, 6차원 x,y,z, roll,pitch, yaw 모두 확인해야함

[신민서]

수정했습니다