commit 82d3725779d377d8d927e5d5d42d24ea3c45af39 Author: hjkim Date: Fri Aug 7 14:09:06 2026 +0900 Initial import: fast_dual_ws (renamed to fhd_fast_tri_ws) FAST-LIVO2 dual/triple-camera VIO-LIO mapping workspace (ROS2 Humble), including the N-camera port from Omni-LIVO, rpg_vikit multi-camera parameter loader, and the um982_driver/gnss_comm packages staged here ahead of RTK fusion work. FAST-LIVO2 and rpg_vikit were previously separate git clones tracking their own GitHub history (Robotic-Developer-Road org, humble/main branches) — that history is preserved locally in src/{FAST-LIVO2,rpg_vikit}/.git-github-backup (not pushed here) and these two are now tracked flat as part of this repo going forward. diff --git a/.claude/settings.local.json b/.claude/settings.local.json new file mode 100644 index 0000000..4fa6c45 --- /dev/null +++ b/.claude/settings.local.json @@ -0,0 +1,52 @@ +{ + "permissions": { + "allow": [ + "Bash(ls /home/gardentech/ | grep -i bag; find /home/gardentech -maxdepth 3 -name \"*.db3\" -o -name \"rosbag*\" -type d 2>/dev/null | head -10)", + "Read(//home/gardentech/**)", + "Bash(python3 -)", + "Bash(python3 -c \"import ast; ast.parse\\(open\\('/home/gardentech/calib_ws/scripts/interactive_calib.py'\\).read\\(\\)\\); print\\('Syntax OK'\\)\")", + "Bash(ros2 topic *)", + "Bash(wait)", + "Bash(python3 -c \"import ast; ast.parse\\(open\\('/home/gardentech/calib_ws/scripts/interactive_calib.py'\\).read\\(\\)\\); ast.parse\\(open\\('/home/gardentech/calib_ws/scripts/auto_tune.py'\\).read\\(\\)\\); print\\('Both OK'\\)\")", + "Bash(ros2 node *)", + "Bash(ros2 run *)", + "Bash(find /home/gardentech -name \"preprocess\" -type f 2>/dev/null; find /opt/ros -name \"preprocess\" -type f 2>/dev/null)", + "Read(//opt/ros/**)", + "Bash(/home/gardentech/calib_ws/build/direct_visual_lidar_calibration/preprocess --help)", + "Bash(ros2 bag *)", + "Bash(xargs ls -la)", + "WebFetch(domain:koide3.github.io)", + "Bash(python3 -m json.tool)", + "Bash(snap list *)", + "Bash(flatpak list *)", + "Read(//snap/**)", + "Bash(apt list *)", + "Bash(nvcc --version)", + "Bash(nvidia-smi)", + "Read(//usr/local/cuda/**)", + "Read(//usr/local/**)", + "Bash(pip3 show *)", + "Bash(ls /usr/local/ | grep -i cuda; cat /proc/driver/nvidia/version 2>/dev/null | head -3)", + "Read(//proc/driver/nvidia/**)", + "WebFetch(domain:github.com)", + "WebFetch(domain:raw.githubusercontent.com)", + "WebSearch", + "Bash(python3 ~/calib_ws/install/direct_visual_lidar_calibration/lib/direct_visual_lidar_calibration/find_matches_superglue.py --help)", + "Bash(python3 -c ' *)", + "Bash(ros2 --version)", + "Bash(printenv)", + "Bash(uname -m && cat /etc/os-release | grep -E \"NAME|VERSION\" | head -4)", + "Read(//usr/lib/**)", + "Bash(dpkg -l)", + "Bash(pkg-config --modversion eigen3)", + "Bash(pkg-config --modversion fmt)", + "Read(//usr/**)", + "Bash(pkg-config --exists sophus)", + "Bash(find /home/gardentech -name \"livox_ros_driver2\" -type d 2>/dev/null | head -5 && find /opt -name \"livox*\" -type d 2>/dev/null | head -5 && ls /home/gardentech/ 2>/dev/null)", + "Read(//opt/**)", + "Bash(python3 -c \"import PyQt5; print\\('PyQt5 ok'\\)\")", + "Bash(python3 -c \"import cv2; print\\('cv2 ok'\\)\")", + "Bash(python3 -c \"import rclpy; print\\('rclpy ok'\\)\")" + ] + } +} diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..eb12844 --- /dev/null +++ b/.gitignore @@ -0,0 +1,17 @@ +# colcon build artifacts — regenerate with `colcon build` +build/ +install/ +log/ + +# local-only backup of FAST-LIVO2/rpg_vikit's original GitHub .git history — +# kept on disk for reference, not pushed here (108MB+, and not useful as +# plain files inside another repo's git objects) +.git-github-backup/ + +# Python +__pycache__/ +*.pyc + +# editors +.vscode/ +*.swp diff --git a/docs/Jetson-이관가이드.md b/docs/Jetson-이관가이드.md new file mode 100644 index 0000000..3ac07ad --- /dev/null +++ b/docs/Jetson-이관가이드.md @@ -0,0 +1,192 @@ +# Jetson 이관 가이드 — fhd_fast_tri_ws / 캘리브레이션 툴 / 녹화 툴 + +> 이 문서가 다루는 범위: **매핑**(`~/fhd_fast_tri_ws` — 3카메라 확장판, 이관 대상), **캘리브레이션** +> (`~/dvlc_gui` + `~/direct_visual_lidar_calibration` + `~/dvlc_ws`), **녹화**(`~/scan_web` + +> `~/camera2_ws` + `~/lidar2_ws` + `~/rtk_ws`) — 이 프로젝트에서 개발한 전체 파이프라인을 +> 최종적으로 이 x86_64 데스크탑에서 Jetson(ARM64)으로 옮길 때 담당자가 확인해야 할 것들이다. +> +> **`~/fast_ws`는 이관 대상에서 제외한다** (2026-08-05 사용자 결정) — `fhd_fast_tri_ws`로 3카메라 +> 확장되기 이전의 구버전 저장소이며, 담고 있던 녹화 GUI(`scan_gui*.py`)도 `scan_web`으로 대체됐다. +> `fast_ws`가 하던 유일한 실질적 역할(라이다 원시 드라이버 실행)도 사실 자체 구현이 아니라 +> `lidar2_ws`를 언더레이로 체이닝해서 쓴 것뿐이었다(§3에서 확인) — 그래서 `scan_web`도 이번에 +> `fast_ws`를 거치지 않고 `lidar2_ws`를 직접 source하도록 고쳤다(`config.py`/`run_scan_web.sh`). +> **즉 Jetson으로 옮기는 매핑 엔진은 `fhd_fast_tri_ws`(3카메라)이고, `fast_ws`는 옮길 필요도 없다.** +> +> **아래 사실관계는 실제로 이 머신을 조사해서 확인한 것이다** (2026-08-05 기준). "일반적으로 +> 고려할 사항"이라고 표시된 항목만 이 머신에서 직접 확인하지 않은 일반론이니, 그 부분은 Jetson +> 현장에서 재확인 필요. + +--- + +## 0. 제일 먼저 알아야 할 것: "통째로 옮긴다"가 위험한 이유 + +**"파일을 통째로 옮긴다"는 접근은 아래 두 가지 이유로 그대로 하면 안 된다.** + +1. **`build/`, `install/`, `log/` 는 x86_64 바이너리다.** 각 ROS2 워크스페이스(`camera2_ws`, + `lidar2_ws`, `dvlc_ws`, `fhd_fast_tri_ws`, `rtk_ws`)의 `build/`/`install/` 디렉터리 안 + 실행파일/라이브러리는 이 PC의 x86_64용으로 컴파일된 것이라 **Jetson(ARM64)에서 그대로 실행 + 안 된다.** 옮겨봐야 못 쓰는 죽은 용량이다. → **`src/`만 옮기고, `build/`·`install/`·`log/`는 + Jetson에서 `colcon build`로 새로 빌드한다.** +2. **일부 소스가 git clone인데 로컬 수정사항이 있다** (§3 참고) — 파일 복사가 아니라 실수로 + `git clone`을 새로 받으면 그 수정사항이 통째로 날아간다. + +즉 실제로 옮길 대상은: **`fhd_fast_tri_ws`, `camera2_ws`, `lidar2_ws`, `dvlc_ws`, `rtk_ws`의 `src/` +디렉터리**, config YAML/JSON, `dvlc_gui`/`scan_web` 전체(빌드 산출물이 없는 순수 Python/HTML), +`direct_visual_lidar_calibration` 전체(git repo). **`fast_ws`는 통째로 제외.** + +--- + +## 1. Jetson OS/JetPack 버전 — 제일 먼저 확인 + +이 PC는 **Ubuntu 22.04.5 LTS (Jammy) + ROS 2 Humble**(`ros-humble-desktop`, apt, amd64), +Python 3.10.12다. + +ROS2 Humble을 표준 apt 레포로 그대로 설치하려면 Jetson도 **Ubuntu 22.04 기반이어야 한다 — +즉 JetPack 6.x**. **JetPack 5.x(Ubuntu 20.04)라면 표준 레포에 Humble이 없고 Foxy/Galactic으로 +버전이 어긋난다** — 이 경우 소스 호환성부터 다시 검토해야 하는 큰 작업이 되므로, **이관 착수 전에 +Jetson에 JetPack 6.x가 올라가 있는지(또는 올릴 수 있는지)부터 확인**할 것. 이게 안 맞으면 나머지 +항목은 다 무의미해진다. + +--- + +## 2. 하드코딩된 `/home/gardentech` 경로 + +**좋은 소식**: `camera2_ws`, `lidar2_ws`, `dvlc_ws`, `dvlc_gui`, `direct_visual_lidar_calibration`, +`fhd_fast_tri_ws`, `rtk_ws`, `rtk` 는 하드코딩된 절대경로가 **하나도 없다** (`dvlc_calib_gui.py`는 +이미 `Path.home()`을 씀 — 그대로 옮겨도 됨). + +**확인 필요한 것**: `scan_web`의 `open_scan_web.sh`/`run_scan_web.sh`와 `~/Desktop/*.desktop` +런처 5개(`Exec=` 줄)에 `/home/gardentech`가 문자 그대로 박혀있다 — Jetson 계정명이 `gardentech`가 +아니면 깨진다. (구버전 `fast_ws`의 하드코딩 파일들은 이관 제외 대상이라 더 이상 고려 안 해도 됨.) + +**해결책 둘 중 하나:** +- **(권장) Jetson에도 계정명을 `gardentech`로 만든다** — 위 파일들을 하나도 안 고쳐도 됨, 가장 + 안전하고 빠름. +- 계정명을 다르게 써야 한다면, 위 파일들에서 `/home/gardentech`를 새 경로로 전부 치환 + (`grep -rl "/home/gardentech" <파일들> | xargs sed -i 's|/home/gardentech|/home/|g'` + 식으로 하되, 치환 후 각 스크립트를 다시 열어서 확인할 것 — 자동 치환만 믿지 말 것). + +--- + +## 3. Git 관련 위험 — 이 항목이 제일 중요함 + +**`dvlc_gui`와 `scan_web`(이번 세션에서 만든 것 전부)은 git 저장소가 아예 아니다** — 버전관리 +이력이 전혀 없다. 파일 복사로만 존재하는 상태라, 이관 중 실수로 덮어쓰면 되돌릴 방법이 없다. +**이관 전에 최소한 `git init` + 첫 커밋** 해두는 걸 권장 — 이관 작업 자체의 안전망도 되고, Jetson +쪽에서도 이후 변경사항을 추적할 수 있게 됨. + +**`fhd_fast_tri_ws/src` 안의 `FAST-LIVO2`, `rpg_vikit`은 git clone인데, 커밋 안 된 로컬 수정사항이 +있다:** +- `FAST-LIVO2`: **34개 파일** 수정됨 +- `rpg_vikit`: 4~5개 파일 수정됨 + +(참고: `fast_ws/src`에도 같은 이름의 `FAST-LIVO2`/`rpg_vikit` 사본이 있었지만 **서로 다른 +수정사항**이 적용돼 있었다 — `fast_ws`가 이관 대상에서 빠지면서 이 혼동 자체가 사라졌다. Jetson엔 +`fhd_fast_tri_ws`쪽 사본만 옮기면 되고, 절대 `fast_ws` 사본과 섞지 말 것.) + +**⚠️ 절대 하면 안 되는 것**: Jetson에서 편하게 하려고 `git clone https://github.com/.../FAST-LIVO2` +를 새로 받는 것. 그러면 이 34개 파일의 로컬 수정사항이 전부 사라지고, 지금 이 PC에서 검증한 동작과 +다르게 빌드됨 — 왜 안 되는지 원인 파악도 어려워짐. **반드시 `rsync`/`tar`로 파일 그대로 복사할 것.** + +`direct_visual_lidar_calibration`만 유일하게 완전히 깨끗한 git repo(106 커밋, 미커밋 변경 0) — +이건 그냥 정상적으로 clone하거나 복사해도 무방. + +--- + +## 4. 빌드 순서 + +`fast_ws`가 빠지면서 예전에 있던 "`lidar2_ws`를 먼저 빌드해야 `fast_ws`가 동작한다"는 의존성 +문제 자체가 없어졌다. 다만 **`lidar2_ws`는 여전히 이관 대상이다** — `scan_web`과 `dvlc_gui` 둘 다 +라이다 원시 드라이버(`livox_ros_driver2`) 실행을 위해 `lidar2_ws`를 직접 source한다 +(`scan_web/backend/config.py`의 `LIDAR2_WS_SETUP`, `dvlc_calib_gui.py`의 `ENV_SETUP`). + +권장 빌드 순서(의존관계상 안전한 순서, 엄격한 강제 순서는 아님): `lidar2_ws` → `camera2_ws` → +`dvlc_ws` → `fhd_fast_tri_ws` → `rtk_ws`. 각 워크스페이스에서 `build/`·`install/`·`log/` 삭제 후 +Jetson에서 `colcon build`로 새로 빌드(x86_64 → ARM64이므로 기존 산출물은 재사용 불가). + +--- + +## 5. Hikvision 카메라 SDK (`/opt/MVS`) — 반드시 별도 설치 + +`hik_camera_ros2_driver`는 `CMakeLists.txt`에서 `/opt/MVS`에 설치된 Hikvision **MVS SDK**의 +`MvCameraControl` 라이브러리를 링크한다 (`package.xml`엔 안 잡히는 순수 CMake `find_library`라 +빌드 전엔 존재조차 드러나지 않음 — 놓치기 쉬움). + +이 PC의 `/opt/MVS`는 **x86_64용 빌드**다. Hikvision이 ARM64(Jetson)용 MVS SDK를 별도로 배포하니, +**Jetson에는 그 ARM64 버전을 따로 받아서 `/opt/MVS`에 설치**해야 한다 — x86_64 SDK를 그대로 +옮기면 카메라 드라이버가 아예 빌드조차 안 되거나, 빌드는 돼도 런타임에 심볼 로드가 실패한다. +(SDK 다운로드는 Hikvision 머신비전 사이트/영업 채널에서 시리얼/모델 기준으로 받아야 함 — 이 부분은 +담당자가 별도로 확인.) + +--- + +## 6. Livox LiDAR 네트워크 설정 + +`lidar2_ws/src/livox_ros_driver2/config/MID360_config.json`에 **고정 IP**가 박혀있다: +- `host_net_info`(이 PC) = `192.168.1.5` +- LiDAR = `192.168.1.192` + +인터페이스 이름이 아니라 IP만 보므로 NIC 이름이 달라도 상관없지만(Jetson이 보통 데스크탑과 다른 +이더넷 인터페이스 이름을 씀 — 그건 문제 안 됨), **Jetson 쪽에서 라이다와 연결되는 이더넷 포트를 +정적 IP `192.168.1.5`로 설정**해야 하고(안 그러면 이 config를 고쳐야 함), 라이다가 여전히 +`192.168.1.192`로 응답하는지도 실물 연결해서 확인 필요. + +--- + +## 7. PyTorch / SuperGlue / CUDA — 이 PC와 정반대 상황이 됨 + +**이 PC는 NVIDIA GPU가 전혀 없다** (`nvidia-smi` 자체가 없음) — 설치된 `torch`는 CPU 전용 빌드고 +`cuda.is_available()`이 `False`다. 캘리브레이션 GUI에서 SuperGlue의 "force_cpu" 체크박스가 +기본으로 켜져있는 이유가 바로 이거다 — GPU가 없어서 옵션이 아니라 필수였던 것. (`requirements.txt`엔 +`torch>=1.1.0`이라고만 느슨하게 박혀있음.) + +**Jetson은 반대로 CUDA GPU가 있는 게 보통 쓰는 이유**인데, 여기서 함정: **PyPI의 일반 +`pip install torch`는 Jetson의 aarch64+Tegra CUDA 조합을 지원하지 않는다.** 그대로 설치하면 +CPU 전용으로 깔리거나(이 PC와 같은 상태로 퇴화) 아예 설치가 안 된다. **NVIDIA가 배포하는 +Jetson 전용 PyTorch wheel(JetPack/L4T 버전에 맞는 것, Jetson Zoo 등)을 따로 설치**해야 GPU를 +실제로 쓸 수 있다 — 설치 방법이 데스크탑과 완전히 다르므로 별도로 검색/확인 필요. +**(일반적으로 고려할 사항)** GPU를 제대로 물리면 SuperGlue를 TensorRT로 변환해서 쓰는 게 순수 +PyTorch 추론보다 Jetson에서 훨씬 빠른 경우가 많음 — 성능이 아쉬우면 고려해볼 것. + +--- + +## 8. 녹화 데이터(`~/bags`, `~/dvlc_data`) — 옮길지 판단 필요 + +홈 디렉터리 전체가 89G인데, 그중 **`~/bags`가 68G, `~/dvlc_data`가 9G**로 대부분이 코드가 아니라 +그동안 테스트로 찍은 실측 데이터다. Jetson으로 코드만 옮기는 거라면 이 데이터까지 통째로 옮길 +필요는 보통 없다(저장공간도 아깝고) — 다만 SuperGlue 매칭 결과나 캘리브레이션 완료된 `calib.json` +등 **앞으로도 계속 쓸 참조 데이터**는 선별해서 옮기는 게 맞다. 담당자가 어떤 bag/캘리브레이션 +결과를 계속 쓸지 판단해서 고를 것. + +**참고**: 캘리브레이션 결과(`calib.json`의 `T_lidar_camera` 외부 파라미터)는 컴퓨팅 플랫폼이 아니라 +**물리적인 라이다-카메라 장착 형상에 종속**된다 — 같은 실물 리그(카메라/라이다를 같은 위치에 같은 +자세로 재장착)라면 Jetson으로 옮긴 뒤 재캘리브레이션할 필요 없이 기존 `calib.json`을 그대로 써도 됨. + +--- + +## 9. 일반적으로 고려할 사항 (이 머신에서 직접 확인 안 한 항목 — Jetson 현장에서 재확인) + +- **USB/시리얼 권한**: GNSS(`/dev/ttyUSB0`) 접근을 위한 `dialout` 그룹 가입, udev 규칙 등은 + Jetson 쪽 사용자 계정에 다시 설정해야 함 (SCAN-운용가이드.md §1.1 참고). +- **전력/발열**: Jetson 보드는 전력 예산이 데스크탑보다 빠듯함 — 라이다+카메라 3대+SLAM을 동시에 + 장시간 돌릴 때 스로틀링/발열 여유가 있는지 필드 조건에서 실측 확인 권장. +- **저장공간**: Jetson 기본 내장 저장장치(eMMC)가 이 PC보다 훨씬 작을 수 있음 — scan_web의 + 디스크 여유공간 경고 임계값(`backend/config.py`의 `DISK_LOW_WARNING_GB`/`DISK_LOW_DANGER_GB`, + 현재 10GB/2GB)이 Jetson 저장장치 크기 기준으로 여전히 적절한지 재검토. +- **연산 성능**: 카메라 3대 VIO + LiDAR-Inertial 오도메트리가 Jetson에서 이 PC와 동등한 실시간 + 성능이 나오는지는 실측 전엔 알 수 없음 — 필요시 해상도/fps 하향, voxel filter 크기 조정 등으로 + 튜닝 여지를 열어둘 것. + +--- + +## 10. 이관 후 검증 + +기본적으로 `~/scan_web/docs/운용가이드.md` §4의 **실기 테스트 체크리스트**를 Jetson에서 그대로 +한 번 더 돌리는 게 제일 확실하다. 추가로 이관 직후 특별히 확인할 것: + +- [ ] 각 워크스페이스 `colcon build` 성공 (§4 순서 — `fast_ws`는 이제 없음) +- [ ] `hik_camera_ros2_driver` 빌드 시 `/opt/MVS`(ARM64판) 링크 성공 (§5) +- [ ] `ros2 topic list`로 `/livox/lidar` 등 기대 토픽이 실제로 뜨는지 (IP 설정, §6) +- [ ] SuperGlue 실행 시 `torch.cuda.is_available()`이 Jetson에서 `True`로 뜨는지 (GPU 실제 사용 여부, §7) +- [ ] `~/scan_web`의 하드코딩 경로(§2)가 실제 Jetson 계정명과 맞는지 +- [ ] 기존 `calib.json` 재사용 시 리그 형상이 실제로 동일한지 육안 확인 (§8) diff --git a/docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md b/docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md new file mode 100644 index 0000000..6cb78a4 --- /dev/null +++ b/docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md @@ -0,0 +1,282 @@ +# 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 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 cams`, `vector Rci_vec/Rcl_vec/Rcw_vec`, `vector Pci_vec/...`, 카메라별 grid/voxel 후보 리스트, `CameraPhotoParams`(카메라별 노출/비네팅) 보유 | +| 조인트 ESIKF | 카메라별 patch residual을 각각 계산한 뒤 **하나의 Jacobian/residual로 병합**해 한 번에 풀이 (`vio.cpp:2429-2717`), 열 구성 `6(pose) + num_cams(노출)`, `addCrossCameraConsistencyConstraint`로 카메라 간 동일 3D점의 광도 일관성 추가 제약 | +| Frame/Feature | `Frame`이 `vector`+`vector 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//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 imgs` (Omni-LIVO `common_lib.h:61-84` 그대로). +- `StatesGroup`에 카메라별 노출시간 `std::vector inv_expo_time_per_cam` 추가 (고정 `DIM_STATE=19` 외부에 별도 보관, Omni-LIVO 방식 그대로 — EKF 상태 벡터 자체의 차원은 불변으로 유지해 기존 LIO 코드에 영향 없음). +- tf 관련 include는 fhd_fast_tri_ws가 이미 tf2로 되어 있으면 그대로 둔다 (Omni-LIVO의 `#include `는 가져오지 않음). + +### 4.2 `include/frame.h` / `src/frame.cpp` +- `vk::AbstractCamera *cam_` → `std::vector cams_`. +- `SE3 T_f_w_` → `std::vector T_f_w_` (+ `T_f_w_prior_`), 카메라별 고정 외부파라미터로 하나의 바디 자세에서 파생. +- `cv::Mat img_` → `std::vector imgs_` (+ `imgs_shared_`). +- 생성자 시그니처를 `Frame(const std::vector&, std::vector&, 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`)는 이미 다중 관측을 지원하는 구조이므로 변경 없음 — 이 부분이 fhd_fast_tri_ws에서 가장 재사용하기 쉬운 기존 자산. + +### 4.5 `include/vio.h` / `src/vio.cpp` (**가장 큰 작업, ~2000줄**) +- 단일 멤버 → 벡터 멤버 전면 교체: + - `cam` → `vector cams` + - `Rci, Pci, Rcl, Pcl, Rcw, Pcw` → `vector Rci_vec/Rcl_vec/Rcw_vec`, `vector Pci_vec/Pcl_vec/Pcw_vec` + - `fx,fy,cx,cy,width,height` → 카메라별 배열 또는 `cams[i]`에서 직접 조회 + - grid 상태(`grid_num, map_index, map_dist, scan_value` 등) → `vector> ..._per_cam_` + - `img_cp, img_rgb` → 카메라별 컨테이너 + - `new_frame_`는 단일 `Frame`을 유지하되, `Frame` 자체가 다중 카메라 이미지를 보유하도록 함(4.2 참고) + - `CameraPhotoParams`(카메라별 노출/비네팅 보정) 신설 +- `processFrame`을 `processFrame(std::vector &imgs, ...)` 시그니처로 변경, 내부에서 카메라별 patch selection → 통합 Jacobian 구성 → `addCrossCameraConsistencyConstraint`(동일 3D점의 카메라 간 광도 일관성 제약) → 1회 EKF 풀이 → 카메라별 참조 patch 갱신 순서로 이식. +- `feat_map`(`unordered_map`)은 카메라 무관 세계좌표 구조이므로 **그대로 유지** — 이 부분은 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::SharedPtr> sub_img_list`, 각각 `std::bind`로 `cam_id` 캡처(Omni-LIVO의 람다 패턴을 `rclcpp` 구독 생성 구문에 맞게 그대로 적용 — ROS1/ROS2 구독 API 차이만 있을 뿐 로직은 동일). +- `img_buffer`/`img_time_buffer`(단일) → `vector> img_buffers`, `vector> 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` | 이미 `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`은 `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 미캘리브레이션 확인) +``` diff --git a/docs/SCAN-운용가이드.md b/docs/SCAN-운용가이드.md new file mode 100644 index 0000000..ac038cd --- /dev/null +++ b/docs/SCAN-운용가이드.md @@ -0,0 +1,120 @@ +# 스캔 → 매핑 운용 가이드 (녹화: 트리플 카메라 GUI / 처리: fhd_fast_tri_ws) + +> 전체 파이프라인: **녹화**는 `~/fast_ws` 의 스캔 GUI(+`~/rtk_ws` UM982 드라이버)로, **매핑(SLAM)** +> 은 이 문서가 있는 `~/fhd_fast_tri_ws` (FAST-LIVO2, 듀얼/트리플 카메라 지원 빌드)로 처리한다. +> 녹화 도구와 처리 애플리케이션이 서로 다른 워크스페이스이므로 이 문서는 `fhd_fast_tri_ws/docs/` +> 에 둔다 (`fast_ws` 는 녹화 GUI 전용 워크스페이스라 전체 파이프라인 문서를 두기에 맞지 않음). + +--- + +## 0. 워크스페이스 역할 정리 + +| 워크스페이스 | 역할 | 비고 | +|---|---|---| +| `~/rtk_ws` | UM982 RTK GNSS 드라이버(파싱+NTRIP+`/ublox_driver/receiver_pvt` 발행) | 녹화 전용 최소 빌드 | +| `~/fast_ws` | LiDAR + cam1/cam2/cam3 시동 및 `ros2 bag record` GUI (`scan_gui_triple.py`) | `rtk_ws`/`camera2_ws` 를 함께 source | +| `~/fhd_fast_tri_ws` | **FAST-LIVO2 매핑 엔진**(dual/triple 카메라 VIO-LIO 빌드, `fastlivo_mapping`) | 녹화된 bag 을 재생해서 SLAM 돌리는 애플리케이션 | +| `~/FAST-LIVO2-RTK-ROS2` | GNSS-RTK 융합이 포함된 별도 FAST-LIVO2 소스(옵티마이저에 `gpsHandler` 있음) | 아직 `fhd_fast_tri_ws` 에 통합 안 됨 — §4 참고 | + +> ⚠️ **중요**: `fhd_fast_tri_ws/src` 에 `gnss_comm`/`um982_driver` 패키지가 같이 들어있지만, +> 현재 `fhd_fast_tri_ws` 의 `fast_livo` (`package.xml`, `LIVMapper.cpp` 등)는 **GNSS 토픽을 +> 구독하지 않는다** (`gnss_comm`/`GnssPVTSolnMsg` 참조 없음). 즉 녹화 bag 에 `/ublox_driver/receiver_pvt` +> 를 같이 담아도 지금의 `fhd_fast_tri_ws` 매핑에는 **아직 반영되지 않는다** — 순수 LiDAR-Inertial-Visual +> 매핑만 수행된다. GNSS 융합이 필요하면 `~/FAST-LIVO2-RTK-ROS2` 통합 작업이 별도로 필요하다. + +--- + +## 1부. 녹화 (fast_ws 스캔 GUI + rtk_ws UM982) + +### 1.1 사전 준비 +- LiDAR, cam1/2/3, UM982 USB 연결. UM982 안테나는 하늘이 트인 곳에 — 실내면 GNSS `No Fix`(SV 0) 가 정상. +- `/dev/ttyUSB0` 권한 확인: + ```bash + ls -la /dev/ttyUSB0 + # 그룹이 dialout 인데 미가입이면: + sudo usermod -aG dialout $USER # 이후 재로그인 필요(영구) + sudo chmod a+rw /dev/ttyUSB0 # 임시(재부팅/재연결 시 초기화) + ``` +- UM982 최초 설정(또는 FRESET 후): `rtk/config_all.py` 로 `BESTNAVB COM3 0.1`, `GPGGA COM3 1`, baud 460800, `MODE ROVER` 저장 확인. +- 터미널에 남아있는 `um982_driver_node` 프로세스가 있으면 포트를 선점해 GUI GPS 시작이 실패한다: + ```bash + ps aux | grep um982_driver_node | grep -v grep && kill -INT + ``` + +### 1.2 GUI 실행 및 녹화 +바탕화면 **"스캔 GUI (트리플 카메라)"** 아이콘 실행 → 좌측 패널 순서대로: + +1. **시동** — LiDAR 즉시 실행, 5초 후 cam1/cam2/cam3 자동 실행. 우측 미리보기 확인. +2. **GPS 시작** — 상태 라벨/좌표 갱신 확인. 색상 의미: + + | 라벨 | 의미 | + |---|---| + | ● No Fix | 위성 미포착 — 실내 정상 | + | ● GPS / ● DGPS | 단독/차분 측위, RTK 아님 | + | ● RTK Float | 보정 진행 중 | + | ● RTK Fixed | 최고 정밀도(cm급) — **녹화는 이 상태 권장** | + +3. **녹화** — 저장 경로/파일명 입력, **"녹화에 GPS 포함"** 체크(GPS 가 켜져 있어야 실제 포함됨) → 녹화 시작 → 종료 시 정지. + - 저장 위치: `~/bags/<파일이름>/<파일이름>_0.db3` + +### 1.3 녹화 검증 +```bash +source /opt/ros/humble/setup.bash +ros2 bag info ~/bags/<파일이름>/ +``` +- 토픽: `/livox/lidar`, `/livox/imu`, `/cam1/image`, `/cam2/image`, `/cam3/image`(+`camera_info` 3종), GPS 포함 시 `/ublox_driver/receiver_pvt` +- GNSS 메시지 수 ÷ Duration ≈ 10Hz 확인. + +--- + +## 2부. 재생 + 매핑 실행 (fhd_fast_tri_ws) + +### 2.1 빌드 확인 (최초 1회 / 소스 수정 시) +```bash +cd ~/fhd_fast_tri_ws +colcon build --packages-select fast_livo +source install/setup.bash +``` + +### 2.2 실행 (터미널 2개) +**터미널 A** — 매핑 노드 + RViz: +```bash +source /opt/ros/humble/setup.bash +source ~/fhd_fast_tri_ws/install/setup.bash +ros2 launch fast_livo mapping_mid360s_triplecam.launch.py use_rviz:=True +``` + +**터미널 B** — 녹화한 bag 재생 (스페이스바로 재생/일시정지): +```bash +source /opt/ros/humble/setup.bash +ros2 bag play -p ~/bags/<파일이름>/ +``` +- `-p` 는 일시정지 상태로 시작 → RViz 창에서 매핑 노드가 준비된 걸 확인한 뒤 스페이스바로 재생. +- 매핑 노드가 구독하는 토픽(`/livox/lidar`, `/livox/imu`, `/cam1/image`, `/cam2/image`, `/cam3/image`)은 + `scan_gui_triple.py` 녹화 토픽명과 동일하므로 리매핑 불필요. + +### 2.3 결과 확인 +- RViz(`fast_livo2.rviz`)에서 포인트클라우드 누적/궤적 확인. +- `/ublox_driver/receiver_pvt` 는 bag 에 있어도 매핑 결과에 영향 없음(§0 참고) — GNSS 데이터 자체 품질만 + 따로 보고 싶으면 별도로 `ros2 topic echo -b ...` 또는 재생 중 `ros2 topic echo /ublox_driver/receiver_pvt` 로 확인. + +--- + +## 3. 트러블슈팅 + +| 증상 | 원인 | 조치 | +|---|---|---| +| GPS 시작 직후 "GPS 없음"으로 복귀 / gnss.log 에 `시리얼 오픈 실패` | 권한 문제 또는 포트 중복 점유 | §1.1 권한/프로세스 확인 | +| 실내에서 계속 No Fix | 정상(위성 미포착) | 옥외/창가 이동, 그래도 안 되면 안테나 케이블 확인 | +| bag 재생해도 RViz 에 아무것도 안 뜸 | `-p` 로 일시정지 상태 → 스페이스바 안 누름, 또는 토픽명 불일치 | 스페이스바로 재생 시작, `ros2 bag info` 로 토픽명 재확인 | +| GNSS 데이터가 매핑에 반영 안 되는 것 같음 | **현재 `fhd_fast_tri_ws` 는 GNSS 미융합** (설계상 아직 없음) | §0 참고 — 필요 시 `FAST-LIVO2-RTK-ROS2` 통합 작업 별도 진행 | + +--- + +## 4. GNSS 융합이 필요해지면 + +`~/FAST-LIVO2-RTK-ROS2/FAST-LIVO2-RTK-ROS2` 소스에는 `optimization.cpp::gpsHandler` 등 GNSS-RTK +융합 로직이 이미 구현돼 있다(원본 `rtk_ws` 문서가 가리키던 "FAST-LIVO2-RTK 백엔드"가 이것). 현재는 +빌드된 워크스페이스가 아니라 압축 해제된 소스 상태([`__MACOSX`](../../FAST-LIVO2-RTK-ROS2) 잔재로 보아 +zip 압축 해제본). `fhd_fast_tri_ws` 의 dual/triple 카메라 지원과 이 GNSS 융합을 합치려면 두 소스를 +비교해 병합하는 별도 작업이 필요하다 — 착수 시 다시 요청. diff --git a/src/FAST-LIVO2/.gitignore b/src/FAST-LIVO2/.gitignore new file mode 100644 index 0000000..e7a814b --- /dev/null +++ b/src/FAST-LIVO2/.gitignore @@ -0,0 +1,2 @@ +Log/* +build/* diff --git a/src/FAST-LIVO2/CMakeLists.txt b/src/FAST-LIVO2/CMakeLists.txt new file mode 100755 index 0000000..2554d97 --- /dev/null +++ b/src/FAST-LIVO2/CMakeLists.txt @@ -0,0 +1,218 @@ +cmake_minimum_required(VERSION 3.10) +project(fast_livo) + +set(CMAKE_BUILD_TYPE "Release") +message(STATUS "Build Type: ${CMAKE_BUILD_TYPE}") + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# Set common compile options +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions") + +# Specific settings for Debug build +set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O0 -g") + +# Detect CPU architecture +message(STATUS "Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}") + +# Specific settings for Release build +if(CMAKE_SYSTEM_PROCESSOR MATCHES "^(arm|aarch64|ARM|AARCH64)") + if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64") + # 64-bit ARM optimizations (e.g., RK3588 and Jetson Orin NX) + set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -mcpu=native -mtune=native -ffast-math") + message(STATUS "Using 64-bit ARM optimizations: -O3 -mcpu=native -mtune=native -ffast-math") + else() + # 32-bit ARM optimizations with NEON support + set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -mcpu=native -mtune=native -mfpu=neon -ffast-math") + message(STATUS "Using 32-bit ARM optimizations: -O3 -mcpu=native -mtune=native -mfpu=neon -ffast-math") + endif() + add_definitions(-DARM_ARCH) +else() + # x86-64 (Intel/AMD) optimizations + set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native -mtune=native -funroll-loops") #-flto + message(STATUS "Using general x86 optimizations: -O3 -march=native -mtune=native -funroll-loops") + add_definitions(-DX86_ARCH) +endif() + +# Define project root directory +add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") + +# Detect CPU core count for potential multithreading optimization +include(ProcessorCount) +ProcessorCount(N) +message(STATUS "Processor count: ${N}") + +# Set the number of cores for multithreading +if(N GREATER 4) + math(EXPR PROC_NUM "4") + add_definitions(-DMP_EN -DMP_PROC_NUM=${PROC_NUM}) + message(STATUS "Multithreading enabled. Cores: ${PROC_NUM}") +elseif(N GREATER 1) + math(EXPR PROC_NUM "${N}") + add_definitions(-DMP_EN -DMP_PROC_NUM=${PROC_NUM}) + message(STATUS "Multithreading enabled. Cores: ${PROC_NUM}") +else() + add_definitions(-DMP_PROC_NUM=1) + message(STATUS "Single core detected. Multithreading disabled.") +endif() + +# Check for OpenMP support +find_package(OpenMP QUIET) +if(OpenMP_CXX_FOUND) + message(STATUS "OpenMP found") + add_compile_options(${OpenMP_CXX_FLAGS}) +else() + message(STATUS "OpenMP not found, proceeding without it") +endif() + +# Check for mimalloc support +find_package(mimalloc QUIET) +if(mimalloc_FOUND) + message(STATUS "mimalloc found") +else() + message(STATUS "mimalloc not found, proceeding without it") +endif() + +# Find ament and required dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(livox_ros_driver2 REQUIRED) +find_package(vikit_common REQUIRED) +find_package(vikit_ros REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_transport REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) +find_package(OpenCV REQUIRED) +find_package(Sophus REQUIRED) +# Support modern cmake Sophus target (ros-humble-sophus) which doesn't set Sophus_INCLUDE_DIRS +if(TARGET Sophus::Sophus AND NOT Sophus_INCLUDE_DIRS) + get_target_property(Sophus_INCLUDE_DIRS Sophus::Sophus INTERFACE_INCLUDE_DIRECTORIES) +endif() +find_package(Boost REQUIRED COMPONENTS thread) +find_package(fmt REQUIRED) +find_package(yaml-cpp REQUIRED) + +# Include directories for dependencies +include_directories( + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Sophus_INCLUDE_DIRS} + ${vikit_common_INCLUDE_DIRS} + ${vikit_ros_INCLUDE_DIRS} + include +) + +set(dependencies + rclcpp + rclpy + geometry_msgs + nav_msgs + sensor_msgs + visualization_msgs + cv_bridge + vikit_common + vikit_ros + image_transport + pcl_ros + pcl_conversions + tf2_ros + livox_ros_driver2 +) + +set(COMMON_DEPENDENCIES OpenMP::OpenMP_CXX fmt::fmt) + +# link_directories(${COMMON_DEPENDENCIES} +# ${vikit_common_LIBRARIES}/libvikit_common.so +# ${vikit_ros_LIBRARIES}/libvikit_ros.so +# ) + +# Add libraries +add_library(vio src/vio.cpp src/frame.cpp src/visual_point.cpp) +add_library(lio src/voxel_map.cpp) +add_library(pre src/preprocess.cpp) +add_library(imu_proc src/IMU_Processing.cpp) +add_library(laser_mapping src/LIVMapper.cpp) +add_library(utils src/utils.cpp) + +ament_target_dependencies(vio ${dependencies} ) +ament_target_dependencies(lio ${dependencies}) +ament_target_dependencies(pre ${dependencies}) +ament_target_dependencies(imu_proc ${dependencies}) +ament_target_dependencies(laser_mapping ${dependencies}) + +# linking libraries or executables to public dependencies +target_link_libraries(laser_mapping + ${CMAKE_SOURCE_DIR}/../../install/vikit_common/lib/libvikit_common.so + ${CMAKE_SOURCE_DIR}/../../install/vikit_ros/lib/libvikit_ros.so + ${COMMON_DEPENDENCIES} + yaml-cpp +) +target_link_libraries(vio ${COMMON_DEPENDENCIES}) +target_link_libraries(lio utils ${COMMON_DEPENDENCIES}) +target_link_libraries(pre ${COMMON_DEPENDENCIES}) +target_link_libraries(imu_proc ${COMMON_DEPENDENCIES}) + +# Add the main executable +add_executable(fastlivo_mapping src/main.cpp) + +ament_target_dependencies(fastlivo_mapping ${dependencies}) + +# Link libraries to the executable +target_link_libraries(fastlivo_mapping + laser_mapping + vio + lio + pre + imu_proc + ${PCL_LIBRARIES} + ${OpenCV_LIBRARIES} + ${Sophus_LIBRARIES} + ${Boost_LIBRARIES} +) + +# Link mimalloc if found +if(mimalloc_FOUND) + target_link_libraries(fastlivo_mapping mimalloc) +endif() + +# Install the executable +install(TARGETS + fastlivo_mapping + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY config launch rviz_cfg urdf + DESTINATION share/${PROJECT_NAME} +) + +# Export dependencies +ament_export_dependencies( + rclcpp + rclpy + geometry_msgs + nav_msgs + sensor_msgs + pcl_ros + pcl_conversions + tf2_ros + livox_ros_driver2 + Eigen3 + PCL + OpenCV + Sophus +) + +ament_package() \ No newline at end of file diff --git a/src/FAST-LIVO2/LICENSE b/src/FAST-LIVO2/LICENSE new file mode 100644 index 0000000..d159169 --- /dev/null +++ b/src/FAST-LIVO2/LICENSE @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/src/FAST-LIVO2/README.md b/src/FAST-LIVO2/README.md new file mode 100644 index 0000000..c2ec79d --- /dev/null +++ b/src/FAST-LIVO2/README.md @@ -0,0 +1,195 @@ +# FAST-LIVO2 ROS2 HUMBLE + +## FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry + +Thanks to hku mars lab chunran zheng for the open source excellent work + +### 📢 News + +- 🔓 **2025-01-23**: Code released! +- 🎉 **2024-10-01**: Accepted by **T-RO '24**! +- 🚀 **2024-07-02**: Conditionally accepted. + +### 📬 Contact + +For further inquiries or assistance, please contact [zhengcr@connect.hku.hk](mailto:zhengcr@connect.hku.hk). + +## 1. Introduction + +FAST-LIVO2 is an efficient and accurate LiDAR-inertial-visual fusion localization and mapping system, demonstrating significant potential for real-time 3D reconstruction and onboard robotic localization in severely degraded environments. + +**Developer**: [Chunran Zheng 郑纯然](https://github.com/xuankuzcr) + +
+ +
+ +### 1.1 Related video + +Our accompanying video is now available on [**Bilibili**](https://www.bilibili.com/video/BV1Ezxge7EEi) and [**YouTube**](https://youtu.be/6dF2DzgbtlY). + +### 1.2 Related paper + +[FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry](https://arxiv.org/pdf/2408.14035) + +[FAST-LIVO2 on Resource-Constrained Platforms](https://arxiv.org/pdf/2501.13876) + +[FAST-LIVO: Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry](https://arxiv.org/pdf/2203.00893) + +[FAST-Calib: LiDAR-Camera Extrinsic Calibration in One Second](https://www.arxiv.org/pdf/2507.17210) + +### 1.3 Our hard-synchronized equipment + +We open-source our handheld device, including CAD files, synchronization scheme, STM32 source code, wiring instructions, and sensor ROS driver. Access these resources at this repository: [**LIV_handhold**](https://github.com/xuankuzcr/LIV_handhold). + +### 1.4 Our associate dataset: FAST-LIVO2-Dataset +Our associate dataset [**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z) used for evaluation is also available online. + +### 1.5 Our LiDAR-camera calibration method +The [**FAST-Calib**](https://github.com/hku-mars/FAST-Calib) toolkit is recommended. Its output extrinsic parameters can be directly filled into the YAML file. + +### MARS-LVIG dataset +[**MARS-LVIG dataset**](https://mars.hku.hk/dataset.html)：A multi-sensor aerial robots SLAM dataset for LiDAR-visual-inertial-GNSS fusion + +## 2. Prerequisited + +### 2.1 Ubuntu and ROS + +Ubuntu 22.04. [ROS Installation](http://wiki.ros.org/ROS/Installation). + +### 2.2 PCL && Eigen && OpenCV + +PCL>=1.8, Follow [PCL Installation](https://pointclouds.org/). + +Eigen>=3.3.4, Follow [Eigen Installation](https://eigen.tuxfamily.org/index.php?title=Main_Page). + +OpenCV>=4.2, Follow [Opencv Installation](http://opencv.org/). + +### 2.3 Sophus + +#### Binary installation +```bash +sudo apt install ros-$ROS_DISTRO-sophus +``` + +#### Building from source +Sophus Installation for the non-templated/double-only version. + +```bash +git clone https://github.com/strasdat/Sophus.git +cd Sophus +git checkout a621ff +mkdir build && cd build && cmake .. +make +sudo make install +``` + +if build fails due to `so2.cpp:32:26: error: lvalue required as left operand of assignment`, modify the code as follows: + +**so2.cpp** +```diff +namespace Sophus +{ + +SO2::SO2() +{ +- unit_complex_.real() = 1.; +- unit_complex_.imag() = 0.; ++ unit_complex_.real(1.); ++ unit_complex_.imag(0.); +} +``` + +### 2.4 Vikit + +Vikit contains camera models, some math and interpolation functions that we need. Vikit is a catkin project, therefore, download it into your catkin workspace source folder. + +For well-known reasons, ROS2 does not have a direct global parameter server and a simple method to obtain the corresponding parameters. For details, please refer to https://discourse.ros.org/t/ros2-global-parameter-server-status/10114/11. I use a special way to get camera parameters in Vikit. While the method I've provided so far is quite simple and not perfect, it meets my needs. More contributions to improve `rpg_vikit` are hoped. + +```bash +# Different from the one used in fast-livo1 +cd fast_ws/src +git clone https://github.com/Robotic-Developer-Road/rpg_vikit.git +``` + +Thanks to the following repositories for the code reference: + +- [uzh-rpg/rpg_vikit](https://github.com/uzh-rpg/rpg_vikit) +- [xuankuzcr/rpg_vikit](https://github.com/xuankuzcr/rpg_vikit) +- [uavfly/vikit](https://github.com/uavfly/vikit) + +### 2.5 **livox_ros_driver2** + +Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2). + +why not use `livox_ros_driver`? Because it is not compatible with ROS2 directly. actually i am not think there s any difference between [livox ros driver](https://github.com/Livox-SDK/livox_ros_driver.git) and [livox ros driver2](https://github.com/Livox-SDK/livox_ros_driver2.git) 's `CustomMsg`, the latter 's ros2 version is sufficient. + +## 3. Build + +Clone the repository and colcon build: + +``` +cd ~/fast_ws/src +git clone https://github.com/Robotic-Developer-Road/FAST-LIVO2.git +cd ../ +colcon build --symlink-install --continue-on-error +source ~/fast_ws/install/setup.bash +``` + +## 4. Run our examples + +Download our collected rosbag files via OneDrive ([**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z)). + +### convert rosbag + +convert ROS1 rosbag to ROS2 rosbag +```bash +pip install rosbags +rosbags-convert --src Retail_Street.bag --dst Retail_Street +``` +- [gitlab rosbags](https://gitlab.com/ternaris/rosbags) +- [pypi rosbags](https://pypi.org/project/rosbags/) + +### change the msg type on rosbag + +Such as dataset `Retail_Street.db3`, because we use `livox_ros2_driver2`'s `CustomMsg`, we need to change the msg type in the rosbag file. +1. use `rosbags-convert` to convert rosbag from ROS1 to ROS2. +2. change the msg type of msg type in **metadata.yaml** as follows: + +**metadata.yaml** +```diff +rosbag2_bagfile_information: + compression_format: '' + compression_mode: '' + custom_data: {} + duration: + nanoseconds: 135470252209 + files: + - duration: + nanoseconds: 135470252209 + message_count: 30157 + path: Retail_Street.db3 + .............. + topic_metadata: + name: /livox/lidar + offered_qos_profiles: '' + serialization_format: cdr +- type: livox_ros_driver/msg/CustomMsg ++ type: livox_ros_driver2/msg/CustomMsg + type_description_hash: RIHS01_94041b4794f52c1d81def2989107fc898a62dacb7a39d5dbe80d4b55e538bf6d + ............... +..... +``` + +### Run the demo + +Do not forget to `source` your ROS2 workspace before running the following command. + +```bash +ros2 launch fast_livo mapping_aviz.launch.py use_rviz:=True +ros2 bag play -p Retail_Street # space bar controls play/pause +``` + +## 5. License + +The source code of this package is released under the [**GPLv2**](http://www.gnu.org/licenses/) license. For commercial use, please contact me at and Prof. Fu Zhang at to discuss an alternative license. \ No newline at end of file diff --git a/src/FAST-LIVO2/Supplementary/LIVO2_supplementary.pdf b/src/FAST-LIVO2/Supplementary/LIVO2_supplementary.pdf new file mode 100644 index 0000000..34c81aa Binary files /dev/null and b/src/FAST-LIVO2/Supplementary/LIVO2_supplementary.pdf differ diff --git a/src/FAST-LIVO2/config/HILTI22.yaml b/src/FAST-LIVO2/config/HILTI22.yaml new file mode 100644 index 0000000..c8cddbc --- /dev/null +++ b/src/FAST-LIVO2/config/HILTI22.yaml @@ -0,0 +1,100 @@ +common: + img_topic: "/alphasense/cam0/image_raw" + lid_topic: "/hesai/pandar" + imu_topic: "/alphasense/imu" + img_en: 1 + lidar_en: 1 + ros_driver_bug_fix: false + +extrin_calib: + # Hilti-2022 + extrinsic_T: [-0.001, -0.00855, 0.055] + extrinsic_R: [0, -1, 0, -1, 0, 0, 0, 0, -1] + + # Hilti-2023 + # extrinsic_T: [-0.006730146149038548, -0.006897049862999071, 0.049898628062256645] + # extrinsic_R: [0.006609639848469365, -0.9999773650294649, 0.0012578115132016717, + # -0.9999762249571927, -0.006612093869054189, -0.0019569708811106104, + # 0.001965243352927244, -0.0012448467359610184, -0.9999972940839232] + + # Hilti + Rcl: [ -0.999926, -0.00670802, 0.0101073, + -0.0100912, -0.00242564, -0.999946, + 0.00673218,-0.999975, 0.00235777 ] + Pcl: [ -0.0549762, 0.0675401, -0.0520599 ] + +time_offset: + imu_time_offset: 0.0 + img_time_offset: 0.0 + exposure_time_init: 0.0 + +preprocess: + hilti_en: true + point_filter_num: 1 + filter_size_surf: 0.1 # 0.2 + lidar_type: 5 # HesaiXT32 + scan_line: 32 + blind: 0.6 # 0.1 0.3 + +vio: + max_iterations: 5 + outlier_threshold: 500 + img_point_cov: 1000 + patch_size: 8 + patch_pyrimid_level: 4 + normal_en: true + raycast_en: false + inverse_composition_en: false + exposure_estimate_en: true + inv_expo_cov: 0.1 + +imu: + imu_en: true + imu_int_frame: 30 + acc_cov: 0.5 # 0.1 + gyr_cov: 0.01 + b_acc_cov: 0.0001 # 0.1 + b_gyr_cov: 0.0001 # 0.1 + +lio: + max_iterations: 5 + dept_err: 0.02 + beam_err: 0.05 + min_eigen_value: 0.0001 # 0.0025 + voxel_size: 0.4 + max_layer: 2 + max_points_num: 100 + layer_init_num: [5, 5, 5, 5, 5] + +local_map: + map_sliding_en: false + half_map_size: 100 + sliding_thresh: 8 + +uav: + imu_rate_odom: false + gravity_align_en: false + +publish: + dense_map_en: true + pub_effect_point_en: false + pub_plane_en: false + pub_scan_num: 1 + blind_rgb_points: 0.0 + +evo: + seq_name: "exp09_cupola" + pose_output_en: true + +pcd_save: + pcd_save_en: false + type: 0 # 0: World Frame, 1: Body Frame; + colmap_output_en: false # need to set interval = -1 + filter_size_pcd: 0.15 + interval: -1 + # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. + +image_save: + img_save_en: false + interval: 1 diff --git a/src/FAST-LIVO2/config/MARS_LVIG.yaml b/src/FAST-LIVO2/config/MARS_LVIG.yaml new file mode 100644 index 0000000..cdff828 --- /dev/null +++ b/src/FAST-LIVO2/config/MARS_LVIG.yaml @@ -0,0 +1,118 @@ +/**: + ros__parameters: + common: + img_topic: "/left_camera/image" + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + img_en: 1 + lidar_en: 1 + ros_driver_bug_fix: false + + extrin_calib: + extrinsic_T: [0.04165, 0.02326, -0.0284] + extrinsic_R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + # MARS_LVIG HKisland HKairport + # Rcl: [0.00438814,-0.999807,-0.0191582, + # -0.00978695,0.0191145,-0.999769, + # 0.999942,0.00457463,-0.00970118] + # Pcl: [0.016069, 0.0871753, -0.0718021] + # MARS_LVIG AMtown AMvalley + Rcl: [ -0.0022464, -0.9997299, -0.0231319, + -0.0084211, 0.0231501, -0.9996966, + 0.9999620, -0.0020509, -0.0084708] + Pcl: [-0.0025563, 0.0567484, -0.0512149] + + time_offset: + imu_time_offset: 0.0 + img_time_offset: -0.1 + exposure_time_init: 0.0132 + # ╔═══════════════════════════════════════════════════════════════════════════════════════╗ + # ║ Configuration Settings ║ + # ╠═══════════════════════════════════════════════════════════════════════════════════════╣ + # ║ Series │ ID │ img_time_offset │ exposure_time_init │ -s (start hover) ║ + # ╠═══════════════════════════════════════════════════════════════════════════════════════╣ + # ║ HKairport │ HKairport01 │ 0.1 │ 0.0 │ 75 ║ + # ║ │ HKairport02 │ -0.1 │ 0.0 │ 60 ║ + # ║ │ HKairport03 │ -0.1 │ 0.0 │ 62 ║ + # ╠═══════════════════════════════════════════════════════════════════════════════════════╣ + # ║ HKisland │ HKisland01 │ 0.0 │ 0.0 │ 118 ║ + # ║ │ HKisland02 │ 0.1 │ 0.0 │ 80 ║ + # ║ │ HKisland03 │ -0.1 │ 0.0 │ 72 ║ + # ╠═══════════════════════════════════════════════════════════════════════════════════════╣ + # ║ AMtown │ AMtown01 │ -0.1 │ 0.0285 │ 75 ║ + # ║ │ AMtown02 │ -0.1 │ 0.0285 │ 50 ║ + # ║ │ AMtown03 │ -0.1 │ 0.0285 │ 106 ║ + # ╠═══════════════════════════════════════════════════════════════════════════════════════╣ + # ║ AMvalley │ AMvalley01 │ -0.1 │ 0.0132 │ 70 ║ + # ║ │ AMvalley02 │ -0.1 │ 0.0132 │ 65 ║ + # ║ │ AMvalley03 │ -0.1 │ 0.0132 │ 68 ║ + # ╚═══════════════════════════════════════════════════════════════════════════════════════╝ + preprocess: + point_filter_num: 1 + filter_size_surf: 0.1 + lidar_type: 1 # Livox Avia LiDAR + scan_line: 6 + blind: 0.8 + + vio: + max_iterations: 5 + outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite + img_point_cov: 1000 # 100 1000 + patch_size: 8 + patch_pyrimid_level: 4 + normal_en: true + raycast_en: false + inverse_composition_en: false + exposure_estimate_en: true + inv_expo_cov: 0.1 + + imu: + imu_en: true + imu_int_frame: 30 + acc_cov: 2.0 # 0.5 + gyr_cov: 0.1 # 0.3 + b_acc_cov: 0.0001 # 0.1 + b_gyr_cov: 0.0001 # 0.1 + + lio: + max_iterations: 5 + dept_err: 0.02 + beam_err: 0.05 + min_eigen_value: 0.005 + voxel_size: 2.0 # 1.0 + max_layer: 2 + max_points_num: 50 + layer_init_num: [5, 5, 5, 5, 5] + + local_map: + map_sliding_en: false + half_map_size: 100 + sliding_thresh: 8.0 + + uav: + imu_rate_odom: false + gravity_align_en: false + + publish: + dense_map_en: true + pub_effect_point_en: false + pub_plane_en: false + pub_scan_num: 1 + blind_rgb_points: 0.0 + + evo: + seq_name: "HKisland03" + pose_output_en: false + + pcd_save: + pcd_save_en: false + type: 0 # 0: World Frame, 1: Body Frame; + colmap_output_en: false # need to set interval = -1 + filter_size_pcd: 0.15 + interval: -1 + # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. + + image_save: + img_save_en: false + interval: 1 \ No newline at end of file diff --git a/src/FAST-LIVO2/config/NTU_VIRAL.yaml b/src/FAST-LIVO2/config/NTU_VIRAL.yaml new file mode 100644 index 0000000..a3db092 --- /dev/null +++ b/src/FAST-LIVO2/config/NTU_VIRAL.yaml @@ -0,0 +1,94 @@ +/**: + ros__parameters: + common: + img_topic: "/left/image_raw" + lid_topic: "/os1_cloud_node1/points" + imu_topic: "/imu/imu" + img_en: 1 + lidar_en: 1 + ros_driver_bug_fix: false + + extrin_calib: + extrinsic_T: [-0.050, 0.000, 0.055] + extrinsic_R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + # NTU_VIRAL + Rcl: [0.0218308, 0.99976, -0.00201407, + -0.0131205, 0.00230088, 0.999911, + 0.999676, -0.0218025, 0.0131676] + Pcl: [0.122993, 0.0398643, -0.0577101] + + time_offset: + lidar_time_offset: -0.1 + imu_time_offset: 0.0 + img_time_offset: 0.0 + exposure_time_init: 0.0 + + preprocess: + point_filter_num: 3 + filter_size_surf: 0.1 + lidar_type: 3 # Ouster + scan_line: 16 + blind: 1.0 + + vio: + max_iterations: 5 + outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite + img_point_cov: 100 # 100 1000 + patch_size: 8 + patch_pyrimid_level: 3 + normal_en: true + raycast_en: false + inverse_composition_en: false + exposure_estimate_en: true + inv_expo_cov: 0.1 + + imu: + imu_en: true + imu_int_frame: 30 + acc_cov: 0.5 # 0.2 + gyr_cov: 0.3 # 0.5 + b_acc_cov: 0.0001 # 0.1 + b_gyr_cov: 0.0001 # 0.1 + + lio: + max_iterations: 5 + dept_err: 0.02 + beam_err: 0.01 + min_eigen_value: 0.0025 # 0.0025 + voxel_size: 0.5 + max_layer: 2 + max_points_num: 50 + layer_init_num: [5, 5, 5, 5, 5] + + local_map: + map_sliding_en: false + half_map_size: 100 + sliding_thresh: 8.0 + + uav: + imu_rate_odom: false + gravity_align_en: false + + publish: + dense_map_en: true + pub_effect_point_en: false + pub_plane_en: false + pub_scan_num: 1 + blind_rgb_points: 0.0 + + evo: + seq_name: "eee_01" + pose_output_en: true + + pcd_save: + pcd_save_en: false + type: 0 # 0: World Frame, 1: Body Frame; + colmap_output_en: false # need to set interval = -1 + filter_size_pcd: 0.15 + interval: -1 + # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. + + image_save: + img_save_en: false + interval: 1 \ No newline at end of file diff --git a/src/FAST-LIVO2/config/avia.yaml b/src/FAST-LIVO2/config/avia.yaml new file mode 100755 index 0000000..00ae725 --- /dev/null +++ b/src/FAST-LIVO2/config/avia.yaml @@ -0,0 +1,94 @@ +/**: + ros__parameters: + common: + img_topic: "/left_camera/image" + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + img_en: 1 + lidar_en: 1 + ros_driver_bug_fix: false + + extrin_calib: + extrinsic_T: [0.04165, 0.02326, -0.0284] + extrinsic_R: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + Rcl: [0.00610193,-0.999863,-0.0154172, + -0.00615449,0.0153796,-0.999863, + 0.999962,0.00619598,-0.0060598] + Pcl: [0.0194384, 0.104689,-0.0251952] + + time_offset: + imu_time_offset: 0.0 + img_time_offset: 0.1 + exposure_time_init: 0.0 + + preprocess: + point_filter_num: 1 + filter_size_surf: 0.1 + lidar_type: 1 # Livox Avia LiDAR + scan_line: 6 + blind: 0.8 + + vio: + max_iterations: 5 + outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite + img_point_cov: 100 # 100 1000 + patch_size: 8 + patch_pyrimid_level: 4 + normal_en: true + raycast_en: false + inverse_composition_en: false + exposure_estimate_en: true + inv_expo_cov: 0.1 + + imu: + imu_en: true + imu_int_frame: 30 + acc_cov: 0.5 # 0.2 + gyr_cov: 0.3 # 0.5 + b_acc_cov: 0.0001 # 0.1 + b_gyr_cov: 0.0001 # 0.1 + + lio: + max_iterations: 5 + dept_err: 0.02 + beam_err: 0.05 + min_eigen_value: 0.0025 # 0.005 + voxel_size: 0.5 + max_layer: 2 + max_points_num: 50 + layer_init_num: [5, 5, 5, 5, 5] + + local_map: + map_sliding_en: false + half_map_size: 100 + sliding_thresh: 8.0 + + uav: + imu_rate_odom: false + gravity_align_en: false + + publish: + dense_map_en: true + pub_effect_point_en: false + pub_plane_en: false + pub_scan_num: 1 + blind_rgb_points: 0.0 + + evo: + seq_name: "CBD_Building_01" + pose_output_en: false + + pcd_save: + pcd_save_en: false + type: 0 # 0: World Frame, 1: Body Frame; + colmap_output_en: false # need to set interval = -1 + filter_size_pcd: 0.15 + interval: -1 + # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. + + image_save: + img_save_en: false + interval: 1 \ No newline at end of file diff --git a/src/FAST-LIVO2/config/camera_MARS_LVIG.yaml b/src/FAST-LIVO2/config/camera_MARS_LVIG.yaml new file mode 100644 index 0000000..4b4e09f --- /dev/null +++ b/src/FAST-LIVO2/config/camera_MARS_LVIG.yaml @@ -0,0 +1,28 @@ +/**: + ros__parameters: + cam_model: Pinhole + # HKisland HKairport + cam_width: 2448 + cam_height: 2048 + scale: 0.25 + cam_fx: 1444.431662789634 + cam_fy: 1444.343536688358 + cam_cx: 1177.801079401826 + cam_cy: 1043.601026568268 + cam_d0: -0.05729528706141188 + cam_d1: 0.1210407244166642 + cam_d2: 0.001274128378760289 + cam_d3: 0.0004389741530109464 + + # AMtown AMvalley + # cam_width: 2448 + # cam_height: 2048 + # scale: 0.25 + # cam_fx: 1453.88 + # cam_fy: 1452.85 + # cam_cx: 1182.53 + # cam_cy: 1045.82 + # cam_d0: -0.052 + # cam_d1: 0.1168 + # cam_d2: 0.0015 + # cam_d3: 0.00013 \ No newline at end of file diff --git a/src/FAST-LIVO2/config/camera_NTU_VIRAL.yaml b/src/FAST-LIVO2/config/camera_NTU_VIRAL.yaml new file mode 100644 index 0000000..bf1308d --- /dev/null +++ b/src/FAST-LIVO2/config/camera_NTU_VIRAL.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + cam_model: Pinhole + cam_width: 752 + cam_height: 480 + scale: 1.0 + cam_fx: 4.250258563372763e+02 + cam_fy: 4.267976260903337e+02 + cam_cx: 3.860151866550880e+02 + cam_cy: 2.419130336743440e+02 + cam_d0: -0.288105327549552 + cam_d1: 0.074578284234601 + cam_d2: 7.784489598138802e-04 + cam_d3: -2.277853975035461e-04 \ No newline at end of file diff --git a/src/FAST-LIVO2/config/camera_cam1.yaml b/src/FAST-LIVO2/config/camera_cam1.yaml new file mode 100644 index 0000000..1a42d7c --- /dev/null +++ b/src/FAST-LIVO2/config/camera_cam1.yaml @@ -0,0 +1,79 @@ +/**: + ros__parameters: + cam_model: Pinhole + cam_width: 1440 + cam_height: 1080 + scale: 1.0 + # --- 4mm lens (cam1, DA9492688, 2026-08-04 재캘리브레이션 ROS camera_calibration, 3카메라 마운트, 현재 활성) --- + # camera2_ws/src/hik_camera_ros2_driver/config/camera_info_cam1.yaml + cam_fx: 1194.054103 + cam_fy: 1193.734257 + cam_cx: 731.768445 + cam_cy: 554.579899 + cam_d0: -0.104538 + cam_d1: 0.094169 + cam_d2: -0.000219 + cam_d3: 0.002476 + # --- 4mm lens (cam1, 2026-07-29 18:44 재캘리브레이션, 폐기) --- + # cam_fx: 1199.340963 + # cam_fy: 1202.587584 + # cam_cx: 743.375449 + # cam_cy: 557.776165 + # cam_d0: -0.103276 + # cam_d1: 0.093289 + # cam_d2: -0.000926 + # cam_d3: 0.003536 + # --- 4mm lens (cam1, 2026-07-29 오전 재캘리브레이션, 폐기 — 같은 문서 재갱신됨) --- + # cam_fx: 1211.492261 + # cam_fy: 1215.271564 + # cam_cx: 734.217435 + # cam_cy: 557.163356 + # cam_d0: -0.105889 + # cam_d1: 0.101806 + # cam_d2: -0.001782 + # cam_d3: 0.003994 + # --- 4mm lens (cam1, 2026-07-08 재캘리브레이션, 폐기 — 2카메라 마운트 시절) --- + # cam_fx: 1199.952310 + # cam_fy: 1203.733478 + # cam_cx: 734.732587 + # cam_cy: 555.984399 + # cam_d0: -0.097963 + # cam_d1: 0.076635 + # cam_d2: -0.000787 + # cam_d3: 0.001356 + # --- 6mm lens (cam1, 2026-06-16 재캘리브레이션, 폐기 — 렌즈 교체됨) --- + # cam_fx: 1789.271411 + # cam_fy: 1794.848936 + # cam_cx: 744.170668 + # cam_cy: 545.346079 + # cam_d0: -0.093183 + # cam_d1: 0.189899 + # cam_d2: 0.002084 + # cam_d3: 0.004098 + # --- 6mm lens (cam1, 2026-06-09 재캘리브레이션, 폐기) --- + # cam_fx: 2317.833528 + # cam_fy: 2322.780813 + # cam_cx: 657.065936 + # cam_cy: 587.150296 + # cam_d0: -0.111649 + # cam_d1: 0.359324 + # cam_d2: -0.001199 + # cam_d3: -0.005997 + # --- 8mm lens v4 (cam1, 2026-05-26) --- + # cam_fx: 2347.038631 + # cam_fy: 2353.175990 + # cam_cx: 791.950958 + # cam_cy: 566.608413 + # cam_d0: -0.128253 + # cam_d1: 0.437160 + # cam_d2: 0.002047 + # cam_d3: 0.007568 + # --- 6mm lens (1440x1080) --- + # cam_fx: 1731.285808 + # cam_fy: 1739.429604 + # cam_cx: 713.349417 + # cam_cy: 547.551014 + # cam_d0: -0.091047 + # cam_d1: 0.101541 + # cam_d2: -0.001141 + # cam_d3: -0.001854 diff --git a/src/FAST-LIVO2/config/camera_cam2.yaml b/src/FAST-LIVO2/config/camera_cam2.yaml new file mode 100644 index 0000000..2b5d8b6 --- /dev/null +++ b/src/FAST-LIVO2/config/camera_cam2.yaml @@ -0,0 +1,43 @@ +/**: + ros__parameters: + cam_model: Pinhole + cam_width: 1440 + cam_height: 1080 + scale: 1.0 + # --- 4mm lens (cam2, DB0174264, 2026-08-04 재캘리브레이션 ROS camera_calibration, 3카메라 마운트, 현재 활성) --- + # camera2_ws/src/hik_camera_ros2_driver/config/camera_info_cam2.yaml + cam_fx: 1193.023682 + cam_fy: 1194.022942 + cam_cx: 716.099838 + cam_cy: 571.401918 + cam_d0: -0.102107 + cam_d1: 0.089022 + cam_d2: 0.003037 + cam_d3: 0.002550 + # --- 4mm lens (cam2, 2026-07-29 18:44 재캘리브레이션, 폐기) --- + # cam_fx: 1199.655517 + # cam_fy: 1203.544082 + # cam_cx: 700.154366 + # cam_cy: 570.801288 + # cam_d0: -0.106583 + # cam_d1: 0.094309 + # cam_d2: -0.001065 + # cam_d3: -0.000934 + # --- 4mm lens (cam2, 2026-07-29 오전 재캘리브레이션, 폐기 — 같은 문서 재갱신됨) --- + # cam_fx: 1201.483318 + # cam_fy: 1205.139403 + # cam_cx: 727.502400 + # cam_cy: 555.107690 + # cam_d0: -0.104569 + # cam_d1: 0.094512 + # cam_d2: -0.001664 + # cam_d3: 0.004386 + # --- 4mm lens (cam2, 2026-07-08 재캘리브레이션, 폐기 — 2카메라 마운트 시절) --- + # cam_fx: 1196.811764 + # cam_fy: 1200.992504 + # cam_cx: 722.810928 + # cam_cy: 557.254838 + # cam_d0: -0.094323 + # cam_d1: 0.074690 + # cam_d2: -0.000652 + # cam_d3: 0.002626 diff --git a/src/FAST-LIVO2/config/camera_cam3.yaml b/src/FAST-LIVO2/config/camera_cam3.yaml new file mode 100644 index 0000000..332d6b0 --- /dev/null +++ b/src/FAST-LIVO2/config/camera_cam3.yaml @@ -0,0 +1,53 @@ +/**: + ros__parameters: + cam_model: Pinhole + cam_width: 1440 + cam_height: 1080 + scale: 1.0 + # --- 4mm lens (cam3, DB0159632, 2026-08-04 재캘리브레이션 ROS camera_calibration, 광축 180도 + # 회전 재장착 이후, 3카메라 마운트, 현재 활성) --- + # camera2_ws/src/hik_camera_ros2_driver/config/camera_info_cam3.yaml + cam_fx: 1195.670418 + cam_fy: 1194.671092 + cam_cx: 708.482568 + cam_cy: 536.506534 + cam_d0: -0.108922 + cam_d1: 0.102295 + cam_d2: -0.000250 + cam_d3: -0.000830 + # --- 4mm lens (cam3, 2026-07-29 18:44 재캘리브레이션, 폐기 — 회전 이전) --- + # cam_fx: 1193.562151 + # cam_fy: 1197.422324 + # cam_cx: 700.744724 + # cam_cy: 555.120434 + # cam_d0: -0.101521 + # cam_d1: 0.081350 + # cam_d2: 0.000448 + # cam_d3: -0.001632 + # --- 4mm lens (cam3, 2026-07-29 오전 재캘리브레이션, 폐기 — 같은 문서 재갱신됨) --- + # cam_fx: 1206.607830 + # cam_fy: 1209.432700 + # cam_cx: 703.507808 + # cam_cy: 549.058561 + # cam_d0: -0.106353 + # cam_d1: 0.087198 + # cam_d2: -0.000903 + # cam_d3: -0.000505 + # --- 4mm lens (cam3, 2026-06-18 재캘리브레이션, 폐기 — 2카메라 마운트 이전 시절) --- + # cam_fx: 1203.078148 + # cam_fy: 1206.096396 + # cam_cx: 699.186863 + # cam_cy: 565.715472 + # cam_d0: -0.102740 + # cam_d1: 0.093985 + # cam_d2: -0.000759 + # cam_d3: -0.001804 + # --- 4mm lens (cam3, 2026-06-08 구버전, 폐기) --- + # cam_fx: 1189.152341 + # cam_fy: 1192.753914 + # cam_cx: 736.714571 + # cam_cy: 558.680089 + # cam_d0: -0.101030 + # cam_d1: 0.082065 + # cam_d2: -0.000642 + # cam_d3: 0.002411 diff --git a/src/FAST-LIVO2/config/camera_dualcam_cam1_cam2.yaml b/src/FAST-LIVO2/config/camera_dualcam_cam1_cam2.yaml new file mode 100644 index 0000000..b371c0e --- /dev/null +++ b/src/FAST-LIVO2/config/camera_dualcam_cam1_cam2.yaml @@ -0,0 +1,61 @@ +# Multi-camera intrinsics for the dual-camera (cam1 + cam2) configuration. +# +# Loaded into the `parameter_blackboard` node like the other camera_*.yaml +# files, but with a `cam_num` + numbered `cam_0`/`cam_1` namespace so that +# vk::camera_loader::loadFromRosNs(nh, "parameter_blackboard", vector<...>&) +# can read each camera's intrinsics under its own "cam_." prefix. Camera +# indices must match the order of entries in extrin_dualcam_cam1_cam2.yaml. +/**: + ros__parameters: + cam_num: 2 + cam_0: + # cam1 (DA9492688, 4mm lens), ROS camera_calibration 2026-08-04, 3-camera rig mount + # (camera2_ws/src/hik_camera_ros2_driver/config/camera_info_cam1.yaml) + # D = [k1, k2, p1, p2, k3=0.0] -> cam_d0..d3 (k3 dropped, it's 0) + # NOTE: this file's intrinsics are current, but extrin_dualcam_cam1_cam2.yaml's + # Rcl/Pcl are from the 2-camera-rig mount and are stale now that the rig is 3-camera. + cam_model: Pinhole + cam_width: 1440 + cam_height: 1080 + scale: 1.0 + cam_fx: 1194.054103 + cam_fy: 1193.734257 + cam_cx: 731.768445 + cam_cy: 554.579899 + cam_d0: -0.104538 + cam_d1: 0.094169 + cam_d2: -0.000219 + cam_d3: 0.002476 + # --- 이전 (2026-07-29 18:44) --- + # cam_fx: 1199.340963 + # cam_fy: 1202.587584 + # cam_cx: 743.375449 + # cam_cy: 557.776165 + # cam_d0: -0.103276 + # cam_d1: 0.093289 + # cam_d2: -0.000926 + # cam_d3: 0.003536 + cam_1: + # cam2 (DB0174264, 4mm lens), ROS camera_calibration 2026-08-04, 3-camera rig mount + # (camera2_ws/src/hik_camera_ros2_driver/config/camera_info_cam2.yaml) + cam_model: Pinhole + cam_width: 1440 + cam_height: 1080 + scale: 1.0 + cam_fx: 1193.023682 + cam_fy: 1194.022942 + cam_cx: 716.099838 + cam_cy: 571.401918 + cam_d0: -0.102107 + cam_d1: 0.089022 + cam_d2: 0.003037 + cam_d3: 0.002550 + # --- 이전 (2026-07-29 18:44) --- + # cam_fx: 1199.655517 + # cam_fy: 1203.544082 + # cam_cx: 700.154366 + # cam_cy: 570.801288 + # cam_d0: -0.106583 + # cam_d1: 0.094309 + # cam_d2: -0.001065 + # cam_d3: -0.000934 diff --git a/src/FAST-LIVO2/config/camera_fisheye_HILTI22.yaml b/src/FAST-LIVO2/config/camera_fisheye_HILTI22.yaml new file mode 100644 index 0000000..c9314ee --- /dev/null +++ b/src/FAST-LIVO2/config/camera_fisheye_HILTI22.yaml @@ -0,0 +1,12 @@ +cam_model: EquidistantCamera +cam_width: 720 +cam_height: 540 +scale: 1.0 +cam_fx: 351.31400364193297 +cam_fy: 351.4911744656785 +cam_cx: 367.8522793375995 +cam_cy: 253.8402144980996 +k1: -0.03696737352869157 +k2: -0.008917880497032812 +k3: 0.008912969593422046 +k4: -0.0037685977496087313 diff --git a/src/FAST-LIVO2/config/camera_mid360s.yaml b/src/FAST-LIVO2/config/camera_mid360s.yaml new file mode 100644 index 0000000..e099c73 --- /dev/null +++ b/src/FAST-LIVO2/config/camera_mid360s.yaml @@ -0,0 +1,37 @@ +/**: + ros__parameters: + cam_model: Pinhole + # 카메라: MVS BinningHorizontal=2, BinningVertical=2 → 720x540 출력 + # scale=1.0 (하드웨어 binning이 이미 해상도 축소) + cam_width: 720 + cam_height: 540 + scale: 1.0 + # --- 8mm lens v4, 720x540 binning 2x2 (1440x1080 × 0.5) --- + cam_fx: 1173.519316 + cam_fy: 1176.587995 + cam_cx: 395.975479 + cam_cy: 283.304207 + cam_d0: -0.128253 + cam_d1: 0.437160 + cam_d2: 0.002047 + cam_d3: 0.007568 + # --- 8mm lens v4, 1440x1080 + software scale=0.5 (binning 미지원 시 대안) --- + # cam_width: 1440 + # cam_height: 1080 + # scale: 0.5 + # cam_fx: 2347.038631 + # cam_fy: 2353.175990 + # cam_cx: 791.950958 + # cam_cy: 566.608413 + # --- 6mm lens, 1440x1080 + scale=0.5 --- + # cam_width: 1440 + # cam_height: 1080 + # scale: 0.5 + # cam_fx: 1731.285808 + # cam_fy: 1739.429604 + # cam_cx: 713.349417 + # cam_cy: 547.551014 + # cam_d0: -0.091047 + # cam_d1: 0.101541 + # cam_d2: -0.001141 + # cam_d3: -0.001854 diff --git a/src/FAST-LIVO2/config/camera_pinhole.yaml b/src/FAST-LIVO2/config/camera_pinhole.yaml new file mode 100644 index 0000000..e14d3fa --- /dev/null +++ b/src/FAST-LIVO2/config/camera_pinhole.yaml @@ -0,0 +1,42 @@ +/**: + ros__parameters: + cam_model: Pinhole + cam_width: 1440 + cam_height: 1080 + scale: 0.5 + # --- 6mm lens (cam1/cam2) --- + # cam_fx: 1731.285808 + # cam_fy: 1739.429604 + # cam_cx: 713.349417 + # cam_cy: 547.551014 + # cam_d0: -0.091047 + # cam_d1: 0.101541 + # cam_d2: -0.001141 + # cam_d3: -0.001854 + # --- 6mm lens (cam1, 2026-06-09 재캘리브레이션) → camera_cam1.yaml 사용 권장 --- + # cam_fx: 2317.833528 + # cam_fy: 2322.780813 + # cam_cx: 657.065936 + # cam_cy: 587.150296 + # cam_d0: -0.111649 + # cam_d1: 0.359324 + # cam_d2: -0.001199 + # cam_d3: -0.005997 + # --- 8mm lens v4 (cam1/cam2, 2026-05-26) --- + cam_fx: 2347.038631 + cam_fy: 2353.175990 + cam_cx: 791.950958 + cam_cy: 566.608413 + cam_d0: -0.128253 + cam_d1: 0.437160 + cam_d2: 0.002047 + cam_d3: 0.007568 + # --- 4mm lens (cam3, 2026-06-08 재캘리브레이션) → camera_cam3.yaml 사용 권장 --- + # cam_fx: 1189.152341 + # cam_fy: 1192.753914 + # cam_cx: 736.714571 + # cam_cy: 558.680089 + # cam_d0: -0.101030 + # cam_d1: 0.082065 + # cam_d2: -0.000642 + # cam_d3: 0.002411 \ No newline at end of file diff --git a/src/FAST-LIVO2/config/camera_triplecam_cam1_cam2_cam3.yaml b/src/FAST-LIVO2/config/camera_triplecam_cam1_cam2_cam3.yaml new file mode 100644 index 0000000..a2ae54a --- /dev/null +++ b/src/FAST-LIVO2/config/camera_triplecam_cam1_cam2_cam3.yaml @@ -0,0 +1,85 @@ +# Multi-camera intrinsics for the triple-camera (cam1 + cam2 + cam3) configuration. +# +# Loaded into the `parameter_blackboard` node like the other camera_*.yaml +# files, but with a `cam_num` + numbered `cam_0`/`cam_1`/`cam_2` namespace so +# that vk::camera_loader::loadFromRosNs(nh, "parameter_blackboard", vector<...>&) +# can read each camera's intrinsics under its own "cam_." prefix. Camera +# indices must match the order of entries in extrin_triplecam_cam1_cam2_cam3.yaml. +# +# Intrinsics below: 2026-08-04 재캘리브레이션 — current for all three. +# 참고: cam3는 2026-08-04에 광축 기준 반시계 180도 회전 재장착됨. 아래 값은 회전 이후 +# 상태로 재촬영한 것이라 그대로 유효함 (camera2_ws/.../camera_info_cam3.yaml 참고). +# Extrinsics (Rcl/Pcl in extrin_triplecam_cam1_cam2_cam3.yaml) also updated to match +# (direct_visual_lidar_calibration, 2026-08-04, dvlc_data/{cam1,cam2,cam3}_preprocessed). +/**: + ros__parameters: + cam_num: 3 + cam_0: + # cam1 (DA9492688, 4mm lens), ROS camera_calibration 2026-08-04 + cam_model: Pinhole + cam_width: 1440 + cam_height: 1080 + scale: 1.0 + cam_fx: 1194.054103 + cam_fy: 1193.734257 + cam_cx: 731.768445 + cam_cy: 554.579899 + cam_d0: -0.104538 + cam_d1: 0.094169 + cam_d2: -0.000219 + cam_d3: 0.002476 + # --- 이전 (2026-07-29 18:44) --- + # cam_fx: 1199.340963 + # cam_fy: 1202.587584 + # cam_cx: 743.375449 + # cam_cy: 557.776165 + # cam_d0: -0.103276 + # cam_d1: 0.093289 + # cam_d2: -0.000926 + # cam_d3: 0.003536 + cam_1: + # cam2 (DB0174264, 4mm lens), ROS camera_calibration 2026-08-04 + cam_model: Pinhole + cam_width: 1440 + cam_height: 1080 + scale: 1.0 + cam_fx: 1193.023682 + cam_fy: 1194.022942 + cam_cx: 716.099838 + cam_cy: 571.401918 + cam_d0: -0.102107 + cam_d1: 0.089022 + cam_d2: 0.003037 + cam_d3: 0.002550 + # --- 이전 (2026-07-29 18:44) --- + # cam_fx: 1199.655517 + # cam_fy: 1203.544082 + # cam_cx: 700.154366 + # cam_cy: 570.801288 + # cam_d0: -0.106583 + # cam_d1: 0.094309 + # cam_d2: -0.001065 + # cam_d3: -0.000934 + cam_2: + # cam3 (DB0159632, 4mm lens, 2026-08-04 광축 180도 회전 재장착), ROS camera_calibration 2026-08-04 + cam_model: Pinhole + cam_width: 1440 + cam_height: 1080 + scale: 1.0 + cam_fx: 1195.670418 + cam_fy: 1194.671092 + cam_cx: 708.482568 + cam_cy: 536.506534 + cam_d0: -0.108922 + cam_d1: 0.102295 + cam_d2: -0.000250 + cam_d3: -0.000830 + # --- 이전 (2026-07-29 18:44, 회전 전) --- + # cam_fx: 1193.562151 + # cam_fy: 1197.422324 + # cam_cx: 700.744724 + # cam_cy: 555.120434 + # cam_d0: -0.101521 + # cam_d1: 0.081350 + # cam_d2: 0.000448 + # cam_d3: -0.001632 diff --git a/src/FAST-LIVO2/config/extrin_cam1.yaml b/src/FAST-LIVO2/config/extrin_cam1.yaml new file mode 100644 index 0000000..9473836 --- /dev/null +++ b/src/FAST-LIVO2/config/extrin_cam1.yaml @@ -0,0 +1,46 @@ +/**: + ros__parameters: + extrin_calib: + # Camera 1 (DA9492688, 4mm lens) extrinsics + # --- 2026-07-09 재캘리브레이션 (direct_visual_lidar_calibration, 현재 활성) --- + # T_lidar_camera: [tx=0.07907, ty=-0.04903, tz=-0.07375, qx=0.30146, qy=-0.49923, qz=0.64110, qw=-0.49887] + # az=-20.99° el=18.65° (config/extrin_dualcam_cam1_cam2.yaml cam_0과 동일 값) + Rcl: [-0.32049782, -0.94065569, -0.11157073, + 0.33865652, -0.00378619, -0.94090245, + 0.88464282, -0.33934134, 0.31977264] + Pcl: [-0.02901190, -0.09635251, -0.06300165] + # --- 2026-06-16 재캘리브레이션 (폐기 — 렌즈 교체됨) --- + # T_lidar_camera: [tx=0.07719, ty=-0.00783, tz=-0.05823, qx=-0.40351, qy=0.40878, qz=-0.58252, qw=0.57510] + # az=-0.7° el=19.9° + # Rcl: [-0.01286891, -0.99991719, -0.00007353, + # 0.34011452, -0.00430811, -0.94037415, + # 0.94029596, -0.01212660, 0.34014180] + # Pcl: [-0.00683908, -0.08104266, -0.05287530] + # --- 2026-05-15 구버전 --- + # Rcl: [ 0.02069918, -0.99972195, 0.01129488, + # 0.27370973, -0.00519927, -0.96179829, + # 0.96158958, 0.02299996, 0.27352600] + # Pcl: [-0.02917199, -0.07698411, -0.11157462] + + # --- 2026-06-09 구버전 (폐기) --- + # az=0.9° el=15.9° + # Rcl: [0.01255090, -0.99982700, 0.01374180, + # 0.27400100, -0.00977792, -0.96168000, + # 0.96164800, 0.01583530, 0.27383100] + # Pcl: [0.04144749, -0.10107580, -0.01683002] + + # --- 2026-06-09 재캘리브레이션 v2 (폐기) --- + # T_lidar_camera: [tx=0.10476, ty=-0.00247, tz=-0.08220] + # az=1.5° el=15.9° + # Rcl: [0.0219646, -0.999651, 0.0146488, + # 0.274753, -0.00805253, -0.961481, + # 0.961264, 0.0251433, 0.274481] + # Pcl: [-0.00356413, -0.10783703, -0.07807754] + + # --- 2026-06-16 재캘리브레이션 (폐기, 내부 캘 2026-06-16 기반 — 위 2026-07-09 값과 중복이라 주석 처리) --- + # T_lidar_camera: [tx=0.07719, ty=-0.00783, tz=-0.05823, qx=-0.40351, qy=0.40878, qz=-0.58252, qw=0.57510] + # az=-0.7° el=19.9° + # Rcl: [-0.01286891, -0.99991719, -0.00007353, + # 0.34011452, -0.00430811, -0.94037415, + # 0.94029596, -0.01212660, 0.34014180] + # Pcl: [-0.00683908, -0.08104266, -0.05287530] diff --git a/src/FAST-LIVO2/config/extrin_cam2.yaml b/src/FAST-LIVO2/config/extrin_cam2.yaml new file mode 100644 index 0000000..6bb6ffd --- /dev/null +++ b/src/FAST-LIVO2/config/extrin_cam2.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + extrin_calib: + # Camera 2 (DB0174264, hik_camera_ros2_driver, 4mm lens) extrinsics + # --- 2026-07-09 재캘리브레이션 (direct_visual_lidar_calibration, 현재 활성) --- + # T_lidar_camera: [tx=0.08467, ty=0.04114, tz=-0.07354, qx=-0.49810, qy=0.30075, qz=-0.49977, qw=0.64162] + # az=20.96° el=18.84° (config/extrin_dualcam_cam1_cam2.yaml cam_1과 동일 값) + Rcl: [ 0.31955890, -0.94093008, 0.11194953, + 0.34172113, 0.00424131, -0.93979183, + 0.88380359, 0.33857436, 0.32289104] + Pcl: [0.01989001, -0.09821752, -0.06501275] + # --- 2026-05-27 (폐기 — 8mm lens v6, 렌즈 교체됨) --- + # T_lidar_camera: [tx=0.05454, ty=0.00438, tz=-0.09330, qx=-0.41061, qy=0.43393, qz=-0.57567, qw=0.55830] + # az=-2.5° el=16.6° + # Rcl: [-0.03939004, -0.99915456, -0.01177214, + # 0.28645369, -0.00000461, -0.95809409, + # 0.95728403, -0.04111154, 0.28621170] + # Pcl: [0.00542794, -0.10500922, -0.02532945] diff --git a/src/FAST-LIVO2/config/extrin_cam3.yaml b/src/FAST-LIVO2/config/extrin_cam3.yaml new file mode 100644 index 0000000..d04ea13 --- /dev/null +++ b/src/FAST-LIVO2/config/extrin_cam3.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + extrin_calib: + # Camera 3 (4mm lens) extrinsics + # --- 2026-06-18 캘리브레이션 (현재 활성, 내부 캘 2026-06-18 기반) --- + # T_lidar_camera: [tx=0.07680, ty=-0.00628, tz=-0.08776, qx=-0.40882, qy=0.39927, qz=-0.57777, qw=0.58277] + # az=0.9° el=-20.3° + Rcl: [ 0.01351752, -0.99988379, 0.00704826, + 0.34695354, -0.00192070, -0.93788035, + 0.93778490, 0.01512324, 0.34688726] + Pcl: [-0.00669994, -0.10896580, -0.04148943] diff --git a/src/FAST-LIVO2/config/extrin_dualcam_cam1_cam2.yaml b/src/FAST-LIVO2/config/extrin_dualcam_cam1_cam2.yaml new file mode 100644 index 0000000..09d344c --- /dev/null +++ b/src/FAST-LIVO2/config/extrin_dualcam_cam1_cam2.yaml @@ -0,0 +1,50 @@ +# Multi-camera extrinsics array for the dual-camera (cam1 + cam2) configuration. +# +# Unlike the other config/*.yaml files in this package, this file is NOT loaded +# through the ROS2 parameter server (rclcpp::Node::declare_parameter cannot +# represent an array-of-structs). Instead its path is passed as the +# `common.camera_config_path` string parameter and parsed directly with +# yaml-cpp by LIVMapper::loadCameraArrayConfig() — see +# docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md §5. +# +# Cameras are listed in the same order they will be indexed (cam_id 0, 1, ...). +# Rcl/Pcl values below are direct_visual_lidar_calibration results. +# +# --- 2026-06-16 / 2026-05-27 (이전 활성, 폐기) --- +# - img_topic: "/cam1/image" +# Rcl: [-0.01286891, -0.99991719, -0.00007353, +# 0.34011452, -0.00430811, -0.94037415, +# 0.94029596, -0.01212660, 0.34014180] +# Pcl: [-0.00683908, -0.08104266, -0.05287530] +# - img_topic: "/cam2/image" +# Rcl: [-0.03939004, -0.99915456, -0.01177214, +# 0.28645369, -0.00000461, -0.95809409, +# 0.95728403, -0.04111154, 0.28621170] +# Pcl: [0.00542794, -0.10500922, -0.02532945] +# +# --- 2026-07-08 재캘리브레이션 (같은 장면 4회 촬영, 폐기) --- +# cam1: T_lidar_camera tx=0.07724 ty=-0.04533 tz=-0.06644 qx=0.29914 qy=-0.50124 qz=0.64017 qw=-0.49945 +# az=-21.2° el=18.6° +# cam2: T_lidar_camera tx=0.07286 ty=0.05137 tz=-0.07307 qx=-0.49719 qy=0.30084 qz=-0.50173 qw=0.64075 +# az=20.8° el=18.9° +# 카메라 간 상대 각도(boresight 기준) = 39.65° +# +# --- 2026-07-09 재캘리브레이션 (현재 활성, bag 5개 서로 다른 위치/각도로 재수집) --- +# cam1: T_lidar_camera tx=0.07907 ty=-0.04903 tz=-0.07375 qx=0.30146 qy=-0.49923 qz=0.64110 qw=-0.49887 +# az=-20.99° el=18.65° +# cam2: T_lidar_camera tx=0.08467 ty=0.04114 tz=-0.07354 qx=-0.49810 qy=0.30075 qz=-0.49977 qw=0.64162 +# az=20.96° el=18.84° +# 카메라 간 상대 각도(boresight 기준) = 39.63° (0708 결과와 0.02° 차이 — 데이터셋이 완전히 달라졌는데도 +# 거의 동일하게 재현됨. 물리적으로 의도한 "약 30°"보다는 실제 마운트 각도가 ~40°였던 것으로 판단됨) +# 광학중심 baseline = 9.04cm +cameras: + - img_topic: "/cam1/image" + Rcl: [-0.32049782, -0.94065569, -0.11157073, + 0.33865652, -0.00378619, -0.94090245, + 0.88464282, -0.33934134, 0.31977264] + Pcl: [-0.02901190, -0.09635251, -0.06300165] + - img_topic: "/cam2/image" + Rcl: [ 0.31955890, -0.94093008, 0.11194953, + 0.34172113, 0.00424131, -0.93979183, + 0.88380359, 0.33857436, 0.32289104] + Pcl: [0.01989001, -0.09821752, -0.06501275] diff --git a/src/FAST-LIVO2/config/extrin_triplecam_cam1_cam2_cam3.yaml b/src/FAST-LIVO2/config/extrin_triplecam_cam1_cam2_cam3.yaml new file mode 100644 index 0000000..5c48612 --- /dev/null +++ b/src/FAST-LIVO2/config/extrin_triplecam_cam1_cam2_cam3.yaml @@ -0,0 +1,63 @@ +# Multi-camera extrinsics array for the triple-camera (cam1 + cam2 + cam3) configuration. +# +# Unlike the other config/*.yaml files in this package, this file is NOT loaded +# through the ROS2 parameter server (rclcpp::Node::declare_parameter cannot +# represent an array-of-structs). Instead its path is passed as the +# `common.camera_config_path` string parameter and parsed directly with +# yaml-cpp by LIVMapper::loadCameraArrayConfig() — see +# docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md §5. +# +# Cameras are listed in the same order they will be indexed (cam_id 0, 1, 2). +# +# --- 2026-08-04 재캘리브레이션 (현재 활성, cam3 광축 180도 회전 재장착 이후, +# 새 내부 캘리브레이션 반영 후 dvlc_gui로 재진행) --- +# direct_visual_lidar_calibration 결과: ~/dvlc_data/{cam1,cam2,cam3}_preprocessed/calib.json +# T_lidar_camera = [tx, ty, tz, qx, qy, qz, qw] (camera pose expressed in the LiDAR frame). +# Rcl/Pcl = T_camera_lidar (Rcl = R(q)^T, Pcl = -R(q)^T @ t), FAST-LIVO2's convention: +# p_camera = Rcl * p_lidar + Pcl. +cameras: + - img_topic: "/cam1/image" + # T_lidar_camera: [tx=0.06959316, ty=0.05919484, tz=-0.11555322, + # qx=-0.59090481, qy=0.19334609, qz=-0.39584302, qw=0.67583807] + Rcl: [ 0.61185119, -0.76354983, 0.20646979, + 0.30655329, -0.01172039, -0.95178134, + 0.72915239, 0.64564254, 0.22689758] + Pcl: [0.02647580, -0.13062162, -0.06274397] + - img_topic: "/cam2/image" + # T_lidar_camera: [tx=0.07159342, ty=-0.00968426, tz=-0.07252498, + # qx=-0.40764730, qy=0.41401200, qz=-0.57697645, qw=0.57403477] + Rcl: [-0.00861552, -0.99995084, -0.00490877, + 0.32486735, 0.00184370, -0.94575780, + 0.94572035, -0.00974290, 0.32483549] + Pcl: [-0.00942298, -0.09183158, -0.04424302] + - img_topic: "/cam3/image" + # T_lidar_camera: [tx=0.05326862, ty=-0.05701828, tz=-0.11326257, + # qx=0.58323967, qy=0.18827243, qz=0.39372916, qw=0.68510024] + Rcl: [ 0.61906168, 0.75910378, 0.20130595, + -0.31987197, 0.00961769, 0.94741196, + 0.71724790, -0.65089857, 0.24876997] + Pcl: [0.03310666, 0.12489384, -0.04714359] + # + # --- 2026-07-29 19:0x (cam3 회전 이전, 폐기) --- + # cam1 Rcl: [0.65273370, -0.72070111, 0.23351364, 0.33495301, -0.00192744, -0.94223286, 0.67951836, 0.69324324, 0.24014290] + # Pcl: [0.04007425, -0.12112355, -0.05677600] + # cam2 Rcl: [-0.00126161, -0.99999910, 0.00046639, 0.32661778, -0.00085287, -0.94515612, 0.94515567, -0.00104009, 0.32661856] + # Pcl: [-0.00738347, -0.09486492, -0.04932296] + # cam3 Rcl: [-0.65141673, -0.72076414, -0.23697110, 0.32432118, 0.01783388, -0.94577890, 0.68590962, -0.69295095, 0.22214178] + # Pcl: [-0.05022920, -0.11681905, -0.06848265] + # + # --- 2026-07-29 오전 (같은 마운트, 이전 내부 캘리브레이션 기준, 폐기) --- + # cam1 Rcl: [0.65853013, -0.71505655, 0.23458940, 0.33385248, -0.00178671, -0.94262364, 0.67444835, 0.69906433, 0.23754681] + # Pcl: [0.03485613, -0.11671711, -0.02883126] + # cam2 Rcl: [-0.02468461, -0.99965882, -0.00853880, 0.33978869, -0.00035691, -0.94050174, 0.94017781, -0.02611731, 0.33968158] + # Pcl: [-0.00320088, -0.10124477, -0.03787376] + # cam3 Rcl: [-0.65892753, -0.71481902, -0.23419710, 0.32256022, 0.01275161, -0.94646305, 0.67953618, -0.69919322, 0.22216979] + # Pcl: [-0.03562020, -0.11326435, -0.03745576] + # + # --- 2026-07-09/06-18 (2카메라 마운트 시절, 폐기) --- + # cam1 Rcl: [-0.32049782, -0.94065569, -0.11157073, 0.33865652, -0.00378619, -0.94090245, 0.88464282, -0.33934134, 0.31977264] + # Pcl: [-0.02901190, -0.09635251, -0.06300165] + # cam2 Rcl: [0.31955890, -0.94093008, 0.11194953, 0.34172113, 0.00424131, -0.93979183, 0.88380359, 0.33857436, 0.32289104] + # Pcl: [0.01989001, -0.09821752, -0.06501275] + # cam3 Rcl: [0.01351752, -0.99988379, 0.00704826, 0.34695354, -0.00192070, -0.93788035, 0.93778490, 0.01512324, 0.34688726] + # Pcl: [-0.00669994, -0.10896580, -0.04148943] diff --git a/src/FAST-LIVO2/config/mid360s.yaml b/src/FAST-LIVO2/config/mid360s.yaml new file mode 100644 index 0000000..a52b30e --- /dev/null +++ b/src/FAST-LIVO2/config/mid360s.yaml @@ -0,0 +1,243 @@ +/**: + ros__parameters: + common: + img_topic: "/camera/image" + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + img_en: 1 + lidar_en: 1 + ros_driver_bug_fix: false + + extrin_calib: + # IMU-to-LiDAR extrinsics (MID-360 internal IMU) + extrinsic_T: [ -0.011, -0.02329, 0.04412 ] + extrinsic_R: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + # Camera-to-LiDAR extrinsics (physical estimate 2026-05-14) + # Source: T_lidar_camera roll=-73deg pitch=0deg yaw=-90deg + # tx=-0.0198m ty=-0.0695m tz=0.0172m (from calibration) + # Rcl = R_camera_lidar = R_lidar_camera.T + # Pcl = -Rcl @ t_lidar_camera + # Rcl: [ 0.00000000, -1.00000000, 0.00000000, # this value is base + # 0.29237170, 0.00000000, -0.95630476, + # 0.95630476, 0.00000000, 0.29237170] + # Pcl: [-0.06950000, 0.02223740, 0.01390604] + + # Rcl: [-0.25354040, -0.96693219, 0.02755752, this value is fail + # -0.39707519, 0.07805545, -0.91446084, + # 0.88207061, -0.24279517, -0.40373500] + # Pcl: [0.76509620, 1.81348647, 0.02357659] + + + # Rcl: [0.00000000, -1.00000000, 0.00000000, # try 2 + # 0.29237170, 0.00000000, -0.95630476, + # 0.95630476, 0.00000000, 0.29237170] + # Pcl: [-0.11960285, 0.17087142, 0.08245752] + + # Rcl: [ 0.06956, -0.99721, -0.02705, # try 3 + # 0.23900, 0.04298, -0.97007, + # 0.96852, 0.06101, 0.24133] + # Pcl: [-0.06746, 0.02440, 0.01927] + + # Rcl: [0.06956000, -0.99721000, -0.02705000, # try 4 + # 0.23900000, 0.04298000, -0.97007000, + # 0.96852000, 0.06101000, 0.24133000] + # Pcl: [0.06043056, 0.01212226, 0.13300489] + + + # Rcl: [0.06956000, -0.99721000, -0.02705000, # try 5 + # 0.23900000, 0.04298000, -0.97007000, + # 0.96852000, 0.06101000, 0.24133000] + # Pcl: [0.06029619, 0.01647827, 0.07757671] + + # direct_visual_lidar_calibration result 1st run (2026-05-15) + # T_lidar_camera: [0.14293, -0.02917, -0.03467, qx=-0.4344, qy=0.4181, qz=-0.5614, qw=0.5668] + # Rcl: [ 0.01995325, -0.99970731, 0.01368083, # try 6 + # 0.27319897, -0.00771101, -0.96192664, + # 0.96175059, 0.02293115, 0.27296514] + # Pcl: [-0.03154008, -0.07261870, -0.12733124] + + # direct_visual_lidar_calibration result 2nd run (2026-05-15) + # T_lidar_camera: [0.12896, -0.02700, -0.04320, qx=-0.4337, qy=0.4185, qz=-0.5608, qw=0.5677] + # Rcl: [ 0.02069918, -0.99972195, 0.01129488, + # 0.27370973, -0.00519927, -0.96179829, # best + # 0.96158958, 0.02299996, 0.27352600] + # Pcl: [-0.02917199, -0.07698411, -0.11157462] + + # Rcl: [0.00705089, -0.99997358, -0.00176582, + # 0.27363987, 0.00362791, -0.96182538, + # 0.96180638, 0.00629853, 0.27365822] + # Pcl: [-0.00462873, -0.01235428, -0.59325839] + + # Rcl: [0.00520558, -0.99997233, 0.00531431, + # 0.28104599, -0.00363718, -0.95968741, + # 0.95968018, 0.00648929, 0.28101928] + # Pcl: [0.00145690, -0.13454580, -0.56836240] + + + # --- Camera 1 (6mm lens) extrinsics --- + # direct_visual_lidar_calibration result (2026-05-15) + # Rcl: [0.02069918, -0.99972195, 0.01129488, + # 0.27370973, -0.00519927, -0.96179829, + # 0.96158958, 0.02299996, 0.27352600] + # Pcl: [-0.02917199, -0.07698411, -0.11157462] + # direct_visual_lidar_calibration result (2026-06-09), 6mm 내부캘 재적용, az=0.9° el=15.9° + # Rcl: [0.01255090, -0.99982700, 0.01374180, + # 0.27400100, -0.00977792, -0.96168000, + # 0.96164800, 0.01583530, 0.27383100] + # Pcl: [0.04144749, -0.10107580, -0.01683002] + # direct_visual_lidar_calibration result (2026-06-16), 6mm 내부캘 재적용, az=-0.7° el=19.9° ← 현재 extrin_cam1.yaml + # Rcl: [-0.01286891, -0.99991719, -0.00007353, + # 0.34011452, -0.00430811, -0.94037415, + # 0.94029596, -0.01212660, 0.34014180] + # Pcl: [-0.00683908, -0.08104266, -0.05287530] + + # --- Camera 2 (hik_camera_ros2_driver, camera_info_cam2.yaml) extrinsics --- + # direct_visual_lidar_calibration result (2026-05-22), 6mm lens + # T_lidar_camera: [tx=0.05225, ty=-0.00876, tz=-0.07362, qx=-0.42082, qy=0.42731, qz=-0.56696, qw=0.56469] + # Rcl: [-0.00806821, -0.99995281, -0.00541114, + # 0.28067349, 0.00292921, -0.95979884, + # 0.95976940, -0.00926262, 0.28063661] + # Pcl: [-0.00873246, -0.08530013, -0.02956579] + + # --- 8mm lens v2 extrinsics --- + # direct_visual_lidar_calibration result (2026-05-24), intrinsic v2 기반 + # T_lidar_camera: [tx=0.01145, ty=-0.00062, tz=-0.05991, qx=-0.41931, qy=0.42038, qz=-0.56709, qw=0.57085] + # Rcl: [0.00337986, -0.99998470, -0.00437918, + # 0.29491832, 0.00518120, -0.95550842, + # 0.95551649, 0.00193799, 0.29493132] + # Pcl: [-0.00091689, -0.06061769, 0.00672712] + + # --- 8mm lens v4 extrinsics --- + # direct_visual_lidar_calibration result (2026-05-26), intrinsic v4 + 파노라마 방위각 반전 크롭 적용 + # T_lidar_camera: [tx=0.03270, ty=-0.09704, tz=-0.10467, qx=-0.42407, qy=0.41384, qz=-0.57397, qw=0.56520] + # Rcl: [-0.00142083, 0.29782367, 0.95461984, # WRONG — az=-89°, 잘못된 크롭에서 얻은 결과 + # -0.99981856, -0.01855673, 0.00430125, + # 0.01899564, -0.95444052, 0.29779599] + # Pcl: [0.03270414, -0.09703870, -0.10467137] + + # --- 8mm lens v2 extrinsics --- + # direct_visual_lidar_calibration result (2026-05-24), intrinsic v2 기반, az≈0° 방향 정상 + # T_lidar_camera: [tx=0.01145, ty=-0.00062, tz=-0.05991, qx=-0.41931, qy=0.42038, qz=-0.56709, qw=0.57085] + # Rcl: [0.00337986, -0.99998470, -0.00437918, + # 0.29491832, 0.00518120, -0.95550842, + # 0.95551649, 0.00193799, 0.29493132] + # Pcl: [-0.00091689, -0.06061769, 0.00672712] + + # --- 8mm lens v5 extrinsics --- + # direct_visual_lidar_calibration result (2026-05-27), intrinsic v4 + 반사판 + LiDAR 마스킹, 4 bags + # T_lidar_camera: [tx=0.00312, ty=0.00582, tz=-0.07062, qx=-0.41467, qy=0.43797, qz=-0.57307, qw=0.55481] + # az=-2.5° el=15.8° + # Rcl: [-0.04046417, -0.99912356, -0.01071296, + # 0.27266698, -0.00072685, -0.96210820, + # 0.96125718, -0.04185198, 0.27245741] + # Pcl: [0.00518588, -0.06878734, 0.01648196] + + # --- 8mm lens v6 extrinsics --- + # direct_visual_lidar_calibration result (2026-05-27), intrinsic v4 + 반사판 + LiDAR 마스킹, 5 bags + # T_lidar_camera: [tx=0.05454, ty=0.00438, tz=-0.09330, qx=-0.41061, qy=0.43393, qz=-0.57567, qw=0.55830] + # az=-2.5° el=16.6° + # Rcl: [-0.03939004, -0.99915456, -0.01177214, + # 0.28645369, -0.00000461, -0.95809409, + # 0.95728403, -0.04111154, 0.28621170] + # Pcl: [0.00542794, -0.10500922, -0.02532945] + + # --- 4mm lens cam3 extrinsics v1 (intrinsic 구값 사용, 폐기) --- + # T_lidar_camera: [tx=0.07440, ty=-0.00376, tz=-0.07364, qx=-0.43050, qy=0.41776, qz=-0.55952, qw=0.57191] + # az=1.5° el=16.3° + # Rcl: [0.02482051, -0.99968430, 0.00390447, + # 0.28030319, 0.00321036, -0.95990615, + # 0.95959057, 0.02491980, 0.28029438] + # Pcl: [-0.00532104, -0.09153097, -0.05065450] + + # --- 4mm lens cam3 extrinsics v2 (현재 활성, intrinsic 재캘 적용) --- + # direct_visual_lidar_calibration result (2026-06-08), intrinsic fx=1189.152341 + # T_lidar_camera: [tx=0.10182, ty=-0.00949, tz=-0.06163] + # az=0.0° el=16.0° (물리적으로 타당) + Rcl: [0.00141494, -0.99999701, -0.00199385, + 0.27599729, 0.00230693, -0.96115564, + 0.96115737, 0.00080968, 0.27599973] + Pcl: [-0.00975341, -0.08731812, -0.08085047] + + + + time_offset: + imu_time_offset: 0.0 + # 2026-07-29: 드라이버 타임스탬프를 라이다 기준 ±1ms로 정밀 보정한 뒤(move_1 bag + # 검증됨) 카메라 3대 체제에서 0.1이 왜곡되고 0.18이 필요한 증상이 재발했었음. + # 원인은 펌웨어(STM32 트리거 유실)가 아니라 카메라 3대가 캐스케이드 USB3 허브 + # 하나를 공유하면서 생긴 대역폭 경합이었음 — 카메라 1대를 허브에서 메인보드 + # 내장 USB 3.0 포트로 옮기니 0.1에서 즉시 정상화됨(벽시계 왜곡 테스트로 확인). + # → img_time_offset은 그대로 0.1 유지, 대신 카메라들을 서로 다른 USB + # 컨트롤러/포트에 분산 연결할 것 (허브 캐스케이드 공유 금지). + img_time_offset: 0.1 + exposure_time_init: 0.0 + + preprocess: + point_filter_num: 1 + filter_size_surf: 0.05 # tuned: 0.03 (more points, higher CPU) + lidar_type: 1 # Livox MID-360 (uses same CustomMsg as AVIA) + scan_line: 6 + blind: 0.2 # tuned: 0.2~0.3 (include closer points) + + vio: + max_iterations: 10 # tuned: 10 (better accuracy, higher CPU) + outlier_threshold: 1000 + img_point_cov: 100 + patch_size: 8 + patch_pyrimid_level: 3 + normal_en: true + raycast_en: true # tuned: true (better accuracy, higher CPU) + inverse_composition_en: false + exposure_estimate_en: true + inv_expo_cov: 0.1 + + imu: + imu_en: true + imu_int_frame: 30 + acc_cov: 0.5 + gyr_cov: 0.3 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + + lio: + max_iterations: 10 + dept_err: 0.02 + beam_err: 0.05 + min_eigen_value: 0.001 # tuned: 0.001 (accept weaker planes, more points) + voxel_size: 0.2 # tuned: 0.2~0.3 (finer map, higher CPU) + max_layer: 3 # tuned: 3 + max_points_num: 100 # tuned: 100 + layer_init_num: [5, 5, 5, 5, 5] + + local_map: + map_sliding_en: true # try_2: 37분 bag → RAM 보호 필수 + half_map_size: 50 + sliding_thresh: 8.0 + + uav: + imu_rate_odom: false # tuned: true + gravity_align_en: true + + publish: + dense_map_en: true + pub_effect_point_en: false + pub_plane_en: false + pub_scan_num: 1 + blind_rgb_points: 0.0 + + evo: + seq_name: "mid360s_mapping" + pose_output_en: false + + pcd_save: + pcd_save_en: true + type: 0 # 0: World Frame, 1: Body Frame + colmap_output_en: false + filter_size_pcd: 0.1 + interval: 100 # tuned: 100 (periodic auto-save every 100 scans) + + image_save: + img_save_en: false + interval: 1 diff --git a/src/FAST-LIVO2/include/IMU_Processing.h b/src/FAST-LIVO2/include/IMU_Processing.h new file mode 100644 index 0000000..cf09609 --- /dev/null +++ b/src/FAST-LIVO2/include/IMU_Processing.h @@ -0,0 +1,91 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#ifndef IMU_PROCESSING_H +#define IMU_PROCESSING_H + +#include +#include +#include "common_lib.h" +#include +#include +#include +#include +extern const bool time_list(PointType &x, PointType &y); + +/// *************IMU Process and undistortion +class ImuProcess +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ImuProcess(); + ~ImuProcess(); + + void Reset(); + void Reset(double start_timestamp, const sensor_msgs::msg::Imu::ConstSharedPtr &lastimu); + void set_extrinsic(const V3D &transl, const M3D &rot); + void set_extrinsic(const V3D &transl); + void set_extrinsic(const MD(4, 4) & T); + void set_gyr_cov_scale(const V3D &scaler); + void set_acc_cov_scale(const V3D &scaler); + void set_gyr_bias_cov(const V3D &b_g); + void set_acc_bias_cov(const V3D &b_a); + void set_inv_expo_cov(const double &inv_expo); + void set_imu_init_frame_num(const int &num); + void disable_imu(); + void disable_gravity_est(); + void disable_bias_est(); + void disable_exposure_est(); + void Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_); + void UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out); + + ofstream fout_imu; + double IMU_mean_acc_norm; + V3D unbiased_gyr; + + V3D cov_acc; + V3D cov_gyr; + V3D cov_bias_gyr; + V3D cov_bias_acc; + double cov_inv_expo; + double first_lidar_time; + bool imu_time_init = false; + bool imu_need_init = true; + M3D Eye3d; + V3D Zero3d; + int lidar_type; + +private: + void IMU_init(const MeasureGroup &meas, StatesGroup &state, int &N); + void Forward_without_imu(LidarMeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out); + PointCloudXYZI pcl_wait_proc; + sensor_msgs::msg::Imu::ConstSharedPtr last_imu; + PointCloudXYZI::Ptr cur_pcl_un_; + vector IMUpose; + M3D Lid_rot_to_IMU; + V3D Lid_offset_to_IMU; + V3D mean_acc; + V3D mean_gyr; + V3D angvel_last; + V3D acc_s_last; + double last_prop_end_time; + double time_last_scan; + int init_iter_num = 1, MAX_INI_COUNT = 20; + bool b_first_frame = true; + bool imu_en = true; + bool gravity_est_en = true; + bool ba_bg_est_en = true; + bool exposure_estimate_en = true; +}; +typedef std::shared_ptr ImuProcessPtr; +#endif \ No newline at end of file diff --git a/src/FAST-LIVO2/include/LIVMapper.h b/src/FAST-LIVO2/include/LIVMapper.h new file mode 100644 index 0000000..fdcd68b --- /dev/null +++ b/src/FAST-LIVO2/include/LIVMapper.h @@ -0,0 +1,232 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#ifndef LIV_MAPPER_H +#define LIV_MAPPER_H + +#include "IMU_Processing.h" +#include "vio.h" +#include "preprocess.h" +#if __has_include() +#include +#else +#include +#endif +#include +#include +#include +#include +#include + +class LIVMapper +{ +public: + LIVMapper(rclcpp::Node::SharedPtr &node, std::string node_name); + ~LIVMapper(); + void initializeSubscribersAndPublishers(rclcpp::Node::SharedPtr &nh, image_transport::ImageTransport &it_); + void initializeComponents(rclcpp::Node::SharedPtr &node); + void initializeFiles(); + void run(rclcpp::Node::SharedPtr &node); + void gravityAlignment(); + void handleFirstFrame(); + void stateEstimationAndMapping(); + void handleVIO(); + void handleLIO(); + void savePCD(); + void processImu(); + + bool sync_packages(LidarMeasureGroup &meas); + void prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D acc_avr, V3D angvel_avr); + void imu_prop_callback(); + void transformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, PointCloudXYZI::Ptr &trans_cloud); + void pointBodyToWorld(const PointType &pi, PointType &po); + void RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po); + void RGBpointBodyToWorld(PointType const *const pi, PointType *const po); + void standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg_in); + void imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr &msg_in); + void img_cbk(const sensor_msgs::msg::Image::ConstSharedPtr &msg_in, int cam_id); + void loadCameraArrayConfig(); + void publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager); + void publish_frame_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudFullRes, VIOManagerPtr vio_manager); + void publish_visual_sub_map(const rclcpp::Publisher::SharedPtr &pubSubVisualMap); + void publish_effect_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudEffect, const std::vector &ptpl_list); + void publish_odometry(const rclcpp::Publisher::SharedPtr &pmavros_pose_publisherubOdomAftMapped); + void publish_mavros(const rclcpp::Publisher::SharedPtr &mavros_pose_publisher); + void publish_path(const rclcpp::Publisher::SharedPtr &pubPath); + void readParameters(rclcpp::Node::SharedPtr &node); + template void set_posestamp(T &out); + template void pointBodyToWorld(const Eigen::Matrix &pi, Eigen::Matrix &po); + template Eigen::Matrix pointBodyToWorld(const Eigen::Matrix &pi); + cv::Mat getImageFromMsg(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg); + + std::mutex mtx_buffer, mtx_buffer_imu_prop; + std::condition_variable sig_buffer; + + SLAM_MODE slam_mode_; + std::unordered_map voxel_map; + + string root_dir; + string lid_topic, imu_topic, seq_name, img_topic; + V3D extT; + M3D extR; + + // Multi-camera configuration (see docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md §5). + // Populated either from `camera_config_path` (yaml-cpp, N-camera array) or, + // when that parameter is empty, as a 1-element fallback built from the + // legacy single-camera parameters (img_topic, extrin_calib.Rcl/Pcl) so that + // existing single-camera launch files keep working unchanged. + std::string camera_config_path; + std::vector camera_img_topics; + std::vector> camera_extrin_Rs; + std::vector> camera_extrin_Ts; + int num_of_cameras = 0; + + // Max allowed timestamp spread (seconds) between cameras' front-of-buffer + // images before sync_packages() treats them as unsynced and drops the + // earliest one. Omni-LIVO's original 0.001s assumes each camera has its own + // sub-ms hardware trigger timestamp; hik_camera_ros2_driver instead copies + // a LiDAR-packet-quantized timestamp from shared memory (see porting plan), + // so independent cameras can legitimately differ by up to ~1 LiDAR period. + // Override via common.camera_time_tolerance once per-camera high-precision + // timestamps are available. + double camera_time_tolerance = 0.001; + + int feats_down_size = 0, max_iterations = 0; + + double res_mean_last = 0.05; + double gyr_cov = 0, acc_cov = 0, inv_expo_cov = 0; + double blind_rgb_points = 0.0; + double last_timestamp_lidar = -1.0, last_timestamp_imu = -1.0; + // Per-camera, not shared: with N independent camera streams, a single + // shared "last image timestamp" wrongly rejects one camera's frame as + // "time going backwards" whenever the other camera's message is processed + // first with an equal-or-later timestamp (common with hik_camera_ros2_driver's + // shared LiDAR-packet-based timestamp — see camera_time_tolerance above). + std::vector last_timestamp_img; + double filter_size_surf_min = 0; + double filter_size_pcd = 0; + double _first_lidar_time = 0.0; + double match_time = 0, solve_time = 0, solve_const_H_time = 0; + + bool lidar_map_inited = false, pcd_save_en = false, img_save_en = false, pub_effect_point_en = false, pose_output_en = false, ros_driver_fix_en = false, hilti_en = false; + int img_save_interval = 1, pcd_save_interval = -1, pcd_save_type = 0; + int pub_scan_num = 1; + + StatesGroup imu_propagate, latest_ekf_state; + + bool new_imu = false, state_update_flg = false, imu_prop_enable = true, ekf_finish_once = false; + deque prop_imu_buffer; + sensor_msgs::msg::Imu newest_imu; + double latest_ekf_time; + nav_msgs::msg::Odometry imu_prop_odom; + rclcpp::Publisher::SharedPtr pubImuPropOdom; + double imu_time_offset = 0.0; + double lidar_time_offset = 0.0; + + bool gravity_align_en = false, gravity_align_finished = false; + + bool sync_jump_flag = false; + + bool lidar_pushed = false, imu_en, gravity_est_en, flg_reset = false, ba_bg_est_en = true; + bool dense_map_en = false; + int img_en = 1, imu_int_frame = 3; + bool normal_en = true; + bool exposure_estimate_en = false; + double exposure_time_init = 0.0; + bool inverse_composition_en = false; + bool raycast_en = false; + int lidar_en = 1; + + // Omni-LIVO multi-camera VIO tuning (see docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md). + bool enable_cross_camera_tracking = false; + bool vio_dynamic_cov_enabled = false; + int vio_dynamic_cov_warmup_frames = 200; + double vio_warmup_cov_scale = 500.0; + double vio_min_cov_scale = 10.0; + double vio_max_cov_scale = 2000.0; + double vio_dynamic_cov_error_max = 50.0; + int vio_max_total_points = 300; + bool is_first_frame = false; + int grid_size, patch_size, grid_n_width, grid_n_height, patch_pyrimid_level; + int outlier_threshold; + double plot_time; + int frame_cnt; + double img_time_offset = 0.0; + deque lid_raw_data_buffer; + deque lid_header_time_buffer; + deque imu_buffer; + std::vector> img_buffers; + std::vector> img_time_buffers; + vector _pv_list; + vector extrinT; + vector extrinR; + vector cameraextrinT; + vector cameraextrinR; + int IMG_POINT_COV; + + PointCloudXYZI::Ptr visual_sub_map; + PointCloudXYZI::Ptr feats_undistort; + PointCloudXYZI::Ptr feats_down_body; + PointCloudXYZI::Ptr feats_down_world; + PointCloudXYZI::Ptr pcl_w_wait_pub; + PointCloudXYZI::Ptr pcl_wait_pub; + PointCloudXYZRGB::Ptr pcl_wait_save; + PointCloudXYZI::Ptr pcl_wait_save_intensity; + + ofstream fout_pre, fout_out, fout_visual_pos, fout_lidar_pos, fout_points; + + pcl::VoxelGrid downSizeFilterSurf; + + V3D euler_cur; + + LidarMeasureGroup LidarMeasures; + StatesGroup _state; + StatesGroup state_propagat; + + nav_msgs::msg::Path path; + nav_msgs::msg::Odometry odomAftMapped; + geometry_msgs::msg::Quaternion geoQuat; + geometry_msgs::msg::PoseStamped msg_body_pose; + + PreprocessPtr p_pre; + ImuProcessPtr p_imu; + VoxelMapManagerPtr voxelmap_manager; + VIOManagerPtr vio_manager; + + rclcpp::Publisher::SharedPtr plane_pub; + rclcpp::Publisher::SharedPtr voxel_pub; + std::shared_ptr sub_pcl; + rclcpp::Subscription::SharedPtr sub_imu; + std::vector::SharedPtr> sub_img_list; + rclcpp::Publisher::SharedPtr pubLaserCloudFullRes; + rclcpp::Publisher::SharedPtr pubNormal; + rclcpp::Publisher::SharedPtr pubSubVisualMap; + rclcpp::Publisher::SharedPtr pubLaserCloudEffect; + rclcpp::Publisher::SharedPtr pubLaserCloudMap; + rclcpp::Publisher::SharedPtr pubOdomAftMapped; + rclcpp::Publisher::SharedPtr pubPath; + rclcpp::Publisher::SharedPtr pubLaserCloudDyn; + rclcpp::Publisher::SharedPtr pubLaserCloudDynRmed; + rclcpp::Publisher::SharedPtr pubLaserCloudDynDbg; + image_transport::Publisher pubImage; + rclcpp::Publisher::SharedPtr mavros_pose_publisher; + rclcpp::TimerBase::SharedPtr imu_prop_timer; + rclcpp::Node::SharedPtr node; + + int frame_num = 0; + double aver_time_consu = 0; + double aver_time_icp = 0; + double aver_time_map_inre = 0; + bool colmap_output_en = false; +}; +#endif \ No newline at end of file diff --git a/src/FAST-LIVO2/include/common_lib.h b/src/FAST-LIVO2/include/common_lib.h new file mode 100755 index 0000000..ba55169 --- /dev/null +++ b/src/FAST-LIVO2/include/common_lib.h @@ -0,0 +1,251 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#ifndef COMMON_LIB_H +#define COMMON_LIB_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +// using namespace Eigen; // avoid cmake error: reference to ‘Matrix’ is ambiguous +using namespace Sophus; + +#define print_line std::cout << __FILE__ << ", " << __LINE__ << std::endl; +#define G_m_s2 (9.81) // Gravaty const in GuangDong/China +#define DIM_STATE (19) // Dimension of states (Let Dim(SO(3)) = 3) +#define INIT_COV (0.01) +#define SIZE_LARGE (500) +#define SIZE_SMALL (100) +#define VEC_FROM_ARRAY(v) v[0], v[1], v[2] +#define MAT_FROM_ARRAY(v) v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8] +#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/" + name)) + +enum LID_TYPE +{ + AVIA = 1, + VELO16 = 2, + OUST64 = 3, + L515 = 4, + XT32 = 5, + PANDAR128 = 6, + ROBOSENSE = 7 +}; +enum SLAM_MODE +{ + ONLY_LO = 0, + ONLY_LIO = 1, + LIVO = 2 +}; +enum EKF_STATE +{ + WAIT = 0, + VIO = 1, + LIO = 2, + LO = 3 +}; + +struct MeasureGroup +{ + double vio_time; + double lio_time; + deque imu; + std::vector imgs; // one entry per camera, indexed by cam_id + MeasureGroup() + { + vio_time = 0.0; + lio_time = 0.0; + imgs.clear(); + }; +}; + +struct LidarMeasureGroup +{ + double lidar_frame_beg_time; + double lidar_frame_end_time; + double last_lio_update_time; + PointCloudXYZI::Ptr lidar; + PointCloudXYZI::Ptr pcl_proc_cur; + PointCloudXYZI::Ptr pcl_proc_next; + deque measures; + EKF_STATE lio_vio_flg; + int lidar_scan_index_now; + + LidarMeasureGroup() + { + lidar_frame_beg_time = -0.0; + lidar_frame_end_time = 0.0; + last_lio_update_time = -1.0; + lio_vio_flg = WAIT; + this->lidar.reset(new PointCloudXYZI()); + this->pcl_proc_cur.reset(new PointCloudXYZI()); + this->pcl_proc_next.reset(new PointCloudXYZI()); + this->measures.clear(); + lidar_scan_index_now = 0; + last_lio_update_time = -1.0; + }; +}; + +typedef struct pointWithVar +{ + Eigen::Vector3d point_b; // point in the lidar body frame + Eigen::Vector3d point_i; // point in the imu body frame + Eigen::Vector3d point_w; // point in the world frame + Eigen::Matrix3d var_nostate; // the var removed the state covarience + Eigen::Matrix3d body_var; + Eigen::Matrix3d var; + Eigen::Matrix3d point_crossmat; + Eigen::Vector3d normal; + pointWithVar() + { + var_nostate = Eigen::Matrix3d::Zero(); + var = Eigen::Matrix3d::Zero(); + body_var = Eigen::Matrix3d::Zero(); + point_crossmat = Eigen::Matrix3d::Zero(); + point_b = Eigen::Vector3d::Zero(); + point_i = Eigen::Vector3d::Zero(); + point_w = Eigen::Vector3d::Zero(); + normal = Eigen::Vector3d::Zero(); + }; +} pointWithVar; + + +struct StatesGroup +{ + StatesGroup() + { + this->rot_end = M3D::Identity(); + this->pos_end = V3D::Zero(); + this->vel_end = V3D::Zero(); + this->bias_g = V3D::Zero(); + this->bias_a = V3D::Zero(); + this->gravity = V3D::Zero(); + this->inv_expo_time_per_cam.clear(); // per-camera exposure (size == num_of_cameras; 1 for single camera) + this->cov = MD(DIM_STATE, DIM_STATE)::Identity() * INIT_COV; + this->cov(6, 6) = 0.00001; // legacy exposure slot in the covariance, kept for IMU_Processing compatibility + this->cov.block<9, 9>(10, 10) = MD(9, 9)::Identity() * 0.00001; + }; + + StatesGroup(const StatesGroup &b) + { + this->rot_end = b.rot_end; + this->pos_end = b.pos_end; + this->vel_end = b.vel_end; + this->bias_g = b.bias_g; + this->bias_a = b.bias_a; + this->gravity = b.gravity; + this->inv_expo_time_per_cam = b.inv_expo_time_per_cam; + this->cov = b.cov; + }; + + StatesGroup &operator=(const StatesGroup &b) + { + this->rot_end = b.rot_end; + this->pos_end = b.pos_end; + this->vel_end = b.vel_end; + this->bias_g = b.bias_g; + this->bias_a = b.bias_a; + this->gravity = b.gravity; + this->inv_expo_time_per_cam = b.inv_expo_time_per_cam; + this->cov = b.cov; + return *this; + }; + + // NOTE: exposure time is no longer part of the ESKF state vector (index 6 + // is now an unused placeholder, kept only so DIM_STATE/cov layout doesn't + // shift). Per-camera exposure is estimated separately inside VIOManager's + // joint update and stored in inv_expo_time_per_cam. + StatesGroup operator+(const Matrix &state_add) + { + StatesGroup a; + a.rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0)); + a.pos_end = this->pos_end + state_add.block<3, 1>(3, 0); + a.inv_expo_time_per_cam = this->inv_expo_time_per_cam; + a.vel_end = this->vel_end + state_add.block<3, 1>(7, 0); + a.bias_g = this->bias_g + state_add.block<3, 1>(10, 0); + a.bias_a = this->bias_a + state_add.block<3, 1>(13, 0); + a.gravity = this->gravity + state_add.block<3, 1>(16, 0); + + a.cov = this->cov; + return a; + }; + + StatesGroup &operator+=(const Matrix &state_add) + { + this->rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0)); + this->pos_end += state_add.block<3, 1>(3, 0); + this->vel_end += state_add.block<3, 1>(7, 0); + this->bias_g += state_add.block<3, 1>(10, 0); + this->bias_a += state_add.block<3, 1>(13, 0); + this->gravity += state_add.block<3, 1>(16, 0); + return *this; + }; + + Matrix operator-(const StatesGroup &b) + { + Matrix a; + M3D rotd(b.rot_end.transpose() * this->rot_end); + a.block<3, 1>(0, 0) = Log(rotd); + a.block<3, 1>(3, 0) = this->pos_end - b.pos_end; + a(6, 0) = 0.0; // exposure not part of the state vector; placeholder + a.block<3, 1>(7, 0) = this->vel_end - b.vel_end; + a.block<3, 1>(10, 0) = this->bias_g - b.bias_g; + a.block<3, 1>(13, 0) = this->bias_a - b.bias_a; + a.block<3, 1>(16, 0) = this->gravity - b.gravity; + return a; + }; + + void resetpose() + { + this->rot_end = M3D::Identity(); + this->pos_end = V3D::Zero(); + this->vel_end = V3D::Zero(); + } + + M3D rot_end; //!< the estimated attitude (rotation matrix) at the end lidar point + V3D pos_end; //!< the estimated position at the end lidar point (world frame) + V3D vel_end; //!< the estimated velocity at the end lidar point (world frame) + std::vector inv_expo_time_per_cam; //!< per-camera inverse exposure time (size == num_of_cameras) + V3D bias_g; //!< gyroscope bias + V3D bias_a; //!< accelerator bias + V3D gravity; //!< the estimated gravity acceleration + Matrix cov; //!< states covariance +}; + +template +auto set_pose6d(const double t, const Matrix &a, const Matrix &g, const Matrix &v, const Matrix &p, + const Matrix &R) +{ + Pose6D rot_kp; + rot_kp.offset_time = t; + for (int i = 0; i < 3; i++) + { + rot_kp.acc[i] = a(i); + rot_kp.gyr[i] = g(i); + rot_kp.vel[i] = v(i); + rot_kp.pos[i] = p(i); + for (int j = 0; j < 3; j++) + rot_kp.rot[i * 3 + j] = R(i, j); + } + // Map(rot_kp.rot, 3,3) = R; + return move(rot_kp); +} + +#endif \ No newline at end of file diff --git a/src/FAST-LIVO2/include/feature.h b/src/FAST-LIVO2/include/feature.h new file mode 100644 index 0000000..82bf1e5 --- /dev/null +++ b/src/FAST-LIVO2/include/feature.h @@ -0,0 +1,57 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#ifndef LIVO_FEATURE_H_ +#define LIVO_FEATURE_H_ + +#include "visual_point.h" + +// A salient image region that is tracked across frames. +struct Feature +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum FeatureType + { + CORNER, + EDGELET + }; + int id_; + int cam_id_; //!< Camera ID that this feature is associated with. + FeatureType type_; //!< Type can be corner or edgelet. + cv::Mat img_; //!< Image associated with the patch feature + Vector2d px_; //!< Coordinates in pixels on pyramid level 0. + Vector3d f_; //!< Unit-bearing vector of the patch feature. + int level_; //!< Image pyramid level where patch feature was extracted. + VisualPoint *point_; //!< Pointer to 3D point which corresponds to the patch feature. + Vector2d grad_; //!< Dominant gradient direction for edglets, normalized. + SE3 T_f_w_; //!< Pose of the frame where the patch feature was extracted. + float *patch_; //!< Pointer to the image patch data. + float score_; //!< Score of the patch feature. + float mean_; //!< Mean intensity of the image patch feature, used for normalization. + double inv_expo_time_; //!< Inverse exposure time of the image where the patch feature was extracted. + + Feature(VisualPoint *_point, float *_patch, const Vector2d &_px, const Vector3d &_f, const SE3 &_T_f_w, int _level, int _cam_id) + : id_(-1), cam_id_(_cam_id), type_(CORNER), px_(_px), f_(_f), T_f_w_(_T_f_w), mean_(0), score_(0), level_(_level), patch_(_patch), point_(_point) + { + } + + inline Vector3d pos() const { return T_f_w_.inverse().translation(); } + + ~Feature() + { + // ROS_WARN("The feature %d has been destructed.", id_); + delete[] patch_; + } +}; + +#endif // LIVO_FEATURE_H_ diff --git a/src/FAST-LIVO2/include/frame.h b/src/FAST-LIVO2/include/frame.h new file mode 100644 index 0000000..3b1c4d2 --- /dev/null +++ b/src/FAST-LIVO2/include/frame.h @@ -0,0 +1,83 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#ifndef LIVO_FRAME_H_ +#define LIVO_FRAME_H_ + +#include +#include + +class VisualPoint; +struct Feature; + +typedef list Features; +typedef vector ImgPyr; + +/// A frame saves the images (one per camera), the associated features and the +/// estimated per-camera poses. Multi-camera support (Omni-LIVO): all cameras +/// share one rigid body pose, but are stored per-camera here because each +/// camera has a different fixed extrinsic offset from that body pose. +class Frame : boost::noncopyable +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id. + int id_; //!< Unique id of the frame. + std::vector cams_; //!< Camera models, one per camera. + std::vector> T_f_w_; //!< Transform (f)rame from (w)orld, one per camera. + std::vector> T_f_w_prior_; //!< Transform (f)rame from (w)orld provided by the IMU prior, one per camera. + std::vector> imgs_shared_; //!< Shared ownership of each camera's image. + std::vector imgs_; //!< Images of the frame, one per camera (Mat headers referencing imgs_shared_). + double timestamp_; + Features fts_; //!< List of features in the image. + + Frame(const std::vector &cams, std::vector &imgs, double timestamp); + ~Frame(); + + /// Initialize new frame and create image pyramid. + void initFrame(std::vector &imgs); + + /// Return number of point observations. + inline size_t nObs() const { return fts_.size(); } + + /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). + inline Vector2d w2c(const Vector3d &xyz_w, int cam_id) const { return cams_[cam_id]->world2cam(T_f_w_[cam_id] * xyz_w); } + + /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c) using the IMU prior pose. + inline Vector2d w2c_prior(const Vector3d &xyz_w, int cam_id) const { return cams_[cam_id]->world2cam(T_f_w_prior_[cam_id] * xyz_w); } + + /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). + inline Vector3d c2f(const double x, const double y, int cam_id) const { return cams_[cam_id]->cam2world(x, y); } + + /// Transforms point coordinates in world-frame (w) to camera-frame (f). + inline Vector3d w2f(const Vector3d &xyz_w, int cam_id) const { return T_f_w_[cam_id] * xyz_w; } + + /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w). + inline Vector3d f2w(const Vector3d &f, int cam_id) const { return T_f_w_[cam_id].inverse() * f; } + + /// Return the pose of the frame in the (w)orld coordinate frame, for a given camera. + inline Vector3d pos(int cam_id) const { return T_f_w_[cam_id].inverse().translation(); } +}; + +typedef std::shared_ptr FramePtr; + +/// Some helper functions for the frame object. +namespace frame_utils +{ + +/// Creates an image pyramid of half-sampled images. +void createImgPyramid(const cv::Mat &img_level_0, int n_levels, ImgPyr &pyr); + +} // namespace frame_utils + +#endif // LIVO_FRAME_H_ diff --git a/src/FAST-LIVO2/include/livox_ros_driver/CustomMsg.h b/src/FAST-LIVO2/include/livox_ros_driver/CustomMsg.h new file mode 100644 index 0000000..144d800 --- /dev/null +++ b/src/FAST-LIVO2/include/livox_ros_driver/CustomMsg.h @@ -0,0 +1,292 @@ +// Generated by gencpp from file livox_ros_driver/CustomMsg.msg +// DO NOT EDIT! + + +#ifndef LIVOX_ROS_DRIVER_MESSAGE_CUSTOMMSG_H +#define LIVOX_ROS_DRIVER_MESSAGE_CUSTOMMSG_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace livox_ros_driver +{ +template +struct CustomMsg_ +{ + typedef CustomMsg_ Type; + + CustomMsg_() + : header() + , timebase(0) + , point_num(0) + , lidar_id(0) + , rsvd() + , points() { + rsvd.assign(0); + } + CustomMsg_(const ContainerAllocator& _alloc) + : header(_alloc) + , timebase(0) + , point_num(0) + , lidar_id(0) + , rsvd() + , points(_alloc) { + (void)_alloc; + rsvd.assign(0); + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint64_t _timebase_type; + _timebase_type timebase; + + typedef uint32_t _point_num_type; + _point_num_type point_num; + + typedef uint8_t _lidar_id_type; + _lidar_id_type lidar_id; + + typedef boost::array _rsvd_type; + _rsvd_type rsvd; + + typedef std::vector< ::livox_ros_driver::CustomPoint_ , typename std::allocator_traits::template rebind_alloc< ::livox_ros_driver::CustomPoint_ >> _points_type; + _points_type points; + + + + + + typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg_ > Ptr; + typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg_ const> ConstPtr; + +}; // struct CustomMsg_ + +typedef ::livox_ros_driver::CustomMsg_ > CustomMsg; + +typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg > CustomMsgPtr; +typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg const> CustomMsgConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::livox_ros_driver::CustomMsg_ & v) +{ +ros::message_operations::Printer< ::livox_ros_driver::CustomMsg_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::livox_ros_driver::CustomMsg_ & lhs, const ::livox_ros_driver::CustomMsg_ & rhs) +{ + return lhs.header == rhs.header && + lhs.timebase == rhs.timebase && + lhs.point_num == rhs.point_num && + lhs.lidar_id == rhs.lidar_id && + lhs.rsvd == rhs.rsvd && + lhs.points == rhs.points; +} + +template +bool operator!=(const ::livox_ros_driver::CustomMsg_ & lhs, const ::livox_ros_driver::CustomMsg_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace livox_ros_driver + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::livox_ros_driver::CustomMsg_ > + : TrueType + { }; + +template +struct IsMessage< ::livox_ros_driver::CustomMsg_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::livox_ros_driver::CustomMsg_ > + : FalseType + { }; + +template +struct IsFixedSize< ::livox_ros_driver::CustomMsg_ const> + : FalseType + { }; + +template +struct HasHeader< ::livox_ros_driver::CustomMsg_ > + : TrueType + { }; + +template +struct HasHeader< ::livox_ros_driver::CustomMsg_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::livox_ros_driver::CustomMsg_ > +{ + static const char* value() + { + return "e4d6829bdfe657cb6c21a746c86b21a6"; + } + + static const char* value(const ::livox_ros_driver::CustomMsg_&) { return value(); } + static const uint64_t static_value1 = 0xe4d6829bdfe657cbULL; + static const uint64_t static_value2 = 0x6c21a746c86b21a6ULL; +}; + +template +struct DataType< ::livox_ros_driver::CustomMsg_ > +{ + static const char* value() + { + return "livox_ros_driver/CustomMsg"; + } + + static const char* value(const ::livox_ros_driver::CustomMsg_&) { return value(); } +}; + +template +struct Definition< ::livox_ros_driver::CustomMsg_ > +{ + static const char* value() + { + return "# Livox publish pointcloud msg format.\n" +"\n" +"Header header # ROS standard message header\n" +"uint64 timebase # The time of first point\n" +"uint32 point_num # Total number of pointclouds\n" +"uint8 lidar_id # Lidar device id number\n" +"uint8[3] rsvd # Reserved use\n" +"CustomPoint[] points # Pointcloud data\n" +"\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +"\n" +"================================================================================\n" +"MSG: livox_ros_driver/CustomPoint\n" +"# Livox costom pointcloud format.\n" +"\n" +"uint32 offset_time # offset time relative to the base time\n" +"float32 x # X axis, unit:m\n" +"float32 y # Y axis, unit:m\n" +"float32 z # Z axis, unit:m\n" +"uint8 reflectivity # reflectivity, 0~255\n" +"uint8 tag # livox tag\n" +"uint8 line # laser number in lidar\n" +"\n" +; + } + + static const char* value(const ::livox_ros_driver::CustomMsg_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::livox_ros_driver::CustomMsg_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.timebase); + stream.next(m.point_num); + stream.next(m.lidar_id); + stream.next(m.rsvd); + stream.next(m.points); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct CustomMsg_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::livox_ros_driver::CustomMsg_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::livox_ros_driver::CustomMsg_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "timebase: "; + Printer::stream(s, indent + " ", v.timebase); + s << indent << "point_num: "; + Printer::stream(s, indent + " ", v.point_num); + s << indent << "lidar_id: "; + Printer::stream(s, indent + " ", v.lidar_id); + s << indent << "rsvd[]" << std::endl; + for (size_t i = 0; i < v.rsvd.size(); ++i) + { + s << indent << " rsvd[" << i << "]: "; + Printer::stream(s, indent + " ", v.rsvd[i]); + } + s << indent << "points[]" << std::endl; + for (size_t i = 0; i < v.points.size(); ++i) + { + s << indent << " points[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::livox_ros_driver::CustomPoint_ >::stream(s, indent + " ", v.points[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // LIVOX_ROS_DRIVER_MESSAGE_CUSTOMMSG_H diff --git a/src/FAST-LIVO2/include/livox_ros_driver/CustomPoint.h b/src/FAST-LIVO2/include/livox_ros_driver/CustomPoint.h new file mode 100644 index 0000000..36ae394 --- /dev/null +++ b/src/FAST-LIVO2/include/livox_ros_driver/CustomPoint.h @@ -0,0 +1,258 @@ +// Generated by gencpp from file livox_ros_driver/CustomPoint.msg +// DO NOT EDIT! + + +#ifndef LIVOX_ROS_DRIVER_MESSAGE_CUSTOMPOINT_H +#define LIVOX_ROS_DRIVER_MESSAGE_CUSTOMPOINT_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace livox_ros_driver +{ +template +struct CustomPoint_ +{ + typedef CustomPoint_ Type; + + CustomPoint_() + : offset_time(0) + , x(0.0) + , y(0.0) + , z(0.0) + , reflectivity(0) + , tag(0) + , line(0) { + } + CustomPoint_(const ContainerAllocator& _alloc) + : offset_time(0) + , x(0.0) + , y(0.0) + , z(0.0) + , reflectivity(0) + , tag(0) + , line(0) { + (void)_alloc; + } + + + + typedef uint32_t _offset_time_type; + _offset_time_type offset_time; + + typedef float _x_type; + _x_type x; + + typedef float _y_type; + _y_type y; + + typedef float _z_type; + _z_type z; + + typedef uint8_t _reflectivity_type; + _reflectivity_type reflectivity; + + typedef uint8_t _tag_type; + _tag_type tag; + + typedef uint8_t _line_type; + _line_type line; + + + + + + typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint_ > Ptr; + typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint_ const> ConstPtr; + +}; // struct CustomPoint_ + +typedef ::livox_ros_driver::CustomPoint_ > CustomPoint; + +typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint > CustomPointPtr; +typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint const> CustomPointConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::livox_ros_driver::CustomPoint_ & v) +{ +ros::message_operations::Printer< ::livox_ros_driver::CustomPoint_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::livox_ros_driver::CustomPoint_ & lhs, const ::livox_ros_driver::CustomPoint_ & rhs) +{ + return lhs.offset_time == rhs.offset_time && + lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.z == rhs.z && + lhs.reflectivity == rhs.reflectivity && + lhs.tag == rhs.tag && + lhs.line == rhs.line; +} + +template +bool operator!=(const ::livox_ros_driver::CustomPoint_ & lhs, const ::livox_ros_driver::CustomPoint_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace livox_ros_driver + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::livox_ros_driver::CustomPoint_ > + : TrueType + { }; + +template +struct IsMessage< ::livox_ros_driver::CustomPoint_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::livox_ros_driver::CustomPoint_ > + : TrueType + { }; + +template +struct IsFixedSize< ::livox_ros_driver::CustomPoint_ const> + : TrueType + { }; + +template +struct HasHeader< ::livox_ros_driver::CustomPoint_ > + : FalseType + { }; + +template +struct HasHeader< ::livox_ros_driver::CustomPoint_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::livox_ros_driver::CustomPoint_ > +{ + static const char* value() + { + return "109a3cc548bb1f96626be89a5008bd6d"; + } + + static const char* value(const ::livox_ros_driver::CustomPoint_&) { return value(); } + static const uint64_t static_value1 = 0x109a3cc548bb1f96ULL; + static const uint64_t static_value2 = 0x626be89a5008bd6dULL; +}; + +template +struct DataType< ::livox_ros_driver::CustomPoint_ > +{ + static const char* value() + { + return "livox_ros_driver/CustomPoint"; + } + + static const char* value(const ::livox_ros_driver::CustomPoint_&) { return value(); } +}; + +template +struct Definition< ::livox_ros_driver::CustomPoint_ > +{ + static const char* value() + { + return "# Livox costom pointcloud format.\n" +"\n" +"uint32 offset_time # offset time relative to the base time\n" +"float32 x # X axis, unit:m\n" +"float32 y # Y axis, unit:m\n" +"float32 z # Z axis, unit:m\n" +"uint8 reflectivity # reflectivity, 0~255\n" +"uint8 tag # livox tag\n" +"uint8 line # laser number in lidar\n" +"\n" +; + } + + static const char* value(const ::livox_ros_driver::CustomPoint_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::livox_ros_driver::CustomPoint_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.offset_time); + stream.next(m.x); + stream.next(m.y); + stream.next(m.z); + stream.next(m.reflectivity); + stream.next(m.tag); + stream.next(m.line); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct CustomPoint_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::livox_ros_driver::CustomPoint_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::livox_ros_driver::CustomPoint_& v) + { + s << indent << "offset_time: "; + Printer::stream(s, indent + " ", v.offset_time); + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "z: "; + Printer::stream(s, indent + " ", v.z); + s << indent << "reflectivity: "; + Printer::stream(s, indent + " ", v.reflectivity); + s << indent << "tag: "; + Printer::stream(s, indent + " ", v.tag); + s << indent << "line: "; + Printer::stream(s, indent + " ", v.line); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // LIVOX_ROS_DRIVER_MESSAGE_CUSTOMPOINT_H diff --git a/src/FAST-LIVO2/include/preprocess.h b/src/FAST-LIVO2/include/preprocess.h new file mode 100755 index 0000000..bb550cf --- /dev/null +++ b/src/FAST-LIVO2/include/preprocess.h @@ -0,0 +1,200 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#ifndef PREPROCESS_H_ +#define PREPROCESS_H_ + +#include "common_lib.h" +#include +#include + +using namespace std; + +#define IS_VALID(a) ((abs(a) > 1e8) ? true : false) + +enum LiDARFeature +{ + Nor, + Poss_Plane, + Real_Plane, + Edge_Jump, + Edge_Plane, + Wire, + ZeroPoint +}; +enum Surround +{ + Prev, + Next +}; +enum E_jump +{ + Nr_nor, + Nr_zero, + Nr_180, + Nr_inf, + Nr_blind +}; + +struct orgtype +{ + double range; + double dista; + double angle[2]; + double intersect; + E_jump edj[2]; + LiDARFeature ftype; + orgtype() + { + range = 0; + edj[Prev] = Nr_nor; + edj[Next] = Nr_nor; + ftype = Nor; + intersect = 2; + } +}; + +/*** Velodyne ***/ +namespace velodyne_ros +{ +struct EIGEN_ALIGN16 Point +{ + PCL_ADD_POINT4D; + float intensity; + float time; + std::uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace velodyne_ros +POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(std::uint16_t, ring, ring)) +/****************/ + +/*** Ouster ***/ +namespace ouster_ros +{ +struct EIGEN_ALIGN16 Point +{ + PCL_ADD_POINT4D; + float intensity; + std::uint32_t t; + std::uint16_t reflectivity; + uint8_t ring; + std::uint16_t ambient; + std::uint32_t range; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace ouster_ros +POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity) + (std::uint32_t, t, t)(std::uint16_t, reflectivity, + reflectivity)(std::uint8_t, ring, ring)(std::uint16_t, ambient, ambient)(std::uint32_t, range, range)) +/****************/ + +/*** Hesai_XT32 ***/ +namespace xt32_ros +{ +struct EIGEN_ALIGN16 Point +{ + PCL_ADD_POINT4D; + float intensity; + double timestamp; + std::uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace xt32_ros +POINT_CLOUD_REGISTER_POINT_STRUCT(xt32_ros::Point, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp)(std::uint16_t, ring, ring)) +/*****************/ + +/*** Hesai_Pandar128 ***/ +namespace Pandar128_ros +{ +struct EIGEN_ALIGN16 Point +{ + PCL_ADD_POINT4D; + uint8_t intensity; + double timestamp; + uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace Pandar128_ros +POINT_CLOUD_REGISTER_POINT_STRUCT(Pandar128_ros::Point, + (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(double, timestamp, timestamp)(std::uint16_t, ring, ring)) +/*****************/ + +/*** Robosense_Airy ***/ +namespace robosense_ros +{ +struct EIGEN_ALIGN16 Point +{ + PCL_ADD_POINT4D; + float intensity; + double timestamp; + uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace robosense_ros +POINT_CLOUD_REGISTER_POINT_STRUCT(robosense_ros::Point, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp)(std::uint16_t, ring, ring)) +/*****************/ + +class Preprocess +{ +public: + // EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Preprocess(); + ~Preprocess(); + + void process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out); + void process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out); + void set(bool feat_en, int lid_type, double bld, int pfilt_num); + + // sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud; + PointCloudXYZI pl_full, pl_corn, pl_surf; + PointCloudXYZI pl_buff[128]; // maximum 128 line lidar + vector typess[128]; // maximum 128 line lidar + int lidar_type, point_filter_num, N_SCANS; + + double blind, blind_sqr; + bool feature_enabled, given_offset_time; + std::shared_ptr> pub_full; + std::shared_ptr> pub_surf; + std::shared_ptr> pub_corn; + +private: + void avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg); + void oust64_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void xt32_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void Pandar128_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void robosense_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void l515_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + void give_feature(PointCloudXYZI &pl, vector &types); + void pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct); + int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); + bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); + bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); + + int group_size; + double disA, disB, inf_bound; + double limit_maxmid, limit_midmin, limit_maxmin; + double p2l_ratio; + double jump_up_limit, jump_down_limit; + double cos160; + double edgea, edgeb; + double smallp_intersect, smallp_ratio; + double vx, vy, vz; +}; +typedef std::shared_ptr PreprocessPtr; + +#endif // PREPROCESS_H_ \ No newline at end of file diff --git a/src/FAST-LIVO2/include/utils/color.h b/src/FAST-LIVO2/include/utils/color.h new file mode 100644 index 0000000..6ffc24f --- /dev/null +++ b/src/FAST-LIVO2/include/utils/color.h @@ -0,0 +1,24 @@ +#ifndef COLOR_H +#define COLOR_H + +#define RESET "\033[0m" +#define BLACK "\033[30m" /* Black */ +#define RED "\033[31m" /* Red */ +#define GREEN "\033[32m" /* Green */ +#define YELLOW "\033[33m" /* Yellow */ +#define BLUE "\033[34m" /* Blue */ +#define MAGENTA "\033[35m" /* Magenta */ +#define CYAN "\033[36m" /* Cyan */ +#define WHITE "\033[37m" /* White */ +#define REDPURPLE "\033[95m" /* Red Purple */ +#define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ +#define BOLDRED "\033[1m\033[31m" /* Bold Red */ +#define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ +#define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ +#define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ +#define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ +#define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ +#define BOLDWHITE "\033[1m\033[37m" /* Bold White */ +#define BOLDREDPURPLE "\033[1m\033[95m" /* Bold Red Purple */ + +#endif // COLOR_H diff --git a/src/FAST-LIVO2/include/utils/so3_math.h b/src/FAST-LIVO2/include/utils/so3_math.h new file mode 100755 index 0000000..a759489 --- /dev/null +++ b/src/FAST-LIVO2/include/utils/so3_math.h @@ -0,0 +1,89 @@ +#ifndef SO3_MATH_H +#define SO3_MATH_H + +#include +#include + +#define SKEW_SYM_MATRX(v) 0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0 + +template Eigen::Matrix Exp(const Eigen::Matrix &&ang) +{ + T ang_norm = ang.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (ang_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang / ang_norm; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_axis); + /// Roderigous Tranformation + return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; + } + else { return Eye3; } +} + +template Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) +{ + T ang_vel_norm = ang_vel.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + + if (ang_vel_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang_vel / ang_vel_norm; + Eigen::Matrix K; + + K << SKEW_SYM_MATRX(r_axis); + + T r_ang = ang_vel_norm * dt; + + /// Roderigous Tranformation + return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; + } + else { return Eye3; } +} + +template Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) +{ + T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (norm > 0.00001) + { + T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_ang); + + /// Roderigous Tranformation + return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; + } + else { return Eye3; } +} + +/* Logrithm of a Rotation Matrix */ +template Eigen::Matrix Log(const Eigen::Matrix &R) +{ + T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); + Eigen::Matrix K(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); + return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); +} + +template Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) +{ + T sy = sqrt(rot(0, 0) * rot(0, 0) + rot(1, 0) * rot(1, 0)); + bool singular = sy < 1e-6; + T x, y, z; + if (!singular) + { + x = atan2(rot(2, 1), rot(2, 2)); + y = atan2(-rot(2, 0), sy); + z = atan2(rot(1, 0), rot(0, 0)); + } + else + { + x = atan2(-rot(1, 2), rot(1, 1)); + y = atan2(-rot(2, 0), sy); + z = 0; + } + Eigen::Matrix ang(x, y, z); + return ang; +} + +#endif diff --git a/src/FAST-LIVO2/include/utils/types.h b/src/FAST-LIVO2/include/utils/types.h new file mode 100644 index 0000000..4e0564a --- /dev/null +++ b/src/FAST-LIVO2/include/utils/types.h @@ -0,0 +1,39 @@ +#ifndef TYPES_H +#define TYPES_H + +#include +#include +#include + +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointXYZRGB PointTypeRGB; +typedef pcl::PointXYZRGBA PointTypeRGBA; +typedef pcl::PointCloud PointCloudXYZI; +typedef std::vector> PointVector; +typedef pcl::PointCloud PointCloudXYZRGB; +typedef pcl::PointCloud PointCloudXYZRGBA; + +typedef Eigen::Vector2f V2F; +typedef Eigen::Vector2d V2D; +typedef Eigen::Vector3d V3D; +typedef Eigen::Matrix3d M3D; +typedef Eigen::Vector3f V3F; +typedef Eigen::Matrix3f M3F; + +#define MD(a, b) Eigen::Matrix +#define VD(a) Eigen::Matrix +#define MF(a, b) Eigen::Matrix +#define VF(a) Eigen::Matrix + +struct Pose6D +{ + /*** the preintegrated Lidar states at the time of IMU measurements in a frame ***/ + double offset_time; // the offset time of IMU measurement w.r.t the first lidar point + double acc[3]; // the preintegrated total acceleration (global frame) at the Lidar origin + double gyr[3]; // the unbiased angular velocity (body frame) at the Lidar origin + double vel[3]; // the preintegrated velocity (global frame) at the Lidar origin + double pos[3]; // the preintegrated position (global frame) at the Lidar origin + double rot[9]; // the preintegrated rotation (global frame) at the Lidar origin +}; + +#endif \ No newline at end of file diff --git a/src/FAST-LIVO2/include/utils/utils.h b/src/FAST-LIVO2/include/utils/utils.h new file mode 100644 index 0000000..9555f70 --- /dev/null +++ b/src/FAST-LIVO2/include/utils/utils.h @@ -0,0 +1,76 @@ +#ifndef UTILS_H +#define UTILS_H + +#include +#include // for int64_t +#include // for std::numeric_limits +#include // for std::out_of_range +#include +#include +#include +#include +#include +#include + +std::vector convertToIntVectorSafe(const std::vector& int64_vector); + +inline double stamp2Sec(const builtin_interfaces::msg::Time& stamp) +{ + return rclcpp::Time(stamp).seconds(); +} + +inline rclcpp::Time sec2Stamp(double timestamp) +{ + int32_t sec = std::floor(timestamp); + auto nanosec_d = (timestamp - std::floor(timestamp)) * 1e9; + uint32_t nanosec = nanosec_d; + return rclcpp::Time(sec, nanosec); +} + +namespace tf +{ + +inline geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw) +{ + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + return tf2::toMsg(q); +} + +inline geometry_msgs::msg::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw) +{ + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + return tf2::toMsg(q); +} + +inline tf2::Quaternion createQuaternionFromYaw(double yaw) +{ + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + return q; +} + +inline tf2::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw) +{ + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + return q; +} +} + +inline geometry_msgs::msg::TransformStamped createTransformStamped( + const tf2::Transform &transform, + const builtin_interfaces::msg::Time &stamp, + const std::string &frame_id, + const std::string &child_frame_id) +{ + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.stamp = stamp; + transform_stamped.header.frame_id = frame_id; + transform_stamped.child_frame_id = child_frame_id; + transform_stamped.transform = tf2::toMsg(transform); + return transform_stamped; +} + +#endif // UTILS_H \ No newline at end of file diff --git a/src/FAST-LIVO2/include/vio.h b/src/FAST-LIVO2/include/vio.h new file mode 100755 index 0000000..6a29010 --- /dev/null +++ b/src/FAST-LIVO2/include/vio.h @@ -0,0 +1,285 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. + +Multi-camera vectorization (N-camera, joint ESIKF, cross-camera photometric +consistency) ported from Omni-LIVO — see +docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md Phase 4. +*/ + +#ifndef VIO_H_ +#define VIO_H_ + +#include "voxel_map.h" +#include "feature.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct SubSparseMap +{ + vector propa_errors; + vector errors; + vector> warp_patch; + vector search_levels; + vector voxel_points; + vector inv_expo_list; + vector add_from_voxel_map; + vector camera_ids; //!< camera each entry above was retrieved for, parallel to the other arrays + + SubSparseMap() + { + propa_errors.reserve(SIZE_LARGE); + errors.reserve(SIZE_LARGE); + warp_patch.reserve(SIZE_LARGE); + search_levels.reserve(SIZE_LARGE); + voxel_points.reserve(SIZE_LARGE); + inv_expo_list.reserve(SIZE_LARGE); + add_from_voxel_map.reserve(SIZE_SMALL); + camera_ids.reserve(SIZE_LARGE); + }; + + void reset() + { + propa_errors.clear(); + errors.clear(); + warp_patch.clear(); + search_levels.clear(); + voxel_points.clear(); + inv_expo_list.clear(); + add_from_voxel_map.clear(); + camera_ids.clear(); + } +}; + +class Warp +{ +public: + Matrix2d A_cur_ref; + int search_level; + Warp(int level, Matrix2d warp_matrix) : search_level(level), A_cur_ref(warp_matrix) {} + ~Warp() {} +}; + +class VOXEL_POINTS +{ +public: + std::vector voxel_points; + int count; + double creation_timestamp_; //!< used by capVisualMap() to evict the oldest voxels first + VOXEL_POINTS(int num) : count(num), creation_timestamp_(-1.0) {} + ~VOXEL_POINTS() + { + for (VisualPoint* vp : voxel_points) + { + if (vp != nullptr) { delete vp; vp = nullptr; } + } + } +}; + +class VIOManager +{ +public: + int grid_size; + int max_total_points = 300; //!< cap on total retrieved+new points across all cameras per frame + int points_per_camera_min = 50; + int points_per_camera_max = 150; + vector cams; //!< one camera model per camera, indexed by cam_id + StatesGroup *state; + StatesGroup *state_propagat; + bool raycast_en = false; + std::vector>> rays_with_sample_points; // [cam_id][grid_idx][sample_points] + std::vector> border_flag; // [cam_id][grid_idx] + + // Per-camera extrinsics: Rci/Pci = IMU->camera, Rcl/Pcl = LiDAR->camera, + // Rcw/Pcw = world->camera (recomputed every frame from the current state). + std::vector Rci_vec; + std::vector Rcl_vec; + std::vector Rcw_vec; + std::vector Pci_vec; + std::vector Pcl_vec; + std::vector Pcw_vec; + + // Precomputed per-camera Jacobian factors (only depend on the fixed IMU->camera extrinsic). + std::vector Jdphi_dR_vec; + std::vector Jdp_dt_vec; + std::vector Jdp_dR_vec; + + M3D Rli; // IMU->LiDAR rotation (camera-independent) + V3D Pli; // IMU->LiDAR translation (camera-independent) + + vector grid_num; + vector map_index; + vector update_flag; + vector map_dist; + vector scan_value; + vector patch_buffer; + bool normal_en, inverse_composition_en, exposure_estimate_en, has_ref_patch_cache; + bool ncc_en = false, colmap_output_en = false; + + int width, height, grid_n_width, grid_n_height, length; + double image_resize_factor; + int patch_pyrimid_level, patch_size, patch_size_total, patch_size_half, border, warp_len; + int max_iterations, total_points; + + double img_point_cov, outlier_threshold, ncc_thre; + + // Per-camera working state for patch selection, mirroring the single-camera + // grid_num/map_dist/etc above but one instance per camera. + std::vector> grid_num_per_cam_; + std::vector> map_dist_per_cam_; + std::vector> retrieve_voxel_points_per_cam_; + std::vector> scan_grid_num_per_cam_; + std::vector> scan_value_per_cam_; + std::vector> scan_append_points_per_cam_; + + std::vector> retrieve_voxel_points_list_buffer_; + std::vector depth_imgs_buffer_; + std::vector grid_num_buffer_; + std::vector map_dist_buffer_; + std::vector retrieve_voxel_points_buffer_; + std::vector scan_value_buffer_; + + SubSparseMap *visual_submap; + + double compute_jacobian_time, update_ekf_time; + double ave_total = 0; + + int frame_count = 0; + bool plot_flag; + + Eigen::Matrix G, H_T_H; + Eigen::MatrixXd K, H_sub_inv; + + ofstream fout_camera, fout_colmap; + unordered_map feat_map; + unordered_map sub_feat_map; + unordered_map warp_map; + vector retrieve_voxel_points; + vector append_voxel_points; + FramePtr new_frame_; + std::vector imgs_cp, imgs_rgb; //!< per-camera debug/RGB-output copies of the current frame + cv::Mat panorama_image; //!< mosaic of imgs_rgb across all cameras, for /rgb_img publishing + + // Per-camera photometric correction (exposure normalization + simple + // vignetting model) applied before cross-camera photometric comparisons. + struct CameraPhotoParams + { + double exposure_factor; + std::vector vignetting; + bool parameters_initialized; + }; + std::vector camera_photo_params; + + void initializeCameraPhotoParams(); + float applyCameraPhotoCorrection(float intensity, int cam_id, const V2D &pixel_pos); + void addCrossCameraConsistencyConstraint(VisualPoint *pt, int source_cam_id, int target_cam_id, Eigen::MatrixXd &H_sub, Eigen::VectorXd &z, int level, int &row_offset); + + int total_cross_camera_observations = 0; + int successful_cross_camera_tracks = 0; + bool enable_cross_camera_tracking = false; + + enum CellType + { + TYPE_MAP = 1, + TYPE_POINTCLOUD, + TYPE_UNKNOWN + }; + + VIOManager(); + ~VIOManager(); + void updateStateInverse(const std::vector &imgs, int level); + void updateState(const std::vector &imgs, int level); + void processFrame(const std::vector &imgs, vector &pg, const unordered_map &feat_map, double frame_timestamp); + void retrieveFromVisualSparseMap(const std::vector imgs, vector &pg, const unordered_map &plane_map); + void generateVisualMapPoints(const std::vector &imgs, vector &pg); + void setImuToLidarExtrinsic(const V3D &transl, const M3D &rot); + void setLidarToCameraExtrinsic(std::vector> &R, std::vector> &P); + void initializeVIO(); + void initializeRaycast(); + void getImagePatch(cv::Mat img, V2D pc, float *patch_tmp, int level); + void computeProjectionJacobian(int cam_idx, V3D p, MD(2, 3) & J); + void computeJacobianAndUpdateEKF(const std::vector imgs); + void resetGrid(); + void updateVisualMapPoints(std::vector &imgs); + void getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, const SE3 &T_cur_ref, + const int level_ref, + const int pyramid_level, const int halfpatch_size, Matrix2d &A_cur_ref); + void getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, const V2D &px_ref, + const V3D &xyz_ref, const V3D &normal_ref, const SE3 &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref); + void warpAffine(const Matrix2d &A_cur_ref, const cv::Mat &img_ref, const Vector2d &px_ref, const int level_ref, const int search_level, + const int pyramid_level, const int halfpatch_size, float *patch); + void insertPointIntoVoxelMap(VisualPoint *pt_new); + void setCurrentTimestamp(double timestamp) { current_timestamp_ = timestamp; } + double current_timestamp_ = -1.0; + void plotTrackedPoints(); + void updateFrameState(StatesGroup state); + void projectPatchFromRefToCur(const unordered_map &plane_map); + void updateReferencePatch(const unordered_map &plane_map); + void precomputeReferencePatches(int level); + void dumpDataForColmap(); + double calculateNCC(float *ref_patch, float *cur_patch, int patch_size); + int getBestSearchLevel(const Matrix2d &A_cur_ref, const int max_level); + V3F getInterpolatedPixel(cv::Mat img, V2D pc); + + // Bookkeeping to bound memory growth over a long run (voxel/point/frame caps). + void cleanupCrossCameraData(); + void cleanupOldVisualPoints(); + void cleanupVisualMapByTimestamp(double oldest_kept_timestamp); + int max_point_age_frames = 50; + int cleanup_interval_frames = 10; + int last_cleanup_frame_id = 0; + + void cleanupOldFrames(); + std::deque frame_history_; + int max_frame_history = 3; + + // Bound visual-map memory by capping feat_map voxel count. When enabled and + // feat_map.size() > max_visual_voxels_, drop oldest voxels (by creation_timestamp_) + // until the cap is satisfied. + bool map_sliding_en_ = false; + int max_visual_voxels_ = 10000; + size_t capVisualMap(); + + // Adaptive per-camera measurement covariance scaling based on recent + // photometric error / point count (Omni-LIVO's "Adaptive Multi-View ESIKF"). + bool enable_dynamic_covariance_ = false; + int dynamic_cov_warmup_frames = 200; + double warmup_cov_scale = 500.0; + double min_cov_scale = 10.0; + double max_cov_scale = 2000.0; + double dynamic_cov_error_max = 50.0; + +private: + std::vector prev_cov_scale_per_cam_; + std::vector prev_avg_error_per_cam_; + std::vector prev_n_meas_per_cam_; + + double prev_cov_scale_ = 10.0; + double prev_avg_error_ = 5.0; + int prev_n_meas_ = 0; + + double calculateCoVarianceScale(double realtime_avg_error, int realtime_n_meas); + double calculateCoVarianceScalePerCam(int cam_idx, double realtime_avg_error, int realtime_n_meas); + + bool isRealCrossCameraPoint(VisualPoint *pt, int current_cam_id); + void updateCrossCameraHistory(VisualPoint *pt, int cam_id); +}; +typedef std::shared_ptr VIOManagerPtr; + +#endif // VIO_H_ diff --git a/src/FAST-LIVO2/include/visual_point.h b/src/FAST-LIVO2/include/visual_point.h new file mode 100644 index 0000000..f373bcb --- /dev/null +++ b/src/FAST-LIVO2/include/visual_point.h @@ -0,0 +1,88 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#ifndef LIVO_POINT_H_ +#define LIVO_POINT_H_ + +#include +#include +#include "common_lib.h" +#include "frame.h" + +class Feature; + +// Cross-camera bookkeeping for a VisualPoint, embedded directly in the point +// (rather than a separate map keyed by point) to avoid lookups on the hot +// path. This is the core state behind Omni-LIVO's Cross-View Temporal +// Migration: it tracks which camera(s) currently/previously see the point and +// how many times the point's "primary" observing camera has changed. +#define MAX_CAMERAS 10 // implementation ceiling for the visibility bitsets, not a geometry assumption + +struct CrossCameraData { + std::bitset currently_visible; // visibility in the current frame + std::bitset previously_visible; // visibility in the previous frame + + int8_t primary_cam_idx; // camera currently treated as this point's primary observer (-1 if none yet) + int8_t previous_cam_idx; // primary camera as of the previous frame + int8_t migration_source_cam; // camera the point migrated from, if any + uint8_t cross_camera_migrations; // number of times the primary camera has changed + bool has_migration_history; + + int last_seen_frame_id; + + CrossCameraData() : + primary_cam_idx(-1), + previous_cam_idx(-1), + migration_source_cam(-1), + cross_camera_migrations(0), + last_seen_frame_id(-1), + has_migration_history(false) + { + currently_visible.reset(); + previously_visible.reset(); + } +}; + +/// A visual map point on the surface of the scene. +class VisualPoint : boost::noncopyable +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static const int MAX_OBS = 15; //!< Maximum number of observations to keep per point (bounds obs_ growth). + + Vector3d pos_; //!< 3d pos of the point in the world coordinate frame. + Vector3d normal_; //!< Surface normal at point. + Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation. + Vector3d previous_normal_; //!< Last updated normal vector. + list obs_; //!< Reference patches which observe the point. + Eigen::Matrix3d covariance_; //!< Covariance of the point. + bool is_converged_; //!< True if the point is converged. + bool is_normal_initialized_; //!< True if the normal is initialized. + bool has_ref_patch_; //!< True if the point has a reference patch. + Feature *ref_patch; //!< Reference patch of the point. + + CrossCameraData cross_cam_data_; //!< Cross-camera visibility/migration bookkeeping for this point. + + std::bitset visible_cameras_cache_; //!< Cache of which cameras recently saw this point. + int cache_frame_id_; //!< Frame id the above cache was computed for. + + VisualPoint(const Vector3d &pos); + ~VisualPoint(); + void findMinScoreFeature(const Vector3d &framepos, Feature *&ftr) const; + void deleteNonRefPatchFeatures(); + void deleteFeatureRef(Feature *ftr); + void addFrameRef(Feature *ftr); + bool getCloseViewObs(const Vector3d &pos, Feature *&obs, const Vector2d &cur_px) const; +}; + +#endif // LIVO_POINT_H_ diff --git a/src/FAST-LIVO2/include/voxel_map.h b/src/FAST-LIVO2/include/voxel_map.h new file mode 100644 index 0000000..2f28d80 --- /dev/null +++ b/src/FAST-LIVO2/include/voxel_map.h @@ -0,0 +1,259 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#ifndef VOXEL_MAP_H_ +#define VOXEL_MAP_H_ + +#include "common_lib.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define VOXELMAP_HASH_P 116101 +#define VOXELMAP_MAX_N 10000000000 + +static int voxel_plane_id = 0; + +typedef struct VoxelMapConfig +{ + double max_voxel_size_; + int max_layer_; + int max_iterations_; + std::vector layer_init_num_; + int max_points_num_; + double planner_threshold_; + double beam_err_; + double dept_err_; + double sigma_num_; + bool is_pub_plane_map_; + + // config of local map sliding + double sliding_thresh; + bool map_sliding_en; + int half_map_size; +} VoxelMapConfig; + +typedef struct PointToPlane +{ + Eigen::Vector3d point_b_; + Eigen::Vector3d point_w_; + Eigen::Vector3d normal_; + Eigen::Vector3d center_; + Eigen::Matrix plane_var_; + M3D body_cov_; + int layer_; + double d_; + double eigen_value_; + bool is_valid_; + float dis_to_plane_; +} PointToPlane; + +typedef struct VoxelPlane +{ + Eigen::Vector3d center_; + Eigen::Vector3d normal_; + Eigen::Vector3d y_normal_; + Eigen::Vector3d x_normal_; + Eigen::Matrix3d covariance_; + Eigen::Matrix plane_var_; + float radius_ = 0; + float min_eigen_value_ = 1; + float mid_eigen_value_ = 1; + float max_eigen_value_ = 1; + float d_ = 0; + int points_size_ = 0; + bool is_plane_ = false; + bool is_init_ = false; + int id_ = 0; + bool is_update_ = false; + VoxelPlane() + { + plane_var_ = Eigen::Matrix::Zero(); + covariance_ = Eigen::Matrix3d::Zero(); + center_ = Eigen::Vector3d::Zero(); + normal_ = Eigen::Vector3d::Zero(); + } +} VoxelPlane; + +class VOXEL_LOCATION +{ +public: + int64_t x, y, z; + + VOXEL_LOCATION(int64_t vx = 0, int64_t vy = 0, int64_t vz = 0) : x(vx), y(vy), z(vz) {} + + bool operator==(const VOXEL_LOCATION &other) const { return (x == other.x && y == other.y && z == other.z); } +}; + +// Hash value +namespace std +{ +template <> struct hash +{ + int64_t operator()(const VOXEL_LOCATION &s) const + { + using std::hash; + using std::size_t; + return ((((s.z) * VOXELMAP_HASH_P) % VOXELMAP_MAX_N + (s.y)) * VOXELMAP_HASH_P) % VOXELMAP_MAX_N + (s.x); + } +}; +} // namespace std + +struct DS_POINT +{ + float xyz[3]; + float intensity; + int count = 0; +}; + +void calcBodyCov(Eigen::Vector3d &pb, const float range_inc, const float degree_inc, Eigen::Matrix3d &cov); + +class VoxelOctoTree +{ + +public: + VoxelOctoTree() = default; + std::vector temp_points_; + VoxelPlane *plane_ptr_; + int layer_; + int octo_state_; // 0 is end of tree, 1 is not + VoxelOctoTree *leaves_[8]; + double voxel_center_[3]; // x, y, z + std::vector layer_init_num_; + float quater_length_; + float planer_threshold_; + int points_size_threshold_; + int update_size_threshold_; + int max_points_num_; + int max_layer_; + int new_points_; + bool init_octo_; + bool update_enable_; + + VoxelOctoTree(int max_layer, int layer, int points_size_threshold, int max_points_num, float planer_threshold) + : max_layer_(max_layer), layer_(layer), points_size_threshold_(points_size_threshold), max_points_num_(max_points_num), + planer_threshold_(planer_threshold) + { + temp_points_.clear(); + octo_state_ = 0; + new_points_ = 0; + update_size_threshold_ = 5; + init_octo_ = false; + update_enable_ = true; + for (int i = 0; i < 8; i++) + { + leaves_[i] = nullptr; + } + plane_ptr_ = new VoxelPlane; + } + + ~VoxelOctoTree() + { + for (int i = 0; i < 8; i++) + { + delete leaves_[i]; + } + delete plane_ptr_; + } + void init_plane(const std::vector &points, VoxelPlane *plane); + void init_octo_tree(); + void cut_octo_tree(); + void UpdateOctoTree(const pointWithVar &pv); + + VoxelOctoTree *find_correspond(Eigen::Vector3d pw); + VoxelOctoTree *Insert(const pointWithVar &pv); +}; + +void loadVoxelConfig(rclcpp::Node::SharedPtr &node, VoxelMapConfig &voxel_config); + +class VoxelMapManager +{ +public: + VoxelMapManager() = default; + VoxelMapConfig config_setting_; + int current_frame_id_ = 0; + rclcpp::Publisher::SharedPtr voxel_map_pub_; + std::unordered_map voxel_map_; + + PointCloudXYZI::Ptr feats_undistort_; + PointCloudXYZI::Ptr feats_down_body_; + PointCloudXYZI::Ptr feats_down_world_; + + M3D extR_; + V3D extT_; + float build_residual_time, ekf_time; + float ave_build_residual_time = 0.0; + float ave_ekf_time = 0.0; + int scan_count = 0; + StatesGroup state_; + V3D position_last_; + + V3D last_slide_position = {0,0,0}; + + geometry_msgs::msg::Quaternion geoQuat_; + + int feats_down_size_; + int effct_feat_num_; + std::vector cross_mat_list_; + std::vector body_cov_list_; + std::vector pv_list_; + std::vector ptpl_list_; + + VoxelMapManager(VoxelMapConfig &config_setting, std::unordered_map &voxel_map) + : config_setting_(config_setting), voxel_map_(voxel_map) + { + current_frame_id_ = 0; + feats_undistort_.reset(new PointCloudXYZI()); + feats_down_body_.reset(new PointCloudXYZI()); + feats_down_world_.reset(new PointCloudXYZI()); + }; + + void StateEstimation(StatesGroup &state_propagat); + void TransformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, + pcl::PointCloud::Ptr &trans_cloud); + + void BuildVoxelMap(); + V3F RGBFromVoxel(const V3D &input_point); + + void UpdateVoxelMap(const std::vector &input_points); + + void BuildResidualListOMP(std::vector &pv_list, std::vector &ptpl_list); + + void build_single_residual(pointWithVar &pv, const VoxelOctoTree *current_octo, const int current_layer, bool &is_sucess, double &prob, + PointToPlane &single_ptpl); + + void pubVoxelMap(); + + void mapSliding(); + void clearMemOutOfMap(const int& x_max,const int& x_min,const int& y_max,const int& y_min,const int& z_max,const int& z_min ); + +private: + void GetUpdatePlane(const VoxelOctoTree *current_octo, const int pub_max_voxel_layer, std::vector &plane_list); + + void pubSinglePlane(visualization_msgs::msg::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane, const float alpha, + const Eigen::Vector3d rgb); + void CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec, geometry_msgs::msg::Quaternion &q); + + void mapJet(double v, double vmin, double vmax, uint8_t &r, uint8_t &g, uint8_t &b); +}; +typedef std::shared_ptr VoxelMapManagerPtr; + +#endif // VOXEL_MAP_H_ \ No newline at end of file diff --git a/src/FAST-LIVO2/launch/mapping_avia_marslvig.launch.py b/src/FAST-LIVO2/launch/mapping_avia_marslvig.launch.py new file mode 100755 index 0000000..7e78750 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_avia_marslvig.launch.py @@ -0,0 +1,111 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + # Find path + config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config") + rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "M300.rviz") + + #Load parameters + avia_config_cmd = os.path.join(config_file_dir, "MARS_LVIG.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_MARS_LVIG.yaml") + + # Param use_rviz + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", + default_value="False", + description="Whether to launch Rviz2", + ) + + avia_config_arg = DeclareLaunchArgument( + 'avia_params_file', + default_value=avia_config_cmd, + description='Full path to the ROS2 parameters file to use for fast_livo2 nodes', + ) + + camera_config_arg = DeclareLaunchArgument( + 'camera_params_file', + default_value=camera_config_cmd, + description='Full path to the ROS2 parameters file to use for vikit_ros nodes', + ) + + # https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40 + use_respawn_arg = DeclareLaunchArgument( + 'use_respawn', + default_value='True', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + avia_params_file = LaunchConfiguration('avia_params_file') + camera_params_file = LaunchConfiguration('camera_params_file') + use_respawn = LaunchConfiguration('use_respawn') + + return LaunchDescription([ + use_rviz_arg, + avia_config_arg, + camera_config_arg, + use_respawn_arg, + + # use parameter_blackboard as global parameters server and load camera params + Node( + package='demo_nodes_cpp', + executable='parameter_blackboard', + name='parameter_blackboard', + # namespace='laserMapping', + parameters=[ + camera_params_file, + ], + output='screen' + ), + + # republish compressed image to raw image + # https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2 + # ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image + Node( + package="image_transport", + executable="republish", + name="republish", + arguments=[ # Array of strings/parametric arguments that will end up in process's argv + 'compressed', + 'raw', + ], + remappings=[ + ("in", "/left_camera/image"), + ("out", "/left_camera/image") + ], + output="screen", + respawn=use_respawn, + ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[ + avia_params_file, + ], + # https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html + prefix=[ + # ("gdb -ex run --args"), + # ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes") + ], + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_aviz.launch.py b/src/FAST-LIVO2/launch/mapping_aviz.launch.py new file mode 100644 index 0000000..a9b17a5 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_aviz.launch.py @@ -0,0 +1,117 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + # Find path + config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config") + rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "fast_livo2.rviz") + + #Load parameters + avia_config_cmd = os.path.join(config_file_dir, "avia.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_pinhole.yaml") + + # Param use_rviz + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", + default_value="False", + description="Whether to launch Rviz2", + ) + + avia_config_arg = DeclareLaunchArgument( + 'avia_params_file', + default_value=avia_config_cmd, + description='Full path to the ROS2 parameters file to use for fast_livo2 nodes', + ) + + camera_config_arg = DeclareLaunchArgument( + 'camera_params_file', + default_value=camera_config_cmd, + description='Full path to the ROS2 parameters file to use for vikit_ros nodes', + ) + + # https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40 + use_respawn_arg = DeclareLaunchArgument( + 'use_respawn', + default_value='True', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + avia_params_file = LaunchConfiguration('avia_params_file') + camera_params_file = LaunchConfiguration('camera_params_file') + use_respawn = LaunchConfiguration('use_respawn') + + return LaunchDescription([ + use_rviz_arg, + avia_config_arg, + camera_config_arg, + use_respawn_arg, + + # play ros2 bag + # ExecuteProcess( + # cmd=[['ros2 bag play ', '~/datasets/Retail_Street ', '--clock ', "-l"]], + # shell=True + # ), + + # use parameter_blackboard as global parameters server and load camera params + Node( + package='demo_nodes_cpp', + executable='parameter_blackboard', + name='parameter_blackboard', + # namespace='laserMapping', + parameters=[ + camera_params_file, + ], + output='screen' + ), + + # republish compressed image to raw image + # https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2 + # ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image + Node( + package="image_transport", + executable="republish", + name="republish", + arguments=[ # Array of strings/parametric arguments that will end up in process's argv + 'compressed', + 'raw', + ], + remappings=[ + ("in", "/left_camera/image"), + ("out", "/left_camera/image") + ], + output="screen", + respawn=use_respawn, + ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[ + avia_params_file, + ], + # https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html + prefix=[ + # ("gdb -ex run --args"), + # ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes") + ], + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_hesaixt32_hilti22.launch b/src/FAST-LIVO2/launch/mapping_hesaixt32_hilti22.launch new file mode 100644 index 0000000..59257b0 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_hesaixt32_hilti22.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/src/FAST-LIVO2/launch/mapping_mid360s.launch.py b/src/FAST-LIVO2/launch/mapping_mid360s.launch.py new file mode 100644 index 0000000..2694640 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_mid360s.launch.py @@ -0,0 +1,99 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +from launch.substitutions import Command + +def generate_launch_description(): + + # Find path + pkg_dir = get_package_share_directory("fast_livo") + config_file_dir = os.path.join(pkg_dir, "config") + rviz_config_file = os.path.join(pkg_dir, "rviz_cfg", "fast_livo2.rviz") + urdf_file = os.path.join(pkg_dir, "urdf", "fori_robot.urdf") + + # Load parameters + mid360s_config_cmd = os.path.join(config_file_dir, "mid360s.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_mid360s.yaml") + + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", + default_value="False", + description="Whether to launch Rviz2", + ) + + mid360s_config_arg = DeclareLaunchArgument( + 'mid360s_params_file', + default_value=mid360s_config_cmd, + description='Full path to the ROS2 parameters file for fast_livo2 (MID-360S)', + ) + + camera_config_arg = DeclareLaunchArgument( + 'camera_params_file', + default_value=camera_config_cmd, + description='Full path to the ROS2 parameters file for camera intrinsics', + ) + + use_respawn_arg = DeclareLaunchArgument( + 'use_respawn', + default_value='True', + description='Whether to respawn if a node crashes.', + ) + + mid360s_params_file = LaunchConfiguration('mid360s_params_file') + camera_params_file = LaunchConfiguration('camera_params_file') + use_respawn = LaunchConfiguration('use_respawn') + + return LaunchDescription([ + use_rviz_arg, + mid360s_config_arg, + camera_config_arg, + use_respawn_arg, + + # Robot model (URDF) — aft_mapped 기준 센서 TF 정적 발행 + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[{ + 'robot_description': open(urdf_file).read() + }], + output='screen' + ), + + # Camera params are read remotely from parameter_blackboard by vikit camera_loader + Node( + package='demo_nodes_cpp', + executable='parameter_blackboard', + name='parameter_blackboard', + parameters=[camera_params_file], + output='screen' + ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[mid360s_params_file], + # Force system libusb over /opt/MVS bundled version (which lacks libusb_set_option) + additional_env={ + "LD_PRELOAD": "/usr/lib/x86_64-linux-gnu/libusb-1.0.so.0" + }, + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_mid360s_cam1.launch.py b/src/FAST-LIVO2/launch/mapping_mid360s_cam1.launch.py new file mode 100644 index 0000000..6264bb5 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_mid360s_cam1.launch.py @@ -0,0 +1,94 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** +# cam1 전용 launch — mid360s.yaml + extrin_cam1.yaml + camera_cam1.yaml (4mm, 2026-07-08 캘리브레이션) + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + pkg_dir = get_package_share_directory("fast_livo") + config_file_dir = os.path.join(pkg_dir, "config") + rviz_config_file = os.path.join(pkg_dir, "rviz_cfg", "fast_livo2.rviz") + urdf_file = os.path.join(pkg_dir, "urdf", "fori_robot.urdf") + + mid360s_config_cmd = os.path.join(config_file_dir, "mid360s.yaml") + extrin_config_cmd = os.path.join(config_file_dir, "extrin_cam1.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_cam1.yaml") + + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", default_value="False", description="Whether to launch Rviz2" + ) + mid360s_config_arg = DeclareLaunchArgument( + "mid360s_params_file", default_value=mid360s_config_cmd, + description="Full path to mid360s.yaml" + ) + extrin_config_arg = DeclareLaunchArgument( + "extrin_params_file", default_value=extrin_config_cmd, + description="Full path to cam1 extrinsic override yaml" + ) + camera_config_arg = DeclareLaunchArgument( + "camera_params_file", default_value=camera_config_cmd, + description="Full path to camera intrinsics yaml (cam1, 4mm, 1440x1080 scale=1.0)" + ) + use_respawn_arg = DeclareLaunchArgument( + "use_respawn", default_value="True", description="Whether to respawn if a node crashes." + ) + + mid360s_params_file = LaunchConfiguration("mid360s_params_file") + extrin_params_file = LaunchConfiguration("extrin_params_file") + camera_params_file = LaunchConfiguration("camera_params_file") + use_respawn = LaunchConfiguration("use_respawn") + + return LaunchDescription([ + use_rviz_arg, + mid360s_config_arg, + extrin_config_arg, + camera_config_arg, + use_respawn_arg, + + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + parameters=[{"robot_description": open(urdf_file).read()}], + output="screen" + ), + + Node( + package="demo_nodes_cpp", + executable="parameter_blackboard", + name="parameter_blackboard", + parameters=[camera_params_file], + output="screen" + ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[ + mid360s_params_file, + extrin_params_file, + {"common": {"img_topic": "/cam1/image"}}, + ], + additional_env={ + "LD_PRELOAD": "/usr/lib/x86_64-linux-gnu/libusb-1.0.so.0" + }, + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_mid360s_cam2.launch.py b/src/FAST-LIVO2/launch/mapping_mid360s_cam2.launch.py new file mode 100644 index 0000000..ba2bae2 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_mid360s_cam2.launch.py @@ -0,0 +1,94 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** +# cam2 전용 launch — mid360s.yaml + extrin_cam2.yaml + camera_cam2.yaml (4mm, 2026-07-08 캘리브레이션) + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + pkg_dir = get_package_share_directory("fast_livo") + config_file_dir = os.path.join(pkg_dir, "config") + rviz_config_file = os.path.join(pkg_dir, "rviz_cfg", "fast_livo2.rviz") + urdf_file = os.path.join(pkg_dir, "urdf", "fori_robot.urdf") + + mid360s_config_cmd = os.path.join(config_file_dir, "mid360s.yaml") + extrin_config_cmd = os.path.join(config_file_dir, "extrin_cam2.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_cam2.yaml") + + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", default_value="False", description="Whether to launch Rviz2" + ) + mid360s_config_arg = DeclareLaunchArgument( + "mid360s_params_file", default_value=mid360s_config_cmd, + description="Full path to mid360s.yaml" + ) + extrin_config_arg = DeclareLaunchArgument( + "extrin_params_file", default_value=extrin_config_cmd, + description="Full path to cam2 extrinsic override yaml" + ) + camera_config_arg = DeclareLaunchArgument( + "camera_params_file", default_value=camera_config_cmd, + description="Full path to camera intrinsics yaml (cam2, 4mm)" + ) + use_respawn_arg = DeclareLaunchArgument( + "use_respawn", default_value="True", description="Whether to respawn if a node crashes." + ) + + mid360s_params_file = LaunchConfiguration("mid360s_params_file") + extrin_params_file = LaunchConfiguration("extrin_params_file") + camera_params_file = LaunchConfiguration("camera_params_file") + use_respawn = LaunchConfiguration("use_respawn") + + return LaunchDescription([ + use_rviz_arg, + mid360s_config_arg, + extrin_config_arg, + camera_config_arg, + use_respawn_arg, + + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + parameters=[{"robot_description": open(urdf_file).read()}], + output="screen" + ), + + Node( + package="demo_nodes_cpp", + executable="parameter_blackboard", + name="parameter_blackboard", + parameters=[camera_params_file], + output="screen" + ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[ + mid360s_params_file, + extrin_params_file, + {"common": {"img_topic": "/cam2/image"}}, + ], + additional_env={ + "LD_PRELOAD": "/usr/lib/x86_64-linux-gnu/libusb-1.0.so.0" + }, + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_mid360s_cam3.launch.py b/src/FAST-LIVO2/launch/mapping_mid360s_cam3.launch.py new file mode 100644 index 0000000..8501974 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_mid360s_cam3.launch.py @@ -0,0 +1,107 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** +# cam3 (4mm lens) 전용 launch — mid360s.yaml + camera_cam3.yaml + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + pkg_dir = get_package_share_directory("fast_livo") + config_file_dir = os.path.join(pkg_dir, "config") + rviz_config_file = os.path.join(pkg_dir, "rviz_cfg", "fast_livo2.rviz") + urdf_file = os.path.join(pkg_dir, "urdf", "fori_robot.urdf") + + mid360s_config_cmd = os.path.join(config_file_dir, "mid360s.yaml") + extrin_config_cmd = os.path.join(config_file_dir, "extrin_cam3.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_cam3.yaml") + + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", + default_value="False", + description="Whether to launch Rviz2", + ) + + mid360s_config_arg = DeclareLaunchArgument( + 'mid360s_params_file', + default_value=mid360s_config_cmd, + description='Full path to the ROS2 parameters file for fast_livo2 (MID-360S)', + ) + + extrin_config_arg = DeclareLaunchArgument( + 'extrin_params_file', + default_value=extrin_config_cmd, + description='Full path to cam3 extrinsic override yaml', + ) + + camera_config_arg = DeclareLaunchArgument( + 'camera_params_file', + default_value=camera_config_cmd, + description='Full path to the ROS2 parameters file for camera intrinsics (cam3, 4mm)', + ) + + use_respawn_arg = DeclareLaunchArgument( + 'use_respawn', + default_value='True', + description='Whether to respawn if a node crashes.', + ) + + mid360s_params_file = LaunchConfiguration('mid360s_params_file') + extrin_params_file = LaunchConfiguration('extrin_params_file') + camera_params_file = LaunchConfiguration('camera_params_file') + use_respawn = LaunchConfiguration('use_respawn') + + return LaunchDescription([ + use_rviz_arg, + mid360s_config_arg, + extrin_config_arg, + camera_config_arg, + use_respawn_arg, + + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[{ + 'robot_description': open(urdf_file).read() + }], + output='screen' + ), + + Node( + package='demo_nodes_cpp', + executable='parameter_blackboard', + name='parameter_blackboard', + parameters=[camera_params_file], + output='screen' + ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[ + mid360s_params_file, + extrin_params_file, + {"common": {"img_topic": "/camera3/image"}}, + ], + additional_env={ + "LD_PRELOAD": "/usr/lib/x86_64-linux-gnu/libusb-1.0.so.0" + }, + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_mid360s_dualcam.launch.py b/src/FAST-LIVO2/launch/mapping_mid360s_dualcam.launch.py new file mode 100644 index 0000000..b7e4187 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_mid360s_dualcam.launch.py @@ -0,0 +1,110 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** +# Dual-camera (cam1 + cam2) launch. Unlike mapping_mid360s_cam{1,2,3}.launch.py +# (which each swap in a single alternative camera), this launches the +# multi-camera VIOManager path with both cameras subscribed simultaneously. +# See docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md. + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + pkg_dir = get_package_share_directory("fast_livo") + config_file_dir = os.path.join(pkg_dir, "config") + rviz_config_file = os.path.join(pkg_dir, "rviz_cfg", "fast_livo2.rviz") + urdf_file = os.path.join(pkg_dir, "urdf", "fori_robot.urdf") + + mid360s_config_cmd = os.path.join(config_file_dir, "mid360s.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_dualcam_cam1_cam2.yaml") + camera_array_config_cmd = os.path.join(config_file_dir, "extrin_dualcam_cam1_cam2.yaml") + + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", default_value="False", description="Whether to launch Rviz2" + ) + mid360s_config_arg = DeclareLaunchArgument( + "mid360s_params_file", default_value=mid360s_config_cmd, + description="Full path to mid360s.yaml (scalar/common parameters)" + ) + camera_config_arg = DeclareLaunchArgument( + "camera_params_file", default_value=camera_config_cmd, + description="Full path to the cam_0/cam_1 intrinsics yaml for parameter_blackboard" + ) + camera_array_config_arg = DeclareLaunchArgument( + "camera_array_config_file", default_value=camera_array_config_cmd, + description="Full path to the yaml-cpp-parsed multi-camera extrinsics/topics array" + ) + use_respawn_arg = DeclareLaunchArgument( + "use_respawn", default_value="True", description="Whether to respawn if a node crashes." + ) + + mid360s_params_file = LaunchConfiguration("mid360s_params_file") + camera_params_file = LaunchConfiguration("camera_params_file") + camera_array_config_file = LaunchConfiguration("camera_array_config_file") + use_respawn = LaunchConfiguration("use_respawn") + + return LaunchDescription([ + use_rviz_arg, + mid360s_config_arg, + camera_config_arg, + camera_array_config_arg, + use_respawn_arg, + + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + parameters=[{"robot_description": open(urdf_file).read()}], + output="screen" + ), + + # Camera intrinsics for both cameras (cam_0/cam_1 namespaces), read + # remotely by vk::camera_loader::loadFromRosNs. + Node( + package="demo_nodes_cpp", + executable="parameter_blackboard", + name="parameter_blackboard", + parameters=[camera_params_file], + output="screen" + ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[ + mid360s_params_file, + { + "common": { + "camera_config_path": camera_array_config_file, + # hik_camera_ros2_driver stamps images from the shared LiDAR-packet + # timestamp (see docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md), so two + # independently-read cameras can legitimately differ by close to one + # MID360 LiDAR period (~0.1s @ 10Hz). Loosen from Omni-LIVO's 1ms + # default until per-camera hardware trigger timestamps are wired up; + # tighten this once that's done. + "camera_time_tolerance": 0.12, + } + }, + {"vio": {"enable_cross_camera_tracking": True}}, + ], + additional_env={ + "LD_PRELOAD": "/usr/lib/x86_64-linux-gnu/libusb-1.0.so.0" + }, + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_mid360s_triplecam.launch.py b/src/FAST-LIVO2/launch/mapping_mid360s_triplecam.launch.py new file mode 100644 index 0000000..6ce8482 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_mid360s_triplecam.launch.py @@ -0,0 +1,105 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** +# Triple-camera (cam1 + cam2 + cam3) launch. Same multi-camera VIOManager path +# as mapping_mid360s_dualcam.launch.py, extended to a 3rd camera. +# See docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md. + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + pkg_dir = get_package_share_directory("fast_livo") + config_file_dir = os.path.join(pkg_dir, "config") + rviz_config_file = os.path.join(pkg_dir, "rviz_cfg", "fast_livo2.rviz") + urdf_file = os.path.join(pkg_dir, "urdf", "fori_robot.urdf") + + mid360s_config_cmd = os.path.join(config_file_dir, "mid360s.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_triplecam_cam1_cam2_cam3.yaml") + camera_array_config_cmd = os.path.join(config_file_dir, "extrin_triplecam_cam1_cam2_cam3.yaml") + + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", default_value="False", description="Whether to launch Rviz2" + ) + mid360s_config_arg = DeclareLaunchArgument( + "mid360s_params_file", default_value=mid360s_config_cmd, + description="Full path to mid360s.yaml (scalar/common parameters)" + ) + camera_config_arg = DeclareLaunchArgument( + "camera_params_file", default_value=camera_config_cmd, + description="Full path to the cam_0/cam_1/cam_2 intrinsics yaml for parameter_blackboard" + ) + camera_array_config_arg = DeclareLaunchArgument( + "camera_array_config_file", default_value=camera_array_config_cmd, + description="Full path to the yaml-cpp-parsed multi-camera extrinsics/topics array" + ) + use_respawn_arg = DeclareLaunchArgument( + "use_respawn", default_value="True", description="Whether to respawn if a node crashes." + ) + + mid360s_params_file = LaunchConfiguration("mid360s_params_file") + camera_params_file = LaunchConfiguration("camera_params_file") + camera_array_config_file = LaunchConfiguration("camera_array_config_file") + use_respawn = LaunchConfiguration("use_respawn") + + return LaunchDescription([ + use_rviz_arg, + mid360s_config_arg, + camera_config_arg, + camera_array_config_arg, + use_respawn_arg, + + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + parameters=[{"robot_description": open(urdf_file).read()}], + output="screen" + ), + + # Camera intrinsics for all three cameras (cam_0/cam_1/cam_2 namespaces), + # read remotely by vk::camera_loader::loadFromRosNs. + Node( + package="demo_nodes_cpp", + executable="parameter_blackboard", + name="parameter_blackboard", + parameters=[camera_params_file], + output="screen" + ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[ + mid360s_params_file, + { + "common": { + "camera_config_path": camera_array_config_file, + # See mapping_mid360s_dualcam.launch.py — same shared + # LiDAR-packet-timestamp quantization applies with 3 cameras. + "camera_time_tolerance": 0.12, + } + }, + {"vio": {"enable_cross_camera_tracking": True}}, + ], + additional_env={ + "LD_PRELOAD": "/usr/lib/x86_64-linux-gnu/libusb-1.0.so.0" + }, + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_ouster_ntu.launch.py b/src/FAST-LIVO2/launch/mapping_ouster_ntu.launch.py new file mode 100644 index 0000000..1b1328f --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_ouster_ntu.launch.py @@ -0,0 +1,117 @@ +#!/usr/bin/python3 +# -- coding: utf-8 --** + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + # Find path + config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config") + rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "ntu_viral.rviz") + + #Load parameters + avia_config_cmd = os.path.join(config_file_dir, "NTU_VIRAL.yaml") + camera_config_cmd = os.path.join(config_file_dir, "camera_NTU_VIRAL.yaml") + + # Param use_rviz + use_rviz_arg = DeclareLaunchArgument( + "use_rviz", + default_value="False", + description="Whether to launch Rviz2", + ) + + avia_config_arg = DeclareLaunchArgument( + 'avia_params_file', + default_value=avia_config_cmd, + description='Full path to the ROS2 parameters file to use for fast_livo2 nodes', + ) + + camera_config_arg = DeclareLaunchArgument( + 'camera_params_file', + default_value=camera_config_cmd, + description='Full path to the ROS2 parameters file to use for vikit_ros nodes', + ) + + # https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40 + use_respawn_arg = DeclareLaunchArgument( + 'use_respawn', + default_value='True', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + avia_params_file = LaunchConfiguration('avia_params_file') + camera_params_file = LaunchConfiguration('camera_params_file') + use_respawn = LaunchConfiguration('use_respawn') + + return LaunchDescription([ + use_rviz_arg, + avia_config_arg, + camera_config_arg, + use_respawn_arg, + + # play ros2 bag + # ExecuteProcess( + # cmd=[['ros2 bag play ', '~/datasets/Retail_Street ', '--clock ', "-l"]], + # shell=True + # ), + + # use parameter_blackboard as global parameters server and load camera params + Node( + package='demo_nodes_cpp', + executable='parameter_blackboard', + name='parameter_blackboard', + # namespace='laserMapping', + parameters=[ + camera_params_file, + ], + output='screen' + ), + + # republish compressed image to raw image + # https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2 + # ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image + Node( + package="image_transport", + executable="republish", + name="republish", + arguments=[ # Array of strings/parametric arguments that will end up in process's argv + 'compressed', + 'raw', + ], + remappings=[ + ("in", "/left_camera/image"), + ("out", "/left_camera/image") + ], + output="screen", + respawn=use_respawn, + ), + + Node( + package="fast_livo", + executable="fastlivo_mapping", + name="laserMapping", + parameters=[ + avia_params_file, + ], + # https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html + prefix=[ + # ("gdb -ex run --args"), + # ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes") + ], + output="screen" + ), + + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + output="screen" + ), + ]) diff --git a/src/FAST-LIVO2/package.xml b/src/FAST-LIVO2/package.xml new file mode 100755 index 0000000..4cd82d3 --- /dev/null +++ b/src/FAST-LIVO2/package.xml @@ -0,0 +1,59 @@ + + + fast_livo + 0.0.0 + + + This is a modified version of LOAM which is original algorithm + is described in the following paper: + J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. + Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + + + claydergc + + BSD + + Ji Zhang + + ament_cmake + + rclcpp + rclpy + sensor_msgs + geometry_msgs + visualization_msgs + nav_msgs + std_msgs + tf2_ros + pcl_ros + pcl_conversions + livox_ros_driver2 + vikit_common + vikit_ros + cv_bridge + image_transport + libopencv-dev + sophus + eigen + fmt + yaml-cpp + + cv_bridge + image_transport + libopencv-dev + sensor_msgs + std_msgs + yaml-cpp + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + + diff --git a/src/FAST-LIVO2/pics/Framework.png b/src/FAST-LIVO2/pics/Framework.png new file mode 100644 index 0000000..c2f1479 Binary files /dev/null and b/src/FAST-LIVO2/pics/Framework.png differ diff --git a/src/FAST-LIVO2/pics/debug_error.png b/src/FAST-LIVO2/pics/debug_error.png new file mode 100644 index 0000000..3baa521 Binary files /dev/null and b/src/FAST-LIVO2/pics/debug_error.png differ diff --git a/src/FAST-LIVO2/pics/rosgraph.png b/src/FAST-LIVO2/pics/rosgraph.png new file mode 100644 index 0000000..26ba96f Binary files /dev/null and b/src/FAST-LIVO2/pics/rosgraph.png differ diff --git a/src/FAST-LIVO2/rviz_cfg/M300.rviz b/src/FAST-LIVO2/rviz_cfg/M300.rviz new file mode 100755 index 0000000..84a33a6 --- /dev/null +++ b/src/FAST-LIVO2/rviz_cfg/M300.rviz @@ -0,0 +1,671 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Status1 + - /Axes1 + - /mapping1 + - /mapping1/currPoints1 + - /mapping1/surround1 + - /mapping1/surround1/Autocompute Value Bounds1 + - /mapping1/PointCloud21 + - /Odometry1 + - /Odometry1/Odometry1 + - /Odometry1/Odometry1/Shape1 + - /Path1 + - /currPoints1/Autocompute Value Bounds1 + - /MarkerArray1/Namespaces1 + - /currPoints2/Autocompute Value Bounds1 + - /Odometry2/Shape1 + - /MarkerArray3 + - /MarkerArray4 + - /MarkerArray5 + - /Image1 + Splitter Ratio: 0.34272301197052 + Tree Height: 538 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Name: Time + SyncMode: 0 + SyncSource: surround +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 160 + Reference Frame: + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Axes + Enabled: true + Length: 4 + Name: Axes + Radius: 1.2000000476837158 + Reference Frame: + Show Trail: false + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 199; 228; 247 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: true + - Alpha: 0.5 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 15 + Min Value: -5 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 238; 238; 236 + Color Transformer: RGB8 + Decay Time: 10000 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 159 + Min Color: 0; 0; 0 + Min Intensity: 5 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 1 + Size (m): 0.004999999888241291 + Style: Points + Topic: /cloud_registered + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.019999999552965164 + Style: Squares + Topic: /cloud_effected + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: mapping + - Class: rviz_common/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.0010000000474974513 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 5 + Axes Radius: 1 + Color: 255; 85; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.800000011920929 + Shaft Radius: 0.5 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: true + Enabled: true + Name: Odometry + - Alpha: 0 + Buffer Length: 2 + Class: rviz_default_plugins/Path + Color: 25; 255; 255 + Enabled: true + Head Diameter: 0 + Head Length: 0 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.699999988079071 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 25; 255; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 + Topic: /path + Unreliable: false + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 + Color Transformer: Intensity + Decay Time: 1000 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_voxel + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/Marker + Enabled: true + Marker Topic: /planner_normal + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Marker Topic: /voxels + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 245; 121; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 15 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Marker Topic: /visualization_marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 115; 210; 22 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 12 + Size (m): 0.05000000074505806 + Style: Points + Topic: /cloud_visual_sub_map + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 237; 212; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_sample_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 239; 41; 41 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 20 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map_fov + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.699999988079071 + Axes Radius: 0.20000000298023224 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Marker Topic: /waypoint_planner/visualize + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: /fsm_node/visualization/exp_traj + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Marker Topic: /fsm_node/visualization/exp_sfcs + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Image Topic: /rgb_img + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 582.7694702148438 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 463.3948974609375 + Y: -4.546019554138184 + Z: -4.951948722009547e-05 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -0.26979732513427734 + Target Frame: drone + Yaw: 3.1317780017852783 + Saved: + - Class: rviz_default_plugins/Orbit + Distance: 117.53474426269531 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -35.713138580322266 + Y: 36.932613372802734 + Z: 4.459701061248779 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.19539840519428253 + Target Frame: + Yaw: 0.17540442943572998 + - Class: rviz_default_plugins/Orbit + Distance: 109.3125 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -22.092714309692383 + Y: 63.322662353515625 + Z: 14.125411987304688 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.035398442298173904 + Target Frame: + Yaw: 5.793589115142822 + - Class: rviz_default_plugins/Orbit + Distance: 85.65605163574219 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 28.252656936645508 + Y: -35.49672317504883 + Z: -36.31112289428711 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5653983950614929 + Target Frame: + Yaw: 0.9104044437408447 + - Class: rviz_default_plugins/Orbit + Distance: 60.1053581237793 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 30.61589241027832 + Y: 29.98663330078125 + Z: -12.290168762207031 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.315398633480072 + Target Frame: + Yaw: 5.788588047027588 +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2488 + X: 72 + Y: 27 diff --git a/src/FAST-LIVO2/rviz_cfg/fast_livo2.rviz b/src/FAST-LIVO2/rviz_cfg/fast_livo2.rviz new file mode 100755 index 0000000..f89870f --- /dev/null +++ b/src/FAST-LIVO2/rviz_cfg/fast_livo2.rviz @@ -0,0 +1,782 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Axes1 + - /mapping1 + - /mapping1/currPoints1 + - /mapping1/surround1 + - /mapping1/surround1/Autocompute Value Bounds1 + - /mapping1/PointCloud21 + - /Odometry1 + - /Odometry1/Odometry1 + - /Odometry1/Odometry1/Shape1 + - /Path1 + - /currPoints1/Autocompute Value Bounds1 + - /Marker1 + - /MarkerArray1/Namespaces1 + - /currPoints2/Autocompute Value Bounds1 + - /Odometry2/Shape1 + - /MarkerArray3 + - /MarkerArray4 + - /MarkerArray5 + - /Image1 + Splitter Ratio: 0.5394402146339417 + Tree Height: 360 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: surround +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 160 + Reference Frame: + Value: false + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.699999988079071 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: camera_init + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: currPoints + Position Transformer: XYZ + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_registered + Use Fixed Frame: true + Use rainbow: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 15 + Min Value: -5 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 238; 238; 236 + Color Transformer: RGB8 + Decay Time: 10000 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 159 + Min Color: 0; 0; 0 + Min Intensity: 5 + Name: surround + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.004999999888241291 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_registered + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 5 + Size (m): 0.019999999552965164 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_effected + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: mapping + - Class: rviz_common/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.0010000000474974513 + Shape: + Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.15000000596046448 + Color: 255; 85; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.800000011920929 + Shaft Radius: 0.5 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /aft_mapped_to_init + Value: true + Enabled: true + Name: Odometry + - Alpha: 0 + Buffer Length: 2 + Class: rviz_default_plugins/Path + Color: 25; 255; 255 + Enabled: true + Head Diameter: 0 + Head Length: 0 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.03999999910593033 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 25; 255; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /path + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 + Color Transformer: Intensity + Decay Time: 1000 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: currPoints + Position Transformer: XYZ + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_voxel + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planner_normal + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 245; 121; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 15 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_ray_sub_map + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_visual_map + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 115; 210; 22 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: surround + Position Transformer: XYZ + Selectable: false + Size (Pixels): 12 + Size (m): 0.05000000074505806 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_visual_sub_map + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 237; 212; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: currPoints + Position Transformer: XYZ + Selectable: true + Size (Pixels): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_sample_points + Use Fixed Frame: true + Use rainbow: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 239; 41; 41 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_visual_map + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 20 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_ray_sub_map_fov + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Shape: + Alpha: 1 + Axes Length: 0.699999988079071 + Axes Radius: 0.20000000298023224 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /aft_mapped_to_init + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rgb_img + Value: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 50.66728210449219 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 5.083343982696533 + Y: -9.8687162399292 + Z: -3.4865479392465204e-05 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3547965884208679 + Target Frame: drone + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 2.5417795181274414 + Saved: + - Class: rviz_default_plugins/Orbit + Distance: 117.53474426269531 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -35.713138580322266 + Y: 36.932613372802734 + Z: 4.459701061248779 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.19539840519428253 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 0.17540442943572998 + - Class: rviz_default_plugins/Orbit + Distance: 109.3125 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -22.092714309692383 + Y: 63.322662353515625 + Z: 14.125411987304688 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.035398442298173904 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 5.793589115142822 + - Class: rviz_default_plugins/Orbit + Distance: 85.65605163574219 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 28.252656936645508 + Y: -35.49672317504883 + Z: -36.31112289428711 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5653983950614929 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 0.9104044437408447 + - Class: rviz_default_plugins/Orbit + Distance: 60.1053581237793 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 30.61589241027832 + Y: 29.98663330078125 + Z: -12.290168762207031 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.315398633480072 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 5.788588047027588 +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 27 diff --git a/src/FAST-LIVO2/rviz_cfg/hilti.rviz b/src/FAST-LIVO2/rviz_cfg/hilti.rviz new file mode 100755 index 0000000..b84c64c --- /dev/null +++ b/src/FAST-LIVO2/rviz_cfg/hilti.rviz @@ -0,0 +1,672 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Axes1 + - /mapping1 + - /mapping1/currPoints1 + - /mapping1/surround1 + - /mapping1/surround1/Autocompute Value Bounds1 + - /mapping1/PointCloud21 + - /Odometry1 + - /Odometry1/Odometry1 + - /Odometry1/Odometry1/Shape1 + - /Path1 + - /currPoints1/Autocompute Value Bounds1 + - /MarkerArray1/Namespaces1 + - /currPoints2/Autocompute Value Bounds1 + - /Odometry2/Shape1 + - /MarkerArray3 + - /MarkerArray4 + - /MarkerArray5 + - /Image1 + Splitter Ratio: 0.34272301197052 + Tree Height: 538 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: surround +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 160 + Reference Frame: + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 0.699999988079071 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 15 + Min Value: -5 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 238; 238; 236 + Color Transformer: RGB8 + Decay Time: 10000 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 159 + Min Color: 0; 0; 0 + Min Intensity: 5 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 1 + Size (m): 0.004999999888241291 + Style: Points + Topic: /cloud_registered + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.019999999552965164 + Style: Squares + Topic: /cloud_effected + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: mapping + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.0010000000474974513 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.15000000596046448 + Color: 255; 85; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.800000011920929 + Shaft Radius: 0.5 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: true + Enabled: true + Name: Odometry + - Alpha: 0 + Buffer Length: 2 + Class: rviz/Path + Color: 25; 255; 255 + Enabled: true + Head Diameter: 0 + Head Length: 0 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.03999999910593033 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 25; 255; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 + Topic: /path + Unreliable: false + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: Intensity + Decay Time: 1000 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_voxel + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_normal + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /voxels + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 245; 121; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 15 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /visualization_marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 115; 210; 22 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 12 + Size (m): 0.05000000074505806 + Style: Points + Topic: /cloud_visual_sub_map + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 237; 212; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_sample_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 239; 41; 41 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 20 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map_fov + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.699999988079071 + Axes Radius: 0.20000000298023224 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /waypoint_planner/visualize + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /fsm_node/visualization/exp_traj + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /fsm_node/visualization/exp_sfcs + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Image + Enabled: true + Image Topic: /rgb_img + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 238; 238; 236 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 44.92388153076172 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -2.7518839836120605 + Y: 2.672811508178711 + Z: -5.34896862518508e-05 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: true + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.1397969275712967 + Target Frame: drone + Yaw: 1.5631110668182373 + Saved: + - Class: rviz/Orbit + Distance: 117.53474426269531 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -35.713138580322266 + Y: 36.932613372802734 + Z: 4.459701061248779 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.19539840519428253 + Target Frame: + Yaw: 0.17540442943572998 + - Class: rviz/Orbit + Distance: 109.3125 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -22.092714309692383 + Y: 63.322662353515625 + Z: 14.125411987304688 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.035398442298173904 + Target Frame: + Yaw: 5.793589115142822 + - Class: rviz/Orbit + Distance: 85.65605163574219 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 28.252656936645508 + Y: -35.49672317504883 + Z: -36.31112289428711 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5653983950614929 + Target Frame: + Yaw: 0.9104044437408447 + - Class: rviz/Orbit + Distance: 60.1053581237793 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 30.61589241027832 + Y: 29.98663330078125 + Z: -12.290168762207031 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.315398633480072 + Target Frame: + Yaw: 5.788588047027588 +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2488 + X: 72 + Y: 27 diff --git a/src/FAST-LIVO2/rviz_cfg/ntu_viral.rviz b/src/FAST-LIVO2/rviz_cfg/ntu_viral.rviz new file mode 100755 index 0000000..265b245 --- /dev/null +++ b/src/FAST-LIVO2/rviz_cfg/ntu_viral.rviz @@ -0,0 +1,671 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Status1 + - /Axes1 + - /mapping1 + - /mapping1/currPoints1 + - /mapping1/surround1 + - /mapping1/surround1/Autocompute Value Bounds1 + - /mapping1/PointCloud21 + - /Odometry1 + - /Odometry1/Odometry1 + - /Odometry1/Odometry1/Shape1 + - /Path1 + - /currPoints1/Autocompute Value Bounds1 + - /MarkerArray1/Namespaces1 + - /currPoints2/Autocompute Value Bounds1 + - /Odometry2/Shape1 + - /MarkerArray3 + - /MarkerArray4 + - /MarkerArray5 + - /Image1 + Splitter Ratio: 0.34272301197052 + Tree Height: 538 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Name: Time + SyncMode: 0 + SyncSource: surround +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 160 + Reference Frame: + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.699999988079071 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: true + - Alpha: 0.5 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 15 + Min Value: -5 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 238; 238; 236 + Color Transformer: RGB8 + Decay Time: 10000 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 159 + Min Color: 0; 0; 0 + Min Intensity: 5 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 1 + Size (m): 0.004999999888241291 + Style: Points + Topic: /cloud_registered + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.019999999552965164 + Style: Squares + Topic: /cloud_effected + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: mapping + - Class: rviz_common/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.0010000000474974513 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.15000000596046448 + Color: 255; 85; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.800000011920929 + Shaft Radius: 0.5 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: true + Enabled: true + Name: Odometry + - Alpha: 0 + Buffer Length: 2 + Class: rviz_default_plugins/Path + Color: 25; 255; 255 + Enabled: true + Head Diameter: 0 + Head Length: 0 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.03999999910593033 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 25; 255; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 + Topic: /path + Unreliable: false + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 + Color Transformer: Intensity + Decay Time: 1000 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_voxel + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/Marker + Enabled: true + Marker Topic: /planner_normal + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Marker Topic: /voxels + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 245; 121; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 15 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Marker Topic: /visualization_marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 115; 210; 22 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 12 + Size (m): 0.05000000074505806 + Style: Points + Topic: /cloud_visual_sub_map + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 237; 212; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_sample_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 239; 41; 41 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 20 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map_fov + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.699999988079071 + Axes Radius: 0.20000000298023224 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Marker Topic: /waypoint_planner/visualize + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: /fsm_node/visualization/exp_traj + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Marker Topic: /fsm_node/visualization/exp_sfcs + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Image Topic: /rgb_img + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 65.96137237548828 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -2.06162166595459 + Y: 2.7847142219543457 + Z: -2.219532325398177e-05 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: true + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.4797966480255127 + Target Frame: drone + Yaw: 3.251800537109375 + Saved: + - Class: rviz_default_plugins/Orbit + Distance: 117.53474426269531 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -35.713138580322266 + Y: 36.932613372802734 + Z: 4.459701061248779 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.19539840519428253 + Target Frame: + Yaw: 0.17540442943572998 + - Class: rviz_default_plugins/Orbit + Distance: 109.3125 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -22.092714309692383 + Y: 63.322662353515625 + Z: 14.125411987304688 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.035398442298173904 + Target Frame: + Yaw: 5.793589115142822 + - Class: rviz_default_plugins/Orbit + Distance: 85.65605163574219 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 28.252656936645508 + Y: -35.49672317504883 + Z: -36.31112289428711 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5653983950614929 + Target Frame: + Yaw: 0.9104044437408447 + - Class: rviz_default_plugins/Orbit + Distance: 60.1053581237793 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 30.61589241027832 + Y: 29.98663330078125 + Z: -12.290168762207031 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.315398633480072 + Target Frame: + Yaw: 5.788588047027588 +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2488 + X: 72 + Y: 27 diff --git a/src/FAST-LIVO2/scripts/colmap_output.sh b/src/FAST-LIVO2/scripts/colmap_output.sh new file mode 100755 index 0000000..f0b35d5 --- /dev/null +++ b/src/FAST-LIVO2/scripts/colmap_output.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +TARGET_DIRS=( + "$(rospack find fast_livo)/Log/Colmap/images" + "$(rospack find fast_livo)/Log/Colmap/sparse/0" +) + +for dir in "${TARGET_DIRS[@]}"; do + if [ -d "$dir" ]; then + rm -rf "$dir" + echo "Removed: $dir" + else + echo "Not found: $dir" + fi +done + +for dir in "${TARGET_DIRS[@]}"; do + if [ ! -d "$dir" ]; then + mkdir -p "$dir" + echo "Created: $dir" + else + echo "Exists: $dir" + fi +done + diff --git a/src/FAST-LIVO2/scripts/mesh.py b/src/FAST-LIVO2/scripts/mesh.py new file mode 100644 index 0000000..f53428e --- /dev/null +++ b/src/FAST-LIVO2/scripts/mesh.py @@ -0,0 +1,114 @@ +import os +import glob +import vdbfusion +import numpy as np +import open3d as o3d +from scipy.spatial import KDTree + +# ------------------------- Key Parameters ------------------------- +# Dataset path +SOURCE_DIR = "/home/chunran/Downloads/" # Replace with your dataset path + +# VDBVolume parameters +VOXEL_SIZE = 0.02 # Voxel size (smaller values increase precision but require more memory) +SDF_TRUNC = 0.1 # Truncation distance for SDF (affects surface thickness, typically a multiple of voxel size) +MIN_WEIGHT = 0.1 # Minimum weight for mesh extraction (filters out noisy voxels) + +# ------------------------- Dataset Class ------------------------- +class Dataset: + def __init__(self, folder: str): + super().__init__() + # Get all .pcd files in the folder + self.scan_files = glob.glob(os.path.join(folder, "*.pcd")) + # Initialize poses as identity matrices + self.poses = np.array([np.eye(4) for _ in range(len(self.scan_files))]) + + def __getitem__(self, idx): + if idx >= len(self.scan_files): + raise IndexError("Index out of range") + + # Compute relative pose + pose = np.linalg.inv(self.poses[0]) @ self.poses[idx] + # Read point cloud + points, colors = self.read_pcd(self.scan_files[idx]) + points = np.array(points, dtype=np.float64) + return points, colors, pose + + def __len__(self): + return len(self.scan_files) + + def read_pcd(self, pcd_file): + # Read .pcd file using Open3D + pcd = o3d.io.read_point_cloud(pcd_file) + # Extract point cloud coordinates + points = np.asarray(pcd.points) + # Extract colors (if available) + if pcd.has_colors(): + colors = np.asarray(pcd.colors) # Open3D colors are in range [0, 1] + else: + colors = np.zeros_like(points) # If no colors, fill with zeros + return points, colors + +# ------------------------- Main Program ------------------------- +if __name__ == '__main__': + # Initialize VDBVolume + print("Initializing VDBVolume...") + vdb_volume = vdbfusion.VDBVolume(voxel_size=VOXEL_SIZE, sdf_trunc=SDF_TRUNC) + + # Load dataset + print("Loading dataset from", SOURCE_DIR) + dataset = Dataset(SOURCE_DIR) + + # Integrate all point clouds into the VDBVolume + print("Integrating point clouds into VDBVolume...") + for i in range(len(dataset)): + scan, colors, origin = dataset[i] + vdb_volume.integrate(scan, origin) + + print("Point cloud integration complete!") + + # Extract triangle mesh + print("Extracting triangle mesh...") + vert, tri = vdb_volume.extract_triangle_mesh(min_weight=MIN_WEIGHT) + + # Create Open3D mesh object + print("Creating Open3D mesh object...") + mesh = o3d.geometry.TriangleMesh( + o3d.utility.Vector3dVector(vert), + o3d.utility.Vector3iVector(tri), + ) + + # Save the mesh + print("Saving the mesh to output_mesh.ply...") + o3d.io.write_triangle_mesh("mesh.ply", mesh) + print("Mesh saved successfully.") + + # ------------------------- Colorize Mesh Vertices ------------------------- + print("Starting mesh colorization...") + if dataset[0][1] is not None: # Check if color information exists + # Combine all point cloud points and colors + pcd_points = np.vstack([dataset[i][0] for i in range(len(dataset))]) + pcd_colors = np.vstack([dataset[i][1] for i in range(len(dataset))]) + + # Use KDTree to find the nearest point for each vertex + kdtree = KDTree(pcd_points) + _, indices = kdtree.query(vert) # Find the nearest point cloud point for each vertex + vertex_colors = pcd_colors[indices] # Assign colors + + # Set mesh vertex colors + mesh.vertex_colors = o3d.utility.Vector3dVector(vertex_colors) + + print("Mesh colorization complete!") + + # Compute vertex normals + print("Computing vertex normals...") + mesh.compute_vertex_normals() + + # Save the textured mesh + print("Saving the textured mesh to textured_mesh.ply...") + o3d.io.write_triangle_mesh("textured_mesh.ply", mesh) + print("Textured mesh saved successfully.") + + # Visualize the final colorized mesh + print("Visualizing the colorized mesh...") + o3d.visualization.draw_geometries([mesh]) diff --git a/src/FAST-LIVO2/src/IMU_Processing.cpp b/src/FAST-LIVO2/src/IMU_Processing.cpp new file mode 100755 index 0000000..996ec41 --- /dev/null +++ b/src/FAST-LIVO2/src/IMU_Processing.cpp @@ -0,0 +1,591 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#include "IMU_Processing.h" +#include + +const bool time_list(PointType &x, PointType &y) { return (x.curvature < y.curvature); } + +ImuProcess::ImuProcess() : Eye3d(M3D::Identity()), + Zero3d(0, 0, 0), b_first_frame(true), imu_need_init(true) +{ + init_iter_num = 1; + cov_acc = V3D(0.1, 0.1, 0.1); + cov_gyr = V3D(0.1, 0.1, 0.1); + cov_bias_gyr = V3D(0.1, 0.1, 0.1); + cov_bias_acc = V3D(0.1, 0.1, 0.1); + cov_inv_expo = 0.2; + mean_acc = V3D(0, 0, -1.0); + mean_gyr = V3D(0, 0, 0); + angvel_last = Zero3d; + acc_s_last = Zero3d; + Lid_offset_to_IMU = Zero3d; + Lid_rot_to_IMU = Eye3d; + last_imu.reset(new sensor_msgs::msg::Imu()); + cur_pcl_un_.reset(new PointCloudXYZI()); +} + +ImuProcess::~ImuProcess() {} + +void ImuProcess::Reset() +{ + RCLCPP_WARN(rclcpp::get_logger(""), "Reset ImuProcess"); + mean_acc = V3D(0, 0, -1.0); + mean_gyr = V3D(0, 0, 0); + angvel_last = Zero3d; + imu_need_init = true; + init_iter_num = 1; + IMUpose.clear(); + last_imu.reset(new sensor_msgs::msg::Imu()); + cur_pcl_un_.reset(new PointCloudXYZI()); +} + +void ImuProcess::disable_imu() +{ + cout << "IMU Disabled !!!!!" << endl; + imu_en = false; + imu_need_init = false; +} + +void ImuProcess::disable_gravity_est() +{ + cout << "Online Gravity Estimation Disabled !!!!!" << endl; + gravity_est_en = false; +} + +void ImuProcess::disable_bias_est() +{ + cout << "Bias Estimation Disabled !!!!!" << endl; + ba_bg_est_en = false; +} + +void ImuProcess::disable_exposure_est() +{ + cout << "Online Time Offset Estimation Disabled !!!!!" << endl; + exposure_estimate_en = false; +} + +void ImuProcess::set_extrinsic(const MD(4, 4) & T) +{ + Lid_offset_to_IMU = T.block<3, 1>(0, 3); + Lid_rot_to_IMU = T.block<3, 3>(0, 0); +} + +void ImuProcess::set_extrinsic(const V3D &transl) +{ + Lid_offset_to_IMU = transl; + Lid_rot_to_IMU.setIdentity(); +} + +void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot) +{ + Lid_offset_to_IMU = transl; + Lid_rot_to_IMU = rot; +} + +void ImuProcess::set_gyr_cov_scale(const V3D &scaler) { cov_gyr = scaler; } + +void ImuProcess::set_acc_cov_scale(const V3D &scaler) { cov_acc = scaler; } + +void ImuProcess::set_gyr_bias_cov(const V3D &b_g) { cov_bias_gyr = b_g; } + +void ImuProcess::set_inv_expo_cov(const double &inv_expo) { cov_inv_expo = inv_expo; } + +void ImuProcess::set_acc_bias_cov(const V3D &b_a) { cov_bias_acc = b_a; } + +void ImuProcess::set_imu_init_frame_num(const int &num) { MAX_INI_COUNT = num; } + +void ImuProcess::IMU_init(const MeasureGroup &meas, StatesGroup &state_inout, int &N) +{ + /** 1. initializing the gravity, gyro bias, acc and gyro covariance + ** 2. normalize the acceleration measurenments to unit gravity **/ + RCLCPP_INFO(rclcpp::get_logger(""),"IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100); + V3D cur_acc, cur_gyr; + + if (b_first_frame) + { + Reset(); + N = 1; + b_first_frame = false; + const auto &imu_acc = meas.imu.front()->linear_acceleration; + const auto &gyr_acc = meas.imu.front()->angular_velocity; + mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; + mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; + // first_lidar_time = meas.lidar_frame_beg_time; + // cout<<"init acc norm: "<linear_acceleration; + const auto &gyr_acc = imu->angular_velocity; + cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; + cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; + + mean_acc += (cur_acc - mean_acc) / N; + mean_gyr += (cur_gyr - mean_gyr) / N; + + // cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - + // mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N); cov_gyr + // = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - + // mean_gyr) * (N - 1.0) / (N * N); + + // cout<<"acc norm: "<points.size(); i++) { + // if (dt < pcl_out->points[i].curvature) { + // dt = pcl_out->points[i].curvature; + // } + // } + // dt = dt / (double)1000; + // std::cout << "dt:" << dt << std::endl; + // double dt = pcl_out->points.back().curvature / double(1000); + + /* covariance propagation */ + // M3D acc_avr_skew; + M3D Exp_f = Exp(state_inout.bias_g, dt); + + F_x.setIdentity(); + cov_w.setZero(); + + F_x.block<3, 3>(0, 0) = Exp(state_inout.bias_g, -dt); + F_x.block<3, 3>(0, 10) = Eye3d * dt; + F_x.block<3, 3>(3, 7) = Eye3d * dt; + // F_x.block<3, 3>(6, 0) = - R_imu * acc_avr_skew * dt; + // F_x.block<3, 3>(6, 12) = - R_imu * dt; + // F_x.block<3, 3>(6, 15) = Eye3d * dt; + + cov_w.block<3, 3>(10, 10).diagonal() = cov_gyr * dt * dt; // for omega in constant model + cov_w.block<3, 3>(7, 7).diagonal() = cov_acc * dt * dt; // for velocity in constant model + // cov_w.block<3, 3>(6, 6) = + // R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt; + // cov_w.block<3, 3>(9, 9).diagonal() = + // cov_bias_gyr * dt * dt; // bias gyro covariance + // cov_w.block<3, 3>(12, 12).diagonal() = + // cov_bias_acc * dt * dt; // bias acc covariance + + // std::cout << "before propagete:" << state_inout.cov.diagonal().transpose() + // << std::endl; + state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; + // std::cout << "cov_w:" << cov_w.diagonal().transpose() << std::endl; + // std::cout << "after propagete:" << state_inout.cov.diagonal().transpose() + // << std::endl; + state_inout.rot_end = state_inout.rot_end * Exp_f; + state_inout.pos_end = state_inout.pos_end + state_inout.vel_end * dt; + + if (lidar_type != L515) + { + auto it_pcl = pcl_out.points.end() - 1; + double dt_j = 0.0; + for(; it_pcl != pcl_out.points.begin(); it_pcl--) + { + dt_j= pcl_end_offset_time - it_pcl->curvature/double(1000); + M3D R_jk(Exp(state_inout.bias_g, - dt_j)); + V3D P_j(it_pcl->x, it_pcl->y, it_pcl->z); + // Using rotation and translation to un-distort points + V3D p_jk; + p_jk = - state_inout.rot_end.transpose() * state_inout.vel_end * dt_j; + + V3D P_compensate = R_jk * P_j + p_jk; + + /// save Undistorted points and their rotation + it_pcl->x = P_compensate(0); + it_pcl->y = P_compensate(1); + it_pcl->z = P_compensate(2); + } + } +} + + +void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out) +{ + double t0 = omp_get_wtime(); + pcl_out.clear(); + /*** add the imu of the last frame-tail to the of current frame-head ***/ + MeasureGroup &meas = lidar_meas.measures.back(); + // cout<<"meas.imu.size: "<header.stamp); + const double &imu_end_time = stamp2Sec(v_imu.back()->header.stamp); + const double prop_beg_time = last_prop_end_time; + // printf("[ IMU ] undistort input size: %zu \n", lidar_meas.pcl_proc_cur->points.size()); + // printf("[ IMU ] IMU data sequence size: %zu \n", meas.imu.size()); + // printf("[ IMU ] lidar_scan_index_now: %d \n", lidar_meas.lidar_scan_index_now); + + const double prop_end_time = lidar_meas.lio_vio_flg == LIO ? meas.lio_time : meas.vio_time; + + /*** cut lidar point based on the propagation-start time and required + * propagation-end time ***/ + // const double pcl_offset_time = (prop_end_time - + // lidar_meas.lidar_frame_beg_time) * 1000.; // the offset time w.r.t scan + // start time auto pcl_it = lidar_meas.pcl_proc_cur->points.begin() + + // lidar_meas.lidar_scan_index_now; auto pcl_it_end = + // lidar_meas.lidar->points.end(); printf("[ IMU ] pcl_it->curvature: %lf + // pcl_offset_time: %lf \n", pcl_it->curvature, pcl_offset_time); while + // (pcl_it != pcl_it_end && pcl_it->curvature <= pcl_offset_time) + // { + // pcl_wait_proc.push_back(*pcl_it); + // pcl_it++; + // lidar_meas.lidar_scan_index_now++; + // } + + // cout<<"pcl_out.size(): "<curvature: + // "<curvature<points.size()); + pcl_wait_proc = *(lidar_meas.pcl_proc_cur); + lidar_meas.lidar_scan_index_now = 0; + IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, state_inout.vel_end, state_inout.pos_end, state_inout.rot_end)); + } + + // printf("[ IMU ] pcl_wait_proc size: %zu \n", pcl_wait_proc.points.size()); + + // sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); + // lidar_meas.debug_show(); + // cout<<"UndistortPcl [ IMU ]: Process lidar from "<points.size()<header.stamp) - first_lidar_time; + // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75); + tau = 1.0; + imu_time_init = true; + } + else + { + // Multi-camera exposure: read camera 0's exposure (covariance propagation only). + tau = (!state_inout.inv_expo_time_per_cam.empty()) ? state_inout.inv_expo_time_per_cam[0] : 1.0; + } + // state_inout.cov(6, 6) = 0.01; + // RCLCPP_ERROR_STREAM(rclcpp::get_logger(""),"lidar_meas.lio_vio_flg"); + // cout<<"lidar_meas.lio_vio_flg: "<header.stamp) < prop_beg_time) continue; + + angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), + 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); + + // angvel_avr<angular_velocity.x, tail->angular_velocity.y, + // tail->angular_velocity.z; + + acc_avr << 0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x), 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), + 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); + + // cout<<"angvel_avr: "<header.stamp) - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; + // #endif + + // imu_time = stamp2Sec(head->header.stamp) - first_lidar_time; + + angvel_avr -= state_inout.bias_g; + acc_avr = acc_avr * G_m_s2 / mean_acc.norm() - state_inout.bias_a; + + if (stamp2Sec(head->header.stamp) < prop_beg_time) + { + // printf("00 \n"); + dt = stamp2Sec(tail->header.stamp) - last_prop_end_time; + offs_t = stamp2Sec(tail->header.stamp) - prop_beg_time; + } + else if (i != v_imu.size() - 2) + { + // printf("11 \n"); + dt = stamp2Sec(tail->header.stamp) - stamp2Sec(head->header.stamp); + offs_t = stamp2Sec(tail->header.stamp) - prop_beg_time; + } + else + { + // printf("22 \n"); + dt = prop_end_time - stamp2Sec(head->header.stamp); + offs_t = prop_end_time - prop_beg_time; + } + + dt_all += dt; + // printf("[ LIO Propagation ] dt: %lf \n", dt); + + /* covariance propagation */ + M3D acc_avr_skew; + M3D Exp_f = Exp(angvel_avr, dt); + acc_avr_skew << SKEW_SYM_MATRX(acc_avr); + + F_x.setIdentity(); + cov_w.setZero(); + + F_x.block<3, 3>(0, 0) = Exp(angvel_avr, -dt); + if (ba_bg_est_en) F_x.block<3, 3>(0, 10) = -Eye3d * dt; + // F_x.block<3,3>(3,0) = R_imu * off_vel_skew * dt; + F_x.block<3, 3>(3, 7) = Eye3d * dt; + F_x.block<3, 3>(7, 0) = -R_imu * acc_avr_skew * dt; + if (ba_bg_est_en) F_x.block<3, 3>(7, 13) = -R_imu * dt; + if (gravity_est_en) F_x.block<3, 3>(7, 16) = Eye3d * dt; + + // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75); + // F_x(6,6) = 0.25 * 2 * CV_PI * 0.5 * cos(2 * CV_PI * 0.5 * imu_time) * (-tau*tau); F_x(18,18) = 0.00001; + if (exposure_estimate_en) cov_w(6, 6) = cov_inv_expo * dt * dt; + cov_w.block<3, 3>(0, 0).diagonal() = cov_gyr * dt * dt; + cov_w.block<3, 3>(7, 7) = R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt; + cov_w.block<3, 3>(10, 10).diagonal() = cov_bias_gyr * dt * dt; // bias gyro covariance + cov_w.block<3, 3>(13, 13).diagonal() = cov_bias_acc * dt * dt; // bias acc covariance + + state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; + // state_inout.cov.block<18,18>(0,0) = F_x.block<18,18>(0,0) * + // state_inout.cov.block<18,18>(0,0) * F_x.block<18,18>(0,0).transpose() + + // cov_w.block<18,18>(0,0); + + // tau = tau + 0.25 * 2 * CV_PI * 0.5 * cos(2 * CV_PI * 0.5 * imu_time) * + // (-tau*tau) * dt; + + // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75); + + /* propogation of IMU attitude */ + R_imu = R_imu * Exp_f; + + /* Specific acceleration (global frame) of IMU */ + acc_imu = R_imu * acc_avr + state_inout.gravity; + + /* propogation of IMU */ + pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt; + + /* velocity of IMU */ + vel_imu = vel_imu + acc_imu * dt; + + /* save the poses at each IMU measurements */ + angvel_last = angvel_avr; + acc_s_last = acc_imu; + + // cout<header.stamp): + // "<header.stamp)<prop_beg_time) + // { + // double note = prop_end_time > imu_end_time ? 1.0 : -1.0; + // dt = note * (prop_end_time - imu_end_time); + // state_inout.vel_end = vel_imu + note * acc_imu * dt; + // state_inout.rot_end = R_imu * Exp(V3D(note * angvel_avr), dt); + // state_inout.pos_end = pos_imu + note * vel_imu * dt + note * 0.5 * + // acc_imu * dt * dt; + // } + // else + // { + // double note = prop_end_time > prop_beg_time ? 1.0 : -1.0; + // dt = note * (prop_end_time - prop_beg_time); + // state_inout.vel_end = vel_imu + note * acc_imu * dt; + // state_inout.rot_end = R_imu * Exp(V3D(note * angvel_avr), dt); + // state_inout.pos_end = pos_imu + note * vel_imu * dt + note * 0.5 * + // acc_imu * dt * dt; + // } + + // cout<<"[ Propagation ] output state: "<offset_time<<" "; + // } + // cout<points.size()<rot); + acc_imu << VEC_FROM_ARRAY(head->acc); + // cout<<"head imu acc: "<vel); + pos_imu << VEC_FROM_ARRAY(head->pos); + angvel_avr << VEC_FROM_ARRAY(head->gyr); + + // printf("head->offset_time: %lf \n", head->offset_time); + // printf("it_pcl->curvature: %lf pt dt: %lf \n", it_pcl->curvature, + // it_pcl->curvature / double(1000) - head->offset_time); + + for (; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--) + { + dt = it_pcl->curvature / double(1000) - head->offset_time; + + /* Transform to the 'end' frame */ + M3D R_i(R_imu * Exp(angvel_avr, dt)); + V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - state_inout.pos_end); + + V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); + // V3D P_compensate = Lid_rot_to_IMU.transpose() * + // (state_inout.rot_end.transpose() * (R_i * (Lid_rot_to_IMU * P_i + + // Lid_offset_to_IMU) + T_ei) - Lid_offset_to_IMU); + V3D P_compensate = (extR_Ri * (R_i * (Lid_rot_to_IMU * P_i + Lid_offset_to_IMU) + T_ei) - exrR_extT); + + /// save Undistorted points and their rotation + it_pcl->x = P_compensate(0); + it_pcl->y = P_compensate(1); + it_pcl->z = P_compensate(2); + + if (it_pcl == pcl_wait_proc.points.begin()) break; + } + } + pcl_out = pcl_wait_proc; + pcl_wait_proc.clear(); + IMUpose.clear(); + } + // printf("[ IMU ] time forward: %lf, backward: %lf.\n", t1 - t0, omp_get_wtime() - t1); +} + +void ImuProcess::Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_) +{ + double t1, t2, t3; + t1 = omp_get_wtime(); + rcpputils::assert_true(lidar_meas.lidar != nullptr); + if (!imu_en) + { + Forward_without_imu(lidar_meas, stat, *cur_pcl_un_); + return; + } + + MeasureGroup meas = lidar_meas.measures.back(); + + if (imu_need_init) + { + double pcl_end_time = lidar_meas.lio_vio_flg == LIO ? meas.lio_time : meas.vio_time; + // lidar_meas.last_lio_update_time = pcl_end_time; + + if (meas.imu.empty()) { return; }; + /// The very first lidar frame + IMU_init(meas, stat, init_iter_num); + + imu_need_init = true; + + last_imu = meas.imu.back(); + + if (init_iter_num > MAX_INI_COUNT) + { + // cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); + imu_need_init = false; + RCLCPP_INFO(rclcpp::get_logger(""), "IMU Initials: Gravity: %.4f %.4f %.4f %.4f; acc covarience: " + "%.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f \n", + stat.gravity[0], stat.gravity[1], stat.gravity[2], mean_acc.norm(), cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], + cov_gyr[2]); + RCLCPP_INFO(rclcpp::get_logger(""), "IMU Initials: ba covarience: %.8f %.8f %.8f; bg covarience: " + "%.8f %.8f %.8f", + cov_bias_acc[0], cov_bias_acc[1], cov_bias_acc[2], cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2]); + fout_imu.open(DEBUG_FILE_DIR("imu.txt"), ios::out); + } + + return; + } + + UndistortPcl(lidar_meas, stat, *cur_pcl_un_); + // cout << "[ IMU ] undistorted point num: " << cur_pcl_un_->size() << endl; +} \ No newline at end of file diff --git a/src/FAST-LIVO2/src/LIVMapper.cpp b/src/FAST-LIVO2/src/LIVMapper.cpp new file mode 100755 index 0000000..d8417cb --- /dev/null +++ b/src/FAST-LIVO2/src/LIVMapper.cpp @@ -0,0 +1,1672 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#include "LIVMapper.h" +#include +#include +#include +#include + +using namespace Sophus; +LIVMapper::LIVMapper(rclcpp::Node::SharedPtr &node, std::string node_name) + : node(std::make_shared(node_name)), + extT(0, 0, 0), + extR(M3D::Identity()) +{ + extrinT.assign(3, 0.0); + extrinR.assign(9, 0.0); + cameraextrinT.assign(3, 0.0); + cameraextrinR.assign(9, 0.0); + + p_pre.reset(new Preprocess()); + p_imu.reset(new ImuProcess()); + + readParameters(this->node); + img_buffers.resize(num_of_cameras); + img_time_buffers.resize(num_of_cameras); + last_timestamp_img.assign(num_of_cameras, -1.0); + + VoxelMapConfig voxel_config; + loadVoxelConfig(this->node, voxel_config); + + visual_sub_map.reset(new PointCloudXYZI()); + feats_undistort.reset(new PointCloudXYZI()); + feats_down_body.reset(new PointCloudXYZI()); + feats_down_world.reset(new PointCloudXYZI()); + pcl_w_wait_pub.reset(new PointCloudXYZI()); + pcl_wait_pub.reset(new PointCloudXYZI()); + pcl_wait_save.reset(new PointCloudXYZRGB()); + pcl_wait_save_intensity.reset(new PointCloudXYZI()); + voxelmap_manager.reset(new VoxelMapManager(voxel_config, voxel_map)); + vio_manager.reset(new VIOManager()); + root_dir = ROOT_DIR; + initializeFiles(); + initializeComponents(this->node); // initialize components errors + path.header.stamp = this->node->now(); + path.header.frame_id = "camera_init"; +} + +LIVMapper::~LIVMapper() {} + +void LIVMapper::readParameters(rclcpp::Node::SharedPtr &node) +{ + // declare parameters + this->node->declare_parameter("common.lid_topic", "/livox/lidar"); + this->node->declare_parameter("common.imu_topic", "/livox/imu"); + this->node->declare_parameter("common.ros_driver_bug_fix", false); + this->node->declare_parameter("common.img_en", 1); + this->node->declare_parameter("common.lidar_en", 1); + this->node->declare_parameter("common.img_topic", "/left_camera/image"); + this->node->declare_parameter("common.camera_config_path", ""); + this->node->declare_parameter("common.camera_time_tolerance", 0.001); + + this->node->declare_parameter("vio.normal_en", true); + this->node->declare_parameter("vio.inverse_composition_en", false); + this->node->declare_parameter("vio.max_iterations", 5); + this->node->declare_parameter("vio.img_point_cov", 100); + this->node->declare_parameter("vio.raycast_en", false); + this->node->declare_parameter("vio.exposure_estimate_en", true); + this->node->declare_parameter("vio.inv_expo_cov", 0.1); + this->node->declare_parameter("vio.enable_cross_camera_tracking", false); + this->node->declare_parameter("vio.enable_dynamic_covariance", false); + this->node->declare_parameter("vio.dynamic_cov_warmup_frames", 200); + this->node->declare_parameter("vio.warmup_cov_scale", 500.0); + this->node->declare_parameter("vio.min_cov_scale", 10.0); + this->node->declare_parameter("vio.max_cov_scale", 2000.0); + this->node->declare_parameter("vio.dynamic_cov_error_max", 50.0); + this->node->declare_parameter("vio.max_total_points", 300); + this->node->declare_parameter("vio.grid_size", 5); + this->node->declare_parameter("vio.grid_n_height", 17); + this->node->declare_parameter("vio.patch_pyrimid_level", 4); + this->node->declare_parameter("vio.patch_size", 8); + this->node->declare_parameter("vio.outlier_threshold", 100); + this->node->declare_parameter("time_offset.exposure_time_init", 0.0); + this->node->declare_parameter("time_offset.img_time_offset", 0.0); + this->node->declare_parameter("time_offset.imu_time_offset", 0.0); + this->node->declare_parameter("time_offset.lidar_time_offset", 0.0); + this->node->declare_parameter("uav.imu_rate_odom", false); + this->node->declare_parameter("uav.gravity_align_en", false); + + this->node->declare_parameter("evo.seq_name", "01"); + this->node->declare_parameter("evo.pose_output_en", false); + this->node->declare_parameter("imu.gyr_cov", 1.0); + this->node->declare_parameter("imu.acc_cov", 1.0); + this->node->declare_parameter("imu.imu_int_frame", 30); + this->node->declare_parameter("imu.imu_en", true); + this->node->declare_parameter("imu.gravity_est_en", true); + this->node->declare_parameter("imu.ba_bg_est_en", true); + + this->node->declare_parameter("preprocess.blind", 0.01); + this->node->declare_parameter("preprocess.hilti_en", false); + this->node->declare_parameter("preprocess.filter_size_surf", 0.5); + this->node->declare_parameter("preprocess.lidar_type", AVIA); + this->node->declare_parameter("preprocess.scan_line",6); + this->node->declare_parameter("preprocess.point_filter_num", 3); + this->node->declare_parameter("preprocess.feature_extract_enabled", false); + + this->node->declare_parameter("pcd_save.interval", -1); + this->node->declare_parameter("pcd_save.pcd_save_en", false); + this->node->declare_parameter("image_save.img_save_en", false); + this->node->declare_parameter("image_save.interval", 1); + + this->node->declare_parameter("pcd_save.type", 0); + this->node->declare_parameter("pcd_save.colmap_output_en", false); + this->node->declare_parameter("pcd_save.filter_size_pcd", 0.5); + this->node->declare_parameter>("extrin_calib.extrinsic_T", vector{}); + this->node->declare_parameter>("extrin_calib.extrinsic_R", vector{}); + this->node->declare_parameter>("extrin_calib.Pcl", vector{}); + this->node->declare_parameter>("extrin_calib.Rcl", vector{}); + this->node->declare_parameter("debug.plot_time", -10); + this->node->declare_parameter("debug.frame_cnt", 6); + + this->node->declare_parameter("publish.blind_rgb_points", 0.01); + this->node->declare_parameter("publish.pub_scan_num", 1); + this->node->declare_parameter("publish.pub_effect_point_en", false); + this->node->declare_parameter("publish.dense_map_en", false); + + // get parameter + this->node->get_parameter("common.lid_topic", lid_topic); + this->node->get_parameter("common.imu_topic", imu_topic); + this->node->get_parameter("common.ros_driver_bug_fix", ros_driver_fix_en); + this->node->get_parameter("common.img_en", img_en); + this->node->get_parameter("common.lidar_en", lidar_en); + this->node->get_parameter("common.img_topic", img_topic); + this->node->get_parameter("common.camera_config_path", camera_config_path); + this->node->get_parameter("common.camera_time_tolerance", camera_time_tolerance); + + this->node->get_parameter("vio.normal_en", normal_en); + this->node->get_parameter("vio.inverse_composition_en", inverse_composition_en); + this->node->get_parameter("vio.max_iterations", max_iterations); + this->node->get_parameter("vio.img_point_cov", IMG_POINT_COV); + this->node->get_parameter("vio.raycast_en", raycast_en); + this->node->get_parameter("vio.exposure_estimate_en", exposure_estimate_en); + this->node->get_parameter("vio.inv_expo_cov", inv_expo_cov); + this->node->get_parameter("vio.enable_cross_camera_tracking", enable_cross_camera_tracking); + this->node->get_parameter("vio.enable_dynamic_covariance", vio_dynamic_cov_enabled); + this->node->get_parameter("vio.dynamic_cov_warmup_frames", vio_dynamic_cov_warmup_frames); + this->node->get_parameter("vio.warmup_cov_scale", vio_warmup_cov_scale); + this->node->get_parameter("vio.min_cov_scale", vio_min_cov_scale); + this->node->get_parameter("vio.max_cov_scale", vio_max_cov_scale); + this->node->get_parameter("vio.dynamic_cov_error_max", vio_dynamic_cov_error_max); + this->node->get_parameter("vio.max_total_points", vio_max_total_points); + this->node->get_parameter("vio.grid_size", grid_size); + this->node->get_parameter("vio.grid_n_height", grid_n_height); + this->node->get_parameter("vio.patch_pyrimid_level", patch_pyrimid_level); + this->node->get_parameter("vio.patch_size", patch_size); + this->node->get_parameter("vio.outlier_threshold", outlier_threshold); + this->node->get_parameter("time_offset.exposure_time_init", exposure_time_init); + this->node->get_parameter("time_offset.img_time_offset", img_time_offset); + this->node->get_parameter("time_offset.imu_time_offset", imu_time_offset); + this->node->get_parameter("time_offset.lidar_time_offset", lidar_time_offset); + this->node->get_parameter("uav.imu_rate_odom", imu_prop_enable); + this->node->get_parameter("uav.gravity_align_en", gravity_align_en); + + this->node->get_parameter("evo.seq_name", seq_name); + this->node->get_parameter("evo.pose_output_en", pose_output_en); + this->node->get_parameter("imu.gyr_cov", gyr_cov); + this->node->get_parameter("imu.acc_cov", acc_cov); + this->node->get_parameter("imu.imu_int_frame", imu_int_frame); + this->node->get_parameter("imu.imu_en", imu_en); + this->node->get_parameter("imu.gravity_est_en", gravity_est_en); + this->node->get_parameter("imu.ba_bg_est_en", ba_bg_est_en); + + this->node->get_parameter("preprocess.blind", p_pre->blind); + this->node->get_parameter("preprocess.filter_size_surf", filter_size_surf_min); + this->node->get_parameter("preprocess.lidar_type", p_pre->lidar_type); + this->node->get_parameter("preprocess.scan_line", p_pre->N_SCANS); + this->node->get_parameter("preprocess.point_filter_num", p_pre->point_filter_num); + this->node->get_parameter("preprocess.feature_extract_enabled", p_pre->feature_enabled); + + this->node->get_parameter("pcd_save.interval", pcd_save_interval); + this->node->get_parameter("pcd_save.pcd_save_en", pcd_save_en); + this->node->get_parameter("pcd_save.colmap_output_en", colmap_output_en); + this->node->get_parameter("pcd_save.filter_size_pcd", filter_size_pcd); + this->node->get_parameter("extrin_calib.extrinsic_T", extrinT); + this->node->get_parameter("extrin_calib.extrinsic_R", extrinR); + this->node->get_parameter("extrin_calib.Pcl", cameraextrinT); + this->node->get_parameter("extrin_calib.Rcl", cameraextrinR); + this->node->get_parameter("debug.plot_time", plot_time); + this->node->get_parameter("debug.frame_cnt", frame_cnt); + + this->node->get_parameter("publish.blind_rgb_points", blind_rgb_points); + this->node->get_parameter("publish.pub_scan_num", pub_scan_num); + this->node->get_parameter("publish.pub_effect_point_en", pub_effect_point_en); + this->node->get_parameter("publish.dense_map_en", dense_map_en); + + loadCameraArrayConfig(); +} + +void LIVMapper::loadCameraArrayConfig() +{ + // ROS2 parameters cannot represent Omni-LIVO's `extrin_calib.cameras` + // array-of-structs YAML directly (declare_parameter only supports flat + // primitive arrays), so that part of the config is parsed by hand with + // yaml-cpp instead of going through the parameter server. See + // docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md §5. + camera_img_topics.clear(); + camera_extrin_Rs.clear(); + camera_extrin_Ts.clear(); + + if (!camera_config_path.empty()) + { + try + { + YAML::Node root = YAML::LoadFile(camera_config_path); + YAML::Node cameras = root["cameras"]; + if (cameras && cameras.IsSequence() && cameras.size() > 0) + { + for (const auto &cam_node : cameras) + { + camera_img_topics.push_back(cam_node["img_topic"].as()); + camera_extrin_Rs.push_back(cam_node["Rcl"].as>()); + camera_extrin_Ts.push_back(cam_node["Pcl"].as>()); + } + num_of_cameras = static_cast(camera_img_topics.size()); + RCLCPP_INFO(this->node->get_logger(), "Loaded %d camera(s) from camera_config_path: %s", num_of_cameras, camera_config_path.c_str()); + return; + } + RCLCPP_WARN(this->node->get_logger(), "camera_config_path '%s' has no non-empty 'cameras' array; falling back to single-camera parameters.", camera_config_path.c_str()); + } + catch (const std::exception &e) + { + RCLCPP_ERROR(this->node->get_logger(), "Failed to parse camera_config_path '%s': %s. Falling back to single-camera parameters.", camera_config_path.c_str(), e.what()); + } + } + + // Fallback: legacy single-camera configuration, unchanged from before + // multi-camera support was added (img_topic, extrin_calib.Rcl/Pcl). + camera_img_topics = {img_topic}; + camera_extrin_Rs = {cameraextrinR}; + camera_extrin_Ts = {cameraextrinT}; + num_of_cameras = 1; +} + +void LIVMapper::initializeComponents(rclcpp::Node::SharedPtr &node) +{ + downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); + + // extrinT.assign({0.04165, 0.02326, -0.0284}); + // extrinR.assign({1, 0, 0, 0, 1, 0, 0, 0, 1}); + // cameraextrinT.assign({0.0194384, 0.104689,-0.0251952}); + // cameraextrinR.assign({0.00610193,-0.999863,-0.0154172,-0.00615449,0.0153796,-0.999863,0.999962,0.00619598,-0.0060598}); + + extT << VEC_FROM_ARRAY(extrinT); + extR << MAT_FROM_ARRAY(extrinR); + + voxelmap_manager->extT_ << VEC_FROM_ARRAY(extrinT); + voxelmap_manager->extR_ << MAT_FROM_ARRAY(extrinR); + + // Intrinsics loading mirrors the extrinsics/topic split in loadCameraArrayConfig(): + // - legacy single-camera configs (camera_config_path empty) keep using the flat + // parameter_blackboard convention (top-level cam_model, cam_width, ...) via the + // single-camera overload, so existing launch files are unaffected. + // - the new multi-camera configs use the numbered cam_0/cam_1/... namespace via + // the vector overload (see rpg_vikit/vikit_ros/src/camera_loader.cpp). + std::vector cams; + if (!camera_config_path.empty()) + { + if (!vk::camera_loader::loadFromRosNs(this->node, "parameter_blackboard", cams) || cams.empty()) + throw std::runtime_error("Camera model(s) not correctly specified under parameter_blackboard cam_0..cam_N."); + } + else + { + vk::AbstractCamera* single_cam = nullptr; + if (!vk::camera_loader::loadFromRosNs(this->node, "parameter_blackboard", single_cam)) throw std::runtime_error("Camera model not correctly specified."); + cams.push_back(single_cam); + } + vio_manager->cams = cams; + + vio_manager->grid_size = grid_size; + vio_manager->patch_size = patch_size; + vio_manager->outlier_threshold = outlier_threshold; + vio_manager->setImuToLidarExtrinsic(extT, extR); + vio_manager->setLidarToCameraExtrinsic(camera_extrin_Rs, camera_extrin_Ts); + vio_manager->state = &_state; + vio_manager->state_propagat = &state_propagat; + vio_manager->max_iterations = max_iterations; + vio_manager->img_point_cov = IMG_POINT_COV; + vio_manager->normal_en = normal_en; + vio_manager->inverse_composition_en = inverse_composition_en; + vio_manager->raycast_en = raycast_en; + vio_manager->grid_n_width = grid_n_width; + vio_manager->grid_n_height = grid_n_height; + vio_manager->patch_pyrimid_level = patch_pyrimid_level; + vio_manager->exposure_estimate_en = exposure_estimate_en; + vio_manager->colmap_output_en = colmap_output_en; + vio_manager->enable_cross_camera_tracking = enable_cross_camera_tracking; + vio_manager->enable_dynamic_covariance_ = vio_dynamic_cov_enabled; + vio_manager->dynamic_cov_warmup_frames = vio_dynamic_cov_warmup_frames; + vio_manager->warmup_cov_scale = vio_warmup_cov_scale; + vio_manager->min_cov_scale = vio_min_cov_scale; + vio_manager->max_cov_scale = vio_max_cov_scale; + vio_manager->dynamic_cov_error_max = vio_dynamic_cov_error_max; + vio_manager->max_total_points = vio_max_total_points; + vio_manager->initializeVIO(); + + p_imu->set_extrinsic(extT, extR); + p_imu->set_gyr_cov_scale(V3D(gyr_cov, gyr_cov, gyr_cov)); + p_imu->set_acc_cov_scale(V3D(acc_cov, acc_cov, acc_cov)); + p_imu->set_inv_expo_cov(inv_expo_cov); + p_imu->set_gyr_bias_cov(V3D(0.0001, 0.0001, 0.0001)); + p_imu->set_acc_bias_cov(V3D(0.0001, 0.0001, 0.0001)); + p_imu->set_imu_init_frame_num(imu_int_frame); + + if (!imu_en) p_imu->disable_imu(); + if (!gravity_est_en) p_imu->disable_gravity_est(); + if (!ba_bg_est_en) p_imu->disable_bias_est(); + if (!exposure_estimate_en) p_imu->disable_exposure_est(); + + slam_mode_ = (img_en && lidar_en) ? LIVO : imu_en ? ONLY_LIO : ONLY_LO; +} + +void LIVMapper::initializeFiles() +{ + if (pcd_save_en && colmap_output_en) + { + const std::string folderPath = std::string(ROOT_DIR) + "/scripts/colmap_output.sh"; + + std::string chmodCommand = "chmod +x " + folderPath; + + int chmodRet = system(chmodCommand.c_str()); + if (chmodRet != 0) { + std::cerr << "Failed to set execute permissions for the script." << std::endl; + return; + } + + int executionRet = system(folderPath.c_str()); + if (executionRet != 0) { + std::cerr << "Failed to execute the script." << std::endl; + return; + } + } + if(colmap_output_en) fout_points.open(std::string(ROOT_DIR) + "Log/Colmap/sparse/0/points3D.txt", std::ios::out); + if(pcd_save_en) fout_lidar_pos.open(std::string(ROOT_DIR) + "Log/pcd/lidar_poses.txt", std::ios::out); + if(img_save_en) fout_visual_pos.open(std::string(ROOT_DIR) + "Log/image/image_poses.txt", std::ios::out); + fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"), std::ios::out); + fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), std::ios::out); +} + +void LIVMapper::initializeSubscribersAndPublishers(rclcpp::Node::SharedPtr &node, image_transport::ImageTransport &it_) +{ + image_transport::ImageTransport it(this->node); + if (p_pre->lidar_type == AVIA) { + sub_pcl = this->node->create_subscription(lid_topic, 200000, std::bind(&LIVMapper::livox_pcl_cbk, this, std::placeholders::_1)); + } else { + sub_pcl = this->node->create_subscription(lid_topic, 200000, std::bind(&LIVMapper::standard_pcl_cbk, this, std::placeholders::_1)); + } + sub_imu = this->node->create_subscription(imu_topic, 200000, std::bind(&LIVMapper::imu_cbk, this, std::placeholders::_1)); + + sub_img_list.clear(); + for (int i = 0; i < num_of_cameras; i++) + { + auto sub = this->node->create_subscription( + camera_img_topics[i], 200000, + [this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) { img_cbk(msg, i); }); + sub_img_list.push_back(sub); + RCLCPP_INFO(this->node->get_logger(), "Subscribed to camera %d image topic: %s", i, camera_img_topics[i].c_str()); + } + + + pubLaserCloudFullRes = this->node->create_publisher("/cloud_registered", 100); + pubNormal = this->node->create_publisher("/visualization_marker", 100); + pubSubVisualMap = this->node->create_publisher("/cloud_visual_sub_map_before", 100); + pubLaserCloudEffect = this->node->create_publisher("/cloud_effected", 100); + pubLaserCloudMap = this->node->create_publisher("/Laser_map", 100); + pubOdomAftMapped = this->node->create_publisher("/aft_mapped_to_init", 10); + pubPath = this->node->create_publisher("/path", 10); + plane_pub = this->node->create_publisher("/planner_normal", 1); + voxel_pub = this->node->create_publisher("/voxels", 1); + pubLaserCloudDyn = this->node->create_publisher("/dyn_obj", 100); + pubLaserCloudDynRmed = this->node->create_publisher("/dyn_obj_removed", 100); + pubLaserCloudDynDbg = this->node->create_publisher("/dyn_obj_dbg_hist", 100); + mavros_pose_publisher = this->node->create_publisher("/mavros/vision_pose/pose", 10); + pubImage = it.advertise("/rgb_img", 1); + pubImuPropOdom = this->node->create_publisher("/LIVO2/imu_propagate", 10000); + imu_prop_timer = this->node->create_wall_timer(0.004s, std::bind(&LIVMapper::imu_prop_callback, this)); + voxelmap_manager->voxel_map_pub_= this->node->create_publisher("/planes", 10000); +} + +void LIVMapper::handleFirstFrame() +{ + if (!is_first_frame) + { + _first_lidar_time = LidarMeasures.last_lio_update_time; + p_imu->first_lidar_time = _first_lidar_time; // Only for IMU data log + is_first_frame = true; + cout << "FIRST LIDAR FRAME!" << endl; + } +} + +void LIVMapper::gravityAlignment() +{ + if (!p_imu->imu_need_init && !gravity_align_finished) + { + std::cout << "Gravity Alignment Starts" << std::endl; + V3D ez(0, 0, -1), gz(_state.gravity); + Eigen::Quaterniond G_q_I0 = Eigen::Quaterniond::FromTwoVectors(gz, ez); + M3D G_R_I0 = G_q_I0.toRotationMatrix(); + + _state.pos_end = G_R_I0 * _state.pos_end; + _state.rot_end = G_R_I0 * _state.rot_end; + _state.vel_end = G_R_I0 * _state.vel_end; + _state.gravity = G_R_I0 * _state.gravity; + gravity_align_finished = true; + std::cout << "Gravity Alignment Finished" << std::endl; + } +} + +void LIVMapper::processImu() +{ + // double t0 = omp_get_wtime(); + + p_imu->Process2(LidarMeasures, _state, feats_undistort); + + if (gravity_align_en) gravityAlignment(); + + state_propagat = _state; + voxelmap_manager->state_ = _state; + voxelmap_manager->feats_undistort_ = feats_undistort; + + // double t_prop = omp_get_wtime(); + + // std::cout << "[ Mapping ] feats_undistort: " << feats_undistort->size() << std::endl; + // std::cout << "[ Mapping ] predict cov: " << _state.cov.diagonal().transpose() << std::endl; + // std::cout << "[ Mapping ] predict sta: " << state_propagat.pos_end.transpose() << state_propagat.vel_end.transpose() << std::endl; +} + +void LIVMapper::stateEstimationAndMapping() +{ + switch (LidarMeasures.lio_vio_flg) + { + case VIO: + handleVIO(); + break; + case LIO: + case LO: + handleLIO(); + break; + } +} + +void LIVMapper::handleVIO() +{ + euler_cur = RotMtoEuler(_state.rot_end); + fout_pre << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " + << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " + << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time_per_cam.empty() ? 0.0 : _state.inv_expo_time_per_cam[0], 0, 0).transpose() << std::endl; + + if (pcl_w_wait_pub->empty() || (pcl_w_wait_pub == nullptr)) + { + std::cout << "[ VIO ] No point!!!" << std::endl; + return; + } + + std::cout << "[ VIO ] Raw feature num: " << pcl_w_wait_pub->points.size() << std::endl; + + if (fabs((LidarMeasures.last_lio_update_time - _first_lidar_time) - plot_time) < (frame_cnt / 2 * 0.1)) + { + vio_manager->plot_flag = true; + } + else + { + vio_manager->plot_flag = false; + } + + vio_manager->processFrame(LidarMeasures.measures.back().imgs, _pv_list, voxelmap_manager->voxel_map_, LidarMeasures.last_lio_update_time - _first_lidar_time); + + if (imu_prop_enable) + { + ekf_finish_once = true; + latest_ekf_state = _state; + latest_ekf_time = LidarMeasures.last_lio_update_time; + state_update_flg = true; + } + + // int size_sub_map = vio_manager->visual_sub_map_cur.size(); + // visual_sub_map->reserve(size_sub_map); + // for (int i = 0; i < size_sub_map; i++) + // { + // PointType temp_map; + // temp_map.x = vio_manager->visual_sub_map_cur[i]->pos_[0]; + // temp_map.y = vio_manager->visual_sub_map_cur[i]->pos_[1]; + // temp_map.z = vio_manager->visual_sub_map_cur[i]->pos_[2]; + // temp_map.intensity = 0.; + // visual_sub_map->push_back(temp_map); + // } + + publish_frame_world(pubLaserCloudFullRes, vio_manager); + publish_img_rgb(pubImage, vio_manager); + + euler_cur = RotMtoEuler(_state.rot_end); + fout_out << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " + << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " + << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time_per_cam.empty() ? 0.0 : _state.inv_expo_time_per_cam[0], 0, 0).transpose() << " " << feats_undistort->points.size() << std::endl; +} + +void LIVMapper::handleLIO() +{ + euler_cur = RotMtoEuler(_state.rot_end); + fout_pre << setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " + << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " + << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time_per_cam.empty() ? 0.0 : _state.inv_expo_time_per_cam[0], 0, 0).transpose() << endl; + + if (feats_undistort->empty() || (feats_undistort == nullptr)) + { + std::cout << "[ LIO ]: No point!!!" << std::endl; + return; + } + + double t0 = omp_get_wtime(); + + downSizeFilterSurf.setInputCloud(feats_undistort); + downSizeFilterSurf.filter(*feats_down_body); + + double t_down = omp_get_wtime(); + + feats_down_size = feats_down_body->points.size(); + voxelmap_manager->feats_down_body_ = feats_down_body; + transformLidar(_state.rot_end, _state.pos_end, feats_down_body, feats_down_world); + voxelmap_manager->feats_down_world_ = feats_down_world; + voxelmap_manager->feats_down_size_ = feats_down_size; + + if (!lidar_map_inited) + { + lidar_map_inited = true; + voxelmap_manager->BuildVoxelMap(); + } + + double t1 = omp_get_wtime(); + + voxelmap_manager->StateEstimation(state_propagat); + _state = voxelmap_manager->state_; + _pv_list = voxelmap_manager->pv_list_; + + double t2 = omp_get_wtime(); + + if (imu_prop_enable) + { + ekf_finish_once = true; + latest_ekf_state = _state; + latest_ekf_time = LidarMeasures.last_lio_update_time; + state_update_flg = true; + } + + if (pose_output_en) + { + static bool pos_opend = false; + static int ocount = 0; + std::ofstream outFile, evoFile; + if (!pos_opend) + { + evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::out); + pos_opend = true; + if (!evoFile.is_open()) RCLCPP_ERROR(this->node->get_logger(), "open fail\n"); + } + else + { + evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::app); + if (!evoFile.is_open()) RCLCPP_ERROR(this->node->get_logger(), "open fail\n"); + } + Eigen::Matrix4d outT; + Eigen::Quaterniond q(_state.rot_end); + evoFile << std::fixed; + evoFile << LidarMeasures.last_lio_update_time << " " << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " " + << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl; + } + + euler_cur = RotMtoEuler(_state.rot_end); + geoQuat = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2)); + publish_odometry(pubOdomAftMapped); + + double t3 = omp_get_wtime(); + + PointCloudXYZI::Ptr world_lidar(new PointCloudXYZI()); + transformLidar(_state.rot_end, _state.pos_end, feats_down_body, world_lidar); + for (size_t i = 0; i < world_lidar->points.size(); i++) + { + voxelmap_manager->pv_list_[i].point_w << world_lidar->points[i].x, world_lidar->points[i].y, world_lidar->points[i].z; + M3D point_crossmat = voxelmap_manager->cross_mat_list_[i]; + M3D var = voxelmap_manager->body_cov_list_[i]; + var = (_state.rot_end * extR) * var * (_state.rot_end * extR).transpose() + + (-point_crossmat) * _state.cov.block<3, 3>(0, 0) * (-point_crossmat).transpose() + _state.cov.block<3, 3>(3, 3); + voxelmap_manager->pv_list_[i].var = var; + } + voxelmap_manager->UpdateVoxelMap(voxelmap_manager->pv_list_); + std::cout << "[ LIO ] Update Voxel Map" << std::endl; + _pv_list = voxelmap_manager->pv_list_; + + double t4 = omp_get_wtime(); + + if(voxelmap_manager->config_setting_.map_sliding_en) + { + voxelmap_manager->mapSliding(); + } + + PointCloudXYZI::Ptr laserCloudFullRes(dense_map_en ? feats_undistort : feats_down_body); + int size = laserCloudFullRes->points.size(); + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + RGBpointBodyToWorld(&laserCloudFullRes->points[i], &laserCloudWorld->points[i]); + } + *pcl_w_wait_pub = *laserCloudWorld; + + publish_frame_world(pubLaserCloudFullRes, vio_manager); + if (pub_effect_point_en) publish_effect_world(pubLaserCloudEffect, voxelmap_manager->ptpl_list_); + if (voxelmap_manager->config_setting_.is_pub_plane_map_) voxelmap_manager->pubVoxelMap(); + publish_path(pubPath); + publish_mavros(mavros_pose_publisher); + + frame_num++; + aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t4 - t0) / frame_num; + + // aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + (t2 - t1) / frame_num; + // aver_time_map_inre = aver_time_map_inre * (frame_num - 1) / frame_num + (t4 - t3) / frame_num; + // aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + (solve_time) / frame_num; + // aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1) / frame_num + solve_const_H_time / frame_num; + // printf("[ mapping time ]: per scan: propagation %0.6f downsample: %0.6f match: %0.6f solve: %0.6f ICP: %0.6f map incre: %0.6f total: %0.6f \n" + // "[ mapping time ]: average: icp: %0.6f construct H: %0.6f, total: %0.6f \n", + // t_prop - t0, t1 - t_prop, match_time, solve_time, t3 - t1, t5 - t3, t5 - t0, aver_time_icp, aver_time_const_H_time, aver_time_consu); + + // printf("\033[1;36m[ LIO mapping time ]: current scan: icp: %0.6f secs, map incre: %0.6f secs, total: %0.6f secs.\033[0m\n" + // "\033[1;36m[ LIO mapping time ]: average: icp: %0.6f secs, map incre: %0.6f secs, total: %0.6f secs.\033[0m\n", + // t2 - t1, t4 - t3, t4 - t0, aver_time_icp, aver_time_map_inre, aver_time_consu); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| LIO Mapping Time |\033[0m\n"); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| %-29s | %-27s |\033[0m\n", "Algorithm Stage", "Time (secs)"); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "DownSample", t_down - t0); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "ICP", t2 - t1); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "updateVoxelMap", t4 - t3); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "Current Total Time", t4 - t0); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "Average Total Time", aver_time_consu); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + + euler_cur = RotMtoEuler(_state.rot_end); + fout_out << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " + << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " + << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time_per_cam.empty() ? 0.0 : _state.inv_expo_time_per_cam[0], 0, 0).transpose() << " " << feats_undistort->points.size() << std::endl; +} + +void LIVMapper::savePCD() +{ + if (pcd_save_en && (pcl_wait_save->points.size() > 0 || pcl_wait_save_intensity->points.size() > 0) && pcd_save_interval < 0) + { + std::string raw_points_dir = std::string(ROOT_DIR) + "Log/pcd/all_raw_points.pcd"; + std::string downsampled_points_dir = std::string(ROOT_DIR) + "Log/pcd/all_downsampled_points.pcd"; + pcl::PCDWriter pcd_writer; + + if (img_en) + { + pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); + pcl::VoxelGrid voxel_filter; + voxel_filter.setInputCloud(pcl_wait_save); + voxel_filter.setLeafSize(filter_size_pcd, filter_size_pcd, filter_size_pcd); + voxel_filter.filter(*downsampled_cloud); + + pcd_writer.writeBinary(raw_points_dir, *pcl_wait_save); // Save the raw point cloud data + std::cout << GREEN << "Raw point cloud data saved to: " << raw_points_dir + << " with point count: " << pcl_wait_save->points.size() << RESET << std::endl; + + pcd_writer.writeBinary(downsampled_points_dir, *downsampled_cloud); // Save the downsampled point cloud data + std::cout << GREEN << "Downsampled point cloud data saved to: " << downsampled_points_dir + << " with point count after filtering: " << downsampled_cloud->points.size() << RESET << std::endl; + + if(colmap_output_en) + { + fout_points << "# 3D point list with one line of data per point\n"; + fout_points << "# POINT_ID, X, Y, Z, R, G, B, ERROR\n"; + for (size_t i = 0; i < downsampled_cloud->size(); ++i) + { + const auto& point = downsampled_cloud->points[i]; + fout_points << i << " " + << std::fixed << std::setprecision(6) + << point.x << " " << point.y << " " << point.z << " " + << static_cast(point.r) << " " + << static_cast(point.g) << " " + << static_cast(point.b) << " " + << 0 << std::endl; + } + } + } + else + { + pcd_writer.writeBinary(raw_points_dir, *pcl_wait_save_intensity); + std::cout << GREEN << "Raw point cloud data saved to: " << raw_points_dir + << " with point count: " << pcl_wait_save_intensity->points.size() << RESET << std::endl; + } + } +} + +void LIVMapper::run(rclcpp::Node::SharedPtr &node) +{ + rclcpp::Rate rate(5000); + while (rclcpp::ok()) + { + rclcpp::spin_some(this->node); + if (!sync_packages(LidarMeasures)) + { + rate.sleep(); + continue; + } + handleFirstFrame(); + + processImu(); + + // if (!p_imu->imu_time_init) continue; + + stateEstimationAndMapping(); + } + savePCD(); +} + +void LIVMapper::prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D acc_avr, V3D angvel_avr) +{ + double mean_acc_norm = p_imu->IMU_mean_acc_norm; + acc_avr = acc_avr * G_m_s2 / mean_acc_norm - imu_prop_state.bias_a; + angvel_avr -= imu_prop_state.bias_g; + + M3D Exp_f = Exp(angvel_avr, dt); + /* propogation of IMU attitude */ + imu_prop_state.rot_end = imu_prop_state.rot_end * Exp_f; + + /* Specific acceleration (global frame) of IMU */ + V3D acc_imu = imu_prop_state.rot_end * acc_avr + V3D(imu_prop_state.gravity[0], imu_prop_state.gravity[1], imu_prop_state.gravity[2]); + + /* propogation of IMU */ + imu_prop_state.pos_end = imu_prop_state.pos_end + imu_prop_state.vel_end * dt + 0.5 * acc_imu * dt * dt; + + /* velocity of IMU */ + imu_prop_state.vel_end = imu_prop_state.vel_end + acc_imu * dt; +} + +void LIVMapper::imu_prop_callback() +{ + if (p_imu->imu_need_init || !new_imu || !ekf_finish_once) { return; } + mtx_buffer_imu_prop.lock(); + new_imu = false; // 控制 propagate 频率和 IMU 频率一致 + if (imu_prop_enable && !prop_imu_buffer.empty()) + { + static double last_t_from_lidar_end_time = 0; + if (state_update_flg) + { + imu_propagate = latest_ekf_state; + // drop all useless imu pkg + while ((!prop_imu_buffer.empty() && stamp2Sec(prop_imu_buffer.front().header.stamp) < latest_ekf_time)) + { + prop_imu_buffer.pop_front(); + } + last_t_from_lidar_end_time = 0; + for (int i = 0; i < prop_imu_buffer.size(); i++) + { + double t_from_lidar_end_time = stamp2Sec(prop_imu_buffer[i].header.stamp) - latest_ekf_time; + double dt = t_from_lidar_end_time - last_t_from_lidar_end_time; + // cout << "prop dt" << dt << ", " << t_from_lidar_end_time << ", " << last_t_from_lidar_end_time << endl; + V3D acc_imu(prop_imu_buffer[i].linear_acceleration.x, prop_imu_buffer[i].linear_acceleration.y, prop_imu_buffer[i].linear_acceleration.z); + V3D omg_imu(prop_imu_buffer[i].angular_velocity.x, prop_imu_buffer[i].angular_velocity.y, prop_imu_buffer[i].angular_velocity.z); + prop_imu_once(imu_propagate, dt, acc_imu, omg_imu); + last_t_from_lidar_end_time = t_from_lidar_end_time; + } + state_update_flg = false; + } + else + { + V3D acc_imu(newest_imu.linear_acceleration.x, newest_imu.linear_acceleration.y, newest_imu.linear_acceleration.z); + V3D omg_imu(newest_imu.angular_velocity.x, newest_imu.angular_velocity.y, newest_imu.angular_velocity.z); + double t_from_lidar_end_time = stamp2Sec(newest_imu.header.stamp) - latest_ekf_time; + double dt = t_from_lidar_end_time - last_t_from_lidar_end_time; + prop_imu_once(imu_propagate, dt, acc_imu, omg_imu); + last_t_from_lidar_end_time = t_from_lidar_end_time; + } + + V3D posi, vel_i; + Eigen::Quaterniond q; + posi = imu_propagate.pos_end; + vel_i = imu_propagate.vel_end; + q = Eigen::Quaterniond(imu_propagate.rot_end); + imu_prop_odom.header.frame_id = "world"; + imu_prop_odom.header.stamp = newest_imu.header.stamp; + imu_prop_odom.pose.pose.position.x = posi.x(); + imu_prop_odom.pose.pose.position.y = posi.y(); + imu_prop_odom.pose.pose.position.z = posi.z(); + imu_prop_odom.pose.pose.orientation.w = q.w(); + imu_prop_odom.pose.pose.orientation.x = q.x(); + imu_prop_odom.pose.pose.orientation.y = q.y(); + imu_prop_odom.pose.pose.orientation.z = q.z(); + imu_prop_odom.twist.twist.linear.x = vel_i.x(); + imu_prop_odom.twist.twist.linear.y = vel_i.y(); + imu_prop_odom.twist.twist.linear.z = vel_i.z(); + pubImuPropOdom->publish(imu_prop_odom); + } + mtx_buffer_imu_prop.unlock(); +} + +void LIVMapper::transformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, PointCloudXYZI::Ptr &trans_cloud) +{ + PointCloudXYZI().swap(*trans_cloud); + trans_cloud->reserve(input_cloud->size()); + for (size_t i = 0; i < input_cloud->size(); i++) + { + pcl::PointXYZINormal p_c = input_cloud->points[i]; + Eigen::Vector3d p(p_c.x, p_c.y, p_c.z); + p = (rot * (extR * p + extT) + t); + PointType pi; + pi.x = p(0); + pi.y = p(1); + pi.z = p(2); + pi.intensity = p_c.intensity; + trans_cloud->points.push_back(pi); + } +} + +void LIVMapper::pointBodyToWorld(const PointType &pi, PointType &po) +{ + V3D p_body(pi.x, pi.y, pi.z); + V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); + po.x = p_global(0); + po.y = p_global(1); + po.z = p_global(2); + po.intensity = pi.intensity; +} + +template void LIVMapper::pointBodyToWorld(const Matrix &pi, Matrix &po) +{ + V3D p_body(pi[0], pi[1], pi[2]); + V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); + po[0] = p_global(0); + po[1] = p_global(1); + po[2] = p_global(2); +} + +template Matrix LIVMapper::pointBodyToWorld(const Matrix &pi) +{ + V3D p(pi[0], pi[1], pi[2]); + p = (_state.rot_end * (extR * p + extT) + _state.pos_end); + Eigen::Matrix po(p[0], p[1], p[2]); + return po; +} + +void LIVMapper::RGBpointBodyToWorld(PointType const *const pi, PointType *const po) +{ + V3D p_body(pi->x, pi->y, pi->z); + V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; + po->curvature = pi->curvature; + po->normal_x = pi->normal_x; + po->normal_y = pi->normal_y; + po->normal_z = pi->normal_z; +} + +void LIVMapper::RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po) +{ + V3D p_body_lidar(pi->x, pi->y, pi->z); + V3D p_body_imu(extR * p_body_lidar + extT); + + po->x = p_body_imu(0); + po->y = p_body_imu(1); + po->z = p_body_imu(2); + po->intensity = pi->intensity; + po->curvature = pi->curvature; + po->normal_x = pi->normal_x; + po->normal_y = pi->normal_y; + po->normal_z = pi->normal_z; +} + +void LIVMapper::standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + if (!lidar_en) return; + mtx_buffer.lock(); + + double cur_head_time = stamp2Sec(msg->header.stamp) + lidar_time_offset; + // cout<<"got feature"<node->get_logger(),"lidar loop back, clear buffer"); + lid_raw_data_buffer.clear(); + } + // ROS_INFO("get point cloud at time: %.6f", stamp2Sec(msg->header.stamp)); + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + p_pre->process(msg, ptr); + lid_raw_data_buffer.push_back(ptr); + lid_header_time_buffer.push_back(cur_head_time); + last_timestamp_lidar = cur_head_time; + + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +void LIVMapper::livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg_in) +{ + if (!lidar_en) return; + mtx_buffer.lock(); + livox_ros_driver2::msg::CustomMsg::SharedPtr msg(new livox_ros_driver2::msg::CustomMsg(*msg_in)); + // if ((abs(stamp2Sec(msg->header.stamp) - last_timestamp_lidar) > 0.2 && last_timestamp_lidar > 0) || sync_jump_flag) + // { + // ROS_WARN("lidar jumps %.3f\n", stamp2Sec(msg->header.stamp) - last_timestamp_lidar); + // sync_jump_flag = true; + // msg->header.stamp = rclcpp::Time().fromSec(last_timestamp_lidar + 0.1); + // } + if (abs(last_timestamp_imu - stamp2Sec(msg->header.stamp)) > 1.0 && !imu_buffer.empty()) + { + double timediff_imu_wrt_lidar = last_timestamp_imu - stamp2Sec(msg->header.stamp); + RCLCPP_INFO(this->node->get_logger(), "\033[95mSelf sync IMU and LiDAR, HARD time lag is %.10lf \n\033[0m", timediff_imu_wrt_lidar - 0.100); + // imu_time_offset = timediff_imu_wrt_lidar; + } + + double cur_head_time = stamp2Sec(msg->header.stamp); + RCLCPP_INFO(this->node->get_logger(), "Get LiDAR, its header time: %.6f", cur_head_time); + if (cur_head_time < last_timestamp_lidar) + { + RCLCPP_ERROR(this->node->get_logger(), "lidar loop back, clear buffer"); + lid_raw_data_buffer.clear(); + } + RCLCPP_INFO(this->node->get_logger(), "get point cloud at time: %.6f", stamp2Sec(msg->header.stamp)); + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + p_pre->process(msg, ptr); + + if (!ptr || ptr->empty()) { + RCLCPP_ERROR(this->node->get_logger(), "Received an empty point cloud"); + mtx_buffer.unlock(); + return; + } + + lid_raw_data_buffer.push_back(ptr); + lid_header_time_buffer.push_back(cur_head_time); + last_timestamp_lidar = cur_head_time; + + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +void LIVMapper::imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr &msg_in) +{ + if (!imu_en) return; + + if (last_timestamp_lidar < 0.0) return; + RCLCPP_INFO(this->node->get_logger(), "get imu at time: %.6f", stamp2Sec(msg_in->header.stamp)); + sensor_msgs::msg::Imu::SharedPtr msg(new sensor_msgs::msg::Imu(*msg_in)); + msg->header.stamp = sec2Stamp(stamp2Sec(msg->header.stamp) - imu_time_offset); + double timestamp = stamp2Sec(msg->header.stamp); + + if (fabs(last_timestamp_lidar - timestamp) > 0.5 && (!ros_driver_fix_en)) + { + RCLCPP_WARN(this->node->get_logger(), "IMU and LiDAR not synced! delta time: %lf .\n", last_timestamp_lidar - timestamp); + } + + if (ros_driver_fix_en) timestamp += std::round(last_timestamp_lidar - timestamp); + msg->header.stamp = sec2Stamp(timestamp); + + mtx_buffer.lock(); + + if (last_timestamp_imu > 0.0 && timestamp < last_timestamp_imu) + { + mtx_buffer.unlock(); + sig_buffer.notify_all(); + RCLCPP_ERROR(this->node->get_logger(), "imu loop back, offset: %lf \n", last_timestamp_imu - timestamp); + return; + } + + if (last_timestamp_imu > 0.0 && timestamp > last_timestamp_imu + 0.2) + { + RCLCPP_WARN(this->node->get_logger(), "imu time stamp Jumps %0.4lf seconds \n", timestamp - last_timestamp_imu); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } + + last_timestamp_imu = timestamp; + + imu_buffer.push_back(msg); + cout<<"got imu: "<imu_need_init) { prop_imu_buffer.push_back(*msg); } + newest_imu = *msg; + new_imu = true; + mtx_buffer_imu_prop.unlock(); + } + sig_buffer.notify_all(); +} + +cv::Mat LIVMapper::getImageFromMsg(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg) +{ + cv::Mat img; + img = cv_bridge::toCvShare(img_msg, "bgr8")->image; + return img; +} + +// static int i = 0; +void LIVMapper::img_cbk(const sensor_msgs::msg::Image::ConstSharedPtr &msg_in, int cam_id) +{ + if (!img_en) return; + if (cam_id < 0 || cam_id >= static_cast(img_buffers.size())) + { + RCLCPP_ERROR(this->node->get_logger(), "Invalid cam_id: %d (num_of_cameras=%d)", cam_id, num_of_cameras); + return; + } + sensor_msgs::msg::Image::SharedPtr msg(new sensor_msgs::msg::Image(*msg_in)); + // if ((abs(stamp2Sec(msg->header.stamp) - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag) + // { + // RCLCPP_WARN(this->node->get_logger(), "img jumps %.3f\n", stamp2Sec(msg->header.stamp) - last_timestamp_img); + // sync_jump_flag = true; + // msg->header.stamp = rclcpp::Time().fromSec(last_timestamp_img + 0.1); + // } + + // Hiliti2022 40Hz + if (hilti_en) + { + static int frame_counter = 0; + if (++frame_counter % 4 != 0) return; + } + // double msg_header_time = stamp2Sec(msg->header.stamp); + double msg_header_time = stamp2Sec(msg->header.stamp) + img_time_offset; + if (abs(msg_header_time - last_timestamp_img[cam_id]) < 0.001) return; + RCLCPP_INFO(this->node->get_logger(), "Get image from camera %d, its header time: %.6f", cam_id, msg_header_time); + if (last_timestamp_lidar < 0) return; + + if (msg_header_time < last_timestamp_img[cam_id]) + { + RCLCPP_ERROR(this->node->get_logger(), "camera %d: image loop back. \n", cam_id); + return; + } + + mtx_buffer.lock(); + + double img_time_correct = msg_header_time; // last_timestamp_lidar + 0.105; + + if (img_time_correct - last_timestamp_img[cam_id] < 0.02) + { + RCLCPP_WARN(this->node->get_logger(), "camera %d: Image need Jumps: %.6f", cam_id, img_time_correct); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } + + cv::Mat img_cur = getImageFromMsg(msg); + img_buffers[cam_id].push_back(img_cur); + img_time_buffers[cam_id].push_back(img_time_correct); + + // ROS_INFO("Correct Image time: %.6f", img_time_correct); + + last_timestamp_img[cam_id] = img_time_correct; + // cv::imshow("img", img); + // cv::waitKey(1); + // cout<<"last_timestamp_img:::"<points.size() <= 1) return false; + + meas.lidar_frame_beg_time = lid_header_time_buffer.front(); // generate lidar_frame_beg_time + meas.lidar_frame_end_time = meas.lidar_frame_beg_time + meas.lidar->points.back().curvature / double(1000); // calc lidar scan end time + meas.pcl_proc_cur = meas.lidar; + lidar_pushed = true; // flag + } + + if (imu_en && last_timestamp_imu < meas.lidar_frame_end_time) + { // waiting imu message needs to be + // larger than _lidar_frame_end_time, + // make sure complete propagate. + // ROS_ERROR("out sync"); + return false; + } + + struct MeasureGroup m; // standard method to keep imu message. + + m.imu.clear(); + m.lio_time = meas.lidar_frame_end_time; + mtx_buffer.lock(); + while (!imu_buffer.empty()) + { + if (stamp2Sec(imu_buffer.front()->header.stamp) > meas.lidar_frame_end_time) break; + m.imu.push_back(imu_buffer.front()); + imu_buffer.pop_front(); + } + lid_raw_data_buffer.pop_front(); + lid_header_time_buffer.pop_front(); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + + meas.lio_vio_flg = LIO; // process lidar topic, so timestamp should be lidar scan end. + meas.measures.push_back(m); + // ROS_INFO("ONlY HAS LiDAR and IMU, NO IMAGE!"); + lidar_pushed = false; // sync one whole lidar scan. + return true; + + break; + } + + case LIVO: + { + /*** For LIVO mode, the time of LIO update is set to be the same as VIO, LIO + * first than VIO imediatly ***/ + EKF_STATE last_lio_vio_flg = meas.lio_vio_flg; + // double t0 = omp_get_wtime(); + switch (last_lio_vio_flg) + { + // double img_capture_time = meas.lidar_frame_beg_time + exposure_time_init; + case WAIT: + case VIO: + { + // printf("!!! meas.lio_vio_flg: %d \n", meas.lio_vio_flg); + + // Multi-camera time-alignment check (Omni-LIVO): all camera buffers + // must have a matching front timestamp within time_tolerance before + // this synced frame is consumed as a bundle. Otherwise drop the + // earliest-timestamped camera's stale frame and retry next tick. + const double time_tolerance = camera_time_tolerance; + double ref_timestamp = img_time_buffers[0].front(); + bool timestamps_consistent = true; + for (int i = 1; i < num_of_cameras; i++) + { + if (std::abs(img_time_buffers[i].front() - ref_timestamp) > time_tolerance) + { + timestamps_consistent = false; + break; + } + } + if (!timestamps_consistent) + { + mtx_buffer.lock(); + int earliest_cam = 0; + for (int i = 1; i < num_of_cameras; i++) + { + if (img_time_buffers[i].front() < img_time_buffers[earliest_cam].front()) earliest_cam = i; + } + img_buffers[earliest_cam].pop_front(); + img_time_buffers[earliest_cam].pop_front(); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + RCLCPP_WARN(this->node->get_logger(), "Camera timestamps inconsistent, dropped stale frame from camera %d", earliest_cam); + return false; + } + + double img_capture_time = ref_timestamp + exposure_time_init; + /*** has img topic, but img topic timestamp larger than lidar end time, + * process lidar topic. After LIO update, the meas.lidar_frame_end_time + * will be refresh. ***/ + if (meas.last_lio_update_time < 0.0) meas.last_lio_update_time = lid_header_time_buffer.front(); + // printf("[ Data Cut ] wait \n"); + // printf("[ Data Cut ] last_lio_update_time: %lf \n", + // meas.last_lio_update_time); + + double lid_newest_time = lid_header_time_buffer.back() + lid_raw_data_buffer.back()->points.back().curvature / double(1000); + double imu_newest_time = stamp2Sec(imu_buffer.back()->header.stamp); + + if (img_capture_time < meas.last_lio_update_time + 0.00001) + { + mtx_buffer.lock(); + for (int i = 0; i < num_of_cameras; i++) + { + img_buffers[i].pop_front(); + img_time_buffers[i].pop_front(); + } + mtx_buffer.unlock(); + sig_buffer.notify_all(); + RCLCPP_ERROR(this->node->get_logger(), "[ Data Cut ] Throw one image frame! \n"); + return false; + } + + if (img_capture_time > lid_newest_time || img_capture_time > imu_newest_time) + { + // RCLCPP_ERROR(this->node->get_logger(), "lost first camera frame"); + // printf("img_capture_time, lid_newest_time, imu_newest_time: %lf , %lf + // , %lf \n", img_capture_time, lid_newest_time, imu_newest_time); + return false; + } + + struct MeasureGroup m; + + // printf("[ Data Cut ] LIO \n"); + // printf("[ Data Cut ] img_capture_time: %lf \n", img_capture_time); + m.imu.clear(); + m.lio_time = img_capture_time; + mtx_buffer.lock(); + while (!imu_buffer.empty()) + { + if (stamp2Sec(imu_buffer.front()->header.stamp) > m.lio_time) break; + + if (stamp2Sec(imu_buffer.front()->header.stamp) > meas.last_lio_update_time) m.imu.push_back(imu_buffer.front()); + + imu_buffer.pop_front(); + // printf("[ Data Cut ] imu time: %lf \n", + // stamp2Sec(imu_buffer.front()->header.stamp)); + } + mtx_buffer.unlock(); + sig_buffer.notify_all(); + + *(meas.pcl_proc_cur) = *(meas.pcl_proc_next); + PointCloudXYZI().swap(*meas.pcl_proc_next); + + int lid_frame_num = lid_raw_data_buffer.size(); + int max_size = meas.pcl_proc_cur->size() + 24000 * lid_frame_num; + meas.pcl_proc_cur->reserve(max_size); + meas.pcl_proc_next->reserve(max_size); + // deque lidar_buffer_tmp; + + while (!lid_raw_data_buffer.empty()) + { + if (lid_header_time_buffer.front() > img_capture_time) break; + auto pcl(lid_raw_data_buffer.front()->points); + double frame_header_time(lid_header_time_buffer.front()); + float max_offs_time_ms = (m.lio_time - frame_header_time) * 1000.0f; + + for (int i = 0; i < pcl.size(); i++) + { + auto pt = pcl[i]; + if (pcl[i].curvature < max_offs_time_ms) + { + pt.curvature += (frame_header_time - meas.last_lio_update_time) * 1000.0f; + meas.pcl_proc_cur->points.push_back(pt); + } + else + { + pt.curvature += (frame_header_time - m.lio_time) * 1000.0f; + meas.pcl_proc_next->points.push_back(pt); + } + } + lid_raw_data_buffer.pop_front(); + lid_header_time_buffer.pop_front(); + } + + meas.measures.push_back(m); + meas.lio_vio_flg = LIO; + // meas.last_lio_update_time = m.lio_time; + // printf("!!! meas.lio_vio_flg: %d \n", meas.lio_vio_flg); + // printf("[ Data Cut ] pcl_proc_cur number: %d \n", meas.pcl_proc_cur + // ->points.size()); printf("[ Data Cut ] LIO process time: %lf \n", + // omp_get_wtime() - t0); + return true; + } + + case LIO: + { + // Same multi-camera consistency check as the WAIT/VIO branch above, + // now applied right before the synced camera bundle is consumed. + const double time_tolerance = camera_time_tolerance; + double ref_timestamp = img_time_buffers[0].front(); + bool timestamps_consistent = true; + for (int i = 1; i < num_of_cameras; i++) + { + if (std::abs(img_time_buffers[i].front() - ref_timestamp) > time_tolerance) + { + timestamps_consistent = false; + break; + } + } + if (!timestamps_consistent) + { + int earliest_cam = 0; + for (int i = 1; i < num_of_cameras; i++) + { + if (img_time_buffers[i].front() < img_time_buffers[earliest_cam].front()) earliest_cam = i; + } + RCLCPP_WARN(this->node->get_logger(), "VIO stage: dropping earliest frame from camera %d", earliest_cam); + img_buffers[earliest_cam].pop_front(); + img_time_buffers[earliest_cam].pop_front(); + sig_buffer.notify_all(); + return false; + } + + double img_capture_time = ref_timestamp + exposure_time_init; + meas.lio_vio_flg = VIO; + // printf("[ Data Cut ] VIO \n"); + meas.measures.clear(); + double imu_time = stamp2Sec(imu_buffer.front()->header.stamp); + + struct MeasureGroup m; + m.vio_time = img_capture_time; + m.lio_time = meas.last_lio_update_time; + m.imgs.resize(num_of_cameras); + mtx_buffer.lock(); + // while ((!imu_buffer.empty() && (imu_time < img_capture_time))) + // { + // imu_time = stamp2Sec(imu_buffer.front()->header.stamp); + // if (imu_time > img_capture_time) break; + // m.imu.push_back(imu_buffer.front()); + // imu_buffer.pop_front(); + // printf("[ Data Cut ] imu time: %lf \n", + // stamp2Sec(imu_buffer.front()->header.stamp)); + // } + for (int i = 0; i < num_of_cameras; i++) + { + m.imgs[i] = img_buffers[i].front(); + img_buffers[i].pop_front(); + img_time_buffers[i].pop_front(); + } + mtx_buffer.unlock(); + sig_buffer.notify_all(); + meas.measures.push_back(m); + lidar_pushed = false; // after VIO update, the _lidar_frame_end_time will be refresh. + // printf("[ Data Cut ] VIO process time: %lf \n", omp_get_wtime() - t0); + return true; + } + + default: + { + // printf("!! WRONG EKF STATE !!"); + return false; + } + // return false; + } + break; + } + + case ONLY_LO: + { + if (!lidar_pushed) + { + // If not in lidar scan, need to generate new meas + if (lid_raw_data_buffer.empty()) return false; + meas.lidar = lid_raw_data_buffer.front(); // push the first lidar topic + meas.lidar_frame_beg_time = lid_header_time_buffer.front(); // generate lidar_beg_time + meas.lidar_frame_end_time = meas.lidar_frame_beg_time + meas.lidar->points.back().curvature / double(1000); // calc lidar scan end time + lidar_pushed = true; + } + struct MeasureGroup m; // standard method to keep imu message. + m.lio_time = meas.lidar_frame_end_time; + mtx_buffer.lock(); + lid_raw_data_buffer.pop_front(); + lid_header_time_buffer.pop_front(); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + lidar_pushed = false; // sync one whole lidar scan. + meas.lio_vio_flg = LO; // process lidar topic, so timestamp should be lidar scan end. + meas.measures.push_back(m); + return true; + break; + } + + default: + { + printf("!! WRONG SLAM TYPE !!"); + return false; + } + } + RCLCPP_ERROR(this->node->get_logger(), "out sync"); +} + +void LIVMapper::publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager) +{ + // panorama_image is a mosaic of all cameras' current frames (single camera + // => just that one image), built by VIOManager::plotTrackedPoints(). + cv::Mat img_rgb = vio_manager->panorama_image; + if (img_rgb.empty()) return; + cv_bridge::CvImage out_msg; + out_msg.header.stamp = this->node->get_clock()->now(); + // out_msg.header.frame_id = "camera_init"; + out_msg.encoding = sensor_msgs::image_encodings::BGR8; + out_msg.image = img_rgb; + pubImage.publish(out_msg.toImageMsg()); +} + +// Provide output format for LiDAR-visual BA +void LIVMapper::publish_frame_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudFullRes, VIOManagerPtr vio_manager) +{ + if (pcl_w_wait_pub->empty()) return; + PointCloudXYZRGB::Ptr laserCloudWorldRGB(new PointCloudXYZRGB()); + static int pub_num = 1; + pub_num++; + + if (LidarMeasures.lio_vio_flg == VIO) + { + *pcl_wait_pub += *pcl_w_wait_pub; + if(pub_num >= pub_scan_num) + { + pub_num = 1; + size_t size = pcl_wait_pub->points.size(); + laserCloudWorldRGB->reserve(size); + // double inv_expo = _state.inv_expo_time; + // Color each point by feather-blending every camera that currently sees + // it, instead of appending one point per camera unmerged. Weight ramps + // from 0 at a camera's FOV border to 1 at its center, normalized across + // all cameras that see the point — so in the overlap band between two + // cameras' FOVs the color cross-fades smoothly instead of showing a hard + // seam / duplicated points with mismatched color. A point seen by only + // one camera always gets that camera's raw color (weight normalizes to + // 1 regardless of the feather curve), so this is a no-op if the rig is + // later mounted with non-overlapping camera FOVs. + std::vector w_sum(size, 0.0), r_sum(size, 0.0), g_sum(size, 0.0), b_sum(size, 0.0); + + for (int cam_idx = 0; cam_idx < (int)vio_manager->cams.size(); ++cam_idx) + { + cv::Mat img_rgb = vio_manager->imgs_rgb[cam_idx]; + auto cam = vio_manager->new_frame_->cams_[cam_idx]; + const double cam_w = cam->width(); + const double cam_h = cam->height(); + for (size_t i = 0; i < size; i++) + { + V3D p_w(pcl_wait_pub->points[i].x, pcl_wait_pub->points[i].y, pcl_wait_pub->points[i].z); + V3D pf(vio_manager->new_frame_->w2f(p_w, cam_idx)); if (pf[2] < 0) continue; + if (pf.norm() <= blind_rgb_points) continue; + V2D pc(vio_manager->new_frame_->w2c(p_w, cam_idx)); + + if (!cam->isInFrame(pc.cast(), 3)) continue; // 100 + + // Feather weight: 0 at the FOV border, 1 at the FOV center. Floor + // of 0.05 so a point right at the very edge of the *only* camera + // that sees it still gets a (near-)full-weight color rather than + // being nearly zeroed out by the normalization below. + double dx = std::min(pc[0], cam_w - 1 - pc[0]) / (cam_w * 0.5); + double dy = std::min(pc[1], cam_h - 1 - pc[1]) / (cam_h * 0.5); + double weight = std::max(0.05, std::min({dx, dy, 1.0})); + + V3F pixel = vio_manager->getInterpolatedPixel(img_rgb, pc); + r_sum[i] += weight * pixel[2]; + g_sum[i] += weight * pixel[1]; + b_sum[i] += weight * pixel[0]; + w_sum[i] += weight; + } + } + + for (size_t i = 0; i < size; i++) + { + if (w_sum[i] <= 0.0) continue; + PointTypeRGB pointRGB; + pointRGB.x = pcl_wait_pub->points[i].x; + pointRGB.y = pcl_wait_pub->points[i].y; + pointRGB.z = pcl_wait_pub->points[i].z; + pointRGB.r = static_cast(std::clamp(r_sum[i] / w_sum[i], 0.0, 255.0)); + pointRGB.g = static_cast(std::clamp(g_sum[i] / w_sum[i], 0.0, 255.0)); + pointRGB.b = static_cast(std::clamp(b_sum[i] / w_sum[i], 0.0, 255.0)); + laserCloudWorldRGB->push_back(pointRGB); + } + } + } + + /*** Publish Frame ***/ + sensor_msgs::msg::PointCloud2 laserCloudmsg; + if (slam_mode_ == LIVO && LidarMeasures.lio_vio_flg == VIO) + { + pcl::toROSMsg(*laserCloudWorldRGB, laserCloudmsg); + } + if (slam_mode_ == ONLY_LIO || slam_mode_ == ONLY_LO) + { + pcl::toROSMsg(*pcl_w_wait_pub, laserCloudmsg); + } + laserCloudmsg.header.stamp = this->node->get_clock()->now(); //.fromSec(last_timestamp_lidar); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFullRes->publish(laserCloudmsg); + + /**************** save map ****************/ + /* 1. make sure you have enough memories + /* 2. noted that pcd save will influence the real-time performences **/ + double update_time = 0.0; + if (LidarMeasures.lio_vio_flg == VIO) { + update_time = LidarMeasures.measures.back().vio_time; + } else { // LIO / LO + update_time = LidarMeasures.measures.back().lio_time; + } + std::stringstream ss_time; + ss_time << std::fixed << std::setprecision(6) << update_time; + + if (pcd_save_en) + { + static int scan_wait_num = 0; + + switch (pcd_save_type) + { + case 0: /** world frame **/ + if (slam_mode_ == LIVO) + { + *pcl_wait_save += *laserCloudWorldRGB; + } + else + { + *pcl_wait_save_intensity += *pcl_w_wait_pub; + } + if(LidarMeasures.lio_vio_flg == LIO || LidarMeasures.lio_vio_flg == LO) scan_wait_num++; + break; + + case 1: /** body frame **/ + if (LidarMeasures.lio_vio_flg == LIO || LidarMeasures.lio_vio_flg == LO) + { + int size = feats_undistort->points.size(); + PointCloudXYZI::Ptr laserCloudBody(new PointCloudXYZI(size, 1)); + for (int i = 0; i < size; i++) + { + RGBpointBodyLidarToIMU(&feats_undistort->points[i], &laserCloudBody->points[i]); + } + *pcl_wait_save_intensity += *laserCloudBody; + scan_wait_num++; + cout << "save body frame points: " << pcl_wait_save_intensity->points.size() << endl; + } + pcd_save_interval = 1; + + break; + + default: + pcd_save_interval = 1; + scan_wait_num++; + break; + } + if ((pcl_wait_save->size() > 0 || pcl_wait_save_intensity->size() > 0) && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) + { + string all_points_dir(string(string(ROOT_DIR) + "Log/pcd/") + ss_time.str() + string(".pcd")); + + pcl::PCDWriter pcd_writer; + + cout << "current scan saved to " << all_points_dir << endl; + if (pcl_wait_save->points.size() > 0) + { + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); // pcl::io::savePCDFileASCII(all_points_dir, *pcl_wait_save); + PointCloudXYZRGB().swap(*pcl_wait_save); + } + if(pcl_wait_save_intensity->points.size() > 0) + { + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save_intensity); + PointCloudXYZI().swap(*pcl_wait_save_intensity); + } + scan_wait_num = 0; + } + + if(LidarMeasures.lio_vio_flg == LIO || LidarMeasures.lio_vio_flg == LO) + { + Eigen::Quaterniond q(_state.rot_end); + fout_lidar_pos << std::fixed << std::setprecision(6); + fout_lidar_pos << LidarMeasures.measures.back().lio_time << " " << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " " << q.x() << " " << q.y() << " " << q.z() + << " " << q.w() << " " << endl; + } + } + if (img_save_en && LidarMeasures.lio_vio_flg == VIO) + { + static int img_wait_num = 0; + img_wait_num++; + + if (img_save_interval > 0 && img_wait_num >= img_save_interval) + { + if (!vio_manager->panorama_image.empty()) + imwrite(string(string(ROOT_DIR) + "Log/image/") + ss_time.str() + string(".png"), vio_manager->panorama_image); + + Eigen::Quaterniond q(_state.rot_end); + fout_visual_pos << std::fixed << std::setprecision(6); + fout_visual_pos << LidarMeasures.measures.back().vio_time << " " << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " " + << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl; + img_wait_num = 0; + } + } + + if(laserCloudWorldRGB->size() > 0) PointCloudXYZI().swap(*pcl_wait_pub); + if(LidarMeasures.lio_vio_flg == VIO) PointCloudXYZI().swap(*pcl_w_wait_pub); +} + +void LIVMapper::publish_visual_sub_map(const rclcpp::Publisher::SharedPtr &pubSubVisualMap) +{ + PointCloudXYZI::Ptr laserCloudFullRes(visual_sub_map); + int size = laserCloudFullRes->points.size(); if (size == 0) return; + PointCloudXYZI::Ptr sub_pcl_visual_map_pub(new PointCloudXYZI()); + *sub_pcl_visual_map_pub = *laserCloudFullRes; + if (1) + { + sensor_msgs::msg::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*sub_pcl_visual_map_pub, laserCloudmsg); + laserCloudmsg.header.stamp = this->node->get_clock()->now(); + laserCloudmsg.header.frame_id = "camera_init"; + pubSubVisualMap->publish(laserCloudmsg); + } +} + +void LIVMapper::publish_effect_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudEffect, const std::vector &ptpl_list) +{ + int effect_feat_num = ptpl_list.size(); + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(effect_feat_num, 1)); + for (int i = 0; i < effect_feat_num; i++) + { + laserCloudWorld->points[i].x = ptpl_list[i].point_w_[0]; + laserCloudWorld->points[i].y = ptpl_list[i].point_w_[1]; + laserCloudWorld->points[i].z = ptpl_list[i].point_w_[2]; + } + sensor_msgs::msg::PointCloud2 laserCloudFullRes3; + pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); + laserCloudFullRes3.header.stamp = this->node->get_clock()->now(); + laserCloudFullRes3.header.frame_id = "camera_init"; + pubLaserCloudEffect->publish(laserCloudFullRes3); +} + +template void LIVMapper::set_posestamp(T &out) +{ + out.position.x = _state.pos_end(0); + out.position.y = _state.pos_end(1); + out.position.z = _state.pos_end(2); + out.orientation.x = geoQuat.x; + out.orientation.y = geoQuat.y; + out.orientation.z = geoQuat.z; + out.orientation.w = geoQuat.w; +} + +void LIVMapper::publish_odometry(const rclcpp::Publisher::SharedPtr &pubOdomAftMapped) +{ + odomAftMapped.header.frame_id = "camera_init"; + odomAftMapped.child_frame_id = "aft_mapped"; + odomAftMapped.header.stamp = this->node->get_clock()->now(); //.ros::Time()fromSec(last_timestamp_lidar); + set_posestamp(odomAftMapped.pose.pose); + + static std::shared_ptr br; + br = std::make_shared(this->node); + tf2::Transform transform; + tf2::Quaternion q; + transform.setOrigin(tf2::Vector3(_state.pos_end(0), _state.pos_end(1), _state.pos_end(2))); + q.setW(geoQuat.w); + q.setX(geoQuat.x); + q.setY(geoQuat.y); + q.setZ(geoQuat.z); + transform.setRotation(q); + br->sendTransform(geometry_msgs::msg::TransformStamped(createTransformStamped(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped"))); + pubOdomAftMapped->publish(odomAftMapped); +} + +void LIVMapper::publish_mavros(const rclcpp::Publisher::SharedPtr &mavros_pose_publisher) +{ + msg_body_pose.header.stamp = this->node->get_clock()->now(); + msg_body_pose.header.frame_id = "camera_init"; + set_posestamp(msg_body_pose.pose); + mavros_pose_publisher->publish(msg_body_pose); +} + +void LIVMapper::publish_path(const rclcpp::Publisher::SharedPtr &pubPath) +{ + set_posestamp(msg_body_pose.pose); + msg_body_pose.header.stamp = this->node->get_clock()->now(); + msg_body_pose.header.frame_id = "camera_init"; + path.poses.push_back(msg_body_pose); + pubPath->publish(path); +} \ No newline at end of file diff --git a/src/FAST-LIVO2/src/frame.cpp b/src/FAST-LIVO2/src/frame.cpp new file mode 100644 index 0000000..f58bd68 --- /dev/null +++ b/src/FAST-LIVO2/src/frame.cpp @@ -0,0 +1,85 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#include +#include "feature.h" +#include "frame.h" +#include "visual_point.h" +#include +#include +#include +#include + +int Frame::frame_counter_ = 0; + +Frame::Frame(const std::vector &cams, std::vector &imgs, double timestamp) + : id_(frame_counter_++), + cams_(cams), + T_f_w_(cams.size(), SE3()), + T_f_w_prior_(cams.size(), SE3()), + timestamp_(timestamp) +{ + initFrame(imgs); +} + +Frame::~Frame() +{ + std::for_each(fts_.begin(), fts_.end(), [&](Feature *i) { delete i; }); +} + +void Frame::initFrame(std::vector &imgs) +{ + if (imgs.empty() || imgs.size() != cams_.size()) + { + throw std::runtime_error("Frame: number of images must match the number of cameras"); + } + + for (size_t i = 0; i < cams_.size(); ++i) + { + if (imgs[i].empty()) { throw std::runtime_error("Frame: one of the provided images is empty"); } + + if (imgs[i].cols != cams_[i]->width() || imgs[i].rows != cams_[i]->height()) + { + throw std::runtime_error("Frame: provided image does not match the size of camera " + std::to_string(i)); + } + + if (imgs[i].type() != CV_8UC1) { throw std::runtime_error("Frame: one of the provided images is not grayscale"); } + } + + // Each camera's image is held via a shared_ptr (clone()'d so Frame owns + // independent data), with imgs_ kept as plain cv::Mat headers referencing + // the same underlying data for call-site compatibility. + imgs_shared_.resize(imgs.size()); + imgs_.resize(imgs.size()); + for (size_t i = 0; i < imgs.size(); ++i) + { + imgs_shared_[i] = std::make_shared(imgs[i].clone()); + imgs_[i] = *imgs_shared_[i]; + } +} + +/// Utility functions for the Frame class +namespace frame_utils +{ + +void createImgPyramid(const cv::Mat &img_level_0, int n_levels, ImgPyr &pyr) +{ + pyr.resize(n_levels); + pyr[0] = img_level_0; + for (int i = 1; i < n_levels; ++i) + { + pyr[i] = cv::Mat(pyr[i - 1].rows / 2, pyr[i - 1].cols / 2, CV_8U); + vk::halfSample(pyr[i - 1], pyr[i]); + } +} + +} // namespace frame_utils diff --git a/src/FAST-LIVO2/src/main.cpp b/src/FAST-LIVO2/src/main.cpp new file mode 100755 index 0000000..61dd592 --- /dev/null +++ b/src/FAST-LIVO2/src/main.cpp @@ -0,0 +1,14 @@ +#include "LIVMapper.h" + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + rclcpp::Node::SharedPtr nh; + image_transport::ImageTransport it_(nh); + LIVMapper mapper(nh, "laserMapping"); + mapper.initializeSubscribersAndPublishers(nh, it_); + mapper.run(nh); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/FAST-LIVO2/src/preprocess.cpp b/src/FAST-LIVO2/src/preprocess.cpp new file mode 100755 index 0000000..bf0d497 --- /dev/null +++ b/src/FAST-LIVO2/src/preprocess.cpp @@ -0,0 +1,1126 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#include "preprocess.h" + +#define RETURN0 0x00 +#define RETURN0AND1 0x10 + +Preprocess::Preprocess() : feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1) +{ + inf_bound = 10; + N_SCANS = 6; + group_size = 8; + disA = 0.01; + disA = 0.1; // B? + p2l_ratio = 225; + limit_maxmid = 6.25; + limit_midmin = 6.25; + limit_maxmin = 3.24; + jump_up_limit = 170.0; + jump_down_limit = 8.0; + cos160 = 160.0; + edgea = 2; + edgeb = 0.1; + smallp_intersect = 172.5; + smallp_ratio = 1.2; + given_offset_time = false; + + jump_up_limit = cos(jump_up_limit / 180 * M_PI); + jump_down_limit = cos(jump_down_limit / 180 * M_PI); + cos160 = cos(cos160 / 180 * M_PI); + smallp_intersect = cos(smallp_intersect / 180 * M_PI); +} + +Preprocess::~Preprocess() {} + +void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) +{ + feature_enabled = feat_en; + lidar_type = lid_type; + blind = bld; + point_filter_num = pfilt_num; +} + +void Preprocess::process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out) +{ + avia_handler(msg); + *pcl_out = pl_surf; +} + +void Preprocess::process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out) +{ + switch (lidar_type) + { + case OUST64: + oust64_handler(msg); + break; + + case VELO16: + velodyne_handler(msg); + break; + + case L515: + l515_handler(msg); + break; + + case XT32: + xt32_handler(msg); + break; + + case PANDAR128: + Pandar128_handler(msg); + break; + + case ROBOSENSE: + robosense_handler(msg); + break; + + default: + printf("Error LiDAR Type: %d \n", lidar_type); + break; + } + *pcl_out = pl_surf; +} + +void Preprocess::avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + double t1 = omp_get_wtime(); + int plsize = msg->point_num; + printf("[ Preprocess ] Input point number: %d \n", plsize); + // printf("point_filter_num: %d\n", point_filter_num); + + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + pl_full.resize(plsize); + + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + uint valid_num = 0; + + if (feature_enabled) + { + for (uint i = 1; i < plsize; i++) + { + if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10)) + { + pl_full[i].x = msg->points[i].x; + pl_full[i].y = msg->points[i].y; + pl_full[i].z = msg->points[i].z; + pl_full[i].intensity = msg->points[i].reflectivity; + pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points + + bool is_new = false; + if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || + (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) + { + pl_buff[msg->points[i].line].push_back(pl_full[i]); + } + } + } + static int count = 0; + static double time = 0.0; + count++; + double t0 = omp_get_wtime(); + for (int j = 0; j < N_SCANS; j++) + { + if (pl_buff[j].size() <= 5) continue; + pcl::PointCloud &pl = pl_buff[j]; + plsize = pl.size(); + vector &types = typess[j]; + types.clear(); + types.resize(plsize); + plsize--; + for (uint i = 0; i < plsize; i++) + { + types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y; + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y; + give_feature(pl, types); + // pl_surf += pl; + } + time += omp_get_wtime() - t0; + printf("Feature extraction time: %lf \n", time / count); + } + else + { + for (uint i = 0; i < plsize; i++) + { + if ((msg->points[i].line < N_SCANS)) // && ((msg->points[i].tag & 0x30) == 0x10)) + { + valid_num++; + + pl_full[i].x = msg->points[i].x; + pl_full[i].y = msg->points[i].y; + pl_full[i].z = msg->points[i].z; + pl_full[i].intensity = msg->points[i].reflectivity; + pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points + + if (i == 0) + pl_full[i].curvature = fabs(pl_full[i].curvature) < 1.0 ? pl_full[i].curvature : 0.0; + else + { + // if(fabs(pl_full[i].curvature - pl_full[i - 1].curvature) > 1.0) ROS_ERROR("time jump: %f", fabs(pl_full[i].curvature - pl_full[i - 1].curvature)); + pl_full[i].curvature = fabs(pl_full[i].curvature - pl_full[i - 1].curvature) < 1.0 + ? pl_full[i].curvature + : pl_full[i - 1].curvature + 0.004166667f; // float(100/24000) + } + + if (valid_num % point_filter_num == 0) + { + if (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z >= blind_sqr) + { + pl_surf.push_back(pl_full[i]); + // if (i % 100 == 0 || i == 0) printf("pl_full[i].curvature: %f \n", + // pl_full[i].curvature); + } + } + } + } + } + printf("[ Preprocess ] Output point number: %zu \n", pl_surf.points.size()); +} + +void Preprocess::l515_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + + double time_stamp = stamp2Sec(msg->header.stamp); + // cout << "===================================" << endl; + // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); + for (int i = 0; i < pl_orig.points.size(); i++) + { + if (i % point_filter_num != 0) continue; + + double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + + if (range < blind_sqr) continue; + + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.normal_x = pl_orig.points[i].r; + added_pt.normal_y = pl_orig.points[i].g; + added_pt.normal_z = pl_orig.points[i].b; + + added_pt.curvature = 0.0; + pl_surf.points.push_back(added_pt); + } + + cout << "pl size:: " << pl_orig.points.size() << endl; + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +void Preprocess::oust64_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + if (feature_enabled) + { + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + + for (uint i = 0; i < plsize; i++) + { + double range = + pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + if (range < blind_sqr) continue; + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; + if (yaw_angle >= 180.0) yaw_angle -= 360.0; + if (yaw_angle <= -180.0) yaw_angle += 360.0; + + added_pt.curvature = pl_orig.points[i].t / 1e6; + if (pl_orig.points[i].ring < N_SCANS) { pl_buff[pl_orig.points[i].ring].push_back(added_pt); } + } + + for (int j = 0; j < N_SCANS; j++) + { + PointCloudXYZI &pl = pl_buff[j]; + int linesize = pl.size(); + vector &types = typess[j]; + types.clear(); + types.resize(linesize); + linesize--; + for (uint i = 0; i < linesize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); + give_feature(pl, types); + } + } + else + { + double time_stamp = stamp2Sec(msg->header.stamp); + // cout << "===================================" << endl; + // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); + for (int i = 0; i < pl_orig.points.size(); i++) + { + if (i % point_filter_num != 0) continue; + + double range = + pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + + if (range < blind_sqr) continue; + + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; + if (yaw_angle >= 180.0) yaw_angle -= 360.0; + if (yaw_angle <= -180.0) yaw_angle += 360.0; + + added_pt.curvature = pl_orig.points[i].t / 1e6; + + // cout<header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +#define MAX_LINE_NUM 64 + +void Preprocess::velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) return; + pl_surf.reserve(plsize); + + bool is_first[MAX_LINE_NUM]; + double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point + double omega_l = 3.61; // scan angular velocity + float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point + float time_last[MAX_LINE_NUM] = {0.0}; // last offset time + + if (pl_orig.points[plsize - 1].time > 0) { given_offset_time = true; } + else + { + given_offset_time = false; + memset(is_first, true, sizeof(is_first)); + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + if (feature_enabled) + { + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + int layer = pl_orig.points[i].ring; + if (layer >= N_SCANS) continue; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms + + if (!given_offset_time) + { + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; + if (is_first[layer]) + { + // printf("layer: %d; is first: %d", layer, is_first[layer]); + yaw_fp[layer] = yaw_angle; + is_first[layer] = false; + added_pt.curvature = 0.0; + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + continue; + } + + if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; } + else { added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; } + + if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l; + + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + } + + pl_buff[layer].points.push_back(added_pt); + } + + for (int j = 0; j < N_SCANS; j++) + { + PointCloudXYZI &pl = pl_buff[j]; + int linesize = pl.size(); + if (linesize < 2) continue; + vector &types = typess[j]; + types.clear(); + types.resize(linesize); + linesize--; + for (uint i = 0; i < linesize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); + give_feature(pl, types); + } + } + else + { + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< blind_sqr) + { + pl_surf.points.push_back(added_pt); + // printf("time mode: %d time: %d \n", given_offset_time, + // pl_orig.points[i].t); + } + } + } + } + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_surf, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +void Preprocess::Pandar128_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + pl_surf.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + pl_surf.reserve(plsize); + + double time_head = pl_orig.points[0].timestamp; + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = static_cast(pl_orig.points[i].intensity) / 255.0f; + added_pt.curvature = (pl_orig.points[i].timestamp - time_head) * 1000.f; + + if (i % point_filter_num == 0) + { + if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind_sqr) + { + pl_surf.points.push_back(added_pt); + // printf("time mode: %d time: %d \n", given_offset_time, + // pl_orig.points[i].t); + } + } + } + + // define a lambda function for the comparison + auto comparePoints = [](const PointType& a, const PointType& b) -> bool + { + return a.curvature < b.curvature; + }; + + // sort the points using the comparison function + std::sort(pl_surf.points.begin(), pl_surf.points.end(), comparePoints); + + // cout << GREEN << "pl_surf.points[0].timestamp: " << pl_surf.points[0].curvature << RESET << endl; + // cout << GREEN << "pl_surf.points[1000].timestamp: " << pl_surf.points[1000].curvature << RESET << endl; + // cout << GREEN << "pl_surf.points[5000].timestamp: " << pl_surf.points[5000].curvature << RESET << endl; + // cout << GREEN << "pl_surf.points[10000].timestamp: " << pl_surf.points[10000].curvature << RESET << endl; + // cout << GREEN << "pl_surf.points[20000].timestamp: " << pl_surf.points[20000].curvature << RESET << endl; + // cout << GREEN << "pl_surf.points[30000].timestamp: " << pl_surf.points[30000].curvature << RESET << endl; + // cout << GREEN << "pl_surf.points[31000].timestamp: " << pl_surf.points[31000].curvature << RESET << endl; +} + +void Preprocess::xt32_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + pl_surf.reserve(plsize); + + bool is_first[MAX_LINE_NUM]; + double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point + double omega_l = 3.61; // scan angular velocity + float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point + float time_last[MAX_LINE_NUM] = {0.0}; // last offset time + + if (pl_orig.points[plsize - 1].timestamp > 0) { given_offset_time = true; } + else + { + given_offset_time = false; + memset(is_first, true, sizeof(is_first)); + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + double time_head = pl_orig.points[0].timestamp; + + if (feature_enabled) + { + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + int layer = pl_orig.points[i].ring; + if (layer >= N_SCANS) continue; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.curvature = pl_orig.points[i].timestamp / 1000.0; // units: ms + + if (!given_offset_time) + { + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; + if (is_first[layer]) + { + // printf("layer: %d; is first: %d", layer, is_first[layer]); + yaw_fp[layer] = yaw_angle; + is_first[layer] = false; + added_pt.curvature = 0.0; + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + continue; + } + + if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; } + else { added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; } + + if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l; + + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + } + + pl_buff[layer].points.push_back(added_pt); + } + + for (int j = 0; j < N_SCANS; j++) + { + PointCloudXYZI &pl = pl_buff[j]; + int linesize = pl.size(); + if (linesize < 2) continue; + vector &types = typess[j]; + types.clear(); + types.resize(linesize); + linesize--; + for (uint i = 0; i < linesize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); + give_feature(pl, types); + } + } + else + { + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< blind_sqr) + { + pl_surf.points.push_back(added_pt); + // printf("time mode: %d time: %d \n", given_offset_time, + // pl_orig.points[i].t); + } + } + } + } + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_surf, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +void Preprocess::robosense_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + pl_surf.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + pl_surf.reserve(plsize); + + double time_head = pl_orig.points[0].timestamp; + for (int i = 0; i < plsize; ++i) + { + if (i % point_filter_num != 0) continue; + + const auto& pt = pl_orig.points[i]; + const double x = pt.x, y = pt.y, z = pt.z; + const double dist_sqr = x * x + y * y + z * z; + const bool is_valid = (dist_sqr >= blind_sqr) && !std::isnan(x) && !std::isnan(y) && !std::isnan(z); + if (!is_valid) continue; + + PointType added_pt; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.x = pt.x; + added_pt.y = pt.y; + added_pt.z = pt.z; + added_pt.intensity = pt.intensity; + added_pt.curvature = (pt.timestamp - time_head) * 1000.0; + pl_surf.points.push_back(added_pt); + } + std::sort(pl_surf.points.begin(), pl_surf.points.end(), [](const PointType &a, const PointType &b) { + return a.curvature < b.curvature; + }); +} + +void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) +{ + int plsize = pl.size(); + int plsize2; + if (plsize == 0) + { + printf("something wrong\n"); + return; + } + uint head = 0; + + while (types[head].range < blind_sqr) + { + head++; + } + + // Surf + plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; + + Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); + Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); + + uint i_nex = 0, i2; + uint last_i = 0; + uint last_i_nex = 0; + int last_state = 0; + int plane_type; + + for (uint i = head; i < plsize2; i++) + { + if (types[i].range < blind_sqr) { continue; } + + i2 = i; + + plane_type = plane_judge(pl, types, i, i_nex, curr_direct); + + if (plane_type == 1) + { + for (uint j = i; j <= i_nex; j++) + { + if (j != i && j != i_nex) { types[j].ftype = Real_Plane; } + else { types[j].ftype = Poss_Plane; } + } + + // if(last_state==1 && fabs(last_direct.sum())>0.5) + if (last_state == 1 && last_direct.norm() > 0.1) + { + double mod = last_direct.transpose() * curr_direct; + if (mod > -0.707 && mod < 0.707) { types[i].ftype = Edge_Plane; } + else { types[i].ftype = Real_Plane; } + } + + i = i_nex - 1; + last_state = 1; + } + else // if(plane_type == 2) + { + i = i_nex; + last_state = 0; + } + // else if(plane_type == 0) + // { + // if(last_state == 1) + // { + // uint i_nex_tem; + // uint j; + // for(j=last_i+1; j<=last_i_nex; j++) + // { + // uint i_nex_tem2 = i_nex_tem; + // Eigen::Vector3d curr_direct2; + + // uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2); + + // if(ttem != 1) + // { + // i_nex_tem = i_nex_tem2; + // break; + // } + // curr_direct = curr_direct2; + // } + + // if(j == last_i+1) + // { + // last_state = 0; + // } + // else + // { + // for(uint k=last_i_nex; k<=i_nex_tem; k++) + // { + // if(k != i_nex_tem) + // { + // types[k].ftype = Real_Plane; + // } + // else + // { + // types[k].ftype = Poss_Plane; + // } + // } + // i = i_nex_tem-1; + // i_nex = i_nex_tem; + // i2 = j-1; + // last_state = 1; + // } + + // } + // } + + last_i = i2; + last_i_nex = i_nex; + last_direct = curr_direct; + } + + plsize2 = plsize > 3 ? plsize - 3 : 0; + for (uint i = head + 3; i < plsize2; i++) + { + if (types[i].range < blind_sqr || types[i].ftype >= Real_Plane) { continue; } + + if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16) { continue; } + + Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); + Eigen::Vector3d vecs[2]; + + for (int j = 0; j < 2; j++) + { + int m = -1; + if (j == 1) { m = 1; } + + if (types[i + m].range < blind_sqr) + { + if (types[i].range > inf_bound) { types[i].edj[j] = Nr_inf; } + else { types[i].edj[j] = Nr_blind; } + continue; + } + + vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z); + vecs[j] = vecs[j] - vec_a; + + types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); + if (types[i].angle[j] < jump_up_limit) { types[i].edj[j] = Nr_180; } + else if (types[i].angle[j] > jump_down_limit) { types[i].edj[j] = Nr_zero; } + } + + types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); + if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && types[i].dista > 0.0225 && types[i].dista > 4 * types[i - 1].dista) + { + if (types[i].intersect > cos160) + { + if (edge_jump_judge(pl, types, i, Prev)) { types[i].ftype = Edge_Jump; } + } + } + else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 && types[i - 1].dista > 4 * types[i].dista) + { + if (types[i].intersect > cos160) + { + if (edge_jump_judge(pl, types, i, Next)) { types[i].ftype = Edge_Jump; } + } + } + else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf) + { + if (edge_jump_judge(pl, types, i, Prev)) { types[i].ftype = Edge_Jump; } + } + else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor) + { + if (edge_jump_judge(pl, types, i, Next)) { types[i].ftype = Edge_Jump; } + } + else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor) + { + if (types[i].ftype == Nor) { types[i].ftype = Wire; } + } + } + + plsize2 = plsize - 1; + double ratio; + for (uint i = head + 1; i < plsize2; i++) + { + if (types[i].range < blind_sqr || types[i - 1].range < blind_sqr || types[i + 1].range < blind_sqr) { continue; } + + if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8) { continue; } + + if (types[i].ftype == Nor) + { + if (types[i - 1].dista > types[i].dista) { ratio = types[i - 1].dista / types[i].dista; } + else { ratio = types[i].dista / types[i - 1].dista; } + + if (types[i].intersect < smallp_intersect && ratio < smallp_ratio) + { + if (types[i - 1].ftype == Nor) { types[i - 1].ftype = Real_Plane; } + if (types[i + 1].ftype == Nor) { types[i + 1].ftype = Real_Plane; } + types[i].ftype = Real_Plane; + } + } + } + + int last_surface = -1; + for (uint j = head; j < plsize; j++) + { + if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane) + { + if (last_surface == -1) { last_surface = j; } + + if (j == uint(last_surface + point_filter_num - 1)) + { + PointType ap; + ap.x = pl[j].x; + ap.y = pl[j].y; + ap.z = pl[j].z; + ap.curvature = pl[j].curvature; + pl_surf.push_back(ap); + + last_surface = -1; + } + } + else + { + if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane) { pl_corn.push_back(pl[j]); } + if (last_surface != -1) + { + PointType ap; + for (uint k = last_surface; k < j; k++) + { + ap.x += pl[k].x; + ap.y += pl[k].y; + ap.z += pl[k].z; + ap.curvature += pl[k].curvature; + } + ap.x /= (j - last_surface); + ap.y /= (j - last_surface); + ap.z /= (j - last_surface); + ap.curvature /= (j - last_surface); + pl_surf.push_back(ap); + } + last_surface = -1; + } + } +} + +void Preprocess::pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct) +{ + pl.height = 1; + pl.width = pl.size(); + sensor_msgs::msg::PointCloud2 output; + pcl::toROSMsg(pl, output); + output.header.frame_id = "livox"; + output.header.stamp = ct; +} + +int Preprocess::plane_judge(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct) +{ + double group_dis = disA * types[i_cur].range + disB; + group_dis = group_dis * group_dis; + // i_nex = i_cur; + + double two_dis; + vector disarr; + disarr.reserve(20); + + for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++) + { + if (types[i_nex].range < blind_sqr) + { + curr_direct.setZero(); + return 2; + } + disarr.push_back(types[i_nex].dista); + } + + for (;;) + { + if ((i_cur >= pl.size()) || (i_nex >= pl.size())) break; + + if (types[i_nex].range < blind_sqr) + { + curr_direct.setZero(); + return 2; + } + vx = pl[i_nex].x - pl[i_cur].x; + vy = pl[i_nex].y - pl[i_cur].y; + vz = pl[i_nex].z - pl[i_cur].z; + two_dis = vx * vx + vy * vy + vz * vz; + if (two_dis >= group_dis) { break; } + disarr.push_back(types[i_nex].dista); + i_nex++; + } + + double leng_wid = 0; + double v1[3], v2[3]; + for (uint j = i_cur + 1; j < i_nex; j++) + { + if ((j >= pl.size()) || (i_cur >= pl.size())) break; + v1[0] = pl[j].x - pl[i_cur].x; + v1[1] = pl[j].y - pl[i_cur].y; + v1[2] = pl[j].z - pl[i_cur].z; + + v2[0] = v1[1] * vz - vy * v1[2]; + v2[1] = v1[2] * vx - v1[0] * vz; + v2[2] = v1[0] * vy - vx * v1[1]; + + double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2]; + if (lw > leng_wid) { leng_wid = lw; } + } + + if ((two_dis * two_dis / leng_wid) < p2l_ratio) + { + curr_direct.setZero(); + return 0; + } + + uint disarrsize = disarr.size(); + for (uint j = 0; j < disarrsize - 1; j++) + { + for (uint k = j + 1; k < disarrsize; k++) + { + if (disarr[j] < disarr[k]) + { + leng_wid = disarr[j]; + disarr[j] = disarr[k]; + disarr[k] = leng_wid; + } + } + } + + if (disarr[disarr.size() - 2] < 1e-16) + { + curr_direct.setZero(); + return 0; + } + + if (lidar_type == AVIA) + { + double dismax_mid = disarr[0] / disarr[disarrsize / 2]; + double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2]; + + if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin) + { + curr_direct.setZero(); + return 0; + } + } + else + { + double dismax_min = disarr[0] / disarr[disarrsize - 2]; + if (dismax_min >= limit_maxmin) + { + curr_direct.setZero(); + return 0; + } + } + + curr_direct << vx, vy, vz; + curr_direct.normalize(); + return 1; +} + +bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir) +{ + if (nor_dir == 0) + { + if (types[i - 1].range < blind_sqr || types[i - 2].range < blind_sqr) { return false; } + } + else if (nor_dir == 1) + { + if (types[i + 1].range < blind_sqr || types[i + 2].range < blind_sqr) { return false; } + } + double d1 = types[i + nor_dir - 1].dista; + double d2 = types[i + 3 * nor_dir - 2].dista; + double d; + + if (d1 < d2) + { + d = d1; + d1 = d2; + d2 = d; + } + + d1 = sqrt(d1); + d2 = sqrt(d2); + + if (d1 > edgea * d2 || (d1 - d2) > edgeb) { return false; } + + return true; +} \ No newline at end of file diff --git a/src/FAST-LIVO2/src/utils.cpp b/src/FAST-LIVO2/src/utils.cpp new file mode 100644 index 0000000..1e453c8 --- /dev/null +++ b/src/FAST-LIVO2/src/utils.cpp @@ -0,0 +1,19 @@ +// utils.cpp +#include +#include // for int64_t +#include // for std::numeric_limits +#include // for std::out_of_range + +std::vector convertToIntVectorSafe(const std::vector& int64_vector) { + std::vector int_vector; + int_vector.reserve(int64_vector.size()); // 预留空间以提高效率 + + for (int64_t value : int64_vector) { + if (value < std::numeric_limits::min() || value > std::numeric_limits::max()) { + throw std::out_of_range("Value is out of range for int"); + } + int_vector.push_back(static_cast(value)); + } + + return int_vector; +} diff --git a/src/FAST-LIVO2/src/vio.cpp b/src/FAST-LIVO2/src/vio.cpp new file mode 100755 index 0000000..96b2f5f --- /dev/null +++ b/src/FAST-LIVO2/src/vio.cpp @@ -0,0 +1,2857 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. + +Multi-camera vectorization (N-camera joint ESIKF + Omni-LIVO's Cross-View +Temporal Migration / Adaptive Multi-View ESIKF) ported from Omni-LIVO — see +docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md Phase 4. Loop-closure hooks that +were declared but never implemented/called in Omni-LIVO itself +(resetAfterLoopClosure, applyTrajectoryTransformToVisualMap, +computeCrossCameraWarpMatrix, evaluateCrossCameraConsistency, +extractPatchSafely) were left out as dead code. +*/ + +#include "vio.h" +#include +#include + +using namespace Eigen; + +VIOManager::VIOManager() +{ +} + +VIOManager::~VIOManager() +{ + delete visual_submap; + for (auto &pair : warp_map) delete pair.second; + warp_map.clear(); + for (auto &pair : feat_map) delete pair.second; + feat_map.clear(); +} + +size_t VIOManager::capVisualMap() +{ + if (!map_sliding_en_) return 0; + if ((int)feat_map.size() <= max_visual_voxels_) return 0; + + // Drop oldest voxels first (smallest creation_timestamp_). + std::vector> aged; + aged.reserve(feat_map.size()); + for (const auto &kv : feat_map) { aged.emplace_back(kv.second->creation_timestamp_, kv.first); } + const size_t to_drop = feat_map.size() - (size_t)max_visual_voxels_; + std::nth_element(aged.begin(), aged.begin() + to_drop, aged.end(), [](const auto &a, const auto &b) { return a.first < b.first; }); + + size_t erased = 0; + for (size_t i = 0; i < to_drop; ++i) + { + auto it = feat_map.find(aged[i].second); + if (it != feat_map.end()) + { + delete it->second; // ~VOXEL_POINTS frees VisualPoints + Features + feat_map.erase(it); + sub_feat_map.erase(aged[i].second); + ++erased; + } + } + std::cout << "[ VIO ] Cap visual map: erased " << erased << " oldest voxels, remaining " << feat_map.size() << std::endl; + return erased; +} + +void VIOManager::setImuToLidarExtrinsic(const V3D &transl, const M3D &rot) +{ + Pli = -rot.transpose() * transl; + Rli = rot.transpose(); +} + +void VIOManager::setLidarToCameraExtrinsic(std::vector> &R, std::vector> &P) +{ + Rcl_vec.resize(cams.size()); + Pcl_vec.resize(cams.size()); + for (size_t i = 0; i < cams.size(); i++) + { + Rcl_vec[i] << R[i][0], R[i][1], R[i][2], + R[i][3], R[i][4], R[i][5], + R[i][6], R[i][7], R[i][8]; + Pcl_vec[i] << P[i][0], P[i][1], P[i][2]; + } +} + +void VIOManager::initializeVIO() +{ + if (cams.empty()) + { + std::cerr << "[VIOManager::initializeVIO] Error: no cameras!" << std::endl; + return; + } + // NOTE (inherited from Omni-LIVO): width/height are derived only from + // cams[0] and reused for every camera's image-buffer indexing throughout + // this file (getImagePatch, precomputeReferencePatches, updateState, ...). + // All cameras in a given deployment must therefore share the same + // processed resolution; a config with mismatched per-camera resolutions + // will silently misbehave. See porting plan risk list. + width = cams[0]->width(); + height = cams[0]->height(); + + visual_submap = new SubSparseMap; + image_resize_factor = cams[0]->scale(); + + Rci_vec.resize(cams.size()); + Rcl_vec.resize(cams.size()); + Rcw_vec.resize(cams.size()); + Pci_vec.resize(cams.size()); + Pcl_vec.resize(cams.size()); + Pcw_vec.resize(cams.size()); + + Jdphi_dR_vec.resize(cams.size()); + Jdp_dt_vec.resize(cams.size()); + Jdp_dR_vec.resize(cams.size()); + + prev_cov_scale_per_cam_.resize(cams.size(), 10.0); + prev_avg_error_per_cam_.resize(cams.size(), 5.0); + prev_n_meas_per_cam_.resize(cams.size(), 0); + + if (state && state->inv_expo_time_per_cam.empty()) + { + state->inv_expo_time_per_cam.resize(cams.size(), 1.0); + std::cout << "[VIO] Initialized " << cams.size() << " camera exposure parameters (inv_expo=1.0)" << std::endl; + } + if (state_propagat && state_propagat->inv_expo_time_per_cam.empty()) + { + state_propagat->inv_expo_time_per_cam.resize(cams.size(), 1.0); + } + + for (size_t i = 0; i < cams.size(); i++) + { + Rci_vec[i] = Rcl_vec[i] * Rli; + Pci_vec[i] = Rcl_vec[i] * Pli + Pcl_vec[i]; + Jdphi_dR_vec[i] = Rci_vec[i]; + Eigen::Vector3d Pic_i = -Rci_vec[i].transpose() * Pci_vec[i]; + Eigen::Matrix3d tmp; + tmp << 0, -Pic_i.z(), Pic_i.y(), + Pic_i.z(), 0, -Pic_i.x(), + -Pic_i.y(), Pic_i.x(), 0; + Jdp_dR_vec[i] = -Rci_vec[i] * tmp; + } + + if (grid_size > 10) + { + grid_n_width = ceil(static_cast(width / grid_size)); + grid_n_height = ceil(static_cast(height / grid_size)); + } + else + { + grid_size = static_cast(height / grid_n_height); + grid_n_height = ceil(static_cast(height / grid_size)); + grid_n_width = ceil(static_cast(width / grid_size)); + } + length = grid_n_width * grid_n_height; + + if (colmap_output_en) + { + fout_colmap.open(DEBUG_FILE_DIR("Colmap/sparse/0/images.txt"), ios::out); + fout_colmap << "# Image list with two lines of data per image:\n"; + fout_colmap << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n"; + fout_colmap << "# POINTS2D[] as (X, Y, POINT3D_ID)\n"; + fout_camera.open(DEBUG_FILE_DIR("Colmap/sparse/0/cameras.txt"), ios::out); + fout_camera << "# Camera list with one line of data per camera:\n"; + fout_camera << "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n"; + for (size_t i = 0; i < cams.size(); i++) + { + fout_camera << i + 1 << " PINHOLE " << cams[i]->width() << " " << cams[i]->height() << " " << std::fixed << std::setprecision(9) << cams[i]->fx() << " " + << cams[i]->fy() << " " << cams[i]->cx() << " " << cams[i]->cy() << std::endl; + } + fout_camera.close(); + } + + // Placeholder sizing only — resetGrid() (called at the top of every + // processFrame()) resizes these to the actual multi-camera cell count + // (length * cams.size()) before they're used. + size_t total_cells = (size_t)length * std::max(1, cams.size()); + grid_num.resize(total_cells); + map_index.resize(total_cells); + map_dist.resize(total_cells); + update_flag.resize(total_cells); + scan_value.resize(total_cells); + + patch_size_total = patch_size * patch_size; + patch_size_half = static_cast(patch_size / 2); + patch_buffer.resize(patch_size_total); + warp_len = patch_size_total * patch_pyrimid_level; + border = (patch_size_half + 2) * 8; + + retrieve_voxel_points.reserve(total_cells); + append_voxel_points.reserve(total_cells); + + sub_feat_map.clear(); + + initializeCameraPhotoParams(); + total_cross_camera_observations = 0; + successful_cross_camera_tracks = 0; + + std::cout << "Cross-camera direct tracking " << (enable_cross_camera_tracking ? "enabled!" : "disabled!") << std::endl; + if (enable_dynamic_covariance_) + { + std::cout << "Dynamic covariance enabled! (warmup=" << dynamic_cov_warmup_frames << " frames, min=" << min_cov_scale << ", max=" << max_cov_scale + << ")" << std::endl; + } + else + { + std::cout << "Dynamic covariance disabled!" << std::endl; + } +} + +void VIOManager::initializeRaycast() +{ + std::cout << "[ VIO ] Starting raycast initialization..." << std::endl; + raycast_en = true; + int num_cameras = cams.size(); + if (num_cameras == 0) + { + std::cerr << "[ VIO ] No cameras available for raycast initialization!" << std::endl; + raycast_en = false; + return; + } + if (grid_size <= 0 || grid_n_height <= 0 || grid_n_width <= 0 || length <= 0) + { + std::cerr << "[ VIO ] Grid parameters not properly initialized: grid_size=" << grid_size << ", grid_n_height=" << grid_n_height + << ", grid_n_width=" << grid_n_width << ", length=" << length << std::endl; + raycast_en = false; + return; + } + + border_flag.clear(); + rays_with_sample_points.clear(); + border_flag.resize(num_cameras); + rays_with_sample_points.resize(num_cameras); + + std::cout << "[ VIO ] Initializing raycast for " << num_cameras << " cameras: grid_size=" << grid_size << ", grid_n_height=" << grid_n_height + << ", grid_n_width=" << grid_n_width << ", length=" << length << std::endl; + + float d_min = 0.1; + float d_max = 3.0; + float step = 0.2; + int total_sample_points = 0; + for (int cam_idx = 0; cam_idx < num_cameras; cam_idx++) + { + border_flag[cam_idx].resize(length, 0); + rays_with_sample_points[cam_idx].clear(); + rays_with_sample_points[cam_idx].reserve(length); + + int camera_sample_points = 0; + for (int grid_row = 1; grid_row <= grid_n_height; grid_row++) + { + for (int grid_col = 1; grid_col <= grid_n_width; grid_col++) + { + std::vector SamplePointsEachGrid; + int index = (grid_row - 1) * grid_n_width + grid_col - 1; + + if (grid_row == 1 || grid_col == 1 || grid_row == grid_n_height || grid_col == grid_n_width) border_flag[cam_idx][index] = 1; + + int u = grid_size / 2 + (grid_col - 1) * grid_size; + int v = grid_size / 2 + (grid_row - 1) * grid_size; + + for (float d_temp = d_min; d_temp <= d_max; d_temp += step) + { + V3D xyz; + try + { + xyz = cams[cam_idx]->cam2world(u, v); + xyz *= d_temp / xyz[2]; + SamplePointsEachGrid.push_back(xyz); + camera_sample_points++; + } + catch (const std::exception &e) + { + std::cerr << "[ VIO ] Exception in camera " << cam_idx << " raycast computation: " << e.what() << std::endl; + } + } + rays_with_sample_points[cam_idx].push_back(SamplePointsEachGrid); + } + } + total_sample_points += camera_sample_points; + } + + std::cout << "[ VIO ] Raycast initialization completed! Total sample points: " << total_sample_points << std::endl; +} + +void VIOManager::resetGrid() +{ + // Grid buffers are indexed per-camera-cell (camera_grid_offset[cam] + + // local_index in retrieveFromVisualSparseMap/generateVisualMapPoints), so + // they must be sized to all cameras' cells combined, not just one + // camera's `length`, or a second+ camera's writes go out of bounds. + size_t total_cells = (size_t)length * std::max(1, cams.size()); + + grid_num.assign(total_cells, TYPE_UNKNOWN); + map_index.assign(total_cells, 0); + map_dist.assign(total_cells, 10000.0f); + update_flag.assign(total_cells, 0); + scan_value.assign(total_cells, 0.0f); + + retrieve_voxel_points.clear(); + retrieve_voxel_points.resize(total_cells); + + append_voxel_points.clear(); + append_voxel_points.resize(total_cells); + + total_points = 0; +} + +void VIOManager::computeProjectionJacobian(int cam_idx, V3D p, MD(2, 3) & J) +{ + double fx_i = cams[cam_idx]->fx(); + double fy_i = cams[cam_idx]->fy(); + const double x = p[0]; + const double y = p[1]; + const double z_inv = 1.0 / p[2]; + const double z_inv_2 = z_inv * z_inv; + + J(0, 0) = fx_i * z_inv; + J(0, 1) = 0.0; + J(0, 2) = -fx_i * x * z_inv_2; + + J(1, 0) = 0.0; + J(1, 1) = fy_i * z_inv; + J(1, 2) = -fy_i * y * z_inv_2; +} + +void VIOManager::getImagePatch(cv::Mat img, V2D pc, float *patch_tmp, int level) +{ + const float u_ref = pc[0]; + const float v_ref = pc[1]; + const int scale = (1 << level); + const int u_ref_i = floorf(pc[0] / scale) * scale; + const int v_ref_i = floorf(pc[1] / scale) * scale; + const float subpix_u_ref = (u_ref - u_ref_i) / scale; + const float subpix_v_ref = (v_ref - v_ref_i) / scale; + const float w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref); + const float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref); + const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; + const float w_ref_br = subpix_u_ref * subpix_v_ref; + for (int x = 0; x < patch_size; x++) + { + uint8_t *img_ptr = (uint8_t *)img.data + (v_ref_i - patch_size_half * scale + x * scale) * width + (u_ref_i - patch_size_half * scale); + for (int y = 0; y < patch_size; y++, img_ptr += scale) + { + patch_tmp[patch_size_total * level + x * patch_size + y] = + w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + w_ref_bl * img_ptr[scale * width] + w_ref_br * img_ptr[scale * width + scale]; + } + } +} + +void VIOManager::insertPointIntoVoxelMap(VisualPoint *pt_new) +{ + V3D pt_w(pt_new->pos_[0], pt_new->pos_[1], pt_new->pos_[2]); + double voxel_size = 0.5; + float loc_xyz[3]; + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = pt_w[j] / voxel_size; + if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + } + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]); + auto iter = feat_map.find(position); + if (iter != feat_map.end()) + { + iter->second->voxel_points.push_back(pt_new); + iter->second->count++; + } + else + { + VOXEL_POINTS *ot = new VOXEL_POINTS(0); + ot->creation_timestamp_ = current_timestamp_; + ot->voxel_points.push_back(pt_new); + feat_map[position] = ot; + } +} + +void VIOManager::getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, const V2D &px_ref, const V3D &xyz_ref, const V3D &normal_ref, + const SE3 &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref) +{ + const V3D t = T_cur_ref.inverse().translation(); + const Eigen::Matrix3d H_cur_ref = T_cur_ref.rotationMatrix() * (normal_ref.dot(xyz_ref) * Eigen::Matrix3d::Identity() - t * normal_ref.transpose()); + const int kHalfPatchSize = 4; + V3D f_du_ref(cam.cam2world(px_ref + Eigen::Vector2d(kHalfPatchSize, 0) * (1 << level_ref))); + V3D f_dv_ref(cam.cam2world(px_ref + Eigen::Vector2d(0, kHalfPatchSize) * (1 << level_ref))); + const V3D f_cur(H_cur_ref * xyz_ref); + const V3D f_du_cur = H_cur_ref * f_du_ref; + const V3D f_dv_cur = H_cur_ref * f_dv_ref; + V2D px_cur(cam.world2cam(f_cur)); + V2D px_du_cur(cam.world2cam(f_du_cur)); + V2D px_dv_cur(cam.world2cam(f_dv_cur)); + A_cur_ref.col(0) = (px_du_cur - px_cur) / kHalfPatchSize; + A_cur_ref.col(1) = (px_dv_cur - px_cur) / kHalfPatchSize; +} + +void VIOManager::getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, + const SE3 &T_cur_ref, const int level_ref, const int pyramid_level, const int halfpatch_size, + Matrix2d &A_cur_ref) +{ + const Vector3d xyz_ref(f_ref * depth_ref); + Vector3d xyz_du_ref(cam.cam2world(px_ref + Vector2d(halfpatch_size, 0) * (1 << level_ref) * (1 << pyramid_level))); + Vector3d xyz_dv_ref(cam.cam2world(px_ref + Vector2d(0, halfpatch_size) * (1 << level_ref) * (1 << pyramid_level))); + xyz_du_ref *= xyz_ref[2] / xyz_du_ref[2]; + xyz_dv_ref *= xyz_ref[2] / xyz_dv_ref[2]; + const Vector2d px_cur(cam.world2cam(T_cur_ref * (xyz_ref))); + const Vector2d px_du(cam.world2cam(T_cur_ref * (xyz_du_ref))); + const Vector2d px_dv(cam.world2cam(T_cur_ref * (xyz_dv_ref))); + A_cur_ref.col(0) = (px_du - px_cur) / halfpatch_size; + A_cur_ref.col(1) = (px_dv - px_cur) / halfpatch_size; +} + +void VIOManager::warpAffine(const Matrix2d &A_cur_ref, const cv::Mat &img_ref, const Vector2d &px_ref, const int level_ref, const int search_level, + const int pyramid_level, const int halfpatch_size, float *patch) +{ + const int patch_size = halfpatch_size * 2; + const Matrix2f A_ref_cur = A_cur_ref.inverse().cast(); + if (isnan(A_ref_cur(0, 0))) + { + printf("Affine warp is NaN, probably camera has no translation\n"); + return; + } + + float *patch_ptr = patch; + for (int y = 0; y < patch_size; ++y) + { + for (int x = 0; x < patch_size; ++x) + { + Vector2f px_patch(x - halfpatch_size, y - halfpatch_size); + px_patch *= (1 << search_level); + px_patch *= (1 << pyramid_level); + const Vector2f px(A_ref_cur * px_patch + px_ref.cast()); + if (px[0] < 0 || px[1] < 0 || px[0] >= img_ref.cols - 1 || px[1] >= img_ref.rows - 1) + patch_ptr[patch_size_total * pyramid_level + y * patch_size + x] = 0; + else + patch_ptr[patch_size_total * pyramid_level + y * patch_size + x] = (float)vk::interpolateMat_8u(img_ref, px[0], px[1]); + } + } +} + +int VIOManager::getBestSearchLevel(const Matrix2d &A_cur_ref, const int max_level) +{ + int search_level = 0; + double D = A_cur_ref.determinant(); + while (D > 3.0 && search_level < max_level) + { + search_level += 1; + D *= 0.25; + } + return search_level; +} + +double VIOManager::calculateNCC(float *ref_patch, float *cur_patch, int patch_size) +{ + double sum_ref = std::accumulate(ref_patch, ref_patch + patch_size, 0.0); + double mean_ref = sum_ref / patch_size; + + double sum_cur = std::accumulate(cur_patch, cur_patch + patch_size, 0.0); + double mean_curr = sum_cur / patch_size; + + double numerator = 0, demoniator1 = 0, demoniator2 = 0; + for (int i = 0; i < patch_size; i++) + { + double n = (ref_patch[i] - mean_ref) * (cur_patch[i] - mean_curr); + numerator += n; + demoniator1 += (ref_patch[i] - mean_ref) * (ref_patch[i] - mean_ref); + demoniator2 += (cur_patch[i] - mean_curr) * (cur_patch[i] - mean_curr); + } + return numerator / sqrt(demoniator1 * demoniator2 + 1e-10); +} + +// True if `pt`'s reference patch was extracted from a *different* camera than +// `current_cam_id` and that other camera has previously handed off +// observations to the current one (i.e. this point is being tracked across +// the boundary between two non-overlapping camera FoVs). +bool VIOManager::isRealCrossCameraPoint(VisualPoint *pt, int current_cam_id) +{ + if (!pt || !pt->ref_patch) return false; + + CrossCameraData &data = pt->cross_cam_data_; + int ref_cam_id = pt->ref_patch->cam_id_; + int cur_cam_id = current_cam_id; + + if (ref_cam_id == cur_cam_id) return false; + + if (data.has_migration_history && data.migration_source_cam == ref_cam_id) { return true; } + + for (auto obs : pt->obs_) + { + if (obs->cam_id_ == ref_cam_id && obs->cam_id_ != cur_cam_id) { return true; } + } + + return false; +} + +void VIOManager::updateCrossCameraHistory(VisualPoint *pt, int cam_id) +{ + if (!pt) return; + + CrossCameraData &data = pt->cross_cam_data_; + if (data.primary_cam_idx != -1 && data.primary_cam_idx != cam_id) + { + if (!data.currently_visible.test(data.primary_cam_idx) && data.currently_visible.test(cam_id)) { data.cross_camera_migrations++; } + } + data.primary_cam_idx = cam_id; +} + +void VIOManager::retrieveFromVisualSparseMap(const std::vector imgs, vector &pg, + const unordered_map &plane_map) +{ + if (feat_map.empty()) + { + std::cout << "[VIO] feat_map is empty, skipping visual point retrieval" << std::endl; + return; + } + if (camera_photo_params.empty()) { initializeCameraPhotoParams(); } + + double ts0 = omp_get_wtime(); + visual_submap->reset(); + sub_feat_map.clear(); + + float voxel_size = 0.5f; + if (!normal_en) warp_map.clear(); + + if (depth_imgs_buffer_.size() != cams.size()) + { + depth_imgs_buffer_.resize(cams.size()); + for (size_t i = 0; i < cams.size(); i++) { depth_imgs_buffer_[i] = cv::Mat::zeros(height, width, CV_32FC1); } + } + else + { + for (auto &depth_img : depth_imgs_buffer_) { depth_img.setTo(0); } + } + std::vector &depth_imgs = depth_imgs_buffer_; + + grid_n_width = ceil(static_cast(width / grid_size)); + grid_n_height = ceil(static_cast(height / grid_size)); + int cells_per_camera = grid_n_width * grid_n_height; + int num_cameras = (int)cams.size(); + int total_cells = cells_per_camera * num_cameras; + + std::vector camera_grid_offset(num_cameras); + for (int i = 0; i < num_cameras; i++) { camera_grid_offset[i] = i * cells_per_camera; } + + grid_num.assign(total_cells, TYPE_UNKNOWN); + map_dist.assign(total_cells, std::numeric_limits::max()); + retrieve_voxel_points.assign(total_cells, nullptr); + scan_value.assign(total_cells, 0.0f); + + if ((int)retrieve_voxel_points_list_buffer_.size() != total_cells) { retrieve_voxel_points_list_buffer_.resize(total_cells); } + for (auto &set : retrieve_voxel_points_list_buffer_) { set.clear(); } + std::vector> &retrieve_voxel_points_list = retrieve_voxel_points_list_buffer_; + + // Splat each LiDAR point's depth into every camera's depth buffer (used + // below for depth-continuity filtering of candidate patches). + for (int i_point = 0; i_point < (int)pg.size(); i_point++) + { + V3D pt_w = pg[i_point].point_w; + int loc_xyz[3]; + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = (int)std::floor(pt_w[j] / voxel_size); + if (loc_xyz[j] < 0) loc_xyz[j] -= 1; + } + VOXEL_LOCATION position(loc_xyz[0], loc_xyz[1], loc_xyz[2]); + sub_feat_map[position] = 0; + +#ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); +#pragma omp parallel for +#endif + for (int cam_idx = 0; cam_idx < (int)cams.size(); cam_idx++) + { + V3D pt_c = new_frame_->w2f(pt_w, cam_idx); + if (pt_c[2] > 0) + { + V2D px = new_frame_->w2c(pt_w, cam_idx); + if (cams[cam_idx]->isInFrame(px.cast(), border)) + { + float depth = (float)pt_c[2]; + int col = (int)px[0]; + int row = (int)px[1]; + if (row >= 0 && row < height && col >= 0 && col < width) + { + float *it = (float *)depth_imgs[cam_idx].data; + it[row * width + col] = depth; + } + } + } + } + } + + // Project every voxel's existing points into every camera to find which + // grid cells have candidate map points visible this frame. + std::vector DeleteKeyList; + for (auto &iter_sf : sub_feat_map) + { + VOXEL_LOCATION position = iter_sf.first; + auto corre_voxel = feat_map.find(position); + if (corre_voxel != feat_map.end()) + { + bool voxel_in_fov = false; + std::vector &voxel_points = corre_voxel->second->voxel_points; + + for (VisualPoint *pt : voxel_points) + { + if (!pt || pt->obs_.empty()) continue; + if (!pt->is_normal_initialized_) continue; + + if (enable_cross_camera_tracking) + { + pt->cross_cam_data_.previously_visible = pt->cross_cam_data_.currently_visible; + pt->cross_cam_data_.currently_visible.reset(); + } + + bool this_pt_in_fov = false; + for (int cam_idx = 0; cam_idx < (int)cams.size(); cam_idx++) + { + V3D dir_c = new_frame_->w2f(pt->pos_, cam_idx); + if (dir_c[2] < 0) continue; + + V2D pc = new_frame_->w2c(pt->pos_, cam_idx); + if (!cams[cam_idx]->isInFrame(pc.cast(), border)) continue; + + this_pt_in_fov = true; + + if (enable_cross_camera_tracking) { pt->cross_cam_data_.currently_visible.set(cam_idx); } + + int grid_col = (int)(pc[0] / grid_size); + int grid_row = (int)(pc[1] / grid_size); + int local_index = grid_row * grid_n_width + grid_col; + int index = camera_grid_offset[cam_idx] + local_index; + + if (local_index < 0 || local_index >= cells_per_camera) continue; + + grid_num[index] = TYPE_MAP; + retrieve_voxel_points_list[index].insert(pt); + + V3D obs_vec = new_frame_->pos(cam_idx) - pt->pos_; + float cur_dist = (float)obs_vec.norm(); + if (cur_dist <= map_dist[index]) + { + map_dist[index] = cur_dist; + retrieve_voxel_points[index] = pt; + } + } + + if (this_pt_in_fov) voxel_in_fov = true; + } + + if (!voxel_in_fov) DeleteKeyList.push_back(position); + } + } + for (auto &key : DeleteKeyList) { sub_feat_map.erase(key); } + + // For every occupied grid cell, pick the best reference patch + warp for + // each candidate point and score it by photometric error, in parallel. + struct CandidatePoint + { + VisualPoint *pt; + float error; + int search_level; + Matrix2d A_cur_ref_zero; + std::vector patch_wrap; + Feature *ref_ftr; + V2D pc; + }; + + int num_threads = 1; +#ifdef MP_EN + num_threads = omp_get_max_threads(); +#endif + std::vector>> thread_camera_processed_points(num_threads); + std::vector>> thread_camera_candidates(num_threads); + std::vector> thread_patch_wrap_buffer(num_threads); + std::vector> thread_patch_buffer_buffer(num_threads); + + for (int t = 0; t < num_threads; t++) + { + thread_camera_processed_points[t].resize(num_cameras); + thread_camera_candidates[t].resize(num_cameras); + thread_patch_wrap_buffer[t].resize(warp_len); + thread_patch_buffer_buffer[t].resize(patch_size_total); + } + +#ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); +#pragma omp parallel for schedule(dynamic, 16) +#endif + for (int i = 0; i < total_cells; i++) + { + int thread_id = 0; +#ifdef MP_EN + thread_id = omp_get_thread_num(); +#endif + std::vector> &camera_candidates = thread_camera_candidates[thread_id]; + std::vector> &camera_processed_points = thread_camera_processed_points[thread_id]; + std::vector &patch_wrap = thread_patch_wrap_buffer[thread_id]; + std::vector &patch_buffer_local = thread_patch_buffer_buffer[thread_id]; + + if (grid_num[i] != TYPE_MAP) continue; + + int cam_idx = i / cells_per_camera; + + for (auto pt : retrieve_voxel_points_list[i]) + { + if (!pt) continue; + if (camera_processed_points[cam_idx].find(pt) != camera_processed_points[cam_idx].end()) continue; + camera_processed_points[cam_idx].insert(pt); + + if (!pt->is_normal_initialized_) continue; + + V2D pc = new_frame_->w2c(pt->pos_, cam_idx); + if (!cams[cam_idx]->isInFrame(pc.cast(), border)) continue; + + V3D pt_cam = new_frame_->w2f(pt->pos_, cam_idx); + bool depth_continous = false; + float *depth_ptr = reinterpret_cast(depth_imgs[cam_idx].data); + + int base_col = int(pc[0]) - patch_size_half; + int base_row = int(pc[1]) - patch_size_half; + int center_offset = patch_size_half; + float pt_depth = static_cast(pt_cam[2]); + + const int sample_offsets[][2] = {{0, 0}, {patch_size, 0}, {0, patch_size}, {patch_size, patch_size}, + {center_offset, 0}, {patch_size, center_offset}, {0, center_offset}, {center_offset, patch_size}}; + + for (int s = 0; s < 8; s++) + { + int col = base_col + sample_offsets[s][0]; + int row = base_row + sample_offsets[s][1]; + if (row < 0 || row >= height || col < 0 || col >= width) continue; + if (s == 0) continue; + + float depth = depth_ptr[row * width + col]; + if (depth == 0.) continue; + if (abs(pt_depth - depth) > 0.5) + { + depth_continous = true; + break; + } + } + if (depth_continous) continue; + + Feature *ref_ftr = nullptr; + if (normal_en) + { + if (pt->obs_.size() == 1) + { + ref_ftr = *pt->obs_.begin(); + pt->ref_patch = ref_ftr; + pt->has_ref_patch_ = true; + } + else if (pt->has_ref_patch_ && pt->ref_patch) + { + if ((int)cams.size() == 1) { ref_ftr = pt->ref_patch; } + else + { + bool found_current_cam = false; + for (auto it = pt->obs_.begin(); it != pt->obs_.end(); ++it) + { + if ((*it)->cam_id_ == cam_idx) + { + ref_ftr = *it; + found_current_cam = true; + break; + } + } + if (!found_current_cam) { ref_ftr = pt->ref_patch; } + else { pt->ref_patch = ref_ftr; } + } + } + else + { + // No reference patch chosen yet: prefer an observation from the + // current camera; otherwise pick whichever observation looks most + // photometrically consistent with a couple of the others. + for (auto it = pt->obs_.begin(); it != pt->obs_.end(); ++it) + { + if ((*it)->cam_id_ == cam_idx) + { + ref_ftr = *it; + break; + } + } + + if (!ref_ftr && pt->obs_.size() > 0) + { + if (pt->obs_.size() <= 3) { ref_ftr = *pt->obs_.begin(); } + else + { + float photometric_errors_min = std::numeric_limits::max(); + int compare_count = 0; + const int max_compare = 2; + + for (auto it = pt->obs_.begin(); it != pt->obs_.end() && compare_count < max_compare + 1; ++it, ++compare_count) + { + Feature *ref_patch_temp = *it; + float *patch_temp = ref_patch_temp->patch_; + float photometric_errors = 0.0f; + int sample_count = 0; + + auto itm = pt->obs_.begin(); + for (int k = 0; k < max_compare && itm != pt->obs_.end(); ++itm, ++k) + { + if ((*itm)->id_ == ref_patch_temp->id_) continue; + float *patch_cache = (*itm)->patch_; + Eigen::Map patch_a(patch_temp, patch_size_total); + Eigen::Map patch_b(patch_cache, patch_size_total); + photometric_errors += (patch_a - patch_b).squaredNorm(); + sample_count++; + } + if (sample_count > 0) photometric_errors /= (float)sample_count; + + if (photometric_errors < photometric_errors_min) + { + photometric_errors_min = photometric_errors; + ref_ftr = ref_patch_temp; + } + } + } + } + + pt->ref_patch = ref_ftr; + pt->has_ref_patch_ = (ref_ftr != nullptr); + } + } + else + { + if (!pt->getCloseViewObs(new_frame_->pos(cam_idx), ref_ftr, pc)) continue; + } + if (!ref_ftr) continue; + + int search_level; + Matrix2d A_cur_ref_zero; + if (normal_en) + { + V3D norm_vec = (ref_ftr->T_f_w_.rotationMatrix() * pt->normal_).normalized(); + V3D pf = ref_ftr->T_f_w_ * pt->pos_; + SE3 T_cur_ref = new_frame_->T_f_w_[cam_idx] * ref_ftr->T_f_w_.inverse(); + getWarpMatrixAffineHomography(*cams[cam_idx], ref_ftr->px_, pf, norm_vec, T_cur_ref, 0, A_cur_ref_zero); + search_level = getBestSearchLevel(A_cur_ref_zero, 2); + } + else + { + auto iter_warp = warp_map.find(ref_ftr->id_); + if (iter_warp != warp_map.end()) + { + search_level = iter_warp->second->search_level; + A_cur_ref_zero = iter_warp->second->A_cur_ref; + } + else + { + getWarpMatrixAffine(*cams[cam_idx], ref_ftr->px_, ref_ftr->f_, (ref_ftr->pos() - pt->pos_).norm(), + new_frame_->T_f_w_[cam_idx] * ref_ftr->T_f_w_.inverse(), ref_ftr->level_, 0, patch_size_half, A_cur_ref_zero); + search_level = getBestSearchLevel(A_cur_ref_zero, 2); + Warp *ot = new Warp(search_level, A_cur_ref_zero); + warp_map[ref_ftr->id_] = ot; + } + } + + for (int pyramid_level = 0; pyramid_level <= patch_pyrimid_level - 1; pyramid_level++) + { + warpAffine(A_cur_ref_zero, ref_ftr->img_, ref_ftr->px_, ref_ftr->level_, search_level, pyramid_level, patch_size_half, patch_wrap.data()); + } + getImagePatch(imgs[cam_idx], pc, patch_buffer_local.data(), 0); + + float error = 0.0; + if (exposure_estimate_en) + { + double cur_inv_expo = (cam_idx < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[cam_idx] : 1.0; + for (int ind = 0; ind < patch_size_total; ind++) + { + float diff = ref_ftr->inv_expo_time_ * patch_wrap[ind] - cur_inv_expo * patch_buffer_local[ind]; + error += diff * diff; + } + } + else + { + for (int ind = 0; ind < patch_size_total; ind++) + { + float diff = patch_wrap[ind] - patch_buffer_local[ind]; + error += diff * diff; + } + } + + if (ncc_en) + { + double ncc = calculateNCC(patch_wrap.data(), patch_buffer_local.data(), patch_size_total); + if (ncc < ncc_thre) continue; + } + + float error_threshold = outlier_threshold * patch_size_total; + if (error > error_threshold) continue; + + CandidatePoint candidate; + candidate.pt = pt; + candidate.error = error; + candidate.search_level = search_level; + candidate.A_cur_ref_zero = A_cur_ref_zero; + candidate.patch_wrap = patch_wrap; + candidate.ref_ftr = ref_ftr; + candidate.pc = pc; + camera_candidates[cam_idx].push_back(candidate); + + CrossCameraData &cc_data = pt->cross_cam_data_; + cc_data.currently_visible.set(cam_idx); + if (cc_data.primary_cam_idx != -1 && cc_data.primary_cam_idx != cam_idx) + { + if (!cc_data.has_migration_history || cc_data.migration_source_cam != cc_data.primary_cam_idx) + { + cc_data.migration_source_cam = cc_data.primary_cam_idx; + cc_data.has_migration_history = true; + cc_data.cross_camera_migrations++; + } + } + cc_data.previous_cam_idx = cam_idx; + cc_data.primary_cam_idx = cam_idx; + } + } + + // Merge per-thread candidates, then keep the best `points_per_camera_min` + // per camera unconditionally, and fill remaining slots (up to + // `max_total_points` total) with the globally-best leftover candidates. + std::vector> per_camera_candidates(num_cameras); + for (int t = 0; t < num_threads; t++) + { + for (int cam_idx = 0; cam_idx < num_cameras; cam_idx++) + { + per_camera_candidates[cam_idx].insert(per_camera_candidates[cam_idx].end(), thread_camera_candidates[t][cam_idx].begin(), + thread_camera_candidates[t][cam_idx].end()); + } + } + + for (int cam_idx = 0; cam_idx < num_cameras; cam_idx++) + { + size_t n_candidates = per_camera_candidates[cam_idx].size(); + if (n_candidates == 0) continue; + size_t n_to_sort = std::min(n_candidates, (size_t)points_per_camera_max); + std::partial_sort(per_camera_candidates[cam_idx].begin(), per_camera_candidates[cam_idx].begin() + n_to_sort, per_camera_candidates[cam_idx].end(), + [](const CandidatePoint &a, const CandidatePoint &b) { return a.error < b.error; }); + } + + std::vector points_per_camera(num_cameras, 0); + for (int cam_idx = 0; cam_idx < num_cameras; cam_idx++) + { + int n_to_keep = std::min((int)per_camera_candidates[cam_idx].size(), points_per_camera_min); + for (int i = 0; i < n_to_keep; i++) + { + const auto &cand = per_camera_candidates[cam_idx][i]; + visual_submap->voxel_points.push_back(cand.pt); + visual_submap->propa_errors.push_back(cand.error); + visual_submap->search_levels.push_back(cand.search_level); + visual_submap->errors.push_back(cand.error); + visual_submap->warp_patch.push_back(cand.patch_wrap); + visual_submap->inv_expo_list.push_back(cand.ref_ftr->inv_expo_time_); + visual_submap->camera_ids.push_back(cam_idx); + points_per_camera[cam_idx]++; + } + per_camera_candidates[cam_idx].erase(per_camera_candidates[cam_idx].begin(), per_camera_candidates[cam_idx].begin() + n_to_keep); + } + + std::vector> remaining_candidates; + remaining_candidates.reserve(max_total_points); + for (int cam_idx = 0; cam_idx < num_cameras; cam_idx++) + { + int max_remaining = points_per_camera_max - points_per_camera[cam_idx]; + int n_remaining = std::min((int)per_camera_candidates[cam_idx].size(), max_remaining); + for (int i = 0; i < n_remaining; i++) { remaining_candidates.push_back({per_camera_candidates[cam_idx][i], cam_idx}); } + } + + std::sort(remaining_candidates.begin(), remaining_candidates.end(), [](const auto &a, const auto &b) { return a.first.error < b.first.error; }); + + int current_total = visual_submap->voxel_points.size(); + int remaining_quota = max_total_points - current_total; + int n_add = std::min((int)remaining_candidates.size(), remaining_quota); + + for (int i = 0; i < n_add; i++) + { + const auto &cand = remaining_candidates[i].first; + int cam_idx = remaining_candidates[i].second; + visual_submap->voxel_points.push_back(cand.pt); + visual_submap->propa_errors.push_back(cand.error); + visual_submap->search_levels.push_back(cand.search_level); + visual_submap->errors.push_back(cand.error); + visual_submap->warp_patch.push_back(cand.patch_wrap); + visual_submap->inv_expo_list.push_back(cand.ref_ftr->inv_expo_time_); + visual_submap->camera_ids.push_back(cam_idx); + points_per_camera[cam_idx]++; + } + + total_points = (int)visual_submap->voxel_points.size(); + + if (raycast_en) + { + float voxel_size_rc = 0.5f; + int loc_xyz[3]; + for (int cam_idx = 0; cam_idx < num_cameras; cam_idx++) + { + int cam_offset = camera_grid_offset[cam_idx]; + for (int local_i = 0; local_i < cells_per_camera; local_i++) + { + int i = cam_offset + local_i; + if (grid_num[i] == TYPE_MAP) continue; + + bool is_border = false; + if (cam_idx < (int)border_flag.size() && local_i < (int)border_flag[cam_idx].size() && border_flag[cam_idx][local_i] == 1) { is_border = true; } + if (is_border) continue; + + bool found_feature = false; + if (cam_idx >= (int)rays_with_sample_points.size()) continue; + if (local_i >= (int)rays_with_sample_points[cam_idx].size()) continue; + + for (const auto &sample_point_cam : rays_with_sample_points[cam_idx][local_i]) + { + V3D sample_point_w = new_frame_->f2w(sample_point_cam, cam_idx); + + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = floor(sample_point_w[j] / voxel_size_rc); + if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + } + + VOXEL_LOCATION sample_pos(loc_xyz[0], loc_xyz[1], loc_xyz[2]); + + auto corre_sub_feat_map = sub_feat_map.find(sample_pos); + if (corre_sub_feat_map != sub_feat_map.end()) break; + + auto corre_feat_map = feat_map.find(sample_pos); + if (corre_feat_map != feat_map.end()) + { + std::vector &voxel_points = corre_feat_map->second->voxel_points; + int voxel_num = voxel_points.size(); + if (voxel_num == 0) continue; + + for (int j = 0; j < voxel_num; j++) + { + VisualPoint *pt = voxel_points[j]; + if (pt == nullptr) continue; + if (pt->obs_.size() == 0) continue; + bool point_visible = false; + for (int check_cam = 0; check_cam < (int)cams.size(); check_cam++) + { + V3D dir = new_frame_->w2f(pt->pos_, check_cam); + if (dir[2] < 0) continue; + + V2D pc = new_frame_->w2c(pt->pos_, check_cam); + if (new_frame_->cams_[check_cam]->isInFrame(pc.cast(), border)) + { + point_visible = true; + + grid_num[i] = TYPE_MAP; + + Vector3d obs_vec = new_frame_->pos(check_cam) - pt->pos_; + float cur_dist = obs_vec.norm(); + if (cur_dist <= map_dist[i]) + { + map_dist[i] = cur_dist; + retrieve_voxel_points[i] = pt; + } + break; + } + } + if (point_visible) + { + found_feature = true; + break; + } + } + + if (found_feature) + { + sub_feat_map[sample_pos] = 0; + break; + } + } + else + { + auto iter = plane_map.find(sample_pos); + if (iter != plane_map.end()) + { + VoxelOctoTree *current_octo; + current_octo = iter->second->find_correspond(sample_point_w); + if (current_octo->plane_ptr_->is_plane_) + { + pointWithVar plane_center; + VoxelPlane &plane = *current_octo->plane_ptr_; + plane_center.point_w = plane.center_; + plane_center.normal = plane.normal_; + visual_submap->add_from_voxel_map.push_back(plane_center); + found_feature = true; + break; + } + } + } + } + if (found_feature) break; + } + } + } +} + +double VIOManager::calculateCoVarianceScale(double realtime_avg_error, int realtime_n_meas) +{ + if (frame_count < dynamic_cov_warmup_frames) { return warmup_cov_scale; } + + double t = (realtime_avg_error - 1.0) / (dynamic_cov_error_max - 1.0); + t = std::min(1.0, std::max(0.0, t)); + double cov_scale = min_cov_scale + t * (max_cov_scale - min_cov_scale); + cov_scale = std::max(min_cov_scale, std::min(max_cov_scale, cov_scale)); + + double alpha = 0.6; + double smoothed_scale = alpha * cov_scale + (1.0 - alpha) * prev_cov_scale_; + prev_cov_scale_ = smoothed_scale; + + if (frame_count % 30 == 0 || smoothed_scale > min_cov_scale * 1.5) + { + printf("[DynCov] frame=%d, MSE=%.1f, cov=%.1f (range: %.1f~%.1f)\n", frame_count, realtime_avg_error, smoothed_scale, min_cov_scale, max_cov_scale); + } + + return smoothed_scale; +} + +double VIOManager::calculateCoVarianceScalePerCam(int cam_idx, double realtime_avg_error, int realtime_n_meas) +{ + if (frame_count < dynamic_cov_warmup_frames) { return warmup_cov_scale; } + + const int min_required_meas = 5; + if (realtime_n_meas < min_required_meas) { return max_cov_scale * 10.0; } + + double t = (realtime_avg_error - 1.0) / (dynamic_cov_error_max - 1.0); + t = std::min(1.0, std::max(0.0, t)); + double cov_scale = min_cov_scale + t * (max_cov_scale - min_cov_scale); + cov_scale = std::max(min_cov_scale, std::min(max_cov_scale, cov_scale)); + + double alpha = 0.6; + double smoothed_scale = alpha * cov_scale + (1.0 - alpha) * prev_cov_scale_per_cam_[cam_idx]; + prev_cov_scale_per_cam_[cam_idx] = smoothed_scale; + + return smoothed_scale; +} + +void VIOManager::computeJacobianAndUpdateEKF(const std::vector imgs) +{ + if (total_points < 2) + { + compute_jacobian_time = update_ekf_time = 0.0; + return; + } + double original_img_point_cov = img_point_cov; + if (enable_dynamic_covariance_) + { + double cov_scale = calculateCoVarianceScale(prev_avg_error_, prev_n_meas_); + img_point_cov = cov_scale; + } + + compute_jacobian_time = update_ekf_time = 0.0; + for (int level = patch_pyrimid_level - 1; level >= 0; level--) + { + if (inverse_composition_en) { updateStateInverse(imgs, level); } + else { updateState(imgs, level); } + } + state->cov -= G * state->cov; + updateFrameState(*state); + img_point_cov = original_img_point_cov; +} + +void VIOManager::generateVisualMapPoints(const std::vector &imgs, std::vector &pg) +{ + if (pg.size() <= 10) return; + + int num_cameras = static_cast(imgs.size()); + int cells_per_camera = grid_n_width * grid_n_height; + int total_cells = cells_per_camera * num_cameras; + + std::vector best_cam_idx(total_cells, -1); + if ((int)append_voxel_points.size() != total_cells) { append_voxel_points.resize(total_cells); } + + std::vector camera_grid_offset(num_cameras); + for (int i = 0; i < num_cameras; i++) { camera_grid_offset[i] = i * cells_per_camera; } + + for (size_t i = 0; i < pg.size(); i++) + { + if (pg[i].normal == V3D(0, 0, 0)) continue; + + V3D pt = pg[i].point_w; + for (size_t cam_idx = 0; cam_idx < imgs.size(); cam_idx++) + { + V2D pc = new_frame_->w2c(pt, cam_idx); + if (!cams[cam_idx]->isInFrame(pc.cast(), border)) continue; + int grid_col = static_cast(pc[0] / grid_size); + int grid_row = static_cast(pc[1] / grid_size); + if (grid_col < 0 || grid_col >= grid_n_width || grid_row < 0 || grid_row >= grid_n_height) continue; + + int local_index = grid_row * grid_n_width + grid_col; + int index = camera_grid_offset[cam_idx] + local_index; + if (index < 0 || index >= total_cells) continue; + if (grid_num[index] == TYPE_MAP) continue; + + float cur_value = vk::shiTomasiScore(imgs[cam_idx], pc[0], pc[1]); + if (cur_value > scan_value[index]) + { + scan_value[index] = cur_value; + append_voxel_points[index] = pg[i]; + best_cam_idx[index] = static_cast(cam_idx); + grid_num[index] = TYPE_POINTCLOUD; + } + } + } + + for (int i = 0; i < total_cells; i++) + { + if (grid_num[i] != TYPE_POINTCLOUD) continue; + if (best_cam_idx[i] < 0 || best_cam_idx[i] >= (int)imgs.size()) continue; + + pointWithVar pt_var = append_voxel_points[i]; + V3D pt = pt_var.point_w; + if (pt.norm() < 0.0001) continue; + + int cam_idx = best_cam_idx[i]; + V2D pc = new_frame_->w2c(pt, cam_idx); + if (!cams[cam_idx]->isInFrame(pc.cast(), border)) continue; + V3D pt_cam = new_frame_->w2f(pt, cam_idx); + if (pt_cam[2] <= 0) continue; + + try + { + float *patch = new float[patch_size_total]; + getImagePatch(imgs[cam_idx], pc, patch, 0); + VisualPoint *pt_new = new VisualPoint(pt); + Vector3d f = cams[cam_idx]->cam2world(pc); + Feature *ftr_new = new Feature(pt_new, patch, pc, f, new_frame_->T_f_w_[cam_idx], 0, cam_idx); + ftr_new->cam_id_ = cam_idx; + ftr_new->img_ = imgs[cam_idx]; + ftr_new->id_ = new_frame_->id_; + ftr_new->inv_expo_time_ = (cam_idx < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[cam_idx] : 1.0; + pt_new->addFrameRef(ftr_new); + + if (enable_cross_camera_tracking) + { + pt_new->cross_cam_data_.primary_cam_idx = cam_idx; + pt_new->cross_cam_data_.cross_camera_migrations = 0; + pt_new->cross_cam_data_.currently_visible.reset(); + pt_new->cross_cam_data_.currently_visible.set(cam_idx); + } + pt_new->covariance_ = pt_var.var; + pt_new->is_normal_initialized_ = true; + V3D norm_vec = new_frame_->T_f_w_[cam_idx].rotationMatrix() * pt_var.normal; + V3D dir = new_frame_->T_f_w_[cam_idx] * pt; + if (dir.dot(norm_vec) < 0) pt_new->normal_ = -pt_var.normal; + else pt_new->normal_ = pt_var.normal; + pt_new->previous_normal_ = pt_new->normal_; + insertPointIntoVoxelMap(pt_new); + } + catch (const std::exception &e) + { + std::cerr << "Exception in generateVisualMapPoints: " << e.what() << std::endl; + continue; + } + } +} + +void VIOManager::updateVisualMapPoints(std::vector &imgs) +{ + if (total_points == 0) return; + + for (int i = 0; i < total_points; i++) + { + VisualPoint *pt = visual_submap->voxel_points[i]; + if (pt == nullptr) continue; + if (pt->is_converged_) + { + pt->deleteNonRefPatchFeatures(); + continue; + } + if (pt->obs_.empty()) continue; + Feature *last_feature = pt->obs_.back(); + for (int cam_idx = 0; cam_idx < (int)cams.size(); cam_idx++) + { + SE3 pose_cur = new_frame_->T_f_w_[cam_idx]; + V2D pc = new_frame_->w2c(pt->pos_, cam_idx); + if (!cams[cam_idx]->isInFrame(pc.cast(), border)) continue; + SE3 pose_ref = last_feature->T_f_w_; + SE3 delta_pose = pose_ref * pose_cur.inverse(); + double delta_p = delta_pose.translation().norm(); + double cos_val = 0.5 * (delta_pose.rotationMatrix().trace() - 1); + if (cos_val > 1.0) cos_val = 1.0; + if (cos_val < -1.0) cos_val = -1.0; + double delta_theta = std::acos(cos_val); + bool add_flag = false; + if (delta_p > 0.5 || delta_theta > 0.3) add_flag = true; + V2D last_px = last_feature->px_; + double pixel_dist = (pc - last_px).norm(); + if (pixel_dist > 40.0) add_flag = true; + if (pt->obs_.size() >= 30) + { + Feature *ref_ftr = nullptr; + pt->findMinScoreFeature(new_frame_->pos(cam_idx), ref_ftr); + if (ref_ftr) pt->deleteFeatureRef(ref_ftr); + } + if (add_flag) + { + float *patch_temp = new float[patch_size_total]; + getImagePatch(imgs[cam_idx], pc, patch_temp, 0); + Vector3d f = cams[cam_idx]->cam2world(pc); + int search_level = ((int)visual_submap->search_levels.size() > i) ? visual_submap->search_levels[i] : 0; + + Feature *ftr_new = new Feature(pt, patch_temp, pc, f, new_frame_->T_f_w_[cam_idx], search_level, cam_idx); + ftr_new->cam_id_ = cam_idx; + ftr_new->img_ = imgs[cam_idx]; + ftr_new->id_ = new_frame_->id_; + ftr_new->inv_expo_time_ = (cam_idx < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[cam_idx] : 1.0; + pt->addFrameRef(ftr_new); + update_flag[i] = 1; + } + } + } +} + +void VIOManager::updateReferencePatch(const unordered_map &plane_map) +{ + if (total_points == 0) return; + for (int i = 0; i < (int)visual_submap->voxel_points.size(); i++) + { + VisualPoint *pt = visual_submap->voxel_points[i]; + if (!pt->is_normal_initialized_ || pt->is_converged_ || pt->obs_.size() <= 5) continue; + if (update_flag[i] == 0) continue; + { + const V3D &p_w = pt->pos_; + float loc_xyz[3]; + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = p_w[j] / 0.5; + if (loc_xyz[j] < 0) loc_xyz[j] -= 1.0f; + } + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]); + auto it_plane = plane_map.find(position); + if (it_plane != plane_map.end()) + { + VoxelOctoTree *current_octo = it_plane->second->find_correspond(p_w); + if (current_octo->plane_ptr_->is_plane_) + { + VoxelPlane &plane = *current_octo->plane_ptr_; + float dis_to_plane = plane.normal_.dot(p_w) + plane.d_; + float dis_to_plane_abs = std::fabs(dis_to_plane); + float dis_to_center = (plane.center_ - p_w).squaredNorm(); + float range_dis = std::sqrt(dis_to_center - dis_to_plane * dis_to_plane); + + if (range_dis <= 3.0f * plane.radius_) + { + Eigen::Matrix J_nq; + J_nq.block<1, 3>(0, 0) = p_w - plane.center_; + J_nq.block<1, 3>(0, 3) = -plane.normal_.transpose(); + double sigma_l = J_nq * plane.plane_var_ * J_nq.transpose(); + sigma_l += plane.normal_.transpose() * pt->covariance_ * plane.normal_; + + if (dis_to_plane_abs < 3.0 * std::sqrt(sigma_l)) + { + if (pt->previous_normal_.dot(plane.normal_) < 0) pt->normal_ = -plane.normal_; + else pt->normal_ = plane.normal_; + + double normal_update = (pt->normal_ - pt->previous_normal_).norm(); + pt->previous_normal_ = pt->normal_; + if (normal_update < 0.0001 && pt->obs_.size() > 10) { pt->is_converged_ = true; } + } + } + } + } + } + float score_max = -1e6; + Feature *best_ref_ftr = nullptr; + for (auto it = pt->obs_.begin(); it != pt->obs_.end(); ++it) + { + Feature *ref_patch_temp = *it; + if (!ref_patch_temp) continue; + float *patch_temp = ref_patch_temp->patch_; + V3D pf = ref_patch_temp->T_f_w_ * pt->pos_; + V3D norm_vec = ref_patch_temp->T_f_w_.rotationMatrix() * pt->normal_; + pf.normalize(); + double cos_angle = pf.dot(norm_vec); + if (std::fabs(ref_patch_temp->mean_) < 1e-6) + { + float sum_val = std::accumulate(patch_temp, patch_temp + patch_size_total, 0.0f); + ref_patch_temp->mean_ = sum_val / (float)patch_size_total; + } + float ref_mean = ref_patch_temp->mean_; + float sumNCC = 0.0f; + int countNCC = 0; + + for (auto itm = pt->obs_.begin(); itm != pt->obs_.end(); ++itm) + { + if ((*itm)->id_ == ref_patch_temp->id_) continue; + float *patch_cache = (*itm)->patch_; + if (std::fabs((*itm)->mean_) < 1e-6) + { + float sum_val2 = std::accumulate(patch_cache, patch_cache + patch_size_total, 0.0f); + (*itm)->mean_ = sum_val2 / (float)patch_size_total; + } + float other_mean = (*itm)->mean_; + + Eigen::Map patch_a(patch_temp, patch_size_total); + Eigen::Map patch_b(patch_cache, patch_size_total); + Eigen::VectorXf diff1 = (patch_a.cast().array() - ref_mean).matrix().cast(); + Eigen::VectorXf diff2 = (patch_b.cast().array() - other_mean).matrix().cast(); + double numerator = diff1.dot(diff2); + double denominator1 = diff1.squaredNorm(); + double denominator2 = diff2.squaredNorm(); + double ncc_val = numerator / std::sqrt(denominator1 * denominator2 + 1e-10); + sumNCC += std::fabs(ncc_val); + countNCC++; + } + float NCC_avg = (countNCC > 0) ? (sumNCC / (float)countNCC) : 0.0f; + float score = NCC_avg + (float)cos_angle; + ref_patch_temp->score_ = score; + if (score > score_max) + { + score_max = score; + best_ref_ftr = ref_patch_temp; + } + } + if (best_ref_ftr) + { + pt->ref_patch = best_ref_ftr; + pt->has_ref_patch_ = true; + } + } +} + +void VIOManager::projectPatchFromRefToCur(const unordered_map &plane_map) +{ + // Debug-only visualization (invoked from processFrame() only when + // plot_flag is set); always uses camera 0. + if (total_points == 0) return; + + int patch_size_dbg = 25; + string dir = string(ROOT_DIR) + "Log/ref_cur_combine/"; + + cv::Mat result = cv::Mat::zeros(height, width, CV_8UC1); + cv::Mat result_normal = cv::Mat::zeros(height, width, CV_8UC1); + + cv::Mat img_photometric_error = new_frame_->imgs_[0].clone(); + + uchar *it = (uchar *)result.data; + uchar *it_normal = (uchar *)result_normal.data; + + struct pixel_member + { + Vector2f pixel_pos; + uint8_t pixel_value; + }; + + int num = 0; + for (int i = 0; i < (int)visual_submap->voxel_points.size(); i++) + { + VisualPoint *pt = visual_submap->voxel_points[i]; + + if (pt->is_normal_initialized_) + { + Feature *ref_ftr = pt->ref_patch; + V2D pc(new_frame_->w2c(pt->pos_, 0)); + V2D pc_prior(new_frame_->w2c_prior(pt->pos_, 0)); + + V3D norm_vec(ref_ftr->T_f_w_.rotationMatrix() * pt->normal_); + V3D pf(ref_ftr->T_f_w_ * pt->pos_); + if (pf.dot(norm_vec) < 0) norm_vec = -norm_vec; + cv::Mat img_cur = new_frame_->imgs_[0]; + cv::Mat img_ref = ref_ftr->img_; + + SE3 T_cur_ref = new_frame_->T_f_w_[0] * ref_ftr->T_f_w_.inverse(); + Matrix2d A_cur_ref; + getWarpMatrixAffineHomography(*cams[0], ref_ftr->px_, pf, norm_vec, T_cur_ref, 0, A_cur_ref); + int search_level = getBestSearchLevel(A_cur_ref.inverse(), 2); + + double D = A_cur_ref.determinant(); + if (D > 3) continue; + num++; + + cv::Mat ref_cur_combine_temp; + int radius = 20; + cv::hconcat(img_cur, img_ref, ref_cur_combine_temp); + cv::cvtColor(ref_cur_combine_temp, ref_cur_combine_temp, CV_GRAY2BGR); + + getImagePatch(img_cur, pc, patch_buffer.data(), 0); + + float error_est = 0.0; + { + double cur_inv_expo = (!state->inv_expo_time_per_cam.empty()) ? state->inv_expo_time_per_cam[0] : 1.0; + Eigen::Map patch_warp(visual_submap->warp_patch[i].data(), patch_size_total); + Eigen::Map patch_buf(patch_buffer.data(), patch_size_total); + Eigen::VectorXf diff = ref_ftr->inv_expo_time_ * patch_warp - cur_inv_expo * patch_buf; + error_est = diff.squaredNorm(); + } + std::string ref_est = "ref_est " + std::to_string(1.0 / ref_ftr->inv_expo_time_); + double cur_inv_expo_str = (!state->inv_expo_time_per_cam.empty()) ? state->inv_expo_time_per_cam[0] : 1.0; + std::string cur_est = "cur_est " + std::to_string(1.0 / cur_inv_expo_str); + std::string cur_optimize = "cur_est " + std::to_string(error_est); + + cv::putText(ref_cur_combine_temp, ref_est, cv::Point2f(ref_ftr->px_[0] + img_cur.cols - 40, ref_ftr->px_[1] + 40), cv::FONT_HERSHEY_COMPLEX, 0.4, + cv::Scalar(0, 255, 0), 1, 8, 0); + cv::putText(ref_cur_combine_temp, cur_est, cv::Point2f(pc[0] - 40, pc[1] + 40), cv::FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(0, 255, 0), 1, 8, 0); + cv::putText(ref_cur_combine_temp, cur_optimize, cv::Point2f(pc[0] - 40, pc[1] + 80), cv::FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(0, 255, 0), 1, 8, 0); + + cv::rectangle(ref_cur_combine_temp, cv::Point2f(ref_ftr->px_[0] + img_cur.cols - radius, ref_ftr->px_[1] - radius), + cv::Point2f(ref_ftr->px_[0] + img_cur.cols + radius, ref_ftr->px_[1] + radius), cv::Scalar(0, 0, 255), 1); + cv::rectangle(ref_cur_combine_temp, cv::Point2f(pc[0] - radius, pc[1] - radius), cv::Point2f(pc[0] + radius, pc[1] + radius), cv::Scalar(0, 255, 0), + 1); + cv::rectangle(ref_cur_combine_temp, cv::Point2f(pc_prior[0] - radius, pc_prior[1] - radius), cv::Point2f(pc_prior[0] + radius, pc_prior[1] + radius), + cv::Scalar(255, 255, 255), 1); + cv::circle(ref_cur_combine_temp, cv::Point2f(ref_ftr->px_[0] + img_cur.cols, ref_ftr->px_[1]), 1, cv::Scalar(0, 0, 255), -1, 8); + cv::circle(ref_cur_combine_temp, cv::Point2f(pc[0], pc[1]), 1, cv::Scalar(0, 255, 0), -1, 8); + cv::circle(ref_cur_combine_temp, cv::Point2f(pc_prior[0], pc_prior[1]), 1, cv::Scalar(255, 255, 255), -1, 8); + cv::imwrite(dir + std::to_string(new_frame_->id_) + "_" + std::to_string(ref_ftr->id_) + "_" + std::to_string(num) + ".png", ref_cur_combine_temp); + + std::vector> pixel_warp_matrix; + for (int y = 0; y < patch_size_dbg; ++y) + { + vector pixel_warp_vec; + for (int x = 0; x < patch_size_dbg; ++x) + { + Vector2f px_patch(x - patch_size_dbg / 2, y - patch_size_dbg / 2); + px_patch *= (1 << search_level); + const Vector2f px_ref(px_patch + ref_ftr->px_.cast()); + uint8_t pixel_value = (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]); + + const Vector2f px(A_cur_ref.cast() * px_patch + pc.cast()); + if (px[0] < 0 || px[1] < 0 || px[0] >= img_cur.cols - 1 || px[1] >= img_cur.rows - 1) continue; + else + { + pixel_member pixel_warp; + pixel_warp.pixel_pos << px[0], px[1]; + pixel_warp.pixel_value = pixel_value; + pixel_warp_vec.push_back(pixel_warp); + } + } + pixel_warp_matrix.push_back(pixel_warp_vec); + } + + float x_min = 1000, y_min = 1000, x_max = 0, y_max = 0; + for (int r = 0; r < (int)pixel_warp_matrix.size(); r++) + { + vector pixel_warp_row = pixel_warp_matrix[r]; + for (int c = 0; c < (int)pixel_warp_row.size(); c++) + { + float x_temp = pixel_warp_row[c].pixel_pos[0]; + float y_temp = pixel_warp_row[c].pixel_pos[1]; + if (x_temp < x_min) x_min = x_temp; + if (y_temp < y_min) y_min = y_temp; + if (x_temp > x_max) x_max = x_temp; + if (y_temp > y_max) y_max = y_temp; + } + } + int x_min_i = floor(x_min); + int y_min_i = floor(y_min); + int x_max_i = ceil(x_max); + int y_max_i = ceil(y_max); + Matrix2f A_cur_ref_Inv = A_cur_ref.inverse().cast(); + for (int px = x_min_i; px < x_max_i; px++) + { + for (int py = y_min_i; py < y_max_i; py++) + { + Eigen::Vector2f pc_temp(px, py); + Vector2f px_patch = A_cur_ref_Inv * (pc_temp - pc.cast()); + if (px_patch[0] > (-patch_size_dbg / 2 * (1 << search_level)) && px_patch[0] < (patch_size_dbg / 2 * (1 << search_level)) && + px_patch[1] > (-patch_size_dbg / 2 * (1 << search_level)) && px_patch[1] < (patch_size_dbg / 2 * (1 << search_level))) + { + const Vector2f px_ref(px_patch + ref_ftr->px_.cast()); + uint8_t pixel_value = (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]); + it_normal[width * py + px] = pixel_value; + } + } + } + } + } + for (int i = 0; i < (int)visual_submap->voxel_points.size(); i++) + { + VisualPoint *pt = visual_submap->voxel_points[i]; + if (!pt->is_normal_initialized_) continue; + + Feature *ref_ftr = pt->ref_patch; + V2D pc(new_frame_->w2c(pt->pos_, 0)); + + Matrix2d A_cur_ref; + getWarpMatrixAffine(*cams[0], ref_ftr->px_, ref_ftr->f_, (ref_ftr->pos() - pt->pos_).norm(), new_frame_->T_f_w_[0] * ref_ftr->T_f_w_.inverse(), 0, 0, + patch_size_half, A_cur_ref); + int search_level = getBestSearchLevel(A_cur_ref.inverse(), 2); + double D = A_cur_ref.determinant(); + if (D > 3) continue; + + cv::Mat img_cur = new_frame_->imgs_[0]; + cv::Mat img_ref = ref_ftr->img_; + for (int y = 0; y < patch_size_dbg; ++y) + { + for (int x = 0; x < patch_size_dbg; ++x) + { + Vector2f px_patch(x - patch_size_dbg / 2, y - patch_size_dbg / 2); + px_patch *= (1 << search_level); + const Vector2f px_ref(px_patch + ref_ftr->px_.cast()); + uint8_t pixel_value = (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]); + + const Vector2f px(A_cur_ref.cast() * px_patch + pc.cast()); + if (px[0] < 0 || px[1] < 0 || px[0] >= img_cur.cols - 1 || px[1] >= img_cur.rows - 1) continue; + else + { + int col = int(px[0]); + int row = int(px[1]); + it[width * row + col] = pixel_value; + } + } + } + } + cv::Mat ref_cur_combine, ref_cur_combine_normal, ref_cur_combine_error; + + cv::hconcat(result, new_frame_->imgs_[0], ref_cur_combine); + cv::hconcat(result_normal, new_frame_->imgs_[0], ref_cur_combine_normal); + + cv::cvtColor(ref_cur_combine, ref_cur_combine, CV_GRAY2BGR); + cv::cvtColor(ref_cur_combine_normal, ref_cur_combine_normal, CV_GRAY2BGR); + cv::absdiff(img_photometric_error, result_normal, img_photometric_error); + cv::hconcat(img_photometric_error, new_frame_->imgs_[0], ref_cur_combine_error); + + cv::imwrite(dir + std::to_string(new_frame_->id_) + "_0_.png", ref_cur_combine); + cv::imwrite(dir + std::to_string(new_frame_->id_) + "_0_photometric.png", ref_cur_combine_error); + cv::imwrite(dir + std::to_string(new_frame_->id_) + "_0_normal.png", ref_cur_combine_normal); +} + +void VIOManager::precomputeReferencePatches(int level) +{ + double t1 = omp_get_wtime(); + if (total_points == 0) return; + + const int num_cams = (int)cams.size(); + const int H_DIM = total_points * patch_size_total; + + H_sub_inv.resize(H_DIM, 6); + H_sub_inv.setZero(); + + for (int i = 0; i < total_points; i++) + { + VisualPoint *pt = visual_submap->voxel_points[i]; + if (!pt) continue; + + int cam_idx = visual_submap->camera_ids[i]; + if (cam_idx < 0 || cam_idx >= num_cams) continue; + + double fx_i = cams[cam_idx]->fx(); + double fy_i = cams[cam_idx]->fy(); + + Feature *ref_ftr = pt->ref_patch; + if (!ref_ftr) continue; + + cv::Mat &img = ref_ftr->img_; + if (img.empty()) continue; + + double depth = (pt->pos_ - ref_ftr->pos()).norm(); + V3D pf = ref_ftr->f_ * depth; + + V2D pc = ref_ftr->px_; + M3D R_ref_w = ref_ftr->T_f_w_.rotationMatrix(); + + MD(2, 3) Jdpi; + computeProjectionJacobian(cam_idx, pf, Jdpi); + + M3D p_hat; + p_hat << SKEW_SYM_MATRX(pt->pos_); + + int search_level = visual_submap->search_levels[i]; + int pyramid_level = level + search_level; + int scale = (1 << pyramid_level); + float inv_scale = 1.0f / scale; + + float u_ref = pc[0]; + float v_ref = pc[1]; + int u_ref_i = (int)std::floor(u_ref / scale) * scale; + int v_ref_i = (int)std::floor(v_ref / scale) * scale; + float subpix_u_ref = (u_ref - u_ref_i) / scale; + float subpix_v_ref = (v_ref - v_ref_i) / scale; + float w_ref_tl = (1.0f - subpix_u_ref) * (1.0f - subpix_v_ref); + float w_ref_tr = subpix_u_ref * (1.0f - subpix_v_ref); + float w_ref_bl = (1.0f - subpix_u_ref) * subpix_v_ref; + float w_ref_br = subpix_u_ref * subpix_v_ref; + + int row_offset_pt = i * patch_size_total; + + for (int px_y = 0; px_y < patch_size; px_y++) + { + int row_img = v_ref_i + px_y * scale - patch_size_half * scale; + if (row_img < 1 || row_img >= img.rows - 1) continue; + + int col_start = (u_ref_i - patch_size_half * scale); + if (col_start < 1 || col_start >= img.cols - 1) continue; + + uint8_t *img_ptr = (uint8_t *)img.data + row_img * img.cols + col_start; + + for (int px_x = 0; px_x < patch_size; px_x++, img_ptr += scale) + { + if ((col_start + px_x * scale) < 1 || (col_start + px_x * scale) >= (img.cols - 1)) continue; + + float du = 0.5f * ((w_ref_tl * img_ptr[scale] + w_ref_tr * img_ptr[scale * 2] + w_ref_bl * img_ptr[scale * img.cols + scale] + + w_ref_br * img_ptr[scale * img.cols + scale * 2]) - + (w_ref_tl * img_ptr[-scale] + w_ref_tr * img_ptr[0] + w_ref_bl * img_ptr[scale * img.cols - scale] + + w_ref_br * img_ptr[scale * img.cols])); + float dv = 0.5f * ((w_ref_tl * img_ptr[scale * img.cols] + w_ref_tr * img_ptr[scale + scale * img.cols] + + w_ref_bl * img_ptr[scale * img.cols * 2] + w_ref_br * img_ptr[scale * img.cols * 2 + scale]) - + (w_ref_tl * img_ptr[-scale * img.cols] + w_ref_tr * img_ptr[-scale * img.cols + scale] + w_ref_bl * img_ptr[0] + + w_ref_br * img_ptr[scale])); + + MD(1, 2) Jimg; + Jimg << du, dv; + Jimg *= inv_scale; + + MD(1, 3) J_dphi = Jimg * Jdpi * (R_ref_w * p_hat); + MD(1, 3) J_dp = -Jimg * Jdpi * R_ref_w; + + MD(1, 3) JdR = J_dphi * Jdphi_dR_vec[cam_idx] + J_dp * Jdp_dR_vec[cam_idx]; + MD(1, 3) Jdt = J_dp * Jdp_dt_vec[cam_idx]; + + int row_cur = row_offset_pt + px_y * patch_size + px_x; + if (row_cur >= 0 && row_cur < H_DIM) { H_sub_inv.block<1, 6>(row_cur, 0) << JdR, Jdt; } + } + } + } + + has_ref_patch_cache = true; + compute_jacobian_time += (omp_get_wtime() - t1); +} + +void VIOManager::updateStateInverse(const std::vector &imgs, int level) +{ + if (total_points == 0) return; + StatesGroup old_state = (*state); + const int num_cams = static_cast(imgs.size()); + const int H_DIM = total_points * patch_size_total; + + if ((int)state->inv_expo_time_per_cam.size() != (int)cams.size()) { state->inv_expo_time_per_cam.resize(cams.size(), 1.0); } + + VectorXd z(H_DIM); + z.setZero(); + MatrixXd H_sub(H_DIM, 6); + H_sub.setZero(); + + bool EKF_end = false; + float last_error = std::numeric_limits::max(); + int n_meas = 0; + + std::vector error_per_cam(num_cams, 0.0f); + std::vector n_meas_per_cam(num_cams, 0); + std::vector last_error_per_cam(num_cams, 0.0f); + + compute_jacobian_time = 0.0; + update_ekf_time = 0.0; + M3D P_wi_hat; + P_wi_hat << SKEW_SYM_MATRX(state->pos_end); + if (!has_ref_patch_cache) { precomputeReferencePatches(level); } + for (int iter = 0; iter < max_iterations; iter++) + { + double t1 = omp_get_wtime(); + n_meas = 0; + float error = 0.0f; + for (int c = 0; c < num_cams; c++) + { + error_per_cam[c] = 0.0f; + n_meas_per_cam[c] = 0; + } + M3D Rwi(state->rot_end); + V3D Pwi(state->pos_end); + + for (int cam_idx = 0; cam_idx < num_cams; cam_idx++) + { + Rcw_vec[cam_idx] = Rci_vec[cam_idx] * Rwi.transpose(); + Pcw_vec[cam_idx] = -Rci_vec[cam_idx] * Rwi.transpose() * Pwi + Pci_vec[cam_idx]; + Jdp_dt_vec[cam_idx] = Rci_vec[cam_idx] * Rwi.transpose(); + } + +#ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); +#pragma omp parallel for schedule(dynamic, 4) reduction(+:error, n_meas) +#endif + for (int i_pt = 0; i_pt < total_points; i_pt++) + { + float patch_error = 0.0f; + const int scale = (1 << level); + + VisualPoint *pt = visual_submap->voxel_points[i_pt]; + if (!pt) continue; + + int cam_idx = visual_submap->camera_ids[i_pt]; + if (cam_idx < 0 || cam_idx >= num_cams) continue; + if (imgs[cam_idx].empty()) continue; + + const cv::Mat &img_cur = imgs[cam_idx]; + double cur_inv_expo = (cam_idx < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[cam_idx] : 1.0; + + V3D pf = Rcw_vec[cam_idx] * pt->pos_ + Pcw_vec[cam_idx]; + if (pf.z() < 1e-6) continue; + V2D pc = cams[cam_idx]->world2cam(pf); + int u_ref_i = static_cast(std::floor(pc[0] / scale)) * scale; + int v_ref_i = static_cast(std::floor(pc[1] / scale)) * scale; + float subpix_u_ref = (pc[0] - u_ref_i) / scale; + float subpix_v_ref = (pc[1] - v_ref_i) / scale; + float w_ref_tl = (1.f - subpix_u_ref) * (1.f - subpix_v_ref); + float w_ref_tr = subpix_u_ref * (1.f - subpix_v_ref); + float w_ref_bl = (1.f - subpix_u_ref) * subpix_v_ref; + float w_ref_br = subpix_u_ref * subpix_v_ref; + const std::vector &P_patch = visual_submap->warp_patch[i_pt]; + double inv_ref_expo = visual_submap->inv_expo_list[i_pt]; + int row_offset_pt = i_pt * patch_size_total; + for (int px_y = 0; px_y < patch_size; px_y++) + { + int row_img = v_ref_i + px_y * scale - patch_size_half * scale; + if (row_img < 1 || row_img >= (img_cur.rows - 1)) continue; + int col_start = u_ref_i - patch_size_half * scale; + if (col_start < 1 || col_start >= (img_cur.cols - 1)) continue; + uint8_t *img_ptr = (uint8_t *)img_cur.data + row_img * img_cur.cols + col_start; + + for (int px_x = 0; px_x < patch_size; px_x++, img_ptr += scale) + { + int cur_col = col_start + px_x * scale; + if (cur_col < 1 || cur_col >= (img_cur.cols - 1)) continue; + float du = 0.5f * ((w_ref_tl * img_ptr[scale] + w_ref_tr * img_ptr[scale * 2] + w_ref_bl * img_ptr[scale * img_cur.cols + scale] + + w_ref_br * img_ptr[scale * img_cur.cols + scale * 2]) - + (w_ref_tl * img_ptr[-scale] + w_ref_tr * img_ptr[0] + w_ref_bl * img_ptr[scale * img_cur.cols - scale] + + w_ref_br * img_ptr[scale * img_cur.cols])); + float dv = 0.5f * ((w_ref_tl * img_ptr[scale * img_cur.cols] + w_ref_tr * img_ptr[scale + scale * img_cur.cols] + + w_ref_bl * img_ptr[scale * img_cur.cols * 2] + w_ref_br * img_ptr[scale * img_cur.cols * 2 + scale]) - + (w_ref_tl * img_ptr[-scale * img_cur.cols] + w_ref_tr * img_ptr[-scale * img_cur.cols + scale] + w_ref_bl * img_ptr[0] + + w_ref_br * img_ptr[scale])); + + MD(1, 2) Jimg; + Jimg << du, dv; + Jimg = Jimg * cur_inv_expo; + Jimg = Jimg * (1.0f / scale); + MD(2, 3) Jdpi; + computeProjectionJacobian(cam_idx, pf, Jdpi); + M3D p_hat; + p_hat << SKEW_SYM_MATRX(pf); + MD(1, 3) J_dphi = Jimg * Jdpi * p_hat; + MD(1, 3) J_dp = -Jimg * Jdpi; + MD(1, 3) JdR_local = J_dphi * Jdphi_dR_vec[cam_idx] + J_dp * Jdp_dR_vec[cam_idx]; + MD(1, 3) Jdt_local = J_dp * Jdp_dt_vec[cam_idx]; + int row_idx = row_offset_pt + px_y * patch_size + px_x; + if (row_idx < 0 || row_idx >= H_DIM) continue; + MD(1, 3) J_dR_ref = H_sub_inv.block<1, 3>(row_idx, 0); + MD(1, 3) J_dt_ref = H_sub_inv.block<1, 3>(row_idx, 3); + MD(1, 3) JdR_final = J_dR_ref * Rwi + J_dt_ref * P_wi_hat * Rwi; + MD(1, 3) Jdt_final = J_dt_ref * Rwi; + H_sub.block<1, 6>(row_idx, 0) << JdR_final, Jdt_final; + float cur_val = w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + w_ref_bl * img_ptr[scale * img_cur.cols] + + w_ref_br * img_ptr[scale * img_cur.cols + scale]; + + int patch_idx = px_y * patch_size + px_x; + double res = cur_inv_expo * cur_val - inv_ref_expo * P_patch[patch_size_total * level + patch_idx]; + z(row_idx) = res; + patch_error += res * res; + n_meas++; + } + } + + visual_submap->errors[i_pt] = patch_error; +#pragma omp atomic + error += patch_error; + } + + for (int i_pt = 0; i_pt < total_points; i_pt++) + { + int cam_idx = visual_submap->camera_ids[i_pt]; + if (cam_idx >= 0 && cam_idx < num_cams) + { + error_per_cam[cam_idx] += visual_submap->errors[i_pt]; + n_meas_per_cam[cam_idx] += patch_size_total; + } + } + + if (n_meas > 0) error /= n_meas; + for (int c = 0; c < num_cams; c++) + { + if (n_meas_per_cam[c] > 0) error_per_cam[c] /= n_meas_per_cam[c]; + } + + compute_jacobian_time += omp_get_wtime() - t1; + double t3 = omp_get_wtime(); + if (error <= last_error && error > 0) + { + old_state = (*state); + last_error = error; + for (int c = 0; c < num_cams; c++) { last_error_per_cam[c] = error_per_cam[c]; } + + std::vector cov_per_cam(num_cams, img_point_cov); + if (enable_dynamic_covariance_) + { + for (int c = 0; c < num_cams; c++) { cov_per_cam[c] = calculateCoVarianceScalePerCam(c, prev_avg_error_per_cam_[c], prev_n_meas_per_cam_[c]); } + } + + MatrixXd H_weighted = H_sub; + VectorXd z_weighted = z; + for (int i_pt = 0; i_pt < total_points; i_pt++) + { + int cam_idx = visual_submap->camera_ids[i_pt]; + if (cam_idx < 0 || cam_idx >= num_cams) continue; + double weight = 1.0 / std::sqrt(cov_per_cam[cam_idx]); + int row_start = i_pt * patch_size_total; + for (int k = 0; k < patch_size_total; k++) + { + int row_idx = row_start + k; + if (row_idx < H_DIM) + { + H_weighted.row(row_idx) *= weight; + z_weighted(row_idx) *= weight; + } + } + } + + auto H_weighted_T = H_weighted.transpose(); + H_T_H.setZero(); + G.setZero(); + H_T_H.block<6, 6>(0, 0) = H_weighted_T * H_weighted; + + MD(DIM_STATE, DIM_STATE) K_1 = (H_T_H + state->cov.inverse()).inverse(); + auto HTz = H_weighted_T * z_weighted; + auto vec = (*state_propagat) - (*state); + G.block(0, 0) = K_1.block(0, 0) * H_T_H.block<6, 6>(0, 0); + MD(DIM_STATE, 1) solution = -K_1.block(0, 0) * HTz + vec - G.block(0, 0) * vec.block<6, 1>(0, 0); + + (*state) += solution; + + if (exposure_estimate_en) + { + std::vector expo_H(num_cams, 0.0); + std::vector expo_b(num_cams, 0.0); + + for (int i_pt = 0; i_pt < total_points; i_pt++) + { + int cam_idx = visual_submap->camera_ids[i_pt]; + if (cam_idx < 0 || cam_idx >= num_cams) continue; + + double inv_ref_expo = visual_submap->inv_expo_list[i_pt]; + const std::vector &P_patch = visual_submap->warp_patch[i_pt]; + int row_offset_pt = i_pt * patch_size_total; + + for (int px_idx = 0; px_idx < patch_size_total; px_idx++) + { + int row_idx = row_offset_pt + px_idx; + if (row_idx >= H_DIM) continue; + + double residual = z_weighted(row_idx); + double ref_val = P_patch[patch_size_total * level + px_idx]; + double cur_inv_expo = state->inv_expo_time_per_cam[cam_idx]; + double cur_val = (residual + inv_ref_expo * ref_val) / (cur_inv_expo + 1e-10); + + double J_expo = cur_val; + expo_H[cam_idx] += J_expo * J_expo; + expo_b[cam_idx] += J_expo * residual; + } + } + + for (int c = 0; c < num_cams; c++) + { + if (expo_H[c] > 1e-6) + { + double delta_expo = -expo_b[c] / expo_H[c]; + state->inv_expo_time_per_cam[c] += delta_expo; + } + } + } + + auto rot_add = solution.block<3, 1>(0, 0); + auto t_add = solution.block<3, 1>(3, 0); + + if ((rot_add.norm() * 57.3f < 0.001f) && (t_add.norm() * 100.0f < 0.001f)) EKF_end = true; + } + else + { + (*state) = old_state; + EKF_end = true; + } + update_ekf_time += omp_get_wtime() - t3; + + if (iter == max_iterations - 1 || EKF_end) break; + } + + prev_avg_error_ = last_error; + prev_n_meas_ = n_meas; + for (int c = 0; c < num_cams; c++) + { + if (c < (int)prev_avg_error_per_cam_.size()) + { + prev_avg_error_per_cam_[c] = last_error_per_cam[c]; + prev_n_meas_per_cam_[c] = n_meas_per_cam[c]; + } + } +} + +void VIOManager::updateState(const std::vector &imgs, int level) +{ + if (total_points == 0) return; + StatesGroup old_state = (*state); + const int num_cams = (int)cams.size(); + int base_measurement_size = total_points * patch_size_total * num_cams; + int cross_cam_measurements = total_points * patch_size_total * num_cams * (num_cams - 1) / 2; + int total_measurements = base_measurement_size + cross_cam_measurements; + + VectorXd z(total_measurements); + z.setZero(); + MatrixXd H_sub(total_measurements, 6 + num_cams); + H_sub.setZero(); + + bool EKF_end = false; + float last_error = std::numeric_limits::max(); + int n_meas = 0; + + std::vector error_per_cam(num_cams, 0.0f); + std::vector n_meas_per_cam(num_cams, 0); + std::vector last_error_per_cam(num_cams, 0.0f); + std::vector row_start_per_cam(num_cams, 0); + + successful_cross_camera_tracks = 0; + + for (int iteration = 0; iteration < max_iterations; iteration++) + { + double t1 = omp_get_wtime(); + M3D Rwi(state->rot_end); + V3D Pwi(state->pos_end); + + if ((int)state->inv_expo_time_per_cam.size() != (int)cams.size()) { state->inv_expo_time_per_cam.resize(cams.size(), 1.0); } + float error = 0.0f; + n_meas = 0; + for (int c = 0; c < num_cams; c++) + { + error_per_cam[c] = 0.0f; + n_meas_per_cam[c] = 0; + } + int cross_cam_row_offset = base_measurement_size; + + struct CrossCameraPair + { + VisualPoint *pt; + int cam_a; + int cam_b; + }; + std::vector cross_camera_pairs; + cross_camera_pairs.reserve(total_points); + + for (int c = 0; c < num_cams; c++) + { + row_start_per_cam[c] = c * total_points * patch_size_total; + if (!imgs[c].empty()) + { + Rcw_vec[c] = Rci_vec[c] * Rwi.transpose(); + Pcw_vec[c] = -Rci_vec[c] * Rwi.transpose() * Pwi + Pci_vec[c]; + Jdp_dt_vec[c] = Rci_vec[c] * Rwi.transpose(); + } + } + +#ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); +#pragma omp parallel for schedule(dynamic, 4) reduction(+:error, n_meas) +#endif + for (int i_pt = 0; i_pt < total_points; i_pt++) + { + float patch_error = 0.0f; + + VisualPoint *pt = visual_submap->voxel_points[i_pt]; + if (!pt) continue; + + int cam_idx = visual_submap->camera_ids[i_pt]; + if (cam_idx < 0 || cam_idx >= num_cams) continue; + if (imgs[cam_idx].empty()) continue; + + const cv::Mat &img_cur = imgs[cam_idx]; + + V3D pf = Rcw_vec[cam_idx] * pt->pos_ + Pcw_vec[cam_idx]; + if (pf.z() < 1e-6) continue; + + V2D pc = cams[cam_idx]->world2cam(pf); + MD(2, 3) Jdpi; + computeProjectionJacobian(cam_idx, pf, Jdpi); + M3D p_hat; + p_hat << SKEW_SYM_MATRX(pf); + int scale = (1 << level); + float inv_scale = 1.0f / scale; + + float u_ref = pc[0]; + float v_ref = pc[1]; + int u_ref_i = floorf(u_ref / scale) * scale; + int v_ref_i = floorf(v_ref / scale) * scale; + float subpix_u_ref = (u_ref - u_ref_i) * inv_scale; + float subpix_v_ref = (v_ref - v_ref_i) * inv_scale; + float w_ref_tl = (1.f - subpix_u_ref) * (1.f - subpix_v_ref); + float w_ref_tr = subpix_u_ref * (1.f - subpix_v_ref); + float w_ref_bl = (1.f - subpix_u_ref) * subpix_v_ref; + float w_ref_br = subpix_u_ref * subpix_v_ref; + const std::vector &P = visual_submap->warp_patch[i_pt]; + double inv_ref_expo = visual_submap->inv_expo_list[i_pt]; + float cur_inv_expo = (cam_idx < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[cam_idx] : 1.0; + if ((int)P.size() <= patch_size_total * level) continue; + + int row_offset_pt = row_start_per_cam[cam_idx] + i_pt * patch_size_total; + for (int px_x = 0; px_x < patch_size; px_x++) + { + int row_img = v_ref_i + px_x * scale - patch_size_half * scale; + if (row_img < 1 || row_img >= (img_cur.rows - 1)) continue; + uint8_t *img_ptr = (uint8_t *)img_cur.data + row_img * width + (u_ref_i - patch_size_half * scale); + + for (int px_y = 0; px_y < patch_size; px_y++, img_ptr += scale) + { + int col_img = (u_ref_i - patch_size_half * scale) + px_y * scale; + if (col_img < 1 || col_img >= (img_cur.cols - 1)) continue; + float du = 0.5f * ((w_ref_tl * img_ptr[scale] + w_ref_tr * img_ptr[scale * 2] + w_ref_bl * img_ptr[scale * width + scale] + + w_ref_br * img_ptr[scale * width + scale * 2]) - + (w_ref_tl * img_ptr[-scale] + w_ref_tr * img_ptr[0] + w_ref_bl * img_ptr[scale * width - scale] + + w_ref_br * img_ptr[scale * width])); + float dv = 0.5f * ((w_ref_tl * img_ptr[scale * width] + w_ref_tr * img_ptr[scale + scale * width] + w_ref_bl * img_ptr[scale * width * 2] + + w_ref_br * img_ptr[scale * width * 2 + scale]) - + (w_ref_tl * img_ptr[-scale * width] + w_ref_tr * img_ptr[-scale * width + scale] + w_ref_bl * img_ptr[0] + + w_ref_br * img_ptr[scale])); + MD(1, 2) Jimg; + Jimg << du, dv; + Jimg *= (float)cur_inv_expo; + Jimg *= inv_scale; + MD(1, 3) Jdphi = Jimg * Jdpi * p_hat; + MD(1, 3) Jdp = -Jimg * Jdpi; + MD(1, 3) JdR = Jdphi * Jdphi_dR_vec[cam_idx] + Jdp * Jdp_dR_vec[cam_idx]; + MD(1, 3) Jdt = Jdp * Jdp_dt_vec[cam_idx]; + float cur_val = + w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + w_ref_bl * img_ptr[scale * width] + w_ref_br * img_ptr[scale * width + scale]; + int idx_patch = px_x * patch_size + px_y; + float ref_val = P[patch_size_total * level + idx_patch]; + + float res; + if (exposure_estimate_en) { res = cur_inv_expo * cur_val - inv_ref_expo * ref_val; } + else { res = ref_val - cur_val; } + float corrected_res = res; + + if (pt->ref_patch && isRealCrossCameraPoint(pt, cam_idx)) + { + V2D px_pos(pc); + float source_intensity = applyCameraPhotoCorrection(inv_ref_expo * ref_val, pt->ref_patch->cam_id_, pt->ref_patch->px_); + float target_intensity = applyCameraPhotoCorrection(cur_inv_expo * cur_val, cam_idx, px_pos); + corrected_res = source_intensity - target_intensity; + } + + float w = 1.0f; + float abs_res = std::abs(corrected_res); + if (abs_res > outlier_threshold) { w = 0.0f; } + else + { + float ratio = corrected_res / outlier_threshold; + float temp = 1.0f - ratio * ratio; + w = temp * temp; + } + + float sqrt_w = sqrtf(w); + int row_here = row_offset_pt + idx_patch; + z(row_here) = sqrt_w * corrected_res; + H_sub(row_here, 0) = sqrt_w * JdR(0); + H_sub(row_here, 1) = sqrt_w * JdR(1); + H_sub(row_here, 2) = sqrt_w * JdR(2); + H_sub(row_here, 3) = sqrt_w * Jdt(0); + H_sub(row_here, 4) = sqrt_w * Jdt(1); + H_sub(row_here, 5) = sqrt_w * Jdt(2); + + if (exposure_estimate_en) + { + float expo_jacobian = cur_val; + int expo_col = 6 + cam_idx; + H_sub(row_here, expo_col) = sqrt_w * expo_jacobian; + } + patch_error += w * (corrected_res * corrected_res); + n_meas++; + } + } + visual_submap->errors[i_pt] = patch_error; + error += patch_error; +#pragma omp critical + { + error_per_cam[cam_idx] += patch_error; + n_meas_per_cam[cam_idx] += patch_size_total; + } + + if (enable_cross_camera_tracking) + { + for (int other_cam = cam_idx + 1; other_cam < num_cams; other_cam++) + { + if (pt->cross_cam_data_.currently_visible.test(cam_idx) && pt->cross_cam_data_.currently_visible.test(other_cam)) + { +#pragma omp critical + { cross_camera_pairs.push_back({pt, cam_idx, other_cam}); } + } + } + } + } + + successful_cross_camera_tracks = 0; + for (const auto &pair : cross_camera_pairs) + { + addCrossCameraConsistencyConstraint(pair.pt, pair.cam_a, pair.cam_b, H_sub, z, level, cross_cam_row_offset); + successful_cross_camera_tracks++; + } + + if (n_meas > 0) { error /= (float)n_meas; } + for (int c = 0; c < num_cams; c++) + { + if (n_meas_per_cam[c] > 0) error_per_cam[c] /= n_meas_per_cam[c]; + } + + compute_jacobian_time += (omp_get_wtime() - t1); + double t3 = omp_get_wtime(); + + if (error <= last_error) + { + old_state = (*state); + last_error = error; + for (int c = 0; c < num_cams; c++) { last_error_per_cam[c] = error_per_cam[c]; } + + std::vector cov_per_cam(num_cams, img_point_cov); + if (enable_dynamic_covariance_) + { + for (int c = 0; c < num_cams; c++) { cov_per_cam[c] = calculateCoVarianceScalePerCam(c, prev_avg_error_per_cam_[c], prev_n_meas_per_cam_[c]); } + } + + MatrixXd H_weighted = H_sub; + VectorXd z_weighted = z; + int cam_meas_size = total_points * patch_size_total; + for (int c = 0; c < num_cams; c++) + { + double weight = 1.0 / std::sqrt(cov_per_cam[c]); + int row_start = row_start_per_cam[c]; + int row_end = row_start + cam_meas_size; + for (int row = row_start; row < row_end && row < total_measurements; row++) + { + H_weighted.row(row) *= weight; + z_weighted(row) *= weight; + } + } + + auto H_weighted_T = H_weighted.transpose(); + H_T_H.setZero(); + G.setZero(); + + int calib_dim = 6 + num_cams; + MatrixXd H_T_H_expo(calib_dim, calib_dim); + H_T_H_expo = H_weighted_T * H_weighted; + + auto HTz = H_weighted_T * z_weighted; + auto vec = (*state_propagat) - (*state); + + MD(6, 6) K_pose = (H_T_H_expo.block<6, 6>(0, 0) + state->cov.block<6, 6>(0, 0).inverse()).inverse(); + VectorXd solution_pose = -K_pose * HTz.head<6>() + vec.head<6>(); + + for (int c = 0; c < num_cams; c++) + { + int expo_idx = 6 + c; + double H_ii = H_T_H_expo(expo_idx, expo_idx); + if (H_ii > 1e-6) + { + double expo_before = state->inv_expo_time_per_cam[c]; + double delta_expo = -HTz(expo_idx) / H_ii; + double expo_after = expo_before + delta_expo; + + const double min_inv_expo = 0.001; + const double max_inv_expo = 100.0; + if (expo_after < min_inv_expo) expo_after = min_inv_expo; + else if (expo_after > max_inv_expo) expo_after = max_inv_expo; + + state->inv_expo_time_per_cam[c] = expo_after; + } + } + + MD(DIM_STATE, 1) solution = MD(DIM_STATE, 1)::Zero(); + solution.head<6>() = solution_pose; + (*state) += solution; + + auto rot_add = solution.block<3, 1>(0, 0); + auto t_add = solution.block<3, 1>(3, 0); + + if ((rot_add.norm() * 57.3f < 0.001f) && (t_add.norm() * 100.f < 0.001f)) { EKF_end = true; } + } + else + { + (*state) = old_state; + EKF_end = true; + } + + update_ekf_time += (omp_get_wtime() - t3); + if (iteration == max_iterations - 1 || EKF_end) break; + } + + prev_avg_error_ = last_error; + prev_n_meas_ = n_meas; + for (int c = 0; c < num_cams; c++) + { + if (c < (int)prev_avg_error_per_cam_.size()) + { + prev_avg_error_per_cam_[c] = last_error_per_cam[c]; + prev_n_meas_per_cam_[c] = n_meas_per_cam[c]; + } + } +} + +void VIOManager::updateFrameState(StatesGroup state) +{ + M3D Rwi(state.rot_end); + V3D Pwi(state.pos_end); + if (new_frame_->T_f_w_.size() != cams.size()) { new_frame_->T_f_w_.resize(cams.size(), SE3()); } + for (int cam_idx = 0; cam_idx < (int)cams.size(); cam_idx++) + { + M3D Rcw = Rci_vec[cam_idx] * Rwi.transpose(); + V3D Pcw = -Rci_vec[cam_idx] * Rwi.transpose() * Pwi + Pci_vec[cam_idx]; + new_frame_->T_f_w_[cam_idx] = SE3(Eigen::Quaterniond(Rcw).normalized().toRotationMatrix(), Pcw); // avoid R is not orthogonal + } +} + +void VIOManager::plotTrackedPoints() +{ + if (visual_submap->voxel_points.empty() || imgs_rgb.empty()) return; + int num_cams = std::min(cams.size(), imgs_rgb.size()); + if (num_cams == 0) return; + + int grid_rows, grid_cols; + if (num_cams <= 1) { grid_rows = grid_cols = 1; } + else + { + grid_rows = (int)ceil(sqrt((double)num_cams)); + grid_cols = (int)ceil((double)num_cams / grid_rows); + if ((grid_rows - 1) * (grid_cols + 1) >= num_cams && (grid_rows - 1) > 0 && abs((grid_rows - 1) - (grid_cols + 1)) < abs(grid_rows - grid_cols)) + { + grid_rows--; + grid_cols++; + } + } + + const double SCALE_FACTOR = 0.25; + int img_width = imgs_rgb[0].cols; + int img_height = imgs_rgb[0].rows; + int display_width = static_cast(img_width * SCALE_FACTOR); + int display_height = static_cast(img_height * SCALE_FACTOR); + + cv::Mat display = cv::Mat(grid_rows * display_height, grid_cols * display_width, CV_8UC3, cv::Scalar(0, 0, 0)); + cv::Scalar normal_color(0, 255, 0); + + for (int cam_idx = 0; cam_idx < num_cams; cam_idx++) + { + cv::Mat cam_img_resized; + cv::resize(imgs_rgb[cam_idx], cam_img_resized, cv::Size(display_width, display_height)); + + int row = cam_idx / grid_cols; + int col = cam_idx % grid_cols; + cv::Rect roi(col * display_width, row * display_height, display_width, display_height); + + for (size_t i = 0; i < visual_submap->voxel_points.size(); i++) + { + VisualPoint *pt = visual_submap->voxel_points[i]; + if (!pt) continue; + + int pt_cam_id = visual_submap->camera_ids[i]; + if (pt_cam_id != cam_idx) continue; + + V3D pt_cam = new_frame_->w2f(pt->pos_, cam_idx); + if (pt_cam[2] > 0) + { + V2D pc = new_frame_->w2c(pt->pos_, cam_idx); + int x = static_cast(pc[0] * SCALE_FACTOR); + int y = static_cast(pc[1] * SCALE_FACTOR); + if (x >= 0 && x < display_width && y >= 0 && y < display_height) { cv::circle(cam_img_resized, cv::Point(x, y), 1, normal_color, -1); } + } + } + + cam_img_resized.copyTo(display(roi)); + } + + panorama_image = display; +} + +V3F VIOManager::getInterpolatedPixel(cv::Mat img, V2D pc) +{ + const float u_ref = pc[0]; + const float v_ref = pc[1]; + const int u_ref_i = floorf(pc[0]); + const int v_ref_i = floorf(pc[1]); + const float subpix_u_ref = (u_ref - u_ref_i); + const float subpix_v_ref = (v_ref - v_ref_i); + const float w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref); + const float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref); + const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; + const float w_ref_br = subpix_u_ref * subpix_v_ref; + uint8_t *img_ptr = (uint8_t *)img.data + ((v_ref_i)*width + (u_ref_i)) * 3; + float B = w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[0 + 3] + w_ref_bl * img_ptr[width * 3] + w_ref_br * img_ptr[width * 3 + 0 + 3]; + float G = w_ref_tl * img_ptr[1] + w_ref_tr * img_ptr[1 + 3] + w_ref_bl * img_ptr[1 + width * 3] + w_ref_br * img_ptr[width * 3 + 1 + 3]; + float R = w_ref_tl * img_ptr[2] + w_ref_tr * img_ptr[2 + 3] + w_ref_bl * img_ptr[2 + width * 3] + w_ref_br * img_ptr[width * 3 + 2 + 3]; + V3F pixel(B, G, R); + return pixel; +} + +void VIOManager::dumpDataForColmap() +{ + static int cnt = 1; + std::ostringstream ss; + ss << std::setw(5) << std::setfill('0') << cnt; + std::string cnt_str = ss.str(); + if (cams.empty()) + { + std::cerr << "No cameras available for COLMAP export" << std::endl; + return; + } + for (size_t cam_idx = 0; cam_idx < cams.size(); ++cam_idx) + { + std::string image_path = std::string(ROOT_DIR) + "Log/Colmap/images/" + cnt_str + "_cam" + std::to_string(cam_idx) + ".png"; + if (cam_idx >= imgs_rgb.size() || imgs_rgb[cam_idx].empty()) { continue; } + + cv::Mat img_rgb = imgs_rgb[cam_idx]; + cv::Mat img_rgb_undistort; + vk::PinholeCamera *pinhole_cam_ptr = dynamic_cast(cams[cam_idx]); + if (!pinhole_cam_ptr) { img_rgb_undistort = img_rgb.clone(); } + else { pinhole_cam_ptr->undistortImage(img_rgb, img_rgb_undistort); } + cv::imwrite(image_path, img_rgb_undistort); + } + static bool cameras_written = false; + if (!cameras_written) + { + fout_camera.open(DEBUG_FILE_DIR("Colmap/sparse/0/cameras.txt"), std::ios::out); + fout_camera << "# Camera list with one line of data per camera:\n"; + fout_camera << "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n"; + for (size_t cam_idx = 0; cam_idx < cams.size(); ++cam_idx) + { + fout_camera << (cam_idx + 1) << " PINHOLE " << cams[cam_idx]->width() << " " << cams[cam_idx]->height() << " " << std::fixed << std::setprecision(6) + << cams[cam_idx]->fx() << " " << cams[cam_idx]->fy() << " " << cams[cam_idx]->cx() << " " << cams[cam_idx]->cy() << std::endl; + } + fout_camera.close(); + cameras_written = true; + } + if (!fout_colmap.is_open()) + { + fout_colmap.open(DEBUG_FILE_DIR("Colmap/sparse/0/images.txt"), std::ios::out); + fout_colmap << "# Image list with two lines of data per image:\n"; + fout_colmap << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n"; + fout_colmap << "# POINTS2D[] as (X, Y, POINT3D_ID)\n"; + } + for (size_t cam_idx = 0; cam_idx < cams.size(); ++cam_idx) + { + if (cam_idx >= new_frame_->T_f_w_.size()) continue; + SE3 cam_pose = new_frame_->T_f_w_[cam_idx]; + Eigen::Quaterniond q(cam_pose.rotationMatrix()); + Eigen::Vector3d t = cam_pose.translation(); + int image_id = cnt * 100 + static_cast(cam_idx); + std::string image_name = cnt_str + "_cam" + std::to_string(cam_idx) + ".png"; + fout_colmap << image_id << " " << std::fixed << std::setprecision(6) << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " " << t.x() << " " + << t.y() << " " << t.z() << " " << (cam_idx + 1) << " " << image_name << std::endl; + fout_colmap << "0.0 0.0 -1.0" << std::endl; + } + + cnt++; +} + +void VIOManager::initializeCameraPhotoParams() +{ + camera_photo_params.resize(cams.size()); + for (size_t i = 0; i < cams.size(); i++) + { + camera_photo_params[i].exposure_factor = 1.0; + camera_photo_params[i].vignetting = {0.0, 0.0, 0.0}; + camera_photo_params[i].parameters_initialized = false; + } +} + +float VIOManager::applyCameraPhotoCorrection(float intensity, int cam_id, const V2D &pixel_pos) +{ + if (cam_id < 0 || cam_id >= (int)camera_photo_params.size()) { return intensity; } + float w = cams[cam_id]->width(); + float h = cams[cam_id]->height(); + + float dx = (pixel_pos[0] / w) - 0.5f; + float dy = (pixel_pos[1] / h) - 0.5f; + float r2 = dx * dx + dy * dy; + float vignette_factor = 1.0f; + if (!camera_photo_params[cam_id].vignetting.empty()) + { + vignette_factor = 1.0f + camera_photo_params[cam_id].vignetting[0] * r2 + camera_photo_params[cam_id].vignetting[1] * r2 * r2 + + camera_photo_params[cam_id].vignetting[2] * r2 * r2 * r2; + } + return (intensity * vignette_factor) * camera_photo_params[cam_id].exposure_factor; +} + +void VIOManager::cleanupOldVisualPoints() +{ + if (feat_map.empty()) return; + + int current_frame_id = new_frame_ ? new_frame_->id_ : 0; + + std::vector voxels_to_delete; + voxels_to_delete.reserve(feat_map.size() / 10); + int total_points_before = 0; + int total_points_deleted = 0; + + for (auto &kv : feat_map) + { + VOXEL_POINTS *voxel_pts = kv.second; + if (!voxel_pts) continue; + + auto &points = voxel_pts->voxel_points; + total_points_before += points.size(); + + points.erase(std::remove_if(points.begin(), points.end(), + [this, current_frame_id, &total_points_deleted](VisualPoint *pt) { + if (!pt) return true; + + int last_seen = pt->cross_cam_data_.last_seen_frame_id; + bool too_old = (last_seen > 0) && ((current_frame_id - last_seen) > max_point_age_frames); + + if (pt->is_converged_ && pt->obs_.size() > 5) + { + too_old = (last_seen > 0) && ((current_frame_id - last_seen) > max_point_age_frames * 2); + } + + if (too_old) + { + delete pt; + total_points_deleted++; + return true; + } + return false; + }), + points.end()); + + if (points.empty()) { voxels_to_delete.push_back(kv.first); } + } + + for (auto &loc : voxels_to_delete) + { + auto it = feat_map.find(loc); + if (it != feat_map.end()) + { + delete it->second; + feat_map.erase(it); + } + } + + if (total_points_deleted > 0) + { + std::cout << "[VIO Memory] Cleaned up " << total_points_deleted << " old points (" << (100.0 * total_points_deleted / std::max(1, total_points_before)) + << "%), " << voxels_to_delete.size() << " empty voxels. Remaining: " << (total_points_before - total_points_deleted) << " points in " + << feat_map.size() << " voxels" << std::endl; + } + + last_cleanup_frame_id = current_frame_id; +} + +void VIOManager::cleanupVisualMapByTimestamp(double oldest_kept_timestamp) +{ + if (feat_map.empty() || oldest_kept_timestamp < 0) { return; } + + int total_voxels_before = feat_map.size(); + int deleted_voxels = 0; + + for (auto it = feat_map.begin(); it != feat_map.end();) + { + if (!it->second) + { + it = feat_map.erase(it); + continue; + } + + double voxel_timestamp = it->second->creation_timestamp_; + if (voxel_timestamp < 0) + { + ++it; + continue; + } + + if (voxel_timestamp < oldest_kept_timestamp) + { + delete it->second; + it = feat_map.erase(it); + deleted_voxels++; + } + else { ++it; } + } + + if (deleted_voxels > 0) + { + std::cout << "[VIO Sliding] Deleted " << deleted_voxels << " visual voxels (" << (100.0 * deleted_voxels / std::max(1, total_voxels_before)) + << "%), remaining: " << feat_map.size() << " voxels" << std::endl; + } +} + +void VIOManager::cleanupOldFrames() +{ + while ((int)frame_history_.size() > max_frame_history) + { + FramePtr old_frame = frame_history_.front(); + frame_history_.pop_front(); + if (old_frame) { old_frame->imgs_shared_.clear(); } + } +} + +void VIOManager::addCrossCameraConsistencyConstraint(VisualPoint *pt, int source_cam_id, int target_cam_id, MatrixXd &H_sub, VectorXd &z, int level, + int &row_offset) +{ + if (!pt || !enable_cross_camera_tracking || source_cam_id < 0 || source_cam_id >= (int)cams.size() || target_cam_id < 0 || + target_cam_id >= (int)cams.size() || source_cam_id == target_cam_id) + { + return; + } + + if (source_cam_id >= (int)new_frame_->imgs_.size() || target_cam_id >= (int)new_frame_->imgs_.size() || new_frame_->imgs_[source_cam_id].empty() || + new_frame_->imgs_[target_cam_id].empty()) + { + return; + } + + V2D source_px = new_frame_->w2c(pt->pos_, source_cam_id); + V2D target_px = new_frame_->w2c(pt->pos_, target_cam_id); + + if (!cams[source_cam_id]->isInFrame(source_px.cast(), border) || !cams[target_cam_id]->isInFrame(target_px.cast(), border)) { return; } + + float source_patch[256]; + float target_patch[256]; + memset(source_patch, 0, sizeof(source_patch)); + memset(target_patch, 0, sizeof(target_patch)); + + getImagePatch(new_frame_->imgs_[source_cam_id], source_px, source_patch, level); + getImagePatch(new_frame_->imgs_[target_cam_id], target_px, target_patch, level); + + if (source_cam_id >= (int)camera_photo_params.size() || target_cam_id >= (int)camera_photo_params.size()) { return; } + + float mean_source = 0, mean_target = 0; + for (int i = 0; i < patch_size_total; i++) + { + mean_source += source_patch[i]; + mean_target += target_patch[i]; + } + mean_source /= patch_size_total; + mean_target /= patch_size_total; + + // Slowly adapt each non-reference camera's exposure_factor toward camera 0 + // (treated as the exposure reference) whenever both patches are + // well-exposed, so applyCameraPhotoCorrection can normalize brightness + // across cameras for the cross-camera residual below. + if (mean_source > 20 && mean_source < 235 && mean_target > 20 && mean_target < 235) + { + double ratio = mean_target / mean_source; + double alpha = 0.005; + + if (source_cam_id == 0 && target_cam_id > 0) + { + camera_photo_params[target_cam_id].exposure_factor = (1 - alpha) * camera_photo_params[target_cam_id].exposure_factor + alpha * ratio; + } + else if (target_cam_id == 0 && source_cam_id > 0) + { + camera_photo_params[source_cam_id].exposure_factor = (1 - alpha) * camera_photo_params[source_cam_id].exposure_factor + alpha / ratio; + } + } + + double source_to_target_factor = camera_photo_params[target_cam_id].exposure_factor / camera_photo_params[source_cam_id].exposure_factor; + + double source_inv_expo = (source_cam_id < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[source_cam_id] : 1.0; + double target_inv_expo = (target_cam_id < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[target_cam_id] : 1.0; + + V3D p_source = new_frame_->w2f(pt->pos_, source_cam_id); + V3D p_target = new_frame_->w2f(pt->pos_, target_cam_id); + if (p_source.z() < 1e-6 || p_target.z() < 1e-6) { return; } + + MD(2, 3) Jdpi_source, Jdpi_target; + computeProjectionJacobian(source_cam_id, p_source, Jdpi_source); + computeProjectionJacobian(target_cam_id, p_target, Jdpi_target); + + M3D p_hat_source, p_hat_target; + p_hat_source << SKEW_SYM_MATRX(p_source); + p_hat_target << SKEW_SYM_MATRX(p_target); + + float scale = 1 << level; + float inv_scale = 1.0f / scale; + + cv::Mat &source_img = new_frame_->imgs_[source_cam_id]; + cv::Mat &target_img = new_frame_->imgs_[target_cam_id]; + + for (int px_y = 0; px_y < patch_size; px_y++) + { + for (int px_x = 0; px_x < patch_size; px_x++) + { + int patch_idx = px_y * patch_size + px_x; + + float source_intensity, target_intensity; + if (exposure_estimate_en) + { + source_intensity = source_inv_expo * source_patch[patch_idx]; + target_intensity = target_inv_expo * target_patch[patch_idx]; + } + else + { + source_intensity = source_patch[patch_idx]; + target_intensity = target_patch[patch_idx] / source_to_target_factor; + } + float residual = source_intensity - target_intensity; + float abs_res = std::abs(residual); + + float weight = (abs_res <= outlier_threshold) + ? (1.0f - (residual / outlier_threshold) * (residual / outlier_threshold)) * + (1.0f - (residual / outlier_threshold) * (residual / outlier_threshold)) + : 0.0f; + float sqrt_weight = std::sqrt(weight); + + int u_source = std::floor(source_px[0]); + int v_source = std::floor(source_px[1]); + float du_source = 0, dv_source = 0; + if (u_source - 1 >= 0 && u_source + 1 < source_img.cols && v_source - 1 >= 0 && v_source + 1 < source_img.rows) + { + du_source = (float)(source_img.at(v_source, u_source + 1) - source_img.at(v_source, u_source - 1)) * 0.5f; + dv_source = (float)(source_img.at(v_source + 1, u_source) - source_img.at(v_source - 1, u_source)) * 0.5f; + } + + int u_target = std::floor(target_px[0]); + int v_target = std::floor(target_px[1]); + float du_target = 0, dv_target = 0; + if (u_target - 1 >= 0 && u_target + 1 < target_img.cols && v_target - 1 >= 0 && v_target + 1 < target_img.rows) + { + du_target = (float)(target_img.at(v_target, u_target + 1) - target_img.at(v_target, u_target - 1)) * 0.5f; + dv_target = (float)(target_img.at(v_target + 1, u_target) - target_img.at(v_target - 1, u_target)) * 0.5f; + } + + MD(1, 2) Jimg_source; + Jimg_source << du_source, dv_source; + Jimg_source *= (float)source_inv_expo; + Jimg_source *= inv_scale; + + MD(1, 3) Jdphi_source = Jimg_source * Jdpi_source * p_hat_source; + MD(1, 3) Jdp_source = -Jimg_source * Jdpi_source; + MD(1, 3) JdR_source = Jdphi_source * Jdphi_dR_vec[source_cam_id] + Jdp_source * Jdp_dR_vec[source_cam_id]; + MD(1, 3) Jdt_source = Jdp_source * Jdp_dt_vec[source_cam_id]; + + MD(1, 2) Jimg_target; + Jimg_target << du_target, dv_target; + Jimg_target *= (float)target_inv_expo; + Jimg_target *= inv_scale; + + MD(1, 3) Jdphi_target = Jimg_target * Jdpi_target * p_hat_target; + MD(1, 3) Jdp_target = -Jimg_target * Jdpi_target; + MD(1, 3) JdR_target = Jdphi_target * Jdphi_dR_vec[target_cam_id] + Jdp_target * Jdp_dR_vec[target_cam_id]; + MD(1, 3) Jdt_target = Jdp_target * Jdp_dt_vec[target_cam_id]; + + MD(1, 3) JdR_coupled = JdR_source - JdR_target; + MD(1, 3) Jdt_coupled = Jdt_source - Jdt_target; + + int curr_row = row_offset + patch_idx; + if (curr_row >= H_sub.rows() || curr_row >= z.size()) continue; + + H_sub(curr_row, 0) = sqrt_weight * JdR_coupled(0); + H_sub(curr_row, 1) = sqrt_weight * JdR_coupled(1); + H_sub(curr_row, 2) = sqrt_weight * JdR_coupled(2); + H_sub(curr_row, 3) = sqrt_weight * Jdt_coupled(0); + H_sub(curr_row, 4) = sqrt_weight * Jdt_coupled(1); + H_sub(curr_row, 5) = sqrt_weight * Jdt_coupled(2); + + if (exposure_estimate_en) + { + float expo_jac_source = source_patch[patch_idx]; + float expo_jac_target = -target_patch[patch_idx]; + + int source_expo_col = 6 + source_cam_id; + int target_expo_col = 6 + target_cam_id; + H_sub(curr_row, source_expo_col) = sqrt_weight * expo_jac_source; + H_sub(curr_row, target_expo_col) = sqrt_weight * expo_jac_target; + } + z(curr_row) = sqrt_weight * residual; + } + } + + row_offset += patch_size_total; +} + +void VIOManager::processFrame(const std::vector &imgs, vector &pg, const unordered_map &feat_map, + double frame_timestamp) +{ + current_timestamp_ = frame_timestamp; + + if (imgs.size() != cams.size()) + { + std::cerr << "[ VIO ] Error: input imgs.size() != cams.size(). is " << imgs.size() << " " << cams.size() << std::endl; + return; + } + imgs_cp.resize(cams.size()); + imgs_rgb.resize(cams.size()); + for (size_t i = 0; i < cams.size(); i++) + { + if (imgs[i].empty()) + { + std::cout << "Camera " << i << " image is empty, skipping." << std::endl; + continue; + } + if (width != imgs[i].cols || height != imgs[i].rows) + { + cv::Mat resized; + cv::resize(imgs[i], resized, cv::Size(width, height), 0, 0, cv::INTER_LINEAR); + imgs_cp[i] = resized.clone(); + imgs_rgb[i] = resized.clone(); + } + else + { + imgs_cp[i] = imgs[i].clone(); + imgs_rgb[i] = imgs[i].clone(); + } + if (imgs_cp[i].channels() == 3) { cv::cvtColor(imgs_cp[i], imgs_cp[i], cv::COLOR_BGR2GRAY); } + } + + new_frame_.reset(new Frame(cams, imgs_cp, frame_timestamp)); + updateFrameState(*state); + + // Omni-LIVO declares/implements resetGrid() but never calls it anywhere in + // its own processFrame() — an upstream bug that leaves total_points (and + // the grid/map_dist/etc. buffers) uninitialized garbage on frames where + // retrieveFromVisualSparseMap() early-returns (feat_map still empty, e.g. + // the very first VIO frame of a run), causing an out-of-bounds access into + // visual_submap->voxel_points downstream. The original single-camera + // FAST-LIVO2 code called resetGrid() here; restored for multi-camera too. + resetGrid(); + + frame_history_.push_back(new_frame_); + cleanupOldFrames(); + + double t1 = omp_get_wtime(); + retrieveFromVisualSparseMap(imgs_cp, pg, feat_map); + double t2 = omp_get_wtime(); + computeJacobianAndUpdateEKF(imgs_cp); + double t3 = omp_get_wtime(); + + generateVisualMapPoints(imgs_cp, pg); + double t4 = omp_get_wtime(); + plotTrackedPoints(); + if (plot_flag) projectPatchFromRefToCur(feat_map); + double t5 = omp_get_wtime(); + updateVisualMapPoints(imgs_cp); + double t6 = omp_get_wtime(); + updateReferencePatch(feat_map); + + if (new_frame_ && (new_frame_->id_ - last_cleanup_frame_id) >= cleanup_interval_frames) { cleanupOldVisualPoints(); } + + double t7 = omp_get_wtime(); + + if (colmap_output_en) dumpDataForColmap(); + + frame_count++; + ave_total = ave_total * (frame_count - 1) / frame_count + (t7 - t1 - (t5 - t4)) / frame_count; + + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| VIO Time (MultiCam) |\033[0m\n"); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| %-29s | %-27zu |\033[0m\n", "Sparse Map Size", feat_map.size()); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| %-29s | %-27s |\033[0m\n", "Algorithm Stage", "Time (secs)"); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "retrieveFromVisualSparseMap", t2 - t1); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "computeJacobianAndUpdateEKF", t3 - t2); + printf("\033[1;32m| %-27s | %-27lf |\033[0m\n", "-> computeJacobian", compute_jacobian_time); + printf("\033[1;32m| %-27s | %-27lf |\033[0m\n", "-> updateEKF", update_ekf_time); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "generateVisualMapPoints", t4 - t3); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "updateVisualMapPoints", t6 - t5); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "updateReferencePatch", t7 - t6); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "Current Total Time", t7 - t1 - (t5 - t4)); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "Average Total Time", ave_total); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); +} diff --git a/src/FAST-LIVO2/src/visual_point.cpp b/src/FAST-LIVO2/src/visual_point.cpp new file mode 100644 index 0000000..64efcba --- /dev/null +++ b/src/FAST-LIVO2/src/visual_point.cpp @@ -0,0 +1,158 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#include "visual_point.h" +#include "feature.h" +#include +#include + +VisualPoint::VisualPoint(const Vector3d &pos) + : pos_(pos), previous_normal_(Vector3d::Zero()), normal_(Vector3d::Zero()), + is_converged_(false), is_normal_initialized_(false), has_ref_patch_(false), + cache_frame_id_(-1) +{ + visible_cameras_cache_.reset(); +} + +VisualPoint::~VisualPoint() +{ + for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) + { + delete(*it); + } + obs_.clear(); + ref_patch = nullptr; +} + +void VisualPoint::addFrameRef(Feature *ftr) +{ + obs_.push_front(ftr); + + // Bound obs_ growth: drop the oldest non-reference observation once the + // cap is exceeded (never delete ref_patch itself). + if (obs_.size() > MAX_OBS) + { + Feature *old_ftr = obs_.back(); + if (old_ftr != ref_patch) + { + delete old_ftr; + obs_.pop_back(); + } + else + { + auto it = obs_.end(); + --it; + if (it != obs_.begin()) + { + --it; + delete (*it); + obs_.erase(it); + } + } + } +} + +void VisualPoint::deleteFeatureRef(Feature *ftr) +{ + if (ref_patch == ftr) + { + ref_patch = nullptr; + has_ref_patch_ = false; + } + for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) + { + if ((*it) == ftr) + { + delete((*it)); + obs_.erase(it); + return; + } + } +} + +bool VisualPoint::getCloseViewObs(const Vector3d &framepos, Feature *&ftr, const Vector2d &cur_px) const +{ + // TODO: get frame with same point of view AND same pyramid level! + if (obs_.size() <= 0) return false; + + Vector3d obs_dir(framepos - pos_); + obs_dir.normalize(); + auto min_it = obs_.begin(); + double min_cos_angle = 0; + for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) + { + Vector3d dir((*it)->T_f_w_.inverse().translation() - pos_); + dir.normalize(); + double cos_angle = obs_dir.dot(dir); + if (cos_angle > min_cos_angle) + { + min_cos_angle = cos_angle; + min_it = it; + } + } + ftr = *min_it; + + // Vector2d ftr_px = ftr->px_; + // double pixel_dist = (cur_px-ftr_px).norm(); + + // if(pixel_dist > 200) + // { + // ROS_ERROR("The pixel dist exceeds 200."); + // return false; + // } + + if (min_cos_angle < 0.5) // assume that observations larger than 60° are useless 0.5 + { + // ROS_ERROR("The obseved angle is larger than 60°."); + return false; + } + + return true; +} + +void VisualPoint::findMinScoreFeature(const Vector3d &framepos, Feature *&ftr) const +{ + if (obs_.empty()) + { + ftr = nullptr; + return; + } + + auto min_it = obs_.begin(); + float min_score = std::numeric_limits::max(); + + for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) + { + if ((*it)->score_ < min_score) + { + min_score = (*it)->score_; + min_it = it; + } + } + ftr = *min_it; +} + +void VisualPoint::deleteNonRefPatchFeatures() +{ + for (auto it = obs_.begin(); it != obs_.end();) + { + if (*it != ref_patch) + { + delete *it; + it = obs_.erase(it); + } + else + { + ++it; + } + } +} \ No newline at end of file diff --git a/src/FAST-LIVO2/src/voxel_map.cpp b/src/FAST-LIVO2/src/voxel_map.cpp new file mode 100644 index 0000000..731222c --- /dev/null +++ b/src/FAST-LIVO2/src/voxel_map.cpp @@ -0,0 +1,987 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +This file is subject to the terms and conditions outlined in the 'LICENSE' file, +which is included as part of this source code package. +*/ + +#include "voxel_map.h" +using namespace Eigen; +void calcBodyCov(Eigen::Vector3d &pb, const float range_inc, const float degree_inc, Eigen::Matrix3d &cov) +{ + if (pb[2] == 0) pb[2] = 0.0001; + float range = sqrt(pb[0] * pb[0] + pb[1] * pb[1] + pb[2] * pb[2]); + float range_var = range_inc * range_inc; + Eigen::Matrix2d direction_var; + direction_var << pow(sin(DEG2RAD(degree_inc)), 2), 0, 0, pow(sin(DEG2RAD(degree_inc)), 2); + Eigen::Vector3d direction(pb); + direction.normalize(); + Eigen::Matrix3d direction_hat; + direction_hat << 0, -direction(2), direction(1), direction(2), 0, -direction(0), -direction(1), direction(0), 0; + Eigen::Vector3d base_vector1(1, 1, -(direction(0) + direction(1)) / direction(2)); + base_vector1.normalize(); + Eigen::Vector3d base_vector2 = base_vector1.cross(direction); + base_vector2.normalize(); + Eigen::Matrix N; + N << base_vector1(0), base_vector2(0), base_vector1(1), base_vector2(1), base_vector1(2), base_vector2(2); + Eigen::Matrix A = range * direction_hat * N; + cov = direction * range_var * direction.transpose() + A * direction_var * A.transpose(); +} + +void loadVoxelConfig(rclcpp::Node::SharedPtr &node, VoxelMapConfig &voxel_config) +{ + // declare parameter + node->declare_parameter("publish.pub_plane_en", false); + node->declare_parameter("lio.max_layer", 1); + node->declare_parameter("lio.voxel_size", 0.5); + node->declare_parameter("lio.min_eigen_value", 0.01); + node->declare_parameter("lio.sigma_num", 3); + node->declare_parameter("lio.beam_err", 0.02); + node->declare_parameter("lio.dept_err", 0.05); + + // Declaration of parameter of type std::vector won't build, https://github.com/ros2/rclcpp/issues/1585 + node->declare_parameter>("lio.layer_init_num", std::vector{5,5,5,5,5}); + node->declare_parameter("lio.max_points_num", 50); + node->declare_parameter("lio.min_iterations", 5); + node->declare_parameter("local_map.map_sliding_en", false); + node->declare_parameter("local_map.half_map_size", 100); + node->declare_parameter("local_map.sliding_thresh", 8.0); + + // get parameter + node->get_parameter("publish.pub_plane_en", voxel_config.is_pub_plane_map_); + node->get_parameter("lio.max_layer", voxel_config.max_layer_); + node->get_parameter("lio.voxel_size", voxel_config.max_voxel_size_); + node->get_parameter("lio.min_eigen_value", voxel_config.planner_threshold_); + node->get_parameter("lio.sigma_num", voxel_config.sigma_num_); + node->get_parameter("lio.beam_err", voxel_config.beam_err_); + node->get_parameter("lio.dept_err", voxel_config.dept_err_); + node->get_parameter("lio.layer_init_num", voxel_config.layer_init_num_); + node->get_parameter("lio.max_points_num", voxel_config.max_points_num_); + node->get_parameter("lio.min_iterations", voxel_config.max_iterations_); + node->get_parameter("local_map.map_sliding_en", voxel_config.map_sliding_en); + node->get_parameter("local_map.half_map_size", voxel_config.half_map_size); + node->get_parameter("local_map.sliding_thresh", voxel_config.sliding_thresh); +} + +void VoxelOctoTree::init_plane(const std::vector &points, VoxelPlane *plane) +{ + plane->plane_var_ = Eigen::Matrix::Zero(); + plane->covariance_ = Eigen::Matrix3d::Zero(); + plane->center_ = Eigen::Vector3d::Zero(); + plane->normal_ = Eigen::Vector3d::Zero(); + plane->points_size_ = points.size(); + plane->radius_ = 0; + for (auto pv : points) + { + plane->covariance_ += pv.point_w * pv.point_w.transpose(); + plane->center_ += pv.point_w; + } + plane->center_ = plane->center_ / plane->points_size_; + plane->covariance_ = plane->covariance_ / plane->points_size_ - plane->center_ * plane->center_.transpose(); + Eigen::EigenSolver es(plane->covariance_); + Eigen::Matrix3cd evecs = es.eigenvectors(); + Eigen::Vector3cd evals = es.eigenvalues(); + Eigen::Vector3d evalsReal; + evalsReal = evals.real(); + Eigen::Matrix3f::Index evalsMin, evalsMax; + evalsReal.rowwise().sum().minCoeff(&evalsMin); + evalsReal.rowwise().sum().maxCoeff(&evalsMax); + int evalsMid = 3 - evalsMin - evalsMax; + Eigen::Vector3d evecMin = evecs.real().col(evalsMin); + Eigen::Vector3d evecMid = evecs.real().col(evalsMid); + Eigen::Vector3d evecMax = evecs.real().col(evalsMax); + Eigen::Matrix3d J_Q; + J_Q << 1.0 / plane->points_size_, 0, 0, 0, 1.0 / plane->points_size_, 0, 0, 0, 1.0 / plane->points_size_; + // && evalsReal(evalsMid) > 0.05 + //&& evalsReal(evalsMid) > 0.01 + if (evalsReal(evalsMin) < planer_threshold_) + { + for (int i = 0; i < points.size(); i++) + { + Eigen::Matrix J; + Eigen::Matrix3d F; + for (int m = 0; m < 3; m++) + { + if (m != (int)evalsMin) + { + Eigen::Matrix F_m = + (points[i].point_w - plane->center_).transpose() / ((plane->points_size_) * (evalsReal[evalsMin] - evalsReal[m])) * + (evecs.real().col(m) * evecs.real().col(evalsMin).transpose() + evecs.real().col(evalsMin) * evecs.real().col(m).transpose()); + F.row(m) = F_m; + } + else + { + Eigen::Matrix F_m; + F_m << 0, 0, 0; + F.row(m) = F_m; + } + } + J.block<3, 3>(0, 0) = evecs.real() * F; + J.block<3, 3>(3, 0) = J_Q; + plane->plane_var_ += J * points[i].var * J.transpose(); + } + + plane->normal_ << evecs.real()(0, evalsMin), evecs.real()(1, evalsMin), evecs.real()(2, evalsMin); + plane->y_normal_ << evecs.real()(0, evalsMid), evecs.real()(1, evalsMid), evecs.real()(2, evalsMid); + plane->x_normal_ << evecs.real()(0, evalsMax), evecs.real()(1, evalsMax), evecs.real()(2, evalsMax); + plane->min_eigen_value_ = evalsReal(evalsMin); + plane->mid_eigen_value_ = evalsReal(evalsMid); + plane->max_eigen_value_ = evalsReal(evalsMax); + plane->radius_ = sqrt(evalsReal(evalsMax)); + plane->d_ = -(plane->normal_(0) * plane->center_(0) + plane->normal_(1) * plane->center_(1) + plane->normal_(2) * plane->center_(2)); + plane->is_plane_ = true; + plane->is_update_ = true; + if (!plane->is_init_) + { + plane->id_ = voxel_plane_id; + voxel_plane_id++; + plane->is_init_ = true; + } + } + else + { + plane->is_update_ = true; + plane->is_plane_ = false; + } +} + +void VoxelOctoTree::init_octo_tree() +{ + if (temp_points_.size() > points_size_threshold_) + { + init_plane(temp_points_, plane_ptr_); + if (plane_ptr_->is_plane_ == true) + { + octo_state_ = 0; + // new added + if (temp_points_.size() > max_points_num_) + { + update_enable_ = false; + std::vector().swap(temp_points_); + new_points_ = 0; + } + } + else + { + octo_state_ = 1; + cut_octo_tree(); + } + init_octo_ = true; + new_points_ = 0; + } +} + +void VoxelOctoTree::cut_octo_tree() +{ + if (layer_ >= max_layer_) + { + octo_state_ = 0; + return; + } + for (size_t i = 0; i < temp_points_.size(); i++) + { + int xyz[3] = {0, 0, 0}; + if (temp_points_[i].point_w[0] > voxel_center_[0]) { xyz[0] = 1; } + if (temp_points_[i].point_w[1] > voxel_center_[1]) { xyz[1] = 1; } + if (temp_points_[i].point_w[2] > voxel_center_[2]) { xyz[2] = 1; } + int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2]; + if (leaves_[leafnum] == nullptr) + { + leaves_[leafnum] = new VoxelOctoTree(max_layer_, layer_ + 1, layer_init_num_[layer_ + 1], max_points_num_, planer_threshold_); + leaves_[leafnum]->layer_init_num_ = layer_init_num_; + leaves_[leafnum]->voxel_center_[0] = voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[1] = voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[2] = voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_; + leaves_[leafnum]->quater_length_ = quater_length_ / 2; + } + leaves_[leafnum]->temp_points_.push_back(temp_points_[i]); + leaves_[leafnum]->new_points_++; + } + for (uint i = 0; i < 8; i++) + { + if (leaves_[i] != nullptr) + { + if (leaves_[i]->temp_points_.size() > leaves_[i]->points_size_threshold_) + { + init_plane(leaves_[i]->temp_points_, leaves_[i]->plane_ptr_); + if (leaves_[i]->plane_ptr_->is_plane_) + { + leaves_[i]->octo_state_ = 0; + // new added + if (leaves_[i]->temp_points_.size() > leaves_[i]->max_points_num_) + { + leaves_[i]->update_enable_ = false; + std::vector().swap(leaves_[i]->temp_points_); + new_points_ = 0; + } + } + else + { + leaves_[i]->octo_state_ = 1; + leaves_[i]->cut_octo_tree(); + } + leaves_[i]->init_octo_ = true; + leaves_[i]->new_points_ = 0; + } + } + } +} + +void VoxelOctoTree::UpdateOctoTree(const pointWithVar &pv) +{ + if (!init_octo_) + { + new_points_++; + temp_points_.push_back(pv); + if (temp_points_.size() > points_size_threshold_) { init_octo_tree(); } + } + else + { + if (plane_ptr_->is_plane_) + { + if (update_enable_) + { + new_points_++; + temp_points_.push_back(pv); + if (new_points_ > update_size_threshold_) + { + init_plane(temp_points_, plane_ptr_); + new_points_ = 0; + } + if (temp_points_.size() >= max_points_num_) + { + update_enable_ = false; + std::vector().swap(temp_points_); + new_points_ = 0; + } + } + } + else + { + if (layer_ < max_layer_) + { + int xyz[3] = {0, 0, 0}; + if (pv.point_w[0] > voxel_center_[0]) { xyz[0] = 1; } + if (pv.point_w[1] > voxel_center_[1]) { xyz[1] = 1; } + if (pv.point_w[2] > voxel_center_[2]) { xyz[2] = 1; } + int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2]; + if (leaves_[leafnum] != nullptr) { leaves_[leafnum]->UpdateOctoTree(pv); } + else + { + leaves_[leafnum] = new VoxelOctoTree(max_layer_, layer_ + 1, layer_init_num_[layer_ + 1], max_points_num_, planer_threshold_); + leaves_[leafnum]->layer_init_num_ = layer_init_num_; + leaves_[leafnum]->voxel_center_[0] = voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[1] = voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[2] = voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_; + leaves_[leafnum]->quater_length_ = quater_length_ / 2; + leaves_[leafnum]->UpdateOctoTree(pv); + } + } + else + { + if (update_enable_) + { + new_points_++; + temp_points_.push_back(pv); + if (new_points_ > update_size_threshold_) + { + init_plane(temp_points_, plane_ptr_); + new_points_ = 0; + } + if (temp_points_.size() > max_points_num_) + { + update_enable_ = false; + std::vector().swap(temp_points_); + new_points_ = 0; + } + } + } + } + } +} + +VoxelOctoTree *VoxelOctoTree::find_correspond(Eigen::Vector3d pw) +{ + if (!init_octo_ || plane_ptr_->is_plane_ || (layer_ >= max_layer_)) return this; + + int xyz[3] = {0, 0, 0}; + xyz[0] = pw[0] > voxel_center_[0] ? 1 : 0; + xyz[1] = pw[1] > voxel_center_[1] ? 1 : 0; + xyz[2] = pw[2] > voxel_center_[2] ? 1 : 0; + int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2]; + + // printf("leafnum: %d. \n", leafnum); + + return (leaves_[leafnum] != nullptr) ? leaves_[leafnum]->find_correspond(pw) : this; +} + +VoxelOctoTree *VoxelOctoTree::Insert(const pointWithVar &pv) +{ + if ((!init_octo_) || (init_octo_ && plane_ptr_->is_plane_) || (init_octo_ && (!plane_ptr_->is_plane_) && (layer_ >= max_layer_))) + { + new_points_++; + temp_points_.push_back(pv); + return this; + } + + if (init_octo_ && (!plane_ptr_->is_plane_) && (layer_ < max_layer_)) + { + int xyz[3] = {0, 0, 0}; + xyz[0] = pv.point_w[0] > voxel_center_[0] ? 1 : 0; + xyz[1] = pv.point_w[1] > voxel_center_[1] ? 1 : 0; + xyz[2] = pv.point_w[2] > voxel_center_[2] ? 1 : 0; + int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2]; + if (leaves_[leafnum] != nullptr) { return leaves_[leafnum]->Insert(pv); } + else + { + leaves_[leafnum] = new VoxelOctoTree(max_layer_, layer_ + 1, layer_init_num_[layer_ + 1], max_points_num_, planer_threshold_); + leaves_[leafnum]->layer_init_num_ = layer_init_num_; + leaves_[leafnum]->voxel_center_[0] = voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[1] = voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_; + leaves_[leafnum]->voxel_center_[2] = voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_; + leaves_[leafnum]->quater_length_ = quater_length_ / 2; + return leaves_[leafnum]->Insert(pv); + } + } + return nullptr; +} + +void VoxelMapManager::StateEstimation(StatesGroup &state_propagat) +{ + cross_mat_list_.clear(); + cross_mat_list_.reserve(feats_down_size_); + body_cov_list_.clear(); + body_cov_list_.reserve(feats_down_size_); + + // build_residual_time = 0.0; + // ekf_time = 0.0; + // double t0 = omp_get_wtime(); + + for (size_t i = 0; i < feats_down_body_->size(); i++) + { + V3D point_this(feats_down_body_->points[i].x, feats_down_body_->points[i].y, feats_down_body_->points[i].z); + if (point_this[2] == 0) { point_this[2] = 0.001; } + M3D var; + calcBodyCov(point_this, config_setting_.dept_err_, config_setting_.beam_err_, var); + body_cov_list_.push_back(var); + point_this = extR_ * point_this + extT_; + M3D point_crossmat; + point_crossmat << SKEW_SYM_MATRX(point_this); + cross_mat_list_.push_back(point_crossmat); + } + + vector().swap(pv_list_); + pv_list_.resize(feats_down_size_); + + int rematch_num = 0; + MD(DIM_STATE, DIM_STATE) G, H_T_H, I_STATE; + G.setZero(); + H_T_H.setZero(); + I_STATE.setIdentity(); + + bool flg_EKF_inited, flg_EKF_converged, EKF_stop_flg = 0; + for (int iterCount = 0; iterCount < config_setting_.max_iterations_; iterCount++) + { + double total_residual = 0.0; + pcl::PointCloud::Ptr world_lidar(new pcl::PointCloud); + TransformLidar(state_.rot_end, state_.pos_end, feats_down_body_, world_lidar); + M3D rot_var = state_.cov.block<3, 3>(0, 0); + M3D t_var = state_.cov.block<3, 3>(3, 3); + for (size_t i = 0; i < feats_down_body_->size(); i++) + { + pointWithVar &pv = pv_list_[i]; + pv.point_b << feats_down_body_->points[i].x, feats_down_body_->points[i].y, feats_down_body_->points[i].z; + pv.point_w << world_lidar->points[i].x, world_lidar->points[i].y, world_lidar->points[i].z; + + M3D cov = body_cov_list_[i]; + M3D point_crossmat = cross_mat_list_[i]; + cov = state_.rot_end * cov * state_.rot_end.transpose() + (-point_crossmat) * rot_var * (-point_crossmat.transpose()) + t_var; + pv.var = cov; + pv.body_var = body_cov_list_[i]; + } + ptpl_list_.clear(); + + // double t1 = omp_get_wtime(); + + BuildResidualListOMP(pv_list_, ptpl_list_); + + // build_residual_time += omp_get_wtime() - t1; + + for (int i = 0; i < ptpl_list_.size(); i++) + { + total_residual += fabs(ptpl_list_[i].dis_to_plane_); + } + effct_feat_num_ = ptpl_list_.size(); + cout << "[ LIO ] Raw feature num: " << feats_undistort_->size() << ", downsampled feature num:" << feats_down_size_ + << " effective feature num: " << effct_feat_num_ << " average residual: " << total_residual / effct_feat_num_ << endl; + + /*** Computation of Measuremnt Jacobian matrix H and measurents covarience + * ***/ + MatrixXd Hsub(effct_feat_num_, 6); + MatrixXd Hsub_T_R_inv(6, effct_feat_num_); + VectorXd R_inv(effct_feat_num_); + VectorXd meas_vec(effct_feat_num_); + meas_vec.setZero(); + for (int i = 0; i < effct_feat_num_; i++) + { + auto &ptpl = ptpl_list_[i]; + V3D point_this(ptpl.point_b_); + point_this = extR_ * point_this + extT_; + V3D point_body(ptpl.point_b_); + M3D point_crossmat; + point_crossmat << SKEW_SYM_MATRX(point_this); + + /*** get the normal vector of closest surface/corner ***/ + + V3D point_world = state_propagat.rot_end * point_this + state_propagat.pos_end; + Eigen::Matrix J_nq; + J_nq.block<1, 3>(0, 0) = point_world - ptpl_list_[i].center_; + J_nq.block<1, 3>(0, 3) = -ptpl_list_[i].normal_; + + M3D var; + // V3D normal_b = state_.rot_end.inverse() * ptpl_list_[i].normal_; + // V3D point_b = ptpl_list_[i].point_b_; + // double cos_theta = fabs(normal_b.dot(point_b) / point_b.norm()); + // ptpl_list_[i].body_cov_ = ptpl_list_[i].body_cov_ * (1.0 / cos_theta) * (1.0 / cos_theta); + + // point_w cov + // var = state_propagat.rot_end * extR_ * ptpl_list_[i].body_cov_ * (state_propagat.rot_end * extR_).transpose() + + // state_propagat.cov.block<3, 3>(3, 3) + (-point_crossmat) * state_propagat.cov.block<3, 3>(0, 0) * (-point_crossmat).transpose(); + + // point_w cov (another_version) + // var = state_propagat.rot_end * extR_ * ptpl_list_[i].body_cov_ * (state_propagat.rot_end * extR_).transpose() + + // state_propagat.cov.block<3, 3>(3, 3) - point_crossmat * state_propagat.cov.block<3, 3>(0, 0) * point_crossmat; + + // point_body cov + var = state_propagat.rot_end * extR_ * ptpl_list_[i].body_cov_ * (state_propagat.rot_end * extR_).transpose(); + + double sigma_l = J_nq * ptpl_list_[i].plane_var_ * J_nq.transpose(); + + R_inv(i) = 1.0 / (0.001 + sigma_l + ptpl_list_[i].normal_.transpose() * var * ptpl_list_[i].normal_); + // R_inv(i) = 1.0 / (sigma_l + ptpl_list_[i].normal_.transpose() * var * ptpl_list_[i].normal_); + + /*** calculate the Measuremnt Jacobian matrix H ***/ + V3D A(point_crossmat * state_.rot_end.transpose() * ptpl_list_[i].normal_); + Hsub.row(i) << VEC_FROM_ARRAY(A), ptpl_list_[i].normal_[0], ptpl_list_[i].normal_[1], ptpl_list_[i].normal_[2]; + Hsub_T_R_inv.col(i) << A[0] * R_inv(i), A[1] * R_inv(i), A[2] * R_inv(i), ptpl_list_[i].normal_[0] * R_inv(i), + ptpl_list_[i].normal_[1] * R_inv(i), ptpl_list_[i].normal_[2] * R_inv(i); + meas_vec(i) = -ptpl_list_[i].dis_to_plane_; + } + EKF_stop_flg = false; + flg_EKF_converged = false; + /*** Iterative Kalman Filter Update ***/ + MatrixXd K(DIM_STATE, effct_feat_num_); + // auto &&Hsub_T = Hsub.transpose(); + auto &&HTz = Hsub_T_R_inv * meas_vec; + // fout_dbg<<"HTz: "<(0, 0) = Hsub_T_R_inv * Hsub; + // EigenSolver> es(H_T_H.block<6,6>(0,0)); + MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H.block(0, 0) + state_.cov.block(0, 0).inverse()).inverse(); + G.block(0, 0) = K_1.block(0, 0) * H_T_H.block<6, 6>(0, 0); + auto vec = state_propagat - state_; + VD(DIM_STATE) + solution = K_1.block(0, 0) * HTz + vec.block(0, 0) - G.block(0, 0) * vec.block<6, 1>(0, 0); + int minRow, minCol; + state_ += solution; + auto rot_add = solution.block<3, 1>(0, 0); + auto t_add = solution.block<3, 1>(3, 0); + if ((rot_add.norm() * 57.3 < 0.01) && (t_add.norm() * 100 < 0.015)) { flg_EKF_converged = true; } + V3D euler_cur = state_.rot_end.eulerAngles(2, 1, 0); + + /*** Rematch Judgement ***/ + + if (flg_EKF_converged || ((rematch_num == 0) && (iterCount == (config_setting_.max_iterations_ - 2)))) { rematch_num++; } + + /*** Convergence Judgements and Covariance Update ***/ + if (!EKF_stop_flg && (rematch_num >= 2 || (iterCount == config_setting_.max_iterations_ - 1))) + { + /*** Covariance Update ***/ + // _state.cov = (I_STATE - G) * _state.cov; + state_.cov.block(0, 0) = + (I_STATE.block(0, 0) - G.block(0, 0)) * state_.cov.block(0, 0); + // total_distance += (_state.pos_end - position_last).norm(); + position_last_ = state_.pos_end; + geoQuat_ = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2)); + + // VD(DIM_STATE) K_sum = K.rowwise().sum(); + // VD(DIM_STATE) P_diag = _state.cov.diagonal(); + EKF_stop_flg = true; + } + if (EKF_stop_flg) break; + } + + // double t2 = omp_get_wtime(); + // scan_count++; + // ekf_time = t2 - t0 - build_residual_time; + + // ave_build_residual_time = ave_build_residual_time * (scan_count - 1) / scan_count + build_residual_time / scan_count; + // ave_ekf_time = ave_ekf_time * (scan_count - 1) / scan_count + ekf_time / scan_count; + + // cout << "[ Mapping ] ekf_time: " << ekf_time << "s, build_residual_time: " << build_residual_time << "s" << endl; + // cout << "[ Mapping ] ave_ekf_time: " << ave_ekf_time << "s, ave_build_residual_time: " << ave_build_residual_time << "s" << endl; +} + +void VoxelMapManager::TransformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, + pcl::PointCloud::Ptr &trans_cloud) +{ + pcl::PointCloud().swap(*trans_cloud); + trans_cloud->reserve(input_cloud->size()); + for (size_t i = 0; i < input_cloud->size(); i++) + { + pcl::PointXYZINormal p_c = input_cloud->points[i]; + Eigen::Vector3d p(p_c.x, p_c.y, p_c.z); + p = (rot * (extR_ * p + extT_) + t); + pcl::PointXYZI pi; + pi.x = p(0); + pi.y = p(1); + pi.z = p(2); + pi.intensity = p_c.intensity; + trans_cloud->points.push_back(pi); + } +} + +void VoxelMapManager::BuildVoxelMap() +{ + float voxel_size = config_setting_.max_voxel_size_; + float planer_threshold = config_setting_.planner_threshold_; + int max_layer = config_setting_.max_layer_; + int max_points_num = config_setting_.max_points_num_; + std::vector layer_init_num = convertToIntVectorSafe(config_setting_.layer_init_num_); + + std::vector input_points; + + for (size_t i = 0; i < feats_down_world_->size(); i++) + { + pointWithVar pv; + pv.point_w << feats_down_world_->points[i].x, feats_down_world_->points[i].y, feats_down_world_->points[i].z; + V3D point_this(feats_down_body_->points[i].x, feats_down_body_->points[i].y, feats_down_body_->points[i].z); + M3D var; + calcBodyCov(point_this, config_setting_.dept_err_, config_setting_.beam_err_, var); + M3D point_crossmat; + point_crossmat << SKEW_SYM_MATRX(point_this); + var = (state_.rot_end * extR_) * var * (state_.rot_end * extR_).transpose() + + (-point_crossmat) * state_.cov.block<3, 3>(0, 0) * (-point_crossmat).transpose() + state_.cov.block<3, 3>(3, 3); + pv.var = var; + input_points.push_back(pv); + } + + uint plsize = input_points.size(); + for (uint i = 0; i < plsize; i++) + { + const pointWithVar p_v = input_points[i]; + float loc_xyz[3]; + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = p_v.point_w[j] / voxel_size; + if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + } + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]); + auto iter = voxel_map_.find(position); + if (iter != voxel_map_.end()) + { + voxel_map_[position]->temp_points_.push_back(p_v); + voxel_map_[position]->new_points_++; + } + else + { + VoxelOctoTree *octo_tree = new VoxelOctoTree(max_layer, 0, layer_init_num[0], max_points_num, planer_threshold); + voxel_map_[position] = octo_tree; + voxel_map_[position]->quater_length_ = voxel_size / 4; + voxel_map_[position]->voxel_center_[0] = (0.5 + position.x) * voxel_size; + voxel_map_[position]->voxel_center_[1] = (0.5 + position.y) * voxel_size; + voxel_map_[position]->voxel_center_[2] = (0.5 + position.z) * voxel_size; + voxel_map_[position]->temp_points_.push_back(p_v); + voxel_map_[position]->new_points_++; + voxel_map_[position]->layer_init_num_ = layer_init_num; + } + } + for (auto iter = voxel_map_.begin(); iter != voxel_map_.end(); ++iter) + { + iter->second->init_octo_tree(); + } +} + +V3F VoxelMapManager::RGBFromVoxel(const V3D &input_point) +{ + int64_t loc_xyz[3]; + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = floor(input_point[j] / config_setting_.max_voxel_size_); + } + + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]); + int64_t ind = loc_xyz[0] + loc_xyz[1] + loc_xyz[2]; + uint k((ind + 100000) % 3); + V3F RGB((k == 0) * 255.0, (k == 1) * 255.0, (k == 2) * 255.0); + // cout<<"RGB: "< &input_points) +{ + float voxel_size = config_setting_.max_voxel_size_; + float planer_threshold = config_setting_.planner_threshold_; + int max_layer = config_setting_.max_layer_; + int max_points_num = config_setting_.max_points_num_; + std::vector layer_init_num = convertToIntVectorSafe(config_setting_.layer_init_num_); + uint plsize = input_points.size(); + for (uint i = 0; i < plsize; i++) + { + const pointWithVar p_v = input_points[i]; + float loc_xyz[3]; + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = p_v.point_w[j] / voxel_size; + if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + } + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]); + auto iter = voxel_map_.find(position); + if (iter != voxel_map_.end()) { voxel_map_[position]->UpdateOctoTree(p_v); } + else + { + VoxelOctoTree *octo_tree = new VoxelOctoTree(max_layer, 0, layer_init_num[0], max_points_num, planer_threshold); + voxel_map_[position] = octo_tree; + voxel_map_[position]->layer_init_num_ = layer_init_num; + voxel_map_[position]->quater_length_ = voxel_size / 4; + voxel_map_[position]->voxel_center_[0] = (0.5 + position.x) * voxel_size; + voxel_map_[position]->voxel_center_[1] = (0.5 + position.y) * voxel_size; + voxel_map_[position]->voxel_center_[2] = (0.5 + position.z) * voxel_size; + voxel_map_[position]->UpdateOctoTree(p_v); + } + } +} + +void VoxelMapManager::BuildResidualListOMP(std::vector &pv_list, std::vector &ptpl_list) +{ + int max_layer = config_setting_.max_layer_; + double voxel_size = config_setting_.max_voxel_size_; + double sigma_num = config_setting_.sigma_num_; + std::mutex mylock; + ptpl_list.clear(); + std::vector all_ptpl_list(pv_list.size()); + std::vector useful_ptpl(pv_list.size()); + std::vector index(pv_list.size()); + for (size_t i = 0; i < index.size(); ++i) + { + index[i] = i; + useful_ptpl[i] = false; + } + #ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); + #pragma omp parallel for + #endif + for (int i = 0; i < index.size(); i++) + { + pointWithVar &pv = pv_list[i]; + float loc_xyz[3]; + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = pv.point_w[j] / voxel_size; + if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + } + VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]); + auto iter = voxel_map_.find(position); + if (iter != voxel_map_.end()) + { + VoxelOctoTree *current_octo = iter->second; + PointToPlane single_ptpl; + bool is_sucess = false; + double prob = 0; + build_single_residual(pv, current_octo, 0, is_sucess, prob, single_ptpl); + if (!is_sucess) + { + VOXEL_LOCATION near_position = position; + if (loc_xyz[0] > (current_octo->voxel_center_[0] + current_octo->quater_length_)) { near_position.x = near_position.x + 1; } + else if (loc_xyz[0] < (current_octo->voxel_center_[0] - current_octo->quater_length_)) { near_position.x = near_position.x - 1; } + if (loc_xyz[1] > (current_octo->voxel_center_[1] + current_octo->quater_length_)) { near_position.y = near_position.y + 1; } + else if (loc_xyz[1] < (current_octo->voxel_center_[1] - current_octo->quater_length_)) { near_position.y = near_position.y - 1; } + if (loc_xyz[2] > (current_octo->voxel_center_[2] + current_octo->quater_length_)) { near_position.z = near_position.z + 1; } + else if (loc_xyz[2] < (current_octo->voxel_center_[2] - current_octo->quater_length_)) { near_position.z = near_position.z - 1; } + auto iter_near = voxel_map_.find(near_position); + if (iter_near != voxel_map_.end()) { build_single_residual(pv, iter_near->second, 0, is_sucess, prob, single_ptpl); } + } + if (is_sucess) + { + mylock.lock(); + useful_ptpl[i] = true; + all_ptpl_list[i] = single_ptpl; + mylock.unlock(); + } + else + { + mylock.lock(); + useful_ptpl[i] = false; + mylock.unlock(); + } + } + } + for (size_t i = 0; i < useful_ptpl.size(); i++) + { + if (useful_ptpl[i]) { ptpl_list.push_back(all_ptpl_list[i]); } + } +} + +void VoxelMapManager::build_single_residual(pointWithVar &pv, const VoxelOctoTree *current_octo, const int current_layer, bool &is_sucess, + double &prob, PointToPlane &single_ptpl) +{ + int max_layer = config_setting_.max_layer_; + double sigma_num = config_setting_.sigma_num_; + + double radius_k = 3; + Eigen::Vector3d p_w = pv.point_w; + if (current_octo->plane_ptr_->is_plane_) + { + VoxelPlane &plane = *current_octo->plane_ptr_; + Eigen::Vector3d p_world_to_center = p_w - plane.center_; + float dis_to_plane = fabs(plane.normal_(0) * p_w(0) + plane.normal_(1) * p_w(1) + plane.normal_(2) * p_w(2) + plane.d_); + float dis_to_center = (plane.center_(0) - p_w(0)) * (plane.center_(0) - p_w(0)) + (plane.center_(1) - p_w(1)) * (plane.center_(1) - p_w(1)) + + (plane.center_(2) - p_w(2)) * (plane.center_(2) - p_w(2)); + float range_dis = sqrt(dis_to_center - dis_to_plane * dis_to_plane); + + if (range_dis <= radius_k * plane.radius_) + { + Eigen::Matrix J_nq; + J_nq.block<1, 3>(0, 0) = p_w - plane.center_; + J_nq.block<1, 3>(0, 3) = -plane.normal_; + double sigma_l = J_nq * plane.plane_var_ * J_nq.transpose(); + sigma_l += plane.normal_.transpose() * pv.var * plane.normal_; + if (dis_to_plane < sigma_num * sqrt(sigma_l)) + { + is_sucess = true; + double this_prob = 1.0 / (sqrt(sigma_l)) * exp(-0.5 * dis_to_plane * dis_to_plane / sigma_l); + if (this_prob > prob) + { + prob = this_prob; + pv.normal = plane.normal_; + single_ptpl.body_cov_ = pv.body_var; + single_ptpl.point_b_ = pv.point_b; + single_ptpl.point_w_ = pv.point_w; + single_ptpl.plane_var_ = plane.plane_var_; + single_ptpl.normal_ = plane.normal_; + single_ptpl.center_ = plane.center_; + single_ptpl.d_ = plane.d_; + single_ptpl.layer_ = current_layer; + single_ptpl.dis_to_plane_ = plane.normal_(0) * p_w(0) + plane.normal_(1) * p_w(1) + plane.normal_(2) * p_w(2) + plane.d_; + } + return; + } + else + { + // is_sucess = false; + return; + } + } + else + { + // is_sucess = false; + return; + } + } + else + { + if (current_layer < max_layer) + { + for (size_t leafnum = 0; leafnum < 8; leafnum++) + { + if (current_octo->leaves_[leafnum] != nullptr) + { + + VoxelOctoTree *leaf_octo = current_octo->leaves_[leafnum]; + build_single_residual(pv, leaf_octo, current_layer + 1, is_sucess, prob, single_ptpl); + } + } + return; + } + else { return; } + } +} + +void VoxelMapManager::pubVoxelMap() +{ + double max_trace = 0.25; + double pow_num = 0.2; + rclcpp::Rate loop(500); + float use_alpha = 0.8; + visualization_msgs::msg::MarkerArray voxel_plane; + voxel_plane.markers.reserve(1000000); + std::vector pub_plane_list; + for (auto iter = voxel_map_.begin(); iter != voxel_map_.end(); iter++) + { + GetUpdatePlane(iter->second, config_setting_.max_layer_, pub_plane_list); + } + for (size_t i = 0; i < pub_plane_list.size(); i++) + { + V3D plane_cov = pub_plane_list[i].plane_var_.block<3, 3>(0, 0).diagonal(); + double trace = plane_cov.sum(); + if (trace >= max_trace) { trace = max_trace; } + trace = trace * (1.0 / max_trace); + trace = pow(trace, pow_num); + uint8_t r, g, b; + mapJet(trace, 0, 1, r, g, b); + Eigen::Vector3d plane_rgb(r / 256.0, g / 256.0, b / 256.0); + double alpha; + if (pub_plane_list[i].is_plane_) { alpha = use_alpha; } + else { alpha = 0; } + pubSinglePlane(voxel_plane, "plane", pub_plane_list[i], alpha, plane_rgb); + } + voxel_map_pub_->publish(voxel_plane); + loop.sleep(); +} + +void VoxelMapManager::GetUpdatePlane(const VoxelOctoTree *current_octo, const int pub_max_voxel_layer, std::vector &plane_list) +{ + if (current_octo->layer_ > pub_max_voxel_layer) { return; } + if (current_octo->plane_ptr_->is_update_) { plane_list.push_back(*current_octo->plane_ptr_); } + if (current_octo->layer_ < current_octo->max_layer_) + { + if (!current_octo->plane_ptr_->is_plane_) + { + for (size_t i = 0; i < 8; i++) + { + if (current_octo->leaves_[i] != nullptr) { GetUpdatePlane(current_octo->leaves_[i], pub_max_voxel_layer, plane_list); } + } + } + } + return; +} + +void VoxelMapManager::pubSinglePlane(visualization_msgs::msg::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane, + const float alpha, const Eigen::Vector3d rgb) +{ + visualization_msgs::msg::Marker plane; + plane.header.frame_id = "camera_init"; + plane.header.stamp = rclcpp::Time(); + plane.ns = plane_ns; + plane.id = single_plane.id_; + plane.type = visualization_msgs::msg::Marker::CYLINDER; + plane.action = visualization_msgs::msg::Marker::ADD; + plane.pose.position.x = single_plane.center_[0]; + plane.pose.position.y = single_plane.center_[1]; + plane.pose.position.z = single_plane.center_[2]; + geometry_msgs::msg::Quaternion q; + CalcVectQuation(single_plane.x_normal_, single_plane.y_normal_, single_plane.normal_, q); + plane.pose.orientation = q; + plane.scale.x = 3 * sqrt(single_plane.max_eigen_value_); + plane.scale.y = 3 * sqrt(single_plane.mid_eigen_value_); + plane.scale.z = 2 * sqrt(single_plane.min_eigen_value_); + plane.color.a = alpha; + plane.color.r = rgb(0); + plane.color.g = rgb(1); + plane.color.b = rgb(2); + plane.lifetime = rclcpp::Duration::from_seconds(0.01); + plane_pub.markers.push_back(plane); +} + +void VoxelMapManager::CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec, + geometry_msgs::msg::Quaternion &q) +{ + Eigen::Matrix3d rot; + rot << x_vec(0), x_vec(1), x_vec(2), y_vec(0), y_vec(1), y_vec(2), z_vec(0), z_vec(1), z_vec(2); + Eigen::Matrix3d rotation = rot.transpose(); + Eigen::Quaterniond eq(rotation); + q.w = eq.w(); + q.x = eq.x(); + q.y = eq.y(); + q.z = eq.z(); +} + +void VoxelMapManager::mapJet(double v, double vmin, double vmax, uint8_t &r, uint8_t &g, uint8_t &b) +{ + r = 255; + g = 255; + b = 255; + + if (v < vmin) { v = vmin; } + + if (v > vmax) { v = vmax; } + + double dr, dg, db; + + if (v < 0.1242) + { + db = 0.504 + ((1. - 0.504) / 0.1242) * v; + dg = dr = 0.; + } + else if (v < 0.3747) + { + db = 1.; + dr = 0.; + dg = (v - 0.1242) * (1. / (0.3747 - 0.1242)); + } + else if (v < 0.6253) + { + db = (0.6253 - v) * (1. / (0.6253 - 0.3747)); + dg = 1.; + dr = (v - 0.3747) * (1. / (0.6253 - 0.3747)); + } + else if (v < 0.8758) + { + db = 0.; + dr = 1.; + dg = (0.8758 - v) * (1. / (0.8758 - 0.6253)); + } + else + { + db = 0.; + dg = 0.; + dr = 1. - (v - 0.8758) * ((1. - 0.504) / (1. - 0.8758)); + } + + r = (uint8_t)(255 * dr); + g = (uint8_t)(255 * dg); + b = (uint8_t)(255 * db); +} + +void VoxelMapManager::mapSliding() +{ + if((position_last_ - last_slide_position).norm() < config_setting_.sliding_thresh) + { + std::cout<first; + bool should_remove = loc.x > x_max || loc.x < x_min || loc.y > y_max || loc.y < y_min || loc.z > z_max || loc.z < z_min; + if (should_remove){ + // last_delete_time = omp_get_wtime(); + delete it->second; + it = voxel_map_.erase(it); + // delete_time += omp_get_wtime() - last_delete_time; + delete_voxel_cout++; + } else { + ++it; + } + } + std::cout< + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/FAST-LIVO2/urdf/mid360_robot.urdf b/src/FAST-LIVO2/urdf/mid360_robot.urdf new file mode 100644 index 0000000..2ac23fe --- /dev/null +++ b/src/FAST-LIVO2/urdf/mid360_robot.urdf @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/gnss_comm/CMakeLists.txt b/src/gnss_comm/CMakeLists.txt new file mode 100644 index 0000000..024af28 --- /dev/null +++ b/src/gnss_comm/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.8) +project(gnss_comm) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/GnssTimeMsg.msg" + "msg/GnssPVTSolnMsg.msg" +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/src/gnss_comm/msg/GnssPVTSolnMsg.msg b/src/gnss_comm/msg/GnssPVTSolnMsg.msg new file mode 100644 index 0000000..eaf99f4 --- /dev/null +++ b/src/gnss_comm/msg/GnssPVTSolnMsg.msg @@ -0,0 +1,22 @@ +# This message contains information of UBX-NAV-PVT message. +# reference: [1]. UBX-18010854-R08, page 132 + +GnssTimeMsg time # GNSS time of the navigation epoch +# GNSS fix type (0=no fix, 1=dead reckoning only, 2=2D-fix, 3=3D-fix, +# 4=GNSS+dead reckoning combined, 5=time only fix) +uint8 fix_type +bool valid_fix # if fix valid (1=valid fix) +bool diff_soln # if differential correction were applied (1=applied) +uint8 carr_soln # carrier phase range solution status (0=no carrier phase, 1=float, 2=fix) +uint8 num_sv # number of satellites used in the solution +float64 latitude # latitude [degree] +float64 longitude # longitude [degree] +float64 altitude # height above ellipsoid [m] +float64 height_msl # height above mean sea level [m] +float64 h_acc # horizontal accuracy estimate [m] +float64 v_acc # vertical accuracy estimate [m] +float64 p_dop # Position DOP +float64 vel_n # NED north velocity [m/s] +float64 vel_e # NED east velocity [m/s] +float64 vel_d # NED down velocity [m/s] +float64 vel_acc # speed accuracy estimate [m/s] diff --git a/src/gnss_comm/msg/GnssTimeMsg.msg b/src/gnss_comm/msg/GnssTimeMsg.msg new file mode 100644 index 0000000..ff72f30 --- /dev/null +++ b/src/gnss_comm/msg/GnssTimeMsg.msg @@ -0,0 +1,5 @@ +# This message contains GNSS time expressed in the form of +# GNSS week number and time of week(in seconds) + +uint32 week +float64 tow diff --git a/src/gnss_comm/package.xml b/src/gnss_comm/package.xml new file mode 100644 index 0000000..d76bc41 --- /dev/null +++ b/src/gnss_comm/package.xml @@ -0,0 +1,25 @@ + + + + gnss_comm + 1.0.0 + + Minimal ROS 2 interface package providing the gnss_comm GNSS PVT solution + messages used by FAST-LIVO2-RTK. Field layout matches the original + HKUST-Aerial-Robotics/gnss_comm ROS 1 messages so converted rosbags + (gnss_comm/msg/GnssPVTSolnMsg) play back directly. + + FAST-LIVO2-RTK ROS2 port + GPLv3 + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/rpg_vikit/.gitignore b/src/rpg_vikit/.gitignore new file mode 100644 index 0000000..b3a0f62 --- /dev/null +++ b/src/rpg_vikit/.gitignore @@ -0,0 +1,3 @@ +vikit_py/build +vikit_common/bin/ +vikit_common/lib/ diff --git a/src/rpg_vikit/README.md b/src/rpg_vikit/README.md new file mode 100644 index 0000000..fdc4157 --- /dev/null +++ b/src/rpg_vikit/README.md @@ -0,0 +1,69 @@ +# Vikit: Vision-Kit for Robotics + +Vikit is a versatile collection of C++ tools and utilities designed for computer vision and robotics projects. This version has been modernized for the ROS2 ecosystem (specifically tested on Jazzy and Humble) with a focus on high-performance parameter handling and cross-node compatibility. + +--- + +## 🔒 Disclaimer & Acknowledgments + +### Usage Policy +This software is provided for **educational and research purposes only**. It shall **not be used for any commercial purposes**. + +### Acknowledgments +We extend our deepest gratitude to the original developers and contributors of the projects that served as the foundation for this kit: +* [uzh-rpg/rpg_vikit](https://github.com/uzh-rpg/rpg_vikit) +* [xuankuzcr/rpg_vikit](https://github.com/xuankuzcr/rpg_vikit) +* [uavfly/vikit](https://github.com/uavfly/vikit) + +--- + +## 🚀 Key Improvements in ROS2 + +### Optimized Parameter Fetching Architecture +One of the most significant challenges in migrating from ROS1 to ROS2 is the removal of the global Parameter Server. In ROS2, parameters are local to each node. Vikit addresses this through a multi-tiered fetching strategy in `params_helper.hpp`: + +1. **Native Node Access**: Direct, high-speed access to parameters owned by the current node handle. +2. **`SyncParametersClient` (High Performance)**: For cross-node parameter access (e.g., retrieving camera intrinsics from a central `parameter_blackboard`). This utilizes optimized ROS2 Service calls to achieve microsecond-level latency, avoiding the overhead of CLI tools. +3. **Command-Line Fallback**: A robust fallback mechanism using `popen` to interface with the ROS2 CLI (`ros2 param get`), ensuring parameter retrieval even in complex edge cases where service clients might be restricted. + +This architecture ensures that vision components can load dozens of camera parameters nearly instantaneously, a critical requirement for real-time SLAM and VIO systems. + +--- + +## 🛠 Installation Guide + +### Prerequisites: Sophus +Vikit relies on Sophus for Lie groups. It is recommended to use version `1.22.10`. + +```bash +git clone https://github.com/strasdat/Sophus.git -b 1.22.10 +cd Sophus && mkdir build && cd build +cmake .. && make -j$(nproc) +sudo make install +``` + +### Building `vikit_common` +`vikit_common` is a pure CMake package and can be installed globally. + +```bash +cd vikit_common +mkdir build && cd build +cmake .. && make -j$(nproc) +sudo make install +``` + +### Building `vikit_ros` +`vikit_ros` is integrated into the ROS2 workspace and should be built using `colcon`. + +```bash +# Move vikit_ros to your workspace src directory +cd ~/ros2_ws +colcon build --symlink-install --packages-select vikit_ros +``` + +--- + +## 📅 Maintenance Info +* **Last Update**: December 2025 +* **Target Systems**: Ubuntu 22.04 (Humble) / 24.04 (Jazzy) +* **Compiler**: C++17 compliant (GCC 9+) diff --git a/src/rpg_vikit/vikit_common/CMakeLists.txt b/src/rpg_vikit/vikit_common/CMakeLists.txt new file mode 100644 index 0000000..3889d0a --- /dev/null +++ b/src/rpg_vikit/vikit_common/CMakeLists.txt @@ -0,0 +1,115 @@ +SET(PROJECT_NAME vikit_common) +PROJECT(${PROJECT_NAME}) +CMAKE_MINIMUM_REQUIRED (VERSION 3.0) +SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo +SET(CMAKE_VERBOSE_MAKEFILE OFF) +SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") +SET(USE_ROS False) # Set False if you want to build this package without Catkin + +# Set build flags. Set IS_ARM on odroid board as environment variable +SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") +IF(DEFINED ENV{ARM_ARCHITECTURE}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv8-a") +ELSE() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") +ENDIF() +message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") +if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") +else() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=armv8-a") +endif() + +IF(CMAKE_COMPILER_IS_GNUCC) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +ELSE() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +ENDIF() +SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") + +# Add plain cmake packages +FIND_PACKAGE(OpenCV REQUIRED) +FIND_PACKAGE(Eigen REQUIRED) +FIND_PACKAGE(Sophus REQUIRED) +FIND_PACKAGE(fmt REQUIRED) + +# Support modern cmake Sophus target (ros-humble-sophus) which doesn't set Sophus_INCLUDE_DIRS +if(TARGET Sophus::Sophus AND NOT Sophus_INCLUDE_DIRS) + get_target_property(Sophus_INCLUDE_DIRS Sophus::Sophus INTERFACE_INCLUDE_DIRECTORIES) +endif() + +# Include dirs +INCLUDE_DIRECTORIES( + include + ${Eigen_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Sophus_INCLUDE_DIRS} + fmt +) + +IF(USE_ROS) + FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp cmake_modules) + LIST(APPEND INCLUDE_DIRECTORIES ${catkin_INCLUDE_DIRS}) + catkin_package( + DEPENDS Eigen OpenCV Sophus + CATKIN_DEPENDS roscpp + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + ) +ELSE() + SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) + SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) +ENDIF() + +# Set Sourcefiles +LIST(APPEND SOURCEFILES src/atan_camera.cpp + src/omni_camera.cpp + src/math_utils.cpp + src/vision.cpp + src/performance_monitor.cpp + src/robust_cost.cpp + src/user_input_thread.cpp + src/pinhole_camera.cpp + src/equidistant_camera.cpp + src/polynomial_camera.cpp + src/homography.cpp + src/img_align.cpp) + +# Create vikit library +ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCEFILES}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} + ${OpenCV_LIBS} + ${Sophus_LIBRARIES} + fmt::fmt) + +IF(USE_ROS) + TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${catkin_LIBRARIES}) +ENDIF() + +# Tests +ADD_EXECUTABLE(test_vk_common_camera test/test_camera.cpp) +TARGET_LINK_LIBRARIES(test_vk_common_camera ${PROJECT_NAME} ${OpenCV_LIBS}) + +ADD_EXECUTABLE(test_vk_common_triangulation test/test_triangulation.cpp) +TARGET_LINK_LIBRARIES(test_vk_common_triangulation ${PROJECT_NAME} ${OpenCV_LIBS}) + +ADD_EXECUTABLE(test_vk_common_patch_score test/test_patch_score.cpp) +TARGET_LINK_LIBRARIES(test_vk_common_patch_score ${PROJECT_NAME} ${OpenCV_LIBS}) + + +################################################################################ +# Create the vikit_commonConfig.cmake file for other cmake projects. +IF(NOT USE_ROS) + # In CMake 3.6 and later, reading the LOCATION property from a target is no longer allowed. + # GET_TARGET_PROPERTY( FULL_LIBRARY_NAME ${PROJECT_NAME} LOCATION ) + set(VIKIT_COMMON_LOCATION $) + SET(vikit_common_LIBRARIES ${FULL_LIBRARY_NAME} ) + SET(vikit_common_LIBRARY_DIR ${PROJECT_BINARY_DIR} ) + SET(vikit_common_INCLUDE_DIR "${PROJECT_SOURCE_DIR}/include") + CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/vikit_commonConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/vikit_commonConfig.cmake @ONLY IMMEDIATE ) + export( PACKAGE vikit_common ) + + INSTALL(DIRECTORY include/vikit DESTINATION ${CMAKE_INSTALL_PREFIX}/include FILES_MATCHING PATTERN "*.h" ) + INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib ) +ENDIF() \ No newline at end of file diff --git a/src/rpg_vikit/vikit_common/CMakeModules/FindEigen.cmake b/src/rpg_vikit/vikit_common/CMakeModules/FindEigen.cmake new file mode 100644 index 0000000..8587367 --- /dev/null +++ b/src/rpg_vikit/vikit_common/CMakeModules/FindEigen.cmake @@ -0,0 +1,81 @@ +############################################################################### +# +# CMake script for finding the Eigen library. +# +# http://eigen.tuxfamily.org/index.php?title=Main_Page +# +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD +# license. +# +# +# Input variables: +# +# - Eigen_ROOT_DIR (optional): When specified, header files and libraries +# will be searched for in `${Eigen_ROOT_DIR}/include` and +# `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order +# will be ignored. When unspecified, the default CMake search order is used. +# This variable can be specified either as a CMake or environment variable. +# If both are set, preference is given to the CMake variable. +# Use this variable for finding packages installed in a nonstandard location, +# or for enforcing that one of multiple package installations is picked up. +# +# Cache variables (not intended to be used in CMakeLists.txt files) +# +# - Eigen_INCLUDE_DIR: Absolute path to package headers. +# +# +# Output variables: +# +# - Eigen_FOUND: Boolean that indicates if the package was found +# - Eigen_INCLUDE_DIRS: Paths to the necessary header files +# - Eigen_VERSION: Version of Eigen library found +# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen +# +# +# Example usage: +# +# # Passing the version means Eigen_FOUND will only be TRUE if a +# # version >= the provided version is found. +# find_package(Eigen 3.1.2) +# if(NOT Eigen_FOUND) +# # Error handling +# endif() +# ... +# add_definitions(${Eigen_DEFINITIONS}) +# ... +# include_directories(${Eigen_INCLUDE_DIRS} ...) +# +############################################################################### + +find_package(PkgConfig) +pkg_check_modules(PC_EIGEN eigen3) +set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) + + +find_path(EIGEN_INCLUDE_DIR Eigen/Core + HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} + "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" + "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility + PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" + "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" + PATH_SUFFIXES eigen3 include/eigen3 include) + +set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) + +mark_as_advanced(EIGEN_INCLUDE_DIR) + +if(EIGEN_FOUND) + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") +endif(EIGEN_FOUND) + + +set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +set(Eigen_FOUND ${EIGEN_FOUND}) +set(Eigen_VERSION ${EIGEN_VERSION}) +set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) diff --git a/src/rpg_vikit/vikit_common/include/vikit/abstract_camera.h b/src/rpg_vikit/vikit_common/include/vikit/abstract_camera.h new file mode 100644 index 0000000..e4b44b4 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/abstract_camera.h @@ -0,0 +1,85 @@ +/* + * abstract_camera.h + * + * Created on: Jul 23, 2012 + * Author: cforster + */ + +#ifndef ABSTRACT_CAMERA_H_ +#define ABSTRACT_CAMERA_H_ + +#include + +namespace vk +{ + +using namespace std; +using namespace Eigen; + +class AbstractCamera +{ +protected: + + int width_; // TODO cannot be const because of omni-camera model + int height_; + double scale_; + +public: + + AbstractCamera() {}; // need this constructor for omni camera + AbstractCamera(int width, int height, double scale) : width_(width), height_(height), scale_(scale){}; + + virtual ~AbstractCamera() {}; + + /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. + virtual Vector3d + cam2world(const double& x, const double& y) const = 0; + + /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. + virtual Vector3d + cam2world(const Vector2d& px) const = 0; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const = 0; + + /// projects unit plane coordinates to camera coordinates + virtual Vector2d + world2cam(const Vector2d& uv) const = 0; + + virtual double + errorMultiplier2() const = 0; + + virtual double + errorMultiplier() const = 0; + + virtual double fx() const = 0; + virtual double fy() const = 0; + virtual double cx() const = 0; + virtual double cy() const = 0; + + inline int width() const { return width_; } + + inline int height() const { return height_; } + + inline double scale() const { return scale_; } + + inline bool isInFrame(const Vector2i & obs, int boundary=0) const + { + if(obs[0]>=boundary && obs[0]=boundary && obs[1]= boundary && obs[0] < width()/(1<= boundary && obs[1] // memset +#include // memset +#include +#include + +namespace vk { +namespace aligned_mem { + + /// Check if the pointer is aligned to the specified byte granularity + inline bool + is_aligned8(const void* ptr) + { + return ((reinterpret_cast(ptr)) & 0x7) == 0; + } + + inline bool + is_aligned16(const void* ptr) + { + return ((reinterpret_cast(ptr)) & 0xF) == 0; + } + + template struct placement_delete + { + enum { Size = (1<= Size) { + placement_delete::free(buf+Size,M-Size); + placement_delete::destruct(buf); + } else { + placement_delete::free(buf, M); + } + } + }; + + template struct placement_delete + { + static inline void free(T*, size_t ) {} + }; + + inline void * aligned_alloc(size_t count, size_t alignment){ + void * mem = NULL; + assert(posix_memalign(&mem, alignment, count) == 0); + return mem; + } + + inline void aligned_free(void * memory) { + free(memory); + } + + template + inline T * aligned_alloc(size_t count, size_t alignment){ + void * data = aligned_alloc(sizeof(T)* count, alignment); + return new (data) T[count]; + } + + template + inline void aligned_free(T * memory, size_t count){ + placement_delete::free(memory, count); + aligned_free(memory); + } + + template inline void memfill(T* data, int n, const T val) + { + T* de = data + n; + for(;data < de; data++) + *data=val; + } + + template<> inline void memfill(unsigned char* data, int n, const unsigned char val) + { + memset(data, val, n); + } + + template<> inline void memfill(signed char* data, int n, const signed char val) + { + memset(data, val, n); + } + + template<> inline void memfill(char* data, int n, const char val) + { + memset(data, val, n); + } + + template + struct AlignedMem { + T* mem; + size_t count; + AlignedMem(size_t c) : count(c) { + mem = aligned_alloc(count, N); + } + ~AlignedMem() { + aligned_free(mem, count); + } + T* data() { return mem; } + const T* data() const { return mem; } + }; + +} // namespace aligned_mem +} // namespace vikit + + +#endif // VIKIT_ALIGNED_MEM_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/atan_camera.h b/src/rpg_vikit/vikit_common/include/vikit/atan_camera.h new file mode 100644 index 0000000..48b0b23 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/atan_camera.h @@ -0,0 +1,96 @@ +/* + * atan_camera.h + * + * Created on: Aug 21, 2012 + * Author: cforster + * + * This class implements the FOV distortion model of Deverneay and Faugeras, + * Straight lines have to be straight, 2001. + * + * The code is an implementation of the ATAN class in PTAM by Georg Klein using Eigen. + */ + +#ifndef ATAN_CAMERA_H_ +#define ATAN_CAMERA_H_ + +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +class ATANCamera : public AbstractCamera { + +private: + double fx_, fy_; //!< focal length + double fx_inv_, fy_inv_; //!< inverse focal length + double cx_, cy_; //!< projection center + double s_, s_inv_; //!< distortion model coeff + double tans_; //!< distortion model coeff + double tans_inv_; //!< distortion model coeff + bool distortion_; //!< use distortion model? + + //! Radial distortion transformation factor: returns ration of distorted / undistorted radius. + inline double rtrans_factor(double r) const + { + if(r < 0.001 || s_ == 0.0) + return 1.0; + else + return (s_inv_* atan(r * tans_) / r); + }; + + //! Inverse radial distortion: returns un-distorted radius from distorted. + inline double invrtrans(double r) const + { + if(s_ == 0.0) + return r; + return (tan(r * s_) * tans_inv_); + }; + +public: + + ATANCamera(double width, double height, double fx, double fy, double dx, double dy, double s); + + ~ATANCamera(); + + virtual Vector3d + cam2world(const double& x, const double& y) const; + + virtual Vector3d + cam2world(const Vector2d& px) const; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const; + + virtual Vector2d + world2cam(const Vector2d& uv) const; + + const Vector2d focal_length() const + { + return Vector2d(fx_, fy_); + } + + virtual double errorMultiplier2() const + { + return fx_; + } + + virtual double errorMultiplier() const + { + return 4*fx_*fy_; + } + + virtual double fx() const { return fx_; }; + virtual double fy() const { return fy_; }; + virtual double cx() const { return cx_; }; + virtual double cy() const { return cy_; }; +}; + +} // end namespace vk + +#endif /* ATAN_CAMERA_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/blender_utils.h b/src/rpg_vikit/vikit_common/include/vikit/blender_utils.h new file mode 100644 index 0000000..c858b56 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/blender_utils.h @@ -0,0 +1,133 @@ +/* + * blender_utils.h + * + * Created on: Feb 13, 2014 + * Author: cforster + */ + +#ifndef VIKIT_BLENDER_UTILS_H_ +#define VIKIT_BLENDER_UTILS_H_ + +#include +#include +#include +#include +#include +#include +#include + +namespace vk { +namespace blender_utils { + +void loadBlenderDepthmap( + const std::string file_name, + const vk::AbstractCamera& cam, + cv::Mat& img) +{ + std::ifstream file_stream(file_name.c_str()); + assert(file_stream.is_open()); + img = cv::Mat(cam.height(), cam.width(), CV_32FC1); + float * img_ptr = img.ptr(); + float depth; + for(int y=0; y> depth; + // blender: + Eigen::Vector2d uv(vk::project2d(cam.cam2world(x,y))); + *img_ptr = depth * sqrt(uv[0]*uv[0] + uv[1]*uv[1] + 1.0); + + // povray + // *img_ptr = depth/100.0; // depth is in [cm], we want [m] + + if(file_stream.peek() == '\n' && x != cam.width()-1 && y != cam.height()-1) + printf("WARNING: did not read the full depthmap!\n"); + } + } +} + +bool getDepthmapNormalAtPoint( + const Vector2i& px, + const cv::Mat& depth, + const int halfpatch_size, + const vk::AbstractCamera& cam, + Vector3d& normal) +{ + assert(cam.width() == depth.cols && cam.height() == depth.rows); + if(!cam.isInFrame(px, halfpatch_size+1)) + return false; + + const size_t n_meas = (halfpatch_size*2+1)*(halfpatch_size*2+1); + list pts; + for(int y = px[1]-halfpatch_size; y<=px[1]+halfpatch_size; ++y) + for(int x = px[0]-halfpatch_size; x<=px[0]+halfpatch_size; ++x) + pts.push_back(cam.cam2world(x,y)*depth.at(y,x)); + + assert(n_meas == pts.size()); + Matrix A; A.resize(n_meas, Eigen::NoChange); + Matrix b; b.resize(n_meas, Eigen::NoChange); + + size_t i = 0; + for(list::iterator it=pts.begin(); it!=pts.end(); ++it) + { + A.row(i) << it->x(), it->y(), it->z(), 1.0; + b[i] = 0; + ++i; + } + + JacobiSVD svd(A, ComputeThinU | ComputeThinV); + + Matrix V = svd.matrixV(); + normal = V.block<3,1>(0,3); + normal.normalize(); + return true; +} + +namespace file_format +{ + +class ImageNameAndPose +{ +public: + ImageNameAndPose() {} + virtual ~ImageNameAndPose() {} + double timestamp_; + std::string image_name_; + Eigen::Vector3d t_; + Eigen::Quaterniond q_; + friend std::ostream& operator <<(std::ostream& out, const ImageNameAndPose& pair); + friend std::istream& operator >>(std::istream& in, ImageNameAndPose& pair); +}; + +std::ostream& operator <<(std::ostream& out, const ImageNameAndPose& gt) +{ + out << gt.timestamp_ << " " << gt.image_name_ << " " + << gt.t_.x() << " " << gt.t_.y() << " " << gt.t_.z() << " " + << gt.q_.x() << " " << gt.q_.y() << " " << gt.q_.z() << " " << gt.q_.w() << " " << std::endl; + return out; +} + +std::istream& operator >>(std::istream& in, ImageNameAndPose& gt) +{ + in >> gt.timestamp_; + in >> gt.image_name_; + double tx, ty, tz, qx, qy, qz, qw; + in >> tx; + in >> ty; + in >> tz; + in >> qx; + in >> qy; + in >> qz; + in >> qw; + gt.t_ = Eigen::Vector3d(tx, ty, tz); + gt.q_ = Eigen::Quaterniond(qw, qx, qy, qz); + gt.q_.normalize(); + return in; +} + +} // namespace file_format +} // namespace blender_utils +} // namespace vk + +#endif // VIKIT_BLENDER_UTILS_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/equidistant_camera.h b/src/rpg_vikit/vikit_common/include/vikit/equidistant_camera.h new file mode 100644 index 0000000..72a5e43 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/equidistant_camera.h @@ -0,0 +1,145 @@ +/* + * equidistant_camera.h + * + * Created on: January 26, 2023 + * Author: xuankuzcr + */ + +#ifndef EQUIDISTANT_CAMERA_H_ +#define EQUIDISTANT_CAMERA_H_ + +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +class EquidistantCamera : public AbstractCamera { + +private: + const double fx_, fy_; + const double cx_, cy_; + bool distortion_; //!< is it pure pinhole model or has it equidistant distortion + double k1_, k2_, k3_, k4_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EquidistantCamera(double width, double height, double scale, + double fx, double fy, double cx, double cy, + double k1=0.0, double k2=0.0, double k3=0.0, double k4=0.0); + + ~EquidistantCamera(); + + virtual Vector3d + cam2world(const double& x, const double& y) const; + + virtual Vector3d + cam2world(const Vector2d& px) const; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const; + + virtual Vector2d + world2cam(const Vector2d& uv) const; + + const Vector2d focal_length() const + { + return Vector2d(fx_, fy_); + } + + inline double thetad_from_theta(const double theta) const + { + const double theta2 = theta * theta; + const double theta4 = theta2 * theta2; + const double theta6 = theta4 * theta2; + const double theta8 = theta4 * theta4; + const double thetad = theta * (1.0 + k1_ * theta2 + k2_ * theta4 + + k3_ * theta6 + k4_ * theta8); + return thetad; + } + + inline double deriv_thetad_from_theta(const double theta) const + { + const double theta2 = theta * theta; + const double theta4 = theta2 * theta2; + const double theta6 = theta4 * theta2; + const double theta8 = theta4 * theta4; + return 1 + 3 * k1_ * theta2 + 5 * k2_ * theta4 + 7 * k3_ * theta6 + + 9 * k4_ * theta8; + } + + inline Eigen::Matrix2d jacobian_2x2(const Eigen::Vector2d& uv) const + { + const double r = uv.norm(); + if (r < 1e-8) + { + return Eigen::Matrix2d::Identity(); + } + + const double inv_r = 1.0 / r; + const double r2 = r * r; + const double dr_du = uv(0) * inv_r; + const double dr_dv = uv(1) * inv_r; + + const double theta = std::atan(r); + const double dtheta_dr = 1.0 / (1 + r * r); + + const double thetad = thetad_from_theta(theta); + const double dthetad_dtheta = deriv_thetad_from_theta(theta); + const double dthetad_dr = dthetad_dtheta * dtheta_dr; + + const double scaling = thetad / r; + const double dscaling_du = (dthetad_dr * dr_du * r - dr_du * thetad) / r2; + const double dscaling_dv = (dthetad_dr * dr_dv * r - dr_dv * thetad) / r2; + + const double dx_du = dscaling_du * uv(0) + scaling; + const double dx_dv = dscaling_dv * uv(0); + const double dy_du = dscaling_du * uv(1); + const double dy_dv = dscaling_dv * uv(1) + scaling; + Eigen::Matrix2d jac; + jac << dx_du, dx_dv, dy_du, dy_dv; + return jac; + } + + inline Eigen::Matrix jacobian_2x3(const Eigen::Vector3d& p) const + { + Eigen::Matrix jac; + const double x = p[0]; + const double y = p[1]; + const double z_inv = 1./p[2]; + const double z_inv_2 = z_inv * z_inv; + jac(0,0) = fx_ * z_inv; + jac(0,1) = 0.0; + jac(0,2) = -fx_ * x * z_inv_2; + jac(1,0) = 0.0; + jac(1,1) = fy_ * z_inv; + jac(1,2) = -fy_ * y * z_inv_2; + return jac; + } + + virtual double errorMultiplier2() const + { + return fabs(fx_); + } + + virtual double errorMultiplier() const + { + return fabs(4.0*fx_*fy_); + } + + virtual double fx() const { return fx_; }; + virtual double fy() const { return fy_; }; + virtual double cx() const { return cx_; }; + virtual double cy() const { return cy_; }; +}; + +} // end namespace vk + + +#endif /* #define EQUIDISTANT_CAMERA_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/file_reader.h b/src/rpg_vikit/vikit_common/include/vikit/file_reader.h new file mode 100644 index 0000000..3c71b56 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/file_reader.h @@ -0,0 +1,101 @@ +/** + * This file is part of dvo. + * + * Copyright 2012 Christian Kerl (Technical University of Munich) + * For more information see . + * + * dvo is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * dvo is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with dvo. If not, see . + */ + +#ifndef VIKIT_FILE_READER_H_ +#define VIKIT_FILE_READER_H_ + +#include +#include + +namespace vk +{ + +/** + * Entry has to support the following operator + * std::istream& operator >>(std::istream&, Entry&); + */ +template +class FileReader +{ +public: + FileReader(const std::string& file) : + hasEntry_(false), + file_(file), + file_stream_(file.c_str()) + {} + + virtual ~FileReader() + { + file_stream_.close(); + } + + void skip(int num_lines) + { + for(int idx = 0; idx < num_lines; ++idx) + { + if(!file_stream_.good()) continue; + file_stream_.ignore(1024, '\n'); + assert(file_stream_.gcount() < 1024); + } + } + + void skipComments() + { + while(file_stream_.good() && file_stream_.peek() == '#') + skip(1); + } + + /// Moves to the next entry in the file. Returns true, if there was a next entry, false otherwise. + bool next() + { + if(file_stream_.good() && !file_stream_.eof()) + { + file_stream_ >> entry_; + hasEntry_ = true; + return true; + } + return false; + } + + /// Read all entries at once. + void readAllEntries(std::vector& entries) + { + if(!hasEntry()) next(); + do + entries.push_back(entry()); + while(next()); + } + + /// Gets the current entry + const Entry& entry() const { return entry_; } + + /// Determines whether the first entry was read + const bool& hasEntry() const { return hasEntry_; } + +private: + bool hasEntry_; + std::string file_; + std::ifstream file_stream_; + Entry entry_; +}; + +} // end namespace vk + +#endif // VIKIT_FILE_READER_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/file_reader_types.h b/src/rpg_vikit/vikit_common/include/vikit/file_reader_types.h new file mode 100644 index 0000000..b4482bf --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/file_reader_types.h @@ -0,0 +1,97 @@ +/* + * file_reader_types.h + * + * Created on: Jul 1, 2014 + * Author: cforster + */ + +#ifndef VIKIT_FILE_READER_TYPES_H_ +#define VIKIT_FILE_READER_TYPES_H_ + +#include +#include +namespace vk { + +/// Common types +namespace file_format { + +/// IMU rotational velocity and linear acceleration +class ImuRotvelLinacc +{ +public: + ImuRotvelLinacc() {} + virtual ~ImuRotvelLinacc() {} + double timestamp_; //!< timestamp in seconds + Eigen::Vector3d w_; //!< angular velocity + Eigen::Vector3d a_; //!< linear acceleration + friend std::ostream& operator <<(std::ostream& out, const ImuRotvelLinacc& pair); + friend std::istream& operator >>(std::istream& in, ImuRotvelLinacc& pair); +}; + +std::ostream& operator <<(std::ostream& out, const ImuRotvelLinacc& gt) +{ + out << gt.timestamp_ << " " + << gt.w_.x() << " " << gt.w_.y() << " " << gt.w_.z() << " " + << gt.a_.x() << " " << gt.a_.y() << " " << gt.a_.z() << std::endl; + return out; +} + +std::istream& operator >>(std::istream& in, ImuRotvelLinacc& gt) +{ + double wx, wy, wz, ax, ay, az; + in >> gt.timestamp_; + in >> wx; + in >> wy; + in >> wz; + in >> ax; + in >> ay; + in >> az; + gt.w_ = Eigen::Vector3d(wx, wy, wz); + gt.a_ = Eigen::Vector3d(ax, ay, az); + return in; +} + + +/// Timestamp with Position and Orientation +class PoseStamped +{ +public: + PoseStamped() {} + virtual ~PoseStamped() {} + double timestamp_; //!< timestamp in seconds + Eigen::Vector3d t_; //!< position + Eigen::Quaterniond q_; //!< orientation + friend std::ostream& operator <<(std::ostream& out, const ImuRotvelLinacc& pair); + friend std::istream& operator >>(std::istream& in, ImuRotvelLinacc& pair); +}; + +std::ostream& operator <<(std::ostream& out, const PoseStamped& gt) +{ + out << gt.timestamp_ << " " + << gt.t_.x() << " " << gt.t_.y() << " " << gt.t_.z() << " " + << gt.q_.x() << " " << gt.q_.y() << " " << gt.q_.z() << " " << gt.q_.w()<< " " + << std::endl; + return out; +} + +std::istream& operator >>(std::istream& in, PoseStamped& gt) +{ + in >> gt.timestamp_; + double tx, ty, tz, qx, qy, qz, qw; + in >> tx; + in >> ty; + in >> tz; + in >> qx; + in >> qy; + in >> qz; + in >> qw; + gt.t_ = Eigen::Vector3d(tx, ty, tz); + gt.q_ = Eigen::Quaterniond(qw, qx, qy, qz); + gt.q_.normalize(); + return in; +} + +} // namespace file_format +} // namespace vk + +#endif // VIKIT_FILE_READER_TYPES_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/homography.h b/src/rpg_vikit/vikit_common/include/vikit/homography.h new file mode 100644 index 0000000..7631787 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/homography.h @@ -0,0 +1,85 @@ +/* + * homography.cpp + * Adaptation of PTAM-GPL HomographyInit class. + * https://github.com/Oxford-PTAM/PTAM-GPL + * Licence: GPLv3 + * Copyright 2008 Isis Innovation Limited + * + * Created on: Sep 2, 2012 + * by: cforster + * + * This class implements the homography decomposition of Faugeras and Lustman's + * 1988 tech report. Code converted to Eigen from PTAM. + * + */ + +#ifndef HOMOGRAPHY_H_ +#define HOMOGRAPHY_H_ + +#include +#include +#include +#include +#include +namespace vk { + +using namespace Eigen; +using namespace std; + +struct HomographyDecomposition +{ + Vector3d t; + Matrix3d R; + double d; + Vector3d n; + + // Resolved Composition + Sophus::SE3 T; //!< second from first + int score; +}; + +class Homography +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Homography (const vector >& _fts1, + const vector >& _fts2, + double _error_multiplier2, + double _thresh_in_px); + + void + calcFromPlaneParams (const Vector3d & normal, + const Vector3d & point_on_plane); + + void + calcFromMatches (); + + size_t + computeMatchesInliers (); + + bool + computeSE3fromMatches (); + + bool + decompose (); + + void + findBestDecomposition (); + + double thresh; + double error_multiplier2; + const vector >& fts_c1; //!< Features on first image on unit plane + const vector >& fts_c2; //!< Features on second image on unit plane + vector inliers; + Sophus::SE3 T_c2_from_c1; //!< Relative translation and rotation of two images + Eigen::Matrix3d H_c2_from_c1; //!< Homography + vector decompositions; +}; + + + + +} /* end namespace vk */ + +#endif /* HOMOGRAPHY_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/img_align.h b/src/rpg_vikit/vikit_common/include/vikit/img_align.h new file mode 100644 index 0000000..ceea10a --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/img_align.h @@ -0,0 +1,153 @@ +/* + * img_align.h + * + * Created on: Aug 22, 2012 + * Author: cforster + */ + +#ifndef IMG_ALIGN_H_ +#define IMG_ALIGN_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; +using namespace vk; +using namespace Sophus; + +//! Forward Compositional Image Alignment +class ForwardCompositionalSE3 : public NLLSSolver<6, Sophus::SE3> { + +protected: + vector& cam_pyr_; + vector& depth_pyr_; + vector& img_pyr_; + vector& tpl_pyr_; + vector& img_pyr_dx_; + vector& img_pyr_dy_; + int level_; + int n_levels_; + PerformanceMonitor permon_; + bool display_; + bool log_; + double res_thresh_; + + virtual double + computeResiduals (const Sophus::SE3& model, bool linearize_system, bool compute_weight_scale = false); + + virtual int + solve(); + + virtual void + update(const ModelType& old_model, ModelType& new_model); + + virtual void + startIteration(); + + virtual void + finishIteration(); + +public: + cv::Mat resimg_; + + ForwardCompositionalSE3( vector& cam_pyr, + vector& depth_pyr, + vector& img_pyr, + vector& tpl_pyr, + vector& img_pyr_dx, + vector& img_pyr_dy, + Sophus::SE3& init_model, + int n_levels, + int n_iter = 50, + float res_thresh = 0.2, + bool display = true, + Method method = LevenbergMarquardt, + int test_id = 0); + + ForwardCompositionalSE3( vector& cam_pyr, + vector& depth_pyr, + vector& img_pyr, + vector& tpl_pyr, + vector& img_pyr_dx, + vector& img_pyr_dy, + int n_levels, + int n_iter = 50, + float res_thresh = 0.2, + bool display = true, + Method method = LevenbergMarquardt, + int test_id = 0); + + void + runOptimization(Sophus::SE3& model, int levelBegin = -1, int levelEnd = -1); + +}; + + +//! Efficient Second Order Minimization (ESM) +class SecondOrderMinimisationSE3 : public NLLSSolver<6, Sophus::SE3> { + +protected: + vector& cam_pyr_; + vector& depth_pyr_; + vector& img_pyr_; + vector& tpl_pyr_; + vector& img_pyr_dx_; + vector& img_pyr_dy_; + vector& tpl_pyr_dx_; + vector& tpl_pyr_dy_; + int level_; + PerformanceMonitor permon_; + bool display_; + bool log_; + float res_thresh_; + + virtual double + computeResiduals (const Sophus::SE3& model, bool linearize_system, bool compute_weight_scale = false); + + virtual int + solve(); + + virtual void + update(const ModelType& old_model, ModelType& new_model); + + virtual void + startIteration(); + + virtual void + finishIteration(); + +public: + cv::Mat resimg_; + + SecondOrderMinimisationSE3( vector& cam_pyr, + vector& depth_pyr, + vector& img_pyr, + vector& tpl_pyr, + vector& img_pyr_dx, + vector& img_pyr_dy, + vector& tpl_pyr_dx, + vector& tpl_pyr_dy, + Sophus::SE3& init_model, + int n_levels, + int n_iter = 50, + float res_thresh = 0.2, + bool display = true, + Method method = LevenbergMarquardt, + int test_id = -1); +}; + +} // end namespace ImgAlign + +#endif /* IMG_ALIGN_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/math_utils.h b/src/rpg_vikit/vikit_common/include/vikit/math_utils.h new file mode 100644 index 0000000..87d6832 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/math_utils.h @@ -0,0 +1,171 @@ +/* + * math_utils.h + * + * Created on: Jul 20, 2012 + * Author: cforster + */ + +#ifndef MATH_UTILS_H_ +#define MATH_UTILS_H_ + + +#include +#include +#include + +namespace vk +{ + +using namespace Eigen; +using namespace std; +using namespace Sophus; + +Vector3d triangulateFeatureNonLin( + const Eigen::Matrix3d& R, + const Vector3d& t, + const Vector3d& feature1, + const Vector3d& feature2); + +/// Assumes the bearing vectors f_c and f_r are on the epipolar plane, i.e. +/// perfect triangulation without noise! +bool depthFromTriangulationExact( + const Eigen::Matrix3d& R_r_c, + const Vector3d& t_r_c, + const Vector3d& f_r, + const Vector3d& f_c, + double& depth_in_r, + double& depth_in_c); + +double reprojError( + const Vector3d& f1, + const Vector3d& f2, + double error_multiplier2); + +double computeInliers( + const vector& features1, + const vector& features2, + const Eigen::Matrix3d& R, + const Vector3d& t, + const double reproj_thresh, + double error_multiplier2, + vector& xyz_vec, + vector& inliers, + vector& outliers); + +void computeInliersOneView( + const vector & feature_sphere_vec, + const vector & xyz_vec, + const Eigen::Matrix3d &R, + const Vector3d &t, + const double reproj_thresh, + const double error_multiplier2, + vector& inliers, + vector& outliers); + +//! Direct Cosine Matrix to Roll Pitch Yaw +Vector3d dcm2rpy(const Eigen::Matrix3d &R); + +//! Roll Pitch Yaw to Direct Cosine Matrix +Eigen::Matrix3d rpy2dcm(const Vector3d &rpy); + +//! Angle Axis parametrization to Quaternion +Quaterniond angax2quat(const Vector3d& n, const double& angle); + +//! Angle Axis parametrization to Matrix representation +Eigen::Matrix3d angax2dcm(const Vector3d& n, const double& angle); + +double sampsonusError( + const Vector2d &v2Dash, + const Eigen::Matrix3d& m3Essential, + const Vector2d& v2); + +inline Eigen::Matrix3d sqew(const Vector3d& v) +{ + Eigen::Matrix3d v_sqew; + v_sqew << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + return v_sqew; +} + +inline double norm_max(const Eigen::VectorXd & v) +{ + double max = -1; + for (int i=0; imax){ + max = abs; + } + } + return max; +} + +inline Vector2d project2d(const Vector3d& v) +{ + return v.head<2>()/v[2]; +} + +inline Vector3d unproject2d(const Vector2d& v) +{ + return Vector3d(v[0], v[1], 1.0); +} + +inline Vector3d project3d(const Vector4d& v) +{ + return v.head<3>()/v[3]; +} + +inline Vector4d unproject3d(const Vector3d& v) +{ + return Vector4d(v[0], v[1], v[2], 1.0); +} + +template +T getMedian(vector& data_vec) +{ + assert(!data_vec.empty()); + typename vector::iterator it = data_vec.begin()+floor(data_vec.size()/2); + nth_element(data_vec.begin(), it, data_vec.end()); + return *it; +} + +inline double pyrFromZero_d(double x_0, int level) +{ + return x_0/(1< & frame_jac) +{ + const double x = xyz[0]; + const double y = xyz[1]; + const double z = xyz[2]; + const double z_2 = z*z; + + frame_jac(0,0) = -1./z *focal_length; + frame_jac(0,1) = 0; + frame_jac(0,2) = x/z_2 *focal_length; + frame_jac(0,3) = x*y/z_2 * focal_length; + frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length; + frame_jac(0,5) = y/z *focal_length; + + frame_jac(1,0) = 0; + frame_jac(1,1) = -1./z *focal_length; + frame_jac(1,2) = y/z_2 *focal_length; + frame_jac(1,3) = (1+y*y/z_2) *focal_length; + frame_jac(1,4) = -x*y/z_2 *focal_length; + frame_jac(1,5) = -x/z *focal_length; +} + +} // end namespace vk + +#endif /* MATH_UTILS_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/nlls_solver.h b/src/rpg_vikit/vikit_common/include/vikit/nlls_solver.h new file mode 100644 index 0000000..e7349c4 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/nlls_solver.h @@ -0,0 +1,169 @@ +/* + * Abstract Nonlinear Least-Squares Solver Class + * + * nlls_solver.h + * + * Created on: Nov 5, 2012 + * Author: cforster + */ + +#ifndef LM_SOLVER_H_ +#define LM_SOLVER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +/** + * \brief Abstract Class for solving nonlinear least-squares (NLLS) problems. + * + * The function implements two algorithms: Levenberg Marquardt and Gauss Newton + * + * Example implementations of this function can be found in the rpl_examples + * package: img_align_2d.cpp, img_align_3d.cpp + * + * Template Parameters: + * D : dimension of the residual + * T : type of the model, e.g. SE2, SE3 + */ + +template +class NLLSSolver { + +public: + typedef T ModelType; + enum Method{GaussNewton, LevenbergMarquardt}; + enum ScaleEstimatorType{UnitScale, TDistScale, MADScale, NormalScale}; + enum WeightFunctionType{UnitWeight, TDistWeight, TukeyWeight, HuberWeight}; + +protected: + Eigen::Matrix H_; //!< Hessian approximation + Eigen::Matrix Jres_; //!< Jacobian x Residual + Eigen::Matrix x_; //!< update step + bool have_prior_; + ModelType prior_; + Eigen::Matrix I_prior_; //!< Prior information matrix (inverse covariance) + double chi2_; + double rho_; + Method method_; + + /// If the flag linearize_system is set, the function must also compute the + /// Jacobian and set the member variables H_, Jres_ + virtual double + computeResiduals (const ModelType& model, + bool linearize_system, + bool compute_weight_scale) = 0; + + /// Solve the linear system H*x = Jres. This function must set the update + /// step in the member variable x_. Must return true if the system could be + /// solved and false if it was singular. + virtual int + solve () = 0; + + virtual void + update (const ModelType& old_model, ModelType& new_model) = 0; + + virtual void + applyPrior (const ModelType& current_model) { } + + virtual void + startIteration () { } + + virtual void + finishIteration () { } + + virtual void + finishTrial () { } + +public: + + /// Damping parameter. If mu > 0, coefficient matrix is positive definite, this + /// ensures that x is a descent direction. If mu is large, x is a short step in + /// the steepest direction. This is good if the current iterate is far from the + /// solution. If mu is small, LM approximates gauss newton iteration and we + /// have (almost) quadratic convergence in the final stages. + double mu_init_, mu_; + double nu_init_, nu_; //!< Increase factor of mu after fail + size_t n_iter_init_, n_iter_; //!< Number of Iterations + size_t n_trials_; //!< Number of trials + size_t n_trials_max_; //!< Max number of trials + size_t n_meas_; //!< Number of measurements + bool stop_; //!< Stop flag + bool verbose_; //!< Output Statistics + double eps_; //!< Stop if update norm is smaller than eps + size_t iter_; //!< Current Iteration + + // robust least squares + bool use_weights_; + float scale_; + robust_cost::ScaleEstimatorPtr scale_estimator_; + robust_cost::WeightFunctionPtr weight_function_; + + NLLSSolver() : + have_prior_(false), + method_(LevenbergMarquardt), + mu_init_(0.01f), + mu_(mu_init_), + nu_init_(2.0), + nu_(nu_init_), + n_iter_init_(15), + n_iter_(n_iter_init_), + n_trials_(0), + n_trials_max_(5), + n_meas_(0), + stop_(false), + verbose_(true), + eps_(0.0000000001), + iter_(0), + use_weights_(false), + scale_(0.0), + scale_estimator_(NULL), + weight_function_(NULL) + { } + + virtual ~NLLSSolver() {} + + /// Calls the GaussNewton or LevenbergMarquardt optimization strategy + void optimize(ModelType& model); + + /// Gauss Newton optimization strategy + void optimizeGaussNewton(ModelType& model); + + /// Levenberg Marquardt optimization strategy + void optimizeLevenbergMarquardt(ModelType& model); + + /// Specify the robust cost that should be used and the appropriate scale estimator + void setRobustCostFunction( + ScaleEstimatorType scale_estimator, + WeightFunctionType weight_function); + + /// Add prior to optimization. + void setPrior( + const ModelType& prior, + const Eigen::Matrix& Information); + + /// Reset all parameters to restart the optimization + void reset(); + + /// Get the squared error + const double& getChi2() const; + + /// The Information matrix is equal to the inverse covariance matrix. + const Eigen::Matrix& getInformationMatrix() const; +}; + +} // end namespace vk + +#include "nlls_solver_impl.hpp" + +#endif /* LM_SOLVER_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/nlls_solver_impl.hpp b/src/rpg_vikit/vikit_common/include/vikit/nlls_solver_impl.hpp new file mode 100644 index 0000000..63a3b74 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/nlls_solver_impl.hpp @@ -0,0 +1,324 @@ +/* + * Abstract Nonlinear Least-Squares Solver Class + * + * nlls_solver.h + * + * Created on: Nov 5, 2012 + * Author: cforster + */ + +#ifndef LM_SOLVER_IMPL_HPP_ +#define LM_SOLVER_IMPL_HPP_ + +#include +#include + +template +void vk::NLLSSolver::optimize(ModelType& model) +{ + if(method_ == GaussNewton) + optimizeGaussNewton(model); + else if(method_ == LevenbergMarquardt) + optimizeLevenbergMarquardt(model); +} + +template +void vk::NLLSSolver::optimizeGaussNewton(ModelType& model) +{ + // Compute weight scale + if(use_weights_) + computeResiduals(model, false, true); + + // Save the old model to rollback in case of unsuccessful update + ModelType old_model(model); + + // perform iterative estimation + for (iter_ = 0; iter_::Identity(D,D); + Jres_.setZero(); + + // compute initial error + n_meas_ = 0; + computeResiduals(model, true, false); + + // add damping term: + H_ += (H_.diagonal()*mu_).asDiagonal(); + + // add prior + if(have_prior_) + applyPrior(model); + + // solve the linear system + if(solve()) + { + // update the model + update(model, new_model); + + // compute error with new model and compare to old error + n_meas_ = 0; + new_chi2 = computeResiduals(new_model, false, false); + rho_ = chi2_-new_chi2; + } + else + { + // matrix was singular and could not be computed + cout << "Matrix is close to singular!" << endl; + cout << "H = " << H_ << endl; + cout << "Jres = " << Jres_ << endl; + rho_ = -1; + } + + if(rho_>0) + { + // update decrased the error -> success + model = new_model; + chi2_ = new_chi2; + stop_ = vk::norm_max(x_)<=eps_; + mu_ *= max(1./3., min(1.-pow(2*rho_-1,3), 2./3.)); + nu_ = 2.; + if(verbose_) + { + cout << "It. " << iter_ + << "\t Trial " << n_trials_ + << "\t Success" + << "\t n_meas = " << n_meas_ + << "\t new_chi2 = " << new_chi2 + << "\t mu = " << mu_ + << "\t nu = " << nu_ + << endl; + } + } + else + { + // update increased the error -> fail + mu_ *= nu_; + nu_ *= 2.; + ++n_trials_; + if (n_trials_ >= n_trials_max_) + stop_ = true; + + if(verbose_) + { + cout << "It. " << iter_ + << "\t Trial " << n_trials_ + << "\t Failure" + << "\t n_meas = " << n_meas_ + << "\t new_chi2 = " << new_chi2 + << "\t mu = " << mu_ + << "\t nu = " << nu_ + << endl; + } + } + + finishTrial(); + + } while(!(rho_>0 || stop_)); + if (stop_) + break; + + finishIteration(); + } +} + + +template +void vk::NLLSSolver::setRobustCostFunction( + ScaleEstimatorType scale_estimator, + WeightFunctionType weight_function) +{ + switch(scale_estimator) + { + case TDistScale: + if(verbose_) + printf("Using TDistribution Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::TDistributionScaleEstimator()); + use_weights_=true; + break; + case MADScale: + if(verbose_) + printf("Using MAD Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::MADScaleEstimator()); + use_weights_=true; + break; + case NormalScale: + if(verbose_) + printf("Using Normal Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::NormalDistributionScaleEstimator()); + use_weights_=true; + break; + default: + if(verbose_) + printf("Using Unit Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::UnitScaleEstimator()); + use_weights_=false; + } + + switch(weight_function) + { + case TDistWeight: + if(verbose_) + printf("Using TDistribution Weight Function\n"); + weight_function_.reset(new robust_cost::TDistributionWeightFunction()); + break; + case TukeyWeight: + if(verbose_) + printf("Using Tukey Weight Function\n"); + weight_function_.reset(new robust_cost::TukeyWeightFunction()); + break; + case HuberWeight: + if(verbose_) + printf("Using Huber Weight Function\n"); + weight_function_.reset(new robust_cost::HuberWeightFunction()); + break; + default: + if(verbose_) + printf("Using Unit Weight Function\n"); + weight_function_.reset(new robust_cost::UnitWeightFunction()); + } +} + +template +void vk::NLLSSolver::setPrior( + const T& prior, + const Eigen::Matrix& Information) +{ + have_prior_ = true; + prior_ = prior; + I_prior_ = Information; +} + +template +void vk::NLLSSolver::reset() +{ + have_prior_ = false; + chi2_ = 1e10; + mu_ = mu_init_; + nu_ = nu_init_; + n_meas_ = 0; + n_iter_ = n_iter_init_; + iter_ = 0; + stop_ = false; +} + +template +inline const double& vk::NLLSSolver::getChi2() const +{ + return chi2_; +} + +template +inline const Eigen::Matrix& vk::NLLSSolver::getInformationMatrix() const +{ + return H_; +} + +#endif /* LM_SOLVER_IMPL_HPP_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/omni_camera.h b/src/rpg_vikit/vikit_common/include/vikit/omni_camera.h new file mode 100644 index 0000000..a17e286 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/omni_camera.h @@ -0,0 +1,87 @@ +/* + * OcamProjector.h + * + * Created on: Sep 22, 2010 + * Author: laurent kneip + */ + +#ifndef OCAMPROJECTOR_H_ +#define OCAMPROJECTOR_H_ + +#include +#include +#include +#include +#include + +#define CMV_MAX_BUF 1024 +#define MAX_POL_LENGTH 64 + +namespace vk { + +using namespace std; +using namespace Eigen; + +struct ocam_model +{ + double pol[MAX_POL_LENGTH]; // the polynomial coefficients: pol[0] + x"pol[1] + x^2*pol[2] + ... + x^(N-1)*pol[N-1] + int length_pol; // length of polynomial + double invpol[MAX_POL_LENGTH]; // the coefficients of the inverse polynomial + int length_invpol; // length of inverse polynomial + double xc; // row coordinate of the center + double yc; // column coordinate of the center + double c; // affine parameter + double d; // affine parameter + double e; // affine parameter + int width; // image width + int height; // image height +}; + +class OmniCamera : public AbstractCamera { + +private: + struct ocam_model ocamModel; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + double error_multiplier_; + + OmniCamera(){} + OmniCamera(string calibFile); + ~OmniCamera(); + + virtual Vector3d + cam2world(const double& x, const double& y) const; + + virtual Vector3d + cam2world(const Vector2d& px) const; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const; + + virtual Vector2d + world2cam(const Vector2d& uv) const; + + double + computeErrorMultiplier(); + + virtual double errorMultiplier2() const + { + return sqrt(error_multiplier_)/2; + } + + virtual double errorMultiplier() const + { + return error_multiplier_; + } + + virtual double fx() const { return 0.0; }; + virtual double fy() const { return 0.0; }; + virtual double cx() const { return 0.0; }; + virtual double cy() const { return 0.0; }; +}; + +} // end namespace vk + +#endif /* OCAMPROJECTOR_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/patch_score.h b/src/rpg_vikit/vikit_common/include/vikit/patch_score.h new file mode 100644 index 0000000..8f0516c --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/patch_score.h @@ -0,0 +1,225 @@ +/* + * patch_score.h + * + * Created on: Dec 5, 2013 + * Author: cforster + */ + +#ifndef VIKIT_PATCH_SCORE_H_ +#define VIKIT_PATCH_SCORE_H_ + +#include + +#if __SSE2__ +#include +#endif + +namespace vk { +namespace patch_score { + + +#if __SSE2__ +// Horizontal sum of uint16s stored in an XMM register +inline int SumXMM_16(__m128i &target) +{ + unsigned short int sums_store[8]; + _mm_storeu_si128((__m128i*)sums_store, target); + return sums_store[0] + sums_store[1] + sums_store[2] + sums_store[3] + + sums_store[4] + sums_store[5] + sums_store[6] + sums_store[7]; +} +// Horizontal sum of uint32s stored in an XMM register +inline int SumXMM_32(__m128i &target) +{ + unsigned int sums_store[4]; + _mm_storeu_si128((__m128i*)sums_store, target); + return sums_store[0] + sums_store[1] + sums_store[2] + sums_store[3]; +} +#endif + +/// Zero Mean Sum of Squared Differences Cost +template +class ZMSSD { +public: + + static const int patch_size_ = 2*HALF_PATCH_SIZE; + static const int patch_area_ = patch_size_*patch_size_; + static const int threshold_ = 2000*patch_area_; + uint8_t* ref_patch_; + int sumA_, sumAA_; + + ZMSSD(uint8_t* ref_patch) : + ref_patch_(ref_patch) + { + uint32_t sumA_uint=0, sumAA_uint=0; + for(int r = 0; r < patch_area_; r++) + { + uint8_t n = ref_patch_[r]; + sumA_uint += n; + sumAA_uint += n*n; + } + sumA_ = sumA_uint; + sumAA_ = sumAA_uint; + } + + static int threshold() { return threshold_; } + + int computeScore(uint8_t* cur_patch) const + { + uint32_t sumB_uint = 0; + uint32_t sumBB_uint = 0; + uint32_t sumAB_uint = 0; + for(int r = 0; r < patch_area_; r++) + { + const uint8_t cur_pixel = cur_patch[r]; + sumB_uint += cur_pixel; + sumBB_uint += cur_pixel*cur_pixel; + sumAB_uint += cur_pixel * ref_patch_[r]; + } + const int sumB = sumB_uint; + const int sumBB = sumBB_uint; + const int sumAB = sumAB_uint; + return sumAA_ - 2*sumAB + sumBB - (sumA_*sumA_ - 2*sumA_*sumB + sumB*sumB)/patch_area_; + } + + int computeScore(uint8_t* cur_patch, int stride) const + { + int sumB, sumBB, sumAB; +#if __SSE2__ + if(patch_size_ == 8) + { + // From PTAM-GPL, Copyright 2008 Isis Innovation Limited + __m128i xImageAsEightBytes; + __m128i xImageAsWords; + __m128i xTemplateAsEightBytes; + __m128i xTemplateAsWords; + __m128i xZero; + __m128i xImageSums; // These sums are 8xuint16 + __m128i xImageSqSums; // These sums are 4xint32 + __m128i xCrossSums; // These sums are 4xint32 + __m128i xProduct; + + xImageSums = _mm_setzero_si128(); + xImageSqSums = _mm_setzero_si128(); + xCrossSums = _mm_setzero_si128(); + xZero = _mm_setzero_si128(); + + uint8_t* imagepointer = cur_patch; + uint8_t* templatepointer = ref_patch_; + long unsigned int cur_stride = stride; + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + sumB = SumXMM_16(xImageSums); + sumAB = SumXMM_32(xCrossSums); + sumBB = SumXMM_32(xImageSqSums); + } + else +#endif + { + uint32_t sumB_uint = 0; + uint32_t sumBB_uint = 0; + uint32_t sumAB_uint = 0; + for(int y=0, r=0; y < patch_size_; ++y) + { + uint8_t* cur_patch_ptr = cur_patch + y*stride; + for(int x=0; x < patch_size_; ++x, ++r) + { + const uint8_t cur_px = cur_patch_ptr[x]; + sumB_uint += cur_px; + sumBB_uint += cur_px * cur_px; + sumAB_uint += cur_px * ref_patch_[r]; + } + } + sumB = sumB_uint; + sumBB = sumBB_uint; + sumAB = sumAB_uint; + } + return sumAA_ - 2*sumAB + sumBB - (sumA_*sumA_ - 2*sumA_*sumB + sumB*sumB)/patch_area_; + } +}; + +} // namespace patch_score +} // namespace vk + +#endif // VIKIT_PATCH_SCORE_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/performance_monitor.h b/src/rpg_vikit/vikit_common/include/vikit/performance_monitor.h new file mode 100644 index 0000000..faf07b8 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/performance_monitor.h @@ -0,0 +1,53 @@ +/* + * performance_monitor.h + * + * Created on: Aug 26, 2011 + * Author: Christian Forster + */ + +#ifndef VIKIT_PERFORMANCE_MONITOR_H +#define VIKIT_PERFORMANCE_MONITOR_H + +#include +#include +#include +#include +#include + +namespace vk +{ + +struct LogItem +{ + double data; + bool set; +}; + +class PerformanceMonitor +{ +public: + PerformanceMonitor(); + ~PerformanceMonitor(); + void init(const std::string& trace_name, const std::string& trace_dir); + void addTimer(const std::string& name); + void addLog(const std::string& name); + void writeToFile(); + void startTimer(const std::string& name); + void stopTimer(const std::string& name); + double getTime(const std::string& name) const; + void log(const std::string& name, double data); + +private: + std::map timers_; + std::map logs_; + std::string trace_name_; // +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +class PinholeCamera : public AbstractCamera { + +private: + const double fx_, fy_; + const double cx_, cy_; + bool distortion_; //!< is it pure pinhole model or has it radial distortion? + double d_[5]; //!< distortion parameters, see http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html + cv::Mat cvK_, cvD_; + cv::Mat undist_map1_, undist_map2_; + bool use_optimization_; + Matrix3d K_; + Matrix3d K_inv_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + PinholeCamera(double width, double height, double scale, + double fx, double fy, double cx, double cy, + double d0=0.0, double d1=0.0, double d2=0.0, double d3=0.0, double d4=0.0); + + ~PinholeCamera(); + + void + initUnistortionMap(); + + virtual Vector3d + cam2world(const double& x, const double& y) const; + + virtual Vector3d + cam2world(const Vector2d& px) const; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const; + + virtual Vector2d + world2cam(const Vector2d& uv) const; + + const Vector2d focal_length() const + { + return Vector2d(fx_, fy_); + } + + virtual double errorMultiplier2() const + { + return fabs(fx_); + } + + virtual double errorMultiplier() const + { + return fabs(4.0*fx_*fy_); + } + + inline const Matrix3d& K() const { return K_; }; + inline const Matrix3d& K_inv() const { return K_inv_; }; + virtual double fx() const { return fx_; }; + virtual double fy() const { return fy_; }; + virtual double cx() const { return cx_; }; + virtual double cy() const { return cy_; }; + inline double d0() const { return d_[0]; }; + inline double d1() const { return d_[1]; }; + inline double d2() const { return d_[2]; }; + inline double d3() const { return d_[3]; }; + inline double d4() const { return d_[4]; }; + + void undistortImage(const cv::Mat& raw, cv::Mat& rectified); + +}; + +} // end namespace vk + + +#endif /* PINHOLE_CAMERA_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/polynomial_camera.h b/src/rpg_vikit/vikit_common/include/vikit/polynomial_camera.h new file mode 100644 index 0000000..ab9e307 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/polynomial_camera.h @@ -0,0 +1,153 @@ +/* + * polynomial_camera.h + * + * Created on: January 26, 2023 + * Author: xuankuzcr + */ + +#ifndef POLYNOMIAL_CAMERA_H_ +#define POLYNOMIAL_CAMERA_H_ + +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +class PolynomialCamera : public AbstractCamera { + +private: + const double fx_, fy_; + const double cx_, cy_; + const double skew_; + bool distortion_; //!< is it pure pinhole model or has it equidistant distortion + double k2_, k3_, k4_, k5_, k6_, k7_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + // PolynomialCamera(double width, double height, double scale, + // double fx, double fy, double cx, double cy, double skew, + // double k2=0.0, double k3=0.0, double k4=0.0, double k5=0.0, double k6=0.0, double k7=0.0); + + PolynomialCamera(double width, double height, // double scale, + double fx, double fy, double cx, double cy, double skew, + double k2=0.0, double k3=0.0, double k4=0.0, double k5=0.0, double k6=0.0, double k7=0.0); + + ~PolynomialCamera(); + + virtual Vector3d + cam2world(const double& x, const double& y) const; + + virtual Vector3d + cam2world(const Vector2d& px) const; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const; + + virtual Vector2d + world2cam(const Vector2d& uv) const; + + const Vector2d focal_length() const + { + return Vector2d(fx_, fy_); + } + + inline double thetad_from_theta(const double theta) const + { + const double theta2 = theta * theta; + const double theta3 = theta2 * theta; + const double theta4 = theta3 * theta; + const double theta5 = theta4 * theta; + const double theta6 = theta5 * theta; + const double theta7 = theta6 * theta; + const double thetad = theta + k2_ * theta2 + k3_ * theta3 + + k4_ * theta4 + k5_ * theta5 + k6_ * theta6 + k7_ * theta7; + return thetad; + } + + inline double deriv_thetad_from_theta(const double theta) const + { + const double theta2 = theta * theta; + const double theta3 = theta2 * theta; + const double theta4 = theta3 * theta; + const double theta5 = theta4 * theta; + const double theta6 = theta5 * theta; + return 1 + 2 * k2_ * theta + 3 * k3_ * theta2 + 4 * k4_ * theta3 + 5 * k5_ * theta4 + + 6 * k6_ * theta5 + 7 * k7_ * theta6; + } + + inline Eigen::Matrix2d jacobian_2x2(const Eigen::Vector2d& uv) const + { + const double r = uv.norm(); + if (r < 1e-8) + { + return Eigen::Matrix2d::Identity(); + } + + const double inv_r = 1.0 / r; + const double r2 = r * r; + const double dr_du = uv(0) * inv_r; + const double dr_dv = uv(1) * inv_r; + + const double theta = std::atan(r); + const double dtheta_dr = 1.0 / (1 + r * r); + + const double thetad = thetad_from_theta(theta); + const double dthetad_dtheta = deriv_thetad_from_theta(theta); + const double dthetad_dr = dthetad_dtheta * dtheta_dr; + + const double scaling = thetad / r; + const double dscaling_du = (dthetad_dr * dr_du * r - dr_du * thetad) / r2; + const double dscaling_dv = (dthetad_dr * dr_dv * r - dr_dv * thetad) / r2; + + const double dx_du = dscaling_du * uv(0) + scaling; + const double dx_dv = dscaling_dv * uv(0); + const double dy_du = dscaling_du * uv(1); + const double dy_dv = dscaling_dv * uv(1) + scaling; + Eigen::Matrix2d jac; + jac << dx_du, dx_dv, dy_du, dy_dv; + return jac; + } + + inline Eigen::Matrix jacobian_2x3(const Eigen::Vector3d& p) const + { + Eigen::Matrix jac; + const double x = p[0]; + const double y = p[1]; + const double z_inv = 1./p[2]; + const double z_inv_2 = z_inv * z_inv; + jac(0,0) = fx_ * z_inv; + jac(0,1) = skew_ * z_inv; + jac(0,2) = -fx_ * x * z_inv_2; + jac(1,0) = 0.0; + jac(1,1) = fy_ * z_inv; + jac(1,2) = -fy_ * y * z_inv_2; + return jac; + } + + virtual double errorMultiplier2() const + { + return fabs(fx_); + } + + virtual double errorMultiplier() const + { + return fabs(4.0*fx_*fy_); + } + + virtual double fx() const { return fx_; }; + virtual double fy() const { return fy_; }; + virtual double cx() const { return cx_; }; + virtual double cy() const { return cy_; }; +}; + +} // end namespace vk + + +#endif /* #define POLYNOMIAL_CAMERA_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/ringbuffer.h b/src/rpg_vikit/vikit_common/include/vikit/ringbuffer.h new file mode 100644 index 0000000..caa2dc6 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/ringbuffer.h @@ -0,0 +1,130 @@ +// This file is part of VisionTools. +// +// Copyright 2011 Hauke Strasdat (Imperial College London) +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef VISIONTOOLS_RING_BUFFER_H +#define VISIONTOOLS_RING_BUFFER_H + +#include +#include +#include + +namespace vk +{ + +template +class RingBuffer +{ +public: + RingBuffer (int size); + + void + push_back (const T & elem); + + bool + empty () const; + + T + get (int i); + + T + getSum () const; + + T + getMean () const; + + int size() + { + return num_elem_; + } + +private: + std::vector arr_; + int begin_; + int end_; + int num_elem_; + int arr_size_; +}; + +template +RingBuffer +::RingBuffer(int size) : + arr_(size), + begin_(0), + end_(-1), + num_elem_(0), + arr_size_(size) +{} + +template +bool RingBuffer +::empty() const +{ + return arr_.empty(); +} + +template +void RingBuffer +::push_back(const T & elem) +{ + if (num_elem_ +T RingBuffer +::get(int i) +{ + assert(i +T RingBuffer +::getSum() const +{ + T sum=0; + for(int i=0; i +T RingBuffer +::getMean() const +{ + if(num_elem_ == 0) + return 0; + return getSum()/num_elem_; +} + +} + +#endif diff --git a/src/rpg_vikit/vikit_common/include/vikit/robust_cost.h b/src/rpg_vikit/vikit_common/include/vikit/robust_cost.h new file mode 100644 index 0000000..d03881a --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/robust_cost.h @@ -0,0 +1,156 @@ +/** +* This file is part of dvo. +* +* Copyright 2012 Christian Kerl (Technical University of Munich) +* For more information see . +* +* dvo is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* dvo is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with dvo. If not, see . +*/ + +#ifndef VIKIT_ROBUST_COST_H_ +#define VIKIT_ROBUST_COST_H_ + +#include +#include +#include +namespace vk { +namespace robust_cost { + +// interface for scale estimators +class ScaleEstimator +{ +public: + virtual ~ScaleEstimator() {}; + virtual float compute(std::vector& errors) const = 0; +}; +typedef std::shared_ptr ScaleEstimatorPtr; + +class UnitScaleEstimator : public ScaleEstimator +{ +public: + UnitScaleEstimator() {} + virtual ~UnitScaleEstimator() {} + virtual float compute(std::vector& errors) const { return 1.0f; }; +}; + +// estimates scale by fitting a t-distribution to the data with the given degrees of freedom +class TDistributionScaleEstimator : public ScaleEstimator +{ +public: + TDistributionScaleEstimator(const float dof = DEFAULT_DOF); + virtual ~TDistributionScaleEstimator() {}; + virtual float compute(std::vector& errors) const; + + static const float DEFAULT_DOF; + static const float INITIAL_SIGMA; +protected: + float dof_; + float initial_sigma_; +}; + +// estimates scale by computing the median absolute deviation +class MADScaleEstimator : public ScaleEstimator +{ +public: + MADScaleEstimator() {}; + virtual ~MADScaleEstimator() {}; + virtual float compute(std::vector& errors) const; + +private: + static const float NORMALIZER;; +}; + +// estimates scale by computing the standard deviation +class NormalDistributionScaleEstimator : public ScaleEstimator +{ +public: + NormalDistributionScaleEstimator() {}; + virtual ~NormalDistributionScaleEstimator() {}; + virtual float compute(std::vector& errors) const; +private: +}; + +/** + * Interface for weight functions. A weight function is the first derivative of a symmetric robust function p(sqrt(t)). + * The errors are assumed to be normalized to unit variance. + * + * See: + * "Lucas-Kanade 20 Years On: A Unifying Framework: Part 2" - Page 23, Equation (54) + */ +class WeightFunction +{ +public: + virtual ~WeightFunction() {}; + virtual float value(const float& x) const = 0; + virtual void configure(const float& param) {}; +}; +typedef std::shared_ptr WeightFunctionPtr; + +class UnitWeightFunction : public WeightFunction +{ +public: + UnitWeightFunction() {}; + virtual ~UnitWeightFunction() {}; + virtual float value(const float& x) const { return 1.0f; }; +}; + +/** + * Tukey's hard re-descending function. + * + * See: + * http://en.wikipedia.org/wiki/Redescending_M-estimator + */ +class TukeyWeightFunction : public WeightFunction +{ +public: + TukeyWeightFunction(const float b = DEFAULT_B); + virtual ~TukeyWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_B; +private: + float b_square; +}; + +class TDistributionWeightFunction : public WeightFunction +{ +public: + TDistributionWeightFunction(const float dof = DEFAULT_DOF); + virtual ~TDistributionWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_DOF; +private: + float dof_; + float normalizer_; +}; + +class HuberWeightFunction : public WeightFunction +{ +public: + HuberWeightFunction(const float k = DEFAULT_K); + virtual ~HuberWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_K; +private: + float k; +}; + +} // namespace robust_cost +} // namespace vk +#endif // VIKIT_ROBUST_COST_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/sample.h b/src/rpg_vikit/vikit_common/include/vikit/sample.h new file mode 100644 index 0000000..a0a047f --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/sample.h @@ -0,0 +1,50 @@ +#ifndef VIKIT_SAMPLE_H_ +#define VIKIT_SAMPLE_H_ + +#include +#include + +namespace vk { + +class Sample +{ +public: + static void setTimeBasedSeed(); + static int uniform(int from, int to); + static double uniform(); + static double gaussian(double sigma); + static std::ranlux24 gen_real; + static std::mt19937 gen_int; +}; + +std::ranlux24 Sample::gen_real; +std::mt19937 Sample::gen_int; + +void Sample::setTimeBasedSeed() +{ + unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); + gen_real = std::ranlux24(seed); + gen_int = std::mt19937(seed); +} + +int Sample::uniform(int from, int to) +{ + std::uniform_int_distribution distribution(from, to); + return distribution(gen_int); +} + +double Sample::uniform() +{ + std::uniform_real_distribution distribution(0.0, 1.0); + return distribution(gen_real); +} + +double Sample::gaussian(double stddev) +{ + std::normal_distribution distribution(0.0, stddev); + return distribution(gen_real); +} + +} // namespace vk + +#endif // VIKIT_SAMPLE_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/timer.h b/src/rpg_vikit/vikit_common/include/vikit/timer.h new file mode 100644 index 0000000..45bbc7f --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/timer.h @@ -0,0 +1,81 @@ +#ifndef TIMER_H +#define TIMER_H + +#include +#include +#include + +namespace vk +{ + +class Timer +{ +private: + timeval start_time_; + double time_; + double accumulated_; +public: + + /// The constructor directly starts the timer. + Timer() : + time_(0.0), + accumulated_(0.0) + { + start(); + } + + ~Timer() + {} + + inline void start() + { + accumulated_ = 0.0; + gettimeofday(&start_time_, NULL); + } + + inline void resume() + { + gettimeofday(&start_time_, NULL); + } + + inline double stop() + { + timeval end_time; + gettimeofday(&end_time, NULL); + long seconds = end_time.tv_sec - start_time_.tv_sec; + long useconds = end_time.tv_usec - start_time_.tv_usec; + time_ = ((seconds) + useconds*0.000001) + accumulated_; + accumulated_ = time_; + return time_; + } + + inline double getTime() const + { + return time_; + } + + inline void reset() + { + time_ = 0.0; + accumulated_ = 0.0; + } + + static double getCurrentTime() + { + timeval time_now; + gettimeofday(&time_now, NULL); + return time_now.tv_sec + time_now.tv_usec*0.000001; + } + + static double getCurrentSecond() + { + timeval time_now; + gettimeofday(&time_now, NULL); + return time_now.tv_sec; + } + +}; + +} // end namespace vk + +#endif diff --git a/src/rpg_vikit/vikit_common/include/vikit/user_input_thread.h b/src/rpg_vikit/vikit_common/include/vikit/user_input_thread.h new file mode 100644 index 0000000..a92cd15 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/user_input_thread.h @@ -0,0 +1,54 @@ +#ifndef USER_INPUT_THREAD_H +#define USER_INPUT_THREAD_H + +#include +#include + +namespace vk { + +/// A class that starts its own thread and listens to the console input. The +/// console input can then be inquired using the getInput() function. +class UserInputThread +{ +public: + UserInputThread(); + ~UserInputThread(); + + /// Returns the latest acquired user input. Default is set to 0. + /// Once this function is called, the input state is reset to the default. + char getInput(); + + /// Stop the thread + void stop(); + +private: + + /// Main loop that waits for new user input + void acquireUserInput(); + + /// Initialize new terminal i/o settings + void initTermios(int echo); + + /// Restore old terminal i/o settings + void resetTermios(); + + /// Read 1 character - echo defines echo mode + int getch_(int echo); + + /// Read 1 character without echo + int getch(); + + /// Read 1 character with echo + int getche(); + + bool stop_; + std::thread * user_input_thread_; + char input_; + + struct termios original_terminal_settings_; + struct termios old_terminal_settings_, new_terminal_settings_; +}; + +} // end namespace vk + +#endif /* USER_INPUT_THREAD_H */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/vision.h b/src/rpg_vikit/vikit_common/include/vikit/vision.h new file mode 100644 index 0000000..b1e5304 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/vision.h @@ -0,0 +1,77 @@ +/* + * vision.h + * + * Created on: May 14, 2013 + * Author: cforster + */ + +#ifndef VIKIT_VISION_H_ +#define VIKIT_VISION_H_ + +#include +#include +#include + +namespace vk +{ + +//! Return value between 0 and 1 +//! WARNING This function does not check whether the x/y is within the border +inline float +interpolateMat_32f(const cv::Mat& mat, float u, float v) +{ + assert(mat.type()==CV_32F); + float x = floor(u); + float y = floor(v); + float subpix_x = u-x; + float subpix_y = v-y; + float wx0 = 1.0-subpix_x; + float wx1 = subpix_x; + float wy0 = 1.0-subpix_y; + float wy1 = subpix_y; + + float val00 = mat.at(y,x); + float val10 = mat.at(y,x+1); + float val01 = mat.at(y+1,x); + float val11 = mat.at(y+1,x+1); + return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11; +} + +//! Return value between 0 and 255 +//! WARNING This function does not check whether the x/y is within the border +inline float +interpolateMat_8u(const cv::Mat& mat, float u, float v) +{ + assert(mat.type()==CV_8U); + int x = floor(u); + int y = floor(v); + float subpix_x = u-x; + float subpix_y = v-y; + + float w00 = (1.0f-subpix_x)*(1.0f-subpix_y); + float w01 = (1.0f-subpix_x)*subpix_y; + float w10 = subpix_x*(1.0f-subpix_y); + float w11 = 1.0f - w00 - w01 - w10; + + const int stride = mat.step.p[0]; + unsigned char* ptr = mat.data + y*stride + x; + return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1]; +} + +void halfSample(const cv::Mat& in, cv::Mat& out); + +float shiTomasiScore(const cv::Mat& img, int u, int v); + +void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst); + +#ifdef __SSE2__ + +/// Used to convert a Kinect depthmap +/// Code by Christian Kerl DVO, GPL Licence +void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale); + +#endif + +} // namespace vk + +#endif // VIKIT_VISION_H_ diff --git a/src/rpg_vikit/vikit_common/package.xml b/src/rpg_vikit/vikit_common/package.xml new file mode 100644 index 0000000..4717938 --- /dev/null +++ b/src/rpg_vikit/vikit_common/package.xml @@ -0,0 +1,30 @@ + + + vikit_common + 0.0.0 + + The vikit_common package + + + cforster + + + GPLv3 + + + ament_cmake + + + rclcpp + cmake_modules + + + rclcpp + + + + + + + + \ No newline at end of file diff --git a/src/rpg_vikit/vikit_common/src/atan_camera.cpp b/src/rpg_vikit/vikit_common/src/atan_camera.cpp new file mode 100644 index 0000000..c35bb54 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/atan_camera.cpp @@ -0,0 +1,86 @@ +/* + * atan_camera.cpp + * + * Created on: Aug 21, 2012 + * Author: cforster + */ + + +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +ATANCamera:: +ATANCamera(double width, double height, + double fx, double fy, + double cx, double cy, + double s) : + AbstractCamera(width, height, 1.0), + fx_(width*fx), fy_(height*fy), + fx_inv_(1.0/fx_), fy_inv_(1.0/fy_), + cx_(cx*width - 0.5), cy_(cy*height - 0.5), + s_(s), s_inv_(1.0/s_) +{ + if(s_ != 0.0) + { + tans_ = 2.0 * tan(s_ / 2.0); + tans_inv_ = 1.0 / tans_; + s_inv_ = 1.0 / s_; + distortion_ = true; + } + else + { + s_inv_ = 0.0; + tans_ = 0.0; + distortion_ = false; + } +} + +ATANCamera:: +~ATANCamera() +{} + +Vector3d ATANCamera:: +cam2world(const double& x, const double& y) const +{ + Vector2d dist_cam((x - cx_) * fx_inv_, + (y - cy_) * fy_inv_); + double dist_r = dist_cam.norm(); + double r = invrtrans(dist_r); + double d_factor; + if(dist_r > 0.01) + d_factor = r / dist_r; + else + d_factor = 1.0; + return unproject2d(d_factor * dist_cam).normalized(); +} + +Vector3d ATANCamera:: +cam2world (const Vector2d& px) const +{ + return cam2world(px[0], px[1]); +} + +Vector2d ATANCamera:: +world2cam(const Vector3d& xyz_c) const +{ + return world2cam(project2d(xyz_c)); +} + +Vector2d ATANCamera:: +world2cam(const Vector2d& uv) const +{ + double r = uv.norm(); + double factor = rtrans_factor(r); + Vector2d dist_cam = factor * uv; + return Vector2d(cx_ + fx_ * dist_cam[0], + cy_ + fy_ * dist_cam[1]); +} + +} /* end vk */ diff --git a/src/rpg_vikit/vikit_common/src/equidistant_camera.cpp b/src/rpg_vikit/vikit_common/src/equidistant_camera.cpp new file mode 100644 index 0000000..2fa5224 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/equidistant_camera.cpp @@ -0,0 +1,111 @@ +/* + * equidistant_camera.cpp + * + * Created on: January 26, 2023 + * Author: xuankuzcr + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +EquidistantCamera:: +EquidistantCamera(double width, double height, double scale, + double fx, double fy, + double cx, double cy, + double k1, double k2, double k3, double k4) : + AbstractCamera(width * scale , height * scale, scale), + fx_(fx * scale), fy_(fy * scale), cx_(cx * scale), cy_(cy * scale), + distortion_(fabs(k1) > 0.0000001) +{ + cout << "scale: " << scale << endl; + k1_ = k1; k2_ = k2; k3_ = k3; k4_ = k4; +} + +EquidistantCamera:: +~EquidistantCamera() +{} + +Vector3d EquidistantCamera:: +cam2world(const double& u, const double& v) const +{ + Vector3d xyz; + if(!distortion_) + { + xyz[0] = (u - cx_)/fx_; + xyz[1] = (v - cy_)/fy_; + xyz[2] = 1.0; + } + else + { + double x = (u - cx_)/fx_; + double y = (v - cy_)/fy_; + const double thetad = std::sqrt(x * x + y * y); + double theta = thetad; + for (int i = 0; i < 5; ++i) + { + const double theta2 = theta * theta; + const double theta4 = theta2 * theta2; + const double theta6 = theta4 * theta2; + const double theta8 = theta4 * theta4; + theta = thetad / + (1.0 + k1_ * theta2 + k2_ * theta4 + k3_ * theta6 + k4_ * theta8); + } + const double scaling = std::tan(theta) / thetad; + x *= scaling; + y *= scaling; + xyz[0] = x; + xyz[1] = y; + xyz[2] = 1.0; + } + return xyz.normalized(); +} + +Vector3d EquidistantCamera:: +cam2world (const Vector2d& uv) const +{ + return cam2world(uv[0], uv[1]); +} + +Vector2d EquidistantCamera:: +world2cam(const Vector3d& xyz) const +{ + return world2cam(project2d(xyz)); +} + +Vector2d EquidistantCamera:: +world2cam(const Vector2d& uv) const +{ + Vector2d px; + if(!distortion_) + { + px[0] = fx_*uv[0] + cx_; + px[1] = fy_*uv[1] + cy_; + } + else + { + double xd, yd; + const double r = uv.norm(); + if (r < 1e-8) + { + return uv; + } + const double theta = std::atan(r); + const double thetad = thetad_from_theta(theta); + const double scaling = thetad / r; + xd = uv[0] * scaling; + yd = uv[1] * scaling; + px[0] = xd*fx_ + cx_; + px[1] = yd*fy_ + cy_; + } + return px; +} + +} // end namespace vk diff --git a/src/rpg_vikit/vikit_common/src/homography.cpp b/src/rpg_vikit/vikit_common/src/homography.cpp new file mode 100644 index 0000000..785f519 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/homography.cpp @@ -0,0 +1,283 @@ +/* + * homography.cpp + * Adaptation of PTAM-GPL HomographyInit class. + * https://github.com/Oxford-PTAM/PTAM-GPL + * Licence: GPLv3 + * Copyright 2008 Isis Innovation Limited + * + * Created on: Sep 2, 2012 + * by: cforster + */ + +#include +#include +#include + +namespace vk { + +Homography:: +Homography(const vector >& _fts1, + const vector >& _fts2, + double _error_multiplier2, + double _thresh_in_px) : + thresh(_thresh_in_px), + error_multiplier2(_error_multiplier2), + fts_c1(_fts1), + fts_c2(_fts2) +{ +} + +void Homography:: +calcFromPlaneParams(const Vector3d& n_c1, const Vector3d& xyz_c1) +{ + double d = n_c1.dot(xyz_c1); // normal distance from plane to KF + H_c2_from_c1 = T_c2_from_c1.rotationMatrix() + (T_c2_from_c1.translation()*n_c1.transpose())/d; +} + +void Homography:: +calcFromMatches() +{ + vector src_pts(fts_c1.size()), dst_pts(fts_c1.size()); + for(size_t i=0; i(0,0); + H_c2_from_c1(0,1) = cvH.at(0,1); + H_c2_from_c1(0,2) = cvH.at(0,2); + H_c2_from_c1(1,0) = cvH.at(1,0); + H_c2_from_c1(1,1) = cvH.at(1,1); + H_c2_from_c1(1,2) = cvH.at(1,2); + H_c2_from_c1(2,0) = cvH.at(2,0); + H_c2_from_c1(2,1) = cvH.at(2,1); + H_c2_from_c1(2,2) = cvH.at(2,2); +} + +size_t Homography:: +computeMatchesInliers() +{ + inliers.clear(); inliers.resize(fts_c1.size()); + size_t n_inliers = 0; + for(size_t i=0; i svd(H_c2_from_c1, ComputeThinU | ComputeThinV); + + Vector3d singular_values = svd.singularValues(); + + double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT) + double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book. + double d3 = fabs(singular_values[2]); + + Matrix3d U = svd.matrixU(); + Matrix3d V = svd.matrixV(); // VT^T + + double s = U.determinant() * V.determinant(); + + double dPrime_PM = d2; + + int nCase; + if(d1 != d2 && d2 != d3) + nCase = 1; + else if( d1 == d2 && d2 == d3) + nCase = 3; + else + nCase = 2; + + if(nCase != 1) + { + printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. "); + return false; + } + + double x1_PM; + double x2; + double x3_PM; + + // All below deals with the case = 1 case. + // Case 1 implies (d1 != d3) + { // Eq. 12 + x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3)); + x2 = 0; + x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3)); + }; + + double e1[4] = {1.0,-1.0, 1.0,-1.0}; + double e3[4] = {1.0, 1.0,-1.0,-1.0}; + + Vector3d np; + HomographyDecomposition decomp; + + // Case 1, d' > 0: + decomp.d = s * dPrime_PM; + for(size_t signs=0; signs<4; signs++) + { + // Eq 13 + decomp.R = Matrix3d::Identity(); + double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2; + double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2; + decomp.R(0,0) = dCosTheta; + decomp.R(0,2) = -dSinTheta; + decomp.R(2,0) = dSinTheta; + decomp.R(2,2) = dCosTheta; + + // Eq 14 + decomp.t[0] = (d1 - d3) * x1_PM * e1[signs]; + decomp.t[1] = 0.0; + decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs]; + + np[0] = x1_PM * e1[signs]; + np[1] = x2; + np[2] = x3_PM * e3[signs]; + decomp.n = V * np; + + decompositions.push_back(decomp); + } + + // Case 1, d' < 0: + decomp.d = s * -dPrime_PM; + for(size_t signs=0; signs<4; signs++) + { + // Eq 15 + decomp.R = -1 * Matrix3d::Identity(); + double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2; + double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2; + decomp.R(0,0) = dCosPhi; + decomp.R(0,2) = dSinPhi; + decomp.R(2,0) = dSinPhi; + decomp.R(2,2) = -dCosPhi; + + // Eq 16 + decomp.t[0] = (d1 + d3) * x1_PM * e1[signs]; + decomp.t[1] = 0.0; + decomp.t[2] = (d1 + d3) * x3_PM * e3[signs]; + + np[0] = x1_PM * e1[signs]; + np[1] = x2; + np[2] = x3_PM * e3[signs]; + decomp.n = V * np; + + decompositions.push_back(decomp); + } + + // Save rotation and translation of the decomposition + for(unsigned int i=0; i(R, t); + } + return true; +} + +bool operator<(const HomographyDecomposition lhs, const HomographyDecomposition rhs) +{ + return lhs.score < rhs.score; +} + +void Homography:: +findBestDecomposition() +{ + assert(decompositions.size() == 8); + for(size_t i=0; i 0.0) + nPositive++; + } + decom.score = -nPositive; + } + + sort(decompositions.begin(), decompositions.end()); + decompositions.resize(4); + + for(size_t i=0; i 0.0) + nPositive++; + }; + decom.score = -nPositive; + } + + sort(decompositions.begin(), decompositions.end()); + decompositions.resize(2); + + // According to Faugeras and Lustman, ambiguity exists if the two scores are equal + // but in practive, better to look at the ratio! + double dRatio = (double) decompositions[1].score / (double) decompositions[0].score; + + if(dRatio < 0.9) // no ambiguity! + decompositions.erase(decompositions.begin() + 1); + else // two-way ambiguity. Resolve by sampsonus score of all points. + { + double dErrorSquaredLimit = thresh * thresh * 4; + double adSampsonusScores[2]; + for(size_t i=0; i<2; i++) + { + Sophus::SE3 T = decompositions[i].T; + Sophus::Matrix3d Essential = T.rotationMatrix() * sqew(T.translation()); + double dSumError = 0; + for(size_t m=0; m < fts_c1.size(); m++ ) + { + double d = sampsonusError(fts_c1[m], Essential, fts_c2[m]); + if(d > dErrorSquaredLimit) + d = dErrorSquaredLimit; + dSumError += d; + } + adSampsonusScores[i] = dSumError; + } + + if(adSampsonusScores[0] <= adSampsonusScores[1]) + decompositions.erase(decompositions.begin() + 1); + else + decompositions.erase(decompositions.begin()); + } +} + + +} /* end namespace vk */ diff --git a/src/rpg_vikit/vikit_common/src/img_align.cpp b/src/rpg_vikit/vikit_common/src/img_align.cpp new file mode 100644 index 0000000..f84ae8b --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/img_align.cpp @@ -0,0 +1,443 @@ +/* + * img_align.cpp + * + * Created on: Aug 22, 2012 + * Author: cforster + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +namespace vk { + +/******************************************************************************* + * Forward Compositional + */ +ForwardCompositionalSE3:: +ForwardCompositionalSE3( vector& cam_pyr, + vector& depth_pyr, + vector& img_pyr, + vector& tpl_pyr, + vector& img_pyr_dx, + vector& img_pyr_dy, + Sophus::SE3& init_model, + int n_levels, + int n_iter, + float res_thresh, + bool display, + Method method, + int test_id) : + cam_pyr_(cam_pyr), + depth_pyr_(depth_pyr), + img_pyr_(img_pyr), + tpl_pyr_(tpl_pyr), + img_pyr_dx_(img_pyr_dx), + img_pyr_dy_(img_pyr_dy), + display_(display), + log_(test_id < 0), + res_thresh_(res_thresh) +{ + n_iter_ = n_iter; + method_ = method; + + // Init Performance Monitor +#if 0 + if(log_) + { + permon_.init("forward", ament_index_cpp::get_package_share_directory("rpl_examples") + "/trace/img_align/data", + test_id, true); + permon_.addLog("iter"); + permon_.addLog("level"); + permon_.addLog("mu"); + permon_.addLog("chi2"); + permon_.addLog("trials"); + } +#endif + + runOptimization(init_model); + +} + +ForwardCompositionalSE3:: +ForwardCompositionalSE3( vector& cam_pyr, + vector& depth_pyr, + vector& img_pyr, + vector& tpl_pyr, + vector& img_pyr_dx, + vector& img_pyr_dy, + int n_levels, + int n_iter, + float res_thresh, + bool display, + Method method, + int test_id) : + cam_pyr_(cam_pyr), + depth_pyr_(depth_pyr), + img_pyr_(img_pyr), + tpl_pyr_(tpl_pyr), + img_pyr_dx_(img_pyr_dx), + img_pyr_dy_(img_pyr_dy), + display_(display), + log_(test_id < 0), + res_thresh_(res_thresh) +{ + n_iter_ = n_iter; + method_ = method; + + // Init Performance Monitor +#if 0 + if(log_) + { + permon_.init("forward", ament_index_cpp::get_package_share_directory("rpl_examples") + "/trace/img_align/data", + test_id, true); + permon_.addLog("iter"); + permon_.addLog("level"); + permon_.addLog("mu"); + permon_.addLog("chi2"); + permon_.addLog("trials"); + } +#endif + +} + +void ForwardCompositionalSE3:: +runOptimization(Sophus::SE3& model, int levelBegin, int levelEnd) +{ + if(levelBegin < 0 || levelBegin > n_levels_-1) + levelBegin = n_levels_-1; + if(levelEnd < 0) + levelEnd = 1; + + // Perform Pyramidal optimization + for(level_=levelBegin; level_>=levelEnd; --level_) + { + mu_ = 0.1; + cout << endl << "PYRAMID LEVEL " << level_ + << endl << "---------------" << endl; + optimize(model); + } +} + +double ForwardCompositionalSE3:: +computeResiduals (const Sophus::SE3& model, bool linearize_system, bool compute_weight_scale) +{ + // Warp the image such that it aligns with the template image + double chi2 = 0; + size_t n_pixels = 0; + + if(linearize_system) + resimg_ = cv::Mat(tpl_pyr_[level_].size(), CV_32F, cv::Scalar(1)); + + for( int v=0; v(v,u); + Vector3d xyz_tpl(cv_float3[0], cv_float3[1], cv_float3[2]); + Vector3d xyz_img(model*xyz_tpl); + Vector2f uv_img_pyr = cam_pyr_[level_].world2cam(xyz_img).cast(); // apply cam model + if( cam_pyr_[level_].isInFrame(uv_img_pyr.cast(), 2) ) + { + // compare image values + float intensity_tpl = + tpl_pyr_[level_].at(v,u); + float intensity_img = + interpolateMat_32f(img_pyr_[level_], uv_img_pyr[0], uv_img_pyr[1]); + + // compute residual (opposite to 2d case because of other jacobian) + float res = intensity_tpl-intensity_img; + + // robustification + if(res > res_thresh_) res = res_thresh_; + if(res < -res_thresh_) res = -res_thresh_; + chi2 += res*res; + n_pixels++; + + if(linearize_system) + { + // get gradient of warped image (~gradient at warped position) + float dx = 0.5*interpolateMat_32f(img_pyr_dx_[level_], uv_img_pyr[0], uv_img_pyr[1]); + float dy = 0.5*interpolateMat_32f(img_pyr_dy_[level_], uv_img_pyr[0], uv_img_pyr[1]); + + // evaluate jacobian + Eigen::Matrix frame_jac; + frameJac_xyz2uv(xyz_img, cam_pyr_[level_].fx(), frame_jac); + + // compute steppest descent images + Vector6d J = dx*frame_jac.row(0) + dy*frame_jac.row(1); + + // compute Hessian and + H_ += J*J.transpose(); + Jres_ += J*res; + + resimg_.at(v,u) = -res; + } + } + } + } + chi2 /= n_pixels; + return chi2; +} + +int ForwardCompositionalSE3:: +solve() +{ + x_ = H_.ldlt().solve(-Jres_); + if((bool) std::isnan((double) x_[0])) + return 0; + return 1; +} + +void ForwardCompositionalSE3:: +update(const ModelType& old_model, ModelType& new_model) +{ + new_model = Sophus::SE3::exp(x_)*(old_model); +} + +void ForwardCompositionalSE3:: +startIteration() +{ +#if 0 + if(log_) + permon_.newMeasurement(); +#endif +} + +void ForwardCompositionalSE3:: +finishIteration() +{ +#if 0 + if(log_) + { + permon_.log("iter", iter_); + permon_.log("level", level_); + permon_.log("mu", mu_); + permon_.log("chi2", chi2_); + permon_.log("trials", n_trials_); + } +#endif + + if(display_) + { + cv::namedWindow("residuals", cv::WINDOW_AUTOSIZE); + cv::imshow("residuals", resimg_*3); + cv::waitKey(0); + } +} + +/******************************************************************************* + * Efficient Second Order Minimization (ESM) + */ +SecondOrderMinimisationSE3:: +SecondOrderMinimisationSE3( vector& cam_pyr, + vector& depth_pyr, + vector& img_pyr, + vector& tpl_pyr, + vector& img_pyr_dx, + vector& img_pyr_dy, + vector& tpl_pyr_dx, + vector& tpl_pyr_dy, + Sophus::SE3& init_model, + int n_levels, + int n_iter, + float res_thresh, + bool display, + Method method, + int test_id) : + cam_pyr_(cam_pyr), + depth_pyr_(depth_pyr), + img_pyr_(img_pyr), + tpl_pyr_(tpl_pyr), + img_pyr_dx_(img_pyr_dx), + img_pyr_dy_(img_pyr_dy), + tpl_pyr_dx_(tpl_pyr_dx), + tpl_pyr_dy_(tpl_pyr_dy), + display_(display), + log_(test_id < 0), + res_thresh_(res_thresh) +{ + n_iter_ = n_iter; + method_ = method; + verbose_ = false; + +#if 0 + if(log_) + { + // Init Performance Monitor + permon_.init("esm", ament_index_cpp::get_package_share_directory("rpl_examples") + "/trace/img_align/data", + test_id, true); + permon_.addLog("iter"); + permon_.addLog("level"); + permon_.addLog("mu"); + permon_.addLog("chi2"); + permon_.addLog("trials"); + } +#endif + + // perform pyramidal optimization + for(level_=n_levels-1; level_>2; --level_) + //level_ = n_levels-1; + { + // Optimize + mu_ = 0.01f; + if(display_) + { + cout << endl << "PYRAMID LEVEL " << level_ + << endl << "patch-width = " << img_pyr_[level_].cols + << endl << "---------------" << endl; + } + optimize(init_model); + } +} + +double SecondOrderMinimisationSE3:: +computeResiduals (const Sophus::SE3& model, bool linearize_system, bool compute_weight_scale) +{ + // Warp the image such that it aligns with the template image + double chi2 = 0; + size_t n_pixels = 0; + + // TODO: to improve access speed, use a pointer and increment every iteration + + // Compute Warp + cv::Mat mask = cv::Mat_(tpl_pyr_[level_].rows, tpl_pyr_[level_].cols, false); + cv::Mat img_warped = cv::Mat_(tpl_pyr_[level_].rows, tpl_pyr_[level_].cols, 1.0); + + for( int v=0; v(v,u); + Vector3d xyz_tpl(cv_float3[0], cv_float3[1], cv_float3[2]); + Vector3d xyz_img(model*xyz_tpl); + Vector2f uv_img_pyr = cam_pyr_[level_].world2cam(xyz_img).cast(); // apply cam model + if( cam_pyr_[level_].isInFrame(uv_img_pyr.cast(), 1) ) + { + img_warped.at(v,u) = interpolateMat_32f(img_pyr_[level_], uv_img_pyr[0], uv_img_pyr[1]); + + if( cam_pyr_[level_].isInFrame(uv_img_pyr.cast(), 2) ) + mask.at(v,u) = true; + } + } + } + + // Compute Warp derivative + cv::Mat img_warped_dx, img_warped_dy; + cv::Sobel(img_warped, img_warped_dx, CV_32F, 1, 0, 1); + cv::Sobel(img_warped, img_warped_dy, CV_32F, 0, 1, 1); + + // Compute Jacobian + if(linearize_system) + resimg_ = cv::Mat_(tpl_pyr_[level_].size(), 1.0); + + for( int v=0; v(v,u)) + { + // compare image values + float intensity_tpl = tpl_pyr_[level_].at(v,u); + float intensity_img = img_warped.at(v,u); + + // compute residual (opposite to 2d case because of other jacobian) + float res = intensity_tpl-intensity_img; + + // robustification + if(res > res_thresh_) res = res_thresh_; + if(res < -res_thresh_) res = -res_thresh_; + chi2 += res*res; + n_pixels++; + + if(linearize_system) + { + // 0.25 because we have two 0.5 factors. First from adding the two gradients + // and the second when we compute the sobel mask + float dx = 0.25*(tpl_pyr_dx_[level_].at(v,u) + img_warped_dx.at(v,u)); + float dy = 0.25*(tpl_pyr_dy_[level_].at(v,u) + img_warped_dy.at(v,u)); + + // evaluate jacobian + cv::Vec3f cv_float3 = depth_pyr_[level_].at(v,u); + Sophus::Vector3d xyz_tpl(cv_float3[0], cv_float3[1], cv_float3[2]); + Sophus::Vector3d xyz_img(model*xyz_tpl); + Eigen::Matrix frame_jac; + frameJac_xyz2uv(xyz_tpl, cam_pyr_[level_].fx(), frame_jac); + + // compute steppest descent images + Vector6d J = dx*frame_jac.row(0) + dy*frame_jac.row(1); + + // compute Hessian + H_ += J*J.transpose(); + Jres_ += J*res; + resimg_.at(v,u) = res; + } + } + } + } + chi2 /= n_pixels; + return chi2; +} + +int SecondOrderMinimisationSE3:: +solve() +{ + x_ = H_.ldlt().solve(-Jres_); + if((bool) std::isnan((double) x_[0])) + return 0; + return 1; +} + +void SecondOrderMinimisationSE3:: +update(const ModelType& old_model, ModelType& new_model) +{ + new_model = Sophus::SE3::exp(x_)*old_model; +} + +void SecondOrderMinimisationSE3:: +startIteration() +{ +#if 0 + if(log_) + permon_.newMeasurement(); +#endif +} + +void SecondOrderMinimisationSE3:: +finishIteration() +{ +#if 0 + if(log_) + { + permon_.log("iter", iter_); + permon_.log("level", level_); + permon_.log("mu", mu_); + permon_.log("chi2", chi2_); + permon_.log("trials", n_trials_); + } +#endif + + if(display_) + { + cv::namedWindow("residuals", cv::WINDOW_AUTOSIZE); + cv::imshow("residuals", resimg_*3); + cv::waitKey(0); + } +} + +} // end namespace vk diff --git a/src/rpg_vikit/vikit_common/src/math_utils.cpp b/src/rpg_vikit/vikit_common/src/math_utils.cpp new file mode 100644 index 0000000..36dc658 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/math_utils.cpp @@ -0,0 +1,202 @@ +/* + * math_utils.cpp + * + * Created on: Jul 20, 2012 + * Author: cforster + */ + +#include + +namespace vk { + +using namespace Eigen; + +Vector3d +triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t, + const Vector3d& feature1, const Vector3d& feature2 ) +{ + Vector3d f2 = R * feature2; + Vector2d b; + b[0] = t.dot(feature1); + b[1] = t.dot(f2); + Matrix2d A; + A(0,0) = feature1.dot(feature1); + A(1,0) = feature1.dot(f2); + A(0,1) = -A(1,0); + A(1,1) = -f2.dot(f2); + Vector2d lambda = A.inverse() * b; + Vector3d xm = lambda[0] * feature1; + Vector3d xn = t + lambda[1] * f2; + return ( xm + xn )/2; +} + +bool +depthFromTriangulationExact( + const Matrix3d& R_r_c, + const Vector3d& t_r_c, + const Vector3d& f_r, + const Vector3d& f_c, + double& depth_in_r, + double& depth_in_c) +{ + // bearing vectors (f_r, f_c) do not need to be unit length + const Vector3d f_c_in_r(R_r_c*f_c); + const double a = f_c_in_r.dot(f_r) / t_r_c.dot(f_r); + const double b = f_c_in_r.dot(t_r_c); + const double denom = (a*b - f_c_in_r.dot(f_c_in_r)); + + if(abs(denom) < 0.000001) + return false; + + depth_in_c = (b-a*t_r_c.dot(t_r_c)) / denom; + depth_in_r = (t_r_c + f_c_in_r*depth_in_c).norm(); + return true; +} + +double +reprojError(const Vector3d& f1, + const Vector3d& f2, + double error_multiplier2) +{ + Vector2d e = project2d(f1) - project2d(f2); + return error_multiplier2 * e.norm(); +} + +double +computeInliers(const vector& features1, // c1 + const vector& features2, // c2 + const Matrix3d& R, // R_c1_c2 + const Vector3d& t, // c1_t + const double reproj_thresh, + double error_multiplier2, + vector& xyz_vec, // in frame c1 + vector& inliers, + vector& outliers) +{ + inliers.clear(); inliers.reserve(features1.size()); + outliers.clear(); outliers.reserve(features1.size()); + xyz_vec.clear(); xyz_vec.reserve(features1.size()); + double tot_error = 0; + //triangulate all features and compute reprojection errors and inliers + for(size_t j=0; j reproj_thresh || e2 > reproj_thresh) + outliers.push_back(j); + else + { + inliers.push_back(j); + tot_error += e1+e2; + } + } + return tot_error; +} + +void +computeInliersOneView(const vector & feature_sphere_vec, + const vector & xyz_vec, + const Matrix3d &R, + const Vector3d &t, + const double reproj_thresh, + const double error_multiplier2, + vector& inliers, + vector& outliers) +{ + inliers.clear(); inliers.reserve(xyz_vec.size()); + outliers.clear(); outliers.reserve(xyz_vec.size()); + for(size_t j = 0; j < xyz_vec.size(); j++ ) + { + double e = reprojError(feature_sphere_vec[j], + R.transpose() * ( xyz_vec[j] - t ), + error_multiplier2); + if(e < reproj_thresh) + inliers.push_back(j); + else + outliers.push_back(j); + } +} + +Vector3d +dcm2rpy(const Matrix3d &R) +{ + Vector3d rpy; + rpy[1] = atan2( -R(2,0), sqrt( pow( R(0,0), 2 ) + pow( R(1,0), 2 ) ) ); + if( fabs( rpy[1] - M_PI/2 ) < 0.00001 ) + { + rpy[2] = 0; + rpy[0] = -atan2( R(0,1), R(1,1) ); + } + else + { + if( fabs( rpy[1] + M_PI/2 ) < 0.00001 ) + { + rpy[2] = 0; + rpy[0] = -atan2( R(0,1), R(1,1) ); + } + else + { + rpy[2] = atan2( R(1,0)/cos(rpy[1]), R(0,0)/cos(rpy[1]) ); + rpy[0] = atan2( R(2,1)/cos(rpy[1]), R(2,2)/cos(rpy[1]) ); + } + } + return rpy; +} + +Matrix3d +rpy2dcm(const Vector3d &rpy) +{ + Matrix3d R1; + R1(0,0) = 1.0; R1(0,1) = 0.0; R1(0,2) = 0.0; + R1(1,0) = 0.0; R1(1,1) = cos(rpy[0]); R1(1,2) = -sin(rpy[0]); + R1(2,0) = 0.0; R1(2,1) = -R1(1,2); R1(2,2) = R1(1,1); + + Matrix3d R2; + R2(0,0) = cos(rpy[1]); R2(0,1) = 0.0; R2(0,2) = sin(rpy[1]); + R2(1,0) = 0.0; R2(1,1) = 1.0; R2(1,2) = 0.0; + R2(2,0) = -R2(0,2); R2(2,1) = 0.0; R2(2,2) = R2(0,0); + + Matrix3d R3; + R3(0,0) = cos(rpy[2]); R3(0,1) = -sin(rpy[2]); R3(0,2) = 0.0; + R3(1,0) = -R3(0,1); R3(1,1) = R3(0,0); R3(1,2) = 0.0; + R3(2,0) = 0.0; R3(2,1) = 0.0; R3(2,2) = 1.0; + + return R3 * R2 * R1; +} + +Quaterniond +angax2quat(const Vector3d& n, const double& angle) +{ + // n must be normalized! + double s(sin(angle/2)); + return Quaterniond( cos(angle/2), n[0]*s, n[1]*s, n[2]*s ); +} + + +Matrix3d +angax2dcm(const Vector3d& n, const double& angle) +{ + // n must be normalized + Matrix3d sqewn(sqew(n)); + return Matrix3d(Matrix3d::Identity() + sqewn*sin(angle) + sqewn*sqewn*(1-cos(angle))); +} + +double +sampsonusError(const Vector2d &v2Dash, const Matrix3d& Essential, const Vector2d& v2) +{ + Vector3d v3Dash = unproject2d(v2Dash); + Vector3d v3 = unproject2d(v2); + + double dError = v3Dash.transpose() * Essential * v3; + + Vector3d fv3 = Essential * v3; + Vector3d fTv3Dash = Essential.transpose() * v3Dash; + + Vector2d fv3Slice = fv3.head<2>(); + Vector2d fTv3DashSlice = fTv3Dash.head<2>(); + + return (dError * dError / (fv3Slice.dot(fv3Slice) + fTv3DashSlice.dot(fTv3DashSlice))); +} + +} // end namespace vk diff --git a/src/rpg_vikit/vikit_common/src/omni_camera.cpp b/src/rpg_vikit/vikit_common/src/omni_camera.cpp new file mode 100644 index 0000000..b4fdf39 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/omni_camera.cpp @@ -0,0 +1,199 @@ +/* + * OcamProjector.cpp + * + * Created on: Sep 22, 2010 + * Author: laurent kneip + */ + +#include +#include +#include + +namespace vk { + +OmniCamera:: +OmniCamera( string calibFile ) +{ + double *pol = ocamModel.pol; + double *invpol = ocamModel.invpol; + double *xc = &ocamModel.xc; + double *yc = &ocamModel.yc; + double *c = &ocamModel.c; + double *d = &ocamModel.d; + double *e = &ocamModel.e; + int *width = &ocamModel.width; + int *height = &ocamModel.height; + int *length_pol = &ocamModel.length_pol; + int *length_invpol = &ocamModel.length_invpol; + FILE *f; + char buf[CMV_MAX_BUF]; + int i; + + printf("Initialize OmniCamera: Read Calibration %s\n", calibFile.c_str()); + + //Open file + if( !( f = fopen( (char*) calibFile.c_str(), "r" ) ) ) + { + printf("Initialize OmniCamera: Cannot read calibration file."); + return; + } + + //Read polynomial coefficients + char* dummy = fgets( buf, CMV_MAX_BUF, f ); + int result = fscanf( f, "\n" ); + result = fscanf( f, "%d", length_pol ); + for( i = 0; i < *length_pol; i++ ) + result = fscanf( f, " %lf", &pol[i] ); + + //Read inverse polynomial coefficients + result = fscanf( f, "\n" ); + dummy = fgets( buf, CMV_MAX_BUF, f ); + result = fscanf( f, "\n" ); + result = fscanf( f, "%d", length_invpol ); + for( i = 0; i < *length_invpol; i++ ) + result = fscanf( f, " %lf", &invpol[i] ); + + //Read center coordinates + result = fscanf( f, "\n" ); + dummy = fgets( buf, CMV_MAX_BUF, f ); + result = fscanf( f, "\n" ); + result = fscanf( f, "%lf %lf\n", xc, yc ); + + //Read affine coefficients + dummy = fgets( buf, CMV_MAX_BUF, f ); + result = fscanf( f, "\n" ); + result = fscanf( f, "%lf %lf %lf\n", c, d, e ); + + //Read image size + dummy = fgets( buf, CMV_MAX_BUF, f ); + result = fscanf( f, "\n" ); + result = fscanf( f, "%d %d", height, width ); + + fclose(f); + + width_ = *width; + height_ = *height; + error_multiplier_ = computeErrorMultiplier(); +} + +OmniCamera:: +~OmniCamera() +{} + +Vector3d OmniCamera:: +cam2world(const double& u, const double& v) const +{ + Vector3d xyz; + + // Important: we exchange x and y since regular pinhole model is working with x along the columns and y along the rows + // Davide's framework is doing exactly the opposite + + double invdet = 1 / ( ocamModel.c - ocamModel.d * ocamModel.e ); + + xyz[0] = invdet * ( ( v - ocamModel.xc ) - ocamModel.d * ( u - ocamModel.yc ) ); + xyz[1] = invdet * ( -ocamModel.e * ( v - ocamModel.xc ) + ocamModel.c * ( u - ocamModel.yc ) ); + + double r = sqrt( pow( xyz[0], 2 ) + pow( xyz[1], 2 ) ); //distance [pixels] of the point from the image center + xyz[2] = ocamModel.pol[0]; + double r_i = 1; + + for( int i = 1; i < ocamModel.length_pol; i++ ) + { + r_i *= r; + xyz[2] += r_i * ocamModel.pol[i]; + } + + xyz.normalize(); + + // change back to pinhole model: + double temp = xyz[0]; + xyz[0] = xyz[1]; + xyz[1] = temp; + xyz[2] = -xyz[2]; + + return xyz; +} + +Vector3d OmniCamera:: +cam2world (const Vector2d& px) const +{ + return cam2world(px[0], px[1]); +} + +Vector2d OmniCamera:: +world2cam(const Vector3d& xyz_c) const +{ + Vector2d uv; + + // transform world-coordinates to Davide's camera frame + Vector3d worldCoordinates_bis; + worldCoordinates_bis[0] = xyz_c[1]; + worldCoordinates_bis[1] = xyz_c[0]; + worldCoordinates_bis[2] = -xyz_c[2]; + + double norm = sqrt( pow( worldCoordinates_bis[0], 2 ) + pow( worldCoordinates_bis[1], 2 ) ); + double theta = atan( worldCoordinates_bis[2]/norm ); + + // Important: we exchange x and y since Pirmin's stuff is working with x along the columns and y along the rows, + // Davide's framework is doing exactly the opposite + double rho; + double t_i; + double x; + double y; + + if(norm != 0) + { + rho = ocamModel.invpol[0]; + + t_i = 1; + + for( int i = 1; i < ocamModel.length_invpol; i++ ) + { + t_i *= theta; + rho += t_i * ocamModel.invpol[i]; + } + + x = worldCoordinates_bis[0] * rho/norm; + y = worldCoordinates_bis[1] * rho/norm; + + // we exchange 0 and 1 in order to have pinhole model again + uv[1] = x * ocamModel.c + y * ocamModel.d + ocamModel.xc; + uv[0] = x * ocamModel.e + y + ocamModel.yc; + } + else + { + // we exchange 0 and 1 in order to have pinhole model again + uv[1] = ocamModel.xc; + uv[0] = ocamModel.yc; + } + + return uv; +} + +Vector2d OmniCamera:: +world2cam(const Vector2d& uv) const +{ + return world2cam(unproject2d(uv).normalized()); +} + +double OmniCamera:: +computeErrorMultiplier() +{ + Vector3d vector1 = cam2world( .5*width_, .5*height_ ); + Vector3d vector2 = cam2world( .5*width_ + .5, .5*height_ ); + vector1 = vector1/vector1.norm(); + vector2 = vector2/vector2.norm(); + + double factor1 = .5/( 1 - vector1.dot(vector2) ); + + vector1 = cam2world( width_, .5*height_ ); + vector2 = cam2world( -.5 + (double) width_ , .5*height_ ); + vector1 = vector1/vector1.norm(); + vector2 = vector2/vector2.norm(); + + double factor2 = .5/( 1 - vector1.dot(vector2) ); + + return ( factor2 + factor1 ) * .5; +} + +} // end namespace vk diff --git a/src/rpg_vikit/vikit_common/src/performance_monitor.cpp b/src/rpg_vikit/vikit_common/src/performance_monitor.cpp new file mode 100644 index 0000000..5e92ade --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/performance_monitor.cpp @@ -0,0 +1,161 @@ +/* + * performance_monitor.cpp + * + * Created on: Aug 26, 2011 + * Author: Christian Forster + */ + +#include +#include +#include + +namespace vk +{ +using namespace std; + +PerformanceMonitor::PerformanceMonitor() +{} + +PerformanceMonitor::~PerformanceMonitor() +{ + ofs_.flush(); + ofs_.close(); +} + +void PerformanceMonitor::init( + const string& trace_name, + const string& trace_dir) +{ + trace_name_ = trace_name; + trace_dir_ = trace_dir; + string filename(trace_dir + "/" + trace_name + ".csv"); + ofs_.open(filename.c_str()); + if(!ofs_.is_open()) + { + printf("Tracefile = %s\n", filename.c_str()); + throw runtime_error("Could not open tracefile."); + } + traceHeader(); +} + +void PerformanceMonitor::addTimer(const string& name) +{ + timers_.insert(make_pair(name, Timer())); +} + +void PerformanceMonitor::addLog(const string& name) +{ + logs_.insert(make_pair(name, LogItem())); +} + +void PerformanceMonitor::writeToFile() +{ + trace(); + + for(auto it = timers_.begin(); it!=timers_.end(); ++it) + it->second.reset(); + for(auto it=logs_.begin(); it!=logs_.end(); ++it) + { + it->second.set = false; + it->second.data = -1; + } +} + +void PerformanceMonitor::startTimer(const string& name) +{ + auto t = timers_.find(name); + if(t == timers_.end()) { + printf("Timer = %s\n", name.c_str()); + throw std::runtime_error("startTimer: Timer not registered"); + } + t->second.start(); +} + +void PerformanceMonitor::stopTimer(const string& name) +{ + auto t = timers_.find(name); + if(t == timers_.end()) { + printf("Timer = %s\n", name.c_str()); + throw std::runtime_error("stopTimer: Timer not registered"); + } + t->second.stop(); +} + +double PerformanceMonitor::getTime(const string& name) const +{ + auto t = timers_.find(name); + if(t == timers_.end()) { + printf("Timer = %s\n", name.c_str()); + throw std::runtime_error("Timer not registered"); + } + return t->second.getTime(); +} + +void PerformanceMonitor::log(const string& name, double data) +{ + auto l = logs_.find(name); + if(l == logs_.end()) { + printf("Logger = %s\n", name.c_str()); + throw std::runtime_error("Logger not registered"); + } + l->second.data = data; + l->second.set = true; +} + +void PerformanceMonitor::trace() +{ + char buffer[128]; + bool first_value = true; + if(!ofs_.is_open()) + throw std::runtime_error("Performance monitor not correctly initialized"); + ofs_.precision(15); + ofs_.setf(std::ios::fixed, std::ios::floatfield ); + for(auto it = timers_.begin(); it!=timers_.end(); ++it) + { + if(first_value) { + ofs_ << it->second.getTime(); + first_value = false; + } + else + ofs_ << "," << it->second.getTime(); + } + for(auto it=logs_.begin(); it!=logs_.end(); ++it) + { + if(first_value) { + ofs_ << it->second.data; + first_value = false; + } + else + ofs_ << "," << it->second.data; + } + ofs_ << "\n"; +} + +void PerformanceMonitor::traceHeader() +{ + if(!ofs_.is_open()) + throw std::runtime_error("Performance monitor not correctly initialized"); + bool first_value = true; + for(auto it = timers_.begin(); it!=timers_.end(); ++it) + { + if(first_value) { + ofs_ << it->first; + first_value = false; + } + else + ofs_ << "," << it->first; + } + for(auto it=logs_.begin(); it!=logs_.end(); ++it) + { + if(first_value) { + ofs_ << it->first; + first_value = false; + } + else + ofs_ << "," << it->first; + } + ofs_ << "\n"; +} + +} // namespace vk + diff --git a/src/rpg_vikit/vikit_common/src/pinhole_camera.cpp b/src/rpg_vikit/vikit_common/src/pinhole_camera.cpp new file mode 100644 index 0000000..4308abf --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/pinhole_camera.cpp @@ -0,0 +1,118 @@ +/* + * pinhole_camera.cpp + * + * Created on: Jul 24, 2012 + * Author: cforster + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +PinholeCamera:: +PinholeCamera(double width, double height, double scale, + double fx, double fy, + double cx, double cy, + double d0, double d1, double d2, double d3, double d4) : + AbstractCamera(width * scale , height * scale, scale), + fx_(fx * scale), fy_(fy * scale), cx_(cx * scale), cy_(cy * scale), + distortion_(fabs(d0) > 0.0000001), + undist_map1_(height_, width_, CV_16SC2), + undist_map2_(height_, width_, CV_16SC2), + use_optimization_(false) +{ + cout << "scale: " << scale << endl; + d_[0] = d0; d_[1] = d1; d_[2] = d2; d_[3] = d3; d_[4] = d4; + cvK_ = (cv::Mat_(3, 3) << fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0); + cvD_ = (cv::Mat_(1, 5) << d_[0], d_[1], d_[2], d_[3], d_[4]); + cv::initUndistortRectifyMap(cvK_, cvD_, cv::Mat_::eye(3,3), cvK_, + cv::Size(width_, height_), CV_16SC2, undist_map1_, undist_map2_); + K_ << fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0; + K_inv_ = K_.inverse(); +} + +PinholeCamera:: +~PinholeCamera() +{} + +Vector3d PinholeCamera:: +cam2world(const double& u, const double& v) const +{ + Vector3d xyz; + if(!distortion_) + { + xyz[0] = (u - cx_)/fx_; + xyz[1] = (v - cy_)/fy_; + xyz[2] = 1.0; + } + else + { + cv::Point2f uv(u,v), px; + const cv::Mat src_pt(1, 1, CV_32FC2, &uv.x); + cv::Mat dst_pt(1, 1, CV_32FC2, &px.x); + cv::undistortPoints(src_pt, dst_pt, cvK_, cvD_); + xyz[0] = px.x; + xyz[1] = px.y; + xyz[2] = 1.0; + } + return xyz.normalized(); +} + +Vector3d PinholeCamera:: +cam2world (const Vector2d& uv) const +{ + return cam2world(uv[0], uv[1]); +} + +Vector2d PinholeCamera:: +world2cam(const Vector3d& xyz) const +{ + return world2cam(project2d(xyz)); +} + +Vector2d PinholeCamera:: +world2cam(const Vector2d& uv) const +{ + Vector2d px; + if(!distortion_) + { + px[0] = fx_*uv[0] + cx_; + px[1] = fy_*uv[1] + cy_; + } + else + { + double x, y, r2, r4, r6, a1, a2, a3, cdist, xd, yd; + x = uv[0]; + y = uv[1]; + r2 = x*x + y*y; + r4 = r2*r2; + r6 = r4*r2; + a1 = 2*x*y; + a2 = r2 + 2*x*x; + a3 = r2 + 2*y*y; + cdist = 1 + d_[0]*r2 + d_[1]*r4 + d_[4]*r6; + xd = x*cdist + d_[2]*a1 + d_[3]*a2; + yd = y*cdist + d_[2]*a3 + d_[3]*a1; + px[0] = xd*fx_ + cx_; + px[1] = yd*fy_ + cy_; + } + return px; +} + +void PinholeCamera:: +undistortImage(const cv::Mat& raw, cv::Mat& rectified) +{ + if(distortion_) + cv::remap(raw, rectified, undist_map1_, undist_map2_, cv::INTER_LINEAR); + else + rectified = raw.clone(); +} + +} // end namespace vk diff --git a/src/rpg_vikit/vikit_common/src/polynomial_camera.cpp b/src/rpg_vikit/vikit_common/src/polynomial_camera.cpp new file mode 100644 index 0000000..91dfbf3 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/polynomial_camera.cpp @@ -0,0 +1,152 @@ +/* + * polynomial_camera.cpp + * + * Created on: January 26, 2023 + * Author: xuankuzcr + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +// PolynomialCamera:: +// PolynomialCamera(double width, double height, double scale, +// double fx, double fy, +// double cx, double cy, double skew, +// double k2, double k3, double k4, double k5, double k6, double k7) : +// AbstractCamera(width * scale , height * scale, scale), +// fx_(fx * scale), fy_(fy * scale), cx_(cx * scale), cy_(cy * scale), skew_(skew * scale), +// distortion_(fabs(k2) > 0.0000001) +// { +// cout << "scale: " << scale << endl; +// k2_ = k2; k3_ = k3; k4_ = k4; k5_ = k5; k6_ = k6; k7_ = k7; +// } + +PolynomialCamera:: +PolynomialCamera(double width, double height, // double scale, + double fx, double fy, + double cx, double cy, double skew, + double k2, double k3, double k4, double k5, double k6, double k7) : + AbstractCamera(width, height, 1.0), + fx_(fx), fy_(fy), cx_(cx), cy_(cy), skew_(skew), + distortion_(fabs(k2) > 0.0000001) +{ + // cout << "scale: " << scale << endl; + k2_ = k2; k3_ = k3; k4_ = k4; k5_ = k5; k6_ = k6; k7_ = k7; +} + +PolynomialCamera:: +~PolynomialCamera() +{} + +Vector3d PolynomialCamera:: +cam2world(const double& u, const double& v) const +{ + Vector3d xyz; + if(!distortion_) + { + // xyz[0] = (u - cx_)/fx_; + // xyz[1] = (v - cy_)/fy_; + // xyz[2] = 1.0; + xyz[1] = (v - cy_)/fy_; + xyz[0] = (u - cx_ - xyz[1]*skew_)/fx_; + xyz[2] = 1.0; + } + else + { + double y = (v - cy_)/fy_; + double x = (u - cx_ - y*skew_)/fx_; + + const double thetad = std::sqrt(x * x + y * y); + double theta = thetad; + for (int i = 0; i < 7; ++i) + { + const double theta2 = theta * theta; + const double theta3 = theta2 * theta; + const double theta4 = theta3 * theta; + const double theta5 = theta4 * theta; + const double theta6 = theta5 * theta; + theta = thetad / + (1.0 + k2_ * theta + k3_ * theta2 + k4_ * theta3 + k5_ * theta4 + k6_ * theta5 + k7_ * theta6); + } + const double scaling = std::tan(theta) / thetad; + x *= scaling; + y *= scaling; + xyz[0] = x; + xyz[1] = y; + xyz[2] = 1.0; + } + return xyz.normalized(); +} + +Vector3d PolynomialCamera:: +cam2world (const Vector2d& uv) const +{ + return cam2world(uv[0], uv[1]); +} + +Vector2d PolynomialCamera:: +world2cam(const Vector3d& xyz) const +{ + // return world2cam(project2d(xyz)); + Vector2d px; + if(!distortion_) + { + px[0] = fx_*xyz[0] + cx_; + px[1] = fy_*xyz[1] + cy_; + } + else + { + double xd, yd; + const double r = sqrt( xyz( 1 ) * xyz( 1 ) + xyz( 0 ) * xyz( 0 )); + // if (r < 1e-8) + // { + // return uv; + // } + const double theta = acos( xyz( 2 ) / xyz.norm( ) ); + const double thetad = thetad_from_theta(theta); + const double scaling = thetad / r; + xd = xyz[0] * scaling; + yd = xyz[1] * scaling; + px[0] = xd*fx_ + yd*skew_ + cx_; + px[1] = yd*fy_ + cy_; + } + return px; +} + +Vector2d PolynomialCamera:: +world2cam(const Vector2d& uv) const +{ + Vector2d px; + if(!distortion_) + { + px[0] = fx_*uv[0] + cx_; + px[1] = fy_*uv[1] + cy_; + } + else + { + double xd, yd; + const double r = uv.norm(); + if (r < 1e-8) + { + return uv; + } + const double theta = std::atan(r); + const double thetad = thetad_from_theta(theta); + const double scaling = thetad / r; + xd = uv[0] * scaling; + yd = uv[1] * scaling; + px[0] = xd*fx_ + yd*skew_ + cx_; + px[1] = yd*fy_ + cy_; + } + return px; +} + +} // end namespace vk diff --git a/src/rpg_vikit/vikit_common/src/robust_cost.cpp b/src/rpg_vikit/vikit_common/src/robust_cost.cpp new file mode 100644 index 0000000..e2bee84 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/robust_cost.cpp @@ -0,0 +1,162 @@ +/** +* This file is part of dvo. +* +* Copyright 2012 Christian Kerl (Technical University of Munich) +* For more information see . +* +* dvo is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* dvo is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with dvo. If not, see . +*/ + +#include +#include +#include +#include + +namespace vk { +namespace robust_cost { + +const float TDistributionScaleEstimator::INITIAL_SIGMA = 5.0f; +const float TDistributionScaleEstimator::DEFAULT_DOF = 5.0f; + +TDistributionScaleEstimator:: +TDistributionScaleEstimator(const float dof) : + dof_(dof), + initial_sigma_(INITIAL_SIGMA) +{} + +float TDistributionScaleEstimator:: +compute(std::vector& errors) const +{ + float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_); + int num = 0; + float lambda = initial_lamda; + int iterations = 0; + do + { + ++iterations; + initial_lamda = lambda; + num = 0; + lambda = 0.0f; + + for(std::vector::iterator it=errors.begin(); it!=errors.end(); ++it) + { + if(std::isfinite(*it)) + { + ++num; + const float error2 = (*it)*(*it); + lambda += error2 * ( (dof_ + 1.0f) / (dof_ + initial_lamda * error2) ); + } + } + lambda = float(num) / lambda; + } while(std::abs(lambda - initial_lamda) > 1e-3); + + return std::sqrt(1.0f / lambda); +} + +const float MADScaleEstimator::NORMALIZER = 1.48f; // 1 / 0.6745 + +float MADScaleEstimator:: +compute(std::vector& errors) const +{ + // error must be in absolute values! + return NORMALIZER * vk::getMedian(errors); +} + +float NormalDistributionScaleEstimator:: +compute(std::vector& errors) const +{ + const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size(); + float var = 0.0; + std::for_each(errors.begin(), errors.end(), [&](const float d) { + var += (d - mean) * (d - mean); + }); + return std::sqrt(var); // return standard deviation +} + +const float TukeyWeightFunction::DEFAULT_B = 4.6851f; + +TukeyWeightFunction::TukeyWeightFunction(const float b) +{ + configure(b); +} + +float TukeyWeightFunction::value(const float& x) const +{ + const float x_square = x * x; + if(x_square <= b_square) + { + const float tmp = 1.0f - x_square / b_square; + return tmp * tmp; + } + else + { + return 0.0f; + } +} + +void TukeyWeightFunction:: +configure(const float& param) +{ + b_square = param * param; +} + +const float TDistributionWeightFunction::DEFAULT_DOF = 5.0f; + +TDistributionWeightFunction:: +TDistributionWeightFunction(const float dof) +{ + configure(dof); +} + +float TDistributionWeightFunction:: +value(const float & x) const +{ + return ((dof_ + 1.0f) / (dof_ + (x * x))); +} + +void TDistributionWeightFunction:: +configure(const float& param) +{ + dof_ = param; + normalizer_ = dof_ / (dof_ + 1.0f); +} + +const float HuberWeightFunction::DEFAULT_K = 1.345f; + +HuberWeightFunction:: +HuberWeightFunction(const float k) +{ + configure(k); +} + +void HuberWeightFunction:: +configure(const float& param) +{ + k = param; +} + +float HuberWeightFunction:: +value(const float& t) const +{ + const float t_abs = std::abs(t); + if(t_abs < k) + return 1.0f; + else + return k / t_abs; +} + +} // namespace robust_cost +} // namespace vk + + diff --git a/src/rpg_vikit/vikit_common/src/user_input_thread.cpp b/src/rpg_vikit/vikit_common/src/user_input_thread.cpp new file mode 100644 index 0000000..39a1924 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/user_input_thread.cpp @@ -0,0 +1,67 @@ +/* + * user_input_thread.cpp + * + * Created on: Jun 12, 2013 + * Author: pizzoli, cforster + */ + +#include +#include +#include + +namespace vk { + +UserInputThread::UserInputThread() : + stop_(false), + input_( (char) 0) +{ + tcgetattr(0, &original_terminal_settings_); // save old terminal i/o settings + new_terminal_settings_ = original_terminal_settings_; // make new settings same as old settings + new_terminal_settings_.c_lflag &= ~ICANON; // disable buffered i/o + new_terminal_settings_.c_lflag &= ~ECHO; // set echo mode + new_terminal_settings_.c_cc[VMIN] = 1; //minimum of number input read. + tcsetattr(0, TCSANOW, &new_terminal_settings_); // use these new terminal i/o settings now + + user_input_thread_ = new std::thread(&UserInputThread::acquireUserInput, this); +} + +UserInputThread::~UserInputThread() +{ + tcsetattr(0, TCSANOW, &original_terminal_settings_); + user_input_thread_->join(); + printf("UserInputThread destructed.\n"); +} + +char UserInputThread::getInput() +{ + char tmp = input_; + input_ = (char) 0; + return tmp; +} + +void UserInputThread::stop() +{ + stop_ = true; +} + +void UserInputThread::acquireUserInput() +{ + int c = 0; + while(!stop_) + { + c = getchar(); // TODO: this is blocking, so the interruption point is not reached... + if ((char)c == ' ') + printf("USER INPUT: SPACE\n"); + else + printf("USER INPUT: %c\n", (char) c); + input_ = (char) c; + c = 0; + + // interruption point: + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } +} + +} // end namespace vk + + diff --git a/src/rpg_vikit/vikit_common/src/vision.cpp b/src/rpg_vikit/vikit_common/src/vision.cpp new file mode 100644 index 0000000..b6d5f58 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/vision.cpp @@ -0,0 +1,295 @@ +/* + * vision.cpp + * + * Created on: May 14, 2013 + * Author: cforster + */ + +#include + +#if __SSE2__ +# include +#elif __ARM_NEON__ +# include +#endif + +namespace vk { + +#ifdef __SSE2__ +void halfSampleSSE2(const unsigned char* in, unsigned char* out, int w, int h) +{ + const unsigned long long mask[2] = {0x00FF00FF00FF00FFull, 0x00FF00FF00FF00FFull}; + const unsigned char* nextRow = in + w; + __m128i m = _mm_loadu_si128((const __m128i*)mask); + int sw = w >> 4; + int sh = h >> 1; + for (int i=0; i> 1)*out.cols; + for( int x = in.cols; x > 0 ; x-=16, in_top += 16, in_bottom += 16, out_data += 8) + { + uint8x8x2_t top = vld2_u8( (const uint8_t *)in_top ); + uint8x8x2_t bottom = vld2_u8( (const uint8_t *)in_bottom ); + uint16x8_t sum = vaddl_u8( top.val[0], top.val[1] ); + sum = vaddw_u8( sum, bottom.val[0] ); + sum = vaddw_u8( sum, bottom.val[1] ); + uint8x8_t final_sum = vshrn_n_u16(sum, 2); + vst1_u8(out_data, final_sum); + } + } +} +#endif + + +void +halfSample(const cv::Mat& in, cv::Mat& out) +{ + assert( in.rows/2==out.rows && in.cols/2==out.cols); + assert( in.type()==CV_8U && out.type()==CV_8U); + +#ifdef __SSE2__ + if(aligned_mem::is_aligned16(in.data) && aligned_mem::is_aligned16(out.data) && ((in.cols % 16) == 0)) + { + halfSampleSSE2(in.data, out.data, in.cols, in.rows); + return; + } +#endif +#ifdef __ARM_NEON__ + if( (in.cols % 16) == 0 ) + { + halfSampleNEON(in, out); + return; + } +#endif + + const int stride = in.step.p[0]; + uint8_t* top = (uint8_t*) in.data; + uint8_t* bottom = top + stride; + uint8_t* end = top + stride*in.rows; + const int out_width = out.cols; + uint8_t* p = (uint8_t*) out.data; + while (bottom < end) + { + for (int j=0; j( (uint16_t (top[0]) + top[1] + bottom[0] + bottom[1])/4 ); + p++; + top += 2; + bottom += 2; + } + top += stride; + bottom += stride; + } +} + + +float +shiTomasiScore(const cv::Mat& img, int u, int v) +{ + assert(img.type() == CV_8UC1); + + float dXX = 0.0; + float dYY = 0.0; + float dXY = 0.0; + const int halfbox_size = 4; + const int box_size = 2*halfbox_size; + const int box_area = box_size*box_size; + const int x_min = u-halfbox_size; + const int x_max = u+halfbox_size; + const int y_min = v-halfbox_size; + const int y_max = v+halfbox_size; + + if(x_min < 1 || x_max >= img.cols-1 || y_min < 1 || y_max >= img.rows-1) + return 0.0; // patch is too close to the boundary + + const int stride = img.step.p[0]; + for( int y=y_min; y::depth, cn*2)); + + int x, y, delta = (int)alignSize((cols + 2)*cn, 16); + AutoBuffer _tempBuf(delta*2 + 64); + deriv_type *trow0 = alignPtr(_tempBuf + cn, 16), *trow1 = alignPtr(trow0 + delta, 16); + +#ifdef __SSE2__ + __m128i z = _mm_setzero_si128(), c3 = _mm_set1_epi16(3), c10 = _mm_set1_epi16(10); +#endif + + for( y = 0; y < rows; y++ ) + { + const uchar* srow0 = src.ptr(y > 0 ? y-1 : rows > 1 ? 1 : 0); + const uchar* srow1 = src.ptr(y); + const uchar* srow2 = src.ptr(y < rows-1 ? y+1 : rows > 1 ? rows-2 : 0); + deriv_type* drow = dst.ptr(y); + + // do vertical convolution + x = 0; +#ifdef __SSE2__ + for( ; x <= colsn - 8; x += 8 ) + { + __m128i s0 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow0 + x)), z); + __m128i s1 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow1 + x)), z); + __m128i s2 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow2 + x)), z); + __m128i t0 = _mm_add_epi16(_mm_mullo_epi16(_mm_add_epi16(s0, s2), c3), _mm_mullo_epi16(s1, c10)); + __m128i t1 = _mm_sub_epi16(s2, s0); + _mm_store_si128((__m128i*)(trow0 + x), t0); + _mm_store_si128((__m128i*)(trow1 + x), t1); + } +#endif + for( ; x < colsn; x++ ) + { + int t0 = (srow0[x] + srow2[x])*3 + srow1[x]*10; + int t1 = srow2[x] - srow0[x]; + trow0[x] = (deriv_type)t0; + trow1[x] = (deriv_type)t1; + } + + // make border + int x0 = (cols > 1 ? 1 : 0)*cn, x1 = (cols > 1 ? cols-2 : 0)*cn; + for( int k = 0; k < cn; k++ ) + { + trow0[-cn + k] = trow0[x0 + k]; trow0[colsn + k] = trow0[x1 + k]; + trow1[-cn + k] = trow1[x0 + k]; trow1[colsn + k] = trow1[x1 + k]; + } + + // do horizontal convolution, interleave the results and store them to dst + x = 0; +#ifdef __SSE2__ + for( ; x <= colsn - 8; x += 8 ) + { + __m128i s0 = _mm_loadu_si128((const __m128i*)(trow0 + x - cn)); + __m128i s1 = _mm_loadu_si128((const __m128i*)(trow0 + x + cn)); + __m128i s2 = _mm_loadu_si128((const __m128i*)(trow1 + x - cn)); + __m128i s3 = _mm_load_si128((const __m128i*)(trow1 + x)); + __m128i s4 = _mm_loadu_si128((const __m128i*)(trow1 + x + cn)); + + __m128i t0 = _mm_sub_epi16(s1, s0); + __m128i t1 = _mm_add_epi16(_mm_mullo_epi16(_mm_add_epi16(s2, s4), c3), _mm_mullo_epi16(s3, c10)); + __m128i t2 = _mm_unpacklo_epi16(t0, t1); + t0 = _mm_unpackhi_epi16(t0, t1); + // this can probably be replaced with aligned stores if we aligned dst properly. + _mm_storeu_si128((__m128i*)(drow + x*2), t2); + _mm_storeu_si128((__m128i*)(drow + x*2 + 8), t0); + } +#endif + for( ; x < colsn; x++ ) + { + deriv_type t0 = (deriv_type)(trow0[x+cn] - trow0[x-cn]); + deriv_type t1 = (deriv_type)((trow1[x+cn] + trow1[x-cn])*3 + trow1[x]*10); + drow[x*2] = t0; drow[x*2+1] = t1; + } + } + +// vector vec_mat; +// cv::split(dst, vec_mat); +// cv::namedWindow("deriv"); +// cv::imshow("deriv", vec_mat[0]); +// cv::namedWindow("derivy"); +// cv::imshow("derivy", vec_mat[1]); +// cv::waitKey(0); +} + +#ifdef __SSE2__ +void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale) +{ + depth_32f.create(depth_16u.rows, depth_16u.cols, CV_32FC1); + + const unsigned short* input_ptr = depth_16u.ptr(); + float* output_ptr = depth_32f.ptr(); + + __m128 _scale = _mm_set1_ps(scale); + __m128 _zero = _mm_setzero_ps(); + __m128 _nan = _mm_set1_ps(std::numeric_limits::quiet_NaN()); + + for(int idx = 0; idx < depth_16u.size().area(); idx += 8, input_ptr += 8, output_ptr += 8) + { + __m128 _input, mask; + __m128i _inputi = _mm_load_si128((__m128i*) input_ptr); + + // load low shorts and convert to float + _input = _mm_cvtepi32_ps(_mm_unpacklo_epi16(_inputi, _mm_setzero_si128())); + + mask = _mm_cmpeq_ps(_input, _zero); + + // zero to nan + _input = _mm_or_ps(_input, _mm_and_ps(mask, _nan)); + // scale + _input = _mm_mul_ps(_input, _scale); + // save + _mm_store_ps(output_ptr + 0, _input); + + // load high shorts and convert to float + _input = _mm_cvtepi32_ps(_mm_unpackhi_epi16(_inputi, _mm_setzero_si128())); + + mask = _mm_cmpeq_ps(_input, _zero); + + // zero to nan + _input = _mm_or_ps(_input, _mm_and_ps(mask, _nan)); + // scale + _input = _mm_mul_ps(_input, _scale); + // save + _mm_store_ps(output_ptr + 4, _input); + } +} +#endif + +} + + diff --git a/src/rpg_vikit/vikit_common/test/data/scene_000.png b/src/rpg_vikit/vikit_common/test/data/scene_000.png new file mode 100644 index 0000000..7fdc8f9 Binary files /dev/null and b/src/rpg_vikit/vikit_common/test/data/scene_000.png differ diff --git a/src/rpg_vikit/vikit_common/test/test_camera.cpp b/src/rpg_vikit/vikit_common/test/test_camera.cpp new file mode 100644 index 0000000..ec48953 --- /dev/null +++ b/src/rpg_vikit/vikit_common/test/test_camera.cpp @@ -0,0 +1,82 @@ +/* + * camera_pinhole_test.cpp + * + * Created on: Oct 26, 2012 + * Author: cforster + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace Eigen; + + +void testTiming(vk::AbstractCamera* cam) +{ + Vector3d xyz; + Vector2d px(320.64, 253.54); + vk::Timer t; + t.start(); + for(size_t i=0; i<1000; ++i) + { + xyz = cam->cam2world(px); + } + t.stop(); + cout << "Time unproject = " << t.getTime()*1000 << "ms" << endl; + + t.start(); + for(size_t i=0; i<1000; ++i) + { + px = cam->world2cam(xyz); + } + t.stop(); + cout << "Time project = " << t.getTime()*1000 << "ms" << endl; +} + +void testAccuracy(vk::AbstractCamera* cam) +{ + double error = 0.0; + vk::Timer t; + for(size_t i=0; i<1000; ++i) + { + Vector2d px(1.0/100.0 * vk::Sample::uniform(0, cam->width()*100), + 1.0/100.0 * vk::Sample::uniform(0, cam->height()*100)); + Vector3d xyz = cam->cam2world(px); + Vector2d px2 = cam->world2cam(xyz); + error += (px-px2).norm(); + } + cout << "Reprojection error = " << error << " (took " << t.stop()*1000 << "ms)" << endl; + +} + +int main(int argc, char **argv) +{ + vk::AbstractCamera* cam_pinhole = + new vk::PinholeCamera(640, 480, + 323.725240539365, 323.53310403533, + 336.407165453746, 235.018271952295, + -0.258617082313663, 0.0623042373522829, 0.000445967802619555, -0.000269839440982019); + + vk::AbstractCamera* cam_atan = + new vk::ATANCamera(752, 480, 0.511496, 0.802603, 0.530199, 0.496011, 0.934092); + + printf("\nPINHOLE CAMERA:\n"); + testTiming(cam_pinhole); + testAccuracy(cam_pinhole); + + printf("\nATAN CAMERA:\n"); + testTiming(cam_atan); + testAccuracy(cam_atan); + + return 0; +} \ No newline at end of file diff --git a/src/rpg_vikit/vikit_common/test/test_patch_score.cpp b/src/rpg_vikit/vikit_common/test/test_patch_score.cpp new file mode 100644 index 0000000..a1cfe2e --- /dev/null +++ b/src/rpg_vikit/vikit_common/test/test_patch_score.cpp @@ -0,0 +1,107 @@ +/* + * test_patch_score.cpp + * + * Created on: Dec 4, 2012 + * Author: cforster + */ + +#include +#include +#include +#include +#include + +namespace { + +const int g_halfpatch_size = 4; +const int g_patch_size = g_halfpatch_size*2; + +void copyPatch(const cv::Mat& img, int x, int y, uint8_t* patch_data) +{ + cv::Mat patch(g_patch_size, g_patch_size, CV_8U, patch_data); + img(cv::Range(y-g_halfpatch_size, y+g_halfpatch_size), + cv::Range(x-g_halfpatch_size, x+g_halfpatch_size)).copyTo(patch); +} + +void copyPatch2(cv::Mat& img, int x, int y, uint8_t* patch_data) +{ + for(int v=0; v PatchScore; + + // create patch + uint8_t* ref_patch = vk::aligned_mem::aligned_alloc(g_patch_size*g_patch_size, 16); + uint8_t* cur_patch = vk::aligned_mem::aligned_alloc(g_patch_size*g_patch_size, 16); + + vk::Timer t; + for(int i=0; i<1000000; ++i) + { + copyPatch(img, x+10, y+10, ref_patch); + copyPatch(img, x, y, cur_patch); + } + printf("Copy patch cost %f\n", t.stop()); + + t.start(); + for(int i=0; i<1000000; ++i) + { + copyPatch2(img, x+10, y+10, ref_patch); + copyPatch2(img, x, y, cur_patch); + } + printf("Copy patch cost %f\n", t.stop()); + + // compute patch score + t.start(); + int b=10; + for(int i=0; i<1000000; ++i) + { + PatchScore patch_score(ref_patch); + b += patch_score.computeScore(cur_patch); + } + printf("Compute cost cost %f, %i\n", t.stop(), b); + + // compute patch score + t.start(); + int c=10; + uint8_t* data_ptr = img.data + (y-g_halfpatch_size)*img.cols + (x-g_halfpatch_size); + for(int i=0; i<1000000; ++i) + { + PatchScore patch_score(ref_patch); + c += patch_score.computeScore(data_ptr, img.cols); + } + printf("Compute cost, stride %f, %i\n", t.stop(), c); + + // check results + { + PatchScore patch_score(ref_patch); + printf("Score = %i\n", patch_score.computeScore(cur_patch)); + } + + // check results + { + PatchScore patch_score(ref_patch); + printf("Score = %i\n", patch_score.computeScore(data_ptr, img.cols)); + } + +} + +} // namespace + + +int main(int argc, char **argv) +{ + std::string img_name("test/data/scene_000.png"); + cv::Mat img(cv::imread(img_name, 0)); + assert(!img.empty()); + + testZMSSD(img); + + return 0; +} diff --git a/src/rpg_vikit/vikit_common/test/test_triangulation.cpp b/src/rpg_vikit/vikit_common/test/test_triangulation.cpp new file mode 100644 index 0000000..37b82c4 --- /dev/null +++ b/src/rpg_vikit/vikit_common/test/test_triangulation.cpp @@ -0,0 +1,39 @@ +/* + * camera_pinhole_test.cpp + * + * Created on: Oct 26, 2012 + * Author: cforster + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace Eigen; + +int main(int argc, char **argv) +{ + vk::PinholeCamera cam(752, 480, 1.0, 414.5, 414.2, 348.8, 240.0); + Matrix4d T_ref_cur(Matrix4d::Identity()); + T_ref_cur(0,3) = 0.5; + Vector2d u_cur(200, 300); + Vector3d f_cur(cam.cam2world(u_cur)); + double depth_cur = 2.0; + Vector3d f_ref(vk::project3d(T_ref_cur*vk::unproject3d(f_cur*depth_cur))); + double depth_ref = f_ref.norm(); + Vector2d u_ref(cam.world2cam(f_ref)); + double z_ref, z_cur; + vk::depthFromTriangulationExact(T_ref_cur.topLeftCorner<3,3>(), T_ref_cur.topRightCorner<3,1>(), f_ref, f_cur, z_ref, z_cur); + printf("depth = %f, triangulated depth = %f\n", depth_cur, z_cur); + printf("depth = %f, triangulated depth = %f\n", depth_ref, z_ref); + + return 0; +} diff --git a/src/rpg_vikit/vikit_common/vikit_commonConfig.cmake.in b/src/rpg_vikit/vikit_common/vikit_commonConfig.cmake.in new file mode 100644 index 0000000..2d7af57 --- /dev/null +++ b/src/rpg_vikit/vikit_common/vikit_commonConfig.cmake.in @@ -0,0 +1,17 @@ +####################################################### +# vikit_common source dir +set( vikit_common_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@") + +####################################################### +# vikit_common build dir +set( vikit_common_DIR "@CMAKE_CURRENT_BINARY_DIR@") + +####################################################### +set( vikit_common_INCLUDE_DIR "@vikit_common_INCLUDE_DIR@" ) +set( vikit_common_INCLUDE_DIRS "@vikit_common_INCLUDE_DIR@" ) + +set( vikit_common_LIBRARIES "@vikit_common_LIBRARIES@" ) +set( vikit_common_LIBRARY "@vikit_common_LIBRARIES@" ) + +set( vikit_common_LIBRARY_DIR "@vikit_common_LIBRARY_DIR@" ) +set( vikit_common_LIBRARY_DIRS "@vikit_common_LIBRARY_DIR@" ) diff --git a/src/rpg_vikit/vikit_py/CMakeLists.txt b/src/rpg_vikit/vikit_py/CMakeLists.txt new file mode 100644 index 0000000..8a5912d --- /dev/null +++ b/src/rpg_vikit/vikit_py/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.5) +project(vikit_py) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) + +# set dependencies +set(dependencies +rclpy +) + +#install python modules +# ament_python_install_package(${PROJECT_NAME}) + +#install python executables +install(PROGRAMS + src/vikit_py/align_trajectory.py + src/vikit_py/cpu_info.py + src/vikit_py/depthmap_utils.py + src/vikit_py/math_utils.py + src/vikit_py/ros_node.py + src/vikit_py/transformations.py + DESTINATION lib/${PROJECT_NAME} +) + +#-------------------------------------------------------------------- +# export dependencies +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/src/rpg_vikit/vikit_py/package.xml b/src/rpg_vikit/vikit_py/package.xml new file mode 100644 index 0000000..050d928 --- /dev/null +++ b/src/rpg_vikit/vikit_py/package.xml @@ -0,0 +1,22 @@ + + + + vikit_py + 0.0.0 + The vikit_py package + Christian Forster + TODO: License declaration + + ament_cmake + + rclcpp + rclpy + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/rpg_vikit/vikit_py/setup.py b/src/rpg_vikit/vikit_py/setup.py new file mode 100644 index 0000000..5aec0e5 --- /dev/null +++ b/src/rpg_vikit/vikit_py/setup.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['vikit_py'], + package_dir={'': 'src'}, + install_requires=['rclpy', 'yaml'], + ) + +setup(**d) \ No newline at end of file diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/.gitignore b/src/rpg_vikit/vikit_py/src/vikit_py/.gitignore new file mode 100644 index 0000000..7e99e36 --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/.gitignore @@ -0,0 +1 @@ +*.pyc \ No newline at end of file diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/__init__.py b/src/rpg_vikit/vikit_py/src/vikit_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/align_trajectory.py b/src/rpg_vikit/vikit_py/src/vikit_py/align_trajectory.py new file mode 100644 index 0000000..f4201f8 --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/align_trajectory.py @@ -0,0 +1,129 @@ +#!/usr/bin/python + +import numpy as np +import vikit_py.transformations as transformations + +def align_sim3(model, data): + """Implementation of the paper: S. Umeyama, Least-Squares Estimation + of Transformation Parameters Between Two Point Patterns, + IEEE Trans. Pattern Anal. Mach. Intell., vol. 13, no. 4, 1991. + + Input: + model -- first trajectory (3xn) + data -- second trajectory (3xn) + + Output: + s -- scale factor (scalar) + R -- rotation matrix (3x3) + t -- translation vector (3x1) + t_error -- translational error per point (1xn) + + """ + + # substract mean + mu_M = model.mean(0).reshape(model.shape[0],1) + mu_D = data.mean(0).reshape(data.shape[0],1) + model_zerocentered = model - mu_M + data_zerocentered = data - mu_D + n = np.shape(model)[0] + + # correlation + C = 1.0/n*np.dot(model_zerocentered.transpose(), data_zerocentered) + sigma2 = 1.0/n*np.multiply(data_zerocentered,data_zerocentered).sum() + U_svd,D_svd,V_svd = np.linalg.linalg.svd(C) + D_svd = np.diag(D_svd) + V_svd = np.transpose(V_svd) + S = np.eye(3) + + if(np.linalg.det(U_svd)*np.linalg.det(V_svd) < 0): + S[2,2] = -1 + + R = np.dot(U_svd, np.dot(S, np.transpose(V_svd))) + s = 1.0/sigma2*np.trace(np.dot(D_svd, S)) + t = mu_M-s*np.dot(R,mu_D) + + # TODO: + # model_aligned = s * R * model + t + # alignment_error = model_aligned - data + # t_error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),0)).A[0] + + return s, R, t #, t_error + +def align_se3(model,data, precision = False): + """Align two trajectories using the method of Horn (closed-form). + + Input: + model -- first trajectory (3xn) + data -- second trajectory (3xn) + + Output: + R -- rotation matrix (3x3) + t -- translation vector (3x1) + t_error -- translational error per point (1xn) + + """ + if not precision: + np.set_printoptions(precision=3,suppress=True) + model_zerocentered = model - model.mean(1).reshape(model.shape[0],1) + data_zerocentered = data - data.mean(1).reshape(data.shape[0],1) + + W = np.zeros( (3,3) ) + for column in range(model.shape[1]): + W += np.outer(model_zerocentered[:,column],data_zerocentered[:,column]) + U,d,Vh = np.linalg.linalg.svd(W.transpose()) + S = np.matrix(np.identity( 3 )) + if(np.linalg.det(U) * np.linalg.det(Vh)<0): + S[2,2] = -1 + R = U*S*Vh + t = data.mean(1).reshape(data.shape[0],1) - R * model.mean(1).reshape(model.shape[0],1) + + model_aligned = R * model + t + alignment_error = model_aligned - data + t_error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),0)).A[0] + + return R, t, t_error + +def _matrix_log(A): + theta = np.arccos((np.trace(A)-1.0)/2.0) + log_theta = 0.5*theta/np.sin(theta) * (A - A.transpose()) + x = np.array([log_theta[2,1], log_theta[0,2], log_theta[1,0]]) + return x + +def hand_eye_calib(q_gt, q_es, p_gt, p_es, I, delta=10, verbose=True): + """Implementation of the least squares solution described in the paper: + Robot Sensor Calibration: Solving AX=XB on the Euclidean Group + by Frank C. Park and Bryan J. Martin + """ + n = np.shape(I)[0] + M = np.zeros([3,3]) + C = np.zeros([3*n, 3]) + b_A = np.zeros([3*n,1]) + b_B = np.zeros([3*n,1]) + for ix, i in enumerate(I): + A1 = transformations.quaternion_matrix(q_es[i,:])[:3,:3] + A2 = transformations.quaternion_matrix(q_es[i+delta,:])[:3,:3] + A = np.dot(A1.transpose(), A2) + B1 = transformations.quaternion_matrix(q_gt[i,:])[:3,:3] + B2 = transformations.quaternion_matrix(q_gt[i+delta,:])[:3,:3] + B = np.dot(B1.transpose(), B2) + alpha = _matrix_log(A) + beta = _matrix_log(B) + M = M + np.dot(np.matrix(beta).transpose(), np.matrix(alpha)) + C[3*ix:3*ix+3,:] = np.eye(3) - A + b_A[3*ix:3*ix+3,0] = np.dot(np.transpose(A1), p_es[i+delta,:]-p_es[i,:]) + b_B[3*ix:3*ix+3,0] = np.dot(np.transpose(B1), p_gt[i+delta,:]-p_gt[i,:]) + + # compute rotation + D,V = np.linalg.linalg.eig(np.dot(M.transpose(), M)) + Lambda = np.diag([np.sqrt(1.0/D[0]), np.sqrt(1.0/D[1]), np.sqrt(1.0/D[2])]) + Vinv = np.linalg.linalg.inv(V) + X = np.dot(V, np.dot(Lambda, np.dot(Vinv, M.transpose()))) + + # compute translation + d = np.zeros([3*n,1]) + for i in range(n): + d[3*i:3*i+3,:] = b_A[3*i:3*i+3,:] - np.dot(X, b_B[3*i:3*i+3,:]) + + b = np.dot(np.linalg.inv(np.dot(np.transpose(C),C)), np.dot(np.transpose(C),d)) + + return np.array(X),b diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/cpu_info.py b/src/rpg_vikit/vikit_py/src/vikit_py/cpu_info.py new file mode 100644 index 0000000..d5a629b --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/cpu_info.py @@ -0,0 +1,11 @@ +#!/usr/bin/python + +import subprocess, re + +def get_cpu_info(): + command = "cat /proc/cpuinfo" + all_info = subprocess.check_output(command, shell=True).strip() + for line in all_info.split("\n"): + if "model name" in line: + model_name = re.sub(".*model name.*:", "", line,1).strip() + return model_name.replace("(R)","").replace("(TM)", "") \ No newline at end of file diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/depthmap_utils.py b/src/rpg_vikit/vikit_py/src/vikit_py/depthmap_utils.py new file mode 100644 index 0000000..7eb639a --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/depthmap_utils.py @@ -0,0 +1,50 @@ +# -*- coding: utf-8 -*- + +import numpy as np +import os +import matplotlib.pyplot as plt +from mpl_toolkits.axes_grid1 import make_axes_locatable + +def load_depthmap(depthmap_full_file_path, depthmap_rows, depthmap_cols, + fileformat=np.float32, is_megapov_depthmap = 0): + depth_array = [] + if depthmap_full_file_path.endswith('.bin'): + try: + depth_array = np.fromfile(depthmap_full_file_path, fileformat, -1, '') # the separator character '' specifies a binary file + except IOError: + print 'Could not open file ' + depthmap_full_file_path + ' for reading binary data.' + raise + else: + if depthmap_full_file_path.endswith('.depth'): + try: + if is_megapov_depthmap: + depth_array = np.fromfile(depthmap_full_file_path, dtype='>d') + else: + depth_array = np.fromfile(depthmap_full_file_path, np.float32, -1, ' ') # the separator character ' ' specifies a text file + except IOError: + print 'Could not open file ' + depthmap_full_file_path + ' for reading text data.' + raise + else: + raise MapIOError('Depthmap filename suffix is not correct.') + if (len(depth_array) != (depthmap_rows * depthmap_cols)): + raise MapIOError('Read data do not match the provided size.') + return depth_array + +def load_povray_depthmap(depthmap_full_file_path, rows, cols, + scale_factor = 1.0, is_megapov_depthmap = 0): + depth_array = [] + try: + depth_array = load_depthmap(depthmap_full_file_path, rows, cols, is_megapov_depthmap) + except IOError: + raise + except MapIOError: + raise + return depth_array * scale_factor + +def show_depthmap(ax, depth_array, rows, cols, min_value = None, max_value = None): + image = np.reshape(depth_array, [rows, cols], 'C') + im = ax.imshow(image, vmin = min_value, vmax = max_value) + divider = make_axes_locatable(ax) + cax = divider.append_axes("right", size="5%", pad=0.05) + plt.colorbar(im, cax=cax) + \ No newline at end of file diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/math_utils.py b/src/rpg_vikit/vikit_py/src/vikit_py/math_utils.py new file mode 100644 index 0000000..1d35ac8 --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/math_utils.py @@ -0,0 +1,22 @@ +# -*- coding: utf-8 -*- +""" +Created on Wed Aug 7 22:13:06 2013 + +@author: cforster +""" + +import numpy as np + +def unproject(a): + """Makes a vector homogeneous""" + return np.append(a, 1) + +def project(a): + """De-homogenises a vector""" + return a[:-1]/float(a[-1]) + +def skew(v): + """Returns the skew-symmetric matrix of a vector""" + return np.matrix([[0, -v[2], v[1]], + [v[2], 0, -v[0]], + [-v[1], v[0], 0]], dtype=np.float32) \ No newline at end of file diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/ros_node.py b/src/rpg_vikit/vikit_py/src/vikit_py/ros_node.py new file mode 100644 index 0000000..7bd05cb --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/ros_node.py @@ -0,0 +1,23 @@ +#!/usr/bin/python + +import os + +class RosNode: + def __init__(self, package, executable): + self._package = package + self._executable = executable + self._param_string = '' + + def add_parameters(self, namespace, parameter_dictionary): + for key in parameter_dictionary.keys(): + if type(parameter_dictionary[key]) is dict: + self.add_parameters(namespace+key+'/', parameter_dictionary[key]) + else: + self._param_string += ' _'+namespace+key+':='+str(parameter_dictionary[key]) + + def run(self, parameter_dictionary, namespace=''): + self.add_parameters(namespace, parameter_dictionary) + print('Starting ROS node with parameters: '+self._param_string) + + os.system('ros2 run ' + self._package + ' ' + self._executable + ' ' + self._param_string) + print('ROS node finished processing.') \ No newline at end of file diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/transformations.py b/src/rpg_vikit/vikit_py/src/vikit_py/transformations.py new file mode 100644 index 0000000..d954cb2 --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/transformations.py @@ -0,0 +1,1709 @@ +# -*- coding: utf-8 -*- +# transformations.py + +# Copyright (c) 2006, Christoph Gohlke +# Copyright (c) 2006-2009, The Regents of the University of California +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the copyright holders nor the names of any +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Homogeneous Transformation Matrices and Quaternions. + +A library for calculating 4x4 matrices for translating, rotating, reflecting, +scaling, shearing, projecting, orthogonalizing, and superimposing arrays of +3D homogeneous coordinates as well as for converting between rotation matrices, +Euler angles, and quaternions. Also includes an Arcball control object and +functions to decompose transformation matrices. + +:Authors: + `Christoph Gohlke `__, + Laboratory for Fluorescence Dynamics, University of California, Irvine + +:Version: 20090418 + +Requirements +------------ + +* `Python 2.6 `__ +* `Numpy 1.3 `__ +* `transformations.c 20090418 `__ + (optional implementation of some functions in C) + +Notes +----- + +Matrices (M) can be inverted using numpy.linalg.inv(M), concatenated using +numpy.dot(M0, M1), or used to transform homogeneous coordinates (v) using +numpy.dot(M, v) for shape (4, \*) "point of arrays", respectively +numpy.dot(v, M.T) for shape (\*, 4) "array of points". + +Calculations are carried out with numpy.float64 precision. + +This Python implementation is not optimized for speed. + +Vector, point, quaternion, and matrix function arguments are expected to be +"array like", i.e. tuple, list, or numpy arrays. + +Return types are numpy arrays unless specified otherwise. + +Angles are in radians unless specified otherwise. + +Quaternions ix+jy+kz+w are represented as [x, y, z, w]. + +Use the transpose of transformation matrices for OpenGL glMultMatrixd(). + +A triple of Euler angles can be applied/interpreted in 24 ways, which can +be specified using a 4 character string or encoded 4-tuple: + + *Axes 4-string*: e.g. 'sxyz' or 'ryxy' + + - first character : rotations are applied to 's'tatic or 'r'otating frame + - remaining characters : successive rotation axis 'x', 'y', or 'z' + + *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) + + - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. + - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed + by 'z', or 'z' is followed by 'x'. Otherwise odd (1). + - repetition : first and last axis are same (1) or different (0). + - frame : rotations are applied to static (0) or rotating (1) frame. + +References +---------- + +(1) Matrices and transformations. Ronald Goldman. + In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. +(2) More matrices and transformations: shear and pseudo-perspective. + Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. +(3) Decomposing a matrix into simple transformations. Spencer Thomas. + In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. +(4) Recovering the data from the transformation matrix. Ronald Goldman. + In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. +(5) Euler angle conversion. Ken Shoemake. + In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. +(6) Arcball rotation control. Ken Shoemake. + In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. +(7) Representing attitude: Euler angles, unit quaternions, and rotation + vectors. James Diebel. 2006. +(8) A discussion of the solution for the best rotation to relate two sets + of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. +(9) Closed-form solution of absolute orientation using unit quaternions. + BKP Horn. J Opt Soc Am A. 1987. 4(4), 629-642. +(10) Quaternions. Ken Shoemake. + http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf +(11) From quaternion to matrix and back. JMP van Waveren. 2005. + http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm +(12) Uniform random rotations. Ken Shoemake. + In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. + + +Examples +-------- + +>>> alpha, beta, gamma = 0.123, -1.234, 2.345 +>>> origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1) +>>> I = identity_matrix() +>>> Rx = rotation_matrix(alpha, xaxis) +>>> Ry = rotation_matrix(beta, yaxis) +>>> Rz = rotation_matrix(gamma, zaxis) +>>> R = concatenate_matrices(Rx, Ry, Rz) +>>> euler = euler_from_matrix(R, 'rxyz') +>>> numpy.allclose([alpha, beta, gamma], euler) +True +>>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') +>>> is_same_transform(R, Re) +True +>>> al, be, ga = euler_from_matrix(Re, 'rxyz') +>>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) +True +>>> qx = quaternion_about_axis(alpha, xaxis) +>>> qy = quaternion_about_axis(beta, yaxis) +>>> qz = quaternion_about_axis(gamma, zaxis) +>>> q = quaternion_multiply(qx, qy) +>>> q = quaternion_multiply(q, qz) +>>> Rq = quaternion_matrix(q) +>>> is_same_transform(R, Rq) +True +>>> S = scale_matrix(1.23, origin) +>>> T = translation_matrix((1, 2, 3)) +>>> Z = shear_matrix(beta, xaxis, origin, zaxis) +>>> R = random_rotation_matrix(numpy.random.rand(3)) +>>> M = concatenate_matrices(T, R, Z, S) +>>> scale, shear, angles, trans, persp = decompose_matrix(M) +>>> numpy.allclose(scale, 1.23) +True +>>> numpy.allclose(trans, (1, 2, 3)) +True +>>> numpy.allclose(shear, (0, math.tan(beta), 0)) +True +>>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) +True +>>> M1 = compose_matrix(scale, shear, angles, trans, persp) +>>> is_same_transform(M, M1) +True + +""" + +from __future__ import division + +import warnings +import math + +import numpy + +# Documentation in HTML format can be generated with Epydoc +__docformat__ = "restructuredtext en" + + +def identity_matrix(): + """Return 4x4 identity/unit matrix. + + >>> I = identity_matrix() + >>> numpy.allclose(I, numpy.dot(I, I)) + True + >>> numpy.sum(I), numpy.trace(I) + (4.0, 4.0) + >>> numpy.allclose(I, numpy.identity(4, dtype=numpy.float64)) + True + + """ + return numpy.identity(4, dtype=numpy.float64) + + +def translation_matrix(direction): + """Return matrix to translate by direction vector. + + >>> v = numpy.random.random(3) - 0.5 + >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) + True + + """ + M = numpy.identity(4) + M[:3, 3] = direction[:3] + return M + + +def translation_from_matrix(matrix): + """Return translation vector from translation matrix. + + >>> v0 = numpy.random.random(3) - 0.5 + >>> v1 = translation_from_matrix(translation_matrix(v0)) + >>> numpy.allclose(v0, v1) + True + + """ + return numpy.array(matrix, copy=False)[:3, 3].copy() + +def convert_3x3_to_4x4(matrix_3x3): + M = numpy.identity(4) + M[:3,:3] = matrix_3x3 + return M + +def reflection_matrix(point, normal): + """Return matrix to mirror at plane defined by point and normal vector. + + >>> v0 = numpy.random.random(4) - 0.5 + >>> v0[3] = 1.0 + >>> v1 = numpy.random.random(3) - 0.5 + >>> R = reflection_matrix(v0, v1) + >>> numpy.allclose(2., numpy.trace(R)) + True + >>> numpy.allclose(v0, numpy.dot(R, v0)) + True + >>> v2 = v0.copy() + >>> v2[:3] += v1 + >>> v3 = v0.copy() + >>> v2[:3] -= v1 + >>> numpy.allclose(v2, numpy.dot(R, v3)) + True + + """ + normal = unit_vector(normal[:3]) + M = numpy.identity(4) + M[:3, :3] -= 2.0 * numpy.outer(normal, normal) + M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal + return M + + +def reflection_from_matrix(matrix): + """Return mirror plane point and normal vector from reflection matrix. + + >>> v0 = numpy.random.random(3) - 0.5 + >>> v1 = numpy.random.random(3) - 0.5 + >>> M0 = reflection_matrix(v0, v1) + >>> point, normal = reflection_from_matrix(M0) + >>> M1 = reflection_matrix(point, normal) + >>> is_same_transform(M0, M1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + # normal: unit eigenvector corresponding to eigenvalue -1 + l, V = numpy.linalg.eig(M[:3, :3]) + i = numpy.where(abs(numpy.real(l) + 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue -1") + normal = numpy.real(V[:, i[0]]).squeeze() + # point: any unit eigenvector corresponding to eigenvalue 1 + l, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + return point, normal + + +def rotation_matrix(angle, direction, point=None): + """Return matrix to rotate about axis defined by point and direction. + + >>> angle = (random.random() - 0.5) * (2*math.pi) + >>> direc = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> R0 = rotation_matrix(angle, direc, point) + >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) + >>> is_same_transform(R0, R1) + True + >>> R0 = rotation_matrix(angle, direc, point) + >>> R1 = rotation_matrix(-angle, -direc, point) + >>> is_same_transform(R0, R1) + True + >>> I = numpy.identity(4, numpy.float64) + >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) + True + >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2, + ... direc, point))) + True + + """ + sina = math.sin(angle) + cosa = math.cos(angle) + direction = unit_vector(direction[:3]) + # rotation matrix around unit vector + R = numpy.array(((cosa, 0.0, 0.0), + (0.0, cosa, 0.0), + (0.0, 0.0, cosa)), dtype=numpy.float64) + R += numpy.outer(direction, direction) * (1.0 - cosa) + direction *= sina + R += numpy.array((( 0.0, -direction[2], direction[1]), + ( direction[2], 0.0, -direction[0]), + (-direction[1], direction[0], 0.0)), + dtype=numpy.float64) + M = numpy.identity(4) + M[:3, :3] = R + if point is not None: + # rotation not around origin + point = numpy.array(point[:3], dtype=numpy.float64, copy=False) + M[:3, 3] = point - numpy.dot(R, point) + return M + + +def rotation_from_matrix(matrix): + """Return rotation angle and axis from rotation matrix. + + >>> angle = (random.random() - 0.5) * (2*math.pi) + >>> direc = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> R0 = rotation_matrix(angle, direc, point) + >>> angle, direc, point = rotation_from_matrix(R0) + >>> R1 = rotation_matrix(angle, direc, point) + >>> is_same_transform(R0, R1) + True + + """ + R = numpy.array(matrix, dtype=numpy.float64, copy=False) + R33 = R[:3, :3] + # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 + l, W = numpy.linalg.eig(R33.T) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + direction = numpy.real(W[:, i[-1]]).squeeze() + # point: unit eigenvector of R33 corresponding to eigenvalue of 1 + l, Q = numpy.linalg.eig(R) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + point = numpy.real(Q[:, i[-1]]).squeeze() + point /= point[3] + # rotation angle depending on direction + cosa = (numpy.trace(R33) - 1.0) / 2.0 + if abs(direction[2]) > 1e-8: + sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] + elif abs(direction[1]) > 1e-8: + sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] + else: + sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] + angle = math.atan2(sina, cosa) + return angle, direction, point + + +def scale_matrix(factor, origin=None, direction=None): + """Return matrix to scale by factor around origin in direction. + + Use factor -1 for point symmetry. + + >>> v = (numpy.random.rand(4, 5) - 0.5) * 20.0 + >>> v[3] = 1.0 + >>> S = scale_matrix(-1.234) + >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) + True + >>> factor = random.random() * 10 - 5 + >>> origin = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> S = scale_matrix(factor, origin) + >>> S = scale_matrix(factor, origin, direct) + + """ + if direction is None: + # uniform scaling + M = numpy.array(((factor, 0.0, 0.0, 0.0), + (0.0, factor, 0.0, 0.0), + (0.0, 0.0, factor, 0.0), + (0.0, 0.0, 0.0, 1.0)), dtype=numpy.float64) + if origin is not None: + M[:3, 3] = origin[:3] + M[:3, 3] *= 1.0 - factor + else: + # nonuniform scaling + direction = unit_vector(direction[:3]) + factor = 1.0 - factor + M = numpy.identity(4) + M[:3, :3] -= factor * numpy.outer(direction, direction) + if origin is not None: + M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction + return M + + +def scale_from_matrix(matrix): + """Return scaling factor, origin and direction from scaling matrix. + + >>> factor = random.random() * 10 - 5 + >>> origin = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> S0 = scale_matrix(factor, origin) + >>> factor, origin, direction = scale_from_matrix(S0) + >>> S1 = scale_matrix(factor, origin, direction) + >>> is_same_transform(S0, S1) + True + >>> S0 = scale_matrix(factor, origin, direct) + >>> factor, origin, direction = scale_from_matrix(S0) + >>> S1 = scale_matrix(factor, origin, direction) + >>> is_same_transform(S0, S1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + factor = numpy.trace(M33) - 2.0 + try: + # direction: unit eigenvector corresponding to eigenvalue factor + l, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(l) - factor) < 1e-8)[0][0] + direction = numpy.real(V[:, i]).squeeze() + direction /= vector_norm(direction) + except IndexError: + # uniform scaling + factor = (factor + 2.0) / 3.0 + direction = None + # origin: any eigenvector corresponding to eigenvalue 1 + l, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 1") + origin = numpy.real(V[:, i[-1]]).squeeze() + origin /= origin[3] + return factor, origin, direction + + +def projection_matrix(point, normal, direction=None, + perspective=None, pseudo=False): + """Return matrix to project onto plane defined by point and normal. + + Using either perspective point, projection direction, or none of both. + + If pseudo is True, perspective projections will preserve relative depth + such that Perspective = dot(Orthogonal, PseudoPerspective). + + >>> P = projection_matrix((0, 0, 0), (1, 0, 0)) + >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) + True + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(3) - 0.5 + >>> P0 = projection_matrix(point, normal) + >>> P1 = projection_matrix(point, normal, direction=direct) + >>> P2 = projection_matrix(point, normal, perspective=persp) + >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) + >>> is_same_transform(P2, numpy.dot(P0, P3)) + True + >>> P = projection_matrix((3, 0, 0), (1, 1, 0), (1, 0, 0)) + >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20.0 + >>> v0[3] = 1.0 + >>> v1 = numpy.dot(P, v0) + >>> numpy.allclose(v1[1], v0[1]) + True + >>> numpy.allclose(v1[0], 3.0-v1[1]) + True + + """ + M = numpy.identity(4) + point = numpy.array(point[:3], dtype=numpy.float64, copy=False) + normal = unit_vector(normal[:3]) + if perspective is not None: + # perspective projection + perspective = numpy.array(perspective[:3], dtype=numpy.float64, + copy=False) + M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) + M[:3, :3] -= numpy.outer(perspective, normal) + if pseudo: + # preserve relative depth + M[:3, :3] -= numpy.outer(normal, normal) + M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) + else: + M[:3, 3] = numpy.dot(point, normal) * perspective + M[3, :3] = -normal + M[3, 3] = numpy.dot(perspective, normal) + elif direction is not None: + # parallel projection + direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) + scale = numpy.dot(direction, normal) + M[:3, :3] -= numpy.outer(direction, normal) / scale + M[:3, 3] = direction * (numpy.dot(point, normal) / scale) + else: + # orthogonal projection + M[:3, :3] -= numpy.outer(normal, normal) + M[:3, 3] = numpy.dot(point, normal) * normal + return M + + +def projection_from_matrix(matrix, pseudo=False): + """Return projection plane and perspective point from projection matrix. + + Return values are same as arguments for projection_matrix function: + point, normal, direction, perspective, and pseudo. + + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(3) - 0.5 + >>> P0 = projection_matrix(point, normal) + >>> result = projection_from_matrix(P0) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, direct) + >>> result = projection_from_matrix(P0) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) + >>> result = projection_from_matrix(P0, pseudo=False) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) + >>> result = projection_from_matrix(P0, pseudo=True) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + l, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not pseudo and len(i): + # point: any eigenvector corresponding to eigenvalue 1 + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + # direction: unit eigenvector corresponding to eigenvalue 0 + l, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 0") + direction = numpy.real(V[:, i[0]]).squeeze() + direction /= vector_norm(direction) + # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 + l, V = numpy.linalg.eig(M33.T) + i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] + if len(i): + # parallel projection + normal = numpy.real(V[:, i[0]]).squeeze() + normal /= vector_norm(normal) + return point, normal, direction, None, False + else: + # orthogonal projection, where normal equals direction vector + return point, direction, None, None, False + else: + # perspective projection + i = numpy.where(abs(numpy.real(l)) > 1e-8)[0] + if not len(i): + raise ValueError( + "no eigenvector not corresponding to eigenvalue 0") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + normal = - M[3, :3] + perspective = M[:3, 3] / numpy.dot(point[:3], normal) + if pseudo: + perspective -= normal + return point, normal, None, perspective, pseudo + + +def clip_matrix(left, right, bottom, top, near, far, perspective=False): + """Return matrix to obtain normalized device coordinates from frustrum. + + The frustrum bounds are axis-aligned along x (left, right), + y (bottom, top) and z (near, far). + + Normalized device coordinates are in range [-1, 1] if coordinates are + inside the frustrum. + + If perspective is True the frustrum is a truncated pyramid with the + perspective point at origin and direction along z axis, otherwise an + orthographic canonical view volume (a box). + + Homogeneous coordinates transformed by the perspective clip matrix + need to be dehomogenized (devided by w coordinate). + + >>> frustrum = numpy.random.rand(6) + >>> frustrum[1] += frustrum[0] + >>> frustrum[3] += frustrum[2] + >>> frustrum[5] += frustrum[4] + >>> M = clip_matrix(*frustrum, perspective=False) + >>> numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0]) + array([-1., -1., -1., 1.]) + >>> numpy.dot(M, [frustrum[1], frustrum[3], frustrum[5], 1.0]) + array([ 1., 1., 1., 1.]) + >>> M = clip_matrix(*frustrum, perspective=True) + >>> v = numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0]) + >>> v / v[3] + array([-1., -1., -1., 1.]) + >>> v = numpy.dot(M, [frustrum[1], frustrum[3], frustrum[4], 1.0]) + >>> v / v[3] + array([ 1., 1., -1., 1.]) + + """ + if left >= right or bottom >= top or near >= far: + raise ValueError("invalid frustrum") + if perspective: + if near <= _EPS: + raise ValueError("invalid frustrum: near <= 0") + t = 2.0 * near + M = ((-t/(right-left), 0.0, (right+left)/(right-left), 0.0), + (0.0, -t/(top-bottom), (top+bottom)/(top-bottom), 0.0), + (0.0, 0.0, -(far+near)/(far-near), t*far/(far-near)), + (0.0, 0.0, -1.0, 0.0)) + else: + M = ((2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)), + (0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)), + (0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)), + (0.0, 0.0, 0.0, 1.0)) + return numpy.array(M, dtype=numpy.float64) + + +def shear_matrix(angle, direction, point, normal): + """Return matrix to shear by angle along direction vector on shear plane. + + The shear plane is defined by a point and normal vector. The direction + vector must be orthogonal to the plane's normal vector. + + A point P is transformed by the shear matrix into P" such that + the vector P-P" is parallel to the direction vector and its extent is + given by the angle of P-P'-P", where P' is the orthogonal projection + of P onto the shear plane. + + >>> angle = (random.random() - 0.5) * 4*math.pi + >>> direct = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.cross(direct, numpy.random.random(3)) + >>> S = shear_matrix(angle, direct, point, normal) + >>> numpy.allclose(1.0, numpy.linalg.det(S)) + True + + """ + normal = unit_vector(normal[:3]) + direction = unit_vector(direction[:3]) + if abs(numpy.dot(normal, direction)) > 1e-6: + raise ValueError("direction and normal vectors are not orthogonal") + angle = math.tan(angle) + M = numpy.identity(4) + M[:3, :3] += angle * numpy.outer(direction, normal) + M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction + return M + + +def shear_from_matrix(matrix): + """Return shear angle, direction and plane from shear matrix. + + >>> angle = (random.random() - 0.5) * 4*math.pi + >>> direct = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.cross(direct, numpy.random.random(3)) + >>> S0 = shear_matrix(angle, direct, point, normal) + >>> angle, direct, point, normal = shear_from_matrix(S0) + >>> S1 = shear_matrix(angle, direct, point, normal) + >>> is_same_transform(S0, S1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + # normal: cross independent eigenvectors corresponding to the eigenvalue 1 + l, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-4)[0] + if len(i) < 2: + raise ValueError("No two linear independent eigenvectors found %s" % l) + V = numpy.real(V[:, i]).squeeze().T + lenorm = -1.0 + for i0, i1 in ((0, 1), (0, 2), (1, 2)): + n = numpy.cross(V[i0], V[i1]) + l = vector_norm(n) + if l > lenorm: + lenorm = l + normal = n + normal /= lenorm + # direction and angle + direction = numpy.dot(M33 - numpy.identity(3), normal) + angle = vector_norm(direction) + direction /= angle + angle = math.atan(angle) + # point: eigenvector corresponding to eigenvalue 1 + l, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 1") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + return angle, direction, point, normal + + +def decompose_matrix(matrix): + """Return sequence of transformations from transformation matrix. + + matrix : array_like + Non-degenerative homogeneous transformation matrix + + Return tuple of: + scale : vector of 3 scaling factors + shear : list of shear factors for x-y, x-z, y-z axes + angles : list of Euler angles about static x, y, z axes + translate : translation vector along x, y, z axes + perspective : perspective partition of matrix + + Raise ValueError if matrix is of wrong type or degenerative. + + >>> T0 = translation_matrix((1, 2, 3)) + >>> scale, shear, angles, trans, persp = decompose_matrix(T0) + >>> T1 = translation_matrix(trans) + >>> numpy.allclose(T0, T1) + True + >>> S = scale_matrix(0.123) + >>> scale, shear, angles, trans, persp = decompose_matrix(S) + >>> scale[0] + 0.123 + >>> R0 = euler_matrix(1, 2, 3) + >>> scale, shear, angles, trans, persp = decompose_matrix(R0) + >>> R1 = euler_matrix(*angles) + >>> numpy.allclose(R0, R1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=True).T + if abs(M[3, 3]) < _EPS: + raise ValueError("M[3, 3] is zero") + M /= M[3, 3] + P = M.copy() + P[:, 3] = 0, 0, 0, 1 + if not numpy.linalg.det(P): + raise ValueError("Matrix is singular") + + scale = numpy.zeros((3, ), dtype=numpy.float64) + shear = [0, 0, 0] + angles = [0, 0, 0] + + if any(abs(M[:3, 3]) > _EPS): + perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) + M[:, 3] = 0, 0, 0, 1 + else: + perspective = numpy.array((0, 0, 0, 1), dtype=numpy.float64) + + translate = M[3, :3].copy() + M[3, :3] = 0 + + row = M[:3, :3].copy() + scale[0] = vector_norm(row[0]) + row[0] /= scale[0] + shear[0] = numpy.dot(row[0], row[1]) + row[1] -= row[0] * shear[0] + scale[1] = vector_norm(row[1]) + row[1] /= scale[1] + shear[0] /= scale[1] + shear[1] = numpy.dot(row[0], row[2]) + row[2] -= row[0] * shear[1] + shear[2] = numpy.dot(row[1], row[2]) + row[2] -= row[1] * shear[2] + scale[2] = vector_norm(row[2]) + row[2] /= scale[2] + shear[1:] /= scale[2] + + if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: + scale *= -1 + row *= -1 + + angles[1] = math.asin(-row[0, 2]) + if math.cos(angles[1]): + angles[0] = math.atan2(row[1, 2], row[2, 2]) + angles[2] = math.atan2(row[0, 1], row[0, 0]) + else: + #angles[0] = math.atan2(row[1, 0], row[1, 1]) + angles[0] = math.atan2(-row[2, 1], row[1, 1]) + angles[2] = 0.0 + + return scale, shear, angles, translate, perspective + + +def compose_matrix(scale=None, shear=None, angles=None, translate=None, + perspective=None): + """Return transformation matrix from sequence of transformations. + + This is the inverse of the decompose_matrix function. + + Sequence of transformations: + scale : vector of 3 scaling factors + shear : list of shear factors for x-y, x-z, y-z axes + angles : list of Euler angles about static x, y, z axes + translate : translation vector along x, y, z axes + perspective : perspective partition of matrix + + >>> scale = numpy.random.random(3) - 0.5 + >>> shear = numpy.random.random(3) - 0.5 + >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) + >>> trans = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(4) - 0.5 + >>> M0 = compose_matrix(scale, shear, angles, trans, persp) + >>> result = decompose_matrix(M0) + >>> M1 = compose_matrix(*result) + >>> is_same_transform(M0, M1) + True + + """ + M = numpy.identity(4) + if perspective is not None: + P = numpy.identity(4) + P[3, :] = perspective[:4] + M = numpy.dot(M, P) + if translate is not None: + T = numpy.identity(4) + T[:3, 3] = translate[:3] + M = numpy.dot(M, T) + if angles is not None: + R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') + M = numpy.dot(M, R) + if shear is not None: + Z = numpy.identity(4) + Z[1, 2] = shear[2] + Z[0, 2] = shear[1] + Z[0, 1] = shear[0] + M = numpy.dot(M, Z) + if scale is not None: + S = numpy.identity(4) + S[0, 0] = scale[0] + S[1, 1] = scale[1] + S[2, 2] = scale[2] + M = numpy.dot(M, S) + M /= M[3, 3] + return M + + +def orthogonalization_matrix(lengths, angles): + """Return orthogonalization matrix for crystallographic cell coordinates. + + Angles are expected in degrees. + + The de-orthogonalization matrix is the inverse. + + >>> O = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.)) + >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) + True + >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) + >>> numpy.allclose(numpy.sum(O), 43.063229) + True + + """ + a, b, c = lengths + angles = numpy.radians(angles) + sina, sinb, _ = numpy.sin(angles) + cosa, cosb, cosg = numpy.cos(angles) + co = (cosa * cosb - cosg) / (sina * sinb) + return numpy.array(( + ( a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0), + (-a*sinb*co, b*sina, 0.0, 0.0), + ( a*cosb, b*cosa, c, 0.0), + ( 0.0, 0.0, 0.0, 1.0)), + dtype=numpy.float64) + + +def superimposition_matrix(v0, v1, scaling=False, usesvd=True): + """Return matrix to transform given vector set into second vector set. + + v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 vectors. + + If usesvd is True, the weighted sum of squared deviations (RMSD) is + minimized according to the algorithm by W. Kabsch [8]. Otherwise the + quaternion based algorithm by B. Horn [9] is used (slower when using + this Python implementation). + + The returned matrix performs rotation, translation and uniform scaling + (if specified). + + >>> v0 = numpy.random.rand(3, 10) + >>> M = superimposition_matrix(v0, v0) + >>> numpy.allclose(M, numpy.identity(4)) + True + >>> R = random_rotation_matrix(numpy.random.random(3)) + >>> v0 = ((1,0,0), (0,1,0), (0,0,1), (1,1,1)) + >>> v1 = numpy.dot(R, v0) + >>> M = superimposition_matrix(v0, v1) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20.0 + >>> v0[3] = 1.0 + >>> v1 = numpy.dot(R, v0) + >>> M = superimposition_matrix(v0, v1) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> S = scale_matrix(random.random()) + >>> T = translation_matrix(numpy.random.random(3)-0.5) + >>> M = concatenate_matrices(T, R, S) + >>> v1 = numpy.dot(M, v0) + >>> v0[:3] += numpy.random.normal(0.0, 1e-9, 300).reshape(3, -1) + >>> M = superimposition_matrix(v0, v1, scaling=True) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> v = numpy.empty((4, 100, 3), dtype=numpy.float64) + >>> v[:, :, 0] = v0 + >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) + >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) + True + + """ + v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] + v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] + + if v0.shape != v1.shape or v0.shape[1] < 3: + raise ValueError("Vector sets are of wrong shape or type.") + + # move centroids to origin + t0 = numpy.mean(v0, axis=1) + t1 = numpy.mean(v1, axis=1) + v0 = v0 - t0.reshape(3, 1) + v1 = v1 - t1.reshape(3, 1) + + if usesvd: + # Singular Value Decomposition of covariance matrix + u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) + # rotation matrix from SVD orthonormal bases + R = numpy.dot(u, vh) + if numpy.linalg.det(R) < 0.0: + # R does not constitute right handed system + R -= numpy.outer(u[:, 2], vh[2, :]*2.0) + s[-1] *= -1.0 + # homogeneous transformation matrix + M = numpy.identity(4) + M[:3, :3] = R + else: + # compute symmetric matrix N + xx, yy, zz = numpy.sum(v0 * v1, axis=1) + xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) + xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) + N = ((xx+yy+zz, yz-zy, zx-xz, xy-yx), + (yz-zy, xx-yy-zz, xy+yx, zx+xz), + (zx-xz, xy+yx, -xx+yy-zz, yz+zy), + (xy-yx, zx+xz, yz+zy, -xx-yy+zz)) + # quaternion: eigenvector corresponding to most positive eigenvalue + l, V = numpy.linalg.eig(N) + q = V[:, numpy.argmax(l)] + q /= vector_norm(q) # unit quaternion + q = numpy.roll(q, -1) # move w component to end + # homogeneous transformation matrix + M = quaternion_matrix(q) + + # scale: ratio of rms deviations from centroid + if scaling: + v0 *= v0 + v1 *= v1 + M[:3, :3] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) + + # translation + M[:3, 3] = t1 + T = numpy.identity(4) + T[:3, 3] = -t0 + M = numpy.dot(M, T) + return M + + +def euler_matrix(ai, aj, ak, axes='sxyz'): + """Return homogeneous rotation matrix from Euler angles and axis sequence. + + ai, aj, ak : Euler's roll, pitch and yaw angles + axes : One of 24 axis sequences as string or encoded tuple + + >>> R = euler_matrix(1, 2, 3, 'syxz') + >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) + True + >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) + >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) + True + >>> ai, aj, ak = (4.0*math.pi) * (numpy.random.random(3) - 0.5) + >>> for axes in _AXES2TUPLE.keys(): + ... R = euler_matrix(ai, aj, ak, axes) + >>> for axes in _TUPLE2AXES.keys(): + ... R = euler_matrix(ai, aj, ak, axes) + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] + except (AttributeError, KeyError): + _ = _TUPLE2AXES[axes] + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i+parity] + k = _NEXT_AXIS[i-parity+1] + + if frame: + ai, ak = ak, ai + if parity: + ai, aj, ak = -ai, -aj, -ak + + si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) + ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) + cc, cs = ci*ck, ci*sk + sc, ss = si*ck, si*sk + + M = numpy.identity(4) + if repetition: + M[i, i] = cj + M[i, j] = sj*si + M[i, k] = sj*ci + M[j, i] = sj*sk + M[j, j] = -cj*ss+cc + M[j, k] = -cj*cs-sc + M[k, i] = -sj*ck + M[k, j] = cj*sc+cs + M[k, k] = cj*cc-ss + else: + M[i, i] = cj*ck + M[i, j] = sj*sc-cs + M[i, k] = sj*cc+ss + M[j, i] = cj*sk + M[j, j] = sj*ss+cc + M[j, k] = sj*cs-sc + M[k, i] = -sj + M[k, j] = cj*si + M[k, k] = cj*ci + return M + + +def euler_from_matrix(matrix, axes='sxyz'): + """Return Euler angles from rotation matrix for specified axis sequence. + + axes : One of 24 axis sequences as string or encoded tuple + + Note that many Euler angle triplets can describe one matrix. + + >>> R0 = euler_matrix(1, 2, 3, 'syxz') + >>> al, be, ga = euler_from_matrix(R0, 'syxz') + >>> R1 = euler_matrix(al, be, ga, 'syxz') + >>> numpy.allclose(R0, R1) + True + >>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5) + >>> for axes in _AXES2TUPLE.keys(): + ... R0 = euler_matrix(axes=axes, *angles) + ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) + ... if not numpy.allclose(R0, R1): print axes, "failed" + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] + except (AttributeError, KeyError): + _ = _TUPLE2AXES[axes] + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i+parity] + k = _NEXT_AXIS[i-parity+1] + + M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] + if repetition: + sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) + if sy > _EPS: + ax = math.atan2( M[i, j], M[i, k]) + ay = math.atan2( sy, M[i, i]) + az = math.atan2( M[j, i], -M[k, i]) + else: + ax = math.atan2(-M[j, k], M[j, j]) + ay = math.atan2( sy, M[i, i]) + az = 0.0 + else: + cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) + if cy > _EPS: + ax = math.atan2( M[k, j], M[k, k]) + ay = math.atan2(-M[k, i], cy) + az = math.atan2( M[j, i], M[i, i]) + else: + ax = math.atan2(-M[j, k], M[j, j]) + ay = math.atan2(-M[k, i], cy) + az = 0.0 + + if parity: + ax, ay, az = -ax, -ay, -az + if frame: + ax, az = az, ax + return ax, ay, az + + +def euler_from_quaternion(quaternion, axes='sxyz'): + """Return Euler angles from quaternion for specified axis sequence. + + >>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947]) + >>> numpy.allclose(angles, [0.123, 0, 0]) + True + + """ + return euler_from_matrix(quaternion_matrix(quaternion), axes) + + +def quaternion_from_euler(ai, aj, ak, axes='sxyz'): + """Return quaternion from Euler angles and axis sequence. + + ai, aj, ak : Euler's roll, pitch and yaw angles + axes : One of 24 axis sequences as string or encoded tuple + + >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') + >>> numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953]) + True + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] + except (AttributeError, KeyError): + _ = _TUPLE2AXES[axes] + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i+parity] + k = _NEXT_AXIS[i-parity+1] + + if frame: + ai, ak = ak, ai + if parity: + aj = -aj + + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + quaternion = numpy.empty((4, ), dtype=numpy.float64) + if repetition: + quaternion[i] = cj*(cs + sc) + quaternion[j] = sj*(cc + ss) + quaternion[k] = sj*(cs - sc) + quaternion[3] = cj*(cc - ss) + else: + quaternion[i] = cj*sc - sj*cs + quaternion[j] = cj*ss + sj*cc + quaternion[k] = cj*cs - sj*sc + quaternion[3] = cj*cc + sj*ss + if parity: + quaternion[j] *= -1 + + return quaternion + + +def quaternion_about_axis(angle, axis): + """Return quaternion for rotation about axis. + + >>> q = quaternion_about_axis(0.123, (1, 0, 0)) + >>> numpy.allclose(q, [0.06146124, 0, 0, 0.99810947]) + True + + """ + quaternion = numpy.zeros((4, ), dtype=numpy.float64) + quaternion[:3] = axis[:3] + qlen = vector_norm(quaternion) + if qlen > _EPS: + quaternion *= math.sin(angle/2.0) / qlen + quaternion[3] = math.cos(angle/2.0) + return quaternion + + +def quaternion_matrix(quaternion): + """Return homogeneous rotation matrix from quaternion. + + >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947]) + >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0))) + True + + """ + q = numpy.array(quaternion[:4], dtype=numpy.float64, copy=True) + nq = numpy.dot(q, q) + if nq < _EPS: + return numpy.identity(4) + q *= math.sqrt(2.0 / nq) + q = numpy.outer(q, q) + return numpy.array(( + (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], 0.0), + ( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], 0.0), + ( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0), + ( 0.0, 0.0, 0.0, 1.0) + ), dtype=numpy.float64) + + +def quaternion_from_matrix(matrix): + """Return quaternion from rotation matrix. + + >>> R = rotation_matrix(0.123, (1, 2, 3)) + >>> q = quaternion_from_matrix(R) + >>> numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095]) + True + + """ + q = numpy.empty((4, ), dtype=numpy.float64) + M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] + t = numpy.trace(M) + if t > M[3, 3]: + q[3] = t + q[2] = M[1, 0] - M[0, 1] + q[1] = M[0, 2] - M[2, 0] + q[0] = M[2, 1] - M[1, 2] + else: + i, j, k = 0, 1, 2 + if M[1, 1] > M[0, 0]: + i, j, k = 1, 2, 0 + if M[2, 2] > M[i, i]: + i, j, k = 2, 0, 1 + t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] + q[i] = t + q[j] = M[i, j] + M[j, i] + q[k] = M[k, i] + M[i, k] + q[3] = M[k, j] - M[j, k] + q *= 0.5 / math.sqrt(t * M[3, 3]) + return q + + +def quaternion_multiply(quaternion1, quaternion0): + """Return multiplication of two quaternions. + + >>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8]) + >>> numpy.allclose(q, [-44, -14, 48, 28]) + True + + """ + x0, y0, z0, w0 = quaternion0 + x1, y1, z1, w1 = quaternion1 + return numpy.array(( + x1*w0 + y1*z0 - z1*y0 + w1*x0, + -x1*z0 + y1*w0 + z1*x0 + w1*y0, + x1*y0 - y1*x0 + z1*w0 + w1*z0, + -x1*x0 - y1*y0 - z1*z0 + w1*w0), dtype=numpy.float64) + + +def quaternion_conjugate(quaternion): + """Return conjugate of quaternion. + + >>> q0 = random_quaternion() + >>> q1 = quaternion_conjugate(q0) + >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3]) + True + + """ + return numpy.array((-quaternion[0], -quaternion[1], + -quaternion[2], quaternion[3]), dtype=numpy.float64) + + +def quaternion_inverse(quaternion): + """Return inverse of quaternion. + + >>> q0 = random_quaternion() + >>> q1 = quaternion_inverse(q0) + >>> numpy.allclose(quaternion_multiply(q0, q1), [0, 0, 0, 1]) + True + + """ + return quaternion_conjugate(quaternion) / numpy.dot(quaternion, quaternion) + + +def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): + """Return spherical linear interpolation between two quaternions. + + >>> q0 = random_quaternion() + >>> q1 = random_quaternion() + >>> q = quaternion_slerp(q0, q1, 0.0) + >>> numpy.allclose(q, q0) + True + >>> q = quaternion_slerp(q0, q1, 1.0, 1) + >>> numpy.allclose(q, q1) + True + >>> q = quaternion_slerp(q0, q1, 0.5) + >>> angle = math.acos(numpy.dot(q0, q)) + >>> numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or \ + numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle) + True + + """ + q0 = unit_vector(quat0[:4]) + q1 = unit_vector(quat1[:4]) + if fraction == 0.0: + return q0 + elif fraction == 1.0: + return q1 + d = numpy.dot(q0, q1) + if abs(abs(d) - 1.0) < _EPS: + return q0 + if shortestpath and d < 0.0: + # invert rotation + d = -d + q1 *= -1.0 + angle = math.acos(d) + spin * math.pi + if abs(angle) < _EPS: + return q0 + isin = 1.0 / math.sin(angle) + q0 *= math.sin((1.0 - fraction) * angle) * isin + q1 *= math.sin(fraction * angle) * isin + q0 += q1 + return q0 + + +def random_quaternion(rand=None): + """Return uniform random unit quaternion. + + rand: array like or None + Three independent random variables that are uniformly distributed + between 0 and 1. + + >>> q = random_quaternion() + >>> numpy.allclose(1.0, vector_norm(q)) + True + >>> q = random_quaternion(numpy.random.random(3)) + >>> q.shape + (4,) + + """ + if rand is None: + rand = numpy.random.rand(3) + else: + assert len(rand) == 3 + r1 = numpy.sqrt(1.0 - rand[0]) + r2 = numpy.sqrt(rand[0]) + pi2 = math.pi * 2.0 + t1 = pi2 * rand[1] + t2 = pi2 * rand[2] + return numpy.array((numpy.sin(t1)*r1, + numpy.cos(t1)*r1, + numpy.sin(t2)*r2, + numpy.cos(t2)*r2), dtype=numpy.float64) + + +def random_rotation_matrix(rand=None): + """Return uniform random rotation matrix. + + rnd: array like + Three independent random variables that are uniformly distributed + between 0 and 1 for each returned quaternion. + + >>> R = random_rotation_matrix() + >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) + True + + """ + return quaternion_matrix(random_quaternion(rand)) + + +class Arcball(object): + """Virtual Trackball Control. + + >>> ball = Arcball() + >>> ball = Arcball(initial=numpy.identity(4)) + >>> ball.place([320, 320], 320) + >>> ball.down([500, 250]) + >>> ball.drag([475, 275]) + >>> R = ball.matrix() + >>> numpy.allclose(numpy.sum(R), 3.90583455) + True + >>> ball = Arcball(initial=[0, 0, 0, 1]) + >>> ball.place([320, 320], 320) + >>> ball.setaxes([1,1,0], [-1, 1, 0]) + >>> ball.setconstrain(True) + >>> ball.down([400, 200]) + >>> ball.drag([200, 400]) + >>> R = ball.matrix() + >>> numpy.allclose(numpy.sum(R), 0.2055924) + True + >>> ball.next() + + """ + + def __init__(self, initial=None): + """Initialize virtual trackball control. + + initial : quaternion or rotation matrix + + """ + self._axis = None + self._axes = None + self._radius = 1.0 + self._center = [0.0, 0.0] + self._vdown = numpy.array([0, 0, 1], dtype=numpy.float64) + self._constrain = False + + if initial is None: + self._qdown = numpy.array([0, 0, 0, 1], dtype=numpy.float64) + else: + initial = numpy.array(initial, dtype=numpy.float64) + if initial.shape == (4, 4): + self._qdown = quaternion_from_matrix(initial) + elif initial.shape == (4, ): + initial /= vector_norm(initial) + self._qdown = initial + else: + raise ValueError("initial not a quaternion or matrix.") + + self._qnow = self._qpre = self._qdown + + def place(self, center, radius): + """Place Arcball, e.g. when window size changes. + + center : sequence[2] + Window coordinates of trackball center. + radius : float + Radius of trackball in window coordinates. + + """ + self._radius = float(radius) + self._center[0] = center[0] + self._center[1] = center[1] + + def setaxes(self, *axes): + """Set axes to constrain rotations.""" + if axes is None: + self._axes = None + else: + self._axes = [unit_vector(axis) for axis in axes] + + def setconstrain(self, constrain): + """Set state of constrain to axis mode.""" + self._constrain = constrain == True + + def getconstrain(self): + """Return state of constrain to axis mode.""" + return self._constrain + + def down(self, point): + """Set initial cursor window coordinates and pick constrain-axis.""" + self._vdown = arcball_map_to_sphere(point, self._center, self._radius) + self._qdown = self._qpre = self._qnow + + if self._constrain and self._axes is not None: + self._axis = arcball_nearest_axis(self._vdown, self._axes) + self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) + else: + self._axis = None + + def drag(self, point): + """Update current cursor window coordinates.""" + vnow = arcball_map_to_sphere(point, self._center, self._radius) + + if self._axis is not None: + vnow = arcball_constrain_to_axis(vnow, self._axis) + + self._qpre = self._qnow + + t = numpy.cross(self._vdown, vnow) + if numpy.dot(t, t) < _EPS: + self._qnow = self._qdown + else: + q = [t[0], t[1], t[2], numpy.dot(self._vdown, vnow)] + self._qnow = quaternion_multiply(q, self._qdown) + + def next(self, acceleration=0.0): + """Continue rotation in direction of last drag.""" + q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) + self._qpre, self._qnow = self._qnow, q + + def matrix(self): + """Return homogeneous rotation matrix.""" + return quaternion_matrix(self._qnow) + + +def arcball_map_to_sphere(point, center, radius): + """Return unit sphere coordinates from window coordinates.""" + v = numpy.array(((point[0] - center[0]) / radius, + (center[1] - point[1]) / radius, + 0.0), dtype=numpy.float64) + n = v[0]*v[0] + v[1]*v[1] + if n > 1.0: + v /= math.sqrt(n) # position outside of sphere + else: + v[2] = math.sqrt(1.0 - n) + return v + + +def arcball_constrain_to_axis(point, axis): + """Return sphere point perpendicular to axis.""" + v = numpy.array(point, dtype=numpy.float64, copy=True) + a = numpy.array(axis, dtype=numpy.float64, copy=True) + v -= a * numpy.dot(a, v) # on plane + n = vector_norm(v) + if n > _EPS: + if v[2] < 0.0: + v *= -1.0 + v /= n + return v + if a[2] == 1.0: + return numpy.array([1, 0, 0], dtype=numpy.float64) + return unit_vector([-a[1], a[0], 0]) + + +def arcball_nearest_axis(point, axes): + """Return axis, which arc is nearest to point.""" + point = numpy.array(point, dtype=numpy.float64, copy=False) + nearest = None + mx = -1.0 + for axis in axes: + t = numpy.dot(arcball_constrain_to_axis(point, axis), point) + if t > mx: + nearest = axis + mx = t + return nearest + + +# epsilon for testing whether a number is close to zero +_EPS = numpy.finfo(float).eps * 4.0 + +# axis sequences for Euler angles +_NEXT_AXIS = [1, 2, 0, 1] + +# map axes strings to/from tuples of inner axis, parity, repetition, frame +_AXES2TUPLE = { + 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), + 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), + 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), + 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), + 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), + 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), + 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), + 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} + +_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) + +# helper functions + +def vector_norm(data, axis=None, out=None): + """Return length, i.e. eucledian norm, of ndarray along axis. + + >>> v = numpy.random.random(3) + >>> n = vector_norm(v) + >>> numpy.allclose(n, numpy.linalg.norm(v)) + True + >>> v = numpy.random.rand(6, 5, 3) + >>> n = vector_norm(v, axis=-1) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) + True + >>> n = vector_norm(v, axis=1) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) + True + >>> v = numpy.random.rand(5, 4, 3) + >>> n = numpy.empty((5, 3), dtype=numpy.float64) + >>> vector_norm(v, axis=1, out=n) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) + True + >>> vector_norm([]) + 0.0 + >>> vector_norm([1.0]) + 1.0 + + """ + data = numpy.array(data, dtype=numpy.float64, copy=True) + if out is None: + if data.ndim == 1: + return math.sqrt(numpy.dot(data, data)) + data *= data + out = numpy.atleast_1d(numpy.sum(data, axis=axis)) + numpy.sqrt(out, out) + return out + else: + data *= data + numpy.sum(data, axis=axis, out=out) + numpy.sqrt(out, out) + + +def unit_vector(data, axis=None, out=None): + """Return ndarray normalized by length, i.e. eucledian norm, along axis. + + >>> v0 = numpy.random.random(3) + >>> v1 = unit_vector(v0) + >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) + True + >>> v0 = numpy.random.rand(5, 4, 3) + >>> v1 = unit_vector(v0, axis=-1) + >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) + >>> numpy.allclose(v1, v2) + True + >>> v1 = unit_vector(v0, axis=1) + >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) + >>> numpy.allclose(v1, v2) + True + >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float64) + >>> unit_vector(v0, axis=1, out=v1) + >>> numpy.allclose(v1, v2) + True + >>> list(unit_vector([])) + [] + >>> list(unit_vector([1.0])) + [1.0] + + """ + if out is None: + data = numpy.array(data, dtype=numpy.float64, copy=True) + if data.ndim == 1: + data /= math.sqrt(numpy.dot(data, data)) + return data + else: + if out is not data: + out[:] = numpy.array(data, copy=False) + data = out + length = numpy.atleast_1d(numpy.sum(data*data, axis)) + numpy.sqrt(length, length) + if axis is not None: + length = numpy.expand_dims(length, axis) + data /= length + if out is None: + return data + + +def random_vector(size): + """Return array of random doubles in the half-open interval [0.0, 1.0). + + >>> v = random_vector(10000) + >>> numpy.all(v >= 0.0) and numpy.all(v < 1.0) + True + >>> v0 = random_vector(10) + >>> v1 = random_vector(10) + >>> numpy.any(v0 == v1) + False + + """ + return numpy.random.random(size) + + +def inverse_matrix(matrix): + """Return inverse of square transformation matrix. + + >>> M0 = random_rotation_matrix() + >>> M1 = inverse_matrix(M0.T) + >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) + True + >>> for size in range(1, 7): + ... M0 = numpy.random.rand(size, size) + ... M1 = inverse_matrix(M0) + ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print size + + """ + return numpy.linalg.inv(matrix) + + +def concatenate_matrices(*matrices): + """Return concatenation of series of transformation matrices. + + >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 + >>> numpy.allclose(M, concatenate_matrices(M)) + True + >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) + True + + """ + M = numpy.identity(4) + for i in matrices: + M = numpy.dot(M, i) + return M + + +def is_same_transform(matrix0, matrix1): + """Return True if two matrices perform same transformation. + + >>> is_same_transform(numpy.identity(4), numpy.identity(4)) + True + >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) + False + + """ + matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) + matrix0 /= matrix0[3, 3] + matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) + matrix1 /= matrix1[3, 3] + return numpy.allclose(matrix0, matrix1) + + +def _import_module(module_name, warn=True, prefix='_py_', ignore='_'): + """Try import all public attributes from module into global namespace. + + Existing attributes with name clashes are renamed with prefix. + Attributes starting with underscore are ignored by default. + + Return True on successful import. + + """ + try: + module = __import__(module_name) + except ImportError: + if warn: + warnings.warn("Failed to import module " + module_name) + else: + for attr in dir(module): + if ignore and attr.startswith(ignore): + continue + if prefix: + if attr in globals(): + globals()[prefix + attr] = globals()[attr] + elif warn: + warnings.warn("No Python implementation of " + attr) + globals()[attr] = getattr(module, attr) + return True diff --git a/src/rpg_vikit/vikit_ros/CMakeLists.txt b/src/rpg_vikit/vikit_ros/CMakeLists.txt new file mode 100644 index 0000000..cd903fd --- /dev/null +++ b/src/rpg_vikit/vikit_ros/CMakeLists.txt @@ -0,0 +1,118 @@ +cmake_minimum_required(VERSION 3.5) +project(vikit_ros) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +add_compile_options(-std=c++17) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3") +add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions") +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -pthread -fexceptions") +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +message("Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}") +if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)") + include(ProcessorCount) + ProcessorCount(N) + message("Processor number: ${N}") + if(N GREATER 4) + add_definitions(-DMP_EN) + add_definitions(-DMP_PROC_NUM=3) + message("Cores for MP: 3") + elseif(N GREATER 3) + add_definitions(-DMP_EN) + add_definitions(-DMP_PROC_NUM=2) + message("Cores for MP: 2") + else() + add_definitions(-DMP_PROC_NUM=1) + endif() +else() + add_definitions(-DMP_PROC_NUM=1) +endif() + +find_package(OpenMP QUIET) +if(OpenMP_FOUND) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +endif() + +# Find packages +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(vikit_common REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(OpenCV REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Sophus REQUIRED) +# Support modern cmake Sophus target (ros-humble-sophus) which doesn't set Sophus_INCLUDE_DIRS +if(TARGET Sophus::Sophus AND NOT Sophus_INCLUDE_DIRS) + get_target_property(Sophus_INCLUDE_DIRS Sophus::Sophus INTERFACE_INCLUDE_DIRECTORIES) +endif() +find_package(tf2_geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +set(dependencies + rclcpp + visualization_msgs + tf2_ros + tf2 + Eigen3 + tf2_geometry_msgs + vikit_common +) + +ament_export_dependencies(rosidl_default_runtime) +ament_export_dependencies(${dependencies}) + +# Link libraries +list(APPEND SOURCEFILES + src/output_helper.cpp + src/camera_loader.cpp +) + +add_library(${PROJECT_NAME} SHARED ${SOURCEFILES}) +# Modify target_include_directories to include the project directory directly +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${CMAKE_CURRENT_SOURCE_DIR} # Add this line to include the project root directory + ${Sophus_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +target_link_libraries(${PROJECT_NAME} + ${cpp_typesupport_target} +) + +ament_target_dependencies(${PROJECT_NAME} ${dependencies}) + +# Install headers directly to the include directory without the project name prefix +install(DIRECTORY include/ + DESTINATION include + COMPONENT ${PROJECT_NAME} +) + +# Install library +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +# Export the include directory for the project +set_target_properties(${PROJECT_NAME} PROPERTIES + PUBLIC_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/include" +) + +# Ensure the include directory is correctly set for downstream packages +ament_export_include_directories(include) + +ament_package() \ No newline at end of file diff --git a/src/rpg_vikit/vikit_ros/include/vikit/camera_loader.h b/src/rpg_vikit/vikit_ros/include/vikit/camera_loader.h new file mode 100644 index 0000000..dafe81a --- /dev/null +++ b/src/rpg_vikit/vikit_ros/include/vikit/camera_loader.h @@ -0,0 +1,32 @@ +/* + * camera_loader.h + * + * Created on: Feb 11, 2014 + * Author: cforster + * Update on: Feb 01, 2025 + * Author: StrangeFly + */ + +#ifndef VIKIT_CAMERA_LOADER_H_ +#define VIKIT_CAMERA_LOADER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { +namespace camera_loader { + +/// Load from ROS Namespace +bool loadFromRosNs(const rclcpp::Node::SharedPtr & nh, const std::string& ns, vk::AbstractCamera*& cam); +bool loadFromRosNs(const rclcpp::Node::SharedPtr & nh, const std::string& ns, std::vector& cam_list); + +} // namespace camera_loader +} // namespace vk + +#endif // VIKIT_CAMERA_LOADER_H_ diff --git a/src/rpg_vikit/vikit_ros/include/vikit/output_helper.h b/src/rpg_vikit/vikit_ros/include/vikit/output_helper.h new file mode 100644 index 0000000..582c142 --- /dev/null +++ b/src/rpg_vikit/vikit_ros/include/vikit/output_helper.h @@ -0,0 +1,102 @@ +/* + * output_helper.h + * + * Created on: Jan 20, 2013 + * Author: cforster + * Update on: Feb 01, 2025 + * Author: StrangeFly + */ + +#ifndef VIKIT_OUTPUT_HELPER_H_ +#define VIKIT_OUTPUT_HELPER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { +namespace output_helper { + +using namespace std; +using namespace Eigen; + +void +publishTfTransform (const Sophus::SE3& T, const rclcpp::Time& stamp, + const string& frame_id, const string& child_frame_id, + tf2_ros::TransformBroadcaster& br); + +void +publishPointMarker (const rclcpp::Publisher::SharedPtr pub, + const Vector3d& pos, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + rclcpp::Duration lifetime = rclcpp::Duration(0,0)); + +void +publishLineMarker (const rclcpp::Publisher::SharedPtr pub, + const Vector3d& start, + const Vector3d& end, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + rclcpp::Duration lifetime = rclcpp::Duration(0,0)); + +void +publishArrowMarker (const rclcpp::Publisher::SharedPtr pub, + const Vector3d& pos, + const Vector3d& dir, + double scale, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color); + +void +publishHexacopterMarker (const rclcpp::Publisher::SharedPtr pub, + const string& frame_id, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color); + +void +publishCameraMarker(const rclcpp::Publisher::SharedPtr pub, + const string& frame_id, + const string& ns, + const rclcpp::Time& timestamp, + int id, + double marker_scale, + const Vector3d& color); +void +publishFrameMarker (const rclcpp::Publisher::SharedPtr pub, + const Matrix3d& rot, + const Vector3d& pos, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + rclcpp::Duration lifetime = rclcpp::Duration(0,0)); + + +} // namespace output_helper +} // namespace vk + + +#endif /* VIKIT_OUTPUT_HELPER_H_ */ diff --git a/src/rpg_vikit/vikit_ros/include/vikit/params_helper.h b/src/rpg_vikit/vikit_ros/include/vikit/params_helper.h new file mode 100644 index 0000000..9e12931 --- /dev/null +++ b/src/rpg_vikit/vikit_ros/include/vikit/params_helper.h @@ -0,0 +1,148 @@ +/* + * ros_params_helper.h + * + * Created on: Feb 22, 2013 + * Author: cforster + * Update on: Feb 01, 2025 + * Author: StrangeFly + * + * from libpointmatcher_ros + */ + +#ifndef ROS_PARAMS_HELPER_H_ +#define ROS_PARAMS_HELPER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +template +T getParam(const std::string& node, const std::string& name, const T& defaultValue) { + // Keep popen as a absolute fallback if no Node handle is available + // But this is very slow! + try { + std::string full_node = node; + if (!full_node.empty() && full_node[0] != '/') full_node = "/" + full_node; + + std::string command = "ros2 param get " + full_node + " " + name + " 2>/dev/null"; + std::unique_ptr pipe(popen(command.c_str(), "r"), pclose); + if (!pipe) { + return defaultValue; + } + std::ostringstream resultStream; + char buffer[128]; + while (fgets(buffer, sizeof(buffer), pipe.get()) != nullptr) { + resultStream << buffer; + } + std::string result = resultStream.str(); + + size_t pos = result.find(": "); + if (pos != std::string::npos) { + std::string valueStr = result.substr(pos + 2); + if (!valueStr.empty() && valueStr.back() == '\n') valueStr.pop_back(); + + if (!valueStr.empty()) { + std::stringstream ss(valueStr); + T value; + if (ss >> value) { + return value; + } + } + } + return defaultValue; + } catch (...) { + return defaultValue; + } +} + +template +T getParam(const std::string& node, const std::string& name) { + // override function could has no default value + if constexpr (std::is_same_v) { + return getParam(node, name, ""); // if std::string, default value is "" + } else if constexpr (std::is_integral_v) { + return getParam(node, name, 0); // if int. defalt 0 + } else if constexpr (std::is_floating_point_v) { + return getParam(node, name, 0.0); // if float，default 0.0 + } else { + throw std::runtime_error("Unsupported type for getParam without default value."); + } +} + +inline +bool hasParam(const rclcpp::Node::SharedPtr &nh, const std::string& name) +{ + return nh->has_parameter(name); +} + +template +T getParam(const rclcpp::Node::SharedPtr &nh, const std::string& name, const T& defaultValue) +{ + T v; + if(nh->get_parameter(name, v)) + { + return v; + } + + // If not found locally, it might be a prefix for a remote node or a local param not yet declared + if (!nh->has_parameter(name)) { + nh->declare_parameter(name, defaultValue); + if (nh->get_parameter(name, v)) return v; + } + + return defaultValue; +} + +// New function for cross-node parameter access +template +T getRemoteParam(const rclcpp::Node::SharedPtr &nh, const std::string& remote_node_name, const std::string& param_name, const T& defaultValue) +{ + // Try local first just in case + std::string full_name = remote_node_name + "." + param_name; // common convention + T v; + if (nh->get_parameter(full_name, v)) return v; + if (nh->get_parameter(param_name, v)) return v; + + // Use SyncParametersClient for high performance cross-node access + try { + auto parameters_client = std::make_shared(nh, remote_node_name); + // Wait briefly for the service to be available + if (parameters_client->wait_for_service(std::chrono::milliseconds(100))) { + auto values = parameters_client->get_parameters({param_name}); + if (!values.empty() && values[0].get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { + return values[0].get_value(); + } + } + } catch (...) { + // Fallback to popen if client fails + return getParam(remote_node_name, param_name, defaultValue); + } + + return defaultValue; +} + +template +T getParam(const rclcpp::Node::SharedPtr &nh, const std::string& name) +{ + T v; + if (nh->get_parameter(name, v)) { + RCLCPP_INFO_STREAM(nh->get_logger(), "Found parameter: " << name << ", value: " << v); + return v; + } + + // If not found, try to declare it (this might be useful if the parameter is expected to be there) + // or just return a default constructed T. + RCLCPP_ERROR_STREAM(nh->get_logger(), "Cannot find value for parameter: " << name << ". Returning default."); + return T(); +} + +} // namespace vk + +#endif // ROS_PARAMS_HELPER_H_ diff --git a/src/rpg_vikit/vikit_ros/package.xml b/src/rpg_vikit/vikit_ros/package.xml new file mode 100644 index 0000000..7e1cdff --- /dev/null +++ b/src/rpg_vikit/vikit_ros/package.xml @@ -0,0 +1,38 @@ + + + vikit_ros + 0.0.0 + + The vikit_ros package + + + cforster + + + GPLv3 + + + + + + + + ament_cmake + rosidl_default_generators + rclcpp + cmake_modules + vikit_common + tf2 + visualization_msgs + + rosidl_default_runtime + rosidl_interface_packages + + + + + + + ament_cmake + + \ No newline at end of file diff --git a/src/rpg_vikit/vikit_ros/src/camera_loader.cpp b/src/rpg_vikit/vikit_ros/src/camera_loader.cpp new file mode 100644 index 0000000..e2134e7 --- /dev/null +++ b/src/rpg_vikit/vikit_ros/src/camera_loader.cpp @@ -0,0 +1,149 @@ +/* + * camera_loader.h + * + * Created on: Feb 11, 2014 + * Author: cforster + * Update on: Feb 01, 2025 + * Author: StrangeFly + */ + +#include + +namespace vk { +namespace camera_loader { + +/// Load from ROS Namespace +bool loadFromRosNs(const rclcpp::Node::SharedPtr & nh, const std::string& ns, vk::AbstractCamera*& cam) +{ + bool res = true; + std::string cam_model(getRemoteParam(nh, ns, "cam_model", "")); + if(cam_model == "Ocam") + { + cam = new vk::OmniCamera(getRemoteParam(nh, ns, "cam_calib_file", "")); + } + else if(cam_model == "Pinhole") + { + cam = new vk::PinholeCamera( + getRemoteParam(nh, ns, "cam_width", 0), + getRemoteParam(nh, ns, "cam_height", 0), + getRemoteParam(nh, ns, "scale", 1.0), + getRemoteParam(nh, ns, "cam_fx", 0.0), + getRemoteParam(nh, ns, "cam_fy", 0.0), + getRemoteParam(nh, ns, "cam_cx", 0.0), + getRemoteParam(nh, ns, "cam_cy", 0.0), + getRemoteParam(nh, ns, "cam_d0", 0.0), + getRemoteParam(nh, ns, "cam_d1", 0.0), + getRemoteParam(nh, ns, "cam_d2", 0.0), + getRemoteParam(nh, ns, "cam_d3", 0.0)); + } + else if(cam_model == "EquidistantCamera") + { + cam = new vk::EquidistantCamera( + getParam(nh, ns+"/cam_width"), + getParam(nh, ns+"/cam_height"), + getParam(nh, ns+"/scale", 1.0), + getParam(nh, ns+"/cam_fx"), + getParam(nh, ns+"/cam_fy"), + getParam(nh, ns+"/cam_cx"), + getParam(nh, ns+"/cam_cy"), + getParam(nh, ns+"/k1", 0.0), + getParam(nh, ns+"/k2", 0.0), + getParam(nh, ns+"/k3", 0.0), + getParam(nh, ns+"/k4", 0.0)); + } + else if(cam_model == "PolynomialCamera") + { + cam = new vk::PolynomialCamera( + getParam(nh, ns+"/cam_width"), + getParam(nh, ns+"/cam_height"), + // getParam(nh, ns+"/scale", 1.0), + getParam(nh, ns+"/cam_fx"), + getParam(nh, ns+"/cam_fy"), + getParam(nh, ns+"/cam_cx"), + getParam(nh, ns+"/cam_cy"), + getParam(nh, ns+"/cam_skew"), + getParam(nh, ns+"/k2", 0.0), + getParam(nh, ns+"/k3", 0.0), + getParam(nh, ns+"/k4", 0.0), + getParam(nh, ns+"/k5", 0.0), + getParam(nh, ns+"/k6", 0.0), + getParam(nh, ns+"/k7", 0.0)); + } + else if(cam_model == "ATAN") + { + cam = new vk::ATANCamera( + getParam(nh, ns+"/cam_width"), + getParam(nh, ns+"/cam_height"), + getParam(nh, ns+"/cam_fx"), + getParam(nh, ns+"/cam_fy"), + getParam(nh, ns+"/cam_cx"), + getParam(nh, ns+"/cam_cy"), + getParam(nh, ns+"/cam_d0")); + } + else + { + cam = NULL; + res = false; + } + return res; +} + +bool loadFromRosNs(const rclcpp::Node::SharedPtr & nh, const std::string& ns, std::vector& cam_list) +{ + // `ns` is the name of the remote node holding the camera parameters + // (typically "parameter_blackboard"). Each camera's parameters are read + // from that remote node under a "cam_." prefix, e.g. "cam_0.cam_model", + // mirroring the single-camera overload above but numbered per camera. + bool res = true; + int cam_num = getRemoteParam(nh, ns, "cam_num", 0); + for (int i = 0; i < cam_num; i++) + { + std::string prefix = "cam_" + std::to_string(i) + "."; + std::string cam_model(getRemoteParam(nh, ns, prefix + "cam_model", "")); + vk::AbstractCamera* cam = nullptr; + if (cam_model == "Ocam") + { + cam = new vk::OmniCamera(getRemoteParam(nh, ns, prefix + "cam_calib_file", "")); + } + else if (cam_model == "Pinhole") + { + cam = new vk::PinholeCamera( + getRemoteParam(nh, ns, prefix + "cam_width", 0), + getRemoteParam(nh, ns, prefix + "cam_height", 0), + getRemoteParam(nh, ns, prefix + "scale", 1.0), + getRemoteParam(nh, ns, prefix + "cam_fx", 0.0), + getRemoteParam(nh, ns, prefix + "cam_fy", 0.0), + getRemoteParam(nh, ns, prefix + "cam_cx", 0.0), + getRemoteParam(nh, ns, prefix + "cam_cy", 0.0), + getRemoteParam(nh, ns, prefix + "cam_d0", 0.0), + getRemoteParam(nh, ns, prefix + "cam_d1", 0.0), + getRemoteParam(nh, ns, prefix + "cam_d2", 0.0), + getRemoteParam(nh, ns, prefix + "cam_d3", 0.0)); + } + else if (cam_model == "EquidistantCamera") + { + cam = new vk::EquidistantCamera( + getRemoteParam(nh, ns, prefix + "cam_width", 0), + getRemoteParam(nh, ns, prefix + "cam_height", 0), + getRemoteParam(nh, ns, prefix + "scale", 1.0), + getRemoteParam(nh, ns, prefix + "cam_fx", 0.0), + getRemoteParam(nh, ns, prefix + "cam_fy", 0.0), + getRemoteParam(nh, ns, prefix + "cam_cx", 0.0), + getRemoteParam(nh, ns, prefix + "cam_cy", 0.0), + getRemoteParam(nh, ns, prefix + "k1", 0.0), + getRemoteParam(nh, ns, prefix + "k2", 0.0), + getRemoteParam(nh, ns, prefix + "k3", 0.0), + getRemoteParam(nh, ns, prefix + "k4", 0.0)); + } + else + { + res = false; + } + cam_list.push_back(cam); + } + + return res; +} + +} // namespace camera_loader +} // namespace vk diff --git a/src/rpg_vikit/vikit_ros/src/output_helper.cpp b/src/rpg_vikit/vikit_ros/src/output_helper.cpp new file mode 100644 index 0000000..7210cd1 --- /dev/null +++ b/src/rpg_vikit/vikit_ros/src/output_helper.cpp @@ -0,0 +1,424 @@ +/* + * output_helper.cpp + * + * Created on: Jan 20, 2013 + * Author: chrigi + * Update on: Feb 01, 2025 + * Author: StrangeFly + */ + +#include +#include + +namespace vk { +namespace output_helper { + +void +publishTfTransform(const Sophus::SE3& T, const rclcpp::Time& stamp, + const std::string& frame_id, const std::string& child_frame_id, + tf2_ros::TransformBroadcaster& br) +{ + geometry_msgs::msg::TransformStamped transform_msg; + + transform_msg.header.stamp = stamp; + transform_msg.header.frame_id = frame_id; + transform_msg.child_frame_id = child_frame_id; + + transform_msg.transform.translation.x = T.translation().x(); + transform_msg.transform.translation.y = T.translation().y(); + transform_msg.transform.translation.z = T.translation().z(); + + Eigen::Matrix3d rotationMatrix = T.rotationMatrix(); + Eigen::Quaterniond q(rotationMatrix); + transform_msg.transform.rotation.x = q.x(); + transform_msg.transform.rotation.y = q.y(); + transform_msg.transform.rotation.z = q.z(); + transform_msg.transform.rotation.w = q.w(); + + br.sendTransform(transform_msg); +} + +void +publishPointMarker(const rclcpp::Publisher::SharedPtr pub, + const Vector3d& pos, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + rclcpp::Duration lifetime) +{ + visualization_msgs::msg::Marker msg; + msg.header.frame_id = "/world"; + msg.header.stamp = timestamp; + msg.ns = ns; + msg.id = id; + msg.type = visualization_msgs::msg::Marker::CUBE; + msg.action = action; // 0 = add/modify + msg.scale.x = marker_scale; + msg.scale.y = marker_scale; + msg.scale.z = marker_scale; + msg.color.a = 1.0; + msg.color.r = color[0]; + msg.color.g = color[1]; + msg.color.b = color[2]; + msg.lifetime = lifetime; + msg.pose.position.x = pos[0]; + msg.pose.position.y = pos[1]; + msg.pose.position.z = pos[2]; + pub->publish(msg); +} + +void +publishLineMarker(const rclcpp::Publisher::SharedPtr pub, + const Vector3d& start, + const Vector3d& end, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + rclcpp::Duration lifetime) +{ + visualization_msgs::msg::Marker msg; + msg.header.frame_id = "/world"; + msg.header.stamp = timestamp; + msg.ns = ns; + msg.id = id; + msg.type = visualization_msgs::msg::Marker::LINE_STRIP; + msg.action = action; // 0 = add/modify + msg.scale.x = marker_scale; + msg.color.a = 1.0; + msg.color.r = color[0]; + msg.color.g = color[1]; + msg.color.b = color[2]; + msg.points.resize(2); + msg.lifetime = lifetime; + msg.points[0].x = start[0]; + msg.points[0].y = start[1]; + msg.points[0].z = start[2]; + msg.points[1].x = end[0]; + msg.points[1].y = end[1]; + msg.points[1].z = end[2]; + pub->publish(msg); +} + + +void +publishArrowMarker(const rclcpp::Publisher::SharedPtr pub, + const Vector3d& pos, + const Vector3d& dir, + double scale, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color) +{ + visualization_msgs::msg::Marker msg; + msg.header.frame_id = "/world"; + msg.header.stamp = timestamp; + msg.ns = ns; + msg.id = id; + msg.type = visualization_msgs::msg::Marker::ARROW; + msg.action = action; // 0 = add/modify + msg.scale.x = marker_scale; + msg.scale.y = marker_scale*0.35; + msg.scale.z = 0.0; + msg.color.a = 1.0; + msg.color.r = color[0]; + msg.color.g = color[1]; + msg.color.b = color[2]; + msg.points.resize(2); + msg.points[0].x = pos[0]; + msg.points[0].y = pos[1]; + msg.points[0].z = pos[2]; + msg.points[1].x = pos[0] + scale*dir[0]; + msg.points[1].y = pos[1] + scale*dir[1]; + msg.points[1].z = pos[2] + scale*dir[2]; + pub->publish(msg); +} + +void +publishHexacopterMarker(const rclcpp::Publisher::SharedPtr pub, + const string& frame_id, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color) +{ + /* + * Function by Markus Achtelik from libsfly_viz. + * Thank you. + */ + const double sqrt2_2 = sqrt(2) / 2; + + visualization_msgs::msg::Marker marker; + + // the marker will be displayed in frame_id + marker.header.frame_id = frame_id; + marker.header.stamp = timestamp; + marker.ns = ns; + marker.action = 0; + marker.id = id; + + // make rotors + marker.type = visualization_msgs::msg::Marker::CYLINDER; + marker.scale.x = 0.2*marker_scale; + marker.scale.y = 0.2*marker_scale; + marker.scale.z = 0.01*marker_scale; + marker.color.r = 0.4; + marker.color.g = 0.4; + marker.color.b = 0.4; + marker.color.a = 0.8; + marker.pose.position.z = 0; + + // front left/right + marker.pose.position.x = 0.19*marker_scale; + marker.pose.position.y = 0.11*marker_scale; + marker.id--; + pub->publish(marker); + + marker.pose.position.x = 0.19*marker_scale; + marker.pose.position.y = -0.11*marker_scale; + marker.id--; + pub->publish(marker); + + // left/right + marker.pose.position.x = 0; + marker.pose.position.y = 0.22*marker_scale; + marker.id--; + pub->publish(marker); + + marker.pose.position.x = 0; + marker.pose.position.y = -0.22*marker_scale; + marker.id--; + pub->publish(marker); + + // back left/right + marker.pose.position.x = -0.19*marker_scale; + marker.pose.position.y = 0.11*marker_scale; + marker.id--; + pub->publish(marker); + + marker.pose.position.x = -0.19*marker_scale; + marker.pose.position.y = -0.11*marker_scale; + marker.id--; + pub->publish(marker); + + // make arms + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.scale.x = 0.44*marker_scale; + marker.scale.y = 0.02*marker_scale; + marker.scale.z = 0.01*marker_scale; + marker.color.r = color[0]; + marker.color.g = color[1]; + marker.color.b = color[2]; + marker.color.a = 1; + + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = -0.015*marker_scale; + marker.pose.orientation.x = 0; + marker.pose.orientation.y = 0; + + marker.pose.orientation.w = sqrt2_2; + marker.pose.orientation.z = sqrt2_2; + marker.id--; + pub->publish(marker); + + // 30 deg rotation 0.9659 0 0 0.2588 + marker.pose.orientation.w = 0.9659; + marker.pose.orientation.z = 0.2588; + marker.id--; + pub->publish(marker); + + marker.pose.orientation.w = 0.9659; + marker.pose.orientation.z = -0.2588; + marker.id--; + pub->publish(marker); +} + +void +publishCameraMarker(const rclcpp::Publisher::SharedPtr pub, + const string& frame_id, + const string& ns, + const rclcpp::Time& timestamp, + int id, + double marker_scale, + const Vector3d& color) +{ + /* + * draw a pyramid as the camera marker + */ + const double sqrt2_2 = sqrt(2) / 2; + + visualization_msgs::msg::Marker marker; + + // the marker will be displayed in frame_id + marker.header.frame_id = frame_id; + marker.header.stamp = timestamp; + marker.ns = ns; + marker.action = 0; + marker.id = id; + + // make rectangles as frame + double r_w = 1.0; + double z_plane = (r_w / 2.0)*marker_scale; + marker.pose.position.x = 0; + marker.pose.position.y = (r_w / 4.0) *marker_scale; + marker.pose.position.z = z_plane; + + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.scale.x = r_w*marker_scale; + marker.scale.y = 0.04*marker_scale; + marker.scale.z = 0.04*marker_scale; + marker.color.r = color[0]; + marker.color.g = color[1]; + marker.color.b = color[2]; + marker.color.a = 1; + + marker.pose.orientation.x = 0; + marker.pose.orientation.y = 0; + marker.pose.orientation.z = 0; + marker.pose.orientation.w = 1; + marker.id--; + pub->publish(marker); + marker.pose.position.y = -(r_w/ 4.0)*marker_scale; + marker.id--; + pub->publish(marker); + + marker.scale.x = (r_w/2.0)*marker_scale; + marker.pose.position.x = (r_w / 2.0) *marker_scale; + marker.pose.position.y = 0; + marker.pose.orientation.w = sqrt2_2; + marker.pose.orientation.z = sqrt2_2; + marker.id--; + pub->publish(marker); + marker.pose.position.x = -(r_w / 2.0) *marker_scale; + marker.id--; + pub->publish(marker); + + // make pyramid edges + marker.scale.x = (3.0*r_w/4.0)*marker_scale; + marker.pose.position.z = 0.5*z_plane; + + marker.pose.position.x = (r_w / 4.0) *marker_scale; + marker.pose.position.y = (r_w / 8.0) *marker_scale; + // 0.08198092, -0.34727674, 0.21462883, 0.9091823 + marker.pose.orientation.x = 0.08198092; + marker.pose.orientation.y = -0.34727674; + marker.pose.orientation.z = 0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub->publish(marker); + + marker.pose.position.x = -(r_w / 4.0) *marker_scale; + marker.pose.position.y = (r_w / 8.0) *marker_scale; + // -0.27395078, -0.22863284, 0.9091823 , 0.21462883 + marker.pose.orientation.x = 0.08198092; + marker.pose.orientation.y = 0.34727674; + marker.pose.orientation.z = -0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub->publish(marker); + + marker.pose.position.x = -(r_w / 4.0) *marker_scale; + marker.pose.position.y = -(r_w / 8.0) *marker_scale; + // -0.08198092, 0.34727674, 0.21462883, 0.9091823 + marker.pose.orientation.x = -0.08198092; + marker.pose.orientation.y = 0.34727674; + marker.pose.orientation.z = 0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub->publish(marker); + + marker.pose.position.x = (r_w / 4.0) *marker_scale; + marker.pose.position.y = -(r_w / 8.0) *marker_scale; + // -0.08198092, -0.34727674, -0.21462883, 0.9091823 + marker.pose.orientation.x = -0.08198092; + marker.pose.orientation.y = -0.34727674; + marker.pose.orientation.z = -0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub->publish(marker); +} + +void publishFrameMarker(const rclcpp::Publisher::SharedPtr pub, + const Matrix3d& rot, + const Vector3d& pos, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + rclcpp::Duration lifetime) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "/world"; + marker.header.stamp = timestamp; + marker.ns = ns; + marker.id = id++; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = action; // 0 = add/modify + marker.points.reserve(2); + geometry_msgs::msg::Point point; + point.x = static_cast(pos.x()); + point.y = static_cast(pos.y()); + point.z = static_cast(pos.z()); + marker.points.push_back(point); + point.x = static_cast(pos.x() + marker_scale*rot(0, 2)); // Draw arrow in z-direction + point.y = static_cast(pos.y() + marker_scale*rot(1, 2)); // Draw arrow in z-direction + point.z = static_cast(pos.z() + marker_scale*rot(2, 2)); // Draw arrow in z-direction + marker.points.push_back(point); + marker.scale.x = 0.5*marker_scale; + marker.scale.y = 0.5*marker_scale; + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + marker.lifetime = lifetime; + pub->publish(marker); + + marker.id = id++; + marker.points.clear(); + point.x = static_cast(pos.x()); + point.y = static_cast(pos.y()); + point.z = static_cast(pos.z()); + marker.points.push_back(point); + point.x = static_cast(pos.x() + marker_scale*rot(0, 0)); // Draw arrow in x-direction + point.y = static_cast(pos.y() + marker_scale*rot(1, 0)); // Draw arrow in x-direction + point.z = static_cast(pos.z() + marker_scale*rot(2, 0)); // Draw arrow in x-direction + marker.points.push_back(point); + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.lifetime = lifetime; + pub->publish(marker); + + marker.id = id++; + marker.points.clear(); + point.x = static_cast(pos.x()); + point.y = static_cast(pos.y()); + point.z = static_cast(pos.z()); + marker.points.push_back(point); + point.x = static_cast(pos.x() + marker_scale*rot(0, 1)); // Draw arrow in y-direction + point.y = static_cast(pos.y() + marker_scale*rot(1, 1)); // Draw arrow in y-direction + point.z = static_cast(pos.z() + marker_scale*rot(2, 1)); // Draw arrow in y-direction + marker.points.push_back(point); + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.lifetime = lifetime; + pub->publish(marker); +} + +} // namespace output_helper +} // namespace vk + + diff --git a/src/um982_driver/CMakeLists.txt b/src/um982_driver/CMakeLists.txt new file mode 100644 index 0000000..2e6005e --- /dev/null +++ b/src/um982_driver/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(um982_driver) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(gnss_comm REQUIRED) + +add_executable(um982_driver_node src/um982_driver_node.cpp) +ament_target_dependencies(um982_driver_node rclcpp gnss_comm) +target_link_libraries(um982_driver_node pthread) + +install(TARGETS um982_driver_node + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/src/um982_driver/README.md b/src/um982_driver/README.md new file mode 100644 index 0000000..3f19386 --- /dev/null +++ b/src/um982_driver/README.md @@ -0,0 +1,68 @@ +# um982_driver + +UM982(Unicore) → `gnss_comm/msg/GnssPVTSolnMsg` 드라이버 노드. FAST-LIVO2-RTK 백엔드가 +구독하는 토픽(`gps.gps_topic`, 기본 `/ublox_driver/receiver_pvt`)으로 발행한다. + +기존 ublox 드라이버를 대체한다. 자세한 배경/검토는 +[`docs/UM982-명령어검토-및-바이너리포팅.md`](../../docs/UM982-명령어검토-및-바이너리포팅.md), +[`docs/UM982-녹화-및-연동-구현.md`](../../docs/UM982-녹화-및-연동-구현.md) 참고. + +## 한 노드에 다 들어있는 이유 +시리얼 포트는 한 프로세스만 점유할 수 있다. 따라서 **RTCM 주입(NTRIP)** 과 **BESTNAV 파싱/발행**이 +같은 노드 안에 있다: + +``` +NTRIP(VRS) ──RTCM3──▶ UM982 ──BESTNAV(binary)+GGA(ascii)──▶ um982_driver ──GnssPVTSolnMsg──▶ 백엔드 + ▲ │ + └────────────────────── GGA(VRS 유지) ───────────────────┘ +``` + +## 수신기 사전 설정 +드라이버 실행 전에 UM982 가 아래를 출력하도록 저장돼 있어야 한다(리포 루트 `rtk/config_all.py`): +``` +BESTNAVB COM3 0.1 # 10Hz 바이너리 (파싱 소스) +GPGGA COM3 1 # NTRIP VRS 업링크용 +``` + +## 빌드 +```bash +colcon build --packages-select gnss_comm um982_driver +source install/setup.bash +``` + +## 실행 +```bash +# config/um982.yaml 에서 serial_port / NTRIP 자격증명 확인 후 +ros2 launch um982_driver um982_driver.launch.py +# 또는 자격증명을 CLI 로: +ros2 run um982_driver um982_driver_node --ros-args \ + --params-file src/um982_driver/config/um982.yaml \ + -p ntrip_user:= -p ntrip_pass:= +``` +`ros2 topic echo /ublox_driver/receiver_pvt` 로 발행 확인. 콘솔에 `RTK FIXED` 전환 로그가 뜬다. + +## BESTNAV → GnssPVTSolnMsg 매핑 +| GnssPVTSolnMsg | BESTNAV(§7.3.26) | 비고 | +|---|---|---| +| `time.week` / `time.tow` | 헤더 Wn / Ms÷1000 | UNIX 복원 → 센서 시간동기 | +| `latitude` / `longitude` | lat / lon | | +| `altitude` | hgt(MSL) + undulation | **타원체고** (백엔드 ENU 입력) | +| `height_msl` | hgt | | +| `h_acc` / `v_acc` | √(latσ²+lonσ²) / hgtσ | GTSAM 공분산 | +| `vel_n/e/d` | hor·cos(trk) / hor·sin(trk) / −vert | 속력·침로 → NED | +| `carr_soln` | pos_type: 48/49/50→2, 32/33/34→1 | RTK Fixed/Float | +| `fix_type`/`valid_fix`/`diff_soln`/`num_sv` | pos_type / #solnSVs | | + +## 파라미터 (config/um982.yaml) +- `serial_port`, `serial_baud`(460800), `gps_topic` +- `use_ntrip`, `ntrip_host/port/mountpoint/user/pass`, `gga_send_period` +- `publish_fixed_only`(기본 false), `inflate_cov_when_not_fixed`(기본 true) + +> 백엔드는 fix 품질을 거르지 않으므로, RTK Fixed 가 아닌 해는 `inflate_cov_when_not_fixed` +> 로 공분산을 키워 영향력을 줄이거나 `publish_fixed_only` 로 아예 배제한다. + +## 검증 +CRC(Appendix 1) 통과 프레임만 발행한다. 파서 로직은 `rtk/rtk_test.py`(파이썬 프로토타입)와 동일. +```bash +python3 rtk/rtk_test.py # 하드웨어 붙이기 전 파서/파이프라인 사전 검증 +``` diff --git a/src/um982_driver/config/um982.yaml b/src/um982_driver/config/um982.yaml new file mode 100644 index 0000000..dd75087 --- /dev/null +++ b/src/um982_driver/config/um982.yaml @@ -0,0 +1,23 @@ +um982_driver: + ros__parameters: + # ── 시리얼 (UM982 COM3 = USB) ── + serial_port: "/dev/ttyUSB0" # 실제 포트로. macOS 는 /dev/tty.usbserial-*, 리눅스 /dev/ttyUSB0 + serial_baud: 460800 # config_all.py / set_baud.py 와 일치 + + # ── 발행 토픽 (백엔드 gps.gps_topic 와 일치) ── + gps_topic: "/ublox_driver/receiver_pvt" + + # ── NTRIP (VRS) ── + use_ntrip: true + ntrip_host: "RTS1.ngii.go.kr" + ntrip_port: 2101 + ntrip_mountpoint: "VRS-RTCM34" + ntrip_user: "" # ⚠️ 여기에 채우거나 launch 인자/환경변수로 주입(리포에 평문 저장 지양) + ntrip_pass: "" + gga_send_period: 10.0 # VRS 유지용 GGA 재전송 주기(초) + + # ── 품질 처리 (백엔드에 fix 품질 필터가 없음) ── + publish_fixed_only: false # true = RTK Fixed(carr_soln=2) 해만 발행 + inflate_cov_when_not_fixed: true # fix 아니면(또는 코스팅 중) h_acc/v_acc 부풀려 GTSAM 가중 약화 + max_corr_age: 30.0 # 보정 나이(diff_age) 초과 시 코스팅으로 간주해 공분산 부풀림. 0=비활성 + # NTRIP 끊겨도 UM982 는 RTK TIMEOUT(기본 600s) 동안 Fixed 유지하므로 필요 diff --git a/src/um982_driver/launch/um982_driver.launch.py b/src/um982_driver/launch/um982_driver.launch.py new file mode 100644 index 0000000..60dca70 --- /dev/null +++ b/src/um982_driver/launch/um982_driver.launch.py @@ -0,0 +1,29 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + default_cfg = os.path.join( + get_package_share_directory('um982_driver'), 'config', 'um982.yaml') + cfg = LaunchConfiguration('config') + + return LaunchDescription([ + DeclareLaunchArgument('config', default_value=default_cfg, + description='파라미터 yaml 경로'), + Node( + package='um982_driver', + executable='um982_driver_node', + name='um982_driver', + output='screen', + parameters=[cfg], + ), + ]) + +# NTRIP 자격증명은 config yaml 에 넣거나(리포에 평문 커밋 지양), 실행 시 덮어쓰기: +# ros2 launch um982_driver um982_driver.launch.py +# ros2 run um982_driver um982_driver_node --ros-args \ +# --params-file .yaml -p ntrip_user:= -p ntrip_pass:= diff --git a/src/um982_driver/package.xml b/src/um982_driver/package.xml new file mode 100644 index 0000000..299f5b3 --- /dev/null +++ b/src/um982_driver/package.xml @@ -0,0 +1,24 @@ + + + + um982_driver + 0.1.0 + + UM982 (Unicore) GNSS/RTK driver for FAST-LIVO2-RTK. Reads BESTNAV binary + over serial, feeds NTRIP RTCM3 corrections back to the receiver, and + publishes gnss_comm/msg/GnssPVTSolnMsg on the topic the backend subscribes to. + + FAST-LIVO2-RTK + GPLv3 + + ament_cmake + + rclcpp + gnss_comm + + ros2launch + + + ament_cmake + + diff --git a/src/um982_driver/src/um982_driver_node.cpp b/src/um982_driver/src/um982_driver_node.cpp new file mode 100644 index 0000000..e8537a2 --- /dev/null +++ b/src/um982_driver/src/um982_driver_node.cpp @@ -0,0 +1,369 @@ +// UM982 (Unicore) driver for FAST-LIVO2-RTK (ROS 2 Humble) +// --------------------------------------------------------------------------- +// 한 노드에서: +// 1) UM982 시리얼(COM3) 오픈 — BESTNAV(binary) + GGA(ascii) 혼합 스트림 수신 +// 2) NTRIP(VRS) 접속 → RTCM3 을 같은 시리얼로 주입(별도 스레드, 자동 재접속) +// 3) 첫 유효 GGA 로 VRS 부트스트랩, 이후 주기적 재전송 +// 4) BESTNAV 를 파싱해 gnss_comm/msg/GnssPVTSolnMsg 로 발행 (gps.gps_topic) +// +// 시리얼 포트는 한 프로세스만 점유 가능하므로 RTCM 주입과 파싱이 반드시 한 노드에 있어야 한다. +// 파서 오프셋 근거: Unicore N4 Reference Commands Manual, §7.3.26(BESTNAV)/Table 7-49(헤더). +// 대상 필드 근거: optimization.cpp::gpsHandler (latitude/longitude/altitude, time, vel, h/v_acc). +// +// 의존성: rclcpp, gnss_comm (시리얼=termios, NTRIP=POSIX 소켓 — 외부 라이브러리 불필요) +// 가정: 호스트가 little-endian (x86_64/aarch64 리눅스 — Unicore 바이너리도 LE). + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using gnss_comm::msg::GnssPVTSolnMsg; + +namespace { + +// ── BESTNAV 바이너리 레이아웃 (little-endian) ────────────────────────────── +constexpr uint8_t SYNC0 = 0xAA, SYNC1 = 0x44, SYNC2 = 0xB5; +constexpr size_t HDR_LEN = 24; // 동기 3B 포함 헤더 총 길이 +constexpr uint16_t BESTNAV_ID = 2118; + +// 헤더 오프셋 +constexpr size_t OFF_MSGID = 4, OFF_MSGLEN = 6, OFF_TSTAT = 9, OFF_WEEK = 10, OFF_MS = 12; +// 본문(절대) 오프셋 = HDR_LEN + 표(§3.3) 상대오프셋 +constexpr size_t OFF_POSTYPE = 28; +constexpr size_t OFF_LAT = 32, OFF_LON = 40, OFF_HGT = 48, OFF_UNDU = 56; +constexpr size_t OFF_LATSTD = 64, OFF_LONSTD = 68, OFF_HGTSTD = 72; +constexpr size_t OFF_DIFFAGE = 80, OFF_NSOLN = 89; +constexpr size_t OFF_HORSPD = 112, OFF_TRKGND = 120, OFF_VERTSPD = 128, OFF_HORSPDSTD = 140; + +// Position/Velocity Type (Table 0-4) +constexpr int32_t POS_NONE = 0, POS_SINGLE = 16; +inline bool is_rtk_fixed(int32_t t) { return t == 48 || t == 49 || t == 50; } +inline bool is_rtk_float(int32_t t) { return t == 32 || t == 33 || t == 34; } + +template +inline T rd(const uint8_t *p, size_t off) { T v; std::memcpy(&v, p + off, sizeof(T)); return v; } + +// NovAtel/Unicore 32-bit CRC (Appendix 1) +uint32_t crc32_value(int i) { + uint32_t crc = static_cast(i); + for (int j = 8; j > 0; --j) + crc = (crc & 1) ? (crc >> 1) ^ 0xEDB88320UL : (crc >> 1); + return crc; +} +uint32_t block_crc32(const uint8_t *buf, size_t n) { + uint32_t crc = 0; + while (n-- != 0) { + uint32_t t1 = (crc >> 8) & 0x00FFFFFFUL; + uint32_t t2 = crc32_value((static_cast(crc) ^ *buf++) & 0xFF); + crc = t1 ^ t2; + } + return crc; +} + +speed_t to_speed(int baud) { + switch (baud) { + case 9600: return B9600; case 19200: return B19200; + case 38400: return B38400; case 57600: return B57600; + case 115200: return B115200; case 230400: return B230400; + case 460800: return B460800; case 921600: return B921600; + default: return B460800; + } +} + +std::string base64(const std::string &in) { + static const char *T = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; + std::string out; + int val = 0, bits = -6; + for (unsigned char c : in) { + val = (val << 8) + c; bits += 8; + while (bits >= 0) { out.push_back(T[(val >> bits) & 0x3F]); bits -= 6; } + } + if (bits > -6) out.push_back(T[((val << 8) >> (bits + 8)) & 0x3F]); + while (out.size() % 4) out.push_back('='); + return out; +} + +} // namespace + +class Um982Driver : public rclcpp::Node { + public: + Um982Driver() : Node("um982_driver") { + // ── 파라미터 ── + serial_port_ = declare_parameter("serial_port", "/dev/ttyUSB0"); + serial_baud_ = declare_parameter("serial_baud", 460800); + gps_topic_ = declare_parameter("gps_topic", "/ublox_driver/receiver_pvt"); + ntrip_host_ = declare_parameter("ntrip_host", "RTS1.ngii.go.kr"); + ntrip_port_ = declare_parameter("ntrip_port", 2101); + ntrip_user_ = declare_parameter("ntrip_user", ""); + ntrip_pass_ = declare_parameter("ntrip_pass", ""); + ntrip_mp_ = declare_parameter("ntrip_mountpoint", "VRS-RTCM34"); + use_ntrip_ = declare_parameter("use_ntrip", true); + fixed_only_ = declare_parameter("publish_fixed_only", false); + inflate_cov_ = declare_parameter("inflate_cov_when_not_fixed", true); + max_corr_age_ = declare_parameter("max_corr_age", 30.0); // 보정 나이 초과 시 코스팅으로 간주 + gga_period_ = declare_parameter("gga_send_period", 10.0); + + pub_ = create_publisher(gps_topic_, rclcpp::QoS(rclcpp::KeepLast(2000))); + + if (!open_serial()) { + RCLCPP_FATAL(get_logger(), "시리얼 오픈 실패: %s", serial_port_.c_str()); + throw std::runtime_error("serial open failed"); + } + RCLCPP_INFO(get_logger(), "UM982 %s @ %d → 발행 %s", + serial_port_.c_str(), serial_baud_, gps_topic_.c_str()); + + running_ = true; + serial_thread_ = std::thread(&Um982Driver::serial_loop, this); + if (use_ntrip_) ntrip_thread_ = std::thread(&Um982Driver::ntrip_loop, this); + } + + ~Um982Driver() override { + running_ = false; + if (serial_thread_.joinable()) serial_thread_.join(); + if (ntrip_thread_.joinable()) ntrip_thread_.join(); + if (serial_fd_ >= 0) ::close(serial_fd_); + if (ntrip_fd_ >= 0) ::close(ntrip_fd_); + } + + private: + // ── 시리얼 ────────────────────────────────────────────────────────────── + bool open_serial() { + serial_fd_ = ::open(serial_port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + if (serial_fd_ < 0) return false; + termios tio{}; + if (tcgetattr(serial_fd_, &tio) != 0) return false; + cfmakeraw(&tio); + cfsetispeed(&tio, to_speed(serial_baud_)); + cfsetospeed(&tio, to_speed(serial_baud_)); + tio.c_cflag |= (CLOCAL | CREAD); + tio.c_cflag &= ~CRTSCTS; + tio.c_cc[VMIN] = 0; + tio.c_cc[VTIME] = 0; + return tcsetattr(serial_fd_, TCSANOW, &tio) == 0; + } + + void serial_loop() { + std::vector buf; + buf.reserve(1 << 16); + uint8_t tmp[4096]; + while (running_ && rclcpp::ok()) { + ssize_t n = ::read(serial_fd_, tmp, sizeof(tmp)); + if (n > 0) buf.insert(buf.end(), tmp, tmp + n); + else { std::this_thread::sleep_for(std::chrono::milliseconds(5)); } + demux(buf); + if (buf.size() > (1 << 18)) buf.erase(buf.begin(), buf.end() - 256); // 안전장치 + } + } + + // BESTNAV(binary) 프레임과 GGA(ascii) 라인을 분리 + void demux(std::vector &buf) { + size_t search = 0; + while (true) { + // 다음 동기 위치 + size_t sync = find_sync(buf, search); + // 동기 이전(head)에서 GGA 추출 + size_t head_end = (sync == std::string::npos) ? buf.size() : sync; + scan_gga(buf, search, head_end); + + if (sync == std::string::npos) { + if (buf.size() > 2) buf.erase(buf.begin(), buf.end() - 2); // 동기 경계 보존 + return; + } + if (buf.size() - sync < HDR_LEN) { buf.erase(buf.begin(), buf.begin() + sync); return; } + uint16_t mlen = rd(buf.data(), sync + OFF_MSGLEN); + size_t total = HDR_LEN + mlen + 4; + if (buf.size() - sync < total) { buf.erase(buf.begin(), buf.begin() + sync); return; } + + const uint8_t *f = buf.data() + sync; + if (rd(f, OFF_MSGID) == BESTNAV_ID) { + uint32_t got = rd(f, HDR_LEN + mlen); + if (block_crc32(f, HDR_LEN + mlen) == got) handle_bestnav(f); + else RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "BESTNAV CRC 불일치 — 스킵"); + } + buf.erase(buf.begin(), buf.begin() + sync + total); + search = 0; + } + } + + static size_t find_sync(const std::vector &b, size_t from) { + for (size_t i = from; i + 2 < b.size(); ++i) + if (b[i] == SYNC0 && b[i + 1] == SYNC1 && b[i + 2] == SYNC2) return i; + return std::string::npos; + } + + // buf[begin,end) 구간에서 $G?GGA 라인을 찾아 최신 GGA(fix 유효분) 갱신 + void scan_gga(const std::vector &buf, size_t begin, size_t end) { + for (size_t p = begin; p + 4 < end; ++p) { + if (buf[p] != '$') continue; + if (!(p + 6 < end && buf[p + 3] == 'G' && buf[p + 4] == 'G' && buf[p + 5] == 'A')) continue; + size_t e = p; + while (e < end && buf[e] != '\r' && buf[e] != '\n') ++e; + if (e >= end) break; // 종결문자 없음 = 라인 미완 → 다음 read 까지 대기(잘린 GGA 전송 방지) + std::string line(reinterpret_cast(&buf[p]), e - p); + // field 6 = fix quality + int comma = 0; size_t i = 0; std::string fix; + for (; i < line.size(); ++i) { + if (line[i] == ',') { if (++comma == 6) { size_t s = i + 1; while (s < line.size() && line[s] != ',') fix.push_back(line[s++]); break; } } + } + if (!fix.empty() && fix != "0") { + std::lock_guard lk(gga_mtx_); + latest_gga_ = line + "\r\n"; + } + p = e; + } + } + + void handle_bestnav(const uint8_t *f) { + int32_t pos_type = rd(f, OFF_POSTYPE); + if (fixed_only_ && !is_rtk_fixed(pos_type)) return; + + GnssPVTSolnMsg m; + m.time.week = rd(f, OFF_WEEK); + m.time.tow = rd(f, OFF_MS) / 1000.0; + + double lat = rd(f, OFF_LAT); + double lon = rd(f, OFF_LON); + double hgt = rd(f, OFF_HGT); // MSL + float undu = rd(f, OFF_UNDU); + m.latitude = lat; + m.longitude = lon; + m.altitude = hgt + undu; // 타원체고 (백엔드 ENU 입력) + m.height_msl = hgt; + + float lat_s = rd(f, OFF_LATSTD); + float lon_s = rd(f, OFF_LONSTD); + float hgt_s = rd(f, OFF_HGTSTD); + float diff_age = rd(f, OFF_DIFFAGE); // 보정 나이(s). 커지면 코스팅 중 + m.h_acc = std::hypot(lat_s, lon_s); + m.v_acc = hgt_s; + + double hor = rd(f, OFF_HORSPD); + double trk = rd(f, OFF_TRKGND) * M_PI / 180.0; + double vert = rd(f, OFF_VERTSPD); + m.vel_n = hor * std::cos(trk); + m.vel_e = hor * std::sin(trk); + m.vel_d = -vert; // BESTNAV vert(+up) → NED down + m.vel_acc = rd(f, OFF_HORSPDSTD); + + m.num_sv = f[OFF_NSOLN]; + m.fix_type = (pos_type == POS_NONE) ? 0 : 3; + m.valid_fix = (pos_type != POS_NONE); + m.diff_soln = (pos_type != POS_NONE && pos_type != POS_SINGLE); + m.carr_soln = is_rtk_fixed(pos_type) ? 2 : (is_rtk_float(pos_type) ? 1 : 0); + m.p_dop = 0.0; + + // 신뢰 낮은 해는 공분산 부풀려 백엔드 가중 약화(백엔드에 품질 필터 없음): + // (1) RTK Fixed 가 아니거나, (2) 보정이 끊겨 코스팅 중(diff_age 과다) + bool coasting = (max_corr_age_ > 0.0 && diff_age > max_corr_age_); + if (inflate_cov_ && (m.carr_soln != 2 || coasting)) { + m.h_acc = std::max(m.h_acc, 0.5); + m.v_acc = std::max(m.v_acc, 1.0); + } + + pub_->publish(m); + + if (pos_type != last_type_) { + RCLCPP_INFO(get_logger(), "상태: %s | lat=%.7f lon=%.7f alt=%.2f hσ=%.3fm nSV=%u", + is_rtk_fixed(pos_type) ? "RTK FIXED" : is_rtk_float(pos_type) ? "RTK FLOAT" + : pos_type == POS_SINGLE ? "SINGLE" : pos_type == POS_NONE ? "NONE" : "OTHER", + lat, lon, m.altitude, m.h_acc, m.num_sv); + last_type_ = pos_type; + } + } + + // ── NTRIP ─────────────────────────────────────────────────────────────── + void ntrip_loop() { + // 첫 유효 GGA 대기(VRS 부트스트랩) + while (running_ && rclcpp::ok() && latest_gga().empty()) + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + auto last_send = std::chrono::steady_clock::now(); + while (running_ && rclcpp::ok()) { + if (ntrip_fd_ < 0) { + if (!ntrip_connect()) { std::this_thread::sleep_for(std::chrono::seconds(2)); continue; } + RCLCPP_INFO(get_logger(), "NTRIP 접속: %s:%d/%s", ntrip_host_.c_str(), ntrip_port_, ntrip_mp_.c_str()); + last_send = std::chrono::steady_clock::now(); + } + uint8_t buf[4096]; + ssize_t n = ::recv(ntrip_fd_, buf, sizeof(buf), 0); + if (n > 0) { + ::write(serial_fd_, buf, n); // RTCM 주입 + } else if (n == 0) { + RCLCPP_WARN(get_logger(), "NTRIP 연결 종료 → 재접속"); + ::close(ntrip_fd_); ntrip_fd_ = -1; continue; + } + // VRS 유지용 GGA 주기 재전송 + auto now = std::chrono::steady_clock::now(); + if (std::chrono::duration(now - last_send).count() > gga_period_) { + std::string g = latest_gga(); + if (!g.empty()) ::send(ntrip_fd_, g.data(), g.size(), MSG_NOSIGNAL); + last_send = now; + } + } + } + + bool ntrip_connect() { + addrinfo hints{}, *res = nullptr; + hints.ai_family = AF_INET; hints.ai_socktype = SOCK_STREAM; + if (getaddrinfo(ntrip_host_.c_str(), std::to_string(ntrip_port_).c_str(), &hints, &res) != 0) return false; + int fd = ::socket(res->ai_family, res->ai_socktype, res->ai_protocol); + timeval tv{5, 0}; + setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); + bool ok = (::connect(fd, res->ai_addr, res->ai_addrlen) == 0); + freeaddrinfo(res); + if (!ok) { ::close(fd); return false; } + + std::string auth = base64(ntrip_user_ + ":" + ntrip_pass_); + std::string req = "GET /" + ntrip_mp_ + " HTTP/1.0\r\n" + "User-Agent: NTRIP um982_driver/1.0\r\nAccept: */*\r\n" + "Connection: close\r\nAuthorization: Basic " + auth + "\r\n\r\n"; + ::send(fd, req.data(), req.size(), MSG_NOSIGNAL); + char resp[1024]; ssize_t r = ::recv(fd, resp, sizeof(resp) - 1, 0); + if (r <= 0) { ::close(fd); return false; } + resp[r] = 0; + if (!strstr(resp, "200 OK") && !strstr(resp, "ICY 200 OK")) { ::close(fd); return false; } + std::string g = latest_gga(); + if (!g.empty()) ::send(fd, g.data(), g.size(), MSG_NOSIGNAL); + ntrip_fd_ = fd; + return true; + } + + std::string latest_gga() { std::lock_guard lk(gga_mtx_); return latest_gga_; } + + // 파라미터 + std::string serial_port_, gps_topic_, ntrip_host_, ntrip_user_, ntrip_pass_, ntrip_mp_; + int serial_baud_{}, ntrip_port_{}; + bool use_ntrip_{}, fixed_only_{}, inflate_cov_{}; + double gga_period_{}, max_corr_age_{}; + + rclcpp::Publisher::SharedPtr pub_; + int serial_fd_{-1}; + std::atomic ntrip_fd_{-1}; + std::atomic running_{false}; + std::thread serial_thread_, ntrip_thread_; + std::mutex gga_mtx_; + std::string latest_gga_; + int32_t last_type_{-999}; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}