융합_로보테크 AI 자율주행 로봇 개발자 과정-26/05/08

TurtleBot3 스테레오 비전 포인트 클라우드 파이프라인

USB 스테레오 카메라 2대로 3D 포인트 클라우드를 생성하고 RViz로 시각화하는 분산 처리 파이프라인. ROS2 Humble 기반.

시스템 구조

영상 캡처는 Raspberry Pi 4가, disparity 연산은 워크스테이션이 분담하는 2-PC 구조.

Pi4 (원격)                          워크스테이션 (로컬)
──────────────                     ──────────────────
usb_cam ×2 (320×240, 10Hz)    →    JPEG 디코드
   ↓                                ↓
rectify ×2 (캘리브레이션 적용)         WLS disparity 연산
   ↓                                ↓
JPEG 인코드 ─── DDS (ID=199) ───→   stereo_image_proc
                                     ↓
                                    /points2 → RViz

네트워크 부하 절감을 위해 Pi4에서 JPEG 압축 후 전송, 워크스테이션에서 raw 디코드.

기술 스택

영역 사용 기술

OS	Ubuntu 22.04 (워크스테이션), Raspberry Pi OS (Pi4)
미들웨어	ROS2 Humble, DDS (ROS_DOMAIN_ID=199)
비전	OpenCV 4.13 (contrib), SGBM, WLS post-filter
이미지전송	image_transport JPEG republish
카메라	USB 카메라 ×2, usb_cam 패키지
좌표변환	tf2_ros static_transform_publisher
시각화	RViz2 (stereo_pc.rviz)

파이프라인 동작

1. 영상 획득 (Pi4)

usb_cam_node_exe 2개를 /dev/video0, /dev/video2에 바인딩. 320×240 / 10Hz 동기 캡처.

2. 렉티피케이션 (Pi4)

image_proc/rectify_node ×2가 left_camera.yaml, right_camera.yaml 캘리브레이션 데이터로 영상 보정. 좌/우 epipolar line 정렬 보장.

3. JPEG 압축 전송 (Pi4 → 워크스테이션)

image_transport/republish ×4로 raw → JPEG 변환 후 발행:

• /left/image_rect/compressed
• /right/image_rect/compressed
• /left/camera_info
• /right/camera_info

4. JPEG 디코드 (워크스테이션)

image_transport republish로 JPEG → raw 복원:

• /left/image_rect_pc
• /right/image_rect_pc

5. Disparity 연산 (워크스테이션)

wls_disparity_node.py에서 두 단계 처리:

1. SGBM (Semi-Global Block Matching): 좌/우 영상 매칭으로 disparity map 생성
2. WLS (Weighted Least Squares) 필터: 신뢰도 기반 평활화, 경계 보존

발행 토픽: /disparity (stereo_msgs/DisparityImage)
주요 파라미터 9종:
파라미터 기본값 역할

min_disparity	-16	음수 허용 (근거리 객체 검출)
disparity_range	80	검색 폭 (16 배수)
block_size	7	SGBM 매칭 윈도우
uniqueness_ratio	8	매칭 유일성 임계 (%)
speckle_window_size	50	점박이 클러스터 임계
speckle_range	8	클러스터 disparity 차이 허용
p1_factor / p2_factor	8/32	SGBM 평활 강도
wls_lambda	8000	WLS 평활 강도 (4000~16000)
wls_sigma	1.5	경계 보존 (0.8~2.5)

6. 포인트 클라우드 생성 (워크스테이션)

stereo_image_proc/point_cloud_node가 /disparity + /left/camera_info로 3D 포인트 변환 → /points2 발행.

7. 좌표계 변환 + 시각화

static_transform_publisher로 base_link → left_camera_frame 정적 TF 발행. 쿼터니언 (-0.5, 0.5, -0.5, 0.5)로 광학 좌표(REP 105) ↔ ROS 좌표(REP 103) 변환.
RViz2가 /points2를 base_link 기준으로 렌더링.

토픽 흐름도

Pi4: usb_cam → rectify → /left/image_rect → JPEG republish → /left/image_rect/compressed
                                                                    │
Local: ─────────────────────────────────────────────────────────────┘
       ↓ raw republish
       /left/image_rect_pc ──┐
                              ├→ wls_disparity_node → /disparity ─┐
       /right/image_rect_pc ─┘                                     ├→ point_cloud_node → /points2 → RViz
                              /left/camera_info ────────────────────┘

결과

동작 확인 항목

• 양 PC 간 ROS2 토픽 송수신 정상 (ROS_DOMAIN_ID=199 도메인 격리)
• 좌/우 캘리브레이션 적용된 rectified 영상 수신 확인
• WLS 필터링된 disparity map 생성 확인
• /points2 포인트 클라우드 발행 확인
• RViz2에서 base_link 프레임 기준 3D 점구름 렌더링 확인

측정 성능

항목 값

입력 해상도	320×240
카메라 프레임레이트	10Hz
Disparity 처리율	약 1Hz (STEREO_SGBM_MODE_3WAY 기준)
고속 모드 처리율	약 10Hz (STEREO_SGBM_MODE_SGBM, 품질 저하)
Pi4 부하	load 0.5~1.0 / 4 cores, 온도 70°C 미만

패키징 상태

워크스테이션 의존성 설치, Pi4 배포, 실행, 종료를 5개 셸 스크립트로 자동화.

scripts/
├── setup_workstation.sh    # 워크스테이션 의존성 (1회)
├── setup_remote.sh         # Pi4 의존성 + 파일 배포 (1회)
├── run_remote.sh           # 원격 카메라 시작
├── run_local.sh            # 로컬 파이프라인 + RViz
└── stop_all.sh             # 전체 종료

남은 한계

1. 처리율 1Hz

WLS 필터가 단일 스레드 동작. 320×240 SGBM 2회 + WLS 후처리 합산 약 1Hz. 카메라 입력 10Hz 대비 9프레임 드롭.
STEREO_SGBM_MODE_SGBM으로 전환 시 10Hz 도달하나 disparity 품질 저하. 품질-속도 트레이드오프 외 선택지 없음.

2. 파라미터 런타임 반영 불가

wls_disparity_node.py가 declare_parameter를 init 시점에만 읽음. ros2 param set으로 값 변경해도 반영되지 않음. 튜닝 시 노드 재시작 필요.
해결 방향: add_on_set_parameters_callback 등록 후 콜백 내에서 SGBM/WLS 객체 재생성.

3. TF 실측값 미반영

현재 정적 TF가 카메라 = base_link 원점 가정 (--x 0 --y 0 --z 0). TurtleBot3 마운트 실측 오프셋 미반영 상태. RViz 점구름과 실제 위치 간 오차 존재.
해결 방향: 마운트 위치 실측 후 run_local.sh 내 static_transform_publisher 인자 수정.

4. OpenCV 버전 충돌

cv2.ximgproc.WLS는 opencv-contrib-python 필요. ultralytics 등은 opencv-python 요구. 동일 환경 공존 시 import 충돌 발생.
현재 회피: ~/.local에 사용자 단위 설치. YOLO 등 병행 사용 시 venv 분리 필요.

5. RViz / TF 기동 순서 의존성

RViz2가 static_transform_publisher보다 먼저 기동되면 Fixed Frame [base_link] does not exist 에러. 자동 복구 로직 없음. RViz 재시작으로 회피.
해결 방향: launch 파일에서 TF publisher를 RViz 의존성으로 등록하거나 TimerAction 도입.

향후 작업 후보

• GPU 기반 stereo (CUDA SGBM, RAFT-Stereo 등 학습 기반 모델) 도입으로 처리율 개선
• 동적 파라미터 콜백 구현
• 카메라 마운트 오프셋 실측 및 launch 파라미터화
• nav2 / octomap 연동으로 3D 장애물 회피 활용

오늘까지 진행한 사진