├── .gitignore ├── doc ├── degradtion.png ├── superloc.png ├── superodom.png ├── tested_platform.png └── uncertainty.gif ├── readme.md ├── ros2_humble_docker ├── Dockerfile ├── container_run.sh └── ros_entrypoint.sh ├── script ├── benchmark_livox.yaml ├── benchmark_ouster.yaml ├── benchmark_velodyne.yaml ├── run.yaml ├── save_benchmark_result.py ├── save_imu_vs_icp.py ├── save_superodom_stats.py ├── save_superodom_stats_matplotlib.py └── visualizers │ ├── base.py │ ├── rerun_vis.py │ ├── rerun_visualizer.py │ └── utils.py ├── super_odometry ├── .gitignore ├── CMakeLists.txt ├── cmake │ ├── FindEigen.cmake │ ├── FindGflags.cmake │ └── FindGlog.cmake ├── config │ ├── livox │ │ └── livox_mid360_calibration.yaml │ ├── livox_mid360.yaml │ ├── os1_128.yaml │ ├── ouster │ │ └── os1_128_calibration.yaml │ ├── velodyne │ │ └── vlp_16_calibration.yaml │ └── vlp_16.yaml ├── include │ ├── super_odometry │ │ ├── FeatureExtraction │ │ │ └── featureExtraction.h │ │ ├── ImuPreintegration │ │ │ └── imuPreintegration.h │ │ ├── LaserMapping │ │ │ └── laserMapping.h │ │ ├── LidarProcess │ │ │ ├── LidarSlam.h │ │ │ ├── LocalMap.h │ │ │ └── factor │ │ │ │ ├── SE3AbsolutatePoseFactor.h │ │ │ │ ├── lidarOptimization.h │ │ │ │ └── pose_local_parameterization.h │ │ ├── LocalMap.h │ │ ├── config │ │ │ └── parameter.h │ │ ├── container │ │ │ └── MapRingBuffer.h │ │ ├── flann │ │ │ ├── nanoflann.h │ │ │ └── octree.h │ │ ├── sensor_data │ │ │ ├── imu │ │ │ │ └── imu_data.h │ │ │ └── pointcloud │ │ │ │ ├── LidarPoint.h │ │ │ │ └── point_os.h │ │ ├── tic_toc.h │ │ └── utils │ │ │ ├── EigenTypes.h │ │ │ ├── Twist.h │ │ │ ├── sophus_utils.hpp │ │ │ ├── superodom_utils.h │ │ │ └── utility.h │ └── utility.h ├── launch │ ├── livox_mid360.launch.py │ ├── os1_128.launch.py │ └── vlp_16.launch.py ├── package.xml ├── readme.md ├── ros2.rviz ├── src │ ├── FeatureExtraction │ │ └── featureExtraction.cpp │ ├── ImuPreintegration │ │ └── imuPreintegration.cpp │ ├── LaserMapping │ │ ├── LocalMap.cpp │ │ ├── laserMapping.cpp │ │ └── lidarOptimization.cpp │ ├── LidarProcess │ │ ├── LidarSlam.cpp │ │ ├── SE3AbsolutatePoseFactor.cpp │ │ └── pose_local_parameterization.cpp │ ├── featureExtraction_node.cpp │ ├── imuPreintegration_node.cpp │ ├── laserMapping_node.cpp │ ├── parameter │ │ └── parameter.cpp │ └── utils │ │ └── superodom_utils.cpp └── start_pose.txt └── super_odometry_msgs ├── CMakeLists.txt ├── msg ├── IterationStats.msg ├── LaserFeature.msg └── OptimizationStats.msg └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | ./velodyne_driver_16_32 2 | ./cmake-build-debug/cmake-build-debug 3 | ./vins_estimator/camke-build-debug 4 | ./vins_estimator/camke-build-release 5 | ./super_odometry/camke-build-debug 6 | *.idea 7 | *.vscode 8 | ./build 9 | ./install 10 | ./log 11 | __pycache__ 12 | *.pyc 13 | *.pyo 14 | *.pyd 15 | *.pyw 16 | *.pyz 17 | *.pywz 18 | -------------------------------------------------------------------------------- /doc/degradtion.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/superxslam/SuperOdom/95a1c85baf84579fd8a3da02e84e8b6127703336/doc/degradtion.png -------------------------------------------------------------------------------- /doc/superloc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/superxslam/SuperOdom/95a1c85baf84579fd8a3da02e84e8b6127703336/doc/superloc.png -------------------------------------------------------------------------------- /doc/superodom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/superxslam/SuperOdom/95a1c85baf84579fd8a3da02e84e8b6127703336/doc/superodom.png -------------------------------------------------------------------------------- /doc/tested_platform.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/superxslam/SuperOdom/95a1c85baf84579fd8a3da02e84e8b6127703336/doc/tested_platform.png -------------------------------------------------------------------------------- /doc/uncertainty.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/superxslam/SuperOdom/95a1c85baf84579fd8a3da02e84e8b6127703336/doc/uncertainty.gif -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # SuperOdometry: Lightweight LiDAR-inertial Odometry and Mapping 2 | 3 |
4 | 5 | [![Website](https://img.shields.io/badge/Website-4385f4?style=flat&logo=googlehome&logoColor=white)](https://superodometry.com/) [![License: GPL v3](https://img.shields.io/badge/License-GPL%20v3-blue.svg)](./LICENSE) 6 | 7 |
8 | 9 |

10 | Super Odometry Pipeline 11 |

12 | 13 | > 🔥 This is a slim version of Super Odometry, containing the LiDAR Odometry component and IMU Odometry component. The LiDAR odometry only provides pose constraints to IMU odometry modules to estimate the bias of IMU. In return, the IMU Odometry module offers pose predictions to the LiDAR Odometry module, serving as an initial guess for ICP optimization. 14 |

15 | Super Odometry Pipeline 16 |

17 | 18 | > 🔥 The system has been widely tested on above platforms equipped with Livox, Velodyne and Ouster LiDAR. 19 | 20 | ## 🔥 1. Key Features 21 | 22 | - **Multi-LiDAR Support** 23 | - Compatible with Livox, Velodyne, and Ouster sensors 24 | - **LiDAR-inertial Fusion** 25 | - Support LiDAR-inertial Fusion 26 | - **Dual-Mode Operation** 27 | - Supports both localization and mapping modes 28 | - **Alignment Risk Prediction** 29 | - Provides alignment risk prediction for ICP algorithms 30 | - **Degeneracy Awareness** 31 | - Robust detection of environmental degeneracy 32 | - **ROS 2.0 Integration** 33 | - Built on ROS 2 Humble for modern robotics development 34 | 35 |

36 | Super Odometry Pipeline 37 |

38 | 39 |

40 | Alignment Risk Prediction 41 |

42 | 43 | > 🔥 6 DOF degeneracy uncertainty detection. We support visualization in both RVIZ and Rerun. 44 | 45 | ## 📦 3. Installation 46 | > Highly recommend to check our docker files to run our code with step 4 and step 5. 47 | ### System Requirements 48 | 49 | - ROS2 Humble 50 | - PCL 51 | - Eigen 52 | - [Sophus](https://github.com/strasdat/Sophus) 53 | - [GTSAM (4.0.2 or 4.1)](https://github.com/borglab/gtsam) 54 | - [Ceres Solver](http://ceres-solver.org/) 55 | 56 | ### Dependencies Installation 57 | 58 | #### Install Sophus 59 | ```bash 60 | git clone http://github.com/strasdat/Sophus.git 61 | cd Sophus && git checkout 97e7161 62 | mkdir build && cd build 63 | cmake .. -DBUILD_TESTS=OFF 64 | make -j8 && sudo make install 65 | ``` 66 | 67 | #### Install GTSAM 68 | ```bash 69 | git clone https://github.com/borglab/gtsam.git 70 | cd gtsam && git checkout 4abef92 71 | mkdir build && cd build 72 | cmake \ 73 | -DGTSAM_USE_SYSTEM_EIGEN=ON \ 74 | -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ 75 | .. 76 | make -j6 && sudo make install 77 | ``` 78 | 79 | #### Install Ceres 80 | ```bash 81 | sudo apt-get install -y libceres-dev 82 | ``` 83 | 84 | #### Install Rerun 85 | ```bash 86 | pip install rerun-sdk 87 | ``` 88 | 89 | ## 🐳 4. Docker Setup 90 | 91 | ### Prerequisites 92 | - [Docker](https://www.docker.com/) 93 | - [NVIDIA Docker](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html) 94 | 95 | ### Building Docker Image 96 | ```bash 97 | cd ros2_humble_docker 98 | docker build -t superodom-ros2:latest . 99 | ``` 100 | 101 | ### Workspace Structure 102 | 103 | First create your own local ROS2 workspace and clone `SuperOdom`: 104 | ```bash 105 | mkdir -p ~/ros2_ws/src 106 | cd ~/ros2_ws/src 107 | git clone https://github.com/superxslam/SuperOdom 108 | ``` 109 | Clone respective repos and ensure they follow this exact structure under `ros2_ws/src`: 110 | ``` 111 | ros2_ws/src 112 | ├── SuperOdom 113 | ├── livox_ros_driver2 114 | └── rviz_2d_overlay_plugins 115 | ``` 116 | You can clone `livox_ros_driver2` and `rviz_2d_overlay_plugins` using the following link: 117 | 118 | - [Livox-ROS-driver2](https://github.com/Livox-SDK/livox_ros_driver2) 119 | - [ROS2-jsk-plugin](https://github.com/teamspatzenhirn/rviz_2d_overlay_plugins) 120 | 121 | > **Important**: Maintain this exact structure within `ros_ws/src` 122 | 123 | ### Docker Container Setup 124 | ```bash 125 | # Allow Docker GUI access 126 | xhost +local:docker 127 | ``` 128 | 129 | Go to `ros2_humble_docker/container_run.sh` and make sure you change exact directory path for `PROJECT_DIR` and `DATASET_DIR` 130 | ```bash 131 | PROJECT_DIR="/path/to/your/superodom" 132 | DATASET_DIR="/path/to/your/dataset" 133 | ``` 134 | > **Important**: `PROJECT_DIR` should be the exact directory to `ros2_ws/src` 135 | 136 | Then launch docker container using the following: 137 | ```bash 138 | # Grant access 139 | cd ros2_humble_docker 140 | sudo chmod -R 777 container_run.sh 141 | 142 | # Start container 143 | ./container_run.sh superodom-ros2 superodom-ros2:latest 144 | 145 | # Source ROS2 146 | source /opt/ros/humble/setup.bash 147 | ``` 148 | > **Important**: To access container, you can open a new bash window and run `docker exec --privileged -it superodom-ros2 /bin/bash` 149 | 150 | Build the workspace within container 151 | ```bash 152 | cd ~/ros2_ws/src/livox_ros_driver2 153 | ./build.sh humble 154 | cd ~/ros2_ws 155 | colcon build 156 | ``` 157 | > **Important**: make sure you first build `livox_ros_driver2` 158 | 159 | ## 🚀 5. Launch SuperOdometry 160 | 161 | To launch SuperOdometry, we provide demo datasets for Livox-mid360, VLP-16 and OS1-128 sensor [Download Link](https://drive.google.com/drive/folders/1oA0kRFIH0_8oyD32IW1vZitfxYunzdBr?usp=sharing) 162 | 163 | For more challange dataset, feel free to download from our website [slam_mode](https://superodometry.com/iccv23_challenge_LiI) and [localization_mode](https://superodometry.com/superloc). You might want to convert ROS1 bag into ROS2 format using this [link](https://docs.openvins.com/dev-ros1-to-ros2.html). 164 | 165 | For user-defined topic names, modify `super_odometry/config/$(YOUR_LiDAR_SENSOR).yaml`: 166 | ```bash 167 | imu_topic: "/your/imu/topic" 168 | laser_topic: "/your/laser/topic" 169 | ``` 170 | For user-defined laser-imu extrinsics, modify `super_odometry/config/$(YOUR_LiDAR_SENSOR)/$(YOUR_LiDAR_SENSOR)_calibration.yaml`: 171 | ```bash 172 | #Rotation from laser frame to imu frame, imu^R_laser 173 | extrinsicRotation_imu_laser: !!opencv-matrix 174 | rows: 3 175 | cols: 3 176 | dt: d 177 | data: [1., 0., 0., 178 | 0., 1., 0., 179 | 0., 0., 1.] 180 | 181 | #Translation from laser frame to imu frame, imu^T_laser 182 | extrinsicTranslation_imu_laser: !!opencv-matrix 183 | rows: 3 184 | cols: 1 185 | dt: d 186 | data: [-0.011, -0.02329, 0.04412] 187 | ``` 188 | 189 | Run SuperOdometry using the following command: 190 | 191 | ```bash 192 | source install/setup.bash 193 | ros2 launch super_odometry livox_mid360.launch.py 194 | ros2 launch super_odometry os1_128.launch.py 195 | ros2 launch super_odometry vlp_16.launch.py 196 | ``` 197 | Play your ROS2 dataset: 198 | ```bash 199 | # launch this in a new bash window 200 | docker exec --privileged -it superodom-ros2 /bin/bash 201 | source install/setup.bash 202 | cd ~/data 203 | ros2 play $(YOUR_ROS2_DATASET) 204 | ``` 205 | 206 | Visualize in RVIZ2: 207 | ```bash 208 | # launch this in a new bash window 209 | docker exec --privileged -it superodom-ros2 /bin/bash 210 | source install/setup.bash 211 | cd ~/ros_ws/src/SuperOdom/super_odometry 212 | rviz2 -d ros2.rviz 213 | ``` 214 | 215 | (⭐ Alternative) Visualize in Rerun: 216 | ```bash 217 | # launch this in a new bash window 218 | docker exec --privileged -it superodom-ros2 /bin/bash 219 | source install/setup.bash 220 | cd ~/ros2_ws/src/SuperOdom/script/visualizers 221 | python3 rerun_visualizer.py 222 | # Open a new bash window on your local device 223 | rerun 224 | ``` 225 | 226 | We also provide tmux script for easy launch with dataset (this script only works after you build the workspace in docker): 227 | ```bash 228 | cd script 229 | tmuxp load run.yaml 230 | ``` 231 | 232 | ## 📍 Localization Mode Configuration 233 | 234 | https://github.com/user-attachments/assets/42cb5480-c283-4608-84be-ff12a05d09e0 235 | 236 | > 🔥 The localization mode allows you to localize your robot by providing an initial pose and ground truth map. 237 | 238 | Update your `super_odometry/config/$(YOUR_LiDAR_SENSOR).yaml` configuration file with: 239 | ```yaml 240 | localization_mode: true # If true, localization mode is enabled; otherwise, SLAM mode is used 241 | read_pose_file: false # Set to true to read initial pose from a txt file 242 | init_x: 0.0 # Initial X position for localization 243 | init_y: 0.0 # Initial Y position for localization 244 | init_z: 0.0 # Initial Z position for localization 245 | init_roll: 0.0 # Initial roll angle 246 | init_pitch: 0.0 # Initial pitch angle 247 | init_yaw: 0.0 # Initial yaw angle 248 | ``` 249 | 250 | Add ground truth map map in launch file 251 | ```yaml 252 | parameters=[LaunchConfiguration("config_file"), 253 | { "calibration_file": LaunchConfiguration("calibration_file"), 254 | "map_dir": os.path.join(home_directory, "/path/to/your/pcd"), 255 | }] 256 | ``` 257 | To quickly launch our localization module, feel free to try out this demo [dataset](https://drive.google.com/drive/folders/1WOTj4j9t5LkKkdajFlj6bZcdmPcsJipz?usp=sharing) using default initial pose configuration. 258 | 259 | 263 | 264 | ## 📚 8. Citations 265 | 266 | ```bibtex 267 | @inproceedings{zhao2021super, 268 | title={Super odometry: IMU-centric LiDAR-visual-inertial estimator for challenging environments}, 269 | author={Zhao, Shibo and Zhang, Hengrui and Wang, Peng and Nogueira, Lucas and Scherer, Sebastian}, 270 | booktitle={2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 271 | pages={8729--8736}, 272 | year={2021}, 273 | organization={IEEE} 274 | } 275 | 276 | @inproceedings{zhao2025superloc, 277 | title={SuperLoc: The Key to Robust LiDAR-Inertial Localization Lies in Predicting Alignment Risks}, 278 | author={Zhao, Shibo and Zhu, Honghao and Gao, Yuanjun and Kim, Beomsoo and Qiu, Yuheng and Johnson, Aaron M. and Scherer, Sebastian}, 279 | booktitle={2025 IEEE International Conference on Robotics and Automation (ICRA)}, 280 | year={2025}, 281 | url={https://arxiv.org/abs/2412.02901} 282 | } 283 | ``` 284 | 285 | ## 9. Next Plan 286 | 🔵 Colorized Point Cloud Visualization — [Video Demo](https://www.youtube.com/watch?v=r7nLDGrz4gE) 287 | 288 | 🟢 Visual Odometry Module — Initial Release 289 | Lightweight and robust visual odometry module integrated into SuperOdometry. 290 | 291 | 292 | ## 📝 10. License 293 | 294 | This package is released under the GPLv3 license. For commercial use, please contact shiboz@andrew.cmu.edu and Prof. Sebastian Scherer. 295 | 296 | ## 🙏 11. Acknowledgements 297 | 298 | Special thanks to Professor Ji Zhang, Professor Michael Kaess, Parv Maheshwari, Yuanjun Gao, Yaoyu Hu for their valuable advice. Thanks to Omar Alama for providing Rerun support. We also acknowledge these foundational works: 299 | 300 | - LOAM: Lidar Odometry and Mapping in Real-time (RSS 2014) 301 | - GTSAM: Georgia Tech Smoothing and Mapping Library 302 | - [FastLIO](https://github.com/hku-mars/FAST_LIO), [LIOSAM](https://github.com/TixiaoShan/LIO-SAM) 303 | -------------------------------------------------------------------------------- /ros2_humble_docker/Dockerfile: -------------------------------------------------------------------------------- 1 | # Author: TaeYoung Kim 2 | # email: tyoung96@yonsei.ac.kr 3 | 4 | FROM osrf/ros:humble-desktop-full 5 | 6 | ENV NVIDIA_VISIBLE_DEVICES \ 7 | ${NVIDIA_VISIBLE_DEVICES:-all} 8 | ENV NVIDIA_DRIVER_CAPABILITIES \ 9 | ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics 10 | 11 | 12 | # Install PCL & Eigen & essential libraries 13 | RUN apt-get update \ 14 | && apt-get install -y \ 15 | build-essential \ 16 | cmake \ 17 | cppcheck \ 18 | gdb \ 19 | git \ 20 | sudo \ 21 | vim \ 22 | wget \ 23 | tmux \ 24 | curl \ 25 | less \ 26 | htop \ 27 | libsm6 libxext6 libgl1-mesa-glx libxrender-dev \ 28 | curl \ 29 | && apt-get clean 30 | 31 | RUN export DEBIAN_FRONTEND=noninteractive \ 32 | && sudo apt-get update \ 33 | && sudo -E apt-get install -y \ 34 | tzdata \ 35 | && sudo ln -fs /usr/share/zoneinfo/America/New_York /etc/localtime \ 36 | && sudo dpkg-reconfigure --frontend noninteractive tzdata \ 37 | && sudo apt-get clean 38 | 39 | RUN apt-get update && apt-get install -y cmake libatlas-base-dev libeigen3-dev libpcl-dev libgoogle-glog-dev libsuitesparse-dev libglew-dev wget unzip git python3-pip 40 | RUN apt-get install -y ros-humble-tf2 ros-humble-cv-bridge ros-humble-pcl-conversions ros-humble-xacro ros-humble-robot-state-publisher \ 41 | ros-humble-rviz2 ros-humble-image-transport ros-humble-image-transport-plugins ros-humble-pcl-ros 42 | 43 | RUN apt-get install -y libceres-dev 44 | 45 | 46 | RUN apt-get install -y python-tk python3-pip 47 | 48 | # Python 3. 49 | RUN pip3 install --upgrade pip 50 | RUN pip3 install --no-cache-dir numpy scipy matplotlib ipython opencv-python 51 | RUN pip3 install --no-cache-dir pandas visdom scikit-image scikit-learn numba Pillow 52 | RUN pip3 install --no-cache-dir pyyaml pycryptodome bagpy Cython 53 | 54 | RUN apt-get -y install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential 55 | # RUN sudo rosdep init 56 | RUN rosdep update 57 | RUN pip3 install -U catkin_tools gym 58 | RUN apt-get -y install ros-humble-grid-map-msgs ros-humble-grid-map 59 | 60 | # Install livox SDK 61 | WORKDIR /root/ 62 | RUN git clone https://github.com/Livox-SDK/Livox-SDK2.git 63 | WORKDIR /root/Livox-SDK2 64 | RUN mkdir build 65 | WORKDIR /root/Livox-SDK2/build 66 | RUN cmake .. && make -j2 && make install 67 | 68 | WORKDIR /root/ 69 | RUN git clone http://github.com/strasdat/Sophus.git \ 70 | && cd Sophus && git checkout 97e7161 \ 71 | && mkdir build && cd build && cmake .. -DBUILD_TESTS=OFF \ 72 | && make -j8 && sudo make install 73 | WORKDIR /root/ 74 | RUN git clone https://github.com/borglab/gtsam.git && \ 75 | cd gtsam && git checkout 4abef92 &&\ 76 | mkdir build && \ 77 | cd build && \ 78 | cmake \ 79 | -DGTSAM_USE_SYSTEM_EIGEN=ON \ 80 | -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ 81 | .. && \ 82 | make -j6 && \ 83 | sudo make install 84 | 85 | 86 | RUN /bin/bash -c '. /opt/ros/humble/setup.bash;' 87 | 88 | RUN pip install rerun-sdk 89 | RUN pip install numpy==1.24.3 matplotlib==3.7.1 90 | RUN pip install --upgrade packaging 91 | RUN pip3 install torch --ignore-installed 92 | 93 | 94 | WORKDIR /root/ros2_ws 95 | 96 | # Load ROS environment at each run 97 | COPY ./ros_entrypoint.sh / 98 | RUN chmod 755 /ros_entrypoint.sh 99 | ENTRYPOINT ["/ros_entrypoint.sh"] 100 | 101 | CMD ["bash"] 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | -------------------------------------------------------------------------------- /ros2_humble_docker/container_run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Author : Taeyoung Kim (https://github.com/Taeyoung96) 3 | 4 | # Set the project directory (PROJECT_DIR) as the parent directory of the current working directory 5 | PROJECT_DIR=$(dirname "$PWD") 6 | 7 | # Move to the parent folder of the project directory 8 | cd "$PROJECT_DIR" 9 | 10 | # Print the current working directory to verify the change 11 | echo "Current working directory: $PROJECT_DIR" 12 | 13 | # Check if arguments are provided for the image name and tag 14 | if [ "$#" -ne 2 ]; then 15 | echo "Usage: $0 " 16 | exit 1 17 | fi 18 | 19 | # Assign the arguments to variables for clarity 20 | CONTAINER_NAME="$1" 21 | IMAGE_NAME="$2" 22 | PROJECT_DIR="/path/to/your/superodom" 23 | DATASET_DIR="/path/to/your/dataset" 24 | 25 | # Launch the nvidia-docker container with the provided image name and tag 26 | docker run --privileged -it \ 27 | --gpus all \ 28 | -e NVIDIA_DRIVER_CAPABILITIES=all \ 29 | -e NVIDIA_VISIBLE_DEVICES=all \ 30 | --volume="$PROJECT_DIR:/root/ros2_ws/src" \ 31 | --volume="$DATASET_DIR:/root/data" \ 32 | --volume=/tmp/.X11-unix:/tmp/.X11-unix:rw \ 33 | --net=host \ 34 | --ipc=host \ 35 | --shm-size=4gb \ 36 | --name="$CONTAINER_NAME" \ 37 | --env="DISPLAY=$DISPLAY" \ 38 | --rm \ 39 | "$IMAGE_NAME" /bin/bash 40 | -------------------------------------------------------------------------------- /ros2_humble_docker/ros_entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | # Ros build 6 | source "/opt/ros/humble/setup.bash" 7 | 8 | echo "==============FAST-LIO ROS2 Docker Env Ready================" 9 | 10 | cd /root/ros2_ws 11 | 12 | exec "$@" 13 | -------------------------------------------------------------------------------- /script/benchmark_livox.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | --- 3 | session_name: SuperOdom_ROS2 4 | suppress_history: false 5 | before_script: docker start superodom-ros2 6 | windows: 7 | - window_name: superodom_ros2 8 | focus: false 9 | layout: main-vertical 10 | panes: 11 | - shell_command: 12 | - docker exec --privileged -it superodom-ros2 /bin/bash 13 | # - DATASET_DIR=/root/data/cic_office_video ## hange to your dataset directory here 14 | # - DATASET_DIR=/root/data/cmu_diablo_campus_0815/scafie_ros2bag ## hange to your dataset directory here 15 | - DATASET_DIR=/root/data/scafie_ros2bag ## hange to your dataset directory here 16 | - mkdir -p $DATASET_DIR/benchmark 17 | - echo $DATASET_DIR > /tmp/current_dataset_dir 18 | - cd ~/ros2_ws 19 | - source /opt/ros/humble/setup.bash 20 | - source install/setup.bash 21 | - ros2 launch super_odometry livox_mid360.launch.py 22 | - shell_command: 23 | - docker exec --privileged -it superodom-ros2 /bin/bash 24 | - cd ~/ros2_ws 25 | - sleep 3 26 | - source /opt/ros/humble/setup.bash 27 | - source install/setup.bash 28 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 29 | - cd $DATASET_DIR 30 | - ros2 bag play *.db3 --rate 3.0 && echo "PLAYBACK_COMPLETE" > /tmp/bag_complete 31 | - shell_command: 32 | - docker exec --privileged -it superodom-ros2 /bin/bash 33 | - cd ~/ros2_ws 34 | - sleep 2 35 | - source /opt/ros/humble/setup.bash 36 | - source install/setup.bash 37 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 38 | - cd $DATASET_DIR/benchmark 39 | # Create benchmark directory and export the name as a file so other panes can read it 40 | - BENCHMARK_DIR=benchmark_livox_$(date '+%Y-%m-%d_%H-%M-%S') 41 | - mkdir -p $BENCHMARK_DIR && cd $BENCHMARK_DIR 42 | - echo $BENCHMARK_DIR > /tmp/current_benchmark_dir 43 | - cp ~/ros2_ws/src/SuperOdom/super_odometry/config/livox_mid360.yaml . 44 | - cp ~/ros2_ws/src/SuperOdom/script/save_benchmark_result.py . 45 | - cp ~/ros2_ws/src/SuperOdom/script/save_superodom_stats_matplotlib.py . 46 | - bash -c "ros2 bag record /super_odometry_stats & RECORD_PID=$!; while [ ! -f /tmp/bag_complete ]; do sleep 1; done; kill $RECORD_PID; rm /tmp/bag_complete; echo 'Recording stopped';" 47 | - shell_command: 48 | - docker exec --privileged -it superodom-ros2 /bin/bash 49 | - cd ~/ros2_ws 50 | - sleep 2 51 | - source /opt/ros/humble/setup.bash 52 | - source install/setup.bash 53 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 54 | - cd $DATASET_DIR/benchmark 55 | - BENCHMARK_DIR=$(cat /tmp/current_benchmark_dir) 56 | - cd $BENCHMARK_DIR 57 | - python3 save_benchmark_result.py 58 | - shell_command: 59 | - docker exec --privileged -it superodom-ros2 /bin/bash 60 | - cd ~/ros2_ws 61 | - sleep 2 62 | - source /opt/ros/humble/setup.bash 63 | - source install/setup.bash 64 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 65 | - cd $DATASET_DIR/benchmark 66 | - BENCHMARK_DIR=$(cat /tmp/current_benchmark_dir) 67 | - cd $BENCHMARK_DIR 68 | - python3 save_superodom_stats_matplotlib.py 69 | - window_name: rviz 70 | focus: false 71 | layout: main-horizontal 72 | panes: 73 | - shell_command: 74 | - docker exec --privileged -it superodom-ros2 /bin/bash 75 | - cd ~/ros2_ws 76 | - source install/setup.bash 77 | - source /opt/ros/humble/setup.bash 78 | - cd /root/ros2_ws/src/SuperOdom/super_odometry 79 | - sleep 5 80 | - rviz2 -d ros2.rviz -------------------------------------------------------------------------------- /script/benchmark_ouster.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | --- 3 | session_name: SuperOdom_ROS2 4 | suppress_history: false 5 | before_script: docker start superodom-ros2 6 | windows: 7 | - window_name: superodom_ros2 8 | focus: false 9 | layout: main-vertical 10 | panes: 11 | - shell_command: 12 | - docker exec --privileged -it superodom-ros2 /bin/bash 13 | # - DATASET_DIR=/root/data/cic_office_video ## hange to your dataset directory here 14 | - DATASET_DIR=/root/data/ouster_dataset/run_05_lester-super-odom-rogers-fix-warehouse-shakeout_2025-02-06-09-07-27/extracted_ouster_data ## hange to your dataset directory here 15 | - mkdir -p $DATASET_DIR/benchmark 16 | - echo $DATASET_DIR > /tmp/current_dataset_dir 17 | - cd ~/ros2_ws 18 | - source /opt/ros/humble/setup.bash 19 | - source install/setup.bash 20 | - ros2 launch super_odometry os0_128.launch.py 21 | - shell_command: 22 | - docker exec --privileged -it superodom-ros2 /bin/bash 23 | - cd ~/ros2_ws 24 | - sleep 3 25 | - source /opt/ros/humble/setup.bash 26 | - source install/setup.bash 27 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 28 | - cd $DATASET_DIR 29 | - ros2 bag play *.mcap --rate 2.0 && echo "PLAYBACK_COMPLETE" > /tmp/bag_complete 30 | - shell_command: 31 | - docker exec --privileged -it superodom-ros2 /bin/bash 32 | - cd ~/ros2_ws 33 | - sleep 2 34 | - source /opt/ros/humble/setup.bash 35 | - source install/setup.bash 36 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 37 | - cd $DATASET_DIR/benchmark 38 | # Create benchmark directory and export the name as a file so other panes can read it 39 | - BENCHMARK_DIR=benchmark_ouster_$(date '+%Y-%m-%d_%H-%M-%S') 40 | - mkdir -p $BENCHMARK_DIR && cd $BENCHMARK_DIR 41 | - echo $BENCHMARK_DIR > /tmp/current_benchmark_dir 42 | - cp ~/ros2_ws/src/SuperOdom/super_odometry/config/os0_128.yaml . #change to sensor config file here 43 | - cp ~/ros2_ws/src/SuperOdom/script/save_benchmark_result.py . 44 | - cp ~/ros2_ws/src/SuperOdom/script/save_superodom_stats_matplotlib.py . 45 | - cp ~/ros2_ws/src/SuperOdom/script/save_imu_vs_icp.py . 46 | - bash -c "ros2 bag record /super_odometry_stats & RECORD_PID=$!; while [ ! -f /tmp/bag_complete ]; do sleep 1; done; kill $RECORD_PID; rm /tmp/bag_complete; echo 'Recording stopped';" 47 | - shell_command: 48 | - docker exec --privileged -it superodom-ros2 /bin/bash 49 | - cd ~/ros2_ws 50 | - sleep 2 51 | - source /opt/ros/humble/setup.bash 52 | - source install/setup.bash 53 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 54 | - cd $DATASET_DIR/benchmark 55 | - BENCHMARK_DIR=$(cat /tmp/current_benchmark_dir) 56 | - cd $BENCHMARK_DIR 57 | - python3 save_benchmark_result.py 58 | - shell_command: 59 | - docker exec --privileged -it superodom-ros2 /bin/bash 60 | - cd ~/ros2_ws 61 | - sleep 2 62 | - source /opt/ros/humble/setup.bash 63 | - source install/setup.bash 64 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 65 | - cd $DATASET_DIR/benchmark 66 | - BENCHMARK_DIR=$(cat /tmp/current_benchmark_dir) 67 | - cd $BENCHMARK_DIR 68 | - python3 save_superodom_stats_matplotlib.py 69 | - shell_command: 70 | - docker exec --privileged -it superodom-ros2 /bin/bash 71 | - cd ~/ros2_ws 72 | - sleep 2 73 | - source /opt/ros/humble/setup.bash 74 | - source install/setup.bash 75 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 76 | - cd $DATASET_DIR/benchmark 77 | - BENCHMARK_DIR=$(cat /tmp/current_benchmark_dir) 78 | - cd $BENCHMARK_DIR 79 | - python3 save_imu_vs_icp.py 80 | - window_name: rviz 81 | focus: false 82 | layout: main-horizontal 83 | panes: 84 | - shell_command: 85 | - docker exec --privileged -it superodom-ros2 /bin/bash 86 | - cd ~/ros2_ws 87 | - source install/setup.bash 88 | - source /opt/ros/humble/setup.bash 89 | - cd /root/ros2_ws/src/SuperOdom/super_odometry 90 | - sleep 5 91 | - rviz2 -d ros2.rviz -------------------------------------------------------------------------------- /script/benchmark_velodyne.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | --- 3 | session_name: SuperOdom_ROS2 4 | suppress_history: false 5 | before_script: docker start superodom-ros2 6 | windows: 7 | - window_name: superodom_ros2 8 | focus: false 9 | layout: main-vertical 10 | panes: 11 | - shell_command: 12 | - docker exec --privileged -it superodom-ros2 /bin/bash 13 | # - DATASET_DIR=/root/data/cic_office_video ## hange to your dataset directory here 14 | - DATASET_DIR=/root/data/cave/big_loop/big_loop ## hange to your dataset directory here 15 | - mkdir -p $DATASET_DIR/benchmark 16 | - echo $DATASET_DIR > /tmp/current_dataset_dir 17 | - cd ~/ros2_ws 18 | - source /opt/ros/humble/setup.bash 19 | - source install/setup.bash 20 | 21 | - ros2 launch super_odometry vlp_16.launch.py 22 | - shell_command: 23 | - docker exec --privileged -it superodom-ros2 /bin/bash 24 | - cd ~/ros2_ws 25 | - sleep 3 26 | - source /opt/ros/humble/setup.bash 27 | - source install/setup.bash 28 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 29 | - cd $DATASET_DIR 30 | # - ros2 bag play *.db3 --topics /velodyne_points --rate 3.0 && echo "PLAYBACK_COMPLETE" > /tmp/bag_complete 31 | - ros2 bag play *.db3 --rate 2.0 && echo "PLAYBACK_COMPLETE" > /tmp/bag_complete 32 | 33 | - shell_command: 34 | - docker exec --privileged -it superodom-ros2 /bin/bash 35 | - cd ~/ros2_ws 36 | - sleep 2 37 | - source /opt/ros/humble/setup.bash 38 | - source install/setup.bash 39 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 40 | - cd $DATASET_DIR/benchmark 41 | # Create benchmark directory and export the name as a file so other panes can read it 42 | - BENCHMARK_DIR=benchmark_velodyne_$(date '+%Y-%m-%d_%H-%M-%S') 43 | - mkdir -p $BENCHMARK_DIR && cd $BENCHMARK_DIR 44 | - echo $BENCHMARK_DIR > /tmp/current_benchmark_dir 45 | - cp ~/ros2_ws/src/SuperOdom/super_odometry/config/vlp_16.yaml . 46 | - cp ~/ros2_ws/src/SuperOdom/script/save_benchmark_result.py . 47 | - cp ~/ros2_ws/src/SuperOdom/script/save_superodom_stats_matplotlib.py . 48 | - bash -c "ros2 bag record /super_odometry_stats & RECORD_PID=$!; while [ ! -f /tmp/bag_complete ]; do sleep 1; done; kill $RECORD_PID; rm /tmp/bag_complete; echo 'Recording stopped';" 49 | - shell_command: 50 | - docker exec --privileged -it superodom-ros2 /bin/bash 51 | - cd ~/ros2_ws 52 | - sleep 2 53 | - source /opt/ros/humble/setup.bash 54 | - source install/setup.bash 55 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 56 | - cd $DATASET_DIR/benchmark 57 | - BENCHMARK_DIR=$(cat /tmp/current_benchmark_dir) 58 | - cd $BENCHMARK_DIR 59 | - python3 save_benchmark_result.py 60 | - shell_command: 61 | - docker exec --privileged -it superodom-ros2 /bin/bash 62 | - cd ~/ros2_ws 63 | - sleep 2 64 | - source /opt/ros/humble/setup.bash 65 | - source install/setup.bash 66 | - DATASET_DIR=$(cat /tmp/current_dataset_dir) 67 | - cd $DATASET_DIR/benchmark 68 | - BENCHMARK_DIR=$(cat /tmp/current_benchmark_dir) 69 | - cd $BENCHMARK_DIR 70 | - python3 save_superodom_stats_matplotlib.py 71 | - window_name: rviz 72 | focus: false 73 | layout: main-horizontal 74 | panes: 75 | - shell_command: 76 | - docker exec --privileged -it superodom-ros2 /bin/bash 77 | - cd ~/ros2_ws 78 | - source install/setup.bash 79 | - source /opt/ros/humble/setup.bash 80 | - cd /root/ros2_ws/src/SuperOdom/super_odometry 81 | - sleep 5 82 | - rviz2 -d ros2.rviz -------------------------------------------------------------------------------- /script/run.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | --- 3 | session_name: SuperOdom_ROS2 4 | 5 | suppress_history: false 6 | 7 | before_script: docker start superodom-ros2 8 | 9 | windows: 10 | - window_name: superodom_ros2 11 | focus: false 12 | layout: main-vertical 13 | panes: 14 | - shell_command: 15 | - docker exec --privileged -it superodom-ros2 /bin/bash 16 | - cd ~/ros2_ws 17 | - source /opt/ros/humble/setup.bash 18 | - source install/setup.bash 19 | - ros2 launch super_odometry livox_mid360.launch.py 20 | - shell_command: 21 | - docker exec --privileged -it superodom-ros2 /bin/bash 22 | - cd ~/ros2_ws 23 | - source /opt/ros/humble/setup.bash 24 | - source install/setup.bash 25 | - cd /root/data/ 26 | - sleep 3 27 | 28 | - window_name: rviz 29 | focus: false 30 | layout: main-horizontal 31 | panes: 32 | - shell_command: 33 | - docker exec --privileged -it superodom-ros2 /bin/bash 34 | - cd ~/ros2_ws 35 | - source install/setup.bash 36 | - source /opt/ros/humble/setup.bash 37 | - cd /root/ros2_ws/src/SuperOdom/super_odometry 38 | - sleep 3 39 | - rviz2 -d ros2.rviz 40 | -------------------------------------------------------------------------------- /script/save_imu_vs_icp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import time 5 | import argparse 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | from datetime import datetime 9 | 10 | # ROS2 imports 11 | import rclpy 12 | from rclpy.node import Node 13 | from std_msgs.msg import Header 14 | 15 | # Import the specific message type 16 | from super_odometry_msgs.msg import OptimizationStats, IterationStats 17 | 18 | class OptimizationComparisonPlotter(Node): 19 | def __init__(self): 20 | super().__init__('optimization_comparison_plotter') 21 | 22 | # Initialize storage for stats messages 23 | self.stats_messages = [] 24 | self.last_message_time = time.time() 25 | 26 | # Create subscription to the stats topic 27 | self.subscription = self.create_subscription( 28 | OptimizationStats, 29 | '/super_odometry_stats', 30 | self.stats_callback, 31 | 10 # QoS profile depth 32 | ) 33 | 34 | # Timer to check for message inactivity 35 | self.timer = self.create_timer(1.0, self.check_inactivity) 36 | 37 | # Create the output directory if it doesn't exist 38 | self.output_dir = "./superodom_stats" 39 | os.makedirs(self.output_dir, exist_ok=True) 40 | 41 | self.get_logger().info(f'Listening for messages on /super_odometry_stats, will save comparison plot to {self.output_dir}...') 42 | 43 | def stats_callback(self, msg): 44 | """Callback for receiving odometry stats messages""" 45 | self.stats_messages.append(msg) 46 | self.last_message_time = time.time() 47 | self.get_logger().info(f'Received odometry stats message, total: {len(self.stats_messages)}') 48 | 49 | def check_inactivity(self): 50 | """Check if no messages have been received for 6 seconds""" 51 | elapsed = time.time() - self.last_message_time 52 | 53 | if self.stats_messages and elapsed >= 6.0: 54 | self.get_logger().info(f'No stats messages received for {elapsed:.1f} seconds - saving comparison plot') 55 | 56 | # Create and save the IMU vs ICP optimization plot 57 | self.create_imu_vs_icp_plot() 58 | 59 | # Reset collection for next batch 60 | self.stats_messages = [] 61 | 62 | def create_imu_vs_icp_plot(self): 63 | """Create a plot comparing iterations[0-2].translation_norm with total_translation""" 64 | if not self.stats_messages: 65 | self.get_logger().warn("No stats messages to analyze") 66 | return 67 | 68 | # Generate timestamp for this batch 69 | timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") 70 | 71 | # Data collections 72 | total_translations = [] # Main total_translation 73 | iter0_translations = [] # Initial iteration (IMU prediction) 74 | iter1_translations = [] # First ICP refinement 75 | iter2_translations = [] # Second ICP refinement 76 | timestamps = [] # Frame indices for x-axis 77 | 78 | valid_messages = 0 79 | 80 | # Collect data from each message 81 | for msg_idx, msg in enumerate(self.stats_messages): 82 | # Only include points that have all the required data 83 | has_total = hasattr(msg, 'total_translation') 84 | has_iterations = hasattr(msg, 'iterations') and len(msg.iterations) >= 3 85 | 86 | if has_total and has_iterations: 87 | # Record the timestamp (use frame index for simplicity) 88 | timestamps.append(msg_idx) 89 | 90 | # Get total translation 91 | total_translations.append(msg.total_translation) 92 | 93 | # Get iteration-specific translation norms 94 | iter0_translations.append(msg.iterations[0].translation_norm) 95 | iter1_translations.append(msg.iterations[1].translation_norm) 96 | iter2_translations.append(msg.iterations[2].translation_norm) 97 | 98 | valid_messages += 1 99 | 100 | if valid_messages == 0: 101 | self.get_logger().warn("No messages with both total_translation and at least 3 iterations found") 102 | return 103 | 104 | # Create the plot 105 | plt.figure(figsize=(12, 8)) 106 | 107 | # Plot each data seriess 108 | plt.plot(timestamps, total_translations, '--', color='blue', linewidth=1.5, markersize=3, 109 | label='IMU Preintegrated Translation (Final Result)') 110 | plt.plot(timestamps, iter0_translations, '--', color='green', linewidth=1.0, alpha=0.8, 111 | label='ICP (Iteration 0)') 112 | plt.plot(timestamps, iter1_translations, '--', color='orange', linewidth=1.0, alpha=0.8, 113 | label='ICP (Iteration 1)') 114 | plt.plot(timestamps, iter2_translations, '--', color='red', linewidth=1.0, alpha=0.8, 115 | label='ICP (Iteration 2)') 116 | 117 | # Add titles and labels 118 | plt.title('IMU/Odometry Prediction vs. ICP Optimization Comparison', fontsize=16) 119 | plt.xlabel('Frame Number', fontsize=14) 120 | plt.ylabel('Translation (meters)', fontsize=14) 121 | 122 | # Add legend with clear description 123 | plt.legend(loc='upper left', fontsize=12) 124 | 125 | # Add annotations explaining the plot 126 | plt.figtext(0.5, 0.01, 127 | "This plot compares the initial odometry/IMU-based prediction (Iteration 0) with\n" 128 | "the subsequent ICP optimization steps (Iterations 1-2) and the final result (Total Translation).", 129 | ha='center', fontsize=10, bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.3)) 130 | 131 | # Add grid for readability 132 | plt.grid(True, linestyle='--', alpha=0.7) 133 | 134 | # Adjust y-axis to start from 0 135 | plt.ylim(bottom=0) 136 | 137 | # Tight layout for better spacing 138 | plt.tight_layout(rect=[0, 0.03, 1, 0.97]) 139 | 140 | # Save the figure 141 | output_file = os.path.join(self.output_dir, f"imu_odometry_vs_icp_optimization_{timestamp}.png") 142 | plt.savefig(output_file, dpi=150) 143 | plt.close() 144 | 145 | # Also save a specific version with the exact requested filename 146 | fixed_output_file = os.path.join(self.output_dir, "imu_odometry_vs_icp_optimization.png") 147 | plt.figure(figsize=(12, 8)) 148 | 149 | # Plot the data again for the fixed filename 150 | plt.plot(timestamps, total_translations, '--', color='blue', linewidth=1.5, markersize=3, 151 | label='IMU Preintegrated Translation (Final Result)') 152 | plt.plot(timestamps, iter0_translations, '--', color='green', linewidth=1.0, alpha=0.8, 153 | label='ICP (Iteration 0)') 154 | plt.plot(timestamps, iter1_translations, '--', color='orange', linewidth=1.0, alpha=0.8, 155 | label='ICP (Iteration 1)') 156 | plt.plot(timestamps, iter2_translations, '--', color='red', linewidth=1.0, alpha=0.8, 157 | label='ICP (Iteration 2)') 158 | 159 | # Add titles and labels 160 | plt.title('IMU/Odometry Prediction vs. ICP Optimization Comparison', fontsize=16) 161 | plt.xlabel('Frame Number', fontsize=14) 162 | plt.ylabel('Translation (meters)', fontsize=14) 163 | 164 | # Add legend with clear description 165 | plt.legend(loc='upper left', fontsize=12) 166 | 167 | # Add annotations explaining the plot 168 | plt.figtext(0.5, 0.01, 169 | "This plot compares the initial odometry/IMU-based prediction (Iteration 0) with\n" 170 | "the subsequent ICP optimization steps (Iterations 1-2) and the final result (Total Translation).", 171 | ha='center', fontsize=10, bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.3)) 172 | 173 | # Add grid for readability 174 | plt.grid(True, linestyle='--', alpha=0.7) 175 | 176 | # Adjust y-axis to start from 0 177 | plt.ylim(bottom=0) 178 | 179 | # Tight layout for better spacing 180 | plt.tight_layout(rect=[0, 0.03, 1, 0.97]) 181 | 182 | plt.savefig(fixed_output_file, dpi=150) 183 | plt.close() 184 | 185 | self.get_logger().info(f"Comparison plot saved to {fixed_output_file} with {valid_messages} data points") 186 | 187 | def main(): 188 | parser = argparse.ArgumentParser(description='Create comparison plot of IMU/odometry vs ICP optimization') 189 | args = parser.parse_args() 190 | 191 | # Initialize ROS2 192 | rclpy.init() 193 | 194 | try: 195 | # Create and use the plotter node 196 | plotter = OptimizationComparisonPlotter() 197 | 198 | # Spin the node to process callbacks 199 | rclpy.spin(plotter) 200 | 201 | except KeyboardInterrupt: 202 | print("Node stopped by user") 203 | except Exception as e: 204 | print(f"Error: {e}") 205 | finally: 206 | # Clean up and shut down 207 | rclpy.shutdown() 208 | 209 | if __name__ == "__main__": 210 | main() -------------------------------------------------------------------------------- /script/save_superodom_stats.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import time 5 | import argparse 6 | import numpy as np 7 | from typing import List, Dict 8 | from datetime import datetime 9 | 10 | # ROS2 imports 11 | import rclpy 12 | from rclpy.node import Node 13 | from std_msgs.msg import Header 14 | 15 | # Import the specific message type 16 | # You'll need to replace this with the actual message path when using the code 17 | from super_odometry_msgs.msg import OptimizationStats, IterationStats 18 | 19 | # Rerun imports 20 | import rerun as rr 21 | 22 | class OdometryStatsSubscriber(Node): 23 | def __init__(self): 24 | super().__init__('odometry_stats_subscriber') 25 | 26 | # Initialize storage for stats messages 27 | self.stats_messages = [] 28 | self.last_message_time = time.time() 29 | 30 | # Create subscription to the stats topic 31 | self.subscription = self.create_subscription( 32 | OptimizationStats, 33 | '/super_odometry_stats', 34 | self.stats_callback, 35 | 10 # QoS profile depth 36 | ) 37 | 38 | # Timer to check for message inactivity 39 | self.timer = self.create_timer(1.0, self.check_inactivity) 40 | 41 | # Create the output directory if it doesn't exist 42 | self.output_dir = "./superodom_stats" 43 | os.makedirs(self.output_dir, exist_ok=True) 44 | 45 | self.get_logger().info(f'Listening for messages on /super_odometry_stats, will save to {self.output_dir}...') 46 | 47 | def stats_callback(self, msg): 48 | """Callback for receiving odometry stats messages""" 49 | self.stats_messages.append(msg) 50 | self.last_message_time = time.time() 51 | self.get_logger().info(f'Received odometry stats message, total: {len(self.stats_messages)}') 52 | 53 | def check_inactivity(self): 54 | """Check if no messages have been received for 6 seconds""" 55 | elapsed = time.time() - self.last_message_time 56 | 57 | if self.stats_messages and elapsed >= 6.0: 58 | self.get_logger().info(f'No stats messages received for {elapsed:.1f} seconds - saving data') 59 | 60 | # Save stats to Rerun 61 | self.save_stats_to_rerun() 62 | 63 | # Reset collection for next batch 64 | self.stats_messages = [] 65 | 66 | def save_stats_to_rerun(self): 67 | """Save the collected stats messages to separate Rerun files for each field""" 68 | if not self.stats_messages: 69 | self.get_logger().warn("No stats messages to save") 70 | return 71 | 72 | # Generate timestamp for this batch 73 | timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") 74 | 75 | # Fields to extract and visualize as requested 76 | fields = [ 77 | "laser_cloud_surf_from_map_num", 78 | "laser_cloud_corner_from_map_num", 79 | "laser_cloud_surf_stack_num", 80 | "laser_cloud_corner_stack_num", 81 | "total_translation", 82 | "total_rotation", 83 | "translation_from_last", 84 | "rotation_from_last", 85 | "time_elapsed", 86 | "latency", 87 | "n_iterations", 88 | "average_distance" 89 | ] 90 | 91 | # Colors for each field's plot 92 | colors = { 93 | "laser_cloud_surf_from_map_num": [0, 255, 0], # Green 94 | "laser_cloud_corner_from_map_num": [0, 0, 255], # Blue 95 | "laser_cloud_surf_stack_num": [255, 255, 0], # Yellow 96 | "laser_cloud_corner_stack_num": [255, 0, 255], # Magenta 97 | "total_translation": [255, 165, 0], # Orange 98 | "total_rotation": [128, 0, 128], # Purple 99 | "translation_from_last": [255, 69, 0], # Red-Orange 100 | "rotation_from_last": [75, 0, 130], # Indigo 101 | "time_elapsed": [0, 128, 128], # Teal 102 | "latency": [255, 0, 0], # Red 103 | "n_iterations": [0, 128, 0], # Dark Green 104 | "average_distance": [70, 130, 180] # Steel Blue 105 | } 106 | 107 | # Process each field separately 108 | for field in fields: 109 | # Collect data for this field 110 | data_points = [] 111 | has_data = False 112 | 113 | for i, msg in enumerate(self.stats_messages): 114 | timestamp_ms = i * 10 # Dummy timestamp 115 | if hasattr(msg, field): 116 | value = getattr(msg, field) 117 | data_points.append((timestamp_ms, value)) 118 | has_data = True 119 | 120 | if not has_data: 121 | self.get_logger().warn(f"No data for field '{field}', skipping") 122 | continue 123 | 124 | # Create a new Rerun recording for this field 125 | output_file = os.path.join(self.output_dir, f"{field}_{timestamp}.rrd") 126 | 127 | # Initialize Rerun for this field 128 | rr.init(f"Super Odometry Stats - {field}") 129 | 130 | # Set up recording to a file 131 | rr.save(output_file) 132 | 133 | # Get color for this field 134 | color = colors.get(field, [100, 100, 100]) # Default gray if not in colors dict 135 | 136 | # Convert data to arrays for plotting 137 | timestamps = [t for t, _ in data_points] 138 | values = [v for _, v in data_points] 139 | 140 | # Create arrays for points (x, y coordinates) 141 | positions = np.column_stack((timestamps, values)) 142 | 143 | # Log points for scatter plot visualization 144 | rr.log( 145 | f"plots/{field}", 146 | rr.Points2D( 147 | positions=positions, 148 | colors=np.array([color] * len(positions)), 149 | radii=np.array([3.0] * len(positions)) 150 | ) 151 | ) 152 | 153 | # Add annotation context for labels 154 | # rr.log( 155 | # f"plots/{field}", 156 | # rr.AnnotationContext( 157 | # title=f"{field} over time", 158 | # x_axis_name="Time", 159 | # y_axis_name=field 160 | # ) 161 | # ) 162 | 163 | # Log individual points with timestamp for time-based viewing 164 | for i, (t, v) in enumerate(data_points): 165 | # Set the time sequence for this data point 166 | rr.set_time_sequence("frame", t) 167 | 168 | # Log the scalar value at this time 169 | rr.log( 170 | f"timeseries/{field}", 171 | rr.Scalar(v) 172 | ) 173 | 174 | self.get_logger().info(f"Field '{field}' recorded to {output_file} with {len(data_points)} data points") 175 | 176 | # Close this recording before moving to the next field 177 | # rr.shutdown() 178 | 179 | def main(): 180 | parser = argparse.ArgumentParser(description='Subscribe to ROS2 super_odometry_stats and visualize with Rerun') 181 | args = parser.parse_args() 182 | 183 | # Initialize ROS2 184 | rclpy.init() 185 | 186 | try: 187 | # Create and use the subscriber node 188 | stats_subscriber = OdometryStatsSubscriber() 189 | 190 | # Spin the node to process callbacks 191 | rclpy.spin(stats_subscriber) 192 | 193 | except KeyboardInterrupt: 194 | print("Node stopped by user") 195 | except Exception as e: 196 | print(f"Error: {e}") 197 | finally: 198 | # Clean up and shut down 199 | rclpy.shutdown() 200 | 201 | if __name__ == "__main__": 202 | main() -------------------------------------------------------------------------------- /script/save_superodom_stats_matplotlib.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import time 5 | import argparse 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | from typing import List, Dict 9 | from datetime import datetime 10 | 11 | # ROS2 imports 12 | import rclpy 13 | from rclpy.node import Node 14 | from std_msgs.msg import Header 15 | 16 | # Import the specific message type 17 | # You'll need to replace this with the actual message path when using the code 18 | from super_odometry_msgs.msg import OptimizationStats, IterationStats 19 | 20 | class OdometryStatsSubscriber(Node): 21 | def __init__(self): 22 | super().__init__('odometry_stats_subscriber') 23 | 24 | # Initialize storage for stats messages 25 | self.stats_messages = [] 26 | self.last_message_time = time.time() 27 | 28 | # Create subscription to the stats topic 29 | self.subscription = self.create_subscription( 30 | OptimizationStats, 31 | '/super_odometry_stats', 32 | self.stats_callback, 33 | 10 # QoS profile depth 34 | ) 35 | 36 | # Timer to check for message inactivity 37 | self.timer = self.create_timer(1.0, self.check_inactivity) 38 | 39 | # Create the output directory if it doesn't exist 40 | self.output_dir = "./superodom_stats" 41 | os.makedirs(self.output_dir, exist_ok=True) 42 | 43 | # Define axis limits for each field 44 | self.axis_limits = { 45 | "laser_cloud_surf_from_map_num": {"y_min": 0, "y_max": None}, 46 | "laser_cloud_corner_from_map_num": {"y_min": 0, "y_max": None}, 47 | "laser_cloud_surf_stack_num": {"y_min": 0, "y_max": None}, 48 | "laser_cloud_corner_stack_num": {"y_min": 0, "y_max": None}, 49 | "total_translation": {"y_min": 0, "y_max": None}, 50 | "total_rotation": {"y_min": 0, "y_max": 6.3}, # 0 to 2π radians 51 | "translation_from_last": {"y_min": 0, "y_max": 1.0}, # Likely small incremental changes 52 | "rotation_from_last": {"y_min": 0, "y_max": 0.2}, # Small angular changes 53 | "time_elapsed": {"y_min": 0, "y_max": None}, 54 | "latency": {"y_min": 0, "y_max": 1000}, # In milliseconds 55 | "n_iterations": {"y_min": 0, "y_max": 50}, # Typical optimization iteration count 56 | "average_distance": {"y_min": 0, "y_max": None} 57 | } 58 | 59 | # Define colors for each field 60 | self.colors = { 61 | "laser_cloud_surf_from_map_num": "green", 62 | "laser_cloud_corner_from_map_num": "blue", 63 | "laser_cloud_surf_stack_num": "gold", 64 | "laser_cloud_corner_stack_num": "magenta", 65 | "total_translation": "orange", 66 | "total_rotation": "purple", 67 | "translation_from_last": "orangered", 68 | "rotation_from_last": "indigo", 69 | "time_elapsed": "teal", 70 | "latency": "red", 71 | "n_iterations": "darkgreen", 72 | "average_distance": "steelblue" 73 | } 74 | 75 | self.get_logger().info(f'Listening for messages on /super_odometry_stats, will save to {self.output_dir}...') 76 | 77 | def stats_callback(self, msg): 78 | """Callback for receiving odometry stats messages""" 79 | self.stats_messages.append(msg) 80 | self.last_message_time = time.time() 81 | self.get_logger().info(f'Received odometry stats message, total: {len(self.stats_messages)}') 82 | 83 | def check_inactivity(self): 84 | """Check if no messages have been received for 6 seconds""" 85 | elapsed = time.time() - self.last_message_time 86 | 87 | if self.stats_messages and elapsed >= 6.0: 88 | self.get_logger().info(f'No stats messages received for {elapsed:.1f} seconds - saving data') 89 | 90 | # Save stats to matplotlib plots 91 | self.save_stats_to_plots() 92 | 93 | # Reset collection for next batch 94 | self.stats_messages = [] 95 | 96 | def save_stats_to_plots(self): 97 | """Save the collected stats messages to separate matplotlib plots for each field""" 98 | if not self.stats_messages: 99 | self.get_logger().warn("No stats messages to save") 100 | return 101 | 102 | # Generate timestamp for this batch 103 | timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") 104 | 105 | # Fields to extract and visualize as requested 106 | fields = [ 107 | "laser_cloud_surf_from_map_num", 108 | "laser_cloud_corner_from_map_num", 109 | "laser_cloud_surf_stack_num", 110 | "laser_cloud_corner_stack_num", 111 | "total_translation", 112 | "total_rotation", 113 | "translation_from_last", 114 | "rotation_from_last", 115 | "time_elapsed", 116 | "latency", 117 | "n_iterations", 118 | "average_distance" 119 | ] 120 | 121 | # Process each field separately 122 | for field in fields: 123 | # Collect data for this field 124 | data_points = [] 125 | has_data = False 126 | 127 | for i, msg in enumerate(self.stats_messages): 128 | timestamp_ms = i * 10 # Dummy timestamp for x-axis 129 | if hasattr(msg, field): 130 | value = getattr(msg, field) 131 | data_points.append((timestamp_ms, value)) 132 | has_data = True 133 | 134 | if not has_data: 135 | self.get_logger().warn(f"No data for field '{field}', skipping") 136 | continue 137 | 138 | # Convert data to arrays for plotting 139 | timestamps = [t for t, _ in data_points] 140 | values = [v for _, v in data_points] 141 | 142 | # Create a new figure for this field 143 | plt.figure(figsize=(10, 6)) 144 | 145 | # Get color for this field 146 | color = self.colors.get(field, "gray") 147 | 148 | # Plot the data as a thin line without markers 149 | plt.plot(timestamps, values, '-', color=color, linewidth=0.8, alpha=1.0) 150 | 151 | # Set titles and labels 152 | plt.title(f"{field} over time", fontsize=14) 153 | plt.xlabel("Time (frame number)", fontsize=12) 154 | plt.ylabel(field, fontsize=12) 155 | 156 | # Get axis limits for this field 157 | limits = self.axis_limits.get(field, {"y_min": None, "y_max": None}) 158 | 159 | # Set y-axis limits 160 | y_min = limits["y_min"] 161 | y_max = limits["y_max"] 162 | 163 | # If y_max is None, calculate it from the data 164 | if y_max is None and values: 165 | y_max = max(values) * 1.1 # Add 10% margin 166 | 167 | plt.ylim(bottom=y_min, top=y_max) 168 | 169 | # Add grid for better readability 170 | plt.grid(True, linestyle='--', alpha=0.7) 171 | 172 | # Tight layout for better spacing 173 | plt.tight_layout() 174 | 175 | # Save the figure 176 | output_file = os.path.join(self.output_dir, f"{field}_{timestamp}.png") 177 | plt.savefig(output_file, dpi=150) 178 | plt.close() 179 | 180 | self.get_logger().info(f"Field '{field}' plotted and saved to {output_file} with {len(data_points)} data points") 181 | 182 | # Create a combined plot with all fields 183 | self.create_combined_plot(fields, timestamp) 184 | 185 | def create_combined_plot(self, fields, timestamp): 186 | """Create a combined plot with all fields in subplots""" 187 | n_fields = len(fields) 188 | 189 | # Calculate grid dimensions for subplots 190 | n_cols = min(3, n_fields) 191 | n_rows = (n_fields + n_cols - 1) // n_cols # Ceiling division 192 | 193 | # Create figure and subplots 194 | fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, 4 * n_rows)) 195 | 196 | # Flatten axes array for easier indexing if we have multiple rows 197 | if n_rows > 1: 198 | axes = axes.flatten() 199 | elif n_cols == 1: 200 | axes = [axes] # Ensure axes is always iterable 201 | 202 | # Plot each field in its own subplot 203 | for i, field in enumerate(fields): 204 | # Collect data for this field 205 | data_points = [] 206 | 207 | for j, msg in enumerate(self.stats_messages): 208 | timestamp_ms = j * 10 # Dummy timestamp for x-axis 209 | if hasattr(msg, field): 210 | value = getattr(msg, field) 211 | data_points.append((timestamp_ms, value)) 212 | 213 | if not data_points: 214 | continue 215 | 216 | # Get the current axis 217 | ax = axes[i] if i < len(axes) else None 218 | 219 | if ax is None: 220 | continue 221 | 222 | # Convert data to arrays for plotting 223 | timestamps = [t for t, _ in data_points] 224 | values = [v for _, v in data_points] 225 | 226 | # Get color for this field 227 | color = self.colors.get(field, "gray") 228 | 229 | # Plot the data with thin lines and no markers 230 | ax.plot(timestamps, values, '-', color=color, linewidth=0.8, alpha=1.0) 231 | 232 | # Set titles and labels 233 | ax.set_title(field, fontsize=10) 234 | ax.set_xlabel("Time", fontsize=8) 235 | ax.set_ylabel("Value", fontsize=8) 236 | 237 | # Get axis limits for this field 238 | limits = self.axis_limits.get(field, {"y_min": None, "y_max": None}) 239 | 240 | # Set y-axis limits 241 | y_min = limits["y_min"] 242 | y_max = limits["y_max"] 243 | 244 | # If y_max is None, calculate it from the data 245 | if y_max is None and values: 246 | y_max = max(values) * 1.1 # Add 10% margin 247 | 248 | ax.set_ylim(bottom=y_min, top=y_max) 249 | 250 | # Add grid 251 | ax.grid(True, linestyle='--', alpha=0.5) 252 | 253 | # Hide any unused subplots 254 | for i in range(len(fields), len(axes)): 255 | if i < len(axes): 256 | axes[i].set_visible(False) 257 | 258 | # Add overall title 259 | fig.suptitle("Super Odometry Statistics", fontsize=16) 260 | 261 | # Adjust layout 262 | plt.tight_layout(rect=[0, 0, 1, 0.96]) # Make room for the suptitle 263 | 264 | # Save the combined figure 265 | output_file = os.path.join(self.output_dir, f"combined_stats_{timestamp}.png") 266 | plt.savefig(output_file, dpi=150) 267 | plt.close() 268 | 269 | self.get_logger().info(f"Combined plot saved to {output_file}") 270 | 271 | def main(): 272 | parser = argparse.ArgumentParser(description='Subscribe to ROS2 super_odometry_stats and visualize with matplotlib') 273 | args = parser.parse_args() 274 | 275 | # Initialize ROS2 276 | rclpy.init() 277 | 278 | try: 279 | # Create and use the subscriber node 280 | stats_subscriber = OdometryStatsSubscriber() 281 | 282 | # Spin the node to process callbacks 283 | rclpy.spin(stats_subscriber) 284 | 285 | except KeyboardInterrupt: 286 | print("Node stopped by user") 287 | except Exception as e: 288 | print(f"Error: {e}") 289 | finally: 290 | # Clean up and shut down 291 | rclpy.shutdown() 292 | 293 | if __name__ == "__main__": 294 | main() -------------------------------------------------------------------------------- /script/visualizers/rerun_vis.py: -------------------------------------------------------------------------------- 1 | from typing_extensions import override 2 | 3 | import rerun as rr 4 | import torch 5 | 6 | from base import SemanticRgbdVis 7 | import utils 8 | 9 | class RerunVis(SemanticRgbdVis): 10 | """Semantic RGBD visualizer using ReRun.io""" 11 | def __init__(self, intrinsics_3x3: torch.FloatTensor, img_size = None, 12 | base_point_size = None, 13 | global_heat_scale = False, 14 | pose_period = 1, img_period = 1, pc_period = 1, 15 | feat_proj_basis_path = None, split_label_vis = False): 16 | super().__init__(intrinsics_3x3, img_size, base_point_size, 17 | global_heat_scale, pose_period, img_period, pc_period, 18 | feat_proj_basis_path) 19 | 20 | rr.init("semantic_mapping_vis", spawn=True) 21 | rr.set_time_seconds("stable_time", 0) 22 | self.base_name = "map" 23 | 24 | rr.log(self.base_name, rr.ViewCoordinates.FLU, static=True) 25 | 26 | rr.log(self.base_name, 27 | rr.Arrows3D( 28 | vectors=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], 29 | colors=[[255, 0, 0], [0, 255, 0], [0, 0, 255]], 30 | )) 31 | self.height = None 32 | self.width = None 33 | self.prev_pose_4x4 = None 34 | self.split_label_vis = split_label_vis 35 | 36 | @override 37 | def _log_rgb_image(self, rgb_img, layer = "rgb"): 38 | super()._log_rgb_image(rgb_img, layer) 39 | rr.log(f"{self.base_name}/pinhole/{layer}", rr.Image(rgb_img)) 40 | 41 | @override 42 | def _log_depth_image(self, depth_img, layer = "depth"): 43 | self.height, self.width = depth_img.shape 44 | self.img_updated = True 45 | rr.log(f"{self.base_name}/pinhole/{layer}", rr.DepthImage(depth_img)) 46 | 47 | @override 48 | def _log_pose(self, pose_4x4, layer = "pose"): 49 | super()._log_pose(pose_4x4, layer) 50 | rr.log(f"{self.base_name}/tf", 51 | rr.Pinhole(image_from_camera=self.intrinsics_3x3, 52 | height=self.height, width=self.width, 53 | camera_xyz=rr.ViewCoordinates.RDF)) 54 | rr_transform = rr.Transform3D(translation=pose_4x4[:3, 3], 55 | mat3x3=pose_4x4[:3, :3], from_parent=False) 56 | rr.log(f"{self.base_name}/tf", rr_transform) 57 | self.prev_pose_4x4 = pose_4x4 58 | 59 | @override 60 | def _log_pc(self, pc_xyz, pc_rgb = None, pc_radii = None, layer = "rgb"): 61 | super()._log_pc(pc_xyz, pc_rgb, layer) 62 | radii = self.base_point_size if pc_radii is None else pc_radii 63 | rr.log(f"{self.base_name}/{layer}", 64 | rr.Points3D(positions=pc_xyz, colors=pc_rgb, radii=radii)) 65 | 66 | @override 67 | def _log_arrows(self, origins, dirs, rgb = None, layer="arrows"): 68 | super()._log_arrows(origins, dirs, rgb, layer) 69 | rr.log(f"{self.base_name}/{layer}_arr", 70 | rr.Arrows3D(vectors=dirs*self.base_point_size*5, origins=origins, 71 | colors=rgb, radii=self.base_point_size/3)) 72 | 73 | @override 74 | def _log_label_pc(self, pc_xyz, pc_labels = None, layer = "label"): 75 | super()._log_label_pc(pc_xyz, pc_labels, layer) 76 | if self.split_label_vis and len(pc_labels.shape)==1 and pc_labels.shape[0] > 0: 77 | unique = torch.unique(pc_labels) 78 | pc_labels_onehot = torch.nn.functional.one_hot(pc_labels) 79 | for i in unique: 80 | rr.log(f"{self.base_name}/{layer}_pc/{i}", 81 | rr.Points3D(positions=pc_xyz[pc_labels_onehot[..., i]==1], class_ids=i, 82 | radii=self.base_point_size)) 83 | else: 84 | rr.log(f"{self.base_name}/{layer}_pc", 85 | rr.Points3D(positions=pc_xyz, class_ids=pc_labels, 86 | radii=self.base_point_size)) 87 | 88 | @override 89 | def _log_label_arrows(self, origins, dirs, labels, layer="arrow_labels"): 90 | super()._log_label_arrows(origins, dirs, labels, layer) 91 | if self.split_label_vis and len(labels.shape)==1 and labels.shape[0] > 0: 92 | unique = torch.unique(labels) 93 | labels_onehot = torch.nn.functional.one_hot(labels) 94 | for i in unique: 95 | m = labels_onehot[..., i]==1 96 | rr.log(f"{self.base_name}/{layer}_arr/{i}", 97 | rr.Arrows3D(vectors=(dirs*self.base_point_size*5)[m], origins=origins[m], 98 | class_ids=i, radii=self.base_point_size/3)) 99 | else: 100 | rr.log(f"{self.base_name}/{layer}_arr", 101 | rr.Arrows3D(vectors=dirs*self.base_point_size*5, origins=origins, 102 | class_ids=labels, radii=self.base_point_size/3)) 103 | 104 | def _log_box(self, box_mins, box_maxs, layer = ""): 105 | box_centers = (box_maxs + box_mins) / 2 106 | box_half_sizes = (box_maxs - box_mins) / 2 107 | rr.log(f"{self.base_name}/{layer}_boxes", 108 | rr.Boxes3D(centers=box_centers, half_sizes=box_half_sizes)) 109 | 110 | 111 | def _log_axes(self, pose_4x4, layer="axes"): 112 | for i in range(pose_4x4.shape[0]): 113 | rr_transform = rr.Transform3D( 114 | translation=pose_4x4[i, :3, 3], mat3x3=pose_4x4[i, :3, :3], 115 | from_parent=False, axis_length=1.0, 116 | scale=2) 117 | rr.log(f"{self.base_name}/{layer}/{i}", rr_transform) 118 | 119 | @override 120 | def next_frame(self, timestamp = None): 121 | super().next_frame(timestamp) 122 | rr.set_time_seconds("stable_time", self.vis_time*0.1) -------------------------------------------------------------------------------- /script/visualizers/utils.py: -------------------------------------------------------------------------------- 1 | """Utility functions supporting the other modules""" 2 | 3 | import torch 4 | import numpy as np 5 | 6 | def norm_std(x): 7 | x = x - x.mean() 8 | s = x.std() 9 | if s == 0: 10 | return torch.ones_like(x)*0.5 11 | else: 12 | return x / s 13 | 14 | def norm_01(x): 15 | x = x - x.min() 16 | mx = x.max() 17 | if mx == 0: 18 | return torch.ones_like(x)*0.5 19 | else: 20 | return x / mx 21 | 22 | def norm_img_01(x): 23 | B,C,H,W = x.shape 24 | x = x - torch.min(x.reshape(B,C, H*W), dim=-1).values.reshape(B,C,1,1) 25 | x = x / torch.max(x.reshape(B,C, H*W), dim=-1).values.reshape(B,C,1,1) 26 | return x 27 | 28 | def compute_cos_sim(vec1: torch.FloatTensor, 29 | vec2: torch.FloatTensor, 30 | softmax: bool = False) -> torch.FloatTensor: 31 | """Compute cosine similarity between two batches of D dim vectors. 32 | 33 | Args: 34 | vec1: NxC float tensor representing batch of vectors 35 | vec2: MxC float tensor representing batch of vectors 36 | softmax: If False, cosine similarity is returned. If True, softmaxed 37 | probability is returned. 38 | Returns: 39 | result: MxN float tensor representing similarity/prob. where result[0,1] 40 | represents the similarity of vec1[0] with vec2[1] 41 | """ 42 | N, C1 = vec1.shape 43 | M, C2 = vec2.shape 44 | if C1 != C2: 45 | raise ValueError(f"vec1 feature dimension '{C1}' does not match vec2" 46 | f"feature dimension '{C2}'") 47 | C = C1 48 | 49 | vec1 = vec1 / vec1.norm(dim = -1, keepdim = True) 50 | vec1 = vec1.reshape(1, N, 1, C) 51 | 52 | vec2 = vec2 / vec2.norm(dim=-1, keepdim = True) 53 | vec2 = vec2.reshape(M, 1, C, 1) 54 | 55 | sim = (vec1 @ vec2).reshape(M, N) 56 | if softmax: 57 | return torch.softmax(100 * sim, dim = -1) 58 | else: 59 | return sim 60 | 61 | def quaternion_to_rotation_matrix(q): 62 | """Convert quaternion to rotation matrix without external dependencies. 63 | 64 | Args: 65 | q: Quaternion in form [w, x, y, z] 66 | 67 | Returns: 68 | 3x3 rotation matrix as numpy array 69 | """ 70 | w, x, y, z = q 71 | 72 | # Calculate rotation matrix elements 73 | xx, xy, xz = x*x, x*y, x*z 74 | yy, yz, zz = y*y, y*z, z*z 75 | wx, wy, wz = w*x, w*y, w*z 76 | 77 | # Build the rotation matrix 78 | R = np.zeros((3, 3), dtype=np.float32) 79 | R[0, 0] = 1 - 2*(yy + zz) 80 | R[0, 1] = 2*(xy - wz) 81 | R[0, 2] = 2*(xz + wy) 82 | R[1, 0] = 2*(xy + wz) 83 | R[1, 1] = 1 - 2*(xx + zz) 84 | R[1, 2] = 2*(yz - wx) 85 | R[2, 0] = 2*(xz - wy) 86 | R[2, 1] = 2*(yz + wx) 87 | R[2, 2] = 1 - 2*(xx + yy) 88 | 89 | return R -------------------------------------------------------------------------------- /super_odometry/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | cmake-build-debug/ 3 | -------------------------------------------------------------------------------- /super_odometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(super_odometry) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++1z") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O4 -Wall -g") 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | SET(TBB_INCLUDE_DIR "/usr/include/tbb") 9 | add_compile_options(-Wall -Wextra -Wpedantic -Wunused-parameter) 10 | 11 | 12 | 13 | # Default to C99 14 | if(NOT CMAKE_C_STANDARD) 15 | set(CMAKE_C_STANDARD 99) 16 | endif() 17 | 18 | # Default to C++17 19 | if(NOT CMAKE_CXX_STANDARD) 20 | set(CMAKE_CXX_STANDARD 17) 21 | endif() 22 | 23 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 24 | add_compile_options(-Wall -Wextra ) 25 | endif() 26 | 27 | if (POLICY CMP0048) 28 | cmake_policy(SET CMP0048 NEW) 29 | endif (POLICY CMP0048) 30 | 31 | find_package(ament_cmake REQUIRED) 32 | find_package(ament_cmake_auto REQUIRED) 33 | find_package(rclcpp REQUIRED) 34 | find_package(std_msgs REQUIRED) 35 | find_package(geometry_msgs REQUIRED) 36 | find_package(nav_msgs REQUIRED) 37 | find_package(sensor_msgs REQUIRED) 38 | find_package(image_transport REQUIRED) 39 | find_package(cv_bridge REQUIRED) 40 | find_package(tf2 REQUIRED) 41 | find_package(tf2_ros REQUIRED) 42 | find_package(tf2_geometry_msgs REQUIRED) 43 | find_package(super_odometry_msgs REQUIRED) 44 | find_package(visualization_msgs REQUIRED) 45 | 46 | find_package(Eigen3 REQUIRED) 47 | find_package(Sophus REQUIRED) 48 | find_package(PCL REQUIRED) 49 | find_package(pcl_conversions REQUIRED) 50 | find_package(Ceres REQUIRED) 51 | find_package(GTSAM REQUIRED QUIET) 52 | find_package(TBB REQUIRED) 53 | find_package(livox_ros_driver2 REQUIRED) 54 | 55 | # Include directories 56 | include_directories( 57 | include 58 | ${PCL_INCLUDE_DIRS} 59 | ${CERES_INCLUDE_DIRS} 60 | ${OpenCV_INCLUDE_DIRS} 61 | ${GTSAM_INCLUDE_DIR} 62 | ${TBB_INCLUDE_DIR} 63 | ${Sophus_INCLUDE_DIRS} 64 | ${pcl_conversions_INCLUDE_DIRS} 65 | ${livox_ros_driver2_INCLUDE_DIRS} 66 | ) 67 | 68 | # Link directories 69 | link_directories( 70 | include 71 | ${PCL_LIBRARY_DIRS} 72 | ${OpenCV_LIBRARY_DIRS} 73 | ${GTSAM_LIBRARY_DIRS} 74 | ) 75 | 76 | set(GTSAM_LIBRARIES gtsam) 77 | 78 | 79 | # SuperOdom Library 80 | add_library(SuperOdomLib SHARED 81 | src/FeatureExtraction/featureExtraction.cpp 82 | src/ImuPreintegration/imuPreintegration.cpp 83 | src/LaserMapping/laserMapping.cpp 84 | src/LaserMapping/lidarOptimization.cpp 85 | src/LaserMapping/LocalMap.cpp 86 | src/LidarProcess/LidarSlam.cpp 87 | src/LidarProcess/pose_local_parameterization.cpp 88 | src/LidarProcess/SE3AbsolutatePoseFactor.cpp 89 | src/parameter/parameter.cpp 90 | src/utils/superodom_utils.cpp 91 | ) 92 | 93 | # Set up library include directory 94 | target_include_directories(SuperOdomLib PUBLIC 95 | $ 96 | $ # /include/mylib 97 | ) 98 | 99 | ament_target_dependencies(SuperOdomLib 100 | rclcpp 101 | std_msgs 102 | geometry_msgs 103 | nav_msgs 104 | sensor_msgs 105 | image_transport 106 | cv_bridge 107 | tf2 108 | tf2_ros 109 | tf2_geometry_msgs 110 | super_odometry_msgs 111 | visualization_msgs 112 | pcl_conversions 113 | livox_ros_driver2 114 | ) 115 | target_link_libraries(SuperOdomLib 116 | ${PCL_LIBRARIES} 117 | ${OpenCV_LIBRARIES} 118 | gtsam 119 | ${CERES_LIBRARIES} 120 | ) 121 | 122 | # IMU PreIntegration node 123 | add_executable(imu_preintegration_node src/imuPreintegration_node.cpp) 124 | ament_target_dependencies(imu_preintegration_node rclcpp nav_msgs sensor_msgs tf2_ros) 125 | target_link_libraries(imu_preintegration_node 126 | gtsam 127 | SuperOdomLib 128 | ) 129 | 130 | 131 | 132 | # Feature Extraction node 133 | add_executable(feature_extraction_node src/featureExtraction_node.cpp) 134 | ament_target_dependencies(feature_extraction_node rclcpp nav_msgs sensor_msgs super_odometry_msgs livox_ros_driver2) 135 | target_link_libraries(feature_extraction_node 136 | ${PCL_LIBRARIES} 137 | ${TBB_LIBRARIES} 138 | $ 139 | $ 140 | SuperOdomLib 141 | ) 142 | 143 | # LiDAR Mapping node 144 | add_executable(laser_mapping_node src/laserMapping_node.cpp) 145 | ament_target_dependencies(laser_mapping_node rclcpp std_msgs nav_msgs sensor_msgs geometry_msgs tf2 tf2_ros) 146 | target_link_libraries(laser_mapping_node 147 | ${PCL_LIBRARIES} 148 | ${CERES_LIBRARIES} 149 | SuperOdomLib 150 | ) 151 | 152 | # Install Exported Libraries 153 | ament_export_targets(export_super_odometry HAS_LIBRARY_TARGET) 154 | 155 | install( 156 | DIRECTORY include/ 157 | DESTINATION include 158 | ) 159 | 160 | install( 161 | TARGETS SuperOdomLib 162 | EXPORT export_super_odometry 163 | LIBRARY DESTINATION lib 164 | ARCHIVE DESTINATION lib 165 | RUNTIME DESTINATION bin 166 | INCLUDES DESTINATION include 167 | ) 168 | 169 | ament_export_include_directories(include) 170 | ament_export_libraries(SuperOdomLib) 171 | 172 | #Install executables 173 | install(TARGETS 174 | imu_preintegration_node 175 | feature_extraction_node 176 | laser_mapping_node 177 | DESTINATION lib/${PROJECT_NAME} 178 | ) 179 | 180 | install(DIRECTORY config launch 181 | DESTINATION share/${PROJECT_NAME}/ 182 | ) 183 | 184 | ament_package() 185 | -------------------------------------------------------------------------------- /super_odometry/cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindEigen.cmake - Find Eigen library, version >= 3. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # EIGEN_FOUND: TRUE iff Eigen is found. 37 | # EIGEN_INCLUDE_DIRS: Include directories for Eigen. 38 | # 39 | # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h 40 | # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 41 | # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 42 | # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 43 | # 44 | # The following variables control the behaviour of this module: 45 | # 46 | # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to 47 | # search for eigen includes, e.g: /timbuktu/eigen3. 48 | # 49 | # The following variables are also defined by this module, but in line with 50 | # CMake recommended FindPackage() module style should NOT be referenced directly 51 | # by callers (use the plural variables detailed above instead). These variables 52 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 53 | # are NOT re-called (i.e. search for library is not repeated) if these variables 54 | # are set with valid values _in the CMake cache_. This means that if these 55 | # variables are set directly in the cache, either by the user in the CMake GUI, 56 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 57 | # explicitly defines a cache variable), then they will be used verbatim, 58 | # bypassing the HINTS variables and other hard-coded search locations. 59 | # 60 | # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the 61 | # include directory of any dependencies. 62 | 63 | # Called if we failed to find Eigen or any of it's required dependencies, 64 | # unsets all public (designed to be used externally) variables and reports 65 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 66 | macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) 67 | unset(EIGEN_FOUND) 68 | unset(EIGEN_INCLUDE_DIRS) 69 | # Make results of search visible in the CMake GUI if Eigen has not 70 | # been found so that user does not have to toggle to advanced view. 71 | mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) 72 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 73 | # use the camelcase library name, not uppercase. 74 | if (Eigen_FIND_QUIETLY) 75 | message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 76 | elseif (Eigen_FIND_REQUIRED) 77 | message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 78 | else() 79 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 80 | # but continues configuration and allows generation. 81 | message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 82 | endif () 83 | return() 84 | endmacro(EIGEN_REPORT_NOT_FOUND) 85 | 86 | # Protect against any alternative find_package scripts for this library having 87 | # been called previously (in a client project) which set EIGEN_FOUND, but not 88 | # the other variables we require / set here which could cause the search logic 89 | # here to fail. 90 | unset(EIGEN_FOUND) 91 | 92 | # Search user-installed locations first, so that we prefer user installs 93 | # to system installs where both exist. 94 | list(APPEND EIGEN_CHECK_INCLUDE_DIRS 95 | /usr/local/include 96 | /usr/local/homebrew/include # Mac OS X 97 | /opt/local/var/macports/software # Mac OS X. 98 | /opt/local/include 99 | /usr/include) 100 | # Additional suffixes to try appending to each search path. 101 | list(APPEND EIGEN_CHECK_PATH_SUFFIXES 102 | eigen3 # Default root directory for Eigen. 103 | Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 104 | Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 105 | 106 | # Search supplied hint directories first if supplied. 107 | find_path(EIGEN_INCLUDE_DIR 108 | NAMES Eigen/Core 109 | PATHS ${EIGEN_INCLUDE_DIR_HINTS} 110 | ${EIGEN_CHECK_INCLUDE_DIRS} 111 | PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) 112 | 113 | if (NOT EIGEN_INCLUDE_DIR OR 114 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 115 | eigen_report_not_found( 116 | "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " 117 | "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") 118 | endif (NOT EIGEN_INCLUDE_DIR OR 119 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 120 | 121 | # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets 122 | # if called. 123 | set(EIGEN_FOUND TRUE) 124 | 125 | # Extract Eigen version from Eigen/src/Core/util/Macros.h 126 | if (EIGEN_INCLUDE_DIR) 127 | set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) 128 | if (NOT EXISTS ${EIGEN_VERSION_FILE}) 129 | eigen_report_not_found( 130 | "Could not find file: ${EIGEN_VERSION_FILE} " 131 | "containing version information in Eigen install located at: " 132 | "${EIGEN_INCLUDE_DIR}.") 133 | else (NOT EXISTS ${EIGEN_VERSION_FILE}) 134 | file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) 135 | 136 | string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" 137 | EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 138 | string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" 139 | EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") 140 | 141 | string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" 142 | EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 143 | string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" 144 | EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") 145 | 146 | string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" 147 | EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 148 | string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" 149 | EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") 150 | 151 | # This is on a single line s/t CMake does not interpret it as a list of 152 | # elements and insert ';' separators which would result in 3.;2.;0 nonsense. 153 | set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") 154 | endif (NOT EXISTS ${EIGEN_VERSION_FILE}) 155 | endif (EIGEN_INCLUDE_DIR) 156 | 157 | # Set standard CMake FindPackage variables if found. 158 | if (EIGEN_FOUND) 159 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 160 | endif (EIGEN_FOUND) 161 | 162 | # Handle REQUIRED / QUIET optional arguments and version. 163 | include(FindPackageHandleStandardArgs) 164 | find_package_handle_standard_args(Eigen 165 | REQUIRED_VARS EIGEN_INCLUDE_DIRS 166 | VERSION_VAR EIGEN_VERSION) 167 | 168 | # Only mark internal variables as advanced if we found Eigen, otherwise 169 | # leave it visible in the standard GUI for the user to set manually. 170 | if (EIGEN_FOUND) 171 | mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) 172 | endif (EIGEN_FOUND) 173 | -------------------------------------------------------------------------------- /super_odometry/config/livox/livox_mid360_calibration.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #Rotation from laser frame to imu frame, imu^R_laser 4 | extrinsicRotation_imu_laser: !!opencv-matrix 5 | rows: 3 6 | cols: 3 7 | dt: d 8 | data: [1., 0., 0., 9 | 0., 1., 0., 10 | 0., 0., 1.] 11 | 12 | #Translation from laser frame to imu frame, imu^T_laser 13 | extrinsicTranslation_imu_laser: !!opencv-matrix 14 | rows: 3 15 | cols: 1 16 | dt: d 17 | data: [-0.011, -0.02329, 0.04412] 18 | 19 | imu_laser_rotation_offset: !!opencv-matrix 20 | rows: 3 21 | cols: 1 22 | dt: d 23 | data: [0.0, 0.0, 0.0] #[0.0, 0.5, 0.0] 24 | 25 | yaw_ratio: 0.0 26 | -------------------------------------------------------------------------------- /super_odometry/config/livox_mid360.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | imu_topic: "/imu/data" 4 | laser_topic: "/lidar/scan" 5 | sensor: "livox" 6 | use_imu_roll_pitch: false 7 | 8 | world_frame: "map" 9 | world_frame_rot: "map_rot" 10 | sensor_frame: "sensor" 11 | sensor_frame_rot: "sensor_rot" 12 | 13 | imu_acc_x_limit: 0.5 14 | imu_acc_y_limit: 0.2 15 | imu_acc_z_limit: 0.4 16 | 17 | # Feature Extraction Params 18 | feature_extraction_node: 19 | scan_line: 4 20 | min_range: 0.2 # 21 | filter_point_size: 3 22 | 23 | # Laser Mapping Params 24 | laser_mapping_node: 25 | mapping_line_resolution: 0.1 26 | mapping_plane_resolution: 0.1 27 | max_iterations: 5 28 | max_surface_features: 4000 29 | 30 | # For localization mode 31 | localization_mode: false 32 | read_pose_file: false 33 | init_x: 0.0 34 | init_y: 0.0 35 | init_z: 0.0 36 | init_roll: 0.0 37 | init_pitch: 0.0 38 | init_yaw: 0.0 39 | 40 | # IMU Preintegration Params 41 | imu_preintegration_node: 42 | lidar_correction_noise: 0.01 43 | 44 | acc_n: 3.9939570888238808e-03 # accelerometer measurement noise standard deviation. #0.2 45 | gyr_n: 1.5636343949698187e-03 # gyroscope measurement noise standard deviation. #0.05 46 | acc_w: 6.4356659353532566e-05 # accelerometer bias random work noise standard deviation. #0.02 47 | gyr_w: 3.5640318696367613e-05 # gyroscope bias random work noise standard deviation. #4.0e-5 48 | g_norm: 9.80511 # 0 means you need to provide both imu-camera and laser-camera extrinsic 49 | -------------------------------------------------------------------------------- /super_odometry/config/os1_128.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | imu_topic: "/ouster/imu" 4 | laser_topic: "/ouster/points" 5 | sensor: "ouster" 6 | use_imu_roll_pitch: false 7 | 8 | world_frame: "map" 9 | world_frame_rot: "map_rot" 10 | sensor_frame: "sensor" 11 | sensor_frame_rot: "sensor_rot" 12 | 13 | imu_acc_x_limit: 0.5 14 | imu_acc_y_limit: 0.2 15 | imu_acc_z_limit: 0.4 16 | 17 | # Feature Extraction Params 18 | feature_extraction_node: 19 | scan_line: 128 20 | min_range: 0.2 # 21 | filter_point_size: 3 22 | 23 | # Laser Mapping Params 24 | laser_mapping_node: 25 | mapping_line_resolution: 0.1 26 | mapping_plane_resolution: 0.2 27 | max_iterations: 5 28 | max_surface_features: 2000 29 | 30 | # For localization mode 31 | localization_mode: false 32 | read_pose_file: false 33 | init_x: 0.0 34 | init_y: 0.0 35 | init_z: 0.0 36 | init_roll: 0.0 37 | init_pitch: 0.0 38 | init_yaw: 0.0 39 | 40 | # IMU Preintegration Params 41 | imu_preintegration_node: 42 | lidar_correction_noise: 0.01 43 | 44 | acc_n: 3.9939570888238808e-03 # accelerometer measurement noise standard deviation. #0.2 45 | gyr_n: 1.5636343949698187e-03 # gyroscope measurement noise standard deviation. #0.05 46 | acc_w: 6.4356659353532566e-05 # accelerometer bias random work noise standard deviation. #0.02 47 | gyr_w: 3.5640318696367613e-05 # gyroscope bias random work noise standard deviation. #4.0e-5 48 | g_norm: 9.80511 # 0 means you need to provide both imu-camera and laser-camera extrinsic 49 | 50 | -------------------------------------------------------------------------------- /super_odometry/config/ouster/os1_128_calibration.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #Rotation from laser frame to imu frame, imu^R_laser 4 | extrinsicRotation_imu_laser: !!opencv-matrix 5 | rows: 3 6 | cols: 3 7 | dt: d 8 | data: [1., 0., 0., 9 | 0., 1., 0., 10 | 0., 0., 1.] 11 | # data: [-1., 0., 0., 12 | # 0., 1., 0., 13 | # 0., 0., -1.] 14 | 15 | #Translation from laser frame to imu frame, imu^T_laser 16 | extrinsicTranslation_imu_laser: !!opencv-matrix 17 | rows: 3 18 | cols: 1 19 | dt: d 20 | #data: [0.0, 0.0, 0.0] 21 | # data: [0.002, 0.010, 0.031] 22 | data: [-0.006253, 0.011775, -0.007645] 23 | 24 | 25 | 26 | imu_laser_rotation_offset: !!opencv-matrix 27 | rows: 3 28 | cols: 1 29 | dt: d 30 | data: [0.0, 0.0, 0.0] #[0.0, 0.5, 0.0] 31 | 32 | yaw_ratio: 0.0 33 | -------------------------------------------------------------------------------- /super_odometry/config/velodyne/vlp_16_calibration.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #Rotation from laser frame to imu frame, imu^R_laser 4 | extrinsicRotation_imu_laser: !!opencv-matrix 5 | rows: 3 6 | cols: 3 7 | dt: d 8 | data: [1., 0., 0., 9 | 0., 1., 0., 10 | 0., 0., 1.] 11 | # data: [-1., 0., 0., 12 | # 0., 1., 0., 13 | # 0., 0., -1.] 14 | 15 | #Translation from laser frame to imu frame, imu^T_laser 16 | extrinsicTranslation_imu_laser: !!opencv-matrix 17 | rows: 3 18 | cols: 1 19 | dt: d 20 | # data: [0.0, 0.0, 0.0] 21 | data: [0.080, 0.029, 0.030] 22 | 23 | 24 | imu_laser_rotation_offset: !!opencv-matrix 25 | rows: 3 26 | cols: 1 27 | dt: d 28 | data: [0.0, 0.0, 0.0] #[0.0, 0.5, 0.0] 29 | 30 | yaw_ratio: 0.0 31 | -------------------------------------------------------------------------------- /super_odometry/config/vlp_16.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | imu_topic: "/imu/data" 4 | laser_topic: "/velodyne_points" 5 | sensor: "velodyne" 6 | use_imu_roll_pitch: false 7 | 8 | world_frame: "map" 9 | world_frame_rot: "map_rot" 10 | sensor_frame: "sensor" 11 | sensor_frame_rot: "sensor_rot" 12 | 13 | imu_acc_x_limit: 0.5 14 | imu_acc_y_limit: 0.2 15 | imu_acc_z_limit: 0.4 16 | 17 | # Feature Extraction Params 18 | feature_extraction_node: 19 | scan_line: 16 20 | min_range: 0.2 # 21 | filter_point_size: 3 22 | 23 | # Laser Mapping Params 24 | laser_mapping_node: 25 | mapping_line_resolution: 0.1 26 | mapping_plane_resolution: 0.2 27 | max_iterations: 5 28 | max_surface_features: 2000 29 | 30 | # For localization mode 31 | localization_mode: false 32 | read_pose_file: false 33 | init_x: 0.0 34 | init_y: 0.0 35 | init_z: 0.0 36 | init_roll: 0.0 37 | init_pitch: 0.0 38 | init_yaw: 0.0 39 | 40 | # IMU Preintegration Params 41 | imu_preintegration_node: 42 | lidar_correction_noise: 0.01 43 | 44 | acc_n: 3.9939570888238808e-03 # accelerometer measurement noise standard deviation. #0.2 45 | gyr_n: 1.5636343949698187e-03 # gyroscope measurement noise standard deviation. #0.05 46 | acc_w: 6.4356659353532566e-05 # accelerometer bias random work noise standard deviation. #0.02 47 | gyr_w: 3.5640318696367613e-05 # gyroscope bias random work noise standard deviation. #4.0e-5 48 | g_norm: 9.80511 # 0 means you need to provide both imu-camera and laser-camera extrinsic 49 | 50 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/FeatureExtraction/featureExtraction.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shibo zhao on 2020-09-27. 3 | // 4 | 5 | #ifndef super_odometry_FEATUREEXTRACTION_H 6 | #define super_odometry_FEATUREEXTRACTION_H 7 | 8 | // #include "super_odometry/logging.h" 9 | 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "rclcpp/rclcpp.hpp" 21 | #include 22 | #include 23 | #include 24 | 25 | #include "super_odometry/container/MapRingBuffer.h" 26 | #include "super_odometry/sensor_data/imu/imu_data.h" 27 | #include "super_odometry/sensor_data/pointcloud/point_os.h" 28 | #include "super_odometry/tic_toc.h" 29 | #include "super_odometry/utils/Twist.h" 30 | #include "super_odometry/config/parameter.h" 31 | 32 | #include 33 | 34 | #include 35 | #include "super_odometry/utils/superodom_utils.h" 36 | 37 | 38 | namespace super_odometry { 39 | 40 | 41 | using std::atan2; 42 | using std::cos; 43 | using std::sin; 44 | std::vector> all_cloud_buf(2); 45 | 46 | constexpr unsigned int BLOCK_TIME_NS = 55296; // Time in ns for one block (measurement + recharge) 47 | constexpr std::size_t NUM_BLOCKS = 12; // Number of blocks in a Velodyne packet 48 | constexpr double LIDAR_MESSAGE_TIME = (double)(NUM_BLOCKS * BLOCK_TIME_NS * 151) * 1e-9; 49 | constexpr double IMU_TIME_LENIENCY = 0.1; 50 | 51 | 52 | struct bounds_t 53 | { 54 | double blindFront; 55 | double blindBack; 56 | double blindRight; 57 | double blindLeft; 58 | }; 59 | 60 | struct feature_extraction_config{ 61 | bounds_t box_size; 62 | int skipFrame; 63 | int N_SCANS; 64 | int provide_point_time; 65 | bool use_dynamic_mask; 66 | bool use_imu_roll_pitch; 67 | bool debug_view_enabled; 68 | float min_range; 69 | float max_range; 70 | int filter_point_size; 71 | SensorType sensor; 72 | double imu_acc_x_limit; 73 | double imu_acc_y_limit; 74 | double imu_acc_z_limit; 75 | }; 76 | 77 | struct ImuMeasurement { 78 | double timestamp; 79 | Eigen::Vector3d accel; 80 | Eigen::Vector3d gyr; 81 | Eigen::Quaterniond orientation; 82 | }; 83 | 84 | typedef feature_extraction_config feature_extraction_config; 85 | 86 | class featureExtraction : public rclcpp::Node { 87 | public: 88 | 89 | /* TODO: return this as a parameter */ 90 | 91 | static constexpr double scanPeriod = 0.100859904 - 20.736e-6; 92 | static constexpr double columnTime = 55.296e-6; 93 | static constexpr double laserTime = 2.304e-6; 94 | 95 | featureExtraction(const rclcpp::NodeOptions & options); 96 | 97 | void initInterface(); 98 | 99 | template 100 | bool synchronize_measurements(MapRingBuffer &measureBuf, 101 | MapRingBuffer::Ptr> &lidarBuf); 102 | 103 | void imuRemovePointDistortion(double lidar_start_time, double lidar_end_time, MapRingBuffer &imuBuf, 104 | pcl::PointCloud::Ptr &lidar_msg); 105 | 106 | void vioRemovePointDistortion(double lidar_start_time, double lidar_end_time, MapRingBuffer&vioBuf, 107 | pcl::PointCloud::Ptr &lidar_msg); 108 | 109 | void undistortionAndFeatureExtraction(); 110 | 111 | void extractFeatures(double lidar_start_time, const pcl::PointCloud::Ptr& lidar_msg, const Eigen::Quaterniond& quaternion); 112 | 113 | void imu_Handler(const sensor_msgs::msg::Imu::SharedPtr msg_in); 114 | 115 | void visual_odom_Handler(const nav_msgs::msg::Odometry::SharedPtr visualOdometry); 116 | 117 | void laserCloudHandler(const sensor_msgs::msg::PointCloud2::SharedPtr laserCloudMsg); 118 | 119 | void livoxHandler(const livox_ros_driver2::msg::CustomMsg::UniquePtr msg); 120 | 121 | void uniformFeatureExtraction(const pcl::PointCloud::Ptr &pc_in, 122 | pcl::PointCloud::Ptr &pc_out_surf, int skip_num, float block_range); 123 | 124 | void assignTimeforPointCloud(pcl::PointCloud::Ptr laserCloudIn_ptr_); 125 | 126 | template 127 | sensor_msgs::msg::PointCloud2 publishCloud(rclcpp::Publisher::SharedPtr thisPub, typename pcl::PointCloud::Ptr thisCloud, rclcpp::Time thisStamp, std::string thisFrame); 128 | 129 | bool readParameters(); 130 | 131 | void publishTopic(double lidar_start_time, 132 | pcl::PointCloud::Ptr laser_no_distortion_points, 133 | pcl::PointCloud::Ptr edgePoints, 134 | pcl::PointCloud::Ptr plannerPoints, 135 | pcl::PointCloud::Ptr depthPoints, 136 | Eigen::Quaterniond q_w_original_l); 137 | 138 | void manageLidarBuffer(pcl::PointCloud::Ptr pointCloud, double timestamp); 139 | 140 | ImuMeasurement parseImuMessage(const sensor_msgs::msg::Imu::SharedPtr& msg); 141 | 142 | double calculateDeltaTime(double current_timestamp); 143 | 144 | Imu::Ptr createImuData(const ImuMeasurement& measurement); 145 | 146 | void updateImuOrientation(Imu::Ptr& imudata); 147 | 148 | void imuInitialization(double timestamp); 149 | 150 | template 151 | void removePointDistortion( 152 | double lidar_start_time, 153 | double lidar_end_time, 154 | MapRingBuffer &buffer, 155 | pcl::PointCloud::Ptr &lidar_msg); 156 | 157 | 158 | 159 | template 160 | Transformd getInterpolatedPose(double timestamp, MapRingBuffer &buffer, 161 | const std::function& extractPose); 162 | 163 | Eigen::Vector3d transformPoint(const point_os::PointcloudXYZITR& point, const Transformd& T_w_original, 164 | const Transformd& point_pose, bool is_imu_data); 165 | 166 | void updatePointPosition(point_os::PointcloudXYZITR& point, const Eigen::Vector3d& new_pos); 167 | 168 | bool isPointValid(const point_os::PointcloudXYZITR& point); 169 | 170 | 171 | Imu::Ptr imu_Init = std::make_shared(); 172 | MapRingBuffer imuBuf; 173 | MapRingBuffer::Ptr> lidarBuf; 174 | MapRingBuffer visualOdomBuf; 175 | 176 | 177 | private: 178 | // ROS Interface 179 | // Subscribers 180 | rclcpp::Subscription::SharedPtr subLaserCloud; 181 | rclcpp::Subscription::SharedPtr subImu; 182 | rclcpp::Subscription::SharedPtr subOdom; 183 | rclcpp::Subscription::SharedPtr subLivoxCloud; 184 | 185 | // Publishers 186 | rclcpp::Publisher::SharedPtr pubLaserCloud; 187 | rclcpp::Publisher::SharedPtr pubEdgePoints; 188 | rclcpp::Publisher::SharedPtr pubPlannerPoints; 189 | rclcpp::Publisher::SharedPtr pubBobPoints; 190 | rclcpp::Publisher::SharedPtr pubLaserFeatureInfo; 191 | std::vector::SharedPtr> pubEachScan; 192 | 193 | rclcpp::CallbackGroup::SharedPtr cb_group_; 194 | 195 | int delay_count_; 196 | std::mutex m_buf; 197 | int frameCount = 0; 198 | 199 | bool PUB_EACH_LINE = false; 200 | bool LASER_IMU_SYNC_SCCUESS = false; 201 | bool LASER_CAMERA_SYNC_SUCCESS = false; 202 | bool IMU_INIT=false; 203 | double m_imuPeriod; 204 | 205 | super_odometry_msgs::msg::LaserFeature laserFeature; 206 | std_msgs::msg::Header FeatureHeader; 207 | Eigen::Quaterniond q_w_original_l; 208 | Eigen::Vector3d t_w_original_l; 209 | pcl::PointCloud::Ptr pointCloudwithTime=nullptr; 210 | pcl::PointCloud::Ptr tmpOusterCloudIn=nullptr ; 211 | feature_extraction_config config_; 212 | }; 213 | 214 | } // namespace super_odometry 215 | 216 | #endif //super_odometry_FEATUREEXTRACTION_H 217 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/ImuPreintegration/imuPreintegration.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shibo zhao on 2020-09-27. 3 | // 4 | #pragma once 5 | #ifndef IMUPREINTEGRATION_H 6 | #define IMUPREINTEGRATION_H 7 | 8 | #include "rclcpp/rclcpp.hpp" 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | #include "utility.h" 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include "super_odometry/utils/Twist.h" 34 | #include "super_odometry/container/MapRingBuffer.h" 35 | #include "super_odometry/config/parameter.h" 36 | #include "super_odometry/tic_toc.h" 37 | #include 38 | #include "super_odometry/sensor_data/imu/imu_data.h" 39 | 40 | 41 | namespace super_odometry { 42 | 43 | using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) 44 | using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) 45 | using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) 46 | using FrameId = std::uint64_t; 47 | 48 | struct imuPreintegration_config{ 49 | float imuAccNoise; 50 | float imuAccBiasN; 51 | float imuGyrNoise; 52 | float imuGyrBiasN; 53 | float imuGravity; 54 | float lidar_correction_noise; 55 | float smooth_factor; 56 | bool use_imu_roll_pitch; 57 | SensorType sensor; 58 | 59 | double imu_acc_x_limit; 60 | double imu_acc_y_limit; 61 | double imu_acc_z_limit; 62 | }; 63 | 64 | class imuPreintegration : public rclcpp::Node { 65 | public: 66 | 67 | imuPreintegration(const rclcpp::NodeOptions & options); 68 | 69 | static constexpr double delta_t = 0; 70 | static constexpr double imu_laser_timedelay= 0.8; 71 | 72 | public: 73 | void initInterface(); 74 | 75 | bool readParameters(); 76 | 77 | void laserodometryHandler(const nav_msgs::msg::Odometry::SharedPtr odomMsg); 78 | 79 | void imuHandler(const sensor_msgs::msg::Imu::SharedPtr imu_raw); 80 | 81 | void initial_system(double currentCorrectionTime, gtsam::Pose3 lidarPose); 82 | 83 | void process_imu_odometry(double currentCorrectionTime, gtsam::Pose3 relativePose); 84 | 85 | bool build_graph(gtsam::Pose3 lidarPose, double curLaserodomtimestamp); 86 | 87 | void repropagate_imuodometry(double currentCorrectionTime); 88 | 89 | bool failureDetection(const gtsam::Vector3 &velCur, 90 | const gtsam::imuBias::ConstantBias &biasCur); 91 | 92 | void obtainCurrodometry(nav_msgs::msg::Odometry::SharedPtr &odomMsg, double ¤tCorrectionTime, 93 | gtsam::Pose3 &lidarPose, 94 | int ¤tResetId); 95 | 96 | void integrate_imumeasurement(double currentCorrectionTime); 97 | 98 | void reset_graph(); 99 | 100 | void resetOptimization(); 101 | 102 | void resetParams(); 103 | 104 | bool handleIMUInitialization(const sensor_msgs::msg::Imu::SharedPtr&imu_raw, 105 | sensor_msgs::msg::Imu& thisImu); 106 | 107 | 108 | void updateAndPublishPath(nav_msgs::msg::Odometry &odometry, const sensor_msgs::msg::Imu& thisImu); 109 | 110 | void publishTransform(nav_msgs::msg::Odometry &odometry, const sensor_msgs::msg::Imu& thisImu); 111 | 112 | void prepareOdometryMessage(nav_msgs::msg::Odometry &odometry, const sensor_msgs::msg::Imu& thisImu, const gtsam::NavState ¤tState); 113 | 114 | void publishOdometry(const sensor_msgs::msg::Imu& thisImu, const gtsam::NavState& currentState, nav_msgs::msg::Odometry &odometry); 115 | 116 | void publishTransformsAndPath(nav_msgs::msg::Odometry &odometry, const sensor_msgs::msg::Imu& thisImu); 117 | 118 | void processTiming(const sensor_msgs::msg::Imu& thisImu); 119 | 120 | void initializeImu(const sensor_msgs::msg::Imu::SharedPtr& imu_raw); 121 | 122 | void correctLivoxGravity(sensor_msgs::msg::Imu& thisImu); 123 | 124 | 125 | sensor_msgs::msg::Imu 126 | imuConverter(const sensor_msgs::msg::Imu &imu_in); 127 | 128 | template 129 | double secs(T msg) { 130 | return msg->header.stamp.sec + msg->header.stamp.nanosec*1e-9; 131 | } 132 | 133 | 134 | private: 135 | 136 | rclcpp::Subscription::SharedPtr subImu; 137 | rclcpp::Subscription::SharedPtr subLaserOdometry; 138 | 139 | 140 | rclcpp::Publisher::SharedPtr pubImuOdometry; 141 | rclcpp::Publisher::SharedPtr pubHealthStatus; 142 | rclcpp::Publisher::SharedPtr pubImuPath; 143 | 144 | 145 | rclcpp::CallbackGroup::SharedPtr cb_group_; 146 | public: 147 | gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise; 148 | gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise; 149 | gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise; 150 | gtsam::noiseModel::Diagonal::shared_ptr correctionNoise; 151 | gtsam::Vector noiseModelBetweenBias; 152 | std::shared_ptr imuIntegratorOpt_; 153 | std::shared_ptr imuIntegratorImu_; 154 | gtsam::Pose3 prevPose_; 155 | gtsam::Vector3 prevVel_; 156 | gtsam::NavState prevState_; 157 | gtsam::imuBias::ConstantBias prevBias_; 158 | gtsam::NavState prevStateOdom; 159 | gtsam::imuBias::ConstantBias prevBiasOdom; 160 | gtsam::ISAM2 optimizer; 161 | gtsam::NonlinearFactorGraph graphFactors; 162 | gtsam::Values graphValues; 163 | gtsam::Pose3 lidarodom_w_pre; 164 | gtsam::Pose3 lidarodom_w_cur; 165 | 166 | 167 | public: 168 | //Modify the extrinsic matrxi between laser and imu, laser and camera 169 | gtsam::Pose3 imu2cam; 170 | gtsam::Pose3 cam2Lidar; 171 | gtsam::Pose3 imu2Lidar; 172 | gtsam::Pose3 lidar2Imu; 173 | 174 | public: 175 | MapRingBuffer imuBuf; 176 | std::deque imuQueOpt; 177 | std::deque imuQueImu; 178 | MapRingBuffer lidarOdomBuf; 179 | std::mutex mBuf; 180 | Imu::Ptr imu_Init = std::make_shared(); 181 | 182 | public: 183 | bool systemInitialized = false; 184 | bool doneFirstOpt = false; 185 | bool health_status = true; 186 | bool imu_init_success = false; 187 | 188 | 189 | Eigen::Quaterniond firstImu; 190 | Eigen::Vector3d gyr_pre; 191 | 192 | double first_imu_time_stamp; 193 | double last_processed_lidar_time = -1; 194 | double lastImuT_imu = -1; 195 | double lastImuT_opt = -1; 196 | int key = 1; 197 | int imuPreintegrationResetId = 0; 198 | int frame_count = 0; 199 | 200 | enum IMU_STATE : uint8_t { 201 | FAIL=0, //lose imu information 202 | SUCCESS=1, //Obtain the good imu data 203 | UNKNOW=2 204 | }; 205 | 206 | IMU_STATE RESULT; 207 | nav_msgs::msg::Odometry::SharedPtr cur_frame = nullptr; 208 | nav_msgs::msg::Odometry::SharedPtr last_frame = nullptr; 209 | imuPreintegration_config config_; 210 | }; 211 | 212 | } 213 | 214 | #endif // IMUPREINTEGRATION_H 215 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/LaserMapping/laserMapping.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shibo zhao on 2020-09-27. 3 | // 4 | 5 | #pragma once 6 | #ifndef super_odometry_LASERMAPPING_H 7 | #define super_odometry_LASERMAPPING_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include "rclcpp/rclcpp.hpp" 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include "super_odometry/LidarProcess/LidarSlam.h" 35 | #include "super_odometry/LidarProcess/LocalMap.h" 36 | #include "super_odometry/tic_toc.h" 37 | #include "super_odometry/utils/Twist.h" 38 | #include "super_odometry/container/MapRingBuffer.h" 39 | #include 40 | #include "super_odometry/config/parameter.h" 41 | #include 42 | #include "super_odometry/utils/superodom_utils.h" 43 | 44 | namespace super_odometry { 45 | struct laser_mapping_config{ 46 | float lineRes; 47 | float planeRes; 48 | int max_iterations; 49 | bool debug_view_enabled; 50 | bool enable_ouster_data; 51 | bool publish_only_feature_points; 52 | bool use_imu_roll_pitch; 53 | int max_surface_features; 54 | double velocity_failure_threshold; 55 | bool auto_voxel_size; 56 | bool forget_far_chunks; 57 | float visual_confidence_factor; 58 | float pos_degeneracy_threshold; 59 | float ori_degeneracy_threshold; 60 | float yaw_ratio; 61 | std::string map_dir; 62 | bool localization_mode; 63 | float init_x; 64 | float init_y; 65 | float init_z; 66 | float init_roll; 67 | float init_pitch; 68 | float init_yaw; 69 | float read_pose_file; 70 | }; 71 | 72 | class laserMapping : public rclcpp::Node { 73 | public: 74 | LidarSLAM slam; 75 | struct SensorData { 76 | pcl::PointCloud::Ptr laserCloudCornerLast; //not used in optimization 77 | pcl::PointCloud::Ptr laserCloudSurfLast; 78 | pcl::PointCloud::Ptr laserCloudFullRes; 79 | Eigen::Quaterniond imuPrediction; 80 | Transformd vioPrediction; //prediction from vio 81 | Transformd lioPrediction; //prediction from lio 82 | Transformd nioPrediction; //prediction from imu network 83 | bool vio_prediction_status; 84 | bool lio_prediction_status; 85 | bool nio_prediction_status; 86 | bool imu_orientation_status; 87 | double timestamp; 88 | }; 89 | 90 | SensorData sensorMeas; 91 | 92 | enum class PredictionSource {IMU_ORIENTATION, LIO_ODOM, VIO_ODOM, NEURAL_IMU_ODOM, CONSTANT_VELOCITY}; 93 | PredictionSource prediction_source; 94 | 95 | public: 96 | laserMapping(const rclcpp::NodeOptions & options); 97 | 98 | void initInterface(); 99 | 100 | void initializationParam(); 101 | 102 | void preprocessDualLidarFeatures(Transformd current_pose, SensorType sensor_type); 103 | 104 | void adjustVoxelSize(); 105 | 106 | void mappingOptimization(Eigen::Vector3i &postion_in_locamap, Transformd start_tf,tf2::Quaternion roll_pitch_quat, 107 | const int laserCloudCornerStackNum, const int laserCloudSurfStackNum); 108 | 109 | void publishTopic(Eigen::Vector3i postion_in_locamap); 110 | 111 | 112 | void laserFeatureInfoHandler(const super_odometry_msgs::msg::LaserFeature::SharedPtr msgIn); 113 | 114 | void laserCloudRawDataHandler(const sensor_msgs::msg::PointCloud2::SharedPtr laserCloudRawdata); 115 | 116 | void extractIMUOdometry(double timeLaserFrame, Transformd &T_w_lidar); 117 | 118 | bool extractVisualIMUOdometryAndCheck(Transformd &T_w_lidar); 119 | 120 | void getOdometryFromTimestamp(MapRingBuffer &buf, const double ×tamp, 121 | Eigen::Vector3d &T, Eigen::Quaterniond &Q); 122 | 123 | void extractRelativeTransform(MapRingBuffer &buf, Transformd &T_pre_cur, bool imu_prediction); 124 | 125 | void setInitialGuess(); 126 | 127 | void selectposePrediction(); 128 | 129 | laserMapping::PredictionSource determinePredictionSource(); 130 | 131 | void initializeFirstFrame(); 132 | 133 | void initializeWithIMU(); 134 | 135 | void selectPosePrediction(); 136 | 137 | //TODO: organize the publish topics and odometry 138 | void publishOdometry(); 139 | 140 | void publishTopic(); 141 | 142 | void process(); 143 | 144 | bool readParameters(); 145 | 146 | bool checkDataAvailable() const; 147 | 148 | SensorData extractSensorData(); 149 | 150 | void clearSensorData(); 151 | 152 | bool useIMUPrediction(const Eigen::Quaterniond& imuPrediction); 153 | 154 | void performSLAMOptimization(); 155 | 156 | void updatePoseAndPublish(); 157 | 158 | 159 | 160 | 161 | template 162 | double secs(T msg) { 163 | return msg->header.stamp.sec + msg->header.stamp.nanosec*1e-9; 164 | } 165 | 166 | private: 167 | static constexpr float vision_laser_time_offset = 0.0; 168 | static constexpr int laserCloudCenWidth = 10; 169 | static constexpr int laserCloudCenHeight = 10; 170 | static constexpr int laserCloudCenDepth = 5; 171 | static constexpr int laserCloudWidth = 21; 172 | static constexpr int laserCloudHeight = 21; 173 | static constexpr int laserCloudDepth = 11; 174 | static constexpr int laserCloudNum =laserCloudWidth * laserCloudHeight * laserCloudDepth; // 4851 175 | 176 | // ros::NodeHandle *pub_node_; 177 | // ros::NodeHandle *private_node_; 178 | 179 | // ROS 180 | // Subscribers 181 | rclcpp::Subscription::SharedPtr subLaserCloudCornerLast; 182 | rclcpp::Subscription::SharedPtr subLaserCloudSurfLast; 183 | rclcpp::Subscription::SharedPtr subIMUOdometry; 184 | rclcpp::Subscription::SharedPtr subVisualOdometry; 185 | rclcpp::Subscription::SharedPtr subLaserCloudFullRes; 186 | rclcpp::Subscription::SharedPtr subLaserRawdata; 187 | rclcpp::Subscription::SharedPtr subLaserFeatureInfo; 188 | // rclcpp::Subscription<>::SharedPtr subTakeoffAlignment; 189 | 190 | // Publisher 191 | rclcpp::Publisher::SharedPtr pubLaserCloudSurround; 192 | rclcpp::Publisher::SharedPtr pubLaserCloudMap; 193 | rclcpp::Publisher::SharedPtr pubLaserCloudPrior; 194 | rclcpp::Publisher::SharedPtr pubLaserCloudFullRes; 195 | rclcpp::Publisher::SharedPtr pubLaserCloudFullRes_rot; 196 | rclcpp::Publisher::SharedPtr pubLaserCloudFullResOusterWithFeatures; 197 | rclcpp::Publisher::SharedPtr pubLaserCloudFullResOuster; 198 | rclcpp::Publisher::SharedPtr pubLaserCloudRawRes; 199 | rclcpp::Publisher::SharedPtr pubOdomAftMapped; 200 | rclcpp::Publisher::SharedPtr pubOdomAftMapped_rot; 201 | rclcpp::Publisher::SharedPtr pubOdomAftMappedHighFrec; 202 | rclcpp::Publisher::SharedPtr pubLaserAfterMappedPath; 203 | rclcpp::Publisher::SharedPtr pubOptimizationStats; 204 | rclcpp::Publisher::SharedPtr pubLaserOdometryIncremental; 205 | rclcpp::Publisher::SharedPtr pubPreviousCloud; 206 | rclcpp::Publisher::SharedPtr pubPreviousPose; 207 | rclcpp::Publisher::SharedPtr pubprediction_source; 208 | rclcpp::Publisher::SharedPtr pubVIOPrediction; 209 | rclcpp::Publisher::SharedPtr pubLIOPrediction; 210 | 211 | rclcpp::TimerBase::SharedPtr process_timer_; 212 | 213 | rclcpp::CallbackGroup::SharedPtr cb_group_; 214 | 215 | MapRingBuffer imu_odom_buf; 216 | MapRingBuffer visual_odom_buf; 217 | 218 | int frameCount = 0; 219 | int waiting_takeoff_timeout = 300; 220 | int startupCount = 10; 221 | int localizationCount = 0; 222 | int laserCloudValidInd[125]; 223 | int laserCloudSurroundInd[125]; 224 | 225 | double timeLaserCloudCornerLast = 0; 226 | double timeLaserCloudSurfLast = 0; 227 | double timeLaserCloudFullRes = 0; 228 | double timeLaserOdometry = 0; 229 | double timeLaserOdometryPrev = 0; 230 | 231 | 232 | bool got_previous_map = false; 233 | bool force_initial_guess = false; 234 | bool odomAvailable = false; 235 | bool lastOdomAvailable = false; 236 | bool laser_imu_sync = false; 237 | bool use_imu_roll_pitch_this_step = false; 238 | bool initialization = false; 239 | bool imuodomAvailable = false; 240 | bool imuorientationAvailable = false; 241 | bool lastimuodomAvaliable=false; 242 | bool imu_initialized = false; 243 | 244 | 245 | pcl::VoxelGrid downSizeFilterCorner; 246 | pcl::VoxelGrid downSizeFilterSurf; 247 | 248 | 249 | std::queue cornerLastBuf; 250 | std::queue surfLastBuf; 251 | std::queue realsenseBuf; 252 | std::queue fullResBuf; 253 | std::queue rawWithFeaturesBuf; 254 | std::queue rawDataBuf; 255 | std::queue odometryBuf; 256 | std::queue IMUPredictionBuf; 257 | std::queue sensorTypeLastBuf; 258 | SensorType last_sensor_type_= SensorType::VELODYNE; 259 | 260 | 261 | // variables for stacking 2 scans 262 | std::queue> cornerDualScanBuf; 263 | std::queue> surfDualScanBuf; 264 | std::queue> fullResDualScanBuf; 265 | std::queue scanTransformBuf; 266 | std::queue sensorTypeBuf; 267 | 268 | pcl::PointCloud::Ptr laserCloudCornerLast; 269 | pcl::PointCloud::Ptr laserCloudSurfLast; 270 | pcl::PointCloud::Ptr laserCloudRealsense; 271 | 272 | pcl::PointCloud::Ptr laserCloudSurround; 273 | pcl::PointCloud::Ptr laserCloudFullRes; 274 | pcl::PointCloud::Ptr laserCloudFullRes_rot; 275 | pcl::PointCloud::Ptr laserCloudRawRes; 276 | pcl::PointCloud::Ptr laserCloudCornerStack; 277 | pcl::PointCloud::Ptr laserCloudSurfStack; 278 | pcl::PointCloud::Ptr laserCloudRawWithFeatures; 279 | pcl::PointCloud::Ptr velodyneLaserCloudRawWithFeatures; 280 | pcl::PointCloud::Ptr ousterLaserCloudRawWithFeatures; 281 | 282 | pcl::PointCloud::Ptr laserCloudPriorOrg; 283 | pcl::PointCloud::Ptr laserCloudPrior; 284 | sensor_msgs::msg::PointCloud2 priorCloudMsg; 285 | 286 | Transformd T_w_lidar; 287 | Transformd last_T_w_lidar; 288 | Transformd last_imu_T; 289 | Transformd total_incremental_T; 290 | Transformd laser_incremental_T; 291 | Transformd forcedInitialGuess; 292 | 293 | Eigen::Quaterniond q_wmap_wodom; 294 | Eigen::Vector3d t_wmap_wodom; 295 | Eigen::Quaterniond q_wodom_curr; 296 | Eigen::Vector3d t_wodom_curr; 297 | Eigen::Quaterniond q_wodom_pre; 298 | Eigen::Vector3d t_wodom_pre; 299 | Eigen::Quaterniond q_w_imu_pre; 300 | Eigen::Vector3d t_w_imu_pre; 301 | 302 | 303 | laser_mapping_config config_; 304 | nav_msgs::msg::Path laserAfterMappedPath; 305 | std::mutex mBuf; 306 | rclcpp::Time timeLatestImuOdometry; 307 | rclcpp::Time timeLastMappingResult; 308 | PointType pointOri, pointSel; 309 | 310 | }; // class laserMapping 311 | 312 | } // namespace super_odometry 313 | #endif //super_odometry_LASERMAPPING_H 314 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/LidarProcess/factor/SE3AbsolutatePoseFactor.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shibo zhao on 2020-09-27. 3 | // 4 | 5 | #ifndef SE3ABSOLUTATEPOSEFACTOR_H 6 | #define SE3ABSOLUTATEPOSEFACTOR_H 7 | 8 | #include 9 | #include 10 | #include "super_odometry/utils/EigenTypes.h" 11 | #include "super_odometry/utils/Twist.h" 12 | #include 13 | 14 | class SE3AbsolutatePoseFactor : public ceres::SizedCostFunction<6, 7> { 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | SE3AbsolutatePoseFactor(const Transformd &T_w_i_meas, 18 | const Eigen::Mat66d &information) 19 | : T_w_i_meas_(T_w_i_meas), information_matrix(information) { 20 | sqrt_information_ = 21 | Eigen::LLT>(information_matrix) 22 | .matrixL() 23 | .transpose(); 24 | } 25 | virtual bool Evaluate(double const *const *parameters, double *residuals, 26 | double **jacobians) const; 27 | public: 28 | const Transformd T_w_i_meas_; 29 | 30 | Eigen::Mat66d information_matrix; 31 | Eigen::Mat66d sqrt_information_; 32 | }; 33 | 34 | #endif // SE3ABSOLUTATEPOSEFACTOR_H 35 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/LidarProcess/factor/lidarOptimization.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shibo zhao on 2020-09-27. 3 | // 4 | 5 | #ifndef _LIDAR_OPTIMIZATION_ANALYTIC_H_ 6 | #define _LIDAR_OPTIMIZATION_ANALYTIC_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t); 14 | 15 | Eigen::Matrix3d skew(const Eigen::Vector3d& mat_in); 16 | 17 | class EdgeAnalyticCostFunction : public ceres::SizedCostFunction<3, 7> { 18 | public: 19 | 20 | EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_); 21 | virtual ~EdgeAnalyticCostFunction() {} 22 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 23 | 24 | Eigen::Vector3d curr_point; 25 | Eigen::Vector3d last_point_a; 26 | Eigen::Vector3d last_point_b; 27 | }; 28 | 29 | class SurfNormAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> { 30 | public: 31 | SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_); 32 | virtual ~SurfNormAnalyticCostFunction() {} 33 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 34 | 35 | Eigen::Vector3d curr_point; 36 | Eigen::Vector3d plane_unit_norm; 37 | double negative_OA_dot_norm; 38 | }; 39 | 40 | class PoseSE3Parameterization : public ceres::LocalParameterization { 41 | public: 42 | 43 | PoseSE3Parameterization() {} 44 | virtual ~PoseSE3Parameterization() {} 45 | virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const; 46 | virtual bool ComputeJacobian(const double* x, double* jacobian) const; 47 | virtual int GlobalSize() const { return 7; } 48 | virtual int LocalSize() const { return 6; } 49 | }; 50 | 51 | 52 | 53 | #endif // _LIDAR_OPTIMIZATION_ANALYTIC_H_ 54 | 55 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/LidarProcess/factor/pose_local_parameterization.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shiboz on 2021-02-06. 3 | // 4 | 5 | #ifndef ARISE_SLAM_MID360_POSE_LOCAL_PARAMETERIZATION_H 6 | #define ARISE_SLAM_MID360_POSE_LOCAL_PARAMETERIZATION_H 7 | 8 | 9 | #include 10 | #include 11 | #include "../../utils/utility.h" 12 | class PoseLocalParameterization : public ceres::LocalParameterization 13 | { 14 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 15 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 16 | virtual int GlobalSize() const { return 7; }; 17 | virtual int LocalSize() const { return 6; }; 18 | }; 19 | 20 | 21 | 22 | 23 | 24 | 25 | #endif //ARISE_SLAM_MID360_POSE_LOCAL_PARAMETERIZATION_H 26 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/config/parameter.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "rclcpp/rclcpp.hpp" 9 | #include "super_odometry/utils/Twist.h" 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | enum class SensorType {VELODYNE, OUSTER, LIVOX}; 42 | extern std::string IMU_TOPIC; 43 | extern std::string LASER_TOPIC; 44 | extern std::string ODOM_TOPIC; 45 | extern std::string DepthUP_TOPIC; 46 | extern std::string DepthDown_TOPIC; 47 | extern std::string ProjectName; 48 | 49 | extern std::string WORLD_FRAME; 50 | extern std::string WORLD_FRAME_ROT; 51 | extern std::string SENSOR_FRAME; 52 | extern std::string SENSOR_FRAME_ROT; 53 | extern SensorType sensor; 54 | 55 | extern int PROVIDE_IMU_LASER_EXTRINSIC; 56 | 57 | extern std::vector RIC; 58 | 59 | extern std::vector TIC; 60 | 61 | extern Eigen::Matrix3d imu_laser_R; 62 | 63 | extern Eigen::Vector3d imu_laser_T; 64 | 65 | extern Eigen::Matrix3d cam_laser_R; 66 | 67 | extern Eigen::Vector3d cam_laser_T; 68 | 69 | extern Eigen::Matrix3d imu_camera_R; 70 | 71 | extern Eigen::Vector3d imu_camera_T; 72 | 73 | extern Eigen::Vector3d imu_laser_offset; 74 | 75 | extern Transformd Tcam_lidar; 76 | 77 | extern Transformd T_i_c; 78 | 79 | extern Transformd T_i_l; 80 | 81 | extern Transformd T_l_i; 82 | 83 | extern float up_realsense_roll; 84 | 85 | extern float up_realsense_pitch; 86 | 87 | extern float up_realsense_yaw; 88 | 89 | extern float up_realsense_x; 90 | 91 | extern float up_realsense_y; 92 | 93 | extern float up_realsense_z; 94 | 95 | extern float down_realsense_roll; 96 | 97 | extern float down_realsense_pitch; 98 | 99 | extern float down_realsense_yaw; 100 | 101 | extern float down_realsense_x; 102 | 103 | extern float down_realsense_y; 104 | 105 | extern float down_realsense_z; 106 | 107 | extern float yaw_ratio; 108 | 109 | extern float IMU_ACC_X_LIMIT; 110 | 111 | extern float IMU_ACC_Y_LIMIT; 112 | 113 | extern float IMU_ACC_Z_LIMIT; 114 | 115 | extern bool USE_IMU_ROLL_PITCH; 116 | 117 | extern std::string SENSOR; 118 | 119 | extern Transformd T_ouster_sensor; 120 | 121 | extern Eigen::Matrix3d ouster_sensor_R; 122 | 123 | extern Eigen::Vector3d ouster_sensor_T; 124 | 125 | bool readGlobalparam(rclcpp::Node::SharedPtr); 126 | 127 | bool readCalibration(rclcpp::Node::SharedPtr); 128 | 129 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/container/MapRingBuffer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ubuntu on 2020/6/29. 3 | // 4 | 5 | #ifndef MAPRINGBUFFER_H 6 | #define MAPRINGBUFFER_H 7 | 8 | #include 9 | #include 10 | 11 | template 12 | class MapRingBuffer { 13 | public: 14 | std::map measMap_; 15 | typename std::map::iterator itMeas_; 16 | 17 | int size; 18 | double maxWaitTime_; 19 | double minWaitTime_; 20 | 21 | MapRingBuffer() { 22 | maxWaitTime_ = 0.1; 23 | minWaitTime_ = 0.0; 24 | } 25 | 26 | virtual ~MapRingBuffer() {} 27 | 28 | bool allocate(const int sizeBuffer) { 29 | if (sizeBuffer <= 0) { 30 | return false; 31 | } else { 32 | size = sizeBuffer; 33 | return true; 34 | } 35 | } 36 | 37 | int getSize() { return measMap_.size(); } 38 | 39 | void addMeas(const Meas& meas, const double& t) { 40 | measMap_.insert(std::make_pair(t, meas)); 41 | 42 | // ensure the size of the map, and remove the last element 43 | if ((int) measMap_.size() > size) { 44 | measMap_.erase(measMap_.begin()); 45 | } 46 | } 47 | 48 | void clear() { measMap_.clear(); } 49 | 50 | void clean(double t) { 51 | while (measMap_.size() >= 1 && measMap_.begin()->first <= t) { 52 | measMap_.erase(measMap_.begin()); 53 | } 54 | } 55 | 56 | bool getNextTime(double actualTime, double& nextTime) { 57 | itMeas_ = measMap_.upper_bound(actualTime); 58 | if (itMeas_ != measMap_.end()) { 59 | nextTime = itMeas_->first; 60 | return true; 61 | } else { 62 | return false; 63 | } 64 | } 65 | void waitTime(double actualTime, double& time) { 66 | double measurementTime = actualTime - maxWaitTime_; 67 | if (!measMap_.empty() && 68 | measMap_.rbegin()->first + minWaitTime_ > measurementTime) { 69 | measurementTime = measMap_.rbegin()->first + minWaitTime_; 70 | } 71 | if (time > measurementTime) { 72 | time = measurementTime; 73 | } 74 | } 75 | bool getLastTime(double& lastTime) { 76 | if (!measMap_.empty()) { 77 | lastTime = measMap_.rbegin()->first; 78 | return true; 79 | } else { 80 | return false; 81 | } 82 | } 83 | 84 | bool getFirstTime(double& firstTime) { 85 | if (!measMap_.empty()) { 86 | firstTime = measMap_.begin()->first; 87 | return true; 88 | } else { 89 | return false; 90 | } 91 | } 92 | 93 | bool getLastMeas(Meas& lastMeas) { 94 | if (!measMap_.empty()) { 95 | lastMeas = measMap_.rbegin()->second; 96 | return true; 97 | } else { 98 | return false; 99 | } 100 | } 101 | 102 | bool getLastLastMeas(Meas& lastlastMeas) { 103 | if (measMap_.size() >= 2) { 104 | auto itr = measMap_.rbegin(); 105 | itr++; 106 | lastlastMeas = itr->second; 107 | return true; 108 | } else { 109 | return false; 110 | } 111 | } 112 | 113 | bool getFirstMeas(Meas& firstMeas) { 114 | if (!measMap_.empty()) { 115 | firstMeas = measMap_.begin()->second; 116 | return true; 117 | } else { 118 | return false; 119 | } 120 | } 121 | 122 | bool hasMeasurementAt(double t) { return measMap_.count(t) > 0; } 123 | 124 | bool empty() { return measMap_.empty(); } 125 | 126 | void printContainer() { 127 | itMeas_ = measMap_.begin(); 128 | while (measMap_.size() >= 1 && itMeas_ != measMap_.end()) { 129 | std::cout << itMeas_->second << " "; 130 | itMeas_++; 131 | } 132 | } 133 | }; 134 | #endif // MAPRINGBUFFER_H 135 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/sensor_data/imu/imu_data.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ubuntu on 2020/6/29. 3 | // 4 | 5 | #ifndef IMU_DATA_H 6 | #define IMU_DATA_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include "super_odometry/container/MapRingBuffer.h" 12 | #include 13 | #include "super_odometry/config/parameter.h" 14 | #include "super_odometry/utils/Twist.h" 15 | 16 | #define Gravity_Norm (9.81) 17 | struct Imu { 18 | 19 | public: 20 | typedef std::shared_ptr Ptr; 21 | typedef std::shared_ptr ConstPtr; 22 | 23 | public: 24 | Imu() : Imu(0.0, Eigen::Vector3d(0.0, 0.0, 0.0), Eigen::Vector3d(0.0, 0.0, 0.0)) {} 25 | Imu(double time, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr) 26 | : time(time), acc(acc), gyr(gyr) 27 | { 28 | initialize(); 29 | } 30 | ~Imu() {} 31 | 32 | void initialize() 33 | { 34 | acc_mean<<0.0,0.0,-1.0; 35 | gyr_mean<<0.0,0.0,0.0; 36 | acc_cov<<0.1,0.1,0.1; 37 | gyr_cov<<0.1,0.1,0.1; 38 | gravity<<0.0,0.0,0.0; 39 | gyr_bias<<0.0,0.0,0.0; 40 | gyr_bias<<0.0,0.0,0.0; 41 | q_w_i.setIdentity(); 42 | } 43 | 44 | // Function to calculate pitch and roll and construct the combined rotation matrix 45 | Eigen::Matrix3d calculatePitchRollMatrix(double ax, double ay, double az) { 46 | // Calculate the pitch angle (theta) 47 | double theta = std::atan2(ax, std::sqrt(ay * ay + az * az)); 48 | 49 | // Calculate the roll angle (phi) 50 | double phi = std::atan2(-ay, az); 51 | 52 | // Construct the pitch rotation matrix 53 | Eigen::Matrix3d R_y; 54 | R_y << std::cos(theta), 0, std::sin(theta), 55 | 0, 1, 0, 56 | -std::sin(theta), 0, std::cos(theta); 57 | 58 | // Construct the roll rotation matrix 59 | Eigen::Matrix3d R_x; 60 | R_x << 1, 0, 0, 61 | 0, std::cos(phi), -std::sin(phi), 62 | 0, std::sin(phi), std::cos(phi); 63 | 64 | // Combine the rotation matrices 65 | Eigen::Matrix3d R = R_x * R_y; 66 | pitch_offset_gravity=theta; 67 | roll_offset_gravity=phi; 68 | return R; 69 | } 70 | 71 | void imuInit(MapRingBuffer imuBuf) { 72 | 73 | int Num = 0; 74 | if (first_imu==false){ 75 | return; 76 | } 77 | 78 | // Initialize if the buffer is not empty 79 | if (!imuBuf.empty()) { 80 | first_imu = false; 81 | const double &time_first = imuBuf.measMap_.begin()->second->time; 82 | const Eigen::Quaterniond rot_first = imuBuf.measMap_.begin()->second->q_w_i; 83 | const Eigen::Vector3d gyr_first = imuBuf.measMap_.begin()->second->gyr; 84 | const Eigen::Vector3d acc_first = imuBuf.measMap_.begin()->second->acc; 85 | acc_mean = acc_first; 86 | gyr_mean = gyr_first; 87 | time = time_first; 88 | 89 | Num = 1; 90 | } 91 | 92 | // Variables for frequency calculation 93 | double time_prev = 0; 94 | double total_time_diff = 0; 95 | int time_diff_count = 0; 96 | 97 | // Iterate through the IMU buffer and update mean and covariance 98 | for (std::map::iterator itMeas_ = imuBuf.measMap_.begin(); itMeas_ != imuBuf.measMap_.end(); ++itMeas_) { 99 | 100 | const double &time_cur = itMeas_->second->time; 101 | const Eigen::Quaterniond rot_cur = itMeas_->second->q_w_i; 102 | const Eigen::Vector3d gyr_cur = itMeas_->second->gyr; 103 | const Eigen::Vector3d acc_cur = itMeas_->second->acc; 104 | 105 | // Calculate time difference for frequency estimation 106 | if (Num > 1) { 107 | double time_diff = time_cur - time_prev; 108 | if (time_diff > 0) { // Ensure valid time difference 109 | total_time_diff += time_diff; 110 | time_diff_count++; 111 | } 112 | } 113 | time_prev = time_cur; 114 | 115 | // Update means 116 | acc_mean += (acc_cur - acc_mean) / Num; 117 | gyr_mean += (gyr_cur - gyr_mean) / Num; 118 | 119 | // Update covariances 120 | acc_cov = acc_cov * (Num - 1.0) / Num + (acc_cur - acc_mean).cwiseProduct(acc_cur - acc_mean) / (Num - 1.0); 121 | gyr_cov = gyr_cov * (Num - 1.0) / Num + (gyr_cur - gyr_mean).cwiseProduct(gyr_cur - gyr_mean) / (Num - 1.0); 122 | Num++; 123 | } 124 | 125 | // if (Num > 1 && time_diff_count > 0) { 126 | // // Get total time span 127 | // double total_time_span = imuBuf.measMap_.rbegin()->second->time - imuBuf.measMap_.begin()->second->time; 128 | // // Calculate actual number of intervals (Num-1) 129 | // imu_frequency = Num / total_time_span; 130 | // } 131 | 132 | //TODO: double check the gravity direction 133 | gravity= - acc_mean / acc_mean.norm() *Gravity_Norm; 134 | gyr_bias = gyr_mean; 135 | acc_bias = acc_mean; 136 | first_imu = false; 137 | 138 | // //Align with Gravity if the IMU is rotated at the beginning. 139 | Roll_Pitch_Gravity_Matrix=calculatePitchRollMatrix(acc_mean.x(), 140 | acc_mean.y(), acc_mean.z()); 141 | 142 | 143 | std::cout<<"imu_laser_R: "< 8 | #include 9 | 10 | struct EIGEN_ALIGN16 _PointXYZTIId { 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | 13 | PCL_ADD_POINT4D 14 | float time; 15 | uint8_t intensity; 16 | uint8_t laserId; 17 | } EIGEN_ALIGN16; 18 | 19 | PCL_EXPORTS std::ostream &operator<<(std::ostream &os, const _PointXYZTIId &p); 20 | 21 | /** \brief A point structure representing Euclidean xyz coordinates, time, 22 | * intensity, and laserId. \ingroup common 23 | */ 24 | struct PointXYZTIId : public _PointXYZTIId { 25 | inline PointXYZTIId(const _PointXYZTIId &p) { 26 | x = p.x; 27 | y = p.y; 28 | z = p.z; 29 | data[3] = 1.0f; 30 | time = p.time; 31 | intensity = p.intensity; 32 | laserId = p.laserId; 33 | } 34 | 35 | inline PointXYZTIId() { 36 | x = 0.0f; 37 | y = 0.0f; 38 | z = 0.0f; 39 | data[3] = 1.0f; 40 | time = 0.0; 41 | intensity = 0; 42 | laserId = 0; 43 | } 44 | 45 | friend std::ostream &operator<<(std::ostream &os, const PointXYZTIId &p); 46 | }; 47 | 48 | struct EIGEN_ALIGN16 PointXYZITR 49 | { 50 | PCL_ADD_POINT4D; 51 | float intensity; 52 | float time; 53 | uint16_t ring; 54 | 55 | static inline PointXYZITR make(float x, float y, float z, float intensity, float t, uint16_t ring) 56 | { 57 | return {x, y, z, 0.f, intensity, t, ring}; 58 | } 59 | }; 60 | // clang-format off 61 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZTIId, 62 | (float, x, x) 63 | (float, y, y) 64 | (float, z, z) 65 | (float, time, time) 66 | (uint8_t, intensity, intensity) 67 | (uint8_t, laserId, laserId)) 68 | 69 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZITR, 70 | (float, x, x) 71 | (float, y, y) 72 | (float, z, z) 73 | (float, intensity, intensity) 74 | (float, time, time) 75 | (uint16_t, ring, ring)) 76 | // clang-format on 77 | 78 | #endif // LIDARPOINT_H 79 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/sensor_data/pointcloud/point_os.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shibo zhao on 2020/6/29. 3 | // 4 | 5 | #ifndef POINT_OS_H 6 | #define POINT_OS_H 7 | 8 | #define PCL_NO_PRECOMPILE 9 | #include 10 | 11 | namespace point_os { 12 | struct EIGEN_ALIGN16 PointOS { 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | PCL_ADD_POINT4D; 15 | float intensity; 16 | float t; 17 | 18 | static inline PointOS make(float x, float y, float z, float intensity, 19 | float t) { 20 | return {x, y, z, 0.0f, intensity, t}; 21 | } 22 | }; 23 | 24 | struct EIGEN_ALIGN16 PointcloudXYZITR { 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | PCL_ADD_POINT4D; 27 | float intensity; 28 | float time; 29 | uint16_t ring; 30 | static inline PointcloudXYZITR make(float x, float y, float z, 31 | float intensity, float t, uint16_t ring) { 32 | return {x, y, z, 0.0f, intensity, t, ring}; 33 | } 34 | }; 35 | 36 | 37 | struct OusterPointXYZIRT { 38 | PCL_ADD_POINT4D; 39 | float intensity; 40 | uint32_t t; 41 | uint16_t reflectivity; 42 | // uint8_t ring; 43 | // uint16_t noise; 44 | uint32_t range; 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | } EIGEN_ALIGN16; 47 | 48 | } // namespace point_os 49 | 50 | // clang-format off 51 | POINT_CLOUD_REGISTER_POINT_STRUCT( point_os::PointOS, 52 | (float, x,x) 53 | (float, y, y) 54 | (float, z, z) 55 | (float, intensity, intensity) 56 | (float, t, t) 57 | ) 58 | 59 | 60 | POINT_CLOUD_REGISTER_POINT_STRUCT( point_os::PointcloudXYZITR, 61 | (float, x,x) 62 | (float, y, y) 63 | (float, z, z) 64 | (float, intensity, intensity) 65 | (float, time, time) 66 | (uint16_t, ring, ring) 67 | ) 68 | 69 | // POINT_CLOUD_REGISTER_POINT_STRUCT(point_os::OusterPointXYZIRT, 70 | // (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) 71 | // (uint32_t, t, t) (uint16_t, reflectivity, reflectivity) 72 | // (uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range) 73 | // ) 74 | 75 | POINT_CLOUD_REGISTER_POINT_STRUCT(point_os::OusterPointXYZIRT, 76 | (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) 77 | (uint32_t, t, t) (uint16_t, reflectivity, reflectivity) 78 | (uint32_t, range, range) 79 | ) 80 | 81 | 82 | // clang-format on 83 | #endif // POINT_OS_H 84 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/tic_toc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | class TicToc 11 | { 12 | public: 13 | TicToc() 14 | { 15 | tic(); 16 | } 17 | 18 | void tic() 19 | { 20 | start = std::chrono::system_clock::now(); 21 | } 22 | 23 | double toc() 24 | { 25 | end = std::chrono::system_clock::now(); 26 | std::chrono::duration elapsed_seconds = end - start; 27 | return elapsed_seconds.count() * 1000; 28 | } 29 | 30 | private: 31 | std::chrono::time_point start, end; 32 | }; 33 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/utils/Twist.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ubuntu on 2020/4/2. 3 | // 4 | 5 | #ifndef TWIST_H 6 | #define TWIST_H 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | //////////////////////////////////////////////////////////////////////////// 17 | // Forward Declarations / typedefs 18 | //////////////////////////////////////////////////////////////////////////// 19 | 20 | typedef pcl::PointXYZI PointType; 21 | template struct Twist; 22 | 23 | typedef Twist Transformf; 24 | 25 | typedef Twist Transformd; 26 | 27 | namespace TwistConstants { 28 | template struct Constants { 29 | EIGEN_ALWAYS_INLINE static const Scalar epsilon() { 30 | return static_cast(1e-10); 31 | } 32 | 33 | EIGEN_ALWAYS_INLINE static const Scalar pi() { 34 | return static_cast(M_PI); 35 | } 36 | }; 37 | 38 | template <> struct Constants { 39 | EIGEN_ALWAYS_INLINE static float epsilon() { 40 | return static_cast(1e-5); 41 | } 42 | 43 | EIGEN_ALWAYS_INLINE static float pi() { return static_cast(M_PI); } 44 | }; 45 | } // namespace TwistConstants 46 | 47 | template struct Twist { 48 | public: 49 | typedef Eigen::Matrix Transformation; 50 | typedef Eigen::Matrix Matrix3x3; 51 | typedef Eigen::Matrix Adjoint; 52 | typedef Eigen::Matrix Tangent; 53 | typedef Eigen::Matrix Tangent3; 54 | typedef Eigen::Matrix HomogeneousPoint; 55 | 56 | public: 57 | public: 58 | Eigen::Quaternion rot; 59 | Eigen::Matrix pos; 60 | 61 | public: 62 | static Twist Identity() { return Twist(); } 63 | 64 | Twist() : rot(T(1), T(0), T(0), T(0)), pos(Tangent3::Zero()) {} 65 | 66 | Twist(const Eigen::Quaternion &rot_in, 67 | const Eigen::Matrix &pos_in) 68 | : rot(rot_in), pos(pos_in) {} 69 | 70 | Twist(const Transformation &matrix) 71 | : rot(matrix.template topLeftCorner<3, 3>()), 72 | pos(matrix.template block<3, 1>(0, 3)) {} 73 | 74 | Twist(const Matrix3x3 &R, const Tangent3 &t) : rot(R), pos(t) {} 75 | 76 | Twist( 77 | const Eigen::Transform &transform) { 78 | this->rot = Eigen::Quaternion{transform.linear()}.normalized(); 79 | this->pos = transform.translation(); 80 | } 81 | 82 | Eigen::Transform transform() const { 83 | Eigen::Transform transform; 84 | transform.linear() = rot.normalized().toRotationMatrix(); 85 | transform.translation() = pos; 86 | return transform; 87 | } 88 | 89 | inline Transformation matrix() const { 90 | Transformation homogenious_matrix; 91 | homogenious_matrix.setIdentity(); 92 | homogenious_matrix.block(0, 0, 3, 3) = 93 | this->rot.normalized().toRotationMatrix(); 94 | homogenious_matrix.col(3).head(3) = this->pos; 95 | 96 | return homogenious_matrix; 97 | } 98 | 99 | inline Eigen::Matrix matrix3x4() const { 100 | Eigen::Matrix matrix; 101 | matrix.block(0, 0, 3, 3) = this->rot.normalized().toRotationMatrix(); 102 | matrix.col(3) = this->pos; 103 | 104 | return matrix; 105 | } 106 | 107 | inline Matrix3x3 rotationMatrix() const { 108 | Matrix3x3 matrix; 109 | matrix = this->rot.normalized().toRotationMatrix(); 110 | 111 | return matrix; 112 | } 113 | 114 | inline T *data() { return rot.coeffs().data(); } 115 | 116 | // Const version of data() above. 117 | T const *data() const { return rot.coeffs().data(); } 118 | 119 | inline Adjoint SE3Adj() const { 120 | const Eigen::Matrix R = this->rot.normalized().toRotationMatrix(); 121 | Adjoint res; 122 | res.template block<3, 3>(0, 0) = R; 123 | res.template block<3, 3>(3, 3) = R; 124 | res.template block<3, 3>(0, 3) = hat(this->pos) * R; 125 | res.template block<3, 3>(3, 0) = Eigen::Matrix::Zero(3, 3); 126 | 127 | return res; 128 | } 129 | 130 | inline Adjoint SO3TransAdj() const { 131 | const Eigen::Matrix R = this->rot.normalized().toRotationMatrix(); 132 | Adjoint res; 133 | res.template block<3, 3>(0, 0) = R; 134 | res.template block<3, 3>(3, 3) = R; 135 | res.template block<3, 3>(0, 3) = hat(this->pos) * R + R * hat(this->pos); 136 | res.template block<3, 3>(3, 0) = Eigen::Matrix::Zero(3, 3); 137 | 138 | return res; 139 | } 140 | 141 | inline static Twist se3exp(const Tangent &a) { 142 | const Eigen::Matrix &omega = a.template tail<3>(); 143 | 144 | T theta; 145 | const Eigen::Quaternion &so3 = expAndTheta(omega, &theta); 146 | const Eigen::Matrix &Omega = hat(omega); 147 | const Eigen::Matrix &Omega_sq = Omega * Omega; 148 | Eigen::Matrix V; 149 | 150 | if (theta < TwistConstants::Constants::epsilon()) { 151 | // Note: That is an accurate expansion! 152 | V = so3.matrix(); 153 | } else { 154 | T theta_sq = theta * theta; 155 | V = (Eigen::Matrix::Identity() + 156 | (static_cast(1) - std::cos(theta)) / (theta_sq)*Omega + 157 | (theta - std::sin(theta)) / (theta_sq * theta) * Omega_sq); 158 | } 159 | 160 | return Twist(so3, V * a.template head<3>()); 161 | } 162 | 163 | inline static Twist so3Transexp(const Tangent &a) { 164 | const Eigen::Matrix &t = a.template head<3>(); 165 | const Eigen::Matrix &omega = a.template tail<3>(); 166 | T theta; 167 | const Eigen::Quaternion &so3 = expAndTheta(omega, &theta); 168 | 169 | return Twist(so3, t); 170 | } 171 | 172 | Twist inverse() const { 173 | // Eigen::Transform 174 | // transform_inv = this->transform().inverse(); 175 | Twist twist_inv; 176 | twist_inv.rot = this->rot.conjugate(); 177 | twist_inv.pos = -twist_inv.rot.toRotationMatrix() * this->pos; 178 | return twist_inv; 179 | } 180 | 181 | Twist operator*(const Twist &other) const { 182 | Eigen::Transform transform_out = 183 | this->transform() * other.transform(); 184 | return Twist(transform_out); 185 | } 186 | 187 | Tangent3 operator*(const Tangent3 &p) const { return rot * p + pos; } 188 | 189 | HomogeneousPoint operator*(const HomogeneousPoint &p) const { 190 | const Tangent3 tp = rot * p.template head<3>() + p(3) * pos; 191 | return HomogeneousPoint(tp(0), tp(1), tp(2), p(3)); 192 | } 193 | 194 | template Twist cast() const { 195 | Twist twist_new{this->rot.template cast(), 196 | this->pos.template cast()}; 197 | return twist_new; 198 | } 199 | 200 | friend std::ostream &operator<<(std::ostream &os, const Twist &twist) { 201 | os << twist.pos.x() << " " << twist.pos.y() << " " << twist.pos.z() << " " 202 | << twist.rot.w() << " " << twist.rot.x() << " " << twist.rot.y() << " " 203 | << twist.rot.z(); 204 | return os; 205 | } 206 | 207 | inline static Eigen::Quaternion expAndTheta(const Tangent3 &omega, 208 | T *theta) { 209 | const T theta_sq = omega.squaredNorm(); 210 | *theta = std::sqrt(theta_sq); 211 | const T half_theta = static_cast(0.5) * (*theta); 212 | 213 | T imag_factor; 214 | T real_factor; 215 | ; 216 | if ((*theta) < TwistConstants::Constants::epsilon()) { 217 | const T theta_po4 = theta_sq * theta_sq; 218 | imag_factor = static_cast(0.5) - 219 | static_cast(1.0 / 48.0) * theta_sq + 220 | static_cast(1.0 / 3840.0) * theta_po4; 221 | real_factor = static_cast(1) - static_cast(0.5) * theta_sq + 222 | static_cast(1.0 / 384.0) * theta_po4; 223 | } else { 224 | const T sin_half_theta = std::sin(half_theta); 225 | imag_factor = sin_half_theta / (*theta); 226 | real_factor = std::cos(half_theta); 227 | } 228 | 229 | return Eigen::Quaternion(real_factor, imag_factor * omega.x(), 230 | imag_factor * omega.y(), 231 | imag_factor * omega.z()); 232 | } 233 | 234 | private: 235 | inline static const Matrix3x3 hat(const Tangent3 &omega) { 236 | Matrix3x3 Omega; 237 | Omega << static_cast(0), -omega(2), omega(1), omega(2), 238 | static_cast(0), -omega(0), -omega(1), omega(0), static_cast(0); 239 | return Omega; 240 | } 241 | 242 | public: 243 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 244 | }; // class Twist 245 | 246 | #endif // TWIST_H 247 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/utils/sophus_utils.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ubuntu on 2020/5/23. 3 | // 4 | 5 | #ifndef SOPHUS_UTILS_HPP 6 | #define SOPHUS_UTILS_HPP 7 | 8 | #include "sophus/se3.hpp" 9 | namespace Sophus { 10 | 11 | template 12 | inline static typename SE3::Tangent logd(const SE3 &se3) { 13 | typename SE3::Tangent upsilon_omega; 14 | upsilon_omega.template tail<3>() = se3.so3().log(); 15 | upsilon_omega.template head<3>() = se3.translation(); 16 | 17 | return upsilon_omega; 18 | } 19 | 20 | template 21 | inline static SE3 expd( 22 | const Eigen::MatrixBase &upsilon_omega) { 23 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); 24 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 6); 25 | 26 | using Scalar = typename Derived::Scalar; 27 | 28 | return SE3(SO3::exp(upsilon_omega.template tail<3>()), 29 | upsilon_omega.template head<3>()); 30 | } 31 | 32 | // exp(phi+e) ~= exp(phi)*exp(J*e) 33 | template 34 | void rightJacobianSO3(const Eigen::MatrixBase &phi, 35 | const Eigen::MatrixBase &J_const) { 36 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1); 37 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2); 38 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 3); 39 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 3, 3); 40 | 41 | using Scalar = typename Derived1::Scalar; 42 | 43 | Eigen::MatrixBase &J = 44 | const_cast &>(J_const); 45 | 46 | Scalar phi_norm2 = phi.squaredNorm(); 47 | Scalar phi_norm = std::sqrt(phi_norm2); 48 | Scalar phi_norm3 = phi_norm2 * phi_norm; 49 | 50 | J.setIdentity(); 51 | 52 | if (Sophus::Constants::epsilon() < phi_norm) { 53 | Eigen::Matrix phi_hat = Sophus::SO3::hat(phi); 54 | Eigen::Matrix phi_hat2 = phi_hat * phi_hat; 55 | 56 | J -= phi_hat * (1 - std::cos(phi_norm)) / phi_norm2; 57 | J += phi_hat2 * (phi_norm - std::sin(phi_norm)) / phi_norm3; 58 | } 59 | } 60 | 61 | // log(exp(phi)exp(e)) ~= phi + J*e 62 | template 63 | void rightJacobianInvSO3(const Eigen::MatrixBase &phi, 64 | const Eigen::MatrixBase &J_const) { 65 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1); 66 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2); 67 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 3); 68 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 3, 3); 69 | 70 | using Scalar = typename Derived1::Scalar; 71 | 72 | Eigen::MatrixBase &J = 73 | const_cast &>(J_const); 74 | 75 | Scalar phi_norm2 = phi.squaredNorm(); 76 | Scalar phi_norm = std::sqrt(phi_norm2); 77 | 78 | J.setIdentity(); 79 | 80 | if (Sophus::Constants::epsilon() < phi_norm) { 81 | Eigen::Matrix phi_hat = Sophus::SO3::hat(phi); 82 | Eigen::Matrix phi_hat2 = phi_hat * phi_hat; 83 | 84 | J += phi_hat / 2; 85 | J += phi_hat2 * (1 / phi_norm2 - (1 + std::cos(phi_norm)) / 86 | (2 * phi_norm * std::sin(phi_norm))); 87 | } 88 | } 89 | 90 | // exp(phi+e) ~= exp(J*e)*exp(phi) 91 | template 92 | void leftJacobianSO3(const Eigen::MatrixBase &phi, 93 | const Eigen::MatrixBase &J_const) { 94 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1); 95 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2); 96 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 3); 97 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 3, 3); 98 | 99 | using Scalar = typename Derived1::Scalar; 100 | 101 | Eigen::MatrixBase &J = 102 | const_cast &>(J_const); 103 | 104 | Scalar phi_norm2 = phi.squaredNorm(); 105 | Scalar phi_norm = std::sqrt(phi_norm2); 106 | Scalar phi_norm3 = phi_norm2 * phi_norm; 107 | 108 | J.setIdentity(); 109 | 110 | if (Sophus::Constants::epsilon() < phi_norm) { 111 | Eigen::Matrix phi_hat = Sophus::SO3::hat(phi); 112 | Eigen::Matrix phi_hat2 = phi_hat * phi_hat; 113 | 114 | J += phi_hat * (1 - std::cos(phi_norm)) / phi_norm2; 115 | J += phi_hat2 * (phi_norm - std::sin(phi_norm)) / phi_norm3; 116 | } 117 | } 118 | 119 | // log(exp(e)*exp(phi)) ~= phi + J*e 120 | template 121 | void leftJacobianInvSO3(const Eigen::MatrixBase &phi, 122 | const Eigen::MatrixBase &J_const) { 123 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1); 124 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2); 125 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 3); 126 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 3, 3); 127 | 128 | using Scalar = typename Derived1::Scalar; 129 | 130 | Eigen::MatrixBase &J = 131 | const_cast &>(J_const); 132 | 133 | Scalar phi_norm2 = phi.squaredNorm(); 134 | Scalar phi_norm = std::sqrt(phi_norm2); 135 | 136 | J.setIdentity(); 137 | 138 | if (Sophus::Constants::epsilon() < phi_norm) { 139 | Eigen::Matrix phi_hat = Sophus::SO3::hat(phi); 140 | Eigen::Matrix phi_hat2 = phi_hat * phi_hat; 141 | 142 | J -= phi_hat / 2; 143 | J += phi_hat2 * (1 / phi_norm2 - (1 + std::cos(phi_norm)) / 144 | (2 * phi_norm * std::sin(phi_norm))); 145 | } 146 | } 147 | 148 | // expd(phi+e) ~= expd(phi)*expd(J*e) 149 | template 150 | void rightJacobianSE3Decoupled(const Eigen::MatrixBase &phi, 151 | const Eigen::MatrixBase &J_const) { 152 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1); 153 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2); 154 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 6); 155 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 6, 6); 156 | 157 | using Scalar = typename Derived1::Scalar; 158 | 159 | Eigen::MatrixBase &J = 160 | const_cast &>(J_const); 161 | 162 | J.setZero(); 163 | 164 | Eigen::Matrix omega = phi.template tail<3>(); 165 | rightJacobianSO3(omega, J.template bottomRightCorner<3, 3>()); 166 | J.template topLeftCorner<3, 3>() = 167 | Sophus::SO3::exp(omega).inverse().matrix(); 168 | } 169 | 170 | // logd(expd(phi)expd(e)) ~= phi + J*e 171 | template 172 | void rightJacobianInvSE3Decoupled(const Eigen::MatrixBase &phi, 173 | const Eigen::MatrixBase &J_const) { 174 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived1); 175 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived2); 176 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived1, 6); 177 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 6, 6); 178 | 179 | using Scalar = typename Derived1::Scalar; 180 | 181 | Eigen::MatrixBase &J = 182 | const_cast &>(J_const); 183 | 184 | J.setZero(); 185 | 186 | Eigen::Matrix omega = phi.template tail<3>(); 187 | rightJacobianInvSO3(omega, J.template bottomRightCorner<3, 3>()); 188 | J.template topLeftCorner<3, 3>() = Sophus::SO3::exp(omega).matrix(); 189 | } 190 | 191 | } // namespace Sophus 192 | 193 | #endif //SOPHUS_UTILS_HPP -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/utils/superodom_utils.h: -------------------------------------------------------------------------------- 1 | // Created by Shibo Zhao on 2025-03-31 2 | 3 | # pragma once 4 | #ifndef SUPER_ODOMETRY_LASER_MAPPING_UTILS_H 5 | #define SUPER_ODOMETRY_LASER_MAPPING_UTILS_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "super_odometry/utils/Twist.h" 17 | #include 18 | #include 19 | #include "super_odometry/sensor_data/pointcloud/point_os.h" 20 | 21 | 22 | namespace super_odometry { 23 | namespace utils { 24 | 25 | 26 | class ScopedTimer { 27 | public: 28 | explicit ScopedTimer(const std::string& name, rclcpp::Logger logger = rclcpp::get_logger("ScopedTimer")) 29 | : name_(name), 30 | start_(std::chrono::high_resolution_clock::now()), 31 | logger_(logger) {} 32 | 33 | ~ScopedTimer() { 34 | auto end = std::chrono::high_resolution_clock::now(); 35 | auto duration = std::chrono::duration_cast(end - start_).count(); 36 | RCLCPP_DEBUG(logger_, "%s took %ld ms", name_.c_str(), duration); 37 | } 38 | 39 | private: 40 | std::string name_; 41 | std::chrono::time_point start_; 42 | rclcpp::Logger logger_; 43 | }; 44 | 45 | 46 | struct OdometryData { 47 | double timestamp; 48 | double duration; 49 | double x, y, z; 50 | double roll, pitch, yaw; 51 | }; 52 | 53 | extern std::vector odometryResults; 54 | 55 | void transformAssociateToMap(Transformd& T_w_curr, 56 | const Transformd& T_w_pre, 57 | const Transformd& T_wodom_curr, 58 | const Transformd& T_wodom_pre); 59 | 60 | inline void transformUpdate(const Eigen::Quaterniond &q_w_curr, const Eigen::Vector3d &t_w_curr, 61 | const Eigen::Quaterniond &q_wodom_curr, const Eigen::Vector3d &t_wodom_curr, 62 | Eigen::Quaterniond &q_wmap_wodom, Eigen::Vector3d &t_wmap_wodom) { 63 | q_wmap_wodom = q_w_curr * q_wodom_curr.inverse(); 64 | t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr; 65 | } 66 | 67 | 68 | bool readPointCloud(const std::string &file_path, pcl::PointCloud::Ptr cloud_out); 69 | 70 | 71 | bool readLocalizationPose(const std::string &file_path, std::vector &odometry_results); 72 | 73 | 74 | bool saveLocalizationPose(double timestamp, const Transformd &T_w_lidar, 75 | const std::string &file_path, std::vector &odometry_results); 76 | 77 | // Transform utilities 78 | void transformAssociateToMap(Transformd& T_w_curr, 79 | const Transformd& T_w_pre, 80 | const Transformd& T_wodom_curr, 81 | const Transformd& T_wodom_pre); 82 | 83 | void transformAssociateToMap(Eigen::Quaterniond& q_w_curr, 84 | Eigen::Vector3d& t_w_curr, 85 | const Eigen::Quaterniond& q_wmap_wodom, 86 | const Eigen::Quaterniond& q_wodom_curr, 87 | const Eigen::Vector3d& t_wodom_curr, 88 | const Eigen::Vector3d& t_wmap_wodom); 89 | 90 | void transformUpdate(Eigen::Quaterniond& q_wmap_wodom, 91 | Eigen::Vector3d& t_wmap_wodom, 92 | const Eigen::Quaterniond& q_w_curr, 93 | const Eigen::Quaterniond& q_wodom_curr, 94 | const Eigen::Vector3d& t_w_curr, 95 | const Eigen::Vector3d& t_wodom_curr); 96 | 97 | void pointAssociateToMap(PointType const *const pi, PointType *const po, 98 | const Eigen::Quaterniond& q_w_curr, 99 | const Eigen::Vector3d& t_w_curr); 100 | 101 | void pointAssociateToMap(pcl::PointXYZHSV const *const pi, pcl::PointXYZHSV *const po, 102 | const Eigen::Quaterniond& q_w_curr, 103 | const Eigen::Vector3d& t_w_curr); 104 | 105 | void pointAssociateTobeMapped(PointType const *const pi, PointType *const po, 106 | const Eigen::Quaterniond& q_w_curr, 107 | const Eigen::Vector3d& t_w_curr); 108 | 109 | tf2::Quaternion extractRollPitch(Eigen::Quaterniond& imu_rotation); 110 | 111 | void printTransform(const Transformd& T, const std::string& name); 112 | 113 | void transformOusterPoints(point_os::OusterPointXYZIRT const *const pi, point_os::PointcloudXYZITR *const po, Transformd &transform); 114 | 115 | 116 | template 117 | inline constexpr T Deg2Rad(const T °) { return deg / 180. * M_PI; } 118 | 119 | template 120 | inline void TransformPoint(PointT &p, const Transformd &transform) { 121 | Eigen::Vector3d temp = p.getVector3fMap().template cast(); 122 | p.getVector3fMap() = (transform * temp).template cast(); 123 | } 124 | 125 | template 126 | inline PointT TransformPointd(const PointT &p, const Transformd &transform) { 127 | PointT out(p); 128 | TransformPoint(out, transform); 129 | return out; 130 | } 131 | 132 | inline bool compare_pair_first(const std::pair a, const std::pair b) // sort from big to small 133 | { 134 | return a.first > b.first; 135 | } 136 | 137 | /*! 138 | * @brief Compute PCA of Nx3 data array and mean value 139 | * @param[in] data Nx3 array (e.g. stacked 3D points) 140 | * @param[out] mean Where to store mean value 141 | * @return The PCA 142 | */ 143 | inline Eigen::SelfAdjointEigenSolver 144 | ComputePCA(const Eigen::Matrix &data, 145 | Eigen::Vector3d &mean) { 146 | mean = data.colwise().mean(); 147 | Eigen::MatrixXd centered = data.rowwise() - mean.transpose(); 148 | Eigen::Matrix3d varianceCovariance = centered.transpose() * centered; 149 | 150 | return Eigen::SelfAdjointEigenSolver(varianceCovariance); 151 | } 152 | 153 | //------------------------------------------------------------------------------ 154 | /*! 155 | * @brief Compute PCA of Nx3 data array and mean value 156 | * @param data Nx3 array (e.g. stacked 3D points) 157 | * @return The PCA 158 | */ 159 | inline Eigen::SelfAdjointEigenSolver 160 | ComputePCA(const Eigen::Matrix &data) { 161 | Eigen::Vector3d mean; 162 | return ComputePCA(data, mean); 163 | } 164 | 165 | template 166 | inline constexpr T Rad2Deg(const T &rad) { return rad / M_PI * 180.; } 167 | 168 | } // namespace utils 169 | } // namespace super_odometry 170 | 171 | #endif // SUPER_ODOMETRY_LASER_MAPPING_UTILS_H 172 | -------------------------------------------------------------------------------- /super_odometry/include/super_odometry/utils/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class Utility 9 | { 10 | public: 11 | template 12 | static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) 13 | { 14 | typedef typename Derived::Scalar Scalar_t; 15 | 16 | Eigen::Quaternion dq; 17 | Eigen::Matrix half_theta = theta; 18 | half_theta /= static_cast(2.0); 19 | dq.w() = static_cast(1.0); 20 | dq.x() = half_theta.x(); 21 | dq.y() = half_theta.y(); 22 | dq.z() = half_theta.z(); 23 | return dq; 24 | } 25 | 26 | template 27 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 28 | { 29 | Eigen::Matrix ans; 30 | ans << typename Derived::Scalar(0), -q(2), q(1), 31 | q(2), typename Derived::Scalar(0), -q(0), 32 | -q(1), q(0), typename Derived::Scalar(0); 33 | return ans; 34 | } 35 | 36 | template 37 | static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) 38 | { 39 | //printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); 40 | //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); 41 | //printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); 42 | //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); 43 | return q; 44 | } 45 | 46 | template 47 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) 48 | { 49 | Eigen::Quaternion qq = positify(q); 50 | Eigen::Matrix ans; 51 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 52 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); 53 | return ans; 54 | } 55 | 56 | template 57 | static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) 58 | { 59 | Eigen::Quaternion pp = positify(p); 60 | Eigen::Matrix ans; 61 | ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); 62 | ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); 63 | return ans; 64 | } 65 | 66 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 67 | { 68 | Eigen::Vector3d n = R.col(0); 69 | Eigen::Vector3d o = R.col(1); 70 | Eigen::Vector3d a = R.col(2); 71 | 72 | Eigen::Vector3d ypr(3); 73 | double y = atan2(n(1), n(0)); 74 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 75 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 76 | ypr(0) = y; 77 | ypr(1) = p; 78 | ypr(2) = r; 79 | 80 | return ypr / M_PI * 180.0; 81 | } 82 | 83 | template 84 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 85 | { 86 | typedef typename Derived::Scalar Scalar_t; 87 | 88 | Scalar_t y = ypr(0) / 180.0 * M_PI; 89 | Scalar_t p = ypr(1) / 180.0 * M_PI; 90 | Scalar_t r = ypr(2) / 180.0 * M_PI; 91 | 92 | Eigen::Matrix Rz; 93 | Rz << cos(y), -sin(y), 0, 94 | sin(y), cos(y), 0, 95 | 0, 0, 1; 96 | 97 | Eigen::Matrix Ry; 98 | Ry << cos(p), 0., sin(p), 99 | 0., 1., 0., 100 | -sin(p), 0., cos(p); 101 | 102 | Eigen::Matrix Rx; 103 | Rx << 1., 0., 0., 104 | 0., cos(r), -sin(r), 105 | 0., sin(r), cos(r); 106 | 107 | return Rz * Ry * Rx; 108 | } 109 | 110 | template 111 | struct uint_ 112 | { 113 | }; 114 | 115 | template 116 | void unroller(const Lambda &f, const IterT &iter, uint_) 117 | { 118 | unroller(f, iter, uint_()); 119 | f(iter + N); 120 | } 121 | 122 | template 123 | void unroller(const Lambda &f, const IterT &iter, uint_<0>) 124 | { 125 | f(iter); 126 | } 127 | 128 | template 129 | static T normalizeAngle(const T& angle_degrees) { 130 | T two_pi(2.0 * 180); 131 | if (angle_degrees > 0) 132 | return angle_degrees - 133 | two_pi * std::floor((angle_degrees + T(180)) / two_pi); 134 | else 135 | return angle_degrees + 136 | two_pi * std::floor((-angle_degrees + T(180)) / two_pi); 137 | }; 138 | }; 139 | -------------------------------------------------------------------------------- /super_odometry/launch/livox_mid360.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.substitutions import LaunchConfiguration, PythonExpression 7 | from launch_ros.actions import Node 8 | import launch_ros 9 | 10 | def get_share_file(package_name, file_name): 11 | return os.path.join(get_package_share_directory(package_name), file_name) 12 | 13 | def generate_launch_description(): 14 | config_path = get_share_file( 15 | package_name="super_odometry", 16 | file_name="config/livox_mid360.yaml") 17 | calib_path = get_share_file( 18 | package_name="super_odometry", 19 | file_name="config/livox/livox_mid360_calibration.yaml" 20 | ) 21 | home_directory = os.path.expanduser("~") 22 | 23 | config_path_arg = DeclareLaunchArgument( 24 | "config_file", 25 | default_value=config_path, 26 | description="Path to config file for super_odometry" 27 | ) 28 | calib_path_arg = DeclareLaunchArgument( 29 | "calibration_file", 30 | default_value=calib_path, 31 | ) 32 | odom_topic_arg = DeclareLaunchArgument( 33 | "odom_topic", 34 | default_value="integrated_to_init" 35 | ) 36 | world_frame_arg = DeclareLaunchArgument( 37 | "world_frame", 38 | default_value="map", 39 | ) 40 | world_frame_rot_arg = DeclareLaunchArgument( 41 | "world_frame_rot", 42 | default_value="map_rot", 43 | ) 44 | sensor_frame_arg = DeclareLaunchArgument( 45 | "sensor_frame", 46 | default_value="sensor", 47 | ) 48 | sensor_frame_rot_arg = DeclareLaunchArgument( 49 | "sensor_frame_rot", 50 | default_value="sensor_rot", 51 | ) 52 | 53 | feature_extraction_node = Node( 54 | package="super_odometry", 55 | executable="feature_extraction_node", 56 | output={ 57 | "stdout": "screen", 58 | "stderr": "screen", 59 | }, 60 | parameters=[LaunchConfiguration("config_file"), 61 | { "calibration_file": LaunchConfiguration("calibration_file"), 62 | }], 63 | ) 64 | 65 | laser_mapping_node = Node( 66 | package="super_odometry", 67 | executable="laser_mapping_node", 68 | output={ 69 | "stdout": "screen", 70 | "stderr": "screen", 71 | }, 72 | parameters=[LaunchConfiguration("config_file"), 73 | { "calibration_file": LaunchConfiguration("calibration_file"), 74 | "map_dir": os.path.join(home_directory, "/path/to/your/pcd"), 75 | }], 76 | remappings=[ 77 | ("laser_odom_to_init", LaunchConfiguration("odom_topic")), 78 | ] 79 | ) 80 | 81 | imu_preintegration_node = Node( 82 | package="super_odometry", 83 | executable="imu_preintegration_node", 84 | output={ 85 | "stdout": "screen", 86 | "stderr": "screen", 87 | }, 88 | parameters=[LaunchConfiguration("config_file"), 89 | { "calibration_file": LaunchConfiguration("calibration_file") 90 | }], 91 | ) 92 | 93 | 94 | return LaunchDescription([ 95 | launch_ros.actions.SetParameter(name='use_sim_time', value='false'), 96 | config_path_arg, 97 | calib_path_arg, 98 | odom_topic_arg, 99 | world_frame_arg, 100 | world_frame_rot_arg, 101 | sensor_frame_arg, 102 | sensor_frame_rot_arg, 103 | feature_extraction_node, 104 | laser_mapping_node, 105 | imu_preintegration_node, 106 | ]) 107 | -------------------------------------------------------------------------------- /super_odometry/launch/os1_128.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.substitutions import LaunchConfiguration, PythonExpression 7 | from launch_ros.actions import Node 8 | import launch_ros 9 | 10 | def get_share_file(package_name, file_name): 11 | return os.path.join(get_package_share_directory(package_name), file_name) 12 | 13 | def generate_launch_description(): 14 | config_path = get_share_file( 15 | package_name="super_odometry", 16 | file_name="config/os1_128.yaml") 17 | calib_path = get_share_file( 18 | package_name="super_odometry", 19 | file_name="config/ouster/os1_128_calibration.yaml" 20 | ) 21 | home_directory = os.path.expanduser("~") 22 | 23 | config_path_arg = DeclareLaunchArgument( 24 | "config_file", 25 | default_value=config_path, 26 | description="Path to config file for super_odometry" 27 | ) 28 | calib_path_arg = DeclareLaunchArgument( 29 | "calibration_file", 30 | default_value=calib_path, 31 | ) 32 | odom_topic_arg = DeclareLaunchArgument( 33 | "odom_topic", 34 | default_value="integrated_to_init" 35 | ) 36 | world_frame_arg = DeclareLaunchArgument( 37 | "world_frame", 38 | default_value="map", 39 | ) 40 | world_frame_rot_arg = DeclareLaunchArgument( 41 | "world_frame_rot", 42 | default_value="map_rot", 43 | ) 44 | sensor_frame_arg = DeclareLaunchArgument( 45 | "sensor_frame", 46 | default_value="sensor", 47 | ) 48 | sensor_frame_rot_arg = DeclareLaunchArgument( 49 | "sensor_frame_rot", 50 | default_value="sensor_rot", 51 | ) 52 | 53 | feature_extraction_node = Node( 54 | package="super_odometry", 55 | executable="feature_extraction_node", 56 | output={ 57 | "stdout": "screen", 58 | "stderr": "screen", 59 | }, 60 | parameters=[LaunchConfiguration("config_file"), 61 | { "calibration_file": LaunchConfiguration("calibration_file"), 62 | }], 63 | ) 64 | 65 | laser_mapping_node = Node( 66 | package="super_odometry", 67 | executable="laser_mapping_node", 68 | output={ 69 | "stdout": "screen", 70 | "stderr": "screen", 71 | }, 72 | parameters=[LaunchConfiguration("config_file"), 73 | { "calibration_file": LaunchConfiguration("calibration_file"), 74 | "map_dir": os.path.join(home_directory, "/path/to/your/pcd"), 75 | }], 76 | remappings=[ 77 | ("laser_odom_to_init", LaunchConfiguration("odom_topic")), 78 | ] 79 | ) 80 | 81 | imu_preintegration_node = Node( 82 | package="super_odometry", 83 | executable="imu_preintegration_node", 84 | output={ 85 | "stdout": "screen", 86 | "stderr": "screen", 87 | }, 88 | parameters=[LaunchConfiguration("config_file"), 89 | { "calibration_file": LaunchConfiguration("calibration_file") 90 | }], 91 | ) 92 | 93 | 94 | return LaunchDescription([ 95 | launch_ros.actions.SetParameter(name='use_sim_time', value='false'), 96 | config_path_arg, 97 | calib_path_arg, 98 | odom_topic_arg, 99 | world_frame_arg, 100 | world_frame_rot_arg, 101 | sensor_frame_arg, 102 | sensor_frame_rot_arg, 103 | feature_extraction_node, 104 | laser_mapping_node, 105 | imu_preintegration_node, 106 | ]) 107 | -------------------------------------------------------------------------------- /super_odometry/launch/vlp_16.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.substitutions import LaunchConfiguration, PythonExpression 7 | from launch_ros.actions import Node 8 | import launch_ros 9 | 10 | def get_share_file(package_name, file_name): 11 | return os.path.join(get_package_share_directory(package_name), file_name) 12 | 13 | def generate_launch_description(): 14 | config_path = get_share_file( 15 | package_name="super_odometry", 16 | file_name="config/vlp_16.yaml") 17 | calib_path = get_share_file( 18 | package_name="super_odometry", 19 | file_name="config/velodyne/vlp_16_calibration.yaml" 20 | ) 21 | home_directory = os.path.expanduser("~") 22 | 23 | config_path_arg = DeclareLaunchArgument( 24 | "config_file", 25 | default_value=config_path, 26 | description="Path to config file for super_odometry" 27 | ) 28 | calib_path_arg = DeclareLaunchArgument( 29 | "calibration_file", 30 | default_value=calib_path, 31 | ) 32 | odom_topic_arg = DeclareLaunchArgument( 33 | "odom_topic", 34 | default_value="integrated_to_init" 35 | ) 36 | world_frame_arg = DeclareLaunchArgument( 37 | "world_frame", 38 | default_value="map", 39 | ) 40 | world_frame_rot_arg = DeclareLaunchArgument( 41 | "world_frame_rot", 42 | default_value="map_rot", 43 | ) 44 | sensor_frame_arg = DeclareLaunchArgument( 45 | "sensor_frame", 46 | default_value="sensor", 47 | ) 48 | sensor_frame_rot_arg = DeclareLaunchArgument( 49 | "sensor_frame_rot", 50 | default_value="sensor_rot", 51 | ) 52 | 53 | feature_extraction_node = Node( 54 | package="super_odometry", 55 | executable="feature_extraction_node", 56 | output={ 57 | "stdout": "screen", 58 | "stderr": "screen", 59 | }, 60 | parameters=[LaunchConfiguration("config_file"), 61 | { "calibration_file": LaunchConfiguration("calibration_file"), 62 | }], 63 | ) 64 | 65 | laser_mapping_node = Node( 66 | package="super_odometry", 67 | executable="laser_mapping_node", 68 | output={ 69 | "stdout": "screen", 70 | "stderr": "screen", 71 | }, 72 | parameters=[LaunchConfiguration("config_file"), 73 | { "calibration_file": LaunchConfiguration("calibration_file"), 74 | "map_dir": os.path.join(home_directory, "/path/to/your/pcd"), 75 | }], 76 | remappings=[ 77 | ("laser_odom_to_init", LaunchConfiguration("odom_topic")), 78 | ] 79 | ) 80 | 81 | imu_preintegration_node = Node( 82 | package="super_odometry", 83 | executable="imu_preintegration_node", 84 | output={ 85 | "stdout": "screen", 86 | "stderr": "screen", 87 | }, 88 | parameters=[LaunchConfiguration("config_file"), 89 | { "calibration_file": LaunchConfiguration("calibration_file") 90 | }], 91 | ) 92 | 93 | 94 | return LaunchDescription([ 95 | launch_ros.actions.SetParameter(name='use_sim_time', value='false'), 96 | config_path_arg, 97 | calib_path_arg, 98 | odom_topic_arg, 99 | world_frame_arg, 100 | world_frame_rot_arg, 101 | sensor_frame_arg, 102 | sensor_frame_rot_arg, 103 | feature_extraction_node, 104 | laser_mapping_node, 105 | imu_preintegration_node, 106 | ]) 107 | -------------------------------------------------------------------------------- /super_odometry/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | super_odometry 5 | 0.0.0 6 | 7 | 8 | This is an implementation of super_odometry, which is based on LOAM algorithm. 9 | LOAM is described in the following paper: 10 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 11 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 12 | 13 | 14 | Shibo Zhao 15 | 16 | BSD 17 | 18 | amend_cmake 19 | 20 | rclcpp 21 | rclpy 22 | tf2 23 | std_msgs 24 | nav_msgs 25 | geometry_msgs 26 | sensor_msgs 27 | super_odometry_msgs 28 | image_transport 29 | 30 | pcl_conversion 31 | sophus 32 | livox_ros_driver2 33 | 34 | ament_lint_auto 35 | ament_lint_common 36 | 37 | 38 | ament_cmake 39 | 40 | 41 | -------------------------------------------------------------------------------- /super_odometry/readme.md: -------------------------------------------------------------------------------- 1 | ## Super Odometry 2 | 3 | Super Odometry is a high-performance odometry library designed for robotics applications. It integrates advanced algorithms for real-time localization and mapping, supporting a wide range of sensors. 4 | 5 | ## How to Run the localization mode? 6 | Please change the yaml file 7 | ``` 8 | localization_mode: true # if true, localization mode is on; Otherwise, SLAM mode is on 9 | read_pose_file: false # read the txt pose as the initial pose for localization 10 | init_x: 13.983960 # initial pose from yaml file for localization 11 | init_y: 1.305790 12 | init_z: 0.002673 13 | init_roll: 0.0 14 | init_pitch: 0.0 15 | init_yaw: -1.150664 16 | ``` 17 | ## Test Sample 18 | 19 | ``` 20 | ros2 bag play cic_office_stopping.db3 --start-offset 50 21 | ros2 launch super_odometry arize_slam.launch.py 22 | 23 | ``` 24 | ## Load the start_pose.txt (below are format samples) 25 | ``` 26 | timespan x y z roll pitch yaw 27 | 50s 13.983960 1.305790 0.002673 0.00 0.0 -1.150664 28 | ``` 29 | ## Put the start_pose.txt same directory of groundtruth map (pointcloud_local.pcd) 30 | 31 | ## Test Bag file 32 | https://drive.google.com/drive/u/0/folders/1R8Tx8nLDC184gjUaMPZjTiklIhsH7RLV -------------------------------------------------------------------------------- /super_odometry/src/LaserMapping/LocalMap.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shibo zhao on 2020-09-27. 3 | // 4 | 5 | #include "super_odometry/LidarProcess/LocalMap.h" 6 | -------------------------------------------------------------------------------- /super_odometry/src/LaserMapping/lidarOptimization.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shibo zhao on 2020-09-27. 3 | // 4 | 5 | #include "super_odometry/LidarProcess/factor/lidarOptimization.h" 6 | 7 | EdgeAnalyticCostFunction::EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_) 8 | : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_){ 9 | 10 | } 11 | 12 | bool EdgeAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 13 | { 14 | 15 | Eigen::Map t_w_curr(parameters[0]); 16 | Eigen::Map q_w_curr(parameters[0]+3); 17 | 18 | Eigen::Vector3d lp; 19 | lp = q_w_curr * curr_point + t_w_curr; //new point 20 | Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b); 21 | Eigen::Vector3d de = last_point_a - last_point_b; 22 | 23 | residuals[0] = nu.x() / de.norm(); 24 | residuals[1] = nu.y() / de.norm(); 25 | residuals[2] = nu.z() / de.norm(); 26 | 27 | if(jacobians != NULL) 28 | { 29 | if(jacobians[0] != NULL) 30 | { 31 | Eigen::Matrix3d skew_lp = skew(curr_point); 32 | Eigen::Matrix dp_by_so3; 33 | (dp_by_so3.block<3,3>(0,0)).setIdentity(); 34 | (dp_by_so3.block<3,3>(0, 3))=-q_w_curr.toRotationMatrix()*skew_lp; 35 | Eigen::Map > J_se3(jacobians[0]); 36 | J_se3.setZero(); 37 | Eigen::Vector3d re = last_point_b - last_point_a; 38 | Eigen::Matrix3d skew_re = skew(re); 39 | 40 | J_se3.block<3,6>(0,0) = skew_re * dp_by_so3/de.norm(); 41 | 42 | } 43 | } 44 | 45 | return true; 46 | 47 | } 48 | 49 | 50 | SurfNormAnalyticCostFunction::SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_) 51 | : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), negative_OA_dot_norm(negative_OA_dot_norm_) { 52 | 53 | } 54 | 55 | bool SurfNormAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 56 | { 57 | Eigen::Map t_w_curr(parameters[0]); 58 | Eigen::Map q_w_curr(parameters[0]+3); 59 | Eigen::Vector3d point_w = q_w_curr * curr_point + t_w_curr; 60 | 61 | residuals[0] = plane_unit_norm.dot(point_w) + negative_OA_dot_norm; 62 | 63 | if(jacobians != NULL) 64 | { 65 | if(jacobians[0] != NULL) 66 | { 67 | 68 | Eigen::Matrix3d skew_point_w = skew(curr_point); 69 | Eigen::Matrix dp_by_so3; 70 | (dp_by_so3.block<3,3>(0,0)).setIdentity(); 71 | dp_by_so3.block<3,3>(0,3) = -q_w_curr.toRotationMatrix()*skew_point_w; 72 | Eigen::Map > J_se3(jacobians[0]); 73 | J_se3.setZero(); 74 | J_se3.block<1,6>(0,0) = plane_unit_norm.transpose() * dp_by_so3; 75 | 76 | } 77 | } 78 | return true; 79 | 80 | } 81 | 82 | 83 | bool PoseSE3Parameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 84 | { 85 | Eigen::Map trans(x); 86 | 87 | Eigen::Quaterniond delta_q; 88 | Eigen::Vector3d delta_t; 89 | getTransformFromSe3(Eigen::Map>(delta), delta_q, delta_t); 90 | Eigen::Map quater(x+3); 91 | Eigen::Map trans_plus(x_plus_delta); 92 | Eigen::Map quater_plus(x_plus_delta+3); 93 | 94 | quater_plus = delta_q * quater; 95 | quater_plus.normalized(); 96 | trans_plus = delta_q * trans + delta_t; 97 | 98 | return true; 99 | } 100 | 101 | 102 | bool PoseSE3Parameterization::ComputeJacobian(const double *x, double *jacobian) const 103 | { 104 | Eigen::Map> j(jacobian); 105 | (j.topRows(6)).setIdentity(); 106 | (j.bottomRows(1)).setZero(); 107 | 108 | return true; 109 | } 110 | 111 | 112 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t){ 113 | Eigen::Vector3d omega(se3.data()+3); 114 | Eigen::Vector3d upsilon(se3.data()); 115 | Eigen::Matrix3d Omega = skew(omega); 116 | 117 | double theta = omega.norm(); 118 | double half_theta = 0.5*theta; 119 | 120 | double imag_factor; 121 | double real_factor = cos(half_theta); 122 | 123 | if(theta<1e-10) 124 | { 125 | double theta_sq = theta*theta; 126 | double theta_po4 = theta_sq*theta_sq; 127 | imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4; 128 | } 129 | else 130 | { 131 | double sin_half_theta = sin(half_theta); 132 | imag_factor = sin_half_theta/theta; 133 | } 134 | 135 | q = Eigen::Quaterniond(real_factor, imag_factor*omega.x(), imag_factor*omega.y(), imag_factor*omega.z()); 136 | 137 | 138 | Eigen::Matrix3d J; 139 | if (theta<1e-10) 140 | { 141 | J = q.matrix(); 142 | } 143 | else 144 | { 145 | Eigen::Matrix3d Omega2 = Omega*Omega; 146 | J = (Eigen::Matrix3d::Identity() + (1-cos(theta))/(theta*theta)*Omega + (theta-sin(theta))/(pow(theta,3))*Omega2); 147 | } 148 | 149 | t = J*upsilon; 150 | } 151 | 152 | Eigen::Matrix skew(const Eigen::Matrix& mat_in){ 153 | Eigen::Matrix skew_mat; 154 | skew_mat.setZero(); 155 | skew_mat(0,1) = -mat_in(2); 156 | skew_mat(0,2) = mat_in(1); 157 | skew_mat(1,2) = -mat_in(0); 158 | skew_mat(1,0) = mat_in(2); 159 | skew_mat(2,0) = -mat_in(1); 160 | skew_mat(2,1) = mat_in(0); 161 | return skew_mat; 162 | } 163 | -------------------------------------------------------------------------------- /super_odometry/src/LidarProcess/SE3AbsolutatePoseFactor.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ubuntu on 2020/7/7. 3 | // 4 | 5 | #include "super_odometry/LidarProcess/factor/SE3AbsolutatePoseFactor.h" 6 | #include "super_odometry/utils/sophus_utils.hpp" 7 | #include "super_odometry/utils/utility.h" 8 | 9 | bool SE3AbsolutatePoseFactor::Evaluate(double const *const *parameters, 10 | double *residuals, 11 | double **jacobians) const { 12 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 13 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], 14 | parameters[0][5]); 15 | 16 | Transformd T_w_i_estimate(Qi, Pi); 17 | 18 | Eigen::Map res(residuals); 19 | 20 | // residual p 21 | res.segment<3>(0) = T_w_i_estimate.pos - T_w_i_meas_.pos; 22 | 23 | // residual R 24 | Eigen::Vector3d error_quat = 2.0 * (T_w_i_meas_.rot.conjugate() * Qi).vec(); 25 | 26 | res.segment<3>(3) = error_quat; 27 | 28 | res.applyOnTheLeft(sqrt_information_); 29 | 30 | if (jacobians) { 31 | 32 | if (jacobians[0]) { 33 | Eigen::Map> jacobian_from( 34 | jacobians[0]); 35 | jacobian_from.setZero(); 36 | 37 | // res_p_dpi 38 | jacobian_from.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); 39 | 40 | // dr_dRi 41 | jacobian_from.block<3, 3>(3, 3) = 42 | (Utility::Qleft(T_w_i_meas_.rot.conjugate() * Qi)) 43 | .bottomRightCorner<3, 3>(); 44 | 45 | // LOG(ERROR) << "\ndR_dqi:\n" << dR_dqi; 46 | 47 | jacobian_from.applyOnTheLeft(sqrt_information_); 48 | } 49 | } 50 | return true; 51 | } 52 | -------------------------------------------------------------------------------- /super_odometry/src/LidarProcess/pose_local_parameterization.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shiboz on 2021-02-06. 3 | // 4 | 5 | #include "super_odometry/LidarProcess/factor/pose_local_parameterization.h" 6 | 7 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 8 | { 9 | Eigen::Map _p(x); 10 | Eigen::Map _q(x + 3); 11 | 12 | Eigen::Map dp(delta); 13 | 14 | Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); 15 | 16 | Eigen::Map p(x_plus_delta); 17 | Eigen::Map q(x_plus_delta + 3); 18 | 19 | p = _p + dp; 20 | q = (_q * dq).normalized(); 21 | 22 | return true; 23 | } 24 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const 25 | { 26 | Eigen::Map> j(jacobian); 27 | j.topRows<6>().setIdentity(); 28 | j.bottomRows<1>().setZero(); 29 | 30 | return true; 31 | } 32 | -------------------------------------------------------------------------------- /super_odometry/src/featureExtraction_node.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shibo zhao on 2020-09-27. 3 | // 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "super_odometry/FeatureExtraction/featureExtraction.h" 6 | 7 | int main(int argc, char **argv) 8 | { 9 | rclcpp::init(argc,argv); 10 | rclcpp::NodeOptions options; 11 | options.arguments({"feature_extraction_node"}); 12 | 13 | std::shared_ptr featureExtraction = 14 | std::make_shared(options); 15 | 16 | featureExtraction->imuBuf.allocate(2000); 17 | featureExtraction->visualOdomBuf.allocate(2000); 18 | featureExtraction->lidarBuf.allocate(50); 19 | featureExtraction->initInterface(); 20 | 21 | rclcpp::executors::MultiThreadedExecutor executor; 22 | executor.add_node(featureExtraction); 23 | executor.spin(); 24 | // rclcpp::spin(featureExtraction->get_node_base_interface()); 25 | rclcpp::shutdown(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /super_odometry/src/imuPreintegration_node.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shibo zhao on 2020-09-27. 3 | // 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "super_odometry/ImuPreintegration/imuPreintegration.h" 6 | 7 | int main(int argc, char **argv) 8 | { 9 | rclcpp::init(argc,argv); 10 | rclcpp::NodeOptions options; 11 | options.arguments({"imu_preintegration_node"}); 12 | 13 | std::shared_ptr imuPreintegration = 14 | std::make_shared(options); 15 | 16 | imuPreintegration->imuBuf.allocate(1000); 17 | imuPreintegration->lidarOdomBuf.allocate(100); 18 | imuPreintegration->initInterface(); 19 | 20 | rclcpp::executors::MultiThreadedExecutor executor; 21 | executor.add_node(imuPreintegration); 22 | executor.spin(); 23 | // rclcpp::spin(imuPreintegration->get_node_base_interface()); 24 | rclcpp::shutdown(); 25 | 26 | return 0; 27 | } 28 | -------------------------------------------------------------------------------- /super_odometry/src/laserMapping_node.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shibo zhao on 2020-09-27. 3 | // 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "super_odometry/LaserMapping/laserMapping.h" 6 | 7 | int main(int argc, char **argv) 8 | { 9 | rclcpp::init(argc, argv); 10 | rclcpp::NodeOptions options; 11 | options.arguments({"laser_mapping_node"}); 12 | 13 | std::shared_ptr laserMapping = 14 | std::make_shared(options); 15 | 16 | laserMapping->initInterface(); 17 | 18 | rclcpp::executors::MultiThreadedExecutor executor; 19 | executor.add_node(laserMapping); 20 | executor.spin(); 21 | // rclcpp::spin(laserMapping->get_node_base_interface()); 22 | rclcpp::shutdown(); 23 | 24 | return 0; 25 | } 26 | 27 | -------------------------------------------------------------------------------- /super_odometry/src/utils/superodom_utils.cpp: -------------------------------------------------------------------------------- 1 | // Created by Shibo on 2025-03-29. 2 | // 3 | 4 | #include "super_odometry/utils/superodom_utils.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace super_odometry { 11 | namespace utils { 12 | 13 | std::vector odometryResults; 14 | 15 | 16 | bool readPointCloud(const std::string &file_path, pcl::PointCloud::Ptr cloud_out) { 17 | std::ifstream file_check(file_path.c_str()); 18 | if (!file_check.good()) { 19 | std::cerr << "Error: File does not exist: " << file_path << std::endl; 20 | return false; 21 | } 22 | file_check.close(); 23 | 24 | pcl::PCDReader reader; 25 | int result = reader.read(file_path, *cloud_out); 26 | 27 | if (result < 0) { 28 | std::cerr << "Error reading PCD file: " << file_path << std::endl; 29 | return false; 30 | } 31 | 32 | return true; 33 | } 34 | 35 | bool readLocalizationPose(const std::string &file_path, std::vector &odometry_results) { 36 | std::string localizationPosePath = file_path; 37 | 38 | // If file_path is a directory, append "start_pose.txt" 39 | size_t lastSlashPos = file_path.find_last_of('/'); 40 | if (lastSlashPos != std::string::npos) { 41 | std::string directory = file_path.substr(0, lastSlashPos + 1); 42 | localizationPosePath = directory + "start_pose.txt"; 43 | } 44 | 45 | std::ifstream file(localizationPosePath); 46 | if (!file.is_open()) { 47 | std::cerr << "Error opening file: " << localizationPosePath << std::endl; 48 | return false; 49 | } 50 | 51 | std::string line; 52 | while (std::getline(file, line)) { 53 | if (line.empty()) { 54 | continue; 55 | } 56 | 57 | std::istringstream iss(line); 58 | OdometryData odom; 59 | if (iss >> odom.duration >> odom.x >> odom.y >> odom.z >> odom.roll >> odom.pitch >> odom.yaw) { 60 | std::cout << "Read odometry data: " << odom.x << " " << odom.y << " " << odom.z << std::endl; 61 | odometry_results.push_back(odom); 62 | } else { 63 | std::cerr << "Error reading line: " << line << std::endl; 64 | } 65 | } 66 | 67 | if (!odometry_results.empty()) { 68 | std::cout << "\033[1;32m Loaded the localization_pose.txt successfully \033[0m" 69 | << odometry_results[0].x << " " << odometry_results[0].y << " " 70 | << odometry_results[0].z << std::endl; 71 | } 72 | 73 | file.close(); 74 | return !odometry_results.empty(); 75 | } 76 | 77 | bool saveLocalizationPose(double timestamp, const Transformd &T_w_lidar, 78 | const std::string &file_path, std::vector &odometry_results) { 79 | std::string saveOdomPath; 80 | size_t lastSlashPos = file_path.find_last_of('/'); 81 | if (lastSlashPos != std::string::npos) { 82 | saveOdomPath = file_path.substr(0, lastSlashPos + 1); // Include the trailing slash 83 | } else { 84 | saveOdomPath = "./"; 85 | } 86 | 87 | OdometryData odom; 88 | { 89 | odom.timestamp = timestamp; 90 | odom.x = T_w_lidar.pos.x(); 91 | odom.y = T_w_lidar.pos.y(); 92 | odom.z = T_w_lidar.pos.z(); 93 | tf2::Quaternion orientation(T_w_lidar.rot.x(), T_w_lidar.rot.y(), T_w_lidar.rot.z(), T_w_lidar.rot.w()); 94 | tf2::Matrix3x3(orientation).getRPY(odom.roll, odom.pitch, odom.yaw); 95 | } 96 | 97 | odometry_results.push_back(odom); 98 | 99 | std::string OdomResultPath = saveOdomPath + "start_pose.txt"; 100 | std::ofstream outFile(OdomResultPath, std::ios::app); 101 | 102 | if (!outFile.is_open()) { 103 | std::cerr << "Error opening file: " << OdomResultPath << std::endl; 104 | return false; 105 | } 106 | 107 | outFile << std::fixed << (odometry_results.size() > 1 ? (odom.timestamp - odometry_results[0].timestamp) : 0.0) << " " 108 | << odom.x << " " << odom.y << " " << odom.z << " " 109 | << odom.roll << " " << odom.pitch << " " << odom.yaw << std::endl; 110 | 111 | outFile.close(); 112 | return true; 113 | } 114 | 115 | void transformAssociateToMap(Transformd& T_w_curr, 116 | const Transformd& T_w_pre, 117 | const Transformd& T_wodom_curr, 118 | const Transformd& T_wodom_pre) { 119 | // Calculate relative transform between previous and current odometry 120 | Transformd T_wodom_pre_curr = T_wodom_pre.inverse() * T_wodom_curr; 121 | 122 | // Apply the relative transform to the previous world pose 123 | T_w_curr = T_w_pre * T_wodom_pre_curr; 124 | } 125 | 126 | void transformAssociateToMap(Eigen::Quaterniond& q_w_curr, 127 | Eigen::Vector3d& t_w_curr, 128 | const Eigen::Quaterniond& q_wmap_wodom, 129 | const Eigen::Quaterniond& q_wodom_curr, 130 | const Eigen::Vector3d& t_wodom_curr, 131 | const Eigen::Vector3d& t_wmap_wodom) { 132 | // Transform from odometry frame to world frame 133 | q_w_curr = q_wmap_wodom * q_wodom_curr; 134 | t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; 135 | } 136 | 137 | void transformUpdate(Eigen::Quaterniond& q_wmap_wodom, 138 | Eigen::Vector3d& t_wmap_wodom, 139 | const Eigen::Quaterniond& q_w_curr, 140 | const Eigen::Quaterniond& q_wodom_curr, 141 | const Eigen::Vector3d& t_w_curr, 142 | const Eigen::Vector3d& t_wodom_curr) { 143 | // Update the transform between world and odometry frames 144 | q_wmap_wodom = q_w_curr * q_wodom_curr.inverse(); 145 | t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr; 146 | } 147 | 148 | void pointAssociateToMap(PointType const *const pi, PointType *const po, 149 | const Eigen::Quaterniond& q_w_curr, 150 | const Eigen::Vector3d& t_w_curr) { 151 | // Transform point from current frame to world frame 152 | Eigen::Vector3d point_curr(pi->x, pi->y, pi->z); 153 | Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr; 154 | po->x = point_w.x(); 155 | po->y = point_w.y(); 156 | po->z = point_w.z(); 157 | po->intensity = pi->intensity; 158 | } 159 | 160 | void pointAssociateToMap(pcl::PointXYZHSV const *const pi, pcl::PointXYZHSV *const po, 161 | const Eigen::Quaterniond& q_w_curr, 162 | const Eigen::Vector3d& t_w_curr) { 163 | // Transform HSV point from current frame to world frame 164 | Eigen::Vector3d point_curr(pi->x, pi->y, pi->z); 165 | Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr; 166 | po->x = point_w.x(); 167 | po->y = point_w.y(); 168 | po->z = point_w.z(); 169 | po->h = pi->h; 170 | po->s = pi->s; 171 | po->v = pi->v; 172 | } 173 | 174 | void pointAssociateTobeMapped(PointType const *const pi, PointType *const po, 175 | const Eigen::Quaterniond& q_w_curr, 176 | const Eigen::Vector3d& t_w_curr) { 177 | // Transform point from world frame to current frame 178 | Eigen::Vector3d point_w(pi->x, pi->y, pi->z); 179 | Eigen::Vector3d point_curr = q_w_curr.inverse() * (point_w - t_w_curr); 180 | po->x = point_curr.x(); 181 | po->y = point_curr.y(); 182 | po->z = point_curr.z(); 183 | po->intensity = pi->intensity; 184 | } 185 | 186 | 187 | tf2::Quaternion extractRollPitch(Eigen::Quaterniond& imu_rotation){ 188 | double imu_roll, imu_pitch, imu_yaw; 189 | tf2::Quaternion orientation(imu_rotation.x(), imu_rotation.y(), imu_rotation.z(), imu_rotation.w()); 190 | tf2::Matrix3x3(orientation).getRPY(imu_roll, imu_pitch, imu_yaw); 191 | tf2::Quaternion quat ; 192 | quat.setRPY(imu_roll,imu_pitch, 0.0); 193 | RCLCPP_INFO(rclcpp::get_logger("super_odometry"), "Using IMU Roll Pitch in ICP: %f %f %f", imu_roll, imu_pitch, imu_yaw); 194 | return quat; 195 | } 196 | 197 | void printTransform(const Transformd& T, const std::string& name){ 198 | std::cout<x, pi->y, pi->z); 204 | Eigen::Vector3d point_w = transform.rot * point_curr + transform.pos; 205 | po->x = point_w.x(); 206 | po->y = point_w.y(); 207 | po->z = point_w.z(); 208 | po->intensity = pi->intensity; 209 | } 210 | 211 | 212 | } // namespace utils 213 | } // namespace super_odometry 214 | -------------------------------------------------------------------------------- /super_odometry/start_pose.txt: -------------------------------------------------------------------------------- 1 | 50 13.983960 1.305790 0.002673 0.00 0.0 -1.150664 2 | 80.099257 4.860932 -3.479999 0.031259 0.0 0.0 2.579807 3 | -------------------------------------------------------------------------------- /super_odometry_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(super_odometry_msgs) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find_package(catkin REQUIRED COMPONENTS 19 | # nav_msgs 20 | # std_msgs 21 | # message_generation 22 | # message_runtime 23 | # geometry_msgs 24 | # sensor_msgs 25 | # ) 26 | # find dependencies 27 | find_package(ament_cmake REQUIRED) 28 | find_package(builtin_interfaces REQUIRED) 29 | find_package(rosidl_default_generators REQUIRED) 30 | find_package(std_msgs REQUIRED) 31 | find_package(geometry_msgs REQUIRED) 32 | find_package(nav_msgs REQUIRED) 33 | find_package(sensor_msgs REQUIRED) 34 | 35 | 36 | # add_message_files( 37 | # FILES 38 | # IterationStats.msg 39 | # OptimizationStats.msg 40 | # LaserFeature.msg 41 | # ) 42 | 43 | rosidl_generate_interfaces(${PROJECT_NAME} 44 | "msg/IterationStats.msg" 45 | "msg/OptimizationStats.msg" 46 | "msg/LaserFeature.msg" 47 | DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs sensor_msgs 48 | ADD_LINTER_TESTS 49 | ) 50 | 51 | if(BUILD_TESTING) 52 | find_package(ament_lint_auto REQUIRED) 53 | ament_lint_auto_find_test_dependencies() 54 | endif() 55 | 56 | ament_package() 57 | -------------------------------------------------------------------------------- /super_odometry_msgs/msg/IterationStats.msg: -------------------------------------------------------------------------------- 1 | # float64 time_elapsed 2 | std_msgs/Header header 3 | float64 translation_norm 4 | float64 rotation_norm 5 | float64 num_surf_from_scan 6 | float64 num_corner_from_scan 7 | -------------------------------------------------------------------------------- /super_odometry_msgs/msg/LaserFeature.msg: -------------------------------------------------------------------------------- 1 | # feature Info 2 | std_msgs/Header header 3 | 4 | int64 sensor 5 | 6 | int64 imu_available 7 | int64 odom_available 8 | 9 | # IMU initial guess for laser mapping 10 | float64 imu_quaternion_x 11 | float64 imu_quaternion_y 12 | float64 imu_quaternion_z 13 | float64 imu_quaternion_w 14 | 15 | # Odometry initial guess for laser mapping 16 | float64 initial_pose_x 17 | float64 initial_pose_y 18 | float64 initial_pose_z 19 | float64 initial_quaternion_x 20 | float64 initial_quaternion_y 21 | float64 initial_quaternion_z 22 | float64 initial_quaternion_w 23 | 24 | # Preintegration reset ID 25 | int64 imu_preintegration_reset_id 26 | 27 | # Point cloud messages 28 | sensor_msgs/PointCloud2 cloud_nodistortion # original cloud remove distortion 29 | sensor_msgs/PointCloud2 cloud_corner # extracted corner feature 30 | sensor_msgs/PointCloud2 cloud_surface # extracted surface feature 31 | sensor_msgs/PointCloud2 cloud_realsense # extracted surface feature from realsense -------------------------------------------------------------------------------- /super_odometry_msgs/msg/OptimizationStats.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | int32 laser_cloud_surf_from_map_num 3 | int32 laser_cloud_corner_from_map_num 4 | int32 laser_cloud_surf_stack_num 5 | int32 laser_cloud_corner_stack_num 6 | float64 total_translation 7 | float64 total_rotation 8 | float64 translation_from_last 9 | float64 rotation_from_last 10 | float64 time_elapsed 11 | float64 latency 12 | int32 n_iterations 13 | float64 average_distance 14 | float64 uncertainty_x 15 | float64 uncertainty_y 16 | float64 uncertainty_z 17 | float64 uncertainty_roll 18 | float64 uncertainty_pitch 19 | float64 uncertainty_yaw 20 | int32 plane_match_success 21 | int32 plane_no_enough_neighbor 22 | int32 plane_neighbor_too_far 23 | int32 plane_badpca_structure 24 | int32 plane_invalid_numerical 25 | int32 plane_mse_too_large 26 | int32 plane_unknown 27 | int32 prediction_source 28 | IterationStats[] iterations -------------------------------------------------------------------------------- /super_odometry_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | super_odometry_msgs 5 | 0.0.0 6 | The super_odometry_msgs package 7 | Shibo Zhao 8 | TODO 9 | 10 | ament_cmake 11 | 12 | rosidl_default_generators 13 | 14 | builtin_interfaces 15 | geometry_msgs 16 | std_msgs 17 | nav_msgs 18 | sensor_msgs 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | rosidl_default_runtime 24 | 25 | rosidl_interface_packages 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | --------------------------------------------------------------------------------