🧠 ORB-SLAM2 구조 정리

---

ORB-SLAM2는 실시간 Visual SLAM을 위한 오픈소스 프레임워크로, 단일 카메라, 스테레오 카메라, RGB-D 카메라를 모두 지원하며 다음과 같은 세 가지 핵심 모듈로 구성됩니다.

---

🔧 1. Tracking (추적)

실시간으로 카메라 위치를 추정하는 모듈

• 입력: 이미지 프레임
• ORB 특징점 추출
• 이전 프레임과의 특징점 매칭
• PnP or ICP 방식으로 카메라 포즈(6DoF) 추정
• KeyFrame 여부 판단
• Relocalization 기능 포함 (위치 추적 실패 시 재인식 시도)

---

🗺️ 2. Local Mapping (로컬 지도 작성)

현재 KeyFrame 주변의 국소적인 3D 맵 업데이트

• KeyFrame의 ORB 특징점으로 새로운 맵 포인트 생성
• 기존 포인트와 연결해서 Local Bundle Adjustment 수행
• 주변 KeyFrame들과의 연결 관계(코비전 그래프)를 업데이트

---

♻️ 3. Loop Closing (루프 클로저)

드리프트 누적을 줄이기 위한 위치 재인식 및 전역 보정

• 새 KeyFrame이 과거에 본 장소와 겹치는지 탐색
• BoW (Bag of Words) 방식으로 유사도 판단
• 루프를 감지하면 Pose Graph Optimization 수행
• 전역적인 Bundle Adjustment로 전체 지도 정합

---

🧩 데이터 흐름 요약

[이미지 입력] 
    ↓
[Tracking] → KeyFrame 결정 → [Local Mapping] ↔ [Loop Closing]
    ↓
[실시간 카메라 위치 추정 (Pose)]

---

🔄 Sensor Fusion (선택사항)

• ORB-SLAM2 자체는 IMU 미지원이지만,
• 확장판인 ORB-SLAM3에서는 Visual-Inertial SLAM 지원됨 (IMU + vSLAM 융합)

---

💡 특징 요약

항목	내용
특징점 방식	ORB (FAST + BRIEF) 기반
지도 방식	Sparse point map (점 기반 지도)
카메라 지원	Mono, Stereo, RGB-D 모두 가능
코드 구조	C++ + OpenCV + Pangolin (시각화)
성능	실시간 동작 가능, 정확도 매우 높음
단점	저조도, 반사, 텍스처 부족 환경에 약함

---

---

🧩 A. ORB-SLAM2 + ROS 연동

📦 1. ROS 패키지 설치

cd ~/catkin_ws/src
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
cd ORB_SLAM2
chmod +x build.sh
./build.sh

• build.sh 실행 시 Pangolin, OpenCV 등 자동 빌드됩니다.
• catkin_make로 ROS 패키지 빌드

📡 2. ROS 토픽 연결

ORB-SLAM2는 sensor_msgs/Image와 sensor_msgs/CameraInfo를 구독합니다.

rosrun ORB_SLAM2 Mono PATH_TO_VOC PATH_TO_CONFIG.yaml

예시:

rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml

• /camera/image_raw 등의 토픽에서 실시간 이미지 수신
• ORB-SLAM2는 내부에서 geometry_msgs/PoseStamped 형태로 추정 위치를 퍼블리시하거나, TF 브로드캐스트도 가능

🧭 3. 연동되는 토픽

토픽 이름	메시지 타입	설명
/camera/image_raw	sensor_msgs/Image	카메라 영상 입력
/camera/camera_info	sensor_msgs/CameraInfo	카메라 내역 (초점거리 등)
/orbslam2/pose	geometry_msgs/PoseStamped	추정된 카메라 위치 출력
/tf	TF 변환	언리얼 혹은 RViz에서 시각화

---

🎮 B. ORB-SLAM2 + Unreal Engine 연동

Unreal은 ROS와 직접 통신할 수는 없지만, 중간 브릿지를 통해 외부 위치 데이터를 언리얼의 Actor 위치로 전달할 수 있습니다.

방법 1: ROS ↔ UDP/Socket ↔ Unreal

📡 1. ORB-SLAM2 → Pose 데이터를 소켓으로 전송 (Python 예시)

import socket
import rospy
from geometry_msgs.msg import PoseStamped

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
unreal_ip = '127.0.0.1'
unreal_port = 9876

def callback(pose_msg):
    x = pose_msg.pose.position.x
    y = pose_msg.pose.position.y
    z = pose_msg.pose.position.z
    data = f"{x},{y},{z}"
    sock.sendto(data.encode(), (unreal_ip, unreal_port))

rospy.init_node('orbslam2_udp_sender')
rospy.Subscriber('/orbslam2/pose', PoseStamped, callback)
rospy.spin()

🎮 2. 언리얼에서 수신 (C++ or Blueprint via UDP)

• Unreal Engine에서 UDP 리스너를 통해 x, y, z 데이터를 받아 Actor 위치 갱신
• 블루프린트 플러그인: UE4 UDP Receiver Plugin

---

방법 2: ROS Integration Plugin for Unreal

• ROSIntegration Plugin 사용
  • ROS 메시지를 직접 언리얼에서 구독 가능
  • 언리얼 Actor에 TF 데이터를 바로 반영

⚠️ 설정이 복잡하고 ROS 버전, Unreal 버전 호환성이 까다로움

---

🏁 결론

• ROS 연동은 안정적이고 RViz 시각화까지 가능 → 연구 및 실증 환경에 적합
• Unreal 연동은 UDP 또는 ROS 브리지 방식으로 가능하며, 실시간 XR/AR 응용에 적합
• 실제 XR 기반 위치 추적이나 VR 게임에서 ORB-SLAM2 결과를 시각화하는 데 적합한 구조

---

📌 요약 먼저: PnP vs ICP

항목	PnP (Perspective-n-Point)	ICP (Iterative Closest Point)
주로 쓰는 데이터	3D 포인트 ↔ 2D 이미지 좌표	두 개의 3D 포인트 클라우드
핵심 목적	카메라 위치 추정 (PnP + RANSAC으로 추정 robust)	포인트 클라우드 간 정합 (등록/정렬)
대표 사용처	비주얼 SLAM (ORB-SLAM, ARKit 등)	라이다 기반 SLAM, 3D 스캔 정합
연산량	상대적으로 작음	상대적으로 큼
요구 조건	3D 맵포인트와 이미지상 특징점 매칭 필요	정밀한 3D 포인트 클라우드 필요

---

🔍 1. PnP (Perspective-n-Point)

✔️ 목적:

3D 포인트 + 2D 이미지상 대응점이 있을 때,
카메라의 6DoF 포즈 (위치 + 회전)를 추정하는 알고리즘입니다.

✔️ 입력:

• 3D 점들 (월드 좌표계 상의 맵포인트들)
• 해당 점이 카메라 이미지에 찍힌 2D 좌표들
• 카메라 내부 파라미터 (초점 거리 등)

✔️ 동작 방식:

• 여러 개의 3D↔2D 쌍이 있으면, 수학적으로 이를 만족하는 카메라 위치와 방향(R, t) 을 역으로 계산
• 보통 EPnP, RPnP, RANSAC PnP 알고리즘을 사용해서 노이즈나 오류를 견딜 수 있게 개선

✔️ 예시:

cv2.solvePnP(points_3d, points_2d, K, distCoeffs)

---

🔍 2. ICP (Iterative Closest Point)

✔️ 목적:

두 개의 3D 포인트 클라우드가 있을 때,
서로 어떻게 겹쳐야 가장 잘 맞는지(변환행렬 R, t)를 추정하는 알고리즘

✔️ 입력:

• 포인트 클라우드 A (기준)
• 포인트 클라우드 B (이동 대상)

✔️ 동작 방식:

1. B의 각 포인트에 대해 A에서 가장 가까운 포인트 찾기
2. 대응점을 기준으로 최적의 변환(R, t)을 계산
3. B에 적용하고, 오차가 줄어들 때까지 반복

✔️ 예시:

• LiDAR SLAM
• 3D 스캐닝 모델 정합

pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setInputSource(cloud_source);
icp.setInputTarget(cloud_target);
icp.align(cloud_aligned);

---

🎯 실제 SLAM 시스템에서는 어떻게 쓰이나?

상황	사용 방식
ORB-SLAM, ARKit 등 비주얼 SLAM	PnP 사용
LiDAR 기반 SLAM (e.g., Cartographer, LOAM)	ICP 사용
RGB-D SLAM	PnP + Depth or ICP 혼용 가능

---

✅ 한줄 정리

• PnP는 “2D 이미지에서 보고 있는 3D 포인트로부터 카메라 위치를 유추”
• ICP는 “두 개의 3D 점 구름을 정렬해서 상대 위치를 계산”

둘 다 SLAM, AR, 3D 재구성에서 핵심적인 위치 추정 도구입니다.

---

이제 PnP (Perspective-n-Point) 알고리즘의 수학적 유도를 차근차근 설명드릴게요.
다소 수학적인 내용이지만, 핵심은 2D 이미지에 보이는 점이 실제 3D 공간의 어느 위치에 대응하는지를 통해 카메라의 위치와 자세(R, t)를 추정하는 것입니다.

---

📌 문제 정의: PnP란?

N개의 3D 포인트 Xi∈R3\mathbf{X}_i \in \mathbb{R}^3 와
그에 대응하는 이미지 내 2D 포인트 xi∈R2\mathbf{x}_i \in \mathbb{R}^2 가 주어졌을 때,
카메라의 회전 행렬 R∈SO(3)\mathbf{R} \in SO(3) 과 이동 벡터 t∈R3\mathbf{t} \in \mathbb{R}^3 를 구하는 문제

---

🔧 1. 카메라 투영 모델

🌐 월드좌표 → 카메라좌표

Xc=R⋅Xw+t\mathbf{X}_c = \mathbf{R} \cdot \mathbf{X}_w + \mathbf{t}

🔍 카메라좌표 → 이미지좌표

xi=K⋅[R∣t]⋅[Xw1]\mathbf{x}_i = \mathbf{K} \cdot [\mathbf{R} | \mathbf{t}] \cdot \begin{bmatrix} \mathbf{X}_w \\ 1 \end{bmatrix}

여기서:

• xi=(ui,vi,1)T\mathbf{x}_i = (u_i, v_i, 1)^T: 이미지의 homogeneous 좌표
• K\mathbf{K}: 카메라 내부 파라미터 행렬

K=[fx0cx0fycy001]\mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}

---

🎯 2. 최소 조건

• 최소 3점 이상 필요 (Perspective-3-Point)
• 실제로는 4점 이상으로 과잉 결정(overdetermined) → 최소자승법 (Least Squares) 사용

---

🔍 3. 비선형 최적화 문제로 정의

min⁡R,t∑i=1N∥xi−π(R⋅Xi+t)∥2\min_{\mathbf{R}, \mathbf{t}} \sum_{i=1}^{N} \left\| \mathbf{x}_i – \pi(\mathbf{R} \cdot \mathbf{X}_i + \mathbf{t}) \right\|^2

• π(⋅)\pi(\cdot): 3D → 2D 프로젝션 함수 (카메라 내부 파라미터 포함)

π(Xc)=[fx⋅XcZc+cxfy⋅YcZc+cy]\pi(\mathbf{X}_c) = \begin{bmatrix} f_x \cdot \frac{X_c}{Z_c} + c_x \\ f_y \cdot \frac{Y_c}{Z_c} + c_y \end{bmatrix}

• 이 식은 비선형이므로 Levenberg-Marquardt 최적화 또는 EPnP 알고리즘 같은 근사 기법 사용

---

🧠 4. RANSAC PnP

실제 환경에서는 매칭된 점 중 일부가 **오류(outlier)**일 수 있기 때문에,
PnP는 보통 RANSAC과 함께 사용합니다:

1. 4~6개의 랜덤 대응점 선택
2. PnP로 R, t 계산
3. 나머지 점에 대해 reprojection error 계산
4. 오차가 작은 점들(inliers)의 개수로 모델 평가
5. 여러 반복 중 가장 좋은 모델 선택

---

✅ 요약 정리

단계	설명
1.	3D ↔ 2D 대응점 확보
2.	투영 모델 수식으로 위치(R, t) 추정
3.	비선형 최적화로 오차 최소화
4.	RANSAC과 함께 사용하면 outlier에 강인함

---