이 문서는 우리가 만든 프로그램이 어떻게 작동하는지 단계별로 설명합니다. 카메라에서 들어온 단일 스테레오 이미지가 화면에 3D 점으로 찍히기까지의 전체 여정을 추적해 보겠습니다.
🏛️ 핵심 아키텍처: 두 개의 스레드
가장 먼저, 우리 프로그램이 두 개의 병렬 스레드에서 실행된다는 점을 이해하는 것이 중요합니다. 이것이 바로 프로그램을 빠르게 만드는 핵심입니다. 하나의 스레드가 무거운 수학 연산을 처리하는 동안, 다른 스레드는 그림을 그립니다.
- VO 스레드 (VO Thread - "공장")
- 파일: src/main.rs
- 함수: run_live_vo_thread
- 임무: 모든 힘든 작업을 수행합니다. 카메라와 통신하고, 비용이 많이 드는 OpenCV 함수(이미지 보정, 스테레오 계산, 특징점 검출)를 실행하며, 3D 포인트와 카메라 자세(Pose)를 계산합니다. 이 스레드의 유일한 목표는 "결과물"을 생산하는 것입니다.
- 메인 스레드 (Main Thread - "전시장")
- 파일: src/main.rs
- 함수: main
- 임무: 모든 그리기를 수행합니다. OpenGL 창과 OpenCV imshow 창을 생성합니다. 99%의 시간을 이전에 마지막으로 받은 결과를 다시 그리고 새로운 결과를 기다리는 데 사용합니다. 이 스레드는 절대로 무거운 계산을 해서는 안 됩니다. 그렇지 않으면 화면이 멈출 것입니다.
- MPSC 채널 (The "컨베이어 벨트")
- 코드: let (tx, rx) = mpsc::channel::<(...)>();
- 임무: 두 스레드를 연결하는 고속 "컨베이어 벨트"입니다.
- **tx (Transmitter)**는 VO 스레드에 있습니다. 최종 결과물을 보냅니다.
- **rx (Receiver)**는 메인 스레드에 있습니다. 결과물을 받습니다.
🗺️ 파트 1: 프레임의 여정 (VO 스레드)
run_live_vo_thread 함수 내부에서 단일 프레임이 처리되는 단계별 과정입니다.
1단계: 초기화 (최초 1회 실행)
루프가 시작되기 전에 모든 것을 준비합니다:
- load_calibration(): stereo_params_PC.yml 파일을 엽니다. 이 함수는 카메라를 정의하는 모든 핵심 매트릭스(K1, D1, R1, P1, Q 등)를 로드합니다. 또한 P1 매트릭스와 image_height를 기반으로 카메라의 *실제 수직 화각(FOV)*을 계산합니다 (약 30.15도임을 확인했습니다).
- init_rectification(): 로드한 보정 데이터를 사용하여 두 개의 "조회 테이블"(RectificationMaps)을 미리 계산합니다. 이 맵은 원본 이미지를 *왜곡(warp)*시켜 완벽하게 정렬되고 왜곡이 제거된 이미지로 만드는 데 사용됩니다.
- cap_combined.set(...): 카메라에게 320x240 이미지 두 개가 나란히 붙어 있는 640x240 크기의 단일 와이드 이미지를 보내도록 요청합니다.
2단계: 획득 및 분할 (루프 시작)
- cap_combined.read(): 카메라에서 하나의 와이드 프레임을 가져옵니다.
- Mat::roi(): roi_l(왼쪽 영역)과 roi_r(오른쪽 영역)을 사용하여 이 와이드 프레임을 curr_left_gray와 curr_right_gray라는 두 개의 개별 Mat 객체로 "잘라냅니다".
3단계: 이미지 보정 (첫 번째 "마법")
스테레오 비전에서 가장 중요한 단계입니다.
- imgproc::remap(): 이 함수를 두 번 호출합니다. 1단계에서 미리 계산한 RectificationMaps를 사용하여 curr_left_gray와 curr_right_gray의 "왜곡을 풉니다".
- 결과: rectified_left와 rectified_right 이미지를 얻습니다. 이 이미지들은 이제 완벽하게 정렬되어, 마치 "완벽한" 핀홀 카메라로 찍은 것처럼 되었습니다. 이 과정은 다음 단계가 작동하기 위해 필수적입니다.
4단계: 시차(Disparity) 및 3D 복원
- compute_disparity_map(): 두 개의 보정된 이미지를 StereoSGBM 알고리즘에 입력합니다. 알고리즘이 두 이미지를 비교하여 disparity_map_curr(시차 맵)를 생성합니다. 이것은 각 픽셀의 색상이 깊이를 나타내는 2D 이미지입니다 (밝을수록 가까움).
- calib3d::reproject_image_to_3d(): 이것이 두 번째 "마법" 단계입니다. 2D 시차 맵과 Q 매트릭스(보정 파일에서 로드한)를 이 함수에 전달합니다. 이 정보는 2D 깊이 이미지를 완전한 3D 포인트 클라우드(point_cloud_curr)로 변환하는 데 사용됩니다. 이제 각 픽셀은 밀리미터(mm) 단위의 실제 (X, Y, Z) 좌표를 갖게 됩니다.
5단계: Visual Odometry (우리는 어디에 있는가?)
이 단계는 이전 프레임 대비 우리가 어떻게 움직였는지 파악합니다.
- detector.detect_and_compute(): rectified_left 이미지에서 수백 개의 "키포인트"(코너나 흥미로운 질감 등)를 찾습니다.
- knn_match(): 이번 프레임의 키포인트와 이전 프레임에서 저장해 둔 키포인트(prev_frame_data)를 비교합니다. 이를 통해 수백 개의 2D 매칭 쌍을 얻습니다.
- 3D 대응점 생성: 모든 2D 매칭 쌍을 반복하면서, 각 쌍에 대해 현재 point_cloud_curr와 이전 prev_frame_data에서 3D (X,Y,Z) 좌표를 조회합니다. 이제 (3D 공간의 A 지점, 3D 공간의 B 지점) 쌍의 목록을 갖게 됩니다. 이것이 우리의 correspondences 리스트입니다 (예: "70개의 유효한 3D 대응점").
- odom.update(): 이 리스트를 Odometry 클래스로 보냅니다. 이 클래스는 RANSAC 알고리즘을 사용하여 이 모든 점들의 움직임을 가장 잘 설명하는 최적의 회전(R) 및 이동(t) 값을 찾습니다. 그런 다음, 이 움직임을 global_rotation과 global_translation에 누적하여 처음 시작점으로부터의 현재 위치를 계속 추적합니다.
6단계: 렌더러를 위한 처리 (Vec<f32> 생성)
이제 최종 결과물을 만듭니다. point_cloud_curr의 모든 픽셀을 반복합니다:
- (X,Y,Z) 가져오기: (mm 단위의) (X, Y, Z) 값을 가져옵니다.
- 필터링: 유효한 값인지 확인합니다: if z_cv.is_finite() && z_cv > 300.0 && z_cv < 10000.0. 이것이 불량 데이터를 제거하는 30cm ~ 10m "깊이 필터"입니다.
- 좌표계 변환: OpenCV 좌표계에서 OpenGL 좌표계로 변환하기 위해 Y축과 Z축을 뒤집습니다 (y_gl = -y_cv, z_gl = -z_cv).
- 색상 계산: 깊이에 따라 흰색(가까움)에서 어두운 회색(멈)으로 변하는 그레이스케일 색상을 계산합니다.
- points_vec.push(...): 6개의 숫자(x_gl, y_gl, z_gl, r, g, b)를 최종 Vec<f32>에 추가합니다.
7단계: 텍스트 그리기 및 전송
- put_text(): 사용자 정의 설정(빨간색, 0.8 크기 등)을 사용하여 point_count, centroid, global_translation 값을 tracking_viz 이미지에 그립니다.
- data_tx.send(...): 세 가지 결과물을 하나로 묶어 "컨베이어 벨트"를 통해 메인 스레드로 전송합니다:
- tracking_viz (텍스트가 그려진 이미지)
- points_vec (거대한 3D 포인트 목록)
- (global_rot, global_trans) (최종 자세)
이제 VO 스레드의 루프가 완료되었으며, 즉시 2단계로 돌아가 다음 프레임 처리를 시작합니다.
📦 파트 2: 데이터 수신 및 사용 (메인 스레드)
이 부분은 훨씬 간단하고 빠릅니다. main 함수의 이벤트 루프입니다.
1단계: "컨베이어 벨트" 확인
- rx.try_recv(): 메인 스레드에서 가장 중요한 부분입니다. VO 스레드에서 새 패키지가 도착했는지 "컨베이어 벨트"를 살짝 엿봅니다.
- Ok(...)일 경우: 새 패키지가 도착했습니다! if 블록 안으로 들어갑니다.
- Err(...)일 경우: 새 패키지가 없습니다. if 블록 전체를 건너뛰고 이전 데이터를 다시 그립니다. 이것이 바로 VO 스레드가 느리더라도 3D 뷰가 여전히 부드럽게 느껴지는 이유입니다.
2단계: 모든 상태 업데이트 (새 패키지 도착 시)
- highgui::imshow(...): tracking_img를 즉시 OpenCV 창에 표시합니다.
- point_cloud.update_data(points_vec): 전체 Vec<f32>를 PointCloud 객체로 보냅니다. 이 객체는 모든 새 정점 데이터를 GPU의 VBO(Vertex Buffer Object)에 업로드합니다.
- 자세(Pose)를 뷰 매트릭스(View Matrix)로 변환: 이것이 "1인칭" 카메라의 핵심 로직입니다:
- vo_rot_na와 vo_trans_na를 cgmath 타입(r_gl, t_gl)으로 변환합니다.
- 카메라의 3D 위치를 vo_camera_pos_gl에 저장합니다.
- 이들을 4x4 vo_pose_matrix로 결합합니다.
- 이 매트릭스를 **역(inverse)**변환하여 **vo_view_matrix**를 만듭니다. (이것은 3D 그래픽스의 핵심 개념입니다: "뷰" 매트릭스는 항상 월드 공간에서의 카메라 "자세"의 역행렬입니다.)
3단계: 장면 렌더링 (매 루프마다 실행)
- let view = vo_view_matrix;: 방금 계산한 (또는 새 패키지가 없으면 이전의) vo_view_matrix를 사용하도록 렌더러에 지시합니다.
- let proj = perspective(...): 시작할 때 계산한 올바른 FOV를 사용하여 투영 매트릭스를 생성합니다.
- let model_matrix = Matrix4::identity();: 포인트 클라우드를 원점(0,0,0)에 그리도록 렌더러에 지시합니다.
- point_cloud.draw(...): 이 함수는 GPU에게 "VBO에 있는 모든 점들을 이 model_matrix, view 매트릭스, proj 매트릭스를 사용해서 그려라"고 명령합니다.
- axis.draw(...): 월드 좌표축에 대해서도 동일한 작업을 수행합니다.
- windowed_context.swap_buffers(): 최종 이미지를 화면에 표시합니다.
🚀 파트 3: "최종 데이터" 활용하기
이제 매 프레임마다 세 가지 매우 가치 있는 데이터를 생성하는 프로그램을 갖게 되었습니다. 이 데이터는 VO 스레드의 data_tx.send(...) 호출 직전에 존재합니다.
- point_cloud_curr (원본 Mat): (유효하지 않은 값 포함) 320x240 = 76,800개의 포인트로 구성된 전체 3D 포인트 클라우드 (OpenCV Mat 형식).
- points_vec (필터링된 Vec<f32>): 깊이 값으로 필터링된 깨끗한 포인트 클라우드 (약 ~29,000개 포인트), OpenGL에서 렌더링 준비 완료된 형식.
- global_translation (자세): 카메라의 (X, Y, Z) 위치를 밀리미터(mm) 단위로 담고 있는 na::Vector3<f32>.
이 데이터를 활용하는 3가지 아이디어는 다음과 같습니다:
아이디어 1: 경로 저장하기 (간단한 SLAM)
카메라가 이동한 경로를 파일에 저장하여 시각화할 수 있습니다.
- run_live_vo_thread (루프 시작 전):
use std::io::Write; // 상단에 추가
let mut camera_path: Vec<na::Vector3<f32>> = Vec::new();
- run_live_vo_thread (루프 내부, odom.update 호출 후):
let (global_rot, global_trans) = odom.get_pose();
camera_path.push(global_trans.clone());
- 맨 마지막 (루프 종료 후):
// 프로그램이 종료될 때 경로를 파일로 저장
let mut file = std::fs::File::create("path_output.txt")?;
for point in camera_path {
writeln!(file, "{} {} {}", point.x, point.y, point.z)?;
}
println!("Saved camera path to path_output.txt");
아이디어 2: 간단한 로봇 내비게이션
자세(pose) 정보를 기초적인 자동화에 사용할 수 있습니다.
- run_live_vo_thread (루프 내부, odom.update 호출 후):
let (global_rot, global_trans) = odom.get_pose();
// 예: 로봇이 1미터(1000mm) 이상 움직였는지 확인
if global_trans.norm() > 1000.0 {
println!("경고: 시작 지점에서 1미터 이상 벗어났습니다!");
// (여기에 로봇 모터에 "후진" 또는 "회전" 같은
// 명령을 보내는 코드를 추가할 수 있습니다)
}
아이디어 3: 3D 스캔 파일(.xyz) 생성하기
3D 환경을 저장하여 MeshLab 같은 프로그램에서 볼 수 있습니다.
- run_live_vo_thread (루프 종료 후):
// 참고: 이것은 *마지막* 프레임만 저장합니다. 실제 3D 스캐너는
// *모든* 프레임의 포인트들을 자세(pose)를 이용해 결합해야 합니다.
// 이미 만들어둔 `points_vec`를 사용할 수 있습니다.
use std::io::Write; // 상단에 추가 (이미 했다면 생략)
let mut file = std::fs::File::create("scan.xyz")?;
// `points_vec`는 [x,y,z,r,g,b, x,y,z,r,g,b, ...] 형식입니다.
// 한 번에 6개 항목씩 건너뜁니다.
for chunk in points_vec.chunks_exact(6) {
let x = chunk[0];
let y = chunk[1];
let z = chunk[2];
let r = (chunk[3] * 255.0) as u8; // 색상 값을 0-255 범위로 변환
let g = (chunk[4] * 255.0) as u8;
let b = (chunk[5] * 255.0) as u8;
// "XYZRGB" 형식으로 저장
writeln!(file, "{} {} {} {} {} {}", x, y, z, r, g, b)?;
}
println!("Saved last point cloud to scan.xyz");
'Depth Map with Dual CAM' 카테고리의 다른 글
| 초저가 USB 쌍안 캠을 사용한 Depth Tracking (0) | 2021.07.12 |
|---|
