azmo/fhd_fast_tri_ws
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial import: fast_dual_ws (renamed to fhd_fast_tri_ws)

Browse Source 
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.
This commit is contained in:
hjkim
2026-08-07 14:09:06 +09:00
commit 82d3725779
149 changed files with 26111 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.claude/settings.local.json
+52
View File
@@ -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'\\)\")"
]
}
}
.gitignore
+17
View File
@@ -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
docs/Jetson-이관가이드.md
+192
View File
@@ -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/<new>|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)
docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md
+282
View File
@@ -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<cv::Mat> img_buffer` |
| 카메라 파라미터 | `vk::camera_loader::loadFromRosNs(node, "parameter_blackboard", cam)` 단일 카메라 오버로드만 호출 (`LIVMapper.cpp:194`) — **벡터 오버로드가 이미 존재하지만 버그가 있고 미사용** (`vikit_ros/src/camera_loader.cpp:91-141`) |
| VIO 코어 | `VIOManager` (`vio.h`/`vio.cpp`, 1876+186줄) — `cam`, `Rci/Pci/Rcl/Pcl`, `fx/fy/cx/cy`, `img_cp/img_rgb`, `new_frame_` 등 전부 **단일 스칼라 멤버** |
| 측정 그룹 | `MeasureGroup::img`가 단일 `cv::Mat` (`common_lib.h:70`) |
| 기존 스캐폴딩 | `extrin_cam1/2/3.yaml`, `camera_cam1/cam3.yaml`, `mapping_mid360s_cam{1,2,3}.launch.py` — **동시 멀티카메라가 아니라 "한 번에 카메라 1대씩 바꿔 쓰는" 상호 배타적 대안 설정**임에 주의 |
### 2.2 `Omni-LIVO` (참조 구현, ROS1)
| 항목 | 내용 |
|---|---|
| 패키지 | `omni_livo` (catkin), 커스텀 `.msg` 없음 (내부 C++ 구조체로만 처리 → ROS2 인터페이스 패키지 불필요) |
| 이미지 구독 | 카메라마다 독립 `ros::Subscriber` + 카메라별 `deque` (`LIVMapper.cpp:312-330`), `message_filters` 미사용, 수동 뮤텍스/타임스탬프 동기화 |
| 동기화 | `sync_packages()`가 `num_of_cameras`개 버퍼가 모두 채워졌는지 확인 후 `time_tolerance=0.001s`로 정합 (`LIVMapper.cpp:904-1165`) |
| 카메라 파라미터 | 외부파라미터: `extrin_calib.cameras`가 **배열-of-구조체** YAML(`cam_id/img_topic/Rcl/Pcl`), 내부파라미터: `cam_0`, `cam_1`, ... 넘버링된 네임스페이스, `XmlRpc::XmlRpcValue`로 파싱 (`LIVMapper.cpp:133-220`) |
| VIO 코어 | `VIOManager`가 `vector<AbstractCamera*> cams`, `vector<M3D> Rci_vec/Rcl_vec/Rcw_vec`, `vector<V3D> Pci_vec/...`, 카메라별 grid/voxel 후보 리스트, `CameraPhotoParams`(카메라별 노출/비네팅) 보유 |
| 조인트 ESIKF | 카메라별 patch residual을 각각 계산한 뒤 **하나의 Jacobian/residual로 병합**해 한 번에 풀이 (`vio.cpp:2429-2717`), 열 구성 `6(pose) + num_cams(노출)`, `addCrossCameraConsistencyConstraint`로 카메라 간 동일 3D점의 광도 일관성 추가 제약 |
| Frame/Feature | `Frame`이 `vector<cams_>`+`vector<SE3> T_f_w_`(카메라별 자세) 보유, `Feature::cam_id_` 태그 추가, `VisualPoint::CrossCameraData`(가시성 비트셋, migration 추적) 신설 |
| 카메라 수 가변성 | 하드코딩 없음. `num_of_cameras = extrin_calib.cameras.size()`. 실사용 예: `avia.yaml`=1대, `Hilti2022.yaml`=3대(4개 중 1개 주석 처리), `NCD4.yaml`=3대 활성, `mid360.yaml`=4대. **카메라 비활성화는 YAML 배열 항목을 주석 처리하는 것만으로 이미 공식적으로 지원되는 방식.** |
| 데이터셋 | `~/Omni-LIVO/README.md` 참조 — LIVOX MID360(10Hz) + ICM40609 IMU(200Hz) + JHEM306GC-HM 카메라 4대(1024×768, 10Hz, 하드웨어 동기화, 십자 배열: Front/Left/Right/Rear). 다운로드: Baidu Netdisk (`README.md:6,53`). LiDAR 토픽 `/livox/lidar` (`lidar_type: 1`, CustomMsg 포맷 — fhd_fast_tri_ws의 `mid360s.yaml`과 동일 컨벤션). Hilti 시퀀스는 40Hz→10Hz 변환 스크립트(`scripts/cvt10hz.py`) 필요하나, **저자 자체 mid360 데이터셋은 이미 10Hz라 변환 불필요**. |
### 2.3 `camera2_ws/src/hik_camera_ros2_driver` (실물 듀얼카메라)
| 항목 | 내용 |
|---|---|
| 노드 구조 | 카메라 1대당 프로세스 1개(동일 노드 클래스), `serial_number`로 물리 장치 매칭 |
| 듀얼 실행 | `launch/hik_camera_dual_launch.py` — 이미 존재, `cam1`/`cam2` 두 프로세스를 별도 파라미터 파일로 기동 |
| 토픽 | `/cam1/image`, `/cam1/camera_info`, `/cam2/image`, `/cam2/camera_info` (`sensor_msgs/Image`, encoding `rgb8` 고정), `frame_id`: `cam1_optical_frame`/`cam2_optical_frame` |
| 하드웨어 트리거 | `TriggerSource`가 `LINE0`으로 하드코딩 (`hik_camera_node.cpp:152`) — 두 카메라 모두 같은 트리거 라인 사용, config로 변경 불가 |
| 타임스탬프 | `use_trigger_timestamp: true`일 때 `livox_ros_driver2`가 쓰는 공유메모리 파일 `/home/<user>/timeshare`의 `low` 필드(LiDAR 패킷 타임스탬프)를 그대로 복사 — **카메라별 개별 트리거 카운터가 아니라 LiDAR 패킷 단위로 양자화된 대략적 시각**임에 유의 |
| ⚠️ 블로커 | **`config/camera_info_cam2.yaml`이 미캘리브레이션 상태** (identity/placeholder 값, 파일 자체 주석으로 명시: "아직 캘리브레이션되지 않음") — FAST-LIVO2류 알고리즘은 정확한 intrinsic/distortion이 없으면 photometric alignment가 근본적으로 깨짐. 실물 하드웨어 통합 전 **반드시 cam2 캘리브레이션 선행 필요** (`direct_visual_lidar_calibration` 사용 가능, cam1에 이미 적용된 이력 있음). |
| ⚠️ 부가 이슈 | `camera_params_cam2.yaml`의 `serial_number: "DB0174264"`와 다른 파일들의 주석/문서에 적힌 `DB074264`가 자리수 불일치 — 배선 전 확인 필요. |
---
## 3. 아키텍처 설계 결정
**결정: Omni-LIVO의 벡터 기반 N-카메라 아키텍처를 구조적으로 그대로 이식한다 (단순화된 "카메라별 독립 실행" 방식은 채택하지 않는다).**
이유:
- 사용자가 명시적으로 "Omni-LIVO의 코드를 참고하여" 확장한다고 했고, 검증도 Omni-LIVO 저자 데이터셋의 4카메라 십자 배열에서 2대만 골라 쓰는 방식으로 하겠다고 했다 — 이는 Omni-LIVO의 **조인트 ESIKF + cross-view migration**이 실제로 동작하는지 확인하겠다는 의도로 해석된다. 카메라별로 완전히 독립적인 EKF를 돌리는 단순화는 Omni-LIVO의 핵심 기여(Cross-View Temporal Migration, Adaptive Multi-View ESIKF)를 재현하지 못한다.
- fhd_fast_tri_ws의 `VoxelMapManager`(LiDAR 평면지도)는 카메라 개수와 무관하게 이미 공유 가능한 구조이므로, `VIOManager`만 확장하면 LiDAR 파이프라인 전체를 재사용할 수 있다.
**파일별 이식 전략**: "새로 작성"이 아니라 "병합"이다. fhd_fast_tri_ws 파일들은 이미 ROS2 API(rclcpp, tf2, livox_ros_driver2)로 되어 있으므로, Omni-LIVO 파일에서 **ROS1 API 호출부만 fhd_fast_tri_ws의 기존 ROS2 대응 코드로 교체**하면서 나머지(멀티카메라 자료구조/알고리즘)는 그대로 가져온다. 이렇게 하면 이미 검증된 ROS2 이식 부분(파라미터 선언, tf2, 노드 라이프사이클)을 재작업하지 않는다.
---
## 4. 파일별 상세 변경 사항
### 4.1 `include/common_lib.h`
- `MeasureGroup::img` (단일 `cv::Mat`) → `std::vector<cv::Mat> imgs` (Omni-LIVO `common_lib.h:61-84` 그대로).
- `StatesGroup`에 카메라별 노출시간 `std::vector<double> inv_expo_time_per_cam` 추가 (고정 `DIM_STATE=19` 외부에 별도 보관, Omni-LIVO 방식 그대로 — EKF 상태 벡터 자체의 차원은 불변으로 유지해 기존 LIO 코드에 영향 없음).
- tf 관련 include는 fhd_fast_tri_ws가 이미 tf2로 되어 있으면 그대로 둔다 (Omni-LIVO의 `#include <tf/transform_broadcaster.h>`는 가져오지 않음).
### 4.2 `include/frame.h` / `src/frame.cpp`
- `vk::AbstractCamera *cam_` → `std::vector<vk::AbstractCamera*> cams_`.
- `SE3 T_f_w_` → `std::vector<SE3> T_f_w_` (+ `T_f_w_prior_`), 카메라별 고정 외부파라미터로 하나의 바디 자세에서 파생.
- `cv::Mat img_` → `std::vector<cv::Mat> imgs_` (+ `imgs_shared_`).
- 생성자 시그니처를 `Frame(const std::vector<AbstractCamera*>&, std::vector<cv::Mat>&, double)`로 변경.
- 신규 `Vector2d w2c(const Vector3d& xyz_w, int cam_id) const` — 카메라 인덱스를 받는 투영 함수.
### 4.3 `include/feature.h`
- `Feature::cam_id_` 필드 추가 — 어떤 카메라에서 추출된 patch인지 태그. (fhd_fast_tri_ws의 `getWarpMatrixAffine`이 이미 `cam`을 파라미터로 받는 구조라 이 필드를 넘겨 받는 방식으로 자연스럽게 연결됨.)
### 4.4 `include/visual_point.h` / `src/visual_point.cpp`
- `CrossCameraData` 구조체 신설: `currently_visible`/`previously_visible` 비트셋(`MAX_CAMERAS=10`, 지오메트리 가정 아님 — 구현상 상한일 뿐), `primary_cam_idx`, `migration_source_cam`, `cross_camera_migrations` 카운터.
- `VisualPoint::obs_` (`list<Feature*>`)는 이미 다중 관측을 지원하는 구조이므로 변경 없음 — 이 부분이 fhd_fast_tri_ws에서 가장 재사용하기 쉬운 기존 자산.
### 4.5 `include/vio.h` / `src/vio.cpp` (**가장 큰 작업, ~2000줄**)
- 단일 멤버 → 벡터 멤버 전면 교체:
- `cam` → `vector<AbstractCamera*> cams`
- `Rci, Pci, Rcl, Pcl, Rcw, Pcw` → `vector<M3D> Rci_vec/Rcl_vec/Rcw_vec`, `vector<V3D> Pci_vec/Pcl_vec/Pcw_vec`
- `fx,fy,cx,cy,width,height` → 카메라별 배열 또는 `cams[i]`에서 직접 조회
- grid 상태(`grid_num, map_index, map_dist, scan_value` 등) → `vector<vector<...>> ..._per_cam_`
- `img_cp, img_rgb` → 카메라별 컨테이너
- `new_frame_`는 단일 `Frame`을 유지하되, `Frame` 자체가 다중 카메라 이미지를 보유하도록 함(4.2 참고)
- `CameraPhotoParams`(카메라별 노출/비네팅 보정) 신설
- `processFrame`을 `processFrame(std::vector<cv::Mat> &imgs, ...)` 시그니처로 변경, 내부에서 카메라별 patch selection → 통합 Jacobian 구성 → `addCrossCameraConsistencyConstraint`(동일 3D점의 카메라 간 광도 일관성 제약) → 1회 EKF 풀이 → 카메라별 참조 patch 갱신 순서로 이식.
- `feat_map`(`unordered_map<VOXEL_LOCATION, VOXEL_POINTS*>`)은 카메라 무관 세계좌표 구조이므로 **그대로 유지** — 이 부분은 Omni-LIVO와 fhd_fast_tri_ws가 사실상 동일해야 함.
- RGB 컬러링(`publish_frame_world` 계열)은 `cams.size()`만큼 순회하며 각 카메라에 투영되는 점을 색칠하는 Omni-LIVO 로직을 채용 (한 점이 2대 카메라에 모두 보이면 중복 append — Omni-LIVO 원 동작 그대로 유지, 병합/평균 로직은 향후 개선 과제로 남김).
### 4.6 `include/LIVMapper.h` / `src/LIVMapper.cpp` (**두 번째로 큰 작업, ~1600줄**)
- `sub_img`(단일) → `vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> sub_img_list`, 각각 `std::bind`로 `cam_id` 캡처(Omni-LIVO의 람다 패턴을 `rclcpp` 구독 생성 구문에 맞게 그대로 적용 — ROS1/ROS2 구독 API 차이만 있을 뿐 로직은 동일).
- `img_buffer`/`img_time_buffer`(단일) → `vector<deque<cv::Mat>> img_buffers`, `vector<deque<double>> img_time_buffers`.
- `img_cbk(msg)` → `img_cbk(msg, cam_id)`.
- `sync_packages()`: Omni-LIVO의 다중 버퍼 정합 로직(모든 카메라 버퍼 non-empty 확인 → `time_tolerance` 비교 → `MeasureGroup::imgs` 채우기)을 그대로 이식. fhd_fast_tri_ws 기존 동기화 로직(뮤텍스/조건변수 기반)의 골격은 유지하고 내부 조건문만 다중화.
- `readParameters()`: 카메라 배열 파싱 로직 교체 — **§5에서 별도 설계** (ROS2 파라미터 서버 한계로 인해 XmlRpc 방식을 그대로 못 가져옴).
- `initializeSubscribersAndPublishers`에서 `image_transport::Publisher`도 카메라별로 필요 시 확장 (디버그/파노라마 퍼블리셔는 Omni-LIVO처럼 `ceil(sqrt(num_cams))` 그리드 모자이크로 구성 가능하나 우선순위 낮음, Phase 5로 미룸).
### 4.7 `rpg_vikit/vikit_ros/src/camera_loader.cpp` / `include/vikit/camera_loader.h`
- **이미 존재하는 벡터 오버로드를 재작성하지 않고 버그만 고쳐서 사용** (완전히 새로 만들 필요 없음, 조사 결과 fhd_fast_tri_ws에 이미 있는 자산):
- 현재 버그: 루프 내 `Pinhole` 분기가 `ns + "/..."`를 읽어야 할 자리에 `cam_ns + "/..."`를 안 쓰는 문제 (§ 2.1 조사 결과) — 각 반복에서 정확히 `cam_ns = ns + "/cam_" + i`를 사용하도록 수정.
- `getParam`/`getRemoteParam` 혼용 문제 정리: fhd_fast_tri_ws의 단일 카메라 로더가 쓰는 `getRemoteParam`(별도 `parameter_blackboard` 노드 대상) 방식으로 통일.
- Omni-LIVO의 `cam_0, cam_1, ...` 넘버링 네임스페이스 컨벤션과 100% 동일하게 맞춰서, intrinsics YAML은 Omni-LIVO 스타일(`config/mid360_cam.yaml`처럼 `cam_0:`, `cam_1:` 최상위 키)을 그대로 재사용 가능하게 한다.
### 4.8 `CMakeLists.txt` / `package.xml`
- `yaml-cpp` 의존성 추가(§5의 직접 YAML 파싱용) — ROS2/Ubuntu에 보통 `libyaml-cpp-dev`로 이미 설치되어 있고, `rclcpp` 자체도 내부적으로 사용하므로 새 시스템 패키지 설치 없이 링크만 추가하면 되는 경우가 많음 (환경에서 확인 필요).
- 변경 불필요: `livox_ros_driver2`(이미 사용 중), `cv_bridge`/`image_transport`(이미 사용 중).
---
## 5. 설정 파일 / launch 구조 재설계
### 5.1 문제
ROS2 `rclcpp::Node::declare_parameter`는 원시 배열(`bool[]`, `int64[]`, `double[]`, `string[]`)만 지원하고, Omni-LIVO가 쓰는 **"구조체의 배열"**(`extrin_calib.cameras: [{cam_id, img_topic, Rcl, Pcl}, ...]`)은 표현할 수 없다. 이는 이 이식 작업에서 가장 파괴적인 ROS1→ROS2 차이점이다.
### 5.2 해결책: 카메라 배열 부분만 `yaml-cpp` 직접 파일 로드
- 메인 config YAML(`mid360s_dual.yaml` 등)의 스칼라/단순배열 파라미터(`common`, `vio`, `lio`, `imu` 등)는 지금처럼 `rclcpp` 파라미터 메커니즘(`ros2 launch`의 `parameters=[yaml_path]`)으로 그대로 로드.
- **`extrin_calib.cameras` 배열만** `LIVMapper`가 노드 파라미터 `camera_config_path`(문자열, YAML 파일 경로)를 받아서 `yaml-cpp`로 **직접 파일을 열어** 파싱한다. 스키마는 Omni-LIVO의 것을 그대로 유지:
```yaml
cameras:
- cam_id: 0
img_topic: "/cam_front"
Rcl: [ ... 9 ... ]
Pcl: [ ... 3 ... ]
- cam_id: 1
img_topic: "/cam_rear"
Rcl: [ ... ]
Pcl: [ ... ]
```
→ `num_of_cameras`는 이 배열의 길이에서 파생 (Omni-LIVO와 동일 철학).
- Intrinsics는 기존 fhd_fast_tri_ws 컨벤션(별도 `parameter_blackboard` 노드 + `camera_*.yaml`)을 유지하되, **카메라 수만큼 네임스페이스를 늘림**: `parameter_blackboard`에 `cam_0/*`, `cam_1/*` 형태로 로드하고 §4.7에서 고친 벡터 로더로 순회 조회.
- 이 방식의 장점: (a) 기존 단일 카메라 config(`mid360s.yaml` 등)와 launch 파일을 전혀 건드리지 않고 그대로 legacy 경로로 남길 수 있음, (b) Omni-LIVO의 YAML 스키마·주석·문서를 거의 그대로 재사용 가능, (c) "카메라 비활성화 = 배열 항목 주석 처리"라는 Omni-LIVO의 검증된 운용 방식을 그대로 물려받음.
### 5.3 신규 launch 파일
- `mapping_mid360s_dualcam.launch.py` (신규) — `parameter_blackboard` 1개(멀티 카메라 intrinsics 전체를 `cam_0/cam_1` 네임스페이스로 로드) + `fastlivo_mapping` 노드 1개(`camera_config_path`로 신규 다중 카메라 extrinsics YAML 지정).
- 기존 `mapping_mid360s_cam{1,2,3}.launch.py`는 **수정하지 않고 그대로 둔다** (단일 카메라 대안 설정으로서 유효, 회귀 없음 보장).
---
## 6. ROS2 이식 시 API 매핑 표
| Omni-LIVO (ROS1) | fhd_fast_tri_ws (ROS2) 대응/조치 |
|---|---|
| `ros::NodeHandle`, `nh.param<T>` | 이미 `rclcpp::Node` + `declare_parameter`/`get_parameter`로 포팅되어 있음 — 신규 멀티카메라 파라미터만 같은 패턴으로 추가 |
| `ros::Subscriber`/`nh.subscribe` (카메라별 람다) | `rclcpp::Node::create_subscription` + `std::bind`, 카메라 인덱스 캡처는 동일 패턴 사용 가능 |
| `XmlRpc::XmlRpcValue` (배열-of-struct 파싱) | **미지원 → §5.2의 `yaml-cpp` 직접 로드로 대체** (가장 중요한 변경점) |
| `tf::TransformBroadcaster`/`tf::Quaternion` | fhd_fast_tri_ws는 이미 tf2 사용 중 — Omni-LIVO 코드 이식 시 해당 라인만 fhd_fast_tri_ws의 기존 tf2 호출로 치환 |
| `livox_ros_driver::CustomMsg` | fhd_fast_tri_ws는 이미 `livox_ros_driver2` 사용 중 — 변경 불필요 |
| `image_transport::ImageTransport` (ROS1) | fhd_fast_tri_ws는 이미 ROS2용 `image_transport` 사용 중 — 카메라별로 인스턴스/퍼블리셔만 늘림 |
| `catkin_package()`, `find_package(catkin ...)` | 해당 없음 (`ament_cmake` 유지) |
| `message_filters` | Omni-LIVO도 안 씀(수동 동기화) → 그대로 수동 동기화 로직 이식, ROS2 `message_filters`로 교체할 필요 없음 |
---
## 7. 실물 하드웨어(camera2_ws) 연동 전 선결 과제
1. **[블로커] cam2 캘리브레이션** — `direct_visual_lidar_calibration`(`~/direct_visual_lidar_calibration`, 이미 설치됨)로 cam1과 동일한 절차 수행 후 `camera_info_cam2.yaml` 갱신. 이게 끝나기 전까지는 실물 2-카메라 통합 테스트가 무의미함 (왜곡 보정이 틀리면 photometric residual 자체가 의미 없음).
2. `camera_params_cam2.yaml`의 `serial_number` 오탈자(`DB0174264` vs 문서상 `DB074264`) 확인 후 실제 장치 시리얼로 통일.
3. `TriggerSource=LINE0` 하드코딩 — 두 카메라를 동일 STM32 트리거 라인에 물릴 것인지, 아니면 드라이버에 `trigger_source` 파라미터를 추가해 분리할 것인지 결정 필요 (현재는 강제로 동일 라인).
4. `timeshare` 공유메모리 타임스탬프가 카메라별이 아니라 전역 1개 파일 기반이므로, 카메라 2대가 완전히 동일한 "가장 최근 LiDAR 패킷 시각"으로 스탬프될 수 있음. Omni-LIVO의 `time_tolerance=0.001s` 정합 로직과 궁합이 맞는지(오히려 지나치게 잘 맞아떨어져서 실제 노출 시각 오차를 못 잡아낼 수 있음) 실물 통합 단계에서 별도 검토.
5. 위 사항들은 **§8 검증(시뮬레이션/데이터셋 기반)과 독립적으로** 병행 진행 가능 — 데이터셋 검증은 이 블로커들과 무관하게 먼저 끝낼 수 있다.
---
## 8. 검증 계획: Omni-LIVO 저자 데이터셋으로 2/4 카메라 검증
### 8.1 데이터 준비
1. Baidu Netdisk에서 `mid360` 계열 시퀀스(4카메라 십자 배열, `/cam_front /cam_left /cam_right /cam_rear`, `/livox/lidar`, `/livox/imu`) 다운로드 (`.bag`, ROS1 형식).
2. **이 머신엔 ROS1이 설치되어 있지 않음** (확인됨: `/opt/ros/`에 `humble`만 존재). `ros1_bridge` 실시간 브리징 대신 **오프라인 변환**을 사용:
- Python 패키지 `rosbags` (Ternaris) 설치 — ROS 설치 없이 순수 파이썬으로 ROS1 bag ↔ ROS2 bag(sqlite3/mcap) 변환 가능.
- 표준 메시지(`sensor_msgs/Image`, `sensor_msgs/Imu`)는 자동 변환됨.
- **Livox `CustomMsg`는 표준 메시지가 아니므로 별도 처리 필요**: `rosbags`의 커스텀 타입스토어에 ROS1 `livox_ros_driver/msg/CustomMsg` 정의(필드: `header, timebase, point_num, lidar_id, rsvd, points[]{offset_time,x,y,z,reflectivity,tag,line}`)를 등록하고, 대상 타입을 fhd_fast_tri_ws가 실제 빌드하는 `livox_ros_driver2/msg/CustomMsg`로 매핑하는 변환 스크립트를 작성한다 (두 메시지는 필드가 동일하므로 1:1 재해석 가능 — 실제 값 변환 로직 불필요, 타입명/패키지명만 재라벨링).
3. 변환된 rosbag2를 `~/bags/` 하위에 저장 (기존 `~/bags` 디렉토리 존재 확인됨).
### 8.2 2-카메라 서브셋 config 작성
- §5.2에서 정의한 신규 `extrin_calib.cameras` YAML을 만들고, 4개 항목 중 2개만 남긴다 (Omni-LIVO의 `NCD1.yaml`/`NCD4.yaml`이 이미 이렇게 일부 카메라를 주석 처리해 운용한 전례를 그대로 따름).
- 권장 조합: **Front + Rear** (서로 반대 방향, 비중첩 FoV 특성과 cross-view migration을 가장 잘 검증할 수 있는 조합) 우선 시도. 필요시 **Front + Left**(인접, 부분 중첩 가능성)로 대조 실험.
- intrinsics는 데이터셋에 동봉된 `mid360_cam.yaml`의 해당 `cam_0`/`cam_2`(혹은 선택한 인덱스) 블록만 사용.
### 8.3 실행 및 성공 기준
- `ros2 bag play`로 변환된 bag 재생 + 신규 `mapping_mid360s_dualcam.launch.py`(카메라 2대 config)로 `fastlivo_mapping` 구동.
- 성공 기준:
1. 크래시/데드락 없이 전체 bag 재생 완료.
2. `/cloud_registered`, `/aft_mapped_to_init` 등 기존 단일 카메라 대비 동일한 위상의 궤적 생성 (LiDAR/IMU 경로는 변경하지 않았으므로 LIO 단독 정확도는 원본과 동일해야 함 — 회귀 여부의 1차 체크포인트).
3. `/rgb_img` 및 저장된 PCD의 RGB 컬러링이 **Front+Rear 두 카메라가 보는 영역 모두**에서 나타나는지 확인 (단일 카메라였다면 한쪽 방향만 컬러링됐을 부분이 이제 양쪽에서 컬러링되어야 함 — 멀티카메라 동작의 가장 직관적인 시각적 증거).
4. (선택, 상급 검증) 두 카메라가 동시에 관측 가능한 3D점이 있는 시퀀스 구간에서 `enable_cross_camera_tracking` on/off 비교로 cross-view migration의 효과(관측 지속성, 드리프트 감소) 정성적 확인.
- 실패 시 디버깅 우선순위: (1) bag 변환/타임스탬프 정합 문제 → (2) `sync_packages()` 다중 버퍼 로직 → (3) `VIOManager` 벡터화 로직의 인덱스 오류 (가장 흔한 버그 유형: 카메라별 벡터 크기 불일치, `cam_id` off-by-one).
### 8.4 이후 (실물 하드웨어 전환)
- §8에서 사용한 것과 동일한 코드/launch 구조에서 이미지 토픽만 `camera2_ws`의 `/cam1/image`, `/cam2/image`로 교체.
- 전제조건: §7의 cam2 캘리브레이션 완료, 그리고 `direct_visual_lidar_calibration`으로 cam1/cam2 각각의 `Rcl`/`Pcl`(LiDAR 대비 외부파라미터) 재산출.
---
## 9. 단계별 로드맵
| Phase | 내용 | 산출물 | 의존성 | 상태 |
|---|---|---|---|---|
| 0 | 본 계획 문서 작성 및 검토 | 이 문서 | - | ✅ 완료 (2026-07-07) |
| 1~3 (재조정) | 카메라 대수와 무관한 자료구조/로더/동기화 스캐폴딩만 우선 구현: `common_lib.h`(`MeasureGroup::imgs` 벡터화), `camera_loader.cpp` 벡터 오버로드 버그 수정, `yaml-cpp` 기반 `extrin_calib.cameras` 배열 직접 로더(`LIVMapper::loadCameraArrayConfig`, 단일카메라 fallback 포함), `LIVMapper.h/.cpp` 멀티토픽 구독(`sub_img_list`)/버퍼(`img_buffers`)/`sync_packages()` 다중화, 신규 `mapping_mid360s_dualcam.launch.py` + `camera_dualcam_cam1_cam2.yaml` + `extrin_dualcam_cam1_cam2.yaml`(cam1+cam2 조합, 기존 calibration 재사용) | `colcon build` 성공 확인. 신규 dualcam launch로 실행 시 `/cam1/image`, `/cam2/image` 양쪽 구독 로그 및 2개 카메라 intrinsics(cam_0/cam_1) 로드 로그 확인, 기존 `mapping_mid360s.launch.py`(단일 카메라) 회귀 없음 확인. **추가로 실데이터 회귀 테스트 완료**: `mapping_mid360s_cam1.launch.py`(`mid360s.yaml`+`extrin_cam1.yaml`+`camera_cam1.yaml`)로 `~/bags/try_2`(37분, `/camera/image`+`/livox/lidar`+`/livox/imu`) 재생 — bag 시작 30초 이전 구간(알려진 불안정 구간)에서는 `corrupted size vs. prev_size`(SIGABRT)로 죽었으나, **`--start-offset 30`으로 재생 시 150초간 크래시/에러 0건, LIO/VIO 사이클 정상 반복(sparse map 33125→122447), `mat_out.txt` 궤적이 매끄럽게 진행(약 7m 이동, NaN/정지 없음)** — 단일 카메라 경로 회귀 없음을 실데이터로 확인. | - | ✅ 완료 (2026-07-07, 실데이터 회귀 테스트 포함). **범위 조정**: `frame.h/.cpp`, `feature.h`, `visual_point.h/.cpp`(cam_id_, CrossCameraData)는 VIOManager(Phase 4)가 실제로 소비하기 전까지는 미사용 코드가 되므로 이번 패스에서 제외하고 Phase 4와 함께 묶어서 진행하기로 함(사용자 확인 완료). `VIOManager`는 아직 완전히 단일 카메라(`vio_manager->cam = cams[0]`)이며, `handleVIO()`도 `imgs[0]`만 소비 — 카메라 1(cam2)은 구독/버퍼링/시간정합까지는 되지만 VIO 갱신에는 아직 반영되지 않음. **미해결 관찰사항**: bag 시작 30초 이전 구간에서 힙 손상(SIGABRT) 발생 — 사용자에 따르면 해당 구간 데이터 자체가 알려진 불안정 구간이라 재현이 Phase 1~3 변경에 의한 회귀인지 원래 있던 이슈인지는 별도 확인 전까지 미확정. |
| 4 | `frame.h/.cpp`, `feature.h`, `visual_point.h/.cpp` 벡터화 + `VIOManager`(`vio.h/.cpp`, 전체 ~2000줄) 완전 벡터화 + 조인트 ESIKF(`updateState`/`updateStateInverse`) + `addCrossCameraConsistencyConstraint` + Adaptive covariance + 메모리 정리(cleanupOldVisualPoints 등) 전부 이식. `common_lib.h`의 `StatesGroup::inv_expo_time`(단일)도 `inv_expo_time_per_cam`(벡터)로 마이그레이션(`IMU_Processing.cpp`, `LIVMapper.cpp` 로깅부 포함 연쇄 수정). `LIVMapper.cpp`의 `setLidarToCameraExtrinsic`/`vio_manager->cams`/`processFrame(imgs)`/RGB 컬러링/`publish_img_rgb`(panorama_image) 전부 다카메라 경로로 연결. Omni-LIVO 자체에 선언만 있고 정의/호출이 전혀 없는 죽은 코드(`resetAfterLoopClosure`, `applyTrajectoryTransformToVisualMap`, `computeCrossCameraWarpMatrix`, `evaluateCrossCameraConsistency`, `extractPatchSafely`, `H_sub_all`/`z_all`, `ideal_total_points`)는 이식하지 않음 | `colcon build` 성공(Eigen/Sophus `Matrix` 이름 충돌 수정 포함 — `common_lib.h`가 의도적으로 `using namespace Eigen`을 안 쓰는 이유였음, `MatrixXd`/`VectorXd`는 `Eigen::` 명시, 템플릿 `Matrix<double,6,6>`은 `MD(6,6)` 매크로로 대체). `~/bags/try_2`(`--start-offset 30`, 150초)로 단일카메라(N=1, 벡터화된 새 코드 경로) 재검증 — 에러 0건, sparse map 정상 성장, 궤적 매끄러움(NaN/Inf 없음) 확인. 신규 dualcam launch로 N=2 카메라 초기화·구독 스모크테스트(`/cam1/image`, `/cam2/image` 둘 다 구독, cam_0/cam_1 intrinsics 로드, "Initialized 2 camera exposure parameters" 등 정상 출력, 크래시 없음) 확인 — 단, 실제 두 카메라 영상 데이터로 VIO 전체 파이프라인을 끝까지 돌려보는 검증은 아직 못 함(2카메라 동시 bag/실물 데이터 없음, §8 데이터셋 준비와 함께 진행 예정) | Phase 1~3 | ✅ 완료 (2026-07-07). **알려진 제약**: `initializeVIO()`가 `width/height`를 `cams[0]`에서만 가져와 전 카메라에 재사용(Omni-LIVO 자체 설계) — 카메라 간 처리 해상도가 다르면(현재 dualcam config: cam1=1440x1080, cam2=720x540) `getImagePatch` 등에서 버퍼 오상=인덱싱 위험. §11 리스크에 추가. |
| 5 | 기존 launch 회귀 재확인 (Phase 4 반영 후) | - | Phase 4 | ✅ 완료 (단일카메라 launch 회귀 재검증으로 겸함) |
| 6 | §8 데이터셋 기반 검증 (bag 변환 → 2카메라 서브셋 실행 → 성공기준 확인). **업데이트**: Omni-LIVO 저자 데이터셋(Baidu Netdisk) 다운로드가 어려워, 사용자가 KITTI 등 대체 데이터셋을 검토 중 — 포팅(Phase 4) 완료 후 데이터셋 준비를 진행하기로 함. KITTI는 전방 스테레오 2대 구성으로 Omni-LIVO의 비중첩 십자 배열과 다르므로, cross-view migration 검증에는 적합하지 않을 수 있음(단순 2카메라 동시 처리 검증에는 사용 가능) — 데이터셋 확정 시 재검토 필요 | 검증 리포트 (궤적/컬러링 비교 스크린샷 or rosbag 지표) | Phase 4, 5 | 대기 |
| 7 | (후속) 실물 `camera2_ws` 연동 — cam2 캘리브레이션 선행 후 진행 | - | §7 블로커 해소, Phase 6 | 대기 |
---
## 10. 리스크 및 미결 사항
- ~~**[High] `vio.cpp` 조인트 ESIKF 이식은 단순 API 치환이 아니라 알고리즘 이식**이라 버그 위험이 크다~~ → Phase 4에서 완료. 실데이터(try_2 bag, N=1)로 150초간 크래시/에러 없이 정상 동작 확인. **2026-07-09 N=2 실물 하드웨어(cam1+cam2, 정지 상태)로 25초간 검증 완료** — 처음 시도에서 두 가지 실제 버그가 드러나 수정함: (1) Omni-LIVO 원본에도 있던 `resetGrid()` 미호출로 인한 SIGSEGV(+ grid 버퍼가 멀티카메라 크기로 안 잡히던 부수 결함), (2) `last_timestamp_img`를 카메라들이 공유해서 서로를 "시간 역행"으로 오판해 이미지가 아예 안 쌓이던 버그. 두 버그 수정 후 크래시 0, VIO 사이클 238회 정상 완료, sparse map 정상 성장, 궤적 NaN/Inf 없음 확인. **다만 정지 상태 테스트라 실제 이동 시 궤적/매핑 품질은 아직 미검증** — 다음 단계로 남음.
- **[High, 신규] 카메라 간 해상도 불일치** — Omni-LIVO의 `VIOManager::initializeVIO()`는 `width`/`height`를 `cams[0]`에서만 가져와 `getImagePatch` 등 전 카메라 이미지 버퍼 인덱싱에 그대로 재사용한다(Omni-LIVO 자체의 설계 제약이며 이번 포팅에서 새로 만든 문제는 아님). 현재 `camera_dualcam_cam1_cam2.yaml`은 cam1=1440x1080, cam2=720x540로 해상도가 다르므로, 실제 두 카메라 이미지가 함께 처리되는 순간(cam_id=1, 즉 cam2 처리 시) `width`(=1440, cam1 기준)로 720폭 이미지 버퍼를 인덱싱해 메모리 오상=크래시/오염 가능성이 있다. **실물 하드웨어 검증(§7/§8) 전에 반드시 해결**해야 함 — 옵션: (a) cam2를 cam1과 동일 해상도로 처리하도록 config 조정, (b) cam1을 720x540로 다운스케일, (c) `width`/`height`를 카메라별로 쓰도록 코드 일반화(Omni-LIVO 원본과 달라짐, 신중히 검토 필요). 초기화/구독만 하는 스모크테스트에서는 이미지가 실제로 안 들어오므로 드러나지 않았음.
- **[Medium] Livox CustomMsg 변환**이 `rosbags` 라이브러리로 매끄럽게 될지 사전 검증 필요 (커스텀 메시지 등록 방식이 버전마다 API가 다를 수 있음). 실패 시 대안: Docker로 ROS1 Noetic 컨테이너를 띄우고 그 안에서 `rosbag`→ CSV/PCD/PNG로 원시 추출 후 ROS2 쪽에서 직접 퍼블리셔 스크립트로 재생하는 방법도 있음(더 번거롭지만 확실함).
- **[결정 필요] 카메라 2대 선택 조합** — Front+Rear(대향) vs Front+Left(인접) 중 어느 쪽을 1차 검증 기준으로 삼을지 §8.2에서 Front+Rear를 기본값으로 제안했으나, 데이터셋의 실제 시퀀스 내용(어느 방향에 특징점이 풍부한지)에 따라 조정 가능.
- **[참고] Phase 4에서 Omni-LIVO의 `updateState`(순방향 조인트 ESIKF)는 원본 FAST-LIVO2와 달리 6-DOF 자세 보정만 `state->cov` 자세 블록으로 풀고(`K_pose`), velocity/bias/gravity는 이 EKF 업데이트에서 직접 갱신하지 않는다(원본은 `G` 풀-스테이트 게인으로 전체 상태에 보정을 전파). 이는 Omni-LIVO 원본 그대로 이식한 의도적 차이이며, 실데이터 회귀 테스트에서 궤적이 기존과 아주 근접하되 완전히 동일하지는 않은 이유이기도 함(정상 동작 확인됨, 다만 참고차 기록).
---
## 11. 핵심 참고 파일 인덱스
```
[포팅 대상 - fhd_fast_tri_ws]
src/FAST-LIVO2/include/common_lib.h MeasureGroup, StatesGroup
src/FAST-LIVO2/include/vio.h / src/vio.cpp VIOManager (최대 작업량)
src/FAST-LIVO2/include/LIVMapper.h / src/LIVMapper.cpp 노드/토픽/동기화
src/FAST-LIVO2/include/frame.h / src/frame.cpp
src/FAST-LIVO2/include/feature.h
src/FAST-LIVO2/include/visual_point.h / src/visual_point.cpp
src/rpg_vikit/vikit_ros/src/camera_loader.cpp 기존 버그있는 벡터 로더
src/rpg_vikit/vikit_ros/include/vikit/camera_loader.h
src/FAST-LIVO2/config/extrin_cam{1,2,3}.yaml 기존 단일교체형 스캐폴딩(참고용, 유지)
src/FAST-LIVO2/launch/mapping_mid360s_cam{1,2,3}.launch.py (참고용, 유지)
[참조 구현 - Omni-LIVO]
Omni-LIVO/include/vio.h / src/vio.cpp 벡터화 VIOManager, 조인트 ESIKF, cross-camera 제약
Omni-LIVO/include/LIVMapper.h / src/LIVMapper.cpp 멀티토픽 구독/동기화/XmlRpc 파싱
Omni-LIVO/include/frame.h / src/frame.cpp
Omni-LIVO/include/feature.h
Omni-LIVO/include/visual_point.h / src/visual_point.cpp CrossCameraData
Omni-LIVO/config/mid360.yaml / mid360_cam.yaml 4카메라 config 스키마 예시
Omni-LIVO/config/NCD1.yaml / NCD4.yaml 카메라 일부 비활성화(주석처리) 실례
Omni-LIVO/README.md 데이터셋 다운로드/센서 스펙
[실물 카메라 드라이버 - camera2_ws]
src/hik_camera_ros2_driver/src/hik_camera_node.cpp
src/hik_camera_ros2_driver/launch/hik_camera_dual_launch.py
src/hik_camera_ros2_driver/config/camera_params_cam{1,2}.yaml
src/hik_camera_ros2_driver/config/camera_info_cam{1,2}.yaml (cam2 미캘리브레이션 확인)
```
docs/SCAN-운용가이드.md
+120
View File
@@ -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 <PID>
```
### 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 융합을 합치려면 두 소스를
비교해 병합하는 별도 작업이 필요하다 — 착수 시 다시 요청.
src/FAST-LIVO2/.gitignore
+2
View File
@@ -0,0 +1,2 @@
Log/*
build/*
src/FAST-LIVO2/CMakeLists.txt
Executable
+218
View File
@@ -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()
src/FAST-LIVO2/LICENSE
+339
View File
@@ -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.
<one line to give the program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
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.
<signature of Ty Coon>, 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.
src/FAST-LIVO2/README.md
+195
View File
@@ -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)
<div align="center">
<img src="pics/Framework.png" width = 100% >
</div>
### 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 <zhengcr@connect.hku.hk> and Prof. Fu Zhang at <fuzhang@hku.hk> to discuss an alternative license.
src/FAST-LIVO2/Supplementary/LIVO2_supplementary.pdf
BIN
View File
Binary file not shown.
src/FAST-LIVO2/config/HILTI22.yaml
+100
View File
@@ -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
src/FAST-LIVO2/config/MARS_LVIG.yaml
+118
View File
@@ -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
src/FAST-LIVO2/config/NTU_VIRAL.yaml
+94
View File
@@ -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
src/FAST-LIVO2/config/avia.yaml
Executable
+94
View File
@@ -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
src/FAST-LIVO2/config/camera_MARS_LVIG.yaml
+28
View File
@@ -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
src/FAST-LIVO2/config/camera_NTU_VIRAL.yaml
+14
View File
@@ -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
src/FAST-LIVO2/config/camera_cam1.yaml
+79
View File
@@ -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
src/FAST-LIVO2/config/camera_cam2.yaml
+43
View File
@@ -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
src/FAST-LIVO2/config/camera_cam3.yaml
+53
View File
@@ -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
src/FAST-LIVO2/config/camera_dualcam_cam1_cam2.yaml
+61
View File
@@ -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_<i>." 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
src/FAST-LIVO2/config/camera_fisheye_HILTI22.yaml
+12
View File
@@ -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
src/FAST-LIVO2/config/camera_mid360s.yaml
+37
View File
@@ -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
src/FAST-LIVO2/config/camera_pinhole.yaml
+42
View File
@@ -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
src/FAST-LIVO2/config/camera_triplecam_cam1_cam2_cam3.yaml
+85
View File
@@ -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_<i>." 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
src/FAST-LIVO2/config/extrin_cam1.yaml
+46
View File
@@ -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]
src/FAST-LIVO2/config/extrin_cam2.yaml
+18
View File
@@ -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]
src/FAST-LIVO2/config/extrin_cam3.yaml
+11
View File
@@ -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]
src/FAST-LIVO2/config/extrin_dualcam_cam1_cam2.yaml
+50
View File
@@ -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]
src/FAST-LIVO2/config/extrin_triplecam_cam1_cam2_cam3.yaml
+63
View File
@@ -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]
src/FAST-LIVO2/config/mid360s.yaml
+243
View File
@@ -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
src/FAST-LIVO2/include/IMU_Processing.h
+91
View File
@@ -0,0 +1,91 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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 <Eigen/Eigen>
#include <fstream>
#include "common_lib.h"
#include <condition_variable>
#include <nav_msgs/msg/odometry.hpp>
#include <utils/so3_math.h>
#include <fstream>
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<Pose6D> 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<ImuProcess> ImuProcessPtr;
#endif
src/FAST-LIVO2/include/LIVMapper.h
+232
View File
@@ -0,0 +1,232 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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(<cv_bridge/cv_bridge.hpp>)
#include <cv_bridge/cv_bridge.hpp>
#else
#include <cv_bridge/cv_bridge.h>
#endif
#include <image_transport/image_transport.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/path.hpp>
#include <vikit/camera_loader.h>
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<sensor_msgs::msg::PointCloud2>::SharedPtr &pubLaserCloudFullRes, VIOManagerPtr vio_manager);
void publish_visual_sub_map(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubSubVisualMap);
void publish_effect_world(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubLaserCloudEffect, const std::vector<PointToPlane> &ptpl_list);
void publish_odometry(const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr &pmavros_pose_publisherubOdomAftMapped);
void publish_mavros(const rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr &mavros_pose_publisher);
void publish_path(const rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr &pubPath);
void readParameters(rclcpp::Node::SharedPtr &node);
template <typename T> void set_posestamp(T &out);
template <typename T> void pointBodyToWorld(const Eigen::Matrix<T, 3, 1> &pi, Eigen::Matrix<T, 3, 1> &po);
template <typename T> Eigen::Matrix<T, 3, 1> pointBodyToWorld(const Eigen::Matrix<T, 3, 1> &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_LOCATION, VoxelOctoTree *> 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<std::string> camera_img_topics;
std::vector<std::vector<double>> camera_extrin_Rs;
std::vector<std::vector<double>> 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<double> 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<sensor_msgs::msg::Imu> prop_imu_buffer;
sensor_msgs::msg::Imu newest_imu;
double latest_ekf_time;
nav_msgs::msg::Odometry imu_prop_odom;
rclcpp::Publisher<nav_msgs::msg::Odometry>::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<PointCloudXYZI::Ptr> lid_raw_data_buffer;
deque<double> lid_header_time_buffer;
deque<sensor_msgs::msg::Imu::ConstSharedPtr> imu_buffer;
std::vector<deque<cv::Mat>> img_buffers;
std::vector<deque<double>> img_time_buffers;
vector<pointWithVar> _pv_list;
vector<double> extrinT;
vector<double> extrinR;
vector<double> cameraextrinT;
vector<double> 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<PointType> 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<visualization_msgs::msg::Marker>::SharedPtr plane_pub;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr voxel_pub;
std::shared_ptr<rclcpp::SubscriptionBase> sub_pcl;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> sub_img_list;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFullRes;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pubNormal;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubSubVisualMap;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudEffect;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudMap;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudDyn;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudDynRmed;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudDynDbg;
image_transport::Publisher pubImage;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::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
src/FAST-LIVO2/include/common_lib.h
Executable
+251
View File
@@ -0,0 +1,251 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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 <utils/so3_math.h>
#include <utils/types.h>
#include <utils/color.h>
#include <utils/utils.h>
#include <opencv2/opencv.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sophus/se3.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Transform.hpp>
#include <tf2/LinearMath/Quaternion.hpp>
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<sensor_msgs::msg::Imu::ConstSharedPtr> imu;
std::vector<cv::Mat> 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<struct MeasureGroup> 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<double, DIM_STATE, 1> &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<double, DIM_STATE, 1> &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<double, DIM_STATE, 1> operator-(const StatesGroup &b)
{
Matrix<double, DIM_STATE, 1> 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<double> 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<double, DIM_STATE, DIM_STATE> cov; //!< states covariance
};
template <typename T>
auto set_pose6d(const double t, const Matrix<T, 3, 1> &a, const Matrix<T, 3, 1> &g, const Matrix<T, 3, 1> &v, const Matrix<T, 3, 1> &p,
const Matrix<T, 3, 3> &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<M3D>(rot_kp.rot, 3,3) = R;
return move(rot_kp);
}
#endif
src/FAST-LIVO2/include/feature.h
+57
View File
@@ -0,0 +1,57 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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<double> 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<double> &_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_
src/FAST-LIVO2/include/frame.h
+83
View File
@@ -0,0 +1,83 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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 <boost/noncopyable.hpp>
#include <vikit/abstract_camera.h>
class VisualPoint;
struct Feature;
typedef list<Feature *> Features;
typedef vector<cv::Mat> 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<vk::AbstractCamera *> cams_; //!< Camera models, one per camera.
std::vector<SE3<double>> T_f_w_; //!< Transform (f)rame from (w)orld, one per camera.
std::vector<SE3<double>> T_f_w_prior_; //!< Transform (f)rame from (w)orld provided by the IMU prior, one per camera.
std::vector<std::shared_ptr<cv::Mat>> imgs_shared_; //!< Shared ownership of each camera's image.
std::vector<cv::Mat> 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<vk::AbstractCamera *> &cams, std::vector<cv::Mat> &imgs, double timestamp);
~Frame();
/// Initialize new frame and create image pyramid.
void initFrame(std::vector<cv::Mat> &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<Frame> 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_
src/FAST-LIVO2/include/livox_ros_driver/CustomMsg.h
+292
View File
@@ -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 <string>
#include <vector>
#include <memory>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <std_msgs/Header.h>
#include <livox_ros_driver/CustomPoint.h>
namespace livox_ros_driver
{
template <class ContainerAllocator>
struct CustomMsg_
{
typedef CustomMsg_<ContainerAllocator> 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_<ContainerAllocator> _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<uint8_t, 3> _rsvd_type;
_rsvd_type rsvd;
typedef std::vector< ::livox_ros_driver::CustomPoint_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >> _points_type;
_points_type points;
typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg_<ContainerAllocator> const> ConstPtr;
}; // struct CustomMsg_
typedef ::livox_ros_driver::CustomMsg_<std::allocator<void> > 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<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::livox_ros_driver::CustomMsg_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::livox_ros_driver::CustomMsg_<ContainerAllocator1> & lhs, const ::livox_ros_driver::CustomMsg_<ContainerAllocator2> & 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::livox_ros_driver::CustomMsg_<ContainerAllocator1> & lhs, const ::livox_ros_driver::CustomMsg_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace livox_ros_driver
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsMessage< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::livox_ros_driver::CustomMsg_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::livox_ros_driver::CustomMsg_<ContainerAllocator> const>
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::livox_ros_driver::CustomMsg_<ContainerAllocator> const>
: TrueType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
{
static const char* value()
{
return "e4d6829bdfe657cb6c21a746c86b21a6";
}
static const char* value(const ::livox_ros_driver::CustomMsg_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xe4d6829bdfe657cbULL;
static const uint64_t static_value2 = 0x6c21a746c86b21a6ULL;
};
template<class ContainerAllocator>
struct DataType< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
{
static const char* value()
{
return "livox_ros_driver/CustomMsg";
}
static const char* value(const ::livox_ros_driver::CustomMsg_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
{
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_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
{
template<typename Stream, typename T> 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<class ContainerAllocator>
struct Printer< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::livox_ros_driver::CustomMsg_<ContainerAllocator>& v)
{
s << indent << "header: ";
s << std::endl;
Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + " ", v.header);
s << indent << "timebase: ";
Printer<uint64_t>::stream(s, indent + " ", v.timebase);
s << indent << "point_num: ";
Printer<uint32_t>::stream(s, indent + " ", v.point_num);
s << indent << "lidar_id: ";
Printer<uint8_t>::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<uint8_t>::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_<ContainerAllocator> >::stream(s, indent + " ", v.points[i]);
}
}
};
} // namespace message_operations
} // namespace ros
#endif // LIVOX_ROS_DRIVER_MESSAGE_CUSTOMMSG_H
src/FAST-LIVO2/include/livox_ros_driver/CustomPoint.h
+258
View File
@@ -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 <string>
#include <vector>
#include <memory>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
namespace livox_ros_driver
{
template <class ContainerAllocator>
struct CustomPoint_
{
typedef CustomPoint_<ContainerAllocator> 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_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint_<ContainerAllocator> const> ConstPtr;
}; // struct CustomPoint_
typedef ::livox_ros_driver::CustomPoint_<std::allocator<void> > 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<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::livox_ros_driver::CustomPoint_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::livox_ros_driver::CustomPoint_<ContainerAllocator1> & lhs, const ::livox_ros_driver::CustomPoint_<ContainerAllocator2> & 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::livox_ros_driver::CustomPoint_<ContainerAllocator1> & lhs, const ::livox_ros_driver::CustomPoint_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace livox_ros_driver
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsMessage< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::livox_ros_driver::CustomPoint_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::livox_ros_driver::CustomPoint_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::livox_ros_driver::CustomPoint_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
{
static const char* value()
{
return "109a3cc548bb1f96626be89a5008bd6d";
}
static const char* value(const ::livox_ros_driver::CustomPoint_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x109a3cc548bb1f96ULL;
static const uint64_t static_value2 = 0x626be89a5008bd6dULL;
};
template<class ContainerAllocator>
struct DataType< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
{
static const char* value()
{
return "livox_ros_driver/CustomPoint";
}
static const char* value(const ::livox_ros_driver::CustomPoint_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
{
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_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
{
template<typename Stream, typename T> 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<class ContainerAllocator>
struct Printer< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::livox_ros_driver::CustomPoint_<ContainerAllocator>& v)
{
s << indent << "offset_time: ";
Printer<uint32_t>::stream(s, indent + " ", v.offset_time);
s << indent << "x: ";
Printer<float>::stream(s, indent + " ", v.x);
s << indent << "y: ";
Printer<float>::stream(s, indent + " ", v.y);
s << indent << "z: ";
Printer<float>::stream(s, indent + " ", v.z);
s << indent << "reflectivity: ";
Printer<uint8_t>::stream(s, indent + " ", v.reflectivity);
s << indent << "tag: ";
Printer<uint8_t>::stream(s, indent + " ", v.tag);
s << indent << "line: ";
Printer<uint8_t>::stream(s, indent + " ", v.line);
}
};
} // namespace message_operations
} // namespace ros
#endif // LIVOX_ROS_DRIVER_MESSAGE_CUSTOMPOINT_H
src/FAST-LIVO2/include/preprocess.h
Executable
+200
View File
@@ -0,0 +1,200 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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 <livox_ros_driver2/msg/custom_msg.hpp>
#include <pcl_conversions/pcl_conversions.h>
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<orgtype> 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<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> pub_full;
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> pub_surf;
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> 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<orgtype> &types);
void pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct);
int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);
bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &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<Preprocess> PreprocessPtr;
#endif // PREPROCESS_H_
src/FAST-LIVO2/include/utils/color.h
+24
View File
@@ -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
src/FAST-LIVO2/include/utils/so3_math.h
Executable
+89
View File
@@ -0,0 +1,89 @@
#ifndef SO3_MATH_H
#define SO3_MATH_H
#include <Eigen/Core>
#include <math.h>
#define SKEW_SYM_MATRX(v) 0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0
template <typename T> Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &&ang)
{
T ang_norm = ang.norm();
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
if (ang_norm > 0.0000001)
{
Eigen::Matrix<T, 3, 1> r_axis = ang / ang_norm;
Eigen::Matrix<T, 3, 3> 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 <typename T, typename Ts> Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang_vel, const Ts &dt)
{
T ang_vel_norm = ang_vel.norm();
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
if (ang_vel_norm > 0.0000001)
{
Eigen::Matrix<T, 3, 1> r_axis = ang_vel / ang_vel_norm;
Eigen::Matrix<T, 3, 3> 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 <typename T> Eigen::Matrix<T, 3, 3> Exp(const T &v1, const T &v2, const T &v3)
{
T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
if (norm > 0.00001)
{
T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};
Eigen::Matrix<T, 3, 3> 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 <typename T> Eigen::Matrix<T, 3, 1> Log(const Eigen::Matrix<T, 3, 3> &R)
{
T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1));
Eigen::Matrix<T, 3, 1> 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 <typename T> Eigen::Matrix<T, 3, 1> RotMtoEuler(const Eigen::Matrix<T, 3, 3> &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<T, 3, 1> ang(x, y, z);
return ang;
}
#endif
src/FAST-LIVO2/include/utils/types.h
+39
View File
@@ -0,0 +1,39 @@
#ifndef TYPES_H
#define TYPES_H
#include <Eigen/Eigen>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointXYZRGB PointTypeRGB;
typedef pcl::PointXYZRGBA PointTypeRGBA;
typedef pcl::PointCloud<PointType> PointCloudXYZI;
typedef std::vector<PointType, Eigen::aligned_allocator<PointType>> PointVector;
typedef pcl::PointCloud<PointTypeRGB> PointCloudXYZRGB;
typedef pcl::PointCloud<PointTypeRGBA> 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<double, (a), (b)>
#define VD(a) Eigen::Matrix<double, (a), 1>
#define MF(a, b) Eigen::Matrix<float, (a), (b)>
#define VF(a) Eigen::Matrix<float, (a), 1>
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
src/FAST-LIVO2/include/utils/utils.h
+76
View File
@@ -0,0 +1,76 @@
#ifndef UTILS_H
#define UTILS_H
#include <vector>
#include <cstdint> // for int64_t
#include <limits> // for std::numeric_limits
#include <stdexcept> // for std::out_of_range
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/LinearMath/Quaternion.h>
std::vector<int> convertToIntVectorSafe(const std::vector<int64_t>& 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
src/FAST-LIVO2/include/vio.h
Executable
+285
View File
@@ -0,0 +1,285 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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 <opencv2/imgproc/imgproc_c.h>
#include <pcl/filters/voxel_grid.h>
#include <set>
#include <sstream>
#include <iomanip>
#include <vikit/math_utils.h>
#include <vikit/robust_cost.h>
#include <vikit/vision.h>
#include <vikit/pinhole_camera.h>
#include <unordered_set>
#include <deque>
struct SubSparseMap
{
vector<float> propa_errors;
vector<float> errors;
vector<vector<float>> warp_patch;
vector<int> search_levels;
vector<VisualPoint *> voxel_points;
vector<double> inv_expo_list;
vector<pointWithVar> add_from_voxel_map;
vector<int> 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<VisualPoint *> 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<vk::AbstractCamera *> cams; //!< one camera model per camera, indexed by cam_id
StatesGroup *state;
StatesGroup *state_propagat;
bool raycast_en = false;
std::vector<std::vector<std::vector<V3D>>> rays_with_sample_points; // [cam_id][grid_idx][sample_points]
std::vector<std::vector<int>> 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<Eigen::Matrix3d> Rci_vec;
std::vector<Eigen::Matrix3d> Rcl_vec;
std::vector<Eigen::Matrix3d> Rcw_vec;
std::vector<Eigen::Vector3d> Pci_vec;
std::vector<Eigen::Vector3d> Pcl_vec;
std::vector<Eigen::Vector3d> Pcw_vec;
// Precomputed per-camera Jacobian factors (only depend on the fixed IMU->camera extrinsic).
std::vector<Eigen::Matrix3d> Jdphi_dR_vec;
std::vector<Eigen::Matrix3d> Jdp_dt_vec;
std::vector<Eigen::Matrix3d> Jdp_dR_vec;
M3D Rli; // IMU->LiDAR rotation (camera-independent)
V3D Pli; // IMU->LiDAR translation (camera-independent)
vector<int> grid_num;
vector<int> map_index;
vector<int> update_flag;
vector<float> map_dist;
vector<float> scan_value;
vector<float> 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<std::vector<int>> grid_num_per_cam_;
std::vector<std::vector<float>> map_dist_per_cam_;
std::vector<std::vector<VisualPoint*>> retrieve_voxel_points_per_cam_;
std::vector<std::vector<int>> scan_grid_num_per_cam_;
std::vector<std::vector<float>> scan_value_per_cam_;
std::vector<std::vector<pointWithVar>> scan_append_points_per_cam_;
std::vector<std::unordered_set<VisualPoint*>> retrieve_voxel_points_list_buffer_;
std::vector<cv::Mat> depth_imgs_buffer_;
std::vector<int> grid_num_buffer_;
std::vector<float> map_dist_buffer_;
std::vector<VisualPoint*> retrieve_voxel_points_buffer_;
std::vector<float> 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<double, DIM_STATE, DIM_STATE> G, H_T_H;
Eigen::MatrixXd K, H_sub_inv;
ofstream fout_camera, fout_colmap;
unordered_map<VOXEL_LOCATION, VOXEL_POINTS *> feat_map;
unordered_map<VOXEL_LOCATION, int> sub_feat_map;
unordered_map<int, Warp *> warp_map;
vector<VisualPoint *> retrieve_voxel_points;
vector<pointWithVar> append_voxel_points;
FramePtr new_frame_;
std::vector<cv::Mat> 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<double> vignetting;
bool parameters_initialized;
};
std::vector<CameraPhotoParams> 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<cv::Mat> &imgs, int level);
void updateState(const std::vector<cv::Mat> &imgs, int level);
void processFrame(const std::vector<cv::Mat> &imgs, vector<pointWithVar> &pg, const unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &feat_map, double frame_timestamp);
void retrieveFromVisualSparseMap(const std::vector<cv::Mat> imgs, vector<pointWithVar> &pg, const unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &plane_map);
void generateVisualMapPoints(const std::vector<cv::Mat> &imgs, vector<pointWithVar> &pg);
void setImuToLidarExtrinsic(const V3D &transl, const M3D &rot);
void setLidarToCameraExtrinsic(std::vector<std::vector<double>> &R, std::vector<std::vector<double>> &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<cv::Mat> imgs);
void resetGrid();
void updateVisualMapPoints(std::vector<cv::Mat> &imgs);
void getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, const SE3<double> &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<double> &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<VOXEL_LOCATION, VoxelOctoTree *> &plane_map);
void updateReferencePatch(const unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &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<FramePtr> 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<double> prev_cov_scale_per_cam_;
std::vector<double> prev_avg_error_per_cam_;
std::vector<int> 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<VIOManager> VIOManagerPtr;
#endif // VIO_H_
src/FAST-LIVO2/include/visual_point.h
+88
View File
@@ -0,0 +1,88 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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 <boost/noncopyable.hpp>
#include <bitset>
#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<MAX_CAMERAS> currently_visible; // visibility in the current frame
std::bitset<MAX_CAMERAS> 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<Feature *> 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<MAX_CAMERAS> 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_
src/FAST-LIVO2/include/voxel_map.h
+259
View File
@@ -0,0 +1,259 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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 <Eigen/Dense>
#include <fstream>
#include <math.h>
#include <mutex>
#include <omp.h>
#include <pcl/common/io.h>
#include <rclcpp/rclcpp.hpp>
#include <thread>
#include <unistd.h>
#include <unordered_map>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#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<int64_t> 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<double, 6, 6> 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<double, 6, 6> 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<double, 6, 6>::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<VOXEL_LOCATION>
{
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<pointWithVar> 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<int> 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<pointWithVar> &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<visualization_msgs::msg::MarkerArray>::SharedPtr voxel_map_pub_;
std::unordered_map<VOXEL_LOCATION, VoxelOctoTree *> 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<M3D> cross_mat_list_;
std::vector<M3D> body_cov_list_;
std::vector<pointWithVar> pv_list_;
std::vector<PointToPlane> ptpl_list_;
VoxelMapManager(VoxelMapConfig &config_setting, std::unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &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<pcl::PointXYZI>::Ptr &trans_cloud);
void BuildVoxelMap();
V3F RGBFromVoxel(const V3D &input_point);
void UpdateVoxelMap(const std::vector<pointWithVar> &input_points);
void BuildResidualListOMP(std::vector<pointWithVar> &pv_list, std::vector<PointToPlane> &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<VoxelPlane> &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<VoxelMapManager> VoxelMapManagerPtr;
#endif // VOXEL_MAP_H_
src/FAST-LIVO2/launch/mapping_avia_marslvig.launch.py
Executable
+111
View File
@@ -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"
),
])
src/FAST-LIVO2/launch/mapping_aviz.launch.py
+117
View File
@@ -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"
),
])
src/FAST-LIVO2/launch/mapping_hesaixt32_hilti22.launch
+14
View File
@@ -0,0 +1,14 @@
<launch>
<arg name="rviz" default="true" />
<rosparam command="load" file="$(find fast_livo)/config/HILTI22.yaml" />
<node pkg="fast_livo" type="fastlivo_mapping" name="laserMapping" output="screen">
<rosparam file="$(find fast_livo)/config/camera_fisheye_HILTI22.yaml" />
</node>
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_livo)/rviz_cfg/hilti.rviz" />
</group>
</launch>
src/FAST-LIVO2/launch/mapping_mid360s.launch.py
+99
View File
@@ -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"
),
])
src/FAST-LIVO2/launch/mapping_mid360s_cam1.launch.py
+94
View File
@@ -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"
),
])
src/FAST-LIVO2/launch/mapping_mid360s_cam2.launch.py
+94
View File
@@ -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"
),
])
src/FAST-LIVO2/launch/mapping_mid360s_cam3.launch.py
+107
View File
@@ -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"
),
])
src/FAST-LIVO2/launch/mapping_mid360s_dualcam.launch.py
+110
View File
@@ -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"
),
])
src/FAST-LIVO2/launch/mapping_mid360s_triplecam.launch.py
+105
View File
@@ -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"
),
])
src/FAST-LIVO2/launch/mapping_ouster_ntu.launch.py
+117
View File
@@ -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"
),
])
src/FAST-LIVO2/package.xml
Executable
+59
View File
@@ -0,0 +1,59 @@
<?xml version="1.0"?>
<package format="3">
<name>fast_livo</name>
<version>0.0.0</version>
<description>
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.
</description>
<maintainer email="dev@livoxtech.com">claydergc</maintainer>
<license>BSD</license>
<author email="zhangji@cmu.edu">Ji Zhang</author>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rclcpp</build_depend>
<build_depend>rclpy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf2_ros</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>pcl_conversions</build_depend>
<build_depend>livox_ros_driver2</build_depend>
<build_depend>vikit_common</build_depend>
<build_depend>vikit_ros</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>libopencv-dev</build_depend>
<build_depend>sophus</build_depend>
<build_depend>eigen</build_depend>
<build_depend>fmt</build_depend>
<build_depend>yaml-cpp</build_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>image_transport</exec_depend>
<exec_depend>libopencv-dev</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>yaml-cpp</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
<!-- https://github.com/ros-perception/image_transport_tutorials/blob/humble/package.xml -->
<!-- <image_transport plugin="${prefix}/resized_plugins.xml"/> -->
</export>
</package>
src/FAST-LIVO2/pics/Framework.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.6 MiB

src/FAST-LIVO2/pics/debug_error.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 143 KiB

src/FAST-LIVO2/pics/rosgraph.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 123 KiB

src/FAST-LIVO2/rviz_cfg/M300.rviz
Executable
+671
View File
@@ -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: <Fixed Frame>
Value: false
- Alpha: 1
Class: rviz_default_plugins/Axes
Enabled: true
Length: 4
Name: Axes
Radius: 1.2000000476837158
Reference Frame: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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
src/FAST-LIVO2/rviz_cfg/fast_livo2.rviz
Executable
+782
View File
@@ -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: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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
src/FAST-LIVO2/rviz_cfg/hilti.rviz
Executable
+672
View File
@@ -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: <Fixed Frame>
Value: false
- Alpha: 1
Class: rviz/Axes
Enabled: true
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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
src/FAST-LIVO2/rviz_cfg/ntu_viral.rviz
Executable
+671
View File
@@ -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: <Fixed Frame>
Value: false
- Alpha: 1
Class: rviz_default_plugins/Axes
Enabled: true
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed 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: <Fixed Frame>
Yaw: 5.788588047027588
Window Geometry:
Displays:
collapsed: false
Height: 1376
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd00000004000000000000018b000004bffc0200000019fb0000001200530065006c0065006300740069006f006e00000001530000005c0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000257000000c900fffffffb0000000a0049006d006100670065010000029a000002620000001600fffffffb0000000a0049006d00610067006501000001c7000001d60000000000000000fb0000000a0049006d00610067006500000001cc000001d10000000000000000fb0000000a0049006d00610067006500000002790000012a0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002470000015c0000000000000000fb0000000a0049006d00610067006501000002d8000000c30000000000000000fb0000000a0049006d00610067006501000002db000000c80000000000000000fb0000000c00430061006d00650072006101000002db000000c80000000000000000fb0000000a0049006d00610067006501000001ec000000eb0000000000000000fb0000000a0049006d00610067006501000002dd000000c80000000000000000fb0000000a0049006d00610067006501000001870000021e0000000000000000fb0000000a0049006d0061006700650000000243000000940000000000000000fb0000000a0049006d006100670065010000029a0000010b0000000000000000fb0000000a0049006d00610067006501000002d6000000c70000000000000000fb0000000a0049006d006100670065010000024c000001510000000000000000fb0000000a0049006d00610067006501000002d3000000c80000000000000000fb0000000a0049006d006100670065010000010c0000029100000000000000000000000100000152000004bffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004bf000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000749000000dbfc0100000005fb0000000a0049006d0061006700650100000000000007490000000000000000fb0000000a0049006d0061006700650100000000000007490000000000000000fb0000000a0049006d00610067006501000000000000062c0000000000000000fb0000000a0049006d00610067006501000000000000062c0000000000000000fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b800000041fc0100000002fb0000000800540069006d00650100000000000009b8000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000827000004bf00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 2488
X: 72
Y: 27
src/FAST-LIVO2/scripts/colmap_output.sh
Executable
+25
View File
@@ -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
src/FAST-LIVO2/scripts/mesh.py
+114
View File
@@ -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])
src/FAST-LIVO2/src/IMU_Processing.cpp
Executable
+591
View File
@@ -0,0 +1,591 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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 <rcpputils/asserts.hpp>
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: "<<mean_acc.norm()<<endl;
}
for (const auto &imu : meas.imu)
{
const auto &imu_acc = imu->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: "<<cur_acc.norm()<<" "<<mean_acc.norm()<<endl;
N++;
}
IMU_mean_acc_norm = mean_acc.norm();
state_inout.gravity = -mean_acc / mean_acc.norm() * G_m_s2;
state_inout.rot_end = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));
state_inout.bias_g = Zero3d; // mean_gyr;
last_imu = meas.imu.back();
}
void ImuProcess::Forward_without_imu(LidarMeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out)
{
pcl_out = *(meas.lidar);
/*** sort point clouds by offset time ***/
const double &pcl_beg_time = meas.lidar_frame_beg_time;
sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
const double &pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000);
meas.last_lio_update_time = pcl_end_time;
const double &pcl_end_offset_time = pcl_out.points.back().curvature / double(1000);
MD(DIM_STATE, DIM_STATE) F_x, cov_w;
double dt = 0;
if (b_first_frame)
{
dt = 0.1;
b_first_frame = false;
}
else { dt = pcl_beg_time - time_last_scan; }
time_last_scan = pcl_beg_time;
// for (size_t i = 0; i < pcl_out->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: "<<meas.imu.size()<<endl;
auto v_imu = meas.imu;
v_imu.push_front(last_imu);
const double &imu_beg_time = stamp2Sec(v_imu.front()->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(): "<<pcl_out.size()<<endl;
// cout<<"pcl_offset_time: "<<pcl_offset_time<<"pcl_it->curvature:
// "<<pcl_it->curvature<<endl;
// cout<<"lidar_meas.lidar_scan_index_now:"<<lidar_meas.lidar_scan_index_now<<endl;
// printf("[ IMU ] last propagation end time: %lf \n", lidar_meas.last_lio_update_time);
if (lidar_meas.lio_vio_flg == LIO)
{
pcl_wait_proc.resize(lidar_meas.pcl_proc_cur->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 "<<prop_beg_time<<" to
// "<<prop_end_time<<", " \
// <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to
// "<<imu_end_time<<endl;
// cout<<"[ IMU ]: point size: "<<lidar_meas.lidar->points.size()<<endl;
/*** Initialize IMU pose ***/
// IMUpose.clear();
/*** forward propagation at each imu point ***/
V3D acc_imu(acc_s_last), angvel_avr(angvel_last), acc_avr, vel_imu(state_inout.vel_end), pos_imu(state_inout.pos_end);
// cout << "[ IMU ] input state: " << state_inout.vel_end.transpose() << " " << state_inout.pos_end.transpose() << endl;
M3D R_imu(state_inout.rot_end);
MD(DIM_STATE, DIM_STATE) F_x, cov_w;
double dt, dt_all = 0.0;
double offs_t;
// double imu_time;
double tau;
if (!imu_time_init)
{
// imu_time = stamp2Sec(v_imu.front()->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: "<<lidar_meas.lio_vio_flg<<endl;
switch (lidar_meas.lio_vio_flg)
{
case LIO:
case VIO:
dt = 0;
for (int i = 0; i < v_imu.size() - 1; i++)
{
auto head = v_imu[i];
auto tail = v_imu[i + 1];
if (stamp2Sec(tail->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<<tail->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: "<<angvel_avr.transpose()<<endl;
// cout<<"acc_avr: "<<acc_avr.transpose()<<endl;
// #ifdef DEBUG_PRINT
fout_imu << setw(10) << stamp2Sec(head->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<<setw(20)<<"offset_t: "<<offs_t<<"stamp2Sec(tail->header.stamp):
// "<<stamp2Sec(tail->header.stamp)<<endl; printf("[ LIO Propagation ]
// offs_t: %lf \n", offs_t);
IMUpose.push_back(set_pose6d(offs_t, acc_imu, angvel_avr, vel_imu, pos_imu, R_imu));
}
// unbiased_gyr = V3D(IMUpose.back().gyr[0], IMUpose.back().gyr[1], IMUpose.back().gyr[2]);
// cout<<"prop end - start: "<<prop_end_time - prop_beg_time<<" dt_all: "<<dt_all<<endl;
lidar_meas.last_lio_update_time = prop_end_time;
// dt = prop_end_time - imu_end_time;
// printf("[ LIO Propagation ] dt: %lf \n", dt);
break;
}
state_inout.vel_end = vel_imu;
state_inout.rot_end = R_imu;
state_inout.pos_end = pos_imu;
// Exposure time is no longer part of the propagated state (see StatesGroup
// in common_lib.h) — VIOManager owns inv_expo_time_per_cam directly.
/*** calculated the pos and attitude prediction at the frame-end ***/
// if (imu_end_time>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: "<<state_inout.vel_end.transpose() <<
// state_inout.pos_end.transpose()<<endl;
last_imu = v_imu.back();
last_prop_end_time = prop_end_time;
double t1 = omp_get_wtime();
// auto pos_liD_e = state_inout.pos_end + state_inout.rot_end *
// Lid_offset_to_IMU; auto R_liD_e = state_inout.rot_end * Lidar_R_to_IMU;
// cout<<"[ IMU ]: vel "<<state_inout.vel_end.transpose()<<" pos
// "<<state_inout.pos_end.transpose()<<"
// ba"<<state_inout.bias_a.transpose()<<" bg
// "<<state_inout.bias_g.transpose()<<endl; cout<<"propagated cov:
// "<<state_inout.cov.diagonal().transpose()<<endl;
// cout<<"UndistortPcl Time:";
// for (auto it = IMUpose.begin(); it != IMUpose.end(); ++it) {
// cout<<it->offset_time<<" ";
// }
// cout<<endl<<"UndistortPcl size:"<<IMUpose.size()<<endl;
// cout<<"Undistorted pcl_out.size: "<<pcl_out.size()
// <<"lidar_meas.size: "<<lidar_meas.lidar->points.size()<<endl;
if (pcl_wait_proc.points.size() < 1) return;
/*** undistort each lidar point (backward propagation), ONLY working for LIO
* update ***/
if (lidar_meas.lio_vio_flg == LIO)
{
auto it_pcl = pcl_wait_proc.points.end() - 1;
M3D extR_Ri(Lid_rot_to_IMU.transpose() * state_inout.rot_end.transpose());
V3D exrR_extT(Lid_rot_to_IMU.transpose() * Lid_offset_to_IMU);
for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
{
auto head = it_kp - 1;
auto tail = it_kp;
R_imu << MAT_FROM_ARRAY(head->rot);
acc_imu << VEC_FROM_ARRAY(head->acc);
// cout<<"head imu acc: "<<acc_imu.transpose()<<endl;
vel_imu << VEC_FROM_ARRAY(head->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;
}
src/FAST-LIVO2/src/LIVMapper.cpp
Executable
+1672
View File
File diff suppressed because it is too large Load Diff
src/FAST-LIVO2/src/frame.cpp
+85
View File
@@ -0,0 +1,85 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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 <boost/bind/bind.hpp>
#include "feature.h"
#include "frame.h"
#include "visual_point.h"
#include <stdexcept>
#include <vikit/math_utils.h>
#include <vikit/performance_monitor.h>
#include <vikit/vision.h>
int Frame::frame_counter_ = 0;
Frame::Frame(const std::vector<vk::AbstractCamera *> &cams, std::vector<cv::Mat> &imgs, double timestamp)
: id_(frame_counter_++),
cams_(cams),
T_f_w_(cams.size(), SE3<double>()),
T_f_w_prior_(cams.size(), SE3<double>()),
timestamp_(timestamp)
{
initFrame(imgs);
}
Frame::~Frame()
{
std::for_each(fts_.begin(), fts_.end(), [&](Feature *i) { delete i; });
}
void Frame::initFrame(std::vector<cv::Mat> &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<cv::Mat>(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
src/FAST-LIVO2/src/main.cpp
Executable
+14
View File
@@ -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;
}
src/FAST-LIVO2/src/preprocess.cpp
Executable
+1126
View File
File diff suppressed because it is too large Load Diff
src/FAST-LIVO2/src/utils.cpp
+19
View File
@@ -0,0 +1,19 @@
// utils.cpp
#include <vector>
#include <cstdint> // for int64_t
#include <limits> // for std::numeric_limits
#include <stdexcept> // for std::out_of_range
std::vector<int> convertToIntVectorSafe(const std::vector<int64_t>& int64_vector) {
std::vector<int> int_vector;
int_vector.reserve(int64_vector.size()); // 预留空间以提高效率
for (int64_t value : int64_vector) {
if (value < std::numeric_limits<int>::min() || value > std::numeric_limits<int>::max()) {
throw std::out_of_range("Value is out of range for int");
}
int_vector.push_back(static_cast<int>(value));
}
return int_vector;
}
src/FAST-LIVO2/src/vio.cpp
Executable
+2857
View File
File diff suppressed because it is too large Load Diff
src/FAST-LIVO2/src/visual_point.cpp
+158
View File
@@ -0,0 +1,158 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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 <stdexcept>
#include <vikit/math_utils.h>
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<float>::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;
}
}
}
src/FAST-LIVO2/src/voxel_map.cpp
+987
View File
@@ -0,0 +1,987 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
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<double, 3, 2> N;
N << base_vector1(0), base_vector2(0), base_vector1(1), base_vector2(1), base_vector1(2), base_vector2(2);
Eigen::Matrix<double, 3, 2> 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<bool>("publish.pub_plane_en", false);
node->declare_parameter<int>("lio.max_layer", 1);
node->declare_parameter<double>("lio.voxel_size", 0.5);
node->declare_parameter<double>("lio.min_eigen_value", 0.01);
node->declare_parameter<double>("lio.sigma_num", 3);
node->declare_parameter<double>("lio.beam_err", 0.02);
node->declare_parameter<double>("lio.dept_err", 0.05);
// Declaration of parameter of type std::vector<int> won't build, https://github.com/ros2/rclcpp/issues/1585
node->declare_parameter<vector<int64_t>>("lio.layer_init_num", std::vector<int64_t>{5,5,5,5,5});
node->declare_parameter<int>("lio.max_points_num", 50);
node->declare_parameter<int>("lio.min_iterations", 5);
node->declare_parameter<bool>("local_map.map_sliding_en", false);
node->declare_parameter<int>("local_map.half_map_size", 100);
node->declare_parameter<double>("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<pointWithVar> &points, VoxelPlane *plane)
{
plane->plane_var_ = Eigen::Matrix<double, 6, 6>::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<Eigen::Matrix3d> 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<double, 6, 3> J;
Eigen::Matrix3d F;
for (int m = 0; m < 3; m++)
{
if (m != (int)evalsMin)
{
Eigen::Matrix<double, 1, 3> 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<double, 1, 3> 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<pointWithVar>().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<pointWithVar>().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<pointWithVar>().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<pointWithVar>().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<pointWithVar>().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<pcl::PointXYZI>::Ptr world_lidar(new pcl::PointCloud<pcl::PointXYZI>);
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<double, 1, 6> 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: "<<HTz<<endl;
H_T_H.block<6, 6>(0, 0) = Hsub_T_R_inv * Hsub;
// EigenSolver<Matrix<double, 6, 6>> es(H_T_H.block<6,6>(0,0));
MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H.block<DIM_STATE, DIM_STATE>(0, 0) + state_.cov.block<DIM_STATE, DIM_STATE>(0, 0).inverse()).inverse();
G.block<DIM_STATE, 6>(0, 0) = K_1.block<DIM_STATE, 6>(0, 0) * H_T_H.block<6, 6>(0, 0);
auto vec = state_propagat - state_;
VD(DIM_STATE)
solution = K_1.block<DIM_STATE, 6>(0, 0) * HTz + vec.block<DIM_STATE, 1>(0, 0) - G.block<DIM_STATE, 6>(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<DIM_STATE, DIM_STATE>(0, 0) =
(I_STATE.block<DIM_STATE, DIM_STATE>(0, 0) - G.block<DIM_STATE, DIM_STATE>(0, 0)) * state_.cov.block<DIM_STATE, DIM_STATE>(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<pcl::PointXYZI>::Ptr &trans_cloud)
{
pcl::PointCloud<pcl::PointXYZI>().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<int> layer_init_num = convertToIntVectorSafe(config_setting_.layer_init_num_);
std::vector<pointWithVar> 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: "<<RGB.transpose()<<endl;
return RGB;
}
void VoxelMapManager::UpdateVoxelMap(const std::vector<pointWithVar> &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<int> 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<pointWithVar> &pv_list, std::vector<PointToPlane> &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<PointToPlane> all_ptpl_list(pv_list.size());
std::vector<bool> useful_ptpl(pv_list.size());
std::vector<size_t> 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<double, 1, 6> 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<VoxelPlane> 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<VoxelPlane> &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<<RED<<"[DEBUG]: Last sliding length "<<(position_last_ - last_slide_position).norm()<<RESET<<"\n";
return;
}
//get global id now
last_slide_position = position_last_;
double t_sliding_start = omp_get_wtime();
float loc_xyz[3];
for (int j = 0; j < 3; j++)
{
loc_xyz[j] = position_last_[j] / config_setting_.max_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]);//discrete global
clearMemOutOfMap((int64_t)loc_xyz[0] + config_setting_.half_map_size, (int64_t)loc_xyz[0] - config_setting_.half_map_size,
(int64_t)loc_xyz[1] + config_setting_.half_map_size, (int64_t)loc_xyz[1] - config_setting_.half_map_size,
(int64_t)loc_xyz[2] + config_setting_.half_map_size, (int64_t)loc_xyz[2] - config_setting_.half_map_size);
double t_sliding_end = omp_get_wtime();
std::cout<<RED<<"[DEBUG]: Map sliding using "<<t_sliding_end - t_sliding_start<<" secs"<<RESET<<"\n";
return;
}
void VoxelMapManager::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 )
{
int delete_voxel_cout = 0;
// double delete_time = 0;
// double last_delete_time = 0;
for (auto it = voxel_map_.begin(); it != voxel_map_.end(); )
{
const VOXEL_LOCATION& loc = it->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<<RED<<"[DEBUG]: Delete "<<delete_voxel_cout<<" root voxels"<<RESET<<"\n";
// std::cout<<RED<<"[DEBUG]: Delete "<<delete_voxel_cout<<" voxels using "<<delete_time<<" s"<<RESET<<"\n";
}
src/FAST-LIVO2/urdf/fori_robot.urdf
+219
View File
@@ -0,0 +1,219 @@
<?xml version="1.0"?>
<robot name="fori_robot">
<!-- 재질 정의 -->
<material name="blue"><color rgba="0 0 0.8 1"/></material>
<material name="black"><color rgba="0 0 0 1"/></material>
<material name="grey"><color rgba="0.5 0.5 0.5 1"/></material>
<material name="dark_gray"><color rgba="0.2 0.2 0.2 1.0"/></material>
<!-- ============================================================
Root: aft_mapped (LiDAR body frame)
FAST-LIVO2가 camera_init → aft_mapped 를 동적으로 발행.
============================================================ -->
<link name="aft_mapped"/>
<!-- Livox MID-360 시각화 -->
<link name="lidar_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry><box size="0.07 0.07 0.04"/></geometry>
<material name="dark_gray"/>
</visual>
</link>
<joint name="aft_mapped_to_lidar" type="fixed">
<parent link="aft_mapped"/>
<child link="lidar_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<!-- Hikrobot 카메라 (캘리브레이션 결과) -->
<link name="camera_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry><box size="0.05 0.09 0.04"/></geometry>
<material name="blue"/>
</visual>
</link>
<joint name="aft_mapped_to_camera" type="fixed">
<parent link="aft_mapped"/>
<child link="camera_link"/>
<origin xyz="0.128964 -0.026998 -0.043195"
rpy="-1.529526 -1.293497 3.122599"/>
</joint>
<link name="camera_optical_link"/>
<joint name="camera_to_optical" type="fixed">
<parent link="camera_link"/>
<child link="camera_optical_link"/>
<origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
</joint>
<!-- ============================================================
Base Link: 실측값 적용
라이다(aft_mapped) 기준: x=-269.53mm, z=-412.9633mm
rpy pitch=-0.277309rad: 라이다 pitch -15.89° 보정 → 수평
============================================================ -->
<link name="base_link">
<visual>
<origin xyz="0 0 0.143" rpy="0 0 0"/>
<geometry><box size="0.631 0.410 0.286"/></geometry>
<material name="blue"/>
</visual>
<collision>
<origin xyz="0 0 0.143" rpy="0 0 0"/>
<geometry><box size="0.631 0.410 0.286"/></geometry>
</collision>
<inertial>
<mass value="20.0"/>
<inertia ixx="0.5" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
</inertial>
</link>
<joint name="aft_mapped_to_base" type="fixed">
<parent link="aft_mapped"/>
<child link="base_link"/>
<origin xyz="-0.26953 0 -0.41296" rpy="0 -0.277309 0"/>
</joint>
<!-- Base Footprint: 실제 지면(Ground) 위치 -->
<link name="base_footprint"/>
<joint name="base_to_footprint" type="fixed">
<parent link="base_link"/>
<child link="base_footprint"/>
<origin xyz="0 0 -0.0639" rpy="0 0 0"/>
</joint>
<!-- ============================================================
휠 및 차축: base_link 기준 (변경 없음)
============================================================ -->
<!-- 1. Front Left -->
<link name="front_left_axle">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.025" length="0.035"/></geometry>
<material name="grey"/>
</visual>
</link>
<joint name="front_left_axle_joint" type="fixed">
<parent link="base_link"/>
<child link="front_left_axle"/>
<origin xyz="0.187 0.2225 0.0631" rpy="0 0 0"/>
</joint>
<link name="front_left_wheel">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
</collision>
</link>
<joint name="front_left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="front_left_wheel"/>
<origin xyz="0.187 0.28 0.0631" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<!-- 2. Front Right -->
<link name="front_right_axle">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.025" length="0.035"/></geometry>
<material name="grey"/>
</visual>
</link>
<joint name="front_right_axle_joint" type="fixed">
<parent link="base_link"/>
<child link="front_right_axle"/>
<origin xyz="0.187 -0.2225 0.0631" rpy="0 0 0"/>
</joint>
<link name="front_right_wheel">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
</collision>
</link>
<joint name="front_right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="front_right_wheel"/>
<origin xyz="0.187 -0.28 0.0631" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<!-- 3. Rear Left -->
<link name="rear_left_axle">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.025" length="0.035"/></geometry>
<material name="grey"/>
</visual>
</link>
<joint name="rear_left_axle_joint" type="fixed">
<parent link="base_link"/>
<child link="rear_left_axle"/>
<origin xyz="-0.187 0.2225 0.0631" rpy="0 0 0"/>
</joint>
<link name="rear_left_wheel">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
</collision>
</link>
<joint name="rear_left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="rear_left_wheel"/>
<origin xyz="-0.187 0.28 0.0631" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<!-- 4. Rear Right -->
<link name="rear_right_axle">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.025" length="0.035"/></geometry>
<material name="grey"/>
</visual>
</link>
<joint name="rear_right_axle_joint" type="fixed">
<parent link="base_link"/>
<child link="rear_right_axle"/>
<origin xyz="-0.187 -0.2225 0.0631" rpy="0 0 0"/>
</joint>
<link name="rear_right_wheel">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
</collision>
</link>
<joint name="rear_right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="rear_right_wheel"/>
<origin xyz="-0.187 -0.28 0.0631" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
</robot>
src/FAST-LIVO2/urdf/mid360_robot.urdf
+93
View File
@@ -0,0 +1,93 @@
<?xml version="1.0"?>
<robot name="mid360_robot">
<!-- ============================================================
Root: aft_mapped
FAST-LIVO2가 camera_init → aft_mapped 를 동적으로 발행.
이 링크 자체는 LiDAR body frame.
============================================================ -->
<link name="aft_mapped"/>
<!-- ============================================================
Livox MID-360 시각화 (aft_mapped 원점에 위치)
실제 크기: 직경 65mm, 높이 38mm → 박스로 근사
============================================================ -->
<link name="lidar_link">
<visual>
<geometry>
<box size="0.07 0.07 0.04"/>
</geometry>
<material name="dark_gray">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
</visual>
</link>
<joint name="aft_mapped_to_lidar" type="fixed">
<parent link="aft_mapped"/>
<child link="lidar_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<!-- ============================================================
로봇 샤시 박스 (임시 크기 — 실측 후 xyz/size 수정)
parent: aft_mapped 기준 z=-0.25m (라이다 아래 250mm, 바닥 수평)
size: 가로 x 세로 x 높이 (m)
============================================================ -->
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.40 0.30 0.15"/>
</geometry>
<material name="light_gray">
<color rgba="0.7 0.7 0.7 0.6"/>
</material>
</visual>
</link>
<joint name="aft_mapped_to_base" type="fixed">
<parent link="aft_mapped"/>
<child link="base_link"/>
<!-- 라이다 pitch -15.89° 보정 → base_link 바닥 수평
rpy pitch = +15.89° = 0.2773 rad 반대 방향 적용 -->
<origin xyz="0 0 -0.25" rpy="0 -0.277309 0"/>
</joint>
<!-- ============================================================
Hikrobot 카메라 (캘리브레이션 결과 적용)
camera_link: REP-103 (x=전방, y=왼쪽, z=위)
============================================================ -->
<link name="camera_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.09 0.04"/>
</geometry>
<material name="blue">
<color rgba="0.2 0.4 0.8 1.0"/>
</material>
</visual>
</link>
<joint name="aft_mapped_to_camera" type="fixed">
<parent link="aft_mapped"/>
<child link="camera_link"/>
<!-- calib.json T_lidar_camera 역변환 결과 -->
<origin xyz="0.128964 -0.026998 -0.043195"
rpy="-1.529526 -1.293497 3.122599"/>
</joint>
<!-- ============================================================
Camera optical frame (ROS 표준)
x=오른쪽, y=아래, z=전방
============================================================ -->
<link name="camera_optical_link"/>
<joint name="camera_to_optical" type="fixed">
<parent link="camera_link"/>
<child link="camera_optical_link"/>
<origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
</joint>
</robot>
src/gnss_comm/CMakeLists.txt
+17
View File
@@ -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()
src/gnss_comm/msg/GnssPVTSolnMsg.msg
+22
View File
@@ -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]
src/gnss_comm/msg/GnssTimeMsg.msg
+5
View File
@@ -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
src/gnss_comm/package.xml
+25
View File
@@ -0,0 +1,25 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>gnss_comm</name>
<version>1.0.0</version>
<description>
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.
</description>
<maintainer email="noreply@example.com">FAST-LIVO2-RTK ROS2 port</maintainer>
<license>GPLv3</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
src/rpg_vikit/.gitignore
+3
View File
@@ -0,0 +1,3 @@
vikit_py/build
vikit_common/bin/
vikit_common/lib/
src/rpg_vikit/README.md
+69
View File
@@ -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+)
src/rpg_vikit/vikit_common/CMakeLists.txt
+115
View File
@@ -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 $<TARGET_FILE:vikit_common>)
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()
src/rpg_vikit/vikit_common/CMakeModules/FindEigen.cmake
+81
View File
@@ -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, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# 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})
src/rpg_vikit/vikit_common/include/vikit/abstract_camera.h
+85
View File
@@ -0,0 +1,85 @@
/*
* abstract_camera.h
*
* Created on: Jul 23, 2012
* Author: cforster
*/
#ifndef ABSTRACT_CAMERA_H_
#define ABSTRACT_CAMERA_H_
#include <Eigen/Core>
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]<width()-boundary
&& obs[1]>=boundary && obs[1]<height()-boundary)
return true;
return false;
}
inline bool isInFrame(const Vector2i &obs, int boundary, int level) const
{
if(obs[0] >= boundary && obs[0] < width()/(1<<level)-boundary
&& obs[1] >= boundary && obs[1] <height()/(1<<level)-boundary)
return true;
return false;
}
};
} // end namespace CSfM
#endif /* ABSTRACT_CAMERA_H_ */
src/rpg_vikit/vikit_common/include/vikit/aligned_mem.h
+127
View File
@@ -0,0 +1,127 @@
/*
* aligned_mem.h
*
* Created on: May 14, 2013
* Author: cforster
*
* Code from libcvd: cvd/internal/aligned_mem.h
* https://github.com/edrosten/libcvd
* Licence: LGPL 2.1
*/
#ifndef VIKIT_ALIGNED_MEM_H_
#define VIKIT_ALIGNED_MEM_H_
#include <string> // memset
#include <string.h> // memset
#include <cassert>
#include <cstdlib>
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<size_t>(ptr)) & 0x7) == 0;
}
inline bool
is_aligned16(const void* ptr)
{
return ((reinterpret_cast<size_t>(ptr)) & 0xF) == 0;
}
template <class T, int N=20> struct placement_delete
{
enum { Size = (1<<N) };
struct Array {
T data[Size];
};
static inline void destruct(T* buf)
{
(*(Array*)buf).~Array();
}
static inline void free(T* buf, size_t M)
{
if (M >= Size) {
placement_delete<T,N>::free(buf+Size,M-Size);
placement_delete<T,N>::destruct(buf);
} else {
placement_delete<T,N-1>::free(buf, M);
}
}
};
template <class T> struct placement_delete<T,-1>
{
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 <class T>
inline T * aligned_alloc(size_t count, size_t alignment){
void * data = aligned_alloc(sizeof(T)* count, alignment);
return new (data) T[count];
}
template <class T>
inline void aligned_free(T * memory, size_t count){
placement_delete<T>::free(memory, count);
aligned_free(memory);
}
template<class T> 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 <class T, int N>
struct AlignedMem {
T* mem;
size_t count;
AlignedMem(size_t c) : count(c) {
mem = aligned_alloc<T>(count, N);
}
~AlignedMem() {
aligned_free<T>(mem, count);
}
T* data() { return mem; }
const T* data() const { return mem; }
};
} // namespace aligned_mem
} // namespace vikit
#endif // VIKIT_ALIGNED_MEM_H_
src/rpg_vikit/vikit_common/include/vikit/atan_camera.h
+96
View File
@@ -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 <stdlib.h>
#include <string>
#include <Eigen/Eigen>
#include <vikit/abstract_camera.h>
#include <vikit/math_utils.h>
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_ */
src/rpg_vikit/vikit_common/include/vikit/blender_utils.h
+133
View File
@@ -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 <list>
#include <string>
#include <vikit/pinhole_camera.h>
#include <vikit/math_utils.h>
#include <opencv2/opencv.hpp>
#include <fstream>
#include <Eigen/Core>
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>();
float depth;
for(int y=0; y<cam.height(); ++y)
{
for(int x=0; x<cam.width(); ++x, ++img_ptr)
{
file_stream >> 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<Vector3d> 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<float>(y,x));
assert(n_meas == pts.size());
Matrix<double, Dynamic, 4> A; A.resize(n_meas, Eigen::NoChange);
Matrix<double, Dynamic, 1> b; b.resize(n_meas, Eigen::NoChange);
size_t i = 0;
for(list<Vector3d>::iterator it=pts.begin(); it!=pts.end(); ++it)
{
A.row(i) << it->x(), it->y(), it->z(), 1.0;
b[i] = 0;
++i;
}
JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV);
Matrix<double, 4, 4> 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_
src/rpg_vikit/vikit_common/include/vikit/equidistant_camera.h
+145
View File
@@ -0,0 +1,145 @@
/*
* equidistant_camera.h
*
* Created on: January 26, 2023
* Author: xuankuzcr
*/
#ifndef EQUIDISTANT_CAMERA_H_
#define EQUIDISTANT_CAMERA_H_
#include <stdlib.h>
#include <string>
#include <Eigen/Eigen>
#include <vikit/abstract_camera.h>
#include <opencv2/opencv.hpp>
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<double, 2, 3> jacobian_2x3(const Eigen::Vector3d& p) const
{
Eigen::Matrix<double, 2, 3> 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_ */
src/rpg_vikit/vikit_common/include/vikit/file_reader.h
+101
View File
@@ -0,0 +1,101 @@
/**
* This file is part of dvo.
*
* Copyright 2012 Christian Kerl <christian.kerl@in.tum.de> (Technical University of Munich)
* For more information see <http://vision.in.tum.de/data/software/dvo>.
*
* 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 <http://www.gnu.org/licenses/>.
*/
#ifndef VIKIT_FILE_READER_H_
#define VIKIT_FILE_READER_H_
#include <fstream>
#include <vector>
namespace vk
{
/**
* Entry has to support the following operator
* std::istream& operator >>(std::istream&, Entry&);
*/
template<class Entry>
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<Entry>& 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_
src/rpg_vikit/vikit_common/include/vikit/file_reader_types.h
+97
View File
@@ -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 <Eigen/Core>
#include <Eigen/Eigen>
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_
src/rpg_vikit/vikit_common/include/vikit/homography.h
+85
View File
@@ -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 <Eigen/Core>
#include <Eigen/StdVector>
#include <Eigen/SVD>
#include <vikit/math_utils.h>
#include <sophus/se3.hpp>
namespace vk {
using namespace Eigen;
using namespace std;
struct HomographyDecomposition
{
Vector3d t;
Matrix3d R;
double d;
Vector3d n;
// Resolved Composition
Sophus::SE3<double> T; //!< second from first
int score;
};
class Homography
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Homography (const vector<Vector2d, aligned_allocator<Vector2d> >& _fts1,
const vector<Vector2d, aligned_allocator<Vector2d> >& _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<Vector2d, aligned_allocator<Vector2d> >& fts_c1; //!< Features on first image on unit plane
const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c2; //!< Features on second image on unit plane
vector<bool> inliers;
Sophus::SE3<double> T_c2_from_c1; //!< Relative translation and rotation of two images
Eigen::Matrix3d H_c2_from_c1; //!< Homography
vector<HomographyDecomposition> decompositions;
};
} /* end namespace vk */
#endif /* HOMOGRAPHY_H_ */
src/rpg_vikit/vikit_common/include/vikit/img_align.h
+153
View File
@@ -0,0 +1,153 @@
/*
* img_align.h
*
* Created on: Aug 22, 2012
* Author: cforster
*/
#ifndef IMG_ALIGN_H_
#define IMG_ALIGN_H_
#include <vector>
#include <stdint.h>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <opencv2/opencv.hpp>
#include <vikit/math_utils.h>
#include <vikit/pinhole_camera.h>
#include <vikit/nlls_solver.h>
#include <vikit/performance_monitor.h>
#include <sophus/se3.hpp>
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<double>> {
protected:
vector<vk::PinholeCamera>& cam_pyr_;
vector<cv::Mat>& depth_pyr_;
vector<cv::Mat>& img_pyr_;
vector<cv::Mat>& tpl_pyr_;
vector<cv::Mat>& img_pyr_dx_;
vector<cv::Mat>& img_pyr_dy_;
int level_;
int n_levels_;
PerformanceMonitor permon_;
bool display_;
bool log_;
double res_thresh_;
virtual double
computeResiduals (const Sophus::SE3<double>& 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<PinholeCamera>& cam_pyr,
vector<cv::Mat>& depth_pyr,
vector<cv::Mat>& img_pyr,
vector<cv::Mat>& tpl_pyr,
vector<cv::Mat>& img_pyr_dx,
vector<cv::Mat>& img_pyr_dy,
Sophus::SE3<double>& 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<PinholeCamera>& cam_pyr,
vector<cv::Mat>& depth_pyr,
vector<cv::Mat>& img_pyr,
vector<cv::Mat>& tpl_pyr,
vector<cv::Mat>& img_pyr_dx,
vector<cv::Mat>& 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<double>& model, int levelBegin = -1, int levelEnd = -1);
};
//! Efficient Second Order Minimization (ESM)
class SecondOrderMinimisationSE3 : public NLLSSolver<6, Sophus::SE3<double>> {
protected:
vector<vk::PinholeCamera>& cam_pyr_;
vector<cv::Mat>& depth_pyr_;
vector<cv::Mat>& img_pyr_;
vector<cv::Mat>& tpl_pyr_;
vector<cv::Mat>& img_pyr_dx_;
vector<cv::Mat>& img_pyr_dy_;
vector<cv::Mat>& tpl_pyr_dx_;
vector<cv::Mat>& tpl_pyr_dy_;
int level_;
PerformanceMonitor permon_;
bool display_;
bool log_;
float res_thresh_;
virtual double
computeResiduals (const Sophus::SE3<double>& 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<PinholeCamera>& cam_pyr,
vector<cv::Mat>& depth_pyr,
vector<cv::Mat>& img_pyr,
vector<cv::Mat>& tpl_pyr,
vector<cv::Mat>& img_pyr_dx,
vector<cv::Mat>& img_pyr_dy,
vector<cv::Mat>& tpl_pyr_dx,
vector<cv::Mat>& tpl_pyr_dy,
Sophus::SE3<double>& 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_ */
src/rpg_vikit/vikit_common/include/vikit/math_utils.h
+171
View File
@@ -0,0 +1,171 @@
/*
* math_utils.h
*
* Created on: Jul 20, 2012
* Author: cforster
*/
#ifndef MATH_UTILS_H_
#define MATH_UTILS_H_
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <sophus/se3.hpp>
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<Vector3d>& features1,
const vector<Vector3d>& features2,
const Eigen::Matrix3d& R,
const Vector3d& t,
const double reproj_thresh,
double error_multiplier2,
vector<Vector3d>& xyz_vec,
vector<int>& inliers,
vector<int>& outliers);
void computeInliersOneView(
const vector<Vector3d> & feature_sphere_vec,
const vector<Vector3d> & xyz_vec,
const Eigen::Matrix3d &R,
const Vector3d &t,
const double reproj_thresh,
const double error_multiplier2,
vector<int>& inliers,
vector<int>& 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; i<v.size(); i++)
{
double abs = fabs(v[i]);
if(abs>max){
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<class T>
T getMedian(vector<T>& data_vec)
{
assert(!data_vec.empty());
typename vector<T>::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<<level); // = 1 / 2^level
}
inline Vector2d pyrFromZero_2d(const Vector2d& uv_0, int level)
{
return Vector2d(pyrFromZero_d(uv_0[0], level),
pyrFromZero_d(uv_0[1], level));
}
inline void
frameJac_xyz2uv(const Vector3d & xyz,
const double & focal_length,
Eigen::Matrix<double,2,6> & 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_ */
src/rpg_vikit/vikit_common/include/vikit/nlls_solver.h
+169
View File
@@ -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 <stdint.h>
#include <stdio.h>
#include <iostream>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <vikit/math_utils.h>
#include <vikit/robust_cost.h>
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 <int D, typename T>
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<double, D, D> H_; //!< Hessian approximation
Eigen::Matrix<double, D, 1> Jres_; //!< Jacobian x Residual
Eigen::Matrix<double, D, 1> x_; //!< update step
bool have_prior_;
ModelType prior_;
Eigen::Matrix<double, D, D> 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<double, D, D>& 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<double, D, D>& getInformationMatrix() const;
};
} // end namespace vk
#include "nlls_solver_impl.hpp"
#endif /* LM_SOLVER_H_ */
src/rpg_vikit/vikit_common/include/vikit/nlls_solver_impl.hpp
+324
View File
@@ -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 <stdexcept>
#include <vikit/math_utils.h>
template <int D, typename T>
void vk::NLLSSolver<D, T>::optimize(ModelType& model)
{
if(method_ == GaussNewton)
optimizeGaussNewton(model);
else if(method_ == LevenbergMarquardt)
optimizeLevenbergMarquardt(model);
}
template <int D, typename T>
void vk::NLLSSolver<D, T>::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_<n_iter_; ++iter_)
{
rho_ = 0;
startIteration();
H_.setZero();
Jres_.setZero();
// compute initial error
n_meas_ = 0;
double new_chi2 = computeResiduals(model, true, false);
// add prior
if(have_prior_)
applyPrior(model);
// solve the linear system
if(!solve())
{
// matrix was singular and could not be computed
std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
std::cout << "H = " << H_ << std::endl;
std::cout << "Jres = " << Jres_ << std::endl;
stop_ = true;
}
// check if error increased since last optimization
if((iter_ > 0 && new_chi2 > chi2_) || stop_)
{
if(verbose_)
{
std::cout << "It. " << iter_
<< "\t Failure"
<< "\t new_chi2 = " << new_chi2
<< "\t Error increased. Stop optimizing."
<< std::endl;
}
model = old_model; // rollback
break;
}
// update the model
ModelType new_model;
update(model, new_model);
old_model = model;
model = new_model;
chi2_ = new_chi2;
if(verbose_)
{
std::cout << "It. " << iter_
<< "\t Success"
<< "\t new_chi2 = " << new_chi2
<< "\t n_meas = " << n_meas_
<< "\t x_norm = " << vk::norm_max(x_)
<< std::endl;
}
finishIteration();
// stop when converged, i.e. update step too small
if(vk::norm_max(x_)<=eps_)
break;
}
}
template <int D, typename T>
void vk::NLLSSolver<D, T>::optimizeLevenbergMarquardt(ModelType& model)
{
// Compute weight scale
if(use_weights_)
computeResiduals(model, false, true);
// compute the initial error
chi2_ = computeResiduals(model, true, false);
if(verbose_)
cout << "init chi2 = " << chi2_
<< "\t n_meas = " << n_meas_
<< endl;
// TODO: compute initial lambda
// Hartley and Zisserman: "A typical init value of lambda is 10^-3 times the
// average of the diagonal elements of J'J"
// Compute Initial Lambda
if(mu_ < 0)
{
double H_max_diag = 0;
double tau = 1e-4;
for(size_t j=0; j<D; ++j)
H_max_diag = max(H_max_diag, fabs(H_(j,j)));
mu_ = tau*H_max_diag;
}
// perform iterative estimation
for (iter_ = 0; iter_<n_iter_; ++iter_)
{
rho_ = 0;
startIteration();
// try to compute and update, if it fails, try with increased mu
n_trials_ = 0;
do
{
// init variables
ModelType new_model;
double new_chi2 = -1;
H_.setZero();
//H_ = mu_ * Matrix<double,D,D>::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 <int D, typename T>
void vk::NLLSSolver<D, T>::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 <int D, typename T>
void vk::NLLSSolver<D, T>::setPrior(
const T& prior,
const Eigen::Matrix<double, D, D>& Information)
{
have_prior_ = true;
prior_ = prior;
I_prior_ = Information;
}
template <int D, typename T>
void vk::NLLSSolver<D, T>::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 <int D, typename T>
inline const double& vk::NLLSSolver<D, T>::getChi2() const
{
return chi2_;
}
template <int D, typename T>
inline const Eigen::Matrix<double, D, D>& vk::NLLSSolver<D, T>::getInformationMatrix() const
{
return H_;
}
#endif /* LM_SOLVER_IMPL_HPP_ */
src/rpg_vikit/vikit_common/include/vikit/omni_camera.h
+87
View File
@@ -0,0 +1,87 @@
/*
* OcamProjector.h
*
* Created on: Sep 22, 2010
* Author: laurent kneip
*/
#ifndef OCAMPROJECTOR_H_
#define OCAMPROJECTOR_H_
#include <stdlib.h>
#include <string>
#include <Eigen/Eigen>
#include <vikit/abstract_camera.h>
#include <vikit/math_utils.h>
#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_ */
src/rpg_vikit/vikit_common/include/vikit/patch_score.h
+225
View File
@@ -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 <stdint.h>
#if __SSE2__
#include <tmmintrin.h>
#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<int HALF_PATCH_SIZE>
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_
src/rpg_vikit/vikit_common/include/vikit/performance_monitor.h
+53
View File
@@ -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 <map>
#include <string>
#include <iostream>
#include <fstream>
#include <vikit/timer.h>
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<std::string, Timer> timers_;
std::map<std::string, LogItem> logs_;
std::string trace_name_; //<! name of the thread that started the performance monitor
std::string trace_dir_; //<! directory where the logfiles are saved
std::ofstream ofs_;
void trace();
void traceHeader();
};
} // namespace vk
#endif // VIKIT_PERFORMANCE_MONITOR_H
src/rpg_vikit/vikit_common/include/vikit/pinhole_camera.h
+93
View File
@@ -0,0 +1,93 @@
/*
* pinhole_camera.h
*
* Created on: Jul 24, 2012
* Author: cforster
*/
#ifndef PINHOLE_CAMERA_H_
#define PINHOLE_CAMERA_H_
#include <stdlib.h>
#include <string>
#include <Eigen/Eigen>
#include <vikit/abstract_camera.h>
#include <opencv2/opencv.hpp>
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_ */

Some files were not shown because too many files have changed in this diff Show More