diff --git a/.gitignore b/.gitignore old mode 100644 new mode 100755 diff --git a/CMakeLists.txt b/CMakeLists.txt old mode 100644 new mode 100755 index 192d15c..3ed7f3a --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) project(lvi_sam) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +add_compile_options(-Wno-error=deprecated-declarations) + ################## 编译开关 compile switch############## # -DIF_OFFICIAL=1: use origin official LVI-SAM code # -DIF_OFFICIAL=0: use modified code of this repo @@ -13,6 +17,7 @@ add_definitions(-DIF_OFFICIAL=0) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++11") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread") +#set(OpenCV_DIR "/LVI-SAM/opencv-4.8.1/build/installed") ###################### ### Packages @@ -37,10 +42,10 @@ find_package(catkin REQUIRED COMPONENTS find_package(OpenMP REQUIRED) find_package(PCL REQUIRED) -find_package(OpenCV REQUIRED) +find_package(OpenCV 4 REQUIRED) find_package(Ceres REQUIRED) find_package(GTSAM REQUIRED QUIET) -find_package(Boost REQUIRED COMPONENTS filesystem program_options system timer) +find_package(Boost REQUIRED COMPONENTS filesystem program_options system timer serialization thread) ###################### @@ -120,7 +125,7 @@ target_link_libraries(${PROJECT_NAME}_visual_loop ${catkin_LIBRARIES} ${PCL_LIBR # IMU Preintegration add_executable(${PROJECT_NAME}_imuPreintegration src/lidar_odometry/imuPreintegration.cpp) -target_link_libraries(${PROJECT_NAME}_imuPreintegration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam Boost::timer) +target_link_libraries(${PROJECT_NAME}_imuPreintegration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam Boost::timer Boost::serialization Boost::thread) # Range Image Projection add_executable(${PROJECT_NAME}_imageProjection src/lidar_odometry/imageProjection.cpp) add_dependencies(${PROJECT_NAME}_imageProjection ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) @@ -133,4 +138,10 @@ target_link_libraries(${PROJECT_NAME}_featureExtraction ${catkin_LIBRARIES} ${PC add_executable(${PROJECT_NAME}_mapOptmization src/lidar_odometry/mapOptmization.cpp) add_dependencies(${PROJECT_NAME}_mapOptmization ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) target_compile_options(${PROJECT_NAME}_mapOptmization PRIVATE ${OpenMP_CXX_FLAGS}) -target_link_libraries(${PROJECT_NAME}_mapOptmization PRIVATE ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam Boost::timer) +target_link_libraries(${PROJECT_NAME}_mapOptmization PRIVATE ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam Boost::timer Boost::serialization Boost::thread) + +# Scan-to-map minimal node (LOAM-style optimizer) +add_executable(${PROJECT_NAME}_scan_to_map src/lidar_odometry/scan_to_map.cpp) +add_dependencies(${PROJECT_NAME}_scan_to_map ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) +target_compile_options(${PROJECT_NAME}_scan_to_map PRIVATE ${OpenMP_CXX_FLAGS}) +target_link_libraries(${PROJECT_NAME}_scan_to_map PRIVATE ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam Boost::timer Boost::serialization Boost::thread) diff --git a/README.md b/README.md old mode 100644 new mode 100755 index 8843a02..b15fe83 --- a/README.md +++ b/README.md @@ -317,3 +317,62 @@ Due to the special IMU (the Euler angle coordinate system is different from the ## Acknowledgement - Origin official [LVI-SAM](https://github.com/TixiaoShan/LVI-SAM). + +--- + +## 添加的 scan_to_map 节点 — 编译与运行说明（我已把源文件和 launch 放入仓库，但未编译） + +我已将一个最小化的 LOAM-style scan-to-map 优化器（角点/面点 + LM）添加到本仓库，用于快速验证与二次开发： + +- 源文件：`src/lidar_odometry/scan_to_map.cpp` +- 启动文件：`launch/kitti_scan_to_map.launch`（默认会加载 `config/KITTI_lidar.yaml` 与 `config/KITTI_camera.yaml`） + +下面给出如何在本地把该文件编译进 `lvi_sam` 包的精确说明（我已在仓库中做了最小的 CMake/package 修改，但我**没有**在容器中编译）： + +1) CMakeLists.txt 中的可执行目标（仓库已包含以下块）： + +```cmake +# Scan-to-map minimal node (LOAM-style optimizer) +add_executable(${PROJECT_NAME}_scan_to_map src/lidar_odometry/scan_to_map.cpp) +add_dependencies(${PROJECT_NAME}_scan_to_map ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) +target_compile_options(${PROJECT_NAME}_scan_to_map PRIVATE ${OpenMP_CXX_FLAGS}) +target_link_libraries(${PROJECT_NAME}_scan_to_map PRIVATE ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam Boost::timer) +``` + +2) `package.xml` 中需要（仓库已追加）声明的依赖示例： + +```xml + pcl_ros + pcl_ros + ceres-solver + ceres-solver +``` + +注意：不同 ROS 发行版或 OS 上 package 名称可能稍有差异（例如在 Debian/Ubuntu 上是 `libceres-dev` 等），如果你在编译时遇到找不到包或找不到库，请按系统提示安装对应的系统/ROS 包。 + +3) 推荐的本地编译步骤（示例，catkin 工作区） + +```bash +cd /home/what/ros_ws/LVI_ws +catkin_make +source devel/setup.bash +``` + +4) 运行（KITTI 示例） + +```bash +rosparam set /use_sim_time true +# 在另一个终端回放 kitti bag： rosbag play /path/to/kitti.bag --clock +roslaunch LVI-SAM-Easyused kitti_scan_to_map.launch +``` + +5) 常见问题与排查建议 + +- 如果出现找不到 `lvi_sam::cloud_info` 的 topic/type，先确保 `imageProjection` 与 `featureExtraction` 节点已启动（它们负责发布 `PROJECT_NAME/lidar/feature/cloud_info`）。 +- 如果链接错误提示找不到 gtsam/ceres/PCL 库，请先确认系统已安装对应开发包，例如（Ubuntu + ROS）： + ```bash + sudo apt install -y libceres-dev libgtsam-dev ros-${ROS_DISTRO}-pcl-ros + ``` +- 若编译失败且错误不明显，把 `catkin_make` 的完整输出贴给我，我可以基于错误信息给出精确修复补丁。 + +如果你同意我现在将继续保持“不编译”的状态，但可以帮你完成后续的本地编译或修复工作，请回复 `A`（我将执行编译并修复编译错误）。如需我撤回 `package.xml` 的依赖修改，也请说明。 diff --git a/README_CN.md b/README_CN.md old mode 100644 new mode 100755 diff --git a/config/M2DGR_camera.yaml b/config/M2DGR_camera.yaml old mode 100644 new mode 100755 diff --git a/config/M2DGR_lidar.yaml b/config/M2DGR_lidar.yaml old mode 100644 new mode 100755 diff --git a/config/brief_k10L6.bin b/config/brief_k10L6.bin old mode 100644 new mode 100755 diff --git a/config/brief_pattern.yml b/config/brief_pattern.yml old mode 100644 new mode 100755 diff --git a/config/extrinsic_parameter_example.pdf b/config/extrinsic_parameter_example.pdf old mode 100644 new mode 100755 diff --git a/config/fisheye_mask_720x540.jpg b/config/fisheye_mask_720x540.jpg old mode 100644 new mode 100755 diff --git a/config/params_camera.yaml b/config/params_camera.yaml old mode 100644 new mode 100755 diff --git a/config/params_camera_official.yaml b/config/params_camera_official.yaml old mode 100644 new mode 100755 diff --git a/config/params_lidar.yaml b/config/params_lidar.yaml old mode 100644 new mode 100755 diff --git a/config/params_lidar_official.yaml b/config/params_lidar_official.yaml old mode 100644 new mode 100755 diff --git a/config/params_lidar_tuning.yaml b/config/params_lidar_tuning.yaml new file mode 100644 index 0000000..058e9b9 --- /dev/null +++ b/config/params_lidar_tuning.yaml @@ -0,0 +1,32 @@ +# Tuning params for IMU preintegration timing and numeric safety +# Load this file with rosparam (e.g. in your launch file) to override defaults in code. + +lvi_sam: + # imu_time_tolerance (seconds) + # Accept small "late" IMU samples relative to correction time. Negative means we accept IMU + # with timestamp up to |imu_time_tolerance| seconds after correction time (useful when IMU + # timestamps are slightly later than lidar/odom timestamps). Default in code: -0.002 + imu_time_tolerance: -0.002 + + # preint_dt_floor (seconds) + # Minimum deltaT below which preintegration is considered too small to be reliable. When + # actual preintegration deltaT <= preint_dt_floor the node will avoid adding degenerate + # IMU/Between factors and use mild priors instead. Default in code: 1e-4 + preint_dt_floor: 1e-4 + + # min_bias_sigma + # Per-component minimum standard deviation (sigma) for the bias-between factor. This avoids + # zero or near-zero sigmas which create singular information matrices for GTSAM. Default: 1e-5 + min_bias_sigma: 1e-5 + + # Suggested tuning for priors (experiment with these if you see large jumps): + # prior_vel_sigma: standard deviation (m/s) for initial velocity prior (default in code: 1.0) + prior_vel_sigma: 1.0 + # prior_bias_sigma: standard deviation for bias prior components (default in code: 1e-2) + prior_bias_sigma: 1e-2 + +# Usage: +# In your launch file, add: +# +# or run: +# rosparam load $(rospack find LVI-SAM-Easyused)/config/params_lidar_tuning.yaml diff --git a/doc/920ad1643a61547f0000adbf12073fbc.jpg b/doc/920ad1643a61547f0000adbf12073fbc.jpg new file mode 100644 index 0000000..521414c Binary files /dev/null and b/doc/920ad1643a61547f0000adbf12073fbc.jpg differ diff --git a/doc/fig/backbag.png b/doc/fig/backbag.png old mode 100644 new mode 100755 diff --git a/doc/fig/gate_01.png b/doc/fig/gate_01.png old mode 100644 new mode 100755 diff --git a/doc/fig/handheld-official.png b/doc/fig/handheld-official.png old mode 100644 new mode 100755 diff --git a/doc/fig/handheld.png b/doc/fig/handheld.png old mode 100644 new mode 100755 diff --git a/doc/fig/imu.png b/doc/fig/imu.png old mode 100644 new mode 100755 diff --git a/doc/fig/kaist1.png b/doc/fig/kaist1.png old mode 100644 new mode 100755 diff --git a/doc/fig/kaist2.png b/doc/fig/kaist2.png old mode 100644 new mode 100755 diff --git a/doc/fig/kitti.png b/doc/fig/kitti.png old mode 100644 new mode 100755 diff --git a/doc/fig/ljj.gif b/doc/fig/ljj.gif old mode 100644 new mode 100755 diff --git a/doc/fig/official-equipment.png b/doc/fig/official-equipment.png old mode 100644 new mode 100755 diff --git a/doc/fig/urbannav.png b/doc/fig/urbannav.png old mode 100644 new mode 100755 diff --git a/doc/paper.pdf b/doc/paper.pdf old mode 100644 new mode 100755 diff --git a/docker/Dockerfile b/docker/Dockerfile old mode 100644 new mode 100755 diff --git a/docker/docker_start.md b/docker/docker_start.md old mode 100644 new mode 100755 diff --git a/docker/ros_entrypoint.sh b/docker/ros_entrypoint.sh old mode 100644 new mode 100755 diff --git a/launch/KAIST.launch b/launch/KAIST.launch old mode 100644 new mode 100755 diff --git a/launch/KITTI.launch b/launch/KITTI.launch old mode 100644 new mode 100755 index 0889bbd..02775d2 --- a/launch/KITTI.launch +++ b/launch/KITTI.launch @@ -11,4 +11,8 @@ + + + + diff --git a/launch/M2DGR.launch b/launch/M2DGR.launch old mode 100644 new mode 100755 diff --git a/launch/UrbanNavDataset.launch b/launch/UrbanNavDataset.launch old mode 100644 new mode 100755 diff --git a/launch/backbag.launch b/launch/backbag.launch old mode 100644 new mode 100755 diff --git a/launch/evaluate_with_evo_gui.py b/launch/evaluate_with_evo_gui.py new file mode 100644 index 0000000..817804f --- /dev/null +++ b/launch/evaluate_with_evo_gui.py @@ -0,0 +1,289 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Simple Tkinter GUI wrapper for evaluate_with_evo.py: +- Select KITTI raw dir, estimated/GT bag, topics, and output dir +- One-click to export TUM and run evo APE/RPE + +Notes: +- Reading bag requires ROS (noetic) Python environment (`import rosbag`). +- GUI requires a graphical environment; on headless servers/containers use the CLI instead. +""" + +import os +import sys +import threading +import tkinter as tk +from tkinter import ttk, filedialog, messagebox + +# Ensure we can import evaluate_with_evo.py in the same directory +SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__)) +if SCRIPT_DIR not in sys.path: + sys.path.insert(0, SCRIPT_DIR) + +try: + from evaluate_with_evo import ( + kitti_oxts_to_tum, + bag_to_tum, + run_evo_ape, + run_evo_rpe, + ensure_dir, + write_tum, + normalize_timebases, + ) +except Exception as e: + # Defer import error handling to runtime + pass + +import shutil + + +class EvoGUI(tk.Tk): + def __init__(self): + super().__init__() + self.title("evo Evaluation GUI (KITTI/ROS bag)") + self.geometry("780x560") + self.resizable(True, True) + + # Variables + self.kitti_dir = tk.StringVar() + self.bag_est = tk.StringVar() + self.est_topic = tk.StringVar(value="/lvi_sam/lidar/mapping/odometry") + self.bag_gt = tk.StringVar() + self.gt_topic = tk.StringVar() + self.out_dir = tk.StringVar(value=os.path.join(SCRIPT_DIR, "..", "bags", "evals")) + self.eval_type = tk.StringVar(value="both") + self.align_type = tk.StringVar(value="se3") + self.save_table = tk.BooleanVar(value=True) + + self._build_ui() + + def _build_ui(self): + pad = {"padx": 6, "pady": 4} + + frm = ttk.Frame(self) + frm.pack(fill=tk.BOTH, expand=True) + + # Row: KITTI raw directory + row = 0 + ttk.Label(frm, text="KITTI raw directory (optional)").grid(row=row, column=0, sticky=tk.W, **pad) + ttk.Entry(frm, textvariable=self.kitti_dir, width=70).grid(row=row, column=1, sticky=tk.EW, **pad) + ttk.Button(frm, text="Browse", command=self._choose_kitti_dir).grid(row=row, column=2, **pad) + + # Row: estimated bag + row += 1 + ttk.Label(frm, text="Estimated bag path").grid(row=row, column=0, sticky=tk.W, **pad) + ttk.Entry(frm, textvariable=self.bag_est, width=70).grid(row=row, column=1, sticky=tk.EW, **pad) + ttk.Button(frm, text="Browse", command=self._choose_bag_est).grid(row=row, column=2, **pad) + + # Row: estimated topic + row += 1 + ttk.Label(frm, text="Estimated topic").grid(row=row, column=0, sticky=tk.W, **pad) + ttk.Entry(frm, textvariable=self.est_topic, width=70).grid(row=row, column=1, sticky=tk.EW, **pad) + ttk.Button(frm, text="List bag topics", command=self._list_topics_est).grid(row=row, column=2, **pad) + + # Row: ground-truth bag (optional) + row += 1 + ttk.Label(frm, text="Ground-truth bag path (optional)").grid(row=row, column=0, sticky=tk.W, **pad) + ttk.Entry(frm, textvariable=self.bag_gt, width=70).grid(row=row, column=1, sticky=tk.EW, **pad) + ttk.Button(frm, text="Browse", command=self._choose_bag_gt).grid(row=row, column=2, **pad) + + # Row: ground-truth topic (optional) + row += 1 + ttk.Label(frm, text="Ground-truth topic (optional)").grid(row=row, column=0, sticky=tk.W, **pad) + ttk.Entry(frm, textvariable=self.gt_topic, width=70).grid(row=row, column=1, sticky=tk.EW, **pad) + ttk.Button(frm, text="List bag topics", command=self._list_topics_gt).grid(row=row, column=2, **pad) + + # Row: output directory + row += 1 + ttk.Label(frm, text="Output directory").grid(row=row, column=0, sticky=tk.W, **pad) + ttk.Entry(frm, textvariable=self.out_dir, width=70).grid(row=row, column=1, sticky=tk.EW, **pad) + ttk.Button(frm, text="Browse", command=self._choose_out_dir).grid(row=row, column=2, **pad) + + # Row: evaluation options + row += 1 + ttk.Label(frm, text="Evaluation type").grid(row=row, column=0, sticky=tk.W, **pad) + ttk.OptionMenu(frm, self.eval_type, self.eval_type.get(), "ape", "rpe", "both").grid(row=row, column=1, sticky=tk.W, **pad) + ttk.Label(frm, text="Alignment").grid(row=row, column=2, sticky=tk.W, **pad) + ttk.OptionMenu(frm, self.align_type, self.align_type.get(), "none", "scale", "se3").grid(row=row, column=3, sticky=tk.W, **pad) + row += 1 + ttk.Checkbutton(frm, text="Save CSV tables", variable=self.save_table).grid(row=row, column=0, sticky=tk.W, **pad) + + # Buttons row + row += 1 + ttk.Button(frm, text="Generate TUM only", command=self._run_generate_only).grid(row=row, column=0, **pad) + ttk.Button(frm, text="Run evaluation", command=self._run_all).grid(row=row, column=1, **pad) + ttk.Button(frm, text="Run APE+RPE (CSV)", command=self._run_both_csv).grid(row=row, column=2, **pad) + ttk.Button(frm, text="Open output dir", command=self._open_out_dir).grid(row=row, column=3, **pad) + ttk.Button(frm, text="Quit", command=self.destroy).grid(row=row, column=4, **pad) + + # Log box + row += 1 + ttk.Label(frm, text="Logs").grid(row=row, column=0, sticky=tk.W, **pad) + self.txt = tk.Text(frm, height=18) + self.txt.grid(row=row, column=0, columnspan=4, sticky=tk.NSEW, padx=6, pady=4) + + # Layout weights + for c in range(0, 4): + frm.grid_columnconfigure(c, weight=1) + frm.grid_rowconfigure(row, weight=1) + + # Pickers + def _choose_kitti_dir(self): + d = filedialog.askdirectory(title="Select KITTI *_sync directory") + if d: + self.kitti_dir.set(d) + + def _choose_bag_est(self): + f = filedialog.askopenfilename(title="Select estimated bag", filetypes=[["ROS bag", "*.bag"], ["All", "*.*"]]) + if f: + self.bag_est.set(f) + + def _choose_bag_gt(self): + f = filedialog.askopenfilename(title="Select ground-truth bag", filetypes=[["ROS bag", "*.bag"], ["All", "*.*"]]) + if f: + self.bag_gt.set(f) + + def _choose_out_dir(self): + d = filedialog.askdirectory(title="Select output directory") + if d: + self.out_dir.set(d) + + # Logging helpers + def log(self, s: str): + self.txt.insert(tk.END, s + "\n") + self.txt.see(tk.END) + self.update_idletasks() + + def _list_topics(self, bag_path: str): + if not bag_path or not os.path.isfile(bag_path): + messagebox.showwarning("Tip", "Please choose a valid bag file") + return + try: + import rosbag + except Exception as e: + messagebox.showerror("Error", "Failed to import rosbag. Source your ROS environment or install ROS Python deps.") + return + try: + from collections import Counter + types = {} + with rosbag.Bag(bag_path, 'r') as bag: + for conn in bag._get_connections(): + types[conn.topic] = conn.datatype + self.log("Bag topics:") + for t, ty in sorted(types.items()): + self.log(f" {t} : {ty}") + except Exception as e: + messagebox.showerror("Error", f"Failed to read bag: {e}") + + def _list_topics_est(self): + self._list_topics(self.bag_est.get()) + + def _list_topics_gt(self): + self._list_topics(self.bag_gt.get()) + + # 执行逻辑 + def _run_thread(self, do_eval: bool, force_eval_type: str = None, force_save_table: bool = None): + try: + out_dir = self.out_dir.get().strip() + ensure_dir(out_dir) + gt_tum = os.path.join(out_dir, 'gt.tum') + est_tum = os.path.join(out_dir, 'est.tum') + + # Ground truth source + if self.bag_gt.get().strip() and self.gt_topic.get().strip(): + self.log(f"Extract GT from bag: {self.bag_gt.get()} [{self.gt_topic.get()}]") + gt = bag_to_tum(self.bag_gt.get().strip(), self.gt_topic.get().strip()) + self.log(f"GT frames: {len(gt)}") + elif self.kitti_dir.get().strip(): + self.log(f"Generate GT from KITTI raw: {self.kitti_dir.get()}") + gt = kitti_oxts_to_tum(self.kitti_dir.get().strip()) + self.log(f"GT frames: {len(gt)}") + else: + messagebox.showerror("Error", "Please provide GT source: KITTI dir or (bag + topic)") + return + + # Estimated trajectory + if not self.bag_est.get().strip(): + messagebox.showerror("Error", "Please provide estimated bag path") + return + self.log(f"Extract estimated trajectory from bag: {self.bag_est.get()} [{self.est_topic.get()}]") + est = bag_to_tum(self.bag_est.get().strip(), self.est_topic.get().strip()) + self.log(f"EST frames: {len(est)}") + + gt, est, gt_offset, est_offset = normalize_timebases(gt, est) + if abs(gt_offset) > 1e-6 or abs(est_offset) > 1e-6: + self.log( + f"Applied timestamp offsets: gt {gt_offset:+.6f} s, est {est_offset:+.6f} s" + ) + + write_tum(gt_tum, gt) + write_tum(est_tum, est) + self.log(f"Saved TUM files to: {gt_tum} and {est_tum}") + + if not do_eval: + self.log("TUM export done. Skipping evo evaluation.") + return + + # evo check + have_ape = shutil.which('evo_ape') is not None + have_rpe = shutil.which('evo_rpe') is not None + if not (have_ape and have_rpe): + self.log("evo not detected. Install via `pip3 install evo`. Skipping evaluation.") + return + + # evaluation + et = force_eval_type if force_eval_type else self.eval_type.get() + align = self.align_type.get() + save_table = self.save_table.get() if force_save_table is None else force_save_table + if et in ("ape", "both"): + self.log("[evo_ape] start...") + run_evo_ape(gt_tum, est_tum, out_dir, align=align, save_table=save_table) + self.log("[evo_ape] done.") + if et in ("rpe", "both"): + self.log("[evo_rpe] start...") + run_evo_rpe(gt_tum, est_tum, out_dir, align=align, save_table=save_table) + self.log("[evo_rpe] done.") + + self.log("All done.") + messagebox.showinfo("Done", f"Output dir: {out_dir}") + + except Exception as e: + messagebox.showerror("Error", str(e)) + self.log(f"Error: {e}") + + def _run_all(self): + threading.Thread(target=self._run_thread, args=(True,), daemon=True).start() + + def _run_generate_only(self): + threading.Thread(target=self._run_thread, args=(False,), daemon=True).start() + + def _run_both_csv(self): + threading.Thread(target=self._run_thread, args=(True, 'both', True), daemon=True).start() + + def _open_out_dir(self): + d = self.out_dir.get().strip() + if not d: + messagebox.showwarning("Tip", "Please set the output directory first") + return + os.makedirs(d, exist_ok=True) + # Try to open in host browser or file manager + try: + if sys.platform.startswith('linux'): + # 在 devcontainer 中可尝试 xdg-open；若失败请手动打开 + os.system(f'xdg-open "{d}" >/dev/null 2>&1 &') + elif sys.platform == 'darwin': + os.system(f'open "{d}"') + elif os.name == 'nt': + os.startfile(d) # type: ignore + except Exception: + pass + + +def main(): + app = EvoGUI() + app.mainloop() + + +if __name__ == '__main__': + main() diff --git a/launch/include/config/robot.urdf.xacro b/launch/include/config/robot.urdf.xacro old mode 100644 new mode 100755 diff --git a/launch/include/config/rviz.rviz b/launch/include/config/rviz.rviz old mode 100644 new mode 100755 diff --git a/launch/include/module_robot_state_publisher.launch b/launch/include/module_robot_state_publisher.launch old mode 100644 new mode 100755 diff --git a/launch/include/module_rviz.launch b/launch/include/module_rviz.launch old mode 100644 new mode 100755 diff --git a/launch/include/module_sam.launch b/launch/include/module_sam.launch old mode 100644 new mode 100755 diff --git a/launch/include/rosconsole/rosconsole_error.conf b/launch/include/rosconsole/rosconsole_error.conf old mode 100644 new mode 100755 diff --git a/launch/include/rosconsole/rosconsole_info.conf b/launch/include/rosconsole/rosconsole_info.conf old mode 100644 new mode 100755 diff --git a/launch/include/rosconsole/rosconsole_warn.conf b/launch/include/rosconsole/rosconsole_warn.conf old mode 100644 new mode 100755 diff --git a/launch/ljj.launch b/launch/ljj.launch old mode 100644 new mode 100755 diff --git a/launch/run.launch b/launch/run.launch old mode 100644 new mode 100755 diff --git a/launch/run_official.launch b/launch/run_official.launch old mode 100644 new mode 100755 diff --git a/model/yolov8n-seg.onnx b/model/yolov8n-seg.onnx new file mode 100644 index 0000000..9ff0616 Binary files /dev/null and b/model/yolov8n-seg.onnx differ diff --git a/model/yolov8n.onnx b/model/yolov8n.onnx new file mode 100644 index 0000000..9490a33 Binary files /dev/null and b/model/yolov8n.onnx differ diff --git a/msg/cloud_info.msg b/msg/cloud_info.msg old mode 100644 new mode 100755 index 75ed399..29df9fd --- a/msg/cloud_info.msg +++ b/msg/cloud_info.msg @@ -26,6 +26,9 @@ float32 odomYaw # Odometry reset ID int64 odomResetId +# Scan end time for odometry timestamp +float64 scanEndTime + # Point cloud messages sensor_msgs/PointCloud2 cloud_deskewed # original cloud deskewed sensor_msgs/PointCloud2 cloud_corner # extracted corner feature diff --git a/package.xml b/package.xml old mode 100644 new mode 100755 index dc92b73..deed828 --- a/package.xml +++ b/package.xml @@ -41,4 +41,10 @@ GTSAM GTSAM + + pcl_ros + pcl_ros + ceres-solver + ceres-solver + diff --git a/src/lidar_odometry/featureExtraction.cpp b/src/lidar_odometry/featureExtraction.cpp old mode 100644 new mode 100755 diff --git a/src/lidar_odometry/imageProjection.cpp b/src/lidar_odometry/imageProjection.cpp old mode 100644 new mode 100755 index e55c7d9..2023d83 --- a/src/lidar_odometry/imageProjection.cpp +++ b/src/lidar_odometry/imageProjection.cpp @@ -557,6 +557,8 @@ class ImageProjection : public ParamServer void publishClouds() { cloudInfo.header = cloudHeader; + cloudInfo.scanEndTime = timeScanNext; + ROS_INFO("scanEndTime: %f, timeScanCur: %f, timeScanNext: %f, cloudQueue.size(): %d", timeScanNext, timeScanCur, timeScanNext, (int)cloudQueue.size()); cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, "base_link"); pubLaserCloudInfo.publish(cloudInfo); } diff --git a/src/lidar_odometry/imuPreintegration.cpp b/src/lidar_odometry/imuPreintegration.cpp old mode 100644 new mode 100755 index a08fdc4..9fad9d2 --- a/src/lidar_odometry/imuPreintegration.cpp +++ b/src/lidar_odometry/imuPreintegration.cpp @@ -106,7 +106,7 @@ class IMUPreintegration : public ParamServer priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e2); // m/s priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good - correctionNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-2); // meter + correctionNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-1); // meter, set to low weight for scan-to-map noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished(); imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread @@ -137,6 +137,19 @@ class IMUPreintegration : public ParamServer void odometryHandler(const nav_msgs::Odometry::ConstPtr &odomMsg) { double currentCorrectionTime = ROS_TIME(odomMsg); + ROS_INFO("currentCorrectionTime = %f", currentCorrectionTime); + + static double prevCorrectionTime = -1; + if (prevCorrectionTime > 0 && currentCorrectionTime <= prevCorrectionTime) { + double time_diff = prevCorrectionTime - currentCorrectionTime; + if (time_diff > 1.0) { // 允许 1s 内的跳跃 + ROS_WARN("Non-monotonic odometry time: %f <= %f, diff=%.6f, skipping", currentCorrectionTime, prevCorrectionTime, time_diff); + return; + } else { + ROS_WARN("Small non-monotonic time diff: %.6f, continuing", time_diff); + } + } + prevCorrectionTime = currentCorrectionTime; // make sure we have imu data to integrate if (imuQueOpt.empty()) @@ -256,6 +269,11 @@ class IMUPreintegration : public ParamServer } // add imu factor to graph const gtsam::PreintegratedImuMeasurements &preint_imu = dynamic_cast(*imuIntegratorOpt_); + ROS_INFO("deltaTij = %f", imuIntegratorOpt_->deltaTij()); + if (imuIntegratorOpt_->deltaTij() == 0) { + ROS_WARN("deltaTij = 0, skipping IMU factor"); + return; + } gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu); graphFactors.add(imu_factor); // add imu bias between factor @@ -311,7 +329,6 @@ class IMUPreintegration : public ParamServer sensor_msgs::Imu *thisImu = &imuQueImu[i]; double imuTime = ROS_TIME(thisImu); double dt = (lastImuQT < 0) ? (1.0 / 500.0) : (imuTime - lastImuQT); - imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z), gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt); lastImuQT = imuTime; diff --git a/src/lidar_odometry/mapOptmization.cpp b/src/lidar_odometry/mapOptmization.cpp old mode 100644 new mode 100755 diff --git a/src/lidar_odometry/scan_to_map.cpp b/src/lidar_odometry/scan_to_map.cpp new file mode 100644 index 0000000..851fc86 --- /dev/null +++ b/src/lidar_odometry/scan_to_map.cpp @@ -0,0 +1,457 @@ +// scan_to_map.cpp +// Minimal LOAM-style scan-to-map optimizer (corner/surf + LM) +// Ported into LVI-SAM-Easyused/lidar_odometry/src/ for integration. +// Note: Add this file to CMakeLists.txt of the package to compile. No build is performed here. + +#include "utility.h" +#include "lvi_sam/cloud_info.h" + +#include +#include +#include +#include +#include + +using namespace gtsam; + +using namespace std; + +class ScanToMap : public ParamServer +{ +public: + ScanToMap() + { + subLaserCloudInfo = nh.subscribe(PROJECT_NAME + "/lidar/feature/cloud_info", 5, &ScanToMap::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay()); + + pubOdomAftMappedROS = nh.advertise(PROJECT_NAME + "/lidar/mapping/odometry", 1); + pubKeyPoses = nh.advertise(PROJECT_NAME + "/lidar/mapping/trajectory", 1); + pubRecentKeyFrame = nh.advertise(PROJECT_NAME + "/lidar/mapping/cloud_registered", 1); + + // Load tuning parameters + nh.param(PROJECT_NAME + "/scan_to_map/max_iter_count", max_iter_count, 30); + nh.param(PROJECT_NAME + "/scan_to_map/convergence_deltaR", convergence_deltaR, 1e-3); + nh.param(PROJECT_NAME + "/scan_to_map/convergence_deltaT", convergence_deltaT, 1e-3); + nh.param(PROJECT_NAME + "/scan_to_map/min_laserCloudSelNum", min_laserCloudSelNum, 50); + nh.param(PROJECT_NAME + "/scan_to_map/imu_weight", imu_weight, 0.01); + + allocateMemory(); + ROS_INFO("ScanToMap node started with tuning params: max_iter=%d, deltaR=%.6f, deltaT=%.6f, min_sel=%d, imu_w=%.4f", + max_iter_count, convergence_deltaR, convergence_deltaT, min_laserCloudSelNum, imu_weight); + } + +private: + ros::Subscriber subLaserCloudInfo; + ros::Publisher pubOdomAftMappedROS; + ros::Publisher pubKeyPoses; + ros::Publisher pubRecentKeyFrame; + + // Tuning parameters + int max_iter_count; + float convergence_deltaR; + float convergence_deltaT; + int min_laserCloudSelNum; + double imu_weight; + + lvi_sam::cloud_info cloudInfo; + pcl::PointCloud::Ptr laserCloudCornerLast; + pcl::PointCloud::Ptr laserCloudSurfLast; + pcl::PointCloud::Ptr laserCloudCornerLastDS; + pcl::PointCloud::Ptr laserCloudSurfLastDS; + + pcl::PointCloud::Ptr laserCloudCornerFromMapDS; + pcl::PointCloud::Ptr laserCloudSurfFromMapDS; + + pcl::PointCloud::Ptr laserCloudOri; + pcl::PointCloud::Ptr coeffSel; + + std::vector laserCloudOriCornerVec; + std::vector coeffSelCornerVec; + std::vector laserCloudOriCornerFlag; + std::vector laserCloudOriSurfVec; + std::vector coeffSelSurfVec; + std::vector laserCloudOriSurfFlag; + + pcl::KdTreeFLANN::Ptr kdtreeCornerFromMap; + pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap; + + pcl::VoxelGrid downSizeFilterCorner; + pcl::VoxelGrid downSizeFilterSurf; + pcl::VoxelGrid downSizeFilterICP; + + int laserCloudCornerLastDSNum = 0; + int laserCloudSurfLastDSNum = 0; + + float transformTobeMapped[6]; + Eigen::Affine3f transPointAssociateToMap; + + bool isDegenerate = false; + cv::Mat matP; + + void allocateMemory() + { + laserCloudCornerLast.reset(new pcl::PointCloud()); + laserCloudSurfLast.reset(new pcl::PointCloud()); + laserCloudCornerLastDS.reset(new pcl::PointCloud()); + laserCloudSurfLastDS.reset(new pcl::PointCloud()); + + laserCloudCornerFromMapDS.reset(new pcl::PointCloud()); + laserCloudSurfFromMapDS.reset(new pcl::PointCloud()); + + laserCloudOri.reset(new pcl::PointCloud()); + coeffSel.reset(new pcl::PointCloud()); + + laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN); + coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN); + laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN); + laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN); + coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN); + laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN); + + std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false); + std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false); + + kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN()); + kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN()); + + downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize); + downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize); + downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize); + + for (int i = 0; i < 6; ++i) transformTobeMapped[i] = 0; + } + + void laserCloudInfoHandler(const lvi_sam::cloud_infoConstPtr &msgIn) + { + cloudInfo = *msgIn; + pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast); + pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast); + + downsampleCurrentScan(); + + // build trivial map from last DS (for demo) — in real use-case use keyframe map + laserCloudCornerFromMapDS = laserCloudCornerLastDS; + laserCloudSurfFromMapDS = laserCloudSurfLastDS; + + if (laserCloudCornerFromMapDS->empty() || laserCloudSurfFromMapDS->empty()) + { + ROS_WARN("map empty, skipping scan2map"); + return; + } + + kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS); + kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS); + + // prepare and run scan2map optimization + scan2MapOptimization(); + + static double lastScanEndTime = 0; + if (cloudInfo.scanEndTime <= lastScanEndTime) { + cloudInfo.scanEndTime = lastScanEndTime + 0.1; + } + lastScanEndTime = cloudInfo.scanEndTime; + + publishOdometry(ros::Time(cloudInfo.scanEndTime)); + } + + void downsampleCurrentScan() + { + laserCloudCornerLastDS->clear(); + downSizeFilterCorner.setInputCloud(laserCloudCornerLast); + downSizeFilterCorner.filter(*laserCloudCornerLastDS); + laserCloudCornerLastDSNum = laserCloudCornerLastDS->size(); + + laserCloudSurfLastDS->clear(); + downSizeFilterSurf.setInputCloud(laserCloudSurfLast); + downSizeFilterSurf.filter(*laserCloudSurfLastDS); + laserCloudSurfLastDSNum = laserCloudSurfLastDS->size(); + } + + void updatePointAssociateToMap() + { + transPointAssociateToMap = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]); + } + + void cornerOptimization() + { + updatePointAssociateToMap(); + + for (int i = 0; i < laserCloudCornerLastDSNum; i++) + { + PointType pointOri = laserCloudCornerLastDS->points[i]; + PointType pointSel; + pointAssociateToMap(&pointOri, &pointSel); + + std::vector pointSearchInd(5); + std::vector pointSearchSqDis(5); + if (kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis) < 5) continue; + + cv::Mat matA1(3,3,CV_32F,cv::Scalar::all(0)); + cv::Mat matD1(1,3,CV_32F,cv::Scalar::all(0)); + cv::Mat matV1(3,3,CV_32F,cv::Scalar::all(0)); + + if (pointSearchSqDis[4] < 1.0) + { + float cx=0,cy=0,cz=0; + for (int j=0;j<5;j++){ + cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x; + cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y; + cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z; + } + cx/=5;cy/=5;cz/=5; + float a11=0,a12=0,a13=0,a22=0,a23=0,a33=0; + for (int j=0;j<5;j++){ + float ax=laserCloudCornerFromMapDS->points[pointSearchInd[j]].x-cx; + float ay=laserCloudCornerFromMapDS->points[pointSearchInd[j]].y-cy; + float az=laserCloudCornerFromMapDS->points[pointSearchInd[j]].z-cz; + a11+=ax*ax; a12+=ax*ay; a13+=ax*az; a22+=ay*ay; a23+=ay*az; a33+=az*az; + } + a11/=5; a12/=5; a13/=5; a22/=5; a23/=5; a33/=5; + matA1.at(0,0)=a11; matA1.at(0,1)=a12; matA1.at(0,2)=a13; + matA1.at(1,0)=a12; matA1.at(1,1)=a22; matA1.at(1,2)=a23; + matA1.at(2,0)=a13; matA1.at(2,1)=a23; matA1.at(2,2)=a33; + cv::eigen(matA1, matD1, matV1); + if (matD1.at(0,0) > 3*matD1.at(0,1)){ + float x0=pointSel.x, y0=pointSel.y, z0=pointSel.z; + float x1 = cx + 0.1*matV1.at(0,0); + float y1 = cy + 0.1*matV1.at(0,1); + float z1 = cz + 0.1*matV1.at(0,2); + float x2 = cx - 0.1*matV1.at(0,0); + float y2 = cy - 0.1*matV1.at(0,1); + float z2 = cz - 0.1*matV1.at(0,2); + float a012 = sqrt(((x0-x1)*(y0-y2)-(x0-x2)*(y0-y1))*((x0-x1)*(y0-y2)-(x0-x2)*(y0-y1)) + + ((x0-x1)*(z0-z2)-(x0-x2)*(z0-z1))*((x0-x1)*(z0-z2)-(x0-x2)*(z0-z1)) + + ((y0-y1)*(z0-z2)-(y0-y2)*(z0-z1))*((y0-y1)*(z0-z2)-(y0-y2)*(z0-z1)) ); + float l12 = sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) + (z1-z2)*(z1-z2)); + float la = ((y1-y2)*((x0-x1)*(y0-y2)-(x0-x2)*(y0-y1)) + (z1-z2)*((x0-x1)*(z0-z2)-(x0-x2)*(z0-z1)))/a012/l12; + float lb = -((x1-x2)*((x0-x1)*(y0-y2)-(x0-x2)*(y0-y1)) - (z1-z2)*((y0-y1)*(z0-z2)-(y0-y2)*(z0-z1)))/a012/l12; + float lc = -((x1-x2)*((x0-x1)*(z0-z2)-(x0-x2)*(z0-z1)) + (y1-y2)*((y0-y1)*(z0-z2)-(y0-y2)*(z0-z1)))/a012/l12; + float ld2 = a012 / l12; + float s = 1 - 0.9*fabs(ld2); + PointType coeff; coeff.x = s*la; coeff.y = s*lb; coeff.z = s*lc; coeff.intensity = s*ld2; + if (s>0.1){ + laserCloudOriCornerVec[i]=pointOri; + coeffSelCornerVec[i]=coeff; + laserCloudOriCornerFlag[i]=true; + } + } + } + } + } + + void surfOptimization() + { + updatePointAssociateToMap(); + + for (int i = 0; i < laserCloudSurfLastDSNum; i++) + { + PointType pointOri = laserCloudSurfLastDS->points[i]; + PointType pointSel; + pointAssociateToMap(&pointOri, &pointSel); + + std::vector pointSearchInd(5); + std::vector pointSearchSqDis(5); + if (kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis) < 5) continue; + + Eigen::Matrix matA0; matA0.setZero(); + Eigen::Matrix matB0; matB0.fill(-1); + Eigen::Vector3f matX0; matX0.setZero(); + if (pointSearchSqDis[4] < 1.0) + { + for (int j=0;j<5;j++){ + matA0(j,0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x; + matA0(j,1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y; + matA0(j,2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z; + } + matX0 = matA0.colPivHouseholderQr().solve(matB0); + float pa=matX0(0,0), pb=matX0(1,0), pc=matX0(2,0), pd=1; + float ps = sqrt(pa*pa + pb*pb + pc*pc); + pa/=ps; pb/=ps; pc/=ps; pd/=ps; + bool planeValid=true; + for (int j=0;j<5;j++){ + if (fabs(pa*laserCloudSurfFromMapDS->points[pointSearchInd[j]].x + pb*laserCloudSurfFromMapDS->points[pointSearchInd[j]].y + pc*laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2){ planeValid=false; break; } + } + if (planeValid){ + float pd2 = pa*pointSel.x + pb*pointSel.y + pc*pointSel.z + pd; + float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x*pointSel.x + pointSel.y*pointSel.y + pointSel.z*pointSel.z)); + PointType coeff; coeff.x = s*pa; coeff.y = s*pb; coeff.z = s*pc; coeff.intensity = s*pd2; + if (s>0.1){ + laserCloudOriSurfVec[i]=pointOri; + coeffSelSurfVec[i]=coeff; + laserCloudOriSurfFlag[i]=true; + } + } + } + } + } + + void combineOptimizationCoeffs() + { + for (int i=0;ipush_back(laserCloudOriCornerVec[i]); coeffSel->push_back(coeffSelCornerVec[i]); } + } + for (int i=0;ipush_back(laserCloudOriSurfVec[i]); coeffSel->push_back(coeffSelSurfVec[i]); } + } + std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false); + std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false); + } + + bool LMOptimization(int iterCount) + { + float srx = sin(transformTobeMapped[1]); + float crx = cos(transformTobeMapped[1]); + float sry = sin(transformTobeMapped[2]); + float cry = cos(transformTobeMapped[2]); + float srz = sin(transformTobeMapped[0]); + float crz = cos(transformTobeMapped[0]); + + int laserCloudSelNum = laserCloudOri->size(); + if (laserCloudSelNum < min_laserCloudSelNum) { + ROS_WARN("Not enough optimization coeffs: %d (min: %d)", laserCloudSelNum, min_laserCloudSelNum); + return false; + } + + cv::Mat matA(laserCloudSelNum,6,CV_32F,cv::Scalar::all(0)); + cv::Mat matB(laserCloudSelNum,1,CV_32F,cv::Scalar::all(0)); + + for (int i=0;ipoints[i]; + PointType coeff = coeffSel->points[i]; + // lidar->camera mapping as in original + PointType p; + p.x = pointOri.y; p.y = pointOri.z; p.z = pointOri.x; + PointType c; c.x = coeff.y; c.y = coeff.z; c.z = coeff.x; c.intensity = coeff.intensity; + + float arx = (crx * sry * srz * p.x + crx * crz * sry * p.y - srx * sry * p.z) * c.x + (-srx * srz * p.x - crz * srx * p.y - crx * p.z) * c.y + (crx * cry * srz * p.x + crx * cry * crz * p.y - cry * srx * p.z) * c.z; + float ary = ((cry * srx * srz - crz * sry) * p.x + (sry * srz + cry * crz * srx) * p.y + crx * cry * p.z) * c.x + ((-cry * crz - srx * sry * srz) * p.x + (cry * srz - crz * srx * sry) * p.y - crx * sry * p.z) * c.z; + float arz = ((crz * srx * sry - cry * srz) * p.x + (-cry * crz - srx * sry * srz) * p.y) * c.x + (crx * crz * p.x - crx * srz * p.y) * c.y + ((sry * srz + cry * crz * srx) * p.x + (crz * sry - cry * srx * srz) * p.y) * c.z; + + matA.at(i,0) = arz; matA.at(i,1) = arx; matA.at(i,2) = ary; + matA.at(i,3) = c.z; matA.at(i,4) = c.x; matA.at(i,5) = c.y; + matB.at(i,0) = -c.intensity; + } + cv::Mat matAt, matAtA, matAtB, matX; + cv::transpose(matA, matAt); + matAtA = matAt * matA; + matAtB = matAt * matB; + cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR); + + transformTobeMapped[0] += matX.at(0,0); + transformTobeMapped[1] += matX.at(1,0); + transformTobeMapped[2] += matX.at(2,0); + transformTobeMapped[3] += matX.at(3,0); + transformTobeMapped[4] += matX.at(4,0); + transformTobeMapped[5] += matX.at(5,0); + + // simple convergence check (magnitude) + float deltaR = sqrt(pow(matX.at(0,0),2) + pow(matX.at(1,0),2) + pow(matX.at(2,0),2)); + float deltaT = sqrt(pow(matX.at(3,0),2) + pow(matX.at(4,0),2) + pow(matX.at(5,0),2)); + if (deltaR < convergence_deltaR && deltaT < convergence_deltaT) + return true; + + return false; + } + + void transformUpdate() + { + // basic IMU-assisted small fusion for roll/pitch if available + if (cloudInfo.imuAvailable == true) + { + if (std::abs(cloudInfo.imuPitchInit) < 1.4) + { + tf::Quaternion imuQuaternion; + tf::Quaternion transformQuaternion; + double rollMid, pitchMid, yawMid; + + // slerp roll + transformQuaternion.setRPY(transformTobeMapped[0], 0, 0); + imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0); + tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imu_weight)).getRPY(rollMid, pitchMid, yawMid); + transformTobeMapped[0] = rollMid; + + // slerp pitch + transformQuaternion.setRPY(0, transformTobeMapped[1], 0); + imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0); + tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imu_weight)).getRPY(rollMid, pitchMid, yawMid); + transformTobeMapped[1] = pitchMid; + } + } + + // apply simple constraints + transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance); + transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance); + transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance); + } + + float constraintTransformation(float value, float limit) + { + if (value < -limit) value = -limit; + if (value > limit) value = limit; + return value; + } + + void scan2MapOptimization() + { + if (laserCloudCornerFromMapDS->empty() || laserCloudSurfFromMapDS->empty()) return; + + if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum) + { + kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS); + kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS); + + for (int iterCount = 0; iterCount < max_iter_count; iterCount++) + { + laserCloudOri->clear(); coeffSel->clear(); + cornerOptimization(); + surfOptimization(); + combineOptimizationCoeffs(); + + if (LMOptimization(iterCount) == true) + break; + } + + transformUpdate(); + } + else + { + ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum); + } + } + + void pointAssociateToMap(PointType const *const pi, PointType *const po) + { + // identity for now (no global transform applied) + po->x = pi->x; po->y = pi->y; po->z = pi->z; po->intensity = pi->intensity; + } + + void publishOdometry(const ros::Time &stamp) + { + nav_msgs::Odometry laserOdometryROS; + laserOdometryROS.header.stamp = stamp; + laserOdometryROS.header.frame_id = "odom"; + laserOdometryROS.child_frame_id = "odom_mapping"; + laserOdometryROS.pose.pose.position.x = transformTobeMapped[3]; + laserOdometryROS.pose.pose.position.y = transformTobeMapped[4]; + laserOdometryROS.pose.pose.position.z = transformTobeMapped[5]; + laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]); + pubOdomAftMappedROS.publish(laserOdometryROS); + + // publish recent key frame (registered) + if (pubRecentKeyFrame.getNumSubscribers() != 0) + { + pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); + *cloudOut += *laserCloudCornerLastDS; + *cloudOut += *laserCloudSurfLastDS; + publishCloud(&pubRecentKeyFrame, cloudOut, ros::Time::now(), "odom"); + } + } +}; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "scan_to_map"); + ScanToMap node; + ros::MultiThreadedSpinner spinner(2); + spinner.spin(); + return 0; +} diff --git a/src/lidar_odometry/utility.h b/src/lidar_odometry/utility.h old mode 100644 new mode 100755 index bf14999..a6c9201 --- a/src/lidar_odometry/utility.h +++ b/src/lidar_odometry/utility.h @@ -14,7 +14,7 @@ #include #include -#include +#include #include #include diff --git a/src/visual_odometry/visual_estimator/estimator.cpp b/src/visual_odometry/visual_estimator/estimator.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/estimator.h b/src/visual_odometry/visual_estimator/estimator.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/estimator_node.cpp b/src/visual_odometry/visual_estimator/estimator_node.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/factor/imu_factor.h b/src/visual_odometry/visual_estimator/factor/imu_factor.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/factor/integration_base.h b/src/visual_odometry/visual_estimator/factor/integration_base.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/factor/marginalization_factor.cpp b/src/visual_odometry/visual_estimator/factor/marginalization_factor.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/factor/marginalization_factor.h b/src/visual_odometry/visual_estimator/factor/marginalization_factor.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/factor/pose_local_parameterization.cpp b/src/visual_odometry/visual_estimator/factor/pose_local_parameterization.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/factor/pose_local_parameterization.h b/src/visual_odometry/visual_estimator/factor/pose_local_parameterization.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/factor/projection_factor.cpp b/src/visual_odometry/visual_estimator/factor/projection_factor.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/factor/projection_factor.h b/src/visual_odometry/visual_estimator/factor/projection_factor.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/factor/projection_td_factor.cpp b/src/visual_odometry/visual_estimator/factor/projection_td_factor.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/factor/projection_td_factor.h b/src/visual_odometry/visual_estimator/factor/projection_td_factor.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/feature_manager.cpp b/src/visual_odometry/visual_estimator/feature_manager.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/feature_manager.h b/src/visual_odometry/visual_estimator/feature_manager.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/initial/initial_aligment.cpp b/src/visual_odometry/visual_estimator/initial/initial_aligment.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/initial/initial_alignment.h b/src/visual_odometry/visual_estimator/initial/initial_alignment.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/initial/initial_ex_rotation.cpp b/src/visual_odometry/visual_estimator/initial/initial_ex_rotation.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/initial/initial_ex_rotation.h b/src/visual_odometry/visual_estimator/initial/initial_ex_rotation.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/initial/initial_sfm.cpp b/src/visual_odometry/visual_estimator/initial/initial_sfm.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/initial/initial_sfm.h b/src/visual_odometry/visual_estimator/initial/initial_sfm.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/initial/solve_5pts.cpp b/src/visual_odometry/visual_estimator/initial/solve_5pts.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/initial/solve_5pts.h b/src/visual_odometry/visual_estimator/initial/solve_5pts.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/parameters.cpp b/src/visual_odometry/visual_estimator/parameters.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/parameters.h b/src/visual_odometry/visual_estimator/parameters.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/utility/CameraPoseVisualization.cpp b/src/visual_odometry/visual_estimator/utility/CameraPoseVisualization.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/utility/CameraPoseVisualization.h b/src/visual_odometry/visual_estimator/utility/CameraPoseVisualization.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/utility/tic_toc.h b/src/visual_odometry/visual_estimator/utility/tic_toc.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/utility/utility.cpp b/src/visual_odometry/visual_estimator/utility/utility.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/utility/utility.h b/src/visual_odometry/visual_estimator/utility/utility.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/utility/visualization.cpp b/src/visual_odometry/visual_estimator/utility/visualization.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_estimator/utility/visualization.h b/src/visual_odometry/visual_estimator/utility/visualization.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/Camera.cc b/src/visual_odometry/visual_feature/camera_models/Camera.cc old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/Camera.h b/src/visual_odometry/visual_feature/camera_models/Camera.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/CameraFactory.cc b/src/visual_odometry/visual_feature/camera_models/CameraFactory.cc old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/CameraFactory.h b/src/visual_odometry/visual_feature/camera_models/CameraFactory.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/CataCamera.cc b/src/visual_odometry/visual_feature/camera_models/CataCamera.cc old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/CataCamera.h b/src/visual_odometry/visual_feature/camera_models/CataCamera.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/CostFunctionFactory.cc b/src/visual_odometry/visual_feature/camera_models/CostFunctionFactory.cc old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/CostFunctionFactory.h b/src/visual_odometry/visual_feature/camera_models/CostFunctionFactory.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/EquidistantCamera.cc b/src/visual_odometry/visual_feature/camera_models/EquidistantCamera.cc old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/EquidistantCamera.h b/src/visual_odometry/visual_feature/camera_models/EquidistantCamera.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/PinholeCamera.cc b/src/visual_odometry/visual_feature/camera_models/PinholeCamera.cc old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/PinholeCamera.h b/src/visual_odometry/visual_feature/camera_models/PinholeCamera.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/ScaramuzzaCamera.cc b/src/visual_odometry/visual_feature/camera_models/ScaramuzzaCamera.cc old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/ScaramuzzaCamera.h b/src/visual_odometry/visual_feature/camera_models/ScaramuzzaCamera.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/gpl.cc b/src/visual_odometry/visual_feature/camera_models/gpl.cc old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/camera_models/gpl.h b/src/visual_odometry/visual_feature/camera_models/gpl.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/feature_tracker.cpp b/src/visual_odometry/visual_feature/feature_tracker.cpp old mode 100644 new mode 100755 index d33709c..aafb3c9 --- a/src/visual_odometry/visual_feature/feature_tracker.cpp +++ b/src/visual_odometry/visual_feature/feature_tracker.cpp @@ -304,3 +304,211 @@ void FeatureTracker::undistortedPoints() } prev_un_pts_map = cur_un_pts_map; } + +void yoloDetect::using_once() +{ + std::call_once(flag_, [this](){this->init();}); +} + +void yoloDetect::init() +{ + try + { + detectioNet = cv::dnn::readNetFromONNX(model_path); + } + catch (const cv::Exception& e) + { + ROS_ERROR("读取模型失败: %s", e.what()); + } + +} + +dataImg yoloDetect::preprocess(cv::Mat &img) +{ + if (img.empty()) + { + std::cout<<"lab is empty"<(tw) / w, static_cast(th) / h); + scale = std::max(scale, 0.01f); + int new_w = static_cast(w * scale); + int new_h = static_cast(h * scale); + + // 3.缩放图像 + cv::Mat resized_img; + resize(bgr_img, resized_img, cv::Size(new_w, new_h)); + + // 4.计算填充量 + int padW = tw - new_w; + int padH = th - new_h; + + int left = padW / 2; + int right = padW - left; + int top = padH / 2; + int bottom = padH - top; + + // 5.填充图像 + cv::Mat padded_img; + copyMakeBorder(resized_img, padded_img, top, bottom, left, right, cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0)); + + // 6.创建blob + cv::Mat blob = cv::dnn::blobFromImage(padded_img, 1.0/255.0, cv::Size(target_size, target_size), cv::Scalar(0,0,0), true, CV_32F); + + dataImg imgdata; + imgdata.scale = scale; + imgdata.padW = padW; + imgdata.padH = padH; + imgdata.input = bgr_img.clone(); + imgdata.blob = blob; + + return imgdata; +} + +void yoloDetect::draw(cv::Mat& img, Target& target) +{ + rectangle(img, target.box, cv::Scalar(0, 255, 0), 2); + std::string label = COCO_CLASSES[target.label]; + putText(img, label, cv::Point(target.box.x, target.box.y - 5), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 2); +} + +void yoloDetect::Inference(cv::Mat &img) +{ + if (detectioNet.empty()) + { + std::cout << "模型加载失败" << std::endl; + return; + } + + dataImg imgdata = preprocess(img); + if (imgdata.blob.empty()) { + std::cout << "预处理失败" << std::endl; + return; + } + + detectioNet.setInput(imgdata.blob); + std::vector outputs; + std::vector outNames = detectioNet.getUnconnectedOutLayersNames(); + detectioNet.forward(outputs, outNames); + + if (outputs.empty()) { + std::cout << "模型输出为空" << std::endl; + return; + } + + cv::Mat output = outputs[0]; + + // 根据格式 float32[batch,84,anchors] 解析 + const int batch_size = output.size[0]; // 通常是 1 + const int num_features = output.size[1]; // 84 = 4(bbox) + 80(classes) + const int num_anchors = output.size[2]; // anchors 数量 + + std::cout << "Output shape: [" << batch_size << ", " << num_features << ", " << num_anchors << "]" << std::endl; + + // 重塑为更方便处理的格式: (84, num_anchors) + cv::Mat output_reshaped = output.reshape(1, num_features); + + PreTargets preTargets; + + for (int i = 0; i < num_anchors; i++) + { + // YOLOv8 没有单独的 objectness 分数，直接使用类别分数 + cv::Mat scores = output_reshaped.rowRange(4, num_features).col(i); + + cv::Point class_id_point; + double max_score; + cv::minMaxLoc(scores, 0, &max_score, 0, &class_id_point); + + if (max_score > confidence_threshold_) + { + int class_id = class_id_point.y; + + // 获取边界框信息 (cx, cy, w, h) - 相对于网络输入尺寸 (640x640) + float cx = output_reshaped.at(0, i); + float cy = output_reshaped.at(1, i); + float w = output_reshaped.at(2, i); + float h = output_reshaped.at(3, i); + + // 坐标映射回原始图像 + // 注意：预处理时进行了缩放和填充，需要逆向操作 + float cx_unpad = (cx - imgdata.padW / 2) / imgdata.scale; + float cy_unpad = (cy - imgdata.padH / 2) / imgdata.scale; + float w_unpad = w / imgdata.scale; + float h_unpad = h / imgdata.scale; + + // 计算边界框的左上角坐标和宽高 + int lx = static_cast(cx_unpad - w_unpad / 2); + int ly = static_cast(cy_unpad - h_unpad / 2); + int width = static_cast(w_unpad); + int height = static_cast(h_unpad); + + // 边界检查 + lx = std::max(0, lx); + ly = std::max(0, ly); + width = std::min(width, img.cols - lx); + height = std::min(height, img.rows - ly); + + if (width <= 5 || height <= 5) { + continue; + } + + cv::Rect box(lx, ly, width, height); + preTargets.boxes.push_back(box); + preTargets.confidences.push_back(static_cast(max_score)); + preTargets.labels.push_back(class_id); + } + } + + // NMS 处理 + std::vector indices; + if (!preTargets.boxes.empty()) { + // 按类别分组进行 NMS + std::map> class_to_indices; + for (size_t i = 0; i < preTargets.boxes.size(); i++) { + class_to_indices[preTargets.labels[i]].push_back(i); + } + + for (auto &pair : class_to_indices) { + std::vector class_boxes; + std::vector class_confidences; + std::vector class_indices; + + for (int idx : pair.second) { + class_boxes.push_back(preTargets.boxes[idx]); + class_confidences.push_back(preTargets.confidences[idx]); + class_indices.push_back(idx); + } + + std::vector class_nms_indices; + cv::dnn::NMSBoxes(class_boxes, class_confidences, + confidence_threshold_, nms_threshold_, class_nms_indices); + + for (int nms_idx : class_nms_indices) { + indices.push_back(class_indices[nms_idx]); + } + } + + // 绘制检测结果 + for (auto idx : indices) { + Target target; + target.box = preTargets.boxes[idx]; + target.confidence = preTargets.confidences[idx]; + target.label = preTargets.labels[idx]; + draw(img, target); + } + + std::cout << "检测到 " << indices.size() << " 个目标" << std::endl; + } +} + + diff --git a/src/visual_odometry/visual_feature/feature_tracker.h b/src/visual_odometry/visual_feature/feature_tracker.h old mode 100644 new mode 100755 index 55b6b75..31026de --- a/src/visual_odometry/visual_feature/feature_tracker.h +++ b/src/visual_odometry/visual_feature/feature_tracker.h @@ -361,4 +361,65 @@ class DepthRegister g *= 255.0; b *= 255.0; } -}; \ No newline at end of file +}; + +// COCO数据集的80个类别名称 +const std::vector COCO_CLASSES = { + "person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", + "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", + "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", + "umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", + "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", + "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", + "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", + "chair", "couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", + "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", + "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush" +}; + +struct dataImg +{ + cv::Mat input; + float scale; + int padW; + int padH; + cv::Mat blob; +}; + +struct PreTargets +{ + std::vector boxes; + std::vector confidences; + std::vector labels; +}; + +struct Target +{ + cv::Rect box; + float confidence; + int label; +}; + +class yoloDetect { +public: + explicit yoloDetect() + { + using_once(); + } + + void using_once(); + void init(); + + dataImg preprocess(cv::Mat &img); + void Inference(cv::Mat &img); + void draw(cv::Mat& img, Target& target); + +private: + int target_size = 640; + double confidence_threshold_ = 0.3; + double nms_threshold_ = 0.3; + std::once_flag flag_; + cv::dnn::Net detectioNet; + std::string model_path = "/LVI-SAM/ls_ws/src/LVI-SAM-Easyused/LVI-SAM-Easyused-master/model/yolov8n.onnx"; +}; + diff --git a/src/visual_odometry/visual_feature/feature_tracker_node.cpp b/src/visual_odometry/visual_feature/feature_tracker_node.cpp old mode 100644 new mode 100755 index 94d63fe..75dbf40 --- a/src/visual_odometry/visual_feature/feature_tracker_node.cpp +++ b/src/visual_odometry/visual_feature/feature_tracker_node.cpp @@ -1,8 +1,9 @@ #include "feature_tracker.h" +#include +#include #define SHOW_UNDISTORTION 0 - // mtx lock for two threads std::mutex mtx_lidar; @@ -29,7 +30,8 @@ bool first_image_flag = true; double last_image_time = 0; bool init_pub = 0; - +// yoloDetect* detect = new yoloDetect(); +cv::Mat pub_img; void img_callback(const sensor_msgs::ImageConstPtr &img_msg) { @@ -189,11 +191,14 @@ void img_callback(const sensor_msgs::ImageConstPtr &img_msg) { ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::RGB8); //cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3); - cv::Mat stereo_img = ptr->image; + pub_img = ptr->image; + + ROS_INFO("Load the model."); + // detect->Inference(pub_img); for (int i = 0; i < NUM_OF_CAM; i++) { - cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW); + cv::Mat tmp_img = pub_img.rowRange(i * ROW, (i + 1) * ROW); cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB); for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++) @@ -216,7 +221,8 @@ void img_callback(const sensor_msgs::ImageConstPtr &img_msg) } } - pub_match.publish(ptr->toImageMsg()); + sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", pub_img).toImageMsg(); + pub_match.publish(msg); } } } @@ -349,8 +355,7 @@ void lidar_callback(const sensor_msgs::PointCloud2ConstPtr& laser_msg) *depthCloud = *depthCloudDS; } -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { // initialize ROS node ros::init(argc, argv, "vins"); ros::NodeHandle n; diff --git a/src/visual_odometry/visual_feature/parameters.cpp b/src/visual_odometry/visual_feature/parameters.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_feature/parameters.h b/src/visual_odometry/visual_feature/parameters.h old mode 100644 new mode 100755 index 2458ece..b550a92 --- a/src/visual_odometry/visual_feature/parameters.h +++ b/src/visual_odometry/visual_feature/parameters.h @@ -13,7 +13,7 @@ #include #include -#include +#include #include #include diff --git a/src/visual_odometry/visual_feature/tic_toc.h b/src/visual_odometry/visual_feature/tic_toc.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/BowVector.cpp b/src/visual_odometry/visual_loop/ThirdParty/DBoW/BowVector.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/BowVector.h b/src/visual_odometry/visual_loop/ThirdParty/DBoW/BowVector.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/DBoW2.h b/src/visual_odometry/visual_loop/ThirdParty/DBoW/DBoW2.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/FBrief.cpp b/src/visual_odometry/visual_loop/ThirdParty/DBoW/FBrief.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/FBrief.h b/src/visual_odometry/visual_loop/ThirdParty/DBoW/FBrief.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/FClass.h b/src/visual_odometry/visual_loop/ThirdParty/DBoW/FClass.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/FeatureVector.cpp b/src/visual_odometry/visual_loop/ThirdParty/DBoW/FeatureVector.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/FeatureVector.h b/src/visual_odometry/visual_loop/ThirdParty/DBoW/FeatureVector.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/QueryResults.cpp b/src/visual_odometry/visual_loop/ThirdParty/DBoW/QueryResults.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/QueryResults.h b/src/visual_odometry/visual_loop/ThirdParty/DBoW/QueryResults.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/ScoringObject.cpp b/src/visual_odometry/visual_loop/ThirdParty/DBoW/ScoringObject.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/ScoringObject.h b/src/visual_odometry/visual_loop/ThirdParty/DBoW/ScoringObject.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/TemplatedDatabase.h b/src/visual_odometry/visual_loop/ThirdParty/DBoW/TemplatedDatabase.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DBoW/TemplatedVocabulary.h b/src/visual_odometry/visual_loop/ThirdParty/DBoW/TemplatedVocabulary.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DUtils/DException.h b/src/visual_odometry/visual_loop/ThirdParty/DUtils/DException.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DUtils/DUtils.h b/src/visual_odometry/visual_loop/ThirdParty/DUtils/DUtils.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DUtils/Random.cpp b/src/visual_odometry/visual_loop/ThirdParty/DUtils/Random.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DUtils/Random.h b/src/visual_odometry/visual_loop/ThirdParty/DUtils/Random.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DUtils/Timestamp.cpp b/src/visual_odometry/visual_loop/ThirdParty/DUtils/Timestamp.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DUtils/Timestamp.h b/src/visual_odometry/visual_loop/ThirdParty/DUtils/Timestamp.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DVision/BRIEF.cpp b/src/visual_odometry/visual_loop/ThirdParty/DVision/BRIEF.cpp old mode 100644 new mode 100755 index 6af73c4..49940d1 --- a/src/visual_odometry/visual_loop/ThirdParty/DVision/BRIEF.cpp +++ b/src/visual_odometry/visual_loop/ThirdParty/DVision/BRIEF.cpp @@ -50,7 +50,7 @@ void BRIEF::compute(const cv::Mat &image, cv::Mat aux; if(image.depth() == 3) { - cv::cvtColor(image, aux, CV_RGB2GRAY); + cv::cvtColor(image, aux, cv::COLOR_RGB2GRAY); } else { diff --git a/src/visual_odometry/visual_loop/ThirdParty/DVision/BRIEF.h b/src/visual_odometry/visual_loop/ThirdParty/DVision/BRIEF.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/DVision/DVision.h b/src/visual_odometry/visual_loop/ThirdParty/DVision/DVision.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/VocabularyBinary.cpp b/src/visual_odometry/visual_loop/ThirdParty/VocabularyBinary.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/ThirdParty/VocabularyBinary.hpp b/src/visual_odometry/visual_loop/ThirdParty/VocabularyBinary.hpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/keyframe.cpp b/src/visual_odometry/visual_loop/keyframe.cpp old mode 100644 new mode 100755 index 0f3f6b8..9665cff --- a/src/visual_odometry/visual_loop/keyframe.cpp +++ b/src/visual_odometry/visual_loop/keyframe.cpp @@ -237,10 +237,10 @@ bool KeyFrame::findConnection(KeyFrame* old_kf) int banner_height = (double)100 * MATCH_IMAGE_SCALE; cv::Mat notation(banner_height, thumbnail.size().width + gap + thumbnail.size().width, CV_8UC3, cv::Scalar(255, 255, 255)); putText(notation, "current frame: " + to_string(index), - cv::Point2f(5, banner_height - 5), CV_FONT_HERSHEY_SIMPLEX, + cv::Point2f(5, banner_height - 5), cv::FONT_HERSHEY_SIMPLEX, MATCH_IMAGE_SCALE*2, cv::Scalar(255), 2); putText(notation, "previous frame: " + to_string(old_kf->index), - cv::Point2f(5 + thumbnail.size().width + gap, banner_height - 5), CV_FONT_HERSHEY_SIMPLEX, + cv::Point2f(5 + thumbnail.size().width + gap, banner_height - 5), cv::FONT_HERSHEY_SIMPLEX, MATCH_IMAGE_SCALE*2, cv::Scalar(255), 2); cv::vconcat(notation, loop_match_img, loop_match_img); // publish matched image diff --git a/src/visual_odometry/visual_loop/keyframe.h b/src/visual_odometry/visual_loop/keyframe.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/loop_detection.cpp b/src/visual_odometry/visual_loop/loop_detection.cpp old mode 100644 new mode 100755 index 4abb176..d3d06f8 --- a/src/visual_odometry/visual_loop/loop_detection.cpp +++ b/src/visual_odometry/visual_loop/loop_detection.cpp @@ -61,7 +61,7 @@ int LoopDetector::detectLoop(KeyFrame* keyframe, int frame_index) { int feature_num = keyframe->keypoints.size(); cv::resize(keyframe->image, compressed_image, cv::Size(376, 240)); - putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); + putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); image_pool[frame_index] = compressed_image; } //first query; then add this frame into database! @@ -79,7 +79,7 @@ int LoopDetector::detectLoop(KeyFrame* keyframe, int frame_index) { loop_result = compressed_image.clone(); if (ret.size() > 0) - putText(loop_result, "neighbour score:" + to_string(ret[0].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255)); + putText(loop_result, "neighbour score:" + to_string(ret[0].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255)); } // visual loop result if (DEBUG_IMAGE) @@ -89,7 +89,7 @@ int LoopDetector::detectLoop(KeyFrame* keyframe, int frame_index) int tmp_index = ret[i].Id; auto it = image_pool.find(tmp_index); cv::Mat tmp_image = (it->second).clone(); - putText(tmp_image, "index: " + to_string(tmp_index) + "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255)); + putText(tmp_image, "index: " + to_string(tmp_index) + "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255)); cv::hconcat(loop_result, tmp_image, loop_result); } } @@ -109,7 +109,7 @@ int LoopDetector::detectLoop(KeyFrame* keyframe, int frame_index) int tmp_index = ret[i].Id; auto it = image_pool.find(tmp_index); cv::Mat tmp_image = (it->second).clone(); - putText(tmp_image, "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); + putText(tmp_image, "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); cv::hconcat(loop_result, tmp_image, loop_result); } } @@ -146,7 +146,7 @@ void LoopDetector::addKeyFrameIntoVoc(KeyFrame* keyframe) { int feature_num = keyframe->keypoints.size(); cv::resize(keyframe->image, compressed_image, cv::Size(376, 240)); - putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); + putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255)); image_pool[keyframe->index] = compressed_image; } diff --git a/src/visual_odometry/visual_loop/loop_detection.h b/src/visual_odometry/visual_loop/loop_detection.h old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/loop_detection_node.cpp b/src/visual_odometry/visual_loop/loop_detection_node.cpp old mode 100644 new mode 100755 diff --git a/src/visual_odometry/visual_loop/parameters.h b/src/visual_odometry/visual_loop/parameters.h old mode 100644 new mode 100755 index aad1fa7..76be50a --- a/src/visual_odometry/visual_loop/parameters.h +++ b/src/visual_odometry/visual_loop/parameters.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include