# Omni-LIVO 멀티카메라 아키텍처의 fhd_fast_tri_ws(ROS2) 이식 계획

- 문서 버전: 2026-07-07 v1
- 대상 워크스페이스: `~/fhd_fast_tri_ws` (ROS2 Humble, `~/fast_ws`의 사본)
- 참조 구현: `~/Omni-LIVO` (ROS1 catkin, 패키지명 `omni_livo`)
- 실물 카메라 드라이버: `~/camera2_ws/src/hik_camera_ros2_driver`
- 검증 데이터셋: Omni-LIVO 저자 공개 데이터셋 (Baidu Netdisk, 4-카메라 십자 배열 rosbag)
- **`~/fast_ws`는 레거시로 유지, 절대 수정하지 않음.**

---

## 0. 요약 (TL;DR)

1. `fhd_fast_tri_ws/src/FAST-LIVO2`는 이미 ROS2로 포팅된 **단일 카메라** FAST-LIVO2다. 카메라 관련 자료구조가 전부 스칼라/단일 멤버(`cv::Mat img`, `vk::AbstractCamera *cam`, `M3D Rcl` 등)로 박혀 있다.
2. `Omni-LIVO`는 동일한 FAST-LIVO2 코드베이스를 **완전히 벡터 기반 N-카메라 구조**로 확장한 ROS1 구현이다. 4대 카메라를 가정한 하드코딩은 어디에도 없으며(`num_of_cameras`는 설정 배열 길이에서 파생), 실제로 저자 자신도 1/3/4대 구성을 config만 바꿔 실행한 전례가 있다(`avia.yaml`=1대, `Hilti2022.yaml`=3대, `mid360.yaml`=4대). 따라서 **"2대 카메라만 쓰기"는 목표 아키텍처에서 정상적으로 지원되는 사용 시나리오**이지, 별도 특수 케이스가 아니다.
3. 핵심 작업은 Omni-LIVO의 `vio.h/.cpp`, `LIVMapper.h/.cpp`, `frame.h/.cpp`, `feature.h`, `visual_point.h/.cpp`에 있는 벡터화 로직을 fhd_fast_tri_ws의 이미 ROS2화된 동명 파일들에 **병합 이식**하는 것이다. LiDAR/IMU 관련 파일(`preprocess.*`, `IMU_Processing.*`, `voxel_map.*`)은 카메라 대수와 무관하므로 손대지 않는다.
4. ROS2 고유 마찰점은 세 가지: (a) ROS2 파라미터 서버가 Omni-LIVO의 `extrin_calib.cameras` 배열(array-of-struct) YAML 구조를 못 받으므로 `yaml-cpp` 직접 파싱으로 전환, (b) tf1→tf2 (fhd_fast_tri_ws에는 이미 반영되어 있어 신규 코드에서만 주의), (c) `livox_ros_driver` → `livox_ros_driver2` (fhd_fast_tri_ws는 이미 v2 사용 중이라 문제 없음).
5. 실물 듀얼카메라(`camera2_ws`)는 **cam2가 아직 캘리브레이션되지 않음** — 이 상태로는 FAST-LIVO2 계열 알고리즘에 투입 불가. 이식 작업과 별개로 반드시 선행되어야 하는 블로커다.
6. 검증은 하드웨어 없이 Omni-LIVO 저자의 공개 rosbag(ROS1 형식)으로 먼저 수행한다. 이 머신에는 **ROS1이 설치되어 있지 않으므로** `ros1_bridge` 대신 순수 Python 라이브러리 `rosbags`로 오프라인 변환한다. Livox `CustomMsg`는 표준 메시지가 아니므로 커스텀 타입 등록이 필요하다.

---

## 1. 목표와 범위

### 목표
- `fhd_fast_tri_ws`의 FAST-LIVO2(ROS2)를 Omni-LIVO(ROS1)의 멀티카메라(N-카메라, 비중첩 FoV) 아키텍처로 확장한다.
- 확장된 코드가 카메라 2대 구성에서 정상 동작함을 Omni-LIVO 저자 데이터셋(4대 중 2대만 사용)으로 검증한다.
- 이후 `camera2_ws`의 실제 듀얼 하드웨어(Hikvision ×2 + STM32 하드싱크)로 전환 가능한 상태를 만든다.

### 범위 밖 (Out of scope)
- `fast_ws` 수정 — 금지.
- Omni-LIVO의 루프 클로저 관련 설정(`loop_closure:` in `NCD4.yaml`) — 코드에서 실제로 읽지도 않는 미구현/사문화된 설정이므로 이식 대상에서 제외.
- 4대 이상 카메라의 실시간 성능 최적화 — 우선 정확성/동작 검증이 목표.

---

## 2. 현재 상태 요약 (조사 결과)

### 2.1 `fhd_fast_tri_ws` (포팅 대상, ROS2)

| 항목 | 내용 |
|---|---|
| 패키지 | `fast_livo` (`src/FAST-LIVO2`, ament_cmake), `vikit_common`/`vikit_ros`/`vikit_py` (`src/rpg_vikit`) |
| 실행 파일 | `fastlivo_mapping` (노드명 `laserMapping`) |
| 이미지 구독 | 단일 `sub_img` (`LIVMapper.h:172`), 콜백 `img_cbk` (`LIVMapper.cpp:906-959`), 단일 `deque<cv::Mat> img_buffer` |
| 카메라 파라미터 | `vk::camera_loader::loadFromRosNs(node, "parameter_blackboard", cam)` 단일 카메라 오버로드만 호출 (`LIVMapper.cpp:194`) — **벡터 오버로드가 이미 존재하지만 버그가 있고 미사용** (`vikit_ros/src/camera_loader.cpp:91-141`) |
| VIO 코어 | `VIOManager` (`vio.h`/`vio.cpp`, 1876+186줄) — `cam`, `Rci/Pci/Rcl/Pcl`, `fx/fy/cx/cy`, `img_cp/img_rgb`, `new_frame_` 등 전부 **단일 스칼라 멤버** |
| 측정 그룹 | `MeasureGroup::img`가 단일 `cv::Mat` (`common_lib.h:70`) |
| 기존 스캐폴딩 | `extrin_cam1/2/3.yaml`, `camera_cam1/cam3.yaml`, `mapping_mid360s_cam{1,2,3}.launch.py` — **동시 멀티카메라가 아니라 "한 번에 카메라 1대씩 바꿔 쓰는" 상호 배타적 대안 설정**임에 주의 |

### 2.2 `Omni-LIVO` (참조 구현, ROS1)

| 항목 | 내용 |
|---|---|
| 패키지 | `omni_livo` (catkin), 커스텀 `.msg` 없음 (내부 C++ 구조체로만 처리 → ROS2 인터페이스 패키지 불필요) |
| 이미지 구독 | 카메라마다 독립 `ros::Subscriber` + 카메라별 `deque` (`LIVMapper.cpp:312-330`), `message_filters` 미사용, 수동 뮤텍스/타임스탬프 동기화 |
| 동기화 | `sync_packages()`가 `num_of_cameras`개 버퍼가 모두 채워졌는지 확인 후 `time_tolerance=0.001s`로 정합 (`LIVMapper.cpp:904-1165`) |
| 카메라 파라미터 | 외부파라미터: `extrin_calib.cameras`가 **배열-of-구조체** YAML(`cam_id/img_topic/Rcl/Pcl`), 내부파라미터: `cam_0`, `cam_1`, ... 넘버링된 네임스페이스, `XmlRpc::XmlRpcValue`로 파싱 (`LIVMapper.cpp:133-220`) |
| VIO 코어 | `VIOManager`가 `vector<AbstractCamera*> cams`, `vector<M3D> Rci_vec/Rcl_vec/Rcw_vec`, `vector<V3D> Pci_vec/...`, 카메라별 grid/voxel 후보 리스트, `CameraPhotoParams`(카메라별 노출/비네팅) 보유 |
| 조인트 ESIKF | 카메라별 patch residual을 각각 계산한 뒤 **하나의 Jacobian/residual로 병합**해 한 번에 풀이 (`vio.cpp:2429-2717`), 열 구성 `6(pose) + num_cams(노출)`, `addCrossCameraConsistencyConstraint`로 카메라 간 동일 3D점의 광도 일관성 추가 제약 |
| Frame/Feature | `Frame`이 `vector<cams_>`+`vector<SE3> T_f_w_`(카메라별 자세) 보유, `Feature::cam_id_` 태그 추가, `VisualPoint::CrossCameraData`(가시성 비트셋, migration 추적) 신설 |
| 카메라 수 가변성 | 하드코딩 없음. `num_of_cameras = extrin_calib.cameras.size()`. 실사용 예: `avia.yaml`=1대, `Hilti2022.yaml`=3대(4개 중 1개 주석 처리), `NCD4.yaml`=3대 활성, `mid360.yaml`=4대. **카메라 비활성화는 YAML 배열 항목을 주석 처리하는 것만으로 이미 공식적으로 지원되는 방식.** |
| 데이터셋 | `~/Omni-LIVO/README.md` 참조 — LIVOX MID360(10Hz) + ICM40609 IMU(200Hz) + JHEM306GC-HM 카메라 4대(1024×768, 10Hz, 하드웨어 동기화, 십자 배열: Front/Left/Right/Rear). 다운로드: Baidu Netdisk (`README.md:6,53`). LiDAR 토픽 `/livox/lidar` (`lidar_type: 1`, CustomMsg 포맷 — fhd_fast_tri_ws의 `mid360s.yaml`과 동일 컨벤션). Hilti 시퀀스는 40Hz→10Hz 변환 스크립트(`scripts/cvt10hz.py`) 필요하나, **저자 자체 mid360 데이터셋은 이미 10Hz라 변환 불필요**. |

### 2.3 `camera2_ws/src/hik_camera_ros2_driver` (실물 듀얼카메라)

| 항목 | 내용 |
|---|---|
| 노드 구조 | 카메라 1대당 프로세스 1개(동일 노드 클래스), `serial_number`로 물리 장치 매칭 |
| 듀얼 실행 | `launch/hik_camera_dual_launch.py` — 이미 존재, `cam1`/`cam2` 두 프로세스를 별도 파라미터 파일로 기동 |
| 토픽 | `/cam1/image`, `/cam1/camera_info`, `/cam2/image`, `/cam2/camera_info` (`sensor_msgs/Image`, encoding `rgb8` 고정), `frame_id`: `cam1_optical_frame`/`cam2_optical_frame` |
| 하드웨어 트리거 | `TriggerSource`가 `LINE0`으로 하드코딩 (`hik_camera_node.cpp:152`) — 두 카메라 모두 같은 트리거 라인 사용, config로 변경 불가 |
| 타임스탬프 | `use_trigger_timestamp: true`일 때 `livox_ros_driver2`가 쓰는 공유메모리 파일 `/home/<user>/timeshare`의 `low` 필드(LiDAR 패킷 타임스탬프)를 그대로 복사 — **카메라별 개별 트리거 카운터가 아니라 LiDAR 패킷 단위로 양자화된 대략적 시각**임에 유의 |
| ⚠️ 블로커 | **`config/camera_info_cam2.yaml`이 미캘리브레이션 상태** (identity/placeholder 값, 파일 자체 주석으로 명시: "아직 캘리브레이션되지 않음") — FAST-LIVO2류 알고리즘은 정확한 intrinsic/distortion이 없으면 photometric alignment가 근본적으로 깨짐. 실물 하드웨어 통합 전 **반드시 cam2 캘리브레이션 선행 필요** (`direct_visual_lidar_calibration` 사용 가능, cam1에 이미 적용된 이력 있음). |
| ⚠️ 부가 이슈 | `camera_params_cam2.yaml`의 `serial_number: "DB0174264"`와 다른 파일들의 주석/문서에 적힌 `DB074264`가 자리수 불일치 — 배선 전 확인 필요. |

---

## 3. 아키텍처 설계 결정

**결정: Omni-LIVO의 벡터 기반 N-카메라 아키텍처를 구조적으로 그대로 이식한다 (단순화된 "카메라별 독립 실행" 방식은 채택하지 않는다).**

이유:
- 사용자가 명시적으로 "Omni-LIVO의 코드를 참고하여" 확장한다고 했고, 검증도 Omni-LIVO 저자 데이터셋의 4카메라 십자 배열에서 2대만 골라 쓰는 방식으로 하겠다고 했다 — 이는 Omni-LIVO의 **조인트 ESIKF + cross-view migration**이 실제로 동작하는지 확인하겠다는 의도로 해석된다. 카메라별로 완전히 독립적인 EKF를 돌리는 단순화는 Omni-LIVO의 핵심 기여(Cross-View Temporal Migration, Adaptive Multi-View ESIKF)를 재현하지 못한다.
- fhd_fast_tri_ws의 `VoxelMapManager`(LiDAR 평면지도)는 카메라 개수와 무관하게 이미 공유 가능한 구조이므로, `VIOManager`만 확장하면 LiDAR 파이프라인 전체를 재사용할 수 있다.

**파일별 이식 전략**: "새로 작성"이 아니라 "병합"이다. fhd_fast_tri_ws 파일들은 이미 ROS2 API(rclcpp, tf2, livox_ros_driver2)로 되어 있으므로, Omni-LIVO 파일에서 **ROS1 API 호출부만 fhd_fast_tri_ws의 기존 ROS2 대응 코드로 교체**하면서 나머지(멀티카메라 자료구조/알고리즘)는 그대로 가져온다. 이렇게 하면 이미 검증된 ROS2 이식 부분(파라미터 선언, tf2, 노드 라이프사이클)을 재작업하지 않는다.

---

## 4. 파일별 상세 변경 사항

### 4.1 `include/common_lib.h`
- `MeasureGroup::img` (단일 `cv::Mat`) → `std::vector<cv::Mat> imgs` (Omni-LIVO `common_lib.h:61-84` 그대로).
- `StatesGroup`에 카메라별 노출시간 `std::vector<double> inv_expo_time_per_cam` 추가 (고정 `DIM_STATE=19` 외부에 별도 보관, Omni-LIVO 방식 그대로 — EKF 상태 벡터 자체의 차원은 불변으로 유지해 기존 LIO 코드에 영향 없음).
- tf 관련 include는 fhd_fast_tri_ws가 이미 tf2로 되어 있으면 그대로 둔다 (Omni-LIVO의 `#include <tf/transform_broadcaster.h>`는 가져오지 않음).

### 4.2 `include/frame.h` / `src/frame.cpp`
- `vk::AbstractCamera *cam_` → `std::vector<vk::AbstractCamera*> cams_`.
- `SE3 T_f_w_` → `std::vector<SE3> T_f_w_` (+ `T_f_w_prior_`), 카메라별 고정 외부파라미터로 하나의 바디 자세에서 파생.
- `cv::Mat img_` → `std::vector<cv::Mat> imgs_` (+ `imgs_shared_`).
- 생성자 시그니처를 `Frame(const std::vector<AbstractCamera*>&, std::vector<cv::Mat>&, double)`로 변경.
- 신규 `Vector2d w2c(const Vector3d& xyz_w, int cam_id) const` — 카메라 인덱스를 받는 투영 함수.

### 4.3 `include/feature.h`
- `Feature::cam_id_` 필드 추가 — 어떤 카메라에서 추출된 patch인지 태그. (fhd_fast_tri_ws의 `getWarpMatrixAffine`이 이미 `cam`을 파라미터로 받는 구조라 이 필드를 넘겨 받는 방식으로 자연스럽게 연결됨.)

### 4.4 `include/visual_point.h` / `src/visual_point.cpp`
- `CrossCameraData` 구조체 신설: `currently_visible`/`previously_visible` 비트셋(`MAX_CAMERAS=10`, 지오메트리 가정 아님 — 구현상 상한일 뿐), `primary_cam_idx`, `migration_source_cam`, `cross_camera_migrations` 카운터.
- `VisualPoint::obs_` (`list<Feature*>`)는 이미 다중 관측을 지원하는 구조이므로 변경 없음 — 이 부분이 fhd_fast_tri_ws에서 가장 재사용하기 쉬운 기존 자산.

### 4.5 `include/vio.h` / `src/vio.cpp` (**가장 큰 작업, ~2000줄**)
- 단일 멤버 → 벡터 멤버 전면 교체:
  - `cam` → `vector<AbstractCamera*> cams`
  - `Rci, Pci, Rcl, Pcl, Rcw, Pcw` → `vector<M3D> Rci_vec/Rcl_vec/Rcw_vec`, `vector<V3D> Pci_vec/Pcl_vec/Pcw_vec`
  - `fx,fy,cx,cy,width,height` → 카메라별 배열 또는 `cams[i]`에서 직접 조회
  - grid 상태(`grid_num, map_index, map_dist, scan_value` 등) → `vector<vector<...>> ..._per_cam_`
  - `img_cp, img_rgb` → 카메라별 컨테이너
  - `new_frame_`는 단일 `Frame`을 유지하되, `Frame` 자체가 다중 카메라 이미지를 보유하도록 함(4.2 참고)
  - `CameraPhotoParams`(카메라별 노출/비네팅 보정) 신설
- `processFrame`을 `processFrame(std::vector<cv::Mat> &imgs, ...)` 시그니처로 변경, 내부에서 카메라별 patch selection → 통합 Jacobian 구성 → `addCrossCameraConsistencyConstraint`(동일 3D점의 카메라 간 광도 일관성 제약) → 1회 EKF 풀이 → 카메라별 참조 patch 갱신 순서로 이식.
- `feat_map`(`unordered_map<VOXEL_LOCATION, VOXEL_POINTS*>`)은 카메라 무관 세계좌표 구조이므로 **그대로 유지** — 이 부분은 Omni-LIVO와 fhd_fast_tri_ws가 사실상 동일해야 함.
- RGB 컬러링(`publish_frame_world` 계열)은 `cams.size()`만큼 순회하며 각 카메라에 투영되는 점을 색칠하는 Omni-LIVO 로직을 채용 (한 점이 2대 카메라에 모두 보이면 중복 append — Omni-LIVO 원 동작 그대로 유지, 병합/평균 로직은 향후 개선 과제로 남김).

### 4.6 `include/LIVMapper.h` / `src/LIVMapper.cpp` (**두 번째로 큰 작업, ~1600줄**)
- `sub_img`(단일) → `vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> sub_img_list`, 각각 `std::bind`로 `cam_id` 캡처(Omni-LIVO의 람다 패턴을 `rclcpp` 구독 생성 구문에 맞게 그대로 적용 — ROS1/ROS2 구독 API 차이만 있을 뿐 로직은 동일).
- `img_buffer`/`img_time_buffer`(단일) → `vector<deque<cv::Mat>> img_buffers`, `vector<deque<double>> img_time_buffers`.
- `img_cbk(msg)` → `img_cbk(msg, cam_id)`.
- `sync_packages()`: Omni-LIVO의 다중 버퍼 정합 로직(모든 카메라 버퍼 non-empty 확인 → `time_tolerance` 비교 → `MeasureGroup::imgs` 채우기)을 그대로 이식. fhd_fast_tri_ws 기존 동기화 로직(뮤텍스/조건변수 기반)의 골격은 유지하고 내부 조건문만 다중화.
- `readParameters()`: 카메라 배열 파싱 로직 교체 — **§5에서 별도 설계** (ROS2 파라미터 서버 한계로 인해 XmlRpc 방식을 그대로 못 가져옴).
- `initializeSubscribersAndPublishers`에서 `image_transport::Publisher`도 카메라별로 필요 시 확장 (디버그/파노라마 퍼블리셔는 Omni-LIVO처럼 `ceil(sqrt(num_cams))` 그리드 모자이크로 구성 가능하나 우선순위 낮음, Phase 5로 미룸).

### 4.7 `rpg_vikit/vikit_ros/src/camera_loader.cpp` / `include/vikit/camera_loader.h`
- **이미 존재하는 벡터 오버로드를 재작성하지 않고 버그만 고쳐서 사용** (완전히 새로 만들 필요 없음, 조사 결과 fhd_fast_tri_ws에 이미 있는 자산):
  - 현재 버그: 루프 내 `Pinhole` 분기가 `ns + "/..."`를 읽어야 할 자리에 `cam_ns + "/..."`를 안 쓰는 문제 (§ 2.1 조사 결과) — 각 반복에서 정확히 `cam_ns = ns + "/cam_" + i`를 사용하도록 수정.
  - `getParam`/`getRemoteParam` 혼용 문제 정리: fhd_fast_tri_ws의 단일 카메라 로더가 쓰는 `getRemoteParam`(별도 `parameter_blackboard` 노드 대상) 방식으로 통일.
  - Omni-LIVO의 `cam_0, cam_1, ...` 넘버링 네임스페이스 컨벤션과 100% 동일하게 맞춰서, intrinsics YAML은 Omni-LIVO 스타일(`config/mid360_cam.yaml`처럼 `cam_0:`, `cam_1:` 최상위 키)을 그대로 재사용 가능하게 한다.

### 4.8 `CMakeLists.txt` / `package.xml`
- `yaml-cpp` 의존성 추가(§5의 직접 YAML 파싱용) — ROS2/Ubuntu에 보통 `libyaml-cpp-dev`로 이미 설치되어 있고, `rclcpp` 자체도 내부적으로 사용하므로 새 시스템 패키지 설치 없이 링크만 추가하면 되는 경우가 많음 (환경에서 확인 필요).
- 변경 불필요: `livox_ros_driver2`(이미 사용 중), `cv_bridge`/`image_transport`(이미 사용 중).

---

## 5. 설정 파일 / launch 구조 재설계

### 5.1 문제
ROS2 `rclcpp::Node::declare_parameter`는 원시 배열(`bool[]`, `int64[]`, `double[]`, `string[]`)만 지원하고, Omni-LIVO가 쓰는 **"구조체의 배열"**(`extrin_calib.cameras: [{cam_id, img_topic, Rcl, Pcl}, ...]`)은 표현할 수 없다. 이는 이 이식 작업에서 가장 파괴적인 ROS1→ROS2 차이점이다.

### 5.2 해결책: 카메라 배열 부분만 `yaml-cpp` 직접 파일 로드
- 메인 config YAML(`mid360s_dual.yaml` 등)의 스칼라/단순배열 파라미터(`common`, `vio`, `lio`, `imu` 등)는 지금처럼 `rclcpp` 파라미터 메커니즘(`ros2 launch`의 `parameters=[yaml_path]`)으로 그대로 로드.
- **`extrin_calib.cameras` 배열만** `LIVMapper`가 노드 파라미터 `camera_config_path`(문자열, YAML 파일 경로)를 받아서 `yaml-cpp`로 **직접 파일을 열어** 파싱한다. 스키마는 Omni-LIVO의 것을 그대로 유지:
  ```yaml
  cameras:
    - cam_id: 0
      img_topic: "/cam_front"
      Rcl: [ ... 9 ... ]
      Pcl: [ ... 3 ... ]
    - cam_id: 1
      img_topic: "/cam_rear"
      Rcl: [ ... ]
      Pcl: [ ... ]
  ```
  → `num_of_cameras`는 이 배열의 길이에서 파생 (Omni-LIVO와 동일 철학).
- Intrinsics는 기존 fhd_fast_tri_ws 컨벤션(별도 `parameter_blackboard` 노드 + `camera_*.yaml`)을 유지하되, **카메라 수만큼 네임스페이스를 늘림**: `parameter_blackboard`에 `cam_0/*`, `cam_1/*` 형태로 로드하고 §4.7에서 고친 벡터 로더로 순회 조회.
- 이 방식의 장점: (a) 기존 단일 카메라 config(`mid360s.yaml` 등)와 launch 파일을 전혀 건드리지 않고 그대로 legacy 경로로 남길 수 있음, (b) Omni-LIVO의 YAML 스키마·주석·문서를 거의 그대로 재사용 가능, (c) "카메라 비활성화 = 배열 항목 주석 처리"라는 Omni-LIVO의 검증된 운용 방식을 그대로 물려받음.

### 5.3 신규 launch 파일
- `mapping_mid360s_dualcam.launch.py` (신규) — `parameter_blackboard` 1개(멀티 카메라 intrinsics 전체를 `cam_0/cam_1` 네임스페이스로 로드) + `fastlivo_mapping` 노드 1개(`camera_config_path`로 신규 다중 카메라 extrinsics YAML 지정).
- 기존 `mapping_mid360s_cam{1,2,3}.launch.py`는 **수정하지 않고 그대로 둔다** (단일 카메라 대안 설정으로서 유효, 회귀 없음 보장).

---

## 6. ROS2 이식 시 API 매핑 표

| Omni-LIVO (ROS1) | fhd_fast_tri_ws (ROS2) 대응/조치 |
|---|---|
| `ros::NodeHandle`, `nh.param<T>` | 이미 `rclcpp::Node` + `declare_parameter`/`get_parameter`로 포팅되어 있음 — 신규 멀티카메라 파라미터만 같은 패턴으로 추가 |
| `ros::Subscriber`/`nh.subscribe` (카메라별 람다) | `rclcpp::Node::create_subscription` + `std::bind`, 카메라 인덱스 캡처는 동일 패턴 사용 가능 |
| `XmlRpc::XmlRpcValue` (배열-of-struct 파싱) | **미지원 → §5.2의 `yaml-cpp` 직접 로드로 대체** (가장 중요한 변경점) |
| `tf::TransformBroadcaster`/`tf::Quaternion` | fhd_fast_tri_ws는 이미 tf2 사용 중 — Omni-LIVO 코드 이식 시 해당 라인만 fhd_fast_tri_ws의 기존 tf2 호출로 치환 |
| `livox_ros_driver::CustomMsg` | fhd_fast_tri_ws는 이미 `livox_ros_driver2` 사용 중 — 변경 불필요 |
| `image_transport::ImageTransport` (ROS1) | fhd_fast_tri_ws는 이미 ROS2용 `image_transport` 사용 중 — 카메라별로 인스턴스/퍼블리셔만 늘림 |
| `catkin_package()`, `find_package(catkin ...)` | 해당 없음 (`ament_cmake` 유지) |
| `message_filters` | Omni-LIVO도 안 씀(수동 동기화) → 그대로 수동 동기화 로직 이식, ROS2 `message_filters`로 교체할 필요 없음 |

---

## 7. 실물 하드웨어(camera2_ws) 연동 전 선결 과제

1. **[블로커] cam2 캘리브레이션** — `direct_visual_lidar_calibration`(`~/direct_visual_lidar_calibration`, 이미 설치됨)로 cam1과 동일한 절차 수행 후 `camera_info_cam2.yaml` 갱신. 이게 끝나기 전까지는 실물 2-카메라 통합 테스트가 무의미함 (왜곡 보정이 틀리면 photometric residual 자체가 의미 없음).
2. `camera_params_cam2.yaml`의 `serial_number` 오탈자(`DB0174264` vs 문서상 `DB074264`) 확인 후 실제 장치 시리얼로 통일.
3. `TriggerSource=LINE0` 하드코딩 — 두 카메라를 동일 STM32 트리거 라인에 물릴 것인지, 아니면 드라이버에 `trigger_source` 파라미터를 추가해 분리할 것인지 결정 필요 (현재는 강제로 동일 라인).
4. `timeshare` 공유메모리 타임스탬프가 카메라별이 아니라 전역 1개 파일 기반이므로, 카메라 2대가 완전히 동일한 "가장 최근 LiDAR 패킷 시각"으로 스탬프될 수 있음. Omni-LIVO의 `time_tolerance=0.001s` 정합 로직과 궁합이 맞는지(오히려 지나치게 잘 맞아떨어져서 실제 노출 시각 오차를 못 잡아낼 수 있음) 실물 통합 단계에서 별도 검토.
5. 위 사항들은 **§8 검증(시뮬레이션/데이터셋 기반)과 독립적으로** 병행 진행 가능 — 데이터셋 검증은 이 블로커들과 무관하게 먼저 끝낼 수 있다.

---

## 8. 검증 계획: Omni-LIVO 저자 데이터셋으로 2/4 카메라 검증

### 8.1 데이터 준비
1. Baidu Netdisk에서 `mid360` 계열 시퀀스(4카메라 십자 배열, `/cam_front /cam_left /cam_right /cam_rear`, `/livox/lidar`, `/livox/imu`) 다운로드 (`.bag`, ROS1 형식).
2. **이 머신엔 ROS1이 설치되어 있지 않음** (확인됨: `/opt/ros/`에 `humble`만 존재). `ros1_bridge` 실시간 브리징 대신 **오프라인 변환**을 사용:
   - Python 패키지 `rosbags` (Ternaris) 설치 — ROS 설치 없이 순수 파이썬으로 ROS1 bag ↔ ROS2 bag(sqlite3/mcap) 변환 가능.
   - 표준 메시지(`sensor_msgs/Image`, `sensor_msgs/Imu`)는 자동 변환됨.
   - **Livox `CustomMsg`는 표준 메시지가 아니므로 별도 처리 필요**: `rosbags`의 커스텀 타입스토어에 ROS1 `livox_ros_driver/msg/CustomMsg` 정의(필드: `header, timebase, point_num, lidar_id, rsvd, points[]{offset_time,x,y,z,reflectivity,tag,line}`)를 등록하고, 대상 타입을 fhd_fast_tri_ws가 실제 빌드하는 `livox_ros_driver2/msg/CustomMsg`로 매핑하는 변환 스크립트를 작성한다 (두 메시지는 필드가 동일하므로 1:1 재해석 가능 — 실제 값 변환 로직 불필요, 타입명/패키지명만 재라벨링).
3. 변환된 rosbag2를 `~/bags/` 하위에 저장 (기존 `~/bags` 디렉토리 존재 확인됨).

### 8.2 2-카메라 서브셋 config 작성
- §5.2에서 정의한 신규 `extrin_calib.cameras` YAML을 만들고, 4개 항목 중 2개만 남긴다 (Omni-LIVO의 `NCD1.yaml`/`NCD4.yaml`이 이미 이렇게 일부 카메라를 주석 처리해 운용한 전례를 그대로 따름).
- 권장 조합: **Front + Rear** (서로 반대 방향, 비중첩 FoV 특성과 cross-view migration을 가장 잘 검증할 수 있는 조합) 우선 시도. 필요시 **Front + Left**(인접, 부분 중첩 가능성)로 대조 실험.
- intrinsics는 데이터셋에 동봉된 `mid360_cam.yaml`의 해당 `cam_0`/`cam_2`(혹은 선택한 인덱스) 블록만 사용.

### 8.3 실행 및 성공 기준
- `ros2 bag play`로 변환된 bag 재생 + 신규 `mapping_mid360s_dualcam.launch.py`(카메라 2대 config)로 `fastlivo_mapping` 구동.
- 성공 기준:
  1. 크래시/데드락 없이 전체 bag 재생 완료.
  2. `/cloud_registered`, `/aft_mapped_to_init` 등 기존 단일 카메라 대비 동일한 위상의 궤적 생성 (LiDAR/IMU 경로는 변경하지 않았으므로 LIO 단독 정확도는 원본과 동일해야 함 — 회귀 여부의 1차 체크포인트).
  3. `/rgb_img` 및 저장된 PCD의 RGB 컬러링이 **Front+Rear 두 카메라가 보는 영역 모두**에서 나타나는지 확인 (단일 카메라였다면 한쪽 방향만 컬러링됐을 부분이 이제 양쪽에서 컬러링되어야 함 — 멀티카메라 동작의 가장 직관적인 시각적 증거).
  4. (선택, 상급 검증) 두 카메라가 동시에 관측 가능한 3D점이 있는 시퀀스 구간에서 `enable_cross_camera_tracking` on/off 비교로 cross-view migration의 효과(관측 지속성, 드리프트 감소) 정성적 확인.
- 실패 시 디버깅 우선순위: (1) bag 변환/타임스탬프 정합 문제 → (2) `sync_packages()` 다중 버퍼 로직 → (3) `VIOManager` 벡터화 로직의 인덱스 오류 (가장 흔한 버그 유형: 카메라별 벡터 크기 불일치, `cam_id` off-by-one).

### 8.4 이후 (실물 하드웨어 전환)
- §8에서 사용한 것과 동일한 코드/launch 구조에서 이미지 토픽만 `camera2_ws`의 `/cam1/image`, `/cam2/image`로 교체.
- 전제조건: §7의 cam2 캘리브레이션 완료, 그리고 `direct_visual_lidar_calibration`으로 cam1/cam2 각각의 `Rcl`/`Pcl`(LiDAR 대비 외부파라미터) 재산출.

---

## 9. 단계별 로드맵

| Phase | 내용 | 산출물 | 의존성 | 상태 |
|---|---|---|---|---|
| 0 | 본 계획 문서 작성 및 검토 | 이 문서 | - | ✅ 완료 (2026-07-07) |
| 1~3 (재조정) | 카메라 대수와 무관한 자료구조/로더/동기화 스캐폴딩만 우선 구현: `common_lib.h`(`MeasureGroup::imgs` 벡터화), `camera_loader.cpp` 벡터 오버로드 버그 수정, `yaml-cpp` 기반 `extrin_calib.cameras` 배열 직접 로더(`LIVMapper::loadCameraArrayConfig`, 단일카메라 fallback 포함), `LIVMapper.h/.cpp` 멀티토픽 구독(`sub_img_list`)/버퍼(`img_buffers`)/`sync_packages()` 다중화, 신규 `mapping_mid360s_dualcam.launch.py` + `camera_dualcam_cam1_cam2.yaml` + `extrin_dualcam_cam1_cam2.yaml`(cam1+cam2 조합, 기존 calibration 재사용) | `colcon build` 성공 확인. 신규 dualcam launch로 실행 시 `/cam1/image`, `/cam2/image` 양쪽 구독 로그 및 2개 카메라 intrinsics(cam_0/cam_1) 로드 로그 확인, 기존 `mapping_mid360s.launch.py`(단일 카메라) 회귀 없음 확인. **추가로 실데이터 회귀 테스트 완료**: `mapping_mid360s_cam1.launch.py`(`mid360s.yaml`+`extrin_cam1.yaml`+`camera_cam1.yaml`)로 `~/bags/try_2`(37분, `/camera/image`+`/livox/lidar`+`/livox/imu`) 재생 — bag 시작 30초 이전 구간(알려진 불안정 구간)에서는 `corrupted size vs. prev_size`(SIGABRT)로 죽었으나, **`--start-offset 30`으로 재생 시 150초간 크래시/에러 0건, LIO/VIO 사이클 정상 반복(sparse map 33125→122447), `mat_out.txt` 궤적이 매끄럽게 진행(약 7m 이동, NaN/정지 없음)** — 단일 카메라 경로 회귀 없음을 실데이터로 확인. | - | ✅ 완료 (2026-07-07, 실데이터 회귀 테스트 포함). **범위 조정**: `frame.h/.cpp`, `feature.h`, `visual_point.h/.cpp`(cam_id_, CrossCameraData)는 VIOManager(Phase 4)가 실제로 소비하기 전까지는 미사용 코드가 되므로 이번 패스에서 제외하고 Phase 4와 함께 묶어서 진행하기로 함(사용자 확인 완료). `VIOManager`는 아직 완전히 단일 카메라(`vio_manager->cam = cams[0]`)이며, `handleVIO()`도 `imgs[0]`만 소비 — 카메라 1(cam2)은 구독/버퍼링/시간정합까지는 되지만 VIO 갱신에는 아직 반영되지 않음. **미해결 관찰사항**: bag 시작 30초 이전 구간에서 힙 손상(SIGABRT) 발생 — 사용자에 따르면 해당 구간 데이터 자체가 알려진 불안정 구간이라 재현이 Phase 1~3 변경에 의한 회귀인지 원래 있던 이슈인지는 별도 확인 전까지 미확정. |
| 4 | `frame.h/.cpp`, `feature.h`, `visual_point.h/.cpp` 벡터화 + `VIOManager`(`vio.h/.cpp`, 전체 ~2000줄) 완전 벡터화 + 조인트 ESIKF(`updateState`/`updateStateInverse`) + `addCrossCameraConsistencyConstraint` + Adaptive covariance + 메모리 정리(cleanupOldVisualPoints 등) 전부 이식. `common_lib.h`의 `StatesGroup::inv_expo_time`(단일)도 `inv_expo_time_per_cam`(벡터)로 마이그레이션(`IMU_Processing.cpp`, `LIVMapper.cpp` 로깅부 포함 연쇄 수정). `LIVMapper.cpp`의 `setLidarToCameraExtrinsic`/`vio_manager->cams`/`processFrame(imgs)`/RGB 컬러링/`publish_img_rgb`(panorama_image) 전부 다카메라 경로로 연결. Omni-LIVO 자체에 선언만 있고 정의/호출이 전혀 없는 죽은 코드(`resetAfterLoopClosure`, `applyTrajectoryTransformToVisualMap`, `computeCrossCameraWarpMatrix`, `evaluateCrossCameraConsistency`, `extractPatchSafely`, `H_sub_all`/`z_all`, `ideal_total_points`)는 이식하지 않음 | `colcon build` 성공(Eigen/Sophus `Matrix` 이름 충돌 수정 포함 — `common_lib.h`가 의도적으로 `using namespace Eigen`을 안 쓰는 이유였음, `MatrixXd`/`VectorXd`는 `Eigen::` 명시, 템플릿 `Matrix<double,6,6>`은 `MD(6,6)` 매크로로 대체). `~/bags/try_2`(`--start-offset 30`, 150초)로 단일카메라(N=1, 벡터화된 새 코드 경로) 재검증 — 에러 0건, sparse map 정상 성장, 궤적 매끄러움(NaN/Inf 없음) 확인. 신규 dualcam launch로 N=2 카메라 초기화·구독 스모크테스트(`/cam1/image`, `/cam2/image` 둘 다 구독, cam_0/cam_1 intrinsics 로드, "Initialized 2 camera exposure parameters" 등 정상 출력, 크래시 없음) 확인 — 단, 실제 두 카메라 영상 데이터로 VIO 전체 파이프라인을 끝까지 돌려보는 검증은 아직 못 함(2카메라 동시 bag/실물 데이터 없음, §8 데이터셋 준비와 함께 진행 예정) | Phase 1~3 | ✅ 완료 (2026-07-07). **알려진 제약**: `initializeVIO()`가 `width/height`를 `cams[0]`에서만 가져와 전 카메라에 재사용(Omni-LIVO 자체 설계) — 카메라 간 처리 해상도가 다르면(현재 dualcam config: cam1=1440x1080, cam2=720x540) `getImagePatch` 등에서 버퍼 오상=인덱싱 위험. §11 리스크에 추가. |
| 5 | 기존 launch 회귀 재확인 (Phase 4 반영 후) | - | Phase 4 | ✅ 완료 (단일카메라 launch 회귀 재검증으로 겸함) |
| 6 | §8 데이터셋 기반 검증 (bag 변환 → 2카메라 서브셋 실행 → 성공기준 확인). **업데이트**: Omni-LIVO 저자 데이터셋(Baidu Netdisk) 다운로드가 어려워, 사용자가 KITTI 등 대체 데이터셋을 검토 중 — 포팅(Phase 4) 완료 후 데이터셋 준비를 진행하기로 함. KITTI는 전방 스테레오 2대 구성으로 Omni-LIVO의 비중첩 십자 배열과 다르므로, cross-view migration 검증에는 적합하지 않을 수 있음(단순 2카메라 동시 처리 검증에는 사용 가능) — 데이터셋 확정 시 재검토 필요 | 검증 리포트 (궤적/컬러링 비교 스크린샷 or rosbag 지표) | Phase 4, 5 | 대기 |
| 7 | (후속) 실물 `camera2_ws` 연동 — cam2 캘리브레이션 선행 후 진행 | - | §7 블로커 해소, Phase 6 | 대기 |

---

## 10. 리스크 및 미결 사항

- ~~**[High] `vio.cpp` 조인트 ESIKF 이식은 단순 API 치환이 아니라 알고리즘 이식**이라 버그 위험이 크다~~ → Phase 4에서 완료. 실데이터(try_2 bag, N=1)로 150초간 크래시/에러 없이 정상 동작 확인. **2026-07-09 N=2 실물 하드웨어(cam1+cam2, 정지 상태)로 25초간 검증 완료** — 처음 시도에서 두 가지 실제 버그가 드러나 수정함: (1) Omni-LIVO 원본에도 있던 `resetGrid()` 미호출로 인한 SIGSEGV(+ grid 버퍼가 멀티카메라 크기로 안 잡히던 부수 결함), (2) `last_timestamp_img`를 카메라들이 공유해서 서로를 "시간 역행"으로 오판해 이미지가 아예 안 쌓이던 버그. 두 버그 수정 후 크래시 0, VIO 사이클 238회 정상 완료, sparse map 정상 성장, 궤적 NaN/Inf 없음 확인. **다만 정지 상태 테스트라 실제 이동 시 궤적/매핑 품질은 아직 미검증** — 다음 단계로 남음.
- **[High, 신규] 카메라 간 해상도 불일치** — Omni-LIVO의 `VIOManager::initializeVIO()`는 `width`/`height`를 `cams[0]`에서만 가져와 `getImagePatch` 등 전 카메라 이미지 버퍼 인덱싱에 그대로 재사용한다(Omni-LIVO 자체의 설계 제약이며 이번 포팅에서 새로 만든 문제는 아님). 현재 `camera_dualcam_cam1_cam2.yaml`은 cam1=1440x1080, cam2=720x540로 해상도가 다르므로, 실제 두 카메라 이미지가 함께 처리되는 순간(cam_id=1, 즉 cam2 처리 시) `width`(=1440, cam1 기준)로 720폭 이미지 버퍼를 인덱싱해 메모리 오상=크래시/오염 가능성이 있다. **실물 하드웨어 검증(§7/§8) 전에 반드시 해결**해야 함 — 옵션: (a) cam2를 cam1과 동일 해상도로 처리하도록 config 조정, (b) cam1을 720x540로 다운스케일, (c) `width`/`height`를 카메라별로 쓰도록 코드 일반화(Omni-LIVO 원본과 달라짐, 신중히 검토 필요). 초기화/구독만 하는 스모크테스트에서는 이미지가 실제로 안 들어오므로 드러나지 않았음.
- **[Medium] Livox CustomMsg 변환**이 `rosbags` 라이브러리로 매끄럽게 될지 사전 검증 필요 (커스텀 메시지 등록 방식이 버전마다 API가 다를 수 있음). 실패 시 대안: Docker로 ROS1 Noetic 컨테이너를 띄우고 그 안에서 `rosbag`→ CSV/PCD/PNG로 원시 추출 후 ROS2 쪽에서 직접 퍼블리셔 스크립트로 재생하는 방법도 있음(더 번거롭지만 확실함).
- **[결정 필요] 카메라 2대 선택 조합** — Front+Rear(대향) vs Front+Left(인접) 중 어느 쪽을 1차 검증 기준으로 삼을지 §8.2에서 Front+Rear를 기본값으로 제안했으나, 데이터셋의 실제 시퀀스 내용(어느 방향에 특징점이 풍부한지)에 따라 조정 가능.
- **[참고] Phase 4에서 Omni-LIVO의 `updateState`(순방향 조인트 ESIKF)는 원본 FAST-LIVO2와 달리 6-DOF 자세 보정만 `state->cov` 자세 블록으로 풀고(`K_pose`), velocity/bias/gravity는 이 EKF 업데이트에서 직접 갱신하지 않는다(원본은 `G` 풀-스테이트 게인으로 전체 상태에 보정을 전파). 이는 Omni-LIVO 원본 그대로 이식한 의도적 차이이며, 실데이터 회귀 테스트에서 궤적이 기존과 아주 근접하되 완전히 동일하지는 않은 이유이기도 함(정상 동작 확인됨, 다만 참고차 기록).

---

## 11. 핵심 참고 파일 인덱스

```
[포팅 대상 - fhd_fast_tri_ws]
src/FAST-LIVO2/include/common_lib.h        MeasureGroup, StatesGroup
src/FAST-LIVO2/include/vio.h / src/vio.cpp VIOManager (최대 작업량)
src/FAST-LIVO2/include/LIVMapper.h / src/LIVMapper.cpp  노드/토픽/동기화
src/FAST-LIVO2/include/frame.h / src/frame.cpp
src/FAST-LIVO2/include/feature.h
src/FAST-LIVO2/include/visual_point.h / src/visual_point.cpp
src/rpg_vikit/vikit_ros/src/camera_loader.cpp   기존 버그있는 벡터 로더
src/rpg_vikit/vikit_ros/include/vikit/camera_loader.h
src/FAST-LIVO2/config/extrin_cam{1,2,3}.yaml    기존 단일교체형 스캐폴딩(참고용, 유지)
src/FAST-LIVO2/launch/mapping_mid360s_cam{1,2,3}.launch.py  (참고용, 유지)

[참조 구현 - Omni-LIVO]
Omni-LIVO/include/vio.h / src/vio.cpp       벡터화 VIOManager, 조인트 ESIKF, cross-camera 제약
Omni-LIVO/include/LIVMapper.h / src/LIVMapper.cpp   멀티토픽 구독/동기화/XmlRpc 파싱
Omni-LIVO/include/frame.h / src/frame.cpp
Omni-LIVO/include/feature.h
Omni-LIVO/include/visual_point.h / src/visual_point.cpp   CrossCameraData
Omni-LIVO/config/mid360.yaml / mid360_cam.yaml   4카메라 config 스키마 예시
Omni-LIVO/config/NCD1.yaml / NCD4.yaml           카메라 일부 비활성화(주석처리) 실례
Omni-LIVO/README.md                              데이터셋 다운로드/센서 스펙

[실물 카메라 드라이버 - camera2_ws]
src/hik_camera_ros2_driver/src/hik_camera_node.cpp
src/hik_camera_ros2_driver/launch/hik_camera_dual_launch.py
src/hik_camera_ros2_driver/config/camera_params_cam{1,2}.yaml
src/hik_camera_ros2_driver/config/camera_info_cam{1,2}.yaml   (cam2 미캘리브레이션 확인)
```
