├── CMakeLists.txt ├── LICENSE ├── README.md ├── cfg ├── dlo.yaml └── params.yaml ├── doc ├── gif │ ├── aquila.gif │ ├── dlo.gif │ ├── kentucky.gif │ ├── losangeles.gif │ └── spot.gif └── img │ ├── aquila.png │ ├── imu_calibration.png │ ├── mc_entrance.png │ ├── mc_topdown.png │ ├── spot.png │ ├── terminal_output.png │ └── test_map.png ├── include ├── dlo │ ├── dlo.h │ ├── map.h │ └── odom.h └── nano_gicp │ ├── gicp │ ├── gicp_settings.hpp │ └── so3.hpp │ ├── impl │ ├── lsq_registration_impl.hpp │ ├── nano_gicp_impl.hpp │ └── nanoflann_impl.hpp │ ├── lsq_registration.hpp │ ├── nano_gicp.hpp │ └── nanoflann.hpp ├── launch ├── dlo.launch └── dlo.rviz ├── package.xml ├── src ├── dlo │ ├── map.cc │ ├── map_node.cc │ ├── odom.cc │ └── odom_node.cc └── nano_gicp │ ├── lsq_registration.cc │ ├── nano_gicp.cc │ └── nanoflann.cc └── srv ├── save_pcd.srv └── save_traj.srv /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################# 2 | # # 3 | # Copyright (c) 2022, University of California, Los Angeles # 4 | # # 5 | # Authors: Kenny J. Chen, Brett T. Lopez # 6 | # Contact: kennyjchen@ucla.edu, btlopez@ucla.edu # 7 | # # 8 | ############################################################# 9 | 10 | cmake_minimum_required(VERSION 3.10.0) 11 | project(direct_lidar_odometry) 12 | 13 | add_compile_options(-std=c++14) 14 | set(CMAKE_BUILD_TYPE "Release") 15 | 16 | find_package( PCL REQUIRED ) 17 | include_directories(${PCL_INCLUDE_DIRS}) 18 | add_definitions(${PCL_DEFINITIONS}) 19 | link_directories(${PCL_LIBRARY_DIRS}) 20 | 21 | find_package( Eigen3 REQUIRED ) 22 | include_directories(${EIGEN3_INCLUDE_DIR}) 23 | 24 | include(FindOpenMP) 25 | if(OPENMP_FOUND) 26 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 27 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 28 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 29 | else(OPENMP_FOUND) 30 | message("ERROR: OpenMP could not be found.") 31 | endif(OPENMP_FOUND) 32 | 33 | set(CMAKE_THREAD_PREFER_PTHREAD TRUE) 34 | set(THREADS_PREFER_PTHREAD_FLAG TRUE) 35 | find_package(Threads REQUIRED) 36 | 37 | find_package(catkin REQUIRED COMPONENTS 38 | roscpp 39 | std_msgs 40 | sensor_msgs 41 | geometry_msgs 42 | pcl_ros 43 | message_generation 44 | ) 45 | 46 | add_service_files( 47 | DIRECTORY srv 48 | FILES 49 | save_pcd.srv 50 | save_traj.srv 51 | ) 52 | 53 | generate_messages() 54 | 55 | catkin_package( 56 | CATKIN_DEPENDS 57 | roscpp 58 | std_msgs 59 | sensor_msgs 60 | geometry_msgs 61 | pcl_ros 62 | INCLUDE_DIRS 63 | include 64 | LIBRARIES 65 | ${PROJECT_NAME} 66 | nano_gicp 67 | nanoflann 68 | ) 69 | 70 | include_directories(include ${catkin_INCLUDE_DIRS}) 71 | 72 | set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) 73 | file(WRITE ${CMAKE_BINARY_DIR}/test_cpuid.cpp "#include ") 74 | try_compile(HAS_CPUID ${CMAKE_BINARY_DIR} ${CMAKE_BINARY_DIR}/test_cpuid.cpp) 75 | file(REMOVE ${CMAKE_BINARY_DIR}/test_cpuid.cpp) 76 | if(HAS_CPUID) 77 | add_compile_definitions(HAS_CPUID) 78 | endif() 79 | 80 | # NanoFLANN 81 | add_library(nanoflann STATIC 82 | src/nano_gicp/nanoflann.cc 83 | ) 84 | target_link_libraries(nanoflann ${PCL_LIBRARIES}) 85 | target_include_directories(nanoflann PUBLIC include ${PCL_INCLUDE_DIRS}) 86 | 87 | # NanoGICP 88 | add_library(nano_gicp STATIC 89 | src/nano_gicp/lsq_registration.cc 90 | src/nano_gicp/nano_gicp.cc 91 | ) 92 | target_link_libraries(nano_gicp ${PCL_LIBRARIES} ${OpenMP_LIBS} nanoflann) 93 | target_include_directories(nano_gicp PUBLIC include ${PCL_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) 94 | 95 | # Odometry Node 96 | add_executable(dlo_odom_node src/dlo/odom_node.cc src/dlo/odom.cc) 97 | add_dependencies(dlo_odom_node ${catkin_EXPORTED_TARGETS}) 98 | target_compile_options(dlo_odom_node PRIVATE ${OpenMP_FLAGS}) 99 | target_link_libraries(dlo_odom_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads nano_gicp) 100 | 101 | # Mapping Node 102 | add_executable (dlo_map_node src/dlo/map_node.cc src/dlo/map.cc) 103 | add_dependencies(dlo_map_node ${catkin_EXPORTED_TARGETS}) 104 | target_compile_options(dlo_map_node PRIVATE ${OpenMP_FLAGS}) 105 | target_link_libraries(dlo_map_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads) 106 | 107 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Kenny J. Chen, Brett T. Lopez 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Direct LiDAR Odometry:
Fast Localization with Dense Point Clouds 2 | 3 | #### [[IEEE RA-L](https://ieeexplore.ieee.org/document/9681177)] [[ArXiv](https://arxiv.org/abs/2110.00605)] [[Video](https://www.youtube.com/watch?v=APot6QP_wvg)] [[Code](https://github.com/vectr-ucla/direct_lidar_odometry)] 4 | 5 | DLO is a lightweight and computationally-efficient frontend LiDAR odometry solution with consistent and accurate localization. It features several algorithmic innovations that increase speed, accuracy, and robustness of pose estimation in perceptually-challenging environments and has been extensively tested on aerial and legged robots. 6 | 7 | This work was part of NASA JPL Team CoSTAR's research and development efforts for the DARPA Subterranean Challenge, in which DLO was the primary state estimation component for our fleet of autonomous aerial vehicles. 8 | 9 |
10 |

11 | drawing 12 | drawing 13 |

14 |

15 | drawing 16 | drawing 17 |

18 |

19 | drawing 20 |

21 | 22 | ## Instructions 23 | DLO requires an input point cloud of type `sensor_msgs::PointCloud2` with an optional IMU input of type `sensor_msgs::Imu`. Note that although IMU data is not required, it can be used for initial gravity alignment and will help with point cloud registration. 24 | 25 | ### Dependencies 26 | Our system has been tested extensively on both Ubuntu 18.04 Bionic with ROS Melodic and Ubuntu 20.04 Focal with ROS Noetic, although other versions may work. The following configuration with required dependencies has been verified to be compatible: 27 | 28 | - Ubuntu 18.04 or 20.04 29 | - ROS Melodic or Noetic (`roscpp`, `std_msgs`, `sensor_msgs`, `geometry_msgs`, `pcl_ros`) 30 | - C++ 14 31 | - CMake >= `3.16.3` 32 | - OpenMP >= `4.5` 33 | - Point Cloud Library >= `1.10.0` 34 | - Eigen >= `3.3.7` 35 | 36 | Installing the binaries from Aptitude should work though: 37 | ```sh 38 | sudo apt install libomp-dev libpcl-dev libeigen3-dev 39 | ``` 40 | 41 | ### Compiling 42 | Create a catkin workspace, clone the `direct_lidar_odometry` repository into the `src` folder, and compile via the [`catkin_tools`](https://catkin-tools.readthedocs.io/en/latest/) package (or [`catkin_make`](http://wiki.ros.org/catkin/commands/catkin_make) if preferred): 43 | ```sh 44 | mkdir ws && cd ws && mkdir src && catkin init && cd src 45 | git clone https://github.com/vectr-ucla/direct_lidar_odometry.git 46 | catkin build 47 | ``` 48 | 49 | ### Execution 50 | After sourcing the workspace, launch the DLO odometry and mapping ROS nodes via: 51 | 52 | ```sh 53 | roslaunch direct_lidar_odometry dlo.launch \ 54 | pointcloud_topic:=/robot/velodyne_points \ 55 | imu_topic:=/robot/vn100/imu 56 | ``` 57 | 58 | Make sure to edit the `pointcloud_topic` and `imu_topic` input arguments with your specific topics. If an IMU is not being used, set the `dlo/imu` ROS param to `false` in `cfg/dlo.yaml`. However, if IMU data is available, please allow DLO to calibrate and gravity align for three seconds before moving. Note that the current implementation assumes that LiDAR and IMU coordinate frames coincide, so please make sure that the sensors are physically mounted near each other. 59 | 60 | If successful, RViz will open and you will see similar terminal outputs to the following: 61 | 62 |

63 | drawing 64 | drawing 65 |

66 | 67 | ### Services 68 | To save DLO's generated map into `.pcd` format, call the following service: 69 | 70 | ```sh 71 | rosservice call /robot/dlo_map/save_pcd LEAF_SIZE SAVE_PATH 72 | ``` 73 | To save the trajectory in KITTI format, call the following service: 74 | 75 | ```sh 76 | rosservice call /robot/dlo_odom/save_traj SAVE_PATH 77 | ``` 78 | 79 | ### Test Data 80 | For your convenience, we provide example test data [here](https://ucla.box.com/shared/static/ziojd3auzp0zzcgwb1ucau9anh69xwv9.bag) (9 minutes, ~4.2GB). To run, first launch DLO (with default point cloud and IMU topics) via: 81 | 82 | ```sh 83 | roslaunch direct_lidar_odometry dlo.launch 84 | ``` 85 | 86 | In a separate terminal session, play back the downloaded bag: 87 | 88 | ```sh 89 | rosbag play dlo_test.bag 90 | ``` 91 | 92 |

93 | drawing 94 |

95 | 96 | ## Citation 97 | If you found this work useful, please cite our manuscript: 98 | 99 | ```bibtex 100 | @article{chen2022direct, 101 | author={Chen, Kenny and Lopez, Brett T. and Agha-mohammadi, Ali-akbar and Mehta, Ankur}, 102 | journal={IEEE Robotics and Automation Letters}, 103 | title={Direct LiDAR Odometry: Fast Localization With Dense Point Clouds}, 104 | year={2022}, 105 | volume={7}, 106 | number={2}, 107 | pages={2000-2007}, 108 | doi={10.1109/LRA.2022.3142739} 109 | } 110 | ``` 111 | 112 | ## Acknowledgements 113 | 114 | We thank the authors of the [FastGICP](https://github.com/SMRT-AIST/fast_gicp) and [NanoFLANN](https://github.com/jlblancoc/nanoflann) open-source packages: 115 | 116 | - Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno, “Voxelized GICP for Fast and Accurate 3D Point Cloud Registration,” in _IEEE International Conference on Robotics and Automation (ICRA)_, IEEE, 2021, pp. 11 054–11 059. 117 | - Jose Luis Blanco and Pranjal Kumar Rai, “NanoFLANN: a C++ Header-Only Fork of FLANN, A Library for Nearest Neighbor (NN) with KD-Trees,” https://github.com/jlblancoc/nanoflann, 2014. 118 | 119 | ## License 120 | This work is licensed under the terms of the MIT license. 121 | 122 |
123 |

124 | drawing 125 | drawing 126 |

127 |

128 | drawing 129 | drawing 130 |

131 | -------------------------------------------------------------------------------- /cfg/dlo.yaml: -------------------------------------------------------------------------------- 1 | ############################################################# 2 | # # 3 | # Copyright (c) 2022, University of California, Los Angeles # 4 | # # 5 | # Authors: Kenny J. Chen, Brett T. Lopez # 6 | # Contact: kennyjchen@ucla.edu, btlopez@ucla.edu # 7 | # # 8 | ############################################################# 9 | 10 | dlo: 11 | 12 | version: 1.4.3 13 | 14 | adaptiveParams: true 15 | 16 | imu: true 17 | gravityAlign: true 18 | 19 | odomNode: 20 | odom_frame: odom 21 | child_frame: base_link 22 | 23 | mapNode: 24 | publishFullMap: true 25 | publishFreq: 1.0 26 | leafSize: 0.25 27 | -------------------------------------------------------------------------------- /cfg/params.yaml: -------------------------------------------------------------------------------- 1 | ############################################################# 2 | # # 3 | # Copyright (c) 2022, University of California, Los Angeles # 4 | # # 5 | # Authors: Kenny J. Chen, Brett T. Lopez # 6 | # Contact: kennyjchen@ucla.edu, btlopez@ucla.edu # 7 | # # 8 | ############################################################# 9 | 10 | dlo: 11 | 12 | odomNode: 13 | 14 | initialPose: 15 | use: false 16 | position: 17 | x: 0.0 18 | y: 0.0 19 | z: 0.0 20 | orientation: 21 | w: 1.0 22 | x: 0.0 23 | y: 0.0 24 | z: 0.0 25 | 26 | preprocessing: 27 | cropBoxFilter: 28 | use: true 29 | size: 1.0 30 | voxelFilter: 31 | scan: 32 | use: true 33 | res: 0.25 34 | submap: 35 | use: true 36 | res: 0.5 37 | 38 | keyframe: 39 | threshD: 5.0 40 | threshR: 45.0 41 | 42 | submap: 43 | keyframe: 44 | knn: 10 45 | kcv: 10 46 | kcc: 10 47 | 48 | imu: 49 | calibTime: 3 50 | bufferSize: 2000 51 | 52 | gicp: 53 | minNumPoints: 10 54 | s2s: 55 | kCorrespondences: 10 56 | maxCorrespondenceDistance: 1.0 57 | maxIterations: 32 58 | transformationEpsilon: 0.01 59 | euclideanFitnessEpsilon: 0.01 60 | ransac: 61 | iterations: 5 62 | outlierRejectionThresh: 1.0 63 | s2m: 64 | kCorrespondences: 20 65 | maxCorrespondenceDistance: 0.5 66 | maxIterations: 32 67 | transformationEpsilon: 0.01 68 | euclideanFitnessEpsilon: 0.01 69 | ransac: 70 | iterations: 5 71 | outlierRejectionThresh: 1.0 72 | -------------------------------------------------------------------------------- /doc/gif/aquila.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vectr-ucla/direct_lidar_odometry/11528c02e89a1619ee3c42ba39ecaa992bd20bf4/doc/gif/aquila.gif -------------------------------------------------------------------------------- /doc/gif/dlo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vectr-ucla/direct_lidar_odometry/11528c02e89a1619ee3c42ba39ecaa992bd20bf4/doc/gif/dlo.gif -------------------------------------------------------------------------------- /doc/gif/kentucky.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vectr-ucla/direct_lidar_odometry/11528c02e89a1619ee3c42ba39ecaa992bd20bf4/doc/gif/kentucky.gif -------------------------------------------------------------------------------- /doc/gif/losangeles.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vectr-ucla/direct_lidar_odometry/11528c02e89a1619ee3c42ba39ecaa992bd20bf4/doc/gif/losangeles.gif -------------------------------------------------------------------------------- /doc/gif/spot.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vectr-ucla/direct_lidar_odometry/11528c02e89a1619ee3c42ba39ecaa992bd20bf4/doc/gif/spot.gif -------------------------------------------------------------------------------- /doc/img/aquila.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vectr-ucla/direct_lidar_odometry/11528c02e89a1619ee3c42ba39ecaa992bd20bf4/doc/img/aquila.png -------------------------------------------------------------------------------- /doc/img/imu_calibration.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vectr-ucla/direct_lidar_odometry/11528c02e89a1619ee3c42ba39ecaa992bd20bf4/doc/img/imu_calibration.png -------------------------------------------------------------------------------- /doc/img/mc_entrance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vectr-ucla/direct_lidar_odometry/11528c02e89a1619ee3c42ba39ecaa992bd20bf4/doc/img/mc_entrance.png -------------------------------------------------------------------------------- /doc/img/mc_topdown.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vectr-ucla/direct_lidar_odometry/11528c02e89a1619ee3c42ba39ecaa992bd20bf4/doc/img/mc_topdown.png -------------------------------------------------------------------------------- /doc/img/spot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vectr-ucla/direct_lidar_odometry/11528c02e89a1619ee3c42ba39ecaa992bd20bf4/doc/img/spot.png -------------------------------------------------------------------------------- /doc/img/terminal_output.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vectr-ucla/direct_lidar_odometry/11528c02e89a1619ee3c42ba39ecaa992bd20bf4/doc/img/terminal_output.png -------------------------------------------------------------------------------- /doc/img/test_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vectr-ucla/direct_lidar_odometry/11528c02e89a1619ee3c42ba39ecaa992bd20bf4/doc/img/test_map.png -------------------------------------------------------------------------------- /include/dlo/dlo.h: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 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 | 25 | #ifdef HAS_CPUID 26 | #include 27 | #endif 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include 52 | #include 53 | #include 54 | 55 | typedef pcl::PointXYZI PointType; 56 | 57 | namespace dlo { 58 | 59 | class OdomNode; 60 | class MapNode; 61 | 62 | } 63 | -------------------------------------------------------------------------------- /include/dlo/map.h: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | #include "dlo/dlo.h" 11 | 12 | class dlo::MapNode { 13 | 14 | public: 15 | 16 | MapNode(ros::NodeHandle node_handle); 17 | ~MapNode(); 18 | 19 | static void abort() { 20 | abort_ = true; 21 | } 22 | 23 | void start(); 24 | void stop(); 25 | 26 | private: 27 | 28 | void abortTimerCB(const ros::TimerEvent& e); 29 | void publishTimerCB(const ros::TimerEvent& e); 30 | 31 | void keyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe); 32 | 33 | bool savePcd(direct_lidar_odometry::save_pcd::Request& req, 34 | direct_lidar_odometry::save_pcd::Response& res); 35 | 36 | void getParams(); 37 | 38 | ros::NodeHandle nh; 39 | ros::Timer abort_timer; 40 | ros::Timer publish_timer; 41 | 42 | ros::Subscriber keyframe_sub; 43 | ros::Publisher map_pub; 44 | 45 | ros::ServiceServer save_pcd_srv; 46 | 47 | pcl::PointCloud::Ptr dlo_map; 48 | pcl::VoxelGrid voxelgrid; 49 | 50 | ros::Time map_stamp; 51 | std::string odom_frame; 52 | 53 | bool publish_full_map_; 54 | double publish_freq_; 55 | double leaf_size_; 56 | 57 | static std::atomic abort_; 58 | 59 | }; 60 | -------------------------------------------------------------------------------- /include/dlo/odom.h: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | #include "dlo/dlo.h" 11 | 12 | class dlo::OdomNode { 13 | 14 | public: 15 | 16 | OdomNode(ros::NodeHandle node_handle); 17 | ~OdomNode(); 18 | 19 | static void abort() { 20 | abort_ = true; 21 | } 22 | 23 | void start(); 24 | void stop(); 25 | 26 | private: 27 | 28 | void abortTimerCB(const ros::TimerEvent& e); 29 | void icpCB(const sensor_msgs::PointCloud2ConstPtr& pc); 30 | void imuCB(const sensor_msgs::Imu::ConstPtr& imu); 31 | bool saveTrajectory(direct_lidar_odometry::save_traj::Request& req, 32 | direct_lidar_odometry::save_traj::Response& res); 33 | 34 | void getParams(); 35 | 36 | void publishToROS(); 37 | void publishPose(); 38 | void publishTransform(); 39 | void publishKeyframe(); 40 | 41 | void preprocessPoints(); 42 | void initializeInputTarget(); 43 | void setInputSources(); 44 | 45 | void initializeDLO(); 46 | void gravityAlign(); 47 | 48 | void getNextPose(); 49 | void integrateIMU(); 50 | 51 | void propagateS2S(Eigen::Matrix4f T); 52 | void propagateS2M(); 53 | 54 | void setAdaptiveParams(); 55 | 56 | void computeMetrics(); 57 | void computeSpaciousness(); 58 | 59 | void transformCurrentScan(); 60 | void updateKeyframes(); 61 | void computeConvexHull(); 62 | void computeConcaveHull(); 63 | void pushSubmapIndices(std::vector dists, int k, std::vector frames); 64 | void getSubmapKeyframes(); 65 | 66 | void debug(); 67 | 68 | double first_imu_time; 69 | 70 | ros::NodeHandle nh; 71 | ros::Timer abort_timer; 72 | 73 | ros::ServiceServer save_traj_srv; 74 | 75 | ros::Subscriber icp_sub; 76 | ros::Subscriber imu_sub; 77 | 78 | ros::Publisher odom_pub; 79 | ros::Publisher pose_pub; 80 | ros::Publisher keyframe_pub; 81 | ros::Publisher kf_pub; 82 | 83 | Eigen::Vector3f origin; 84 | std::vector> trajectory; 85 | std::vector, pcl::PointCloud::Ptr>> keyframes; 86 | std::vector>> keyframe_normals; 87 | 88 | std::atomic dlo_initialized; 89 | std::atomic imu_calibrated; 90 | 91 | std::string odom_frame; 92 | std::string child_frame; 93 | 94 | pcl::PointCloud::Ptr original_scan; 95 | pcl::PointCloud::Ptr current_scan; 96 | pcl::PointCloud::Ptr current_scan_t; 97 | 98 | pcl::PointCloud::Ptr keyframes_cloud; 99 | pcl::PointCloud::Ptr keyframe_cloud; 100 | int num_keyframes; 101 | 102 | pcl::ConvexHull convex_hull; 103 | pcl::ConcaveHull concave_hull; 104 | std::vector keyframe_convex; 105 | std::vector keyframe_concave; 106 | 107 | pcl::PointCloud::Ptr submap_cloud; 108 | std::vector> submap_normals; 109 | 110 | std::vector submap_kf_idx_curr; 111 | std::vector submap_kf_idx_prev; 112 | std::atomic submap_hasChanged; 113 | 114 | pcl::PointCloud::Ptr source_cloud; 115 | pcl::PointCloud::Ptr target_cloud; 116 | 117 | ros::Time scan_stamp; 118 | 119 | double curr_frame_stamp; 120 | double prev_frame_stamp; 121 | std::vector comp_times; 122 | 123 | nano_gicp::NanoGICP gicp_s2s; 124 | nano_gicp::NanoGICP gicp; 125 | 126 | pcl::CropBox crop; 127 | pcl::VoxelGrid vf_scan; 128 | pcl::VoxelGrid vf_submap; 129 | 130 | nav_msgs::Odometry odom; 131 | nav_msgs::Odometry kf; 132 | 133 | geometry_msgs::PoseStamped pose_ros; 134 | 135 | Eigen::Matrix4f T; 136 | Eigen::Matrix4f T_s2s, T_s2s_prev; 137 | Eigen::Quaternionf q_final; 138 | 139 | Eigen::Vector3f pose_s2s; 140 | Eigen::Matrix3f rotSO3_s2s; 141 | Eigen::Quaternionf rotq_s2s; 142 | 143 | Eigen::Vector3f pose; 144 | Eigen::Matrix3f rotSO3; 145 | Eigen::Quaternionf rotq; 146 | 147 | Eigen::Matrix4f imu_SE3; 148 | 149 | struct XYZd { 150 | double x; 151 | double y; 152 | double z; 153 | }; 154 | 155 | struct ImuBias { 156 | XYZd gyro; 157 | XYZd accel; 158 | }; 159 | 160 | ImuBias imu_bias; 161 | 162 | struct ImuMeas { 163 | double stamp; 164 | XYZd ang_vel; 165 | XYZd lin_accel; 166 | }; 167 | 168 | ImuMeas imu_meas; 169 | 170 | boost::circular_buffer imu_buffer; 171 | 172 | static bool comparatorImu(ImuMeas m1, ImuMeas m2) { 173 | return (m1.stamp < m2.stamp); 174 | }; 175 | 176 | struct Metrics { 177 | std::vector spaciousness; 178 | }; 179 | 180 | Metrics metrics; 181 | 182 | static std::atomic abort_; 183 | std::atomic stop_publish_thread; 184 | std::atomic stop_publish_keyframe_thread; 185 | std::atomic stop_metrics_thread; 186 | std::atomic stop_debug_thread; 187 | 188 | std::thread publish_thread; 189 | std::thread publish_keyframe_thread; 190 | std::thread metrics_thread; 191 | std::thread debug_thread; 192 | 193 | std::mutex mtx_imu; 194 | 195 | std::string cpu_type; 196 | std::vector cpu_percents; 197 | clock_t lastCPU, lastSysCPU, lastUserCPU; 198 | int numProcessors; 199 | 200 | // Parameters 201 | std::string version_; 202 | 203 | bool gravity_align_; 204 | 205 | double keyframe_thresh_dist_; 206 | double keyframe_thresh_rot_; 207 | 208 | int submap_knn_; 209 | int submap_kcv_; 210 | int submap_kcc_; 211 | double submap_concave_alpha_; 212 | 213 | bool initial_pose_use_; 214 | Eigen::Vector3f initial_position_; 215 | Eigen::Quaternionf initial_orientation_; 216 | 217 | bool crop_use_; 218 | double crop_size_; 219 | 220 | bool vf_scan_use_; 221 | double vf_scan_res_; 222 | 223 | bool vf_submap_use_; 224 | double vf_submap_res_; 225 | 226 | bool adaptive_params_use_; 227 | 228 | bool imu_use_; 229 | int imu_calib_time_; 230 | int imu_buffer_size_; 231 | 232 | int gicp_min_num_points_; 233 | 234 | int gicps2s_k_correspondences_; 235 | double gicps2s_max_corr_dist_; 236 | int gicps2s_max_iter_; 237 | double gicps2s_transformation_ep_; 238 | double gicps2s_euclidean_fitness_ep_; 239 | int gicps2s_ransac_iter_; 240 | double gicps2s_ransac_inlier_thresh_; 241 | 242 | int gicps2m_k_correspondences_; 243 | double gicps2m_max_corr_dist_; 244 | int gicps2m_max_iter_; 245 | double gicps2m_transformation_ep_; 246 | double gicps2m_euclidean_fitness_ep_; 247 | int gicps2m_ransac_iter_; 248 | double gicps2m_ransac_inlier_thresh_; 249 | 250 | }; 251 | -------------------------------------------------------------------------------- /include/nano_gicp/gicp/gicp_settings.hpp: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | /*********************************************************************** 11 | * BSD 3-Clause License 12 | * 13 | * Copyright (c) 2020, SMRT-AIST 14 | * All rights reserved. 15 | * 16 | * Redistribution and use in source and binary forms, with or without 17 | * modification, are permitted provided that the following conditions are met: 18 | * 19 | * 1. Redistributions of source code must retain the above copyright notice, this 20 | * list of conditions and the following disclaimer. 21 | * 22 | * 2. Redistributions in binary form must reproduce the above copyright notice, 23 | * this list of conditions and the following disclaimer in the documentation 24 | * and/or other materials provided with the distribution. 25 | * 26 | * 3. Neither the name of the copyright holder nor the names of its 27 | * contributors may be used to endorse or promote products derived from 28 | * this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 31 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 32 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 33 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 34 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 35 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 36 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 37 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 38 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 39 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 40 | *************************************************************************/ 41 | 42 | #ifndef NANO_GICP_NANO_SETTINGS_HPP 43 | #define NANO_GICP_NANO_SETTINGS_HPP 44 | 45 | namespace nano_gicp { 46 | 47 | enum class RegularizationMethod { NONE, MIN_EIG, NORMALIZED_MIN_EIG, PLANE, FROBENIUS }; 48 | 49 | } 50 | 51 | #endif -------------------------------------------------------------------------------- /include/nano_gicp/gicp/so3.hpp: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | /*********************************************************************** 11 | * BSD 3-Clause License 12 | * 13 | * Copyright (c) 2020, SMRT-AIST 14 | * All rights reserved. 15 | * 16 | * Redistribution and use in source and binary forms, with or without 17 | * modification, are permitted provided that the following conditions are met: 18 | * 19 | * 1. Redistributions of source code must retain the above copyright notice, this 20 | * list of conditions and the following disclaimer. 21 | * 22 | * 2. Redistributions in binary form must reproduce the above copyright notice, 23 | * this list of conditions and the following disclaimer in the documentation 24 | * and/or other materials provided with the distribution. 25 | * 26 | * 3. Neither the name of the copyright holder nor the names of its 27 | * contributors may be used to endorse or promote products derived from 28 | * this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 31 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 32 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 33 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 34 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 35 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 36 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 37 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 38 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 39 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 40 | *************************************************************************/ 41 | 42 | #ifndef NANO_GICP_SO3_HPP 43 | #define NANO_GICP_SO3_HPP 44 | 45 | #include 46 | #include 47 | 48 | namespace nano_gicp { 49 | 50 | inline Eigen::Matrix3f skew(const Eigen::Vector3f& x) { 51 | Eigen::Matrix3f skew = Eigen::Matrix3f::Zero(); 52 | skew(0, 1) = -x[2]; 53 | skew(0, 2) = x[1]; 54 | skew(1, 0) = x[2]; 55 | skew(1, 2) = -x[0]; 56 | skew(2, 0) = -x[1]; 57 | skew(2, 1) = x[0]; 58 | 59 | return skew; 60 | } 61 | 62 | inline Eigen::Matrix3d skewd(const Eigen::Vector3d& x) { 63 | Eigen::Matrix3d skew = Eigen::Matrix3d::Zero(); 64 | skew(0, 1) = -x[2]; 65 | skew(0, 2) = x[1]; 66 | skew(1, 0) = x[2]; 67 | skew(1, 2) = -x[0]; 68 | skew(2, 0) = -x[1]; 69 | skew(2, 1) = x[0]; 70 | 71 | return skew; 72 | } 73 | 74 | /* 75 | * SO3 expmap code taken from Sophus 76 | * https://github.com/strasdat/Sophus/blob/593db47500ea1a2de5f0e6579c86147991509c59/sophus/so3.hpp#L585 77 | * 78 | * Copyright 2011-2017 Hauke Strasdat 79 | * 2012-2017 Steven Lovegrove 80 | * 81 | * Permission is hereby granted, free of charge, to any person obtaining a copy 82 | * of this software and associated documentation files (the "Software"), to 83 | * deal in the Software without restriction, including without limitation the 84 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 85 | * sell copies of the Software, and to permit persons to whom the Software is 86 | * furnished to do so, subject to the following conditions: 87 | * 88 | * The above copyright notice and this permission notice shall be included in 89 | * all copies or substantial portions of the Software. 90 | * 91 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 92 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 93 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 94 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 95 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 96 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 97 | * IN THE SOFTWARE. 98 | */ 99 | inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) { 100 | double theta_sq = omega.dot(omega); 101 | 102 | double theta; 103 | double imag_factor; 104 | double real_factor; 105 | if(theta_sq < 1e-10) { 106 | theta = 0; 107 | double theta_quad = theta_sq * theta_sq; 108 | imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad; 109 | real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad; 110 | } else { 111 | theta = std::sqrt(theta_sq); 112 | double half_theta = 0.5 * theta; 113 | imag_factor = std::sin(half_theta) / theta; 114 | real_factor = std::cos(half_theta); 115 | } 116 | 117 | return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z()); 118 | } 119 | 120 | } // namespace nano_gicp 121 | 122 | #endif -------------------------------------------------------------------------------- /include/nano_gicp/impl/lsq_registration_impl.hpp: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | /*********************************************************************** 11 | * BSD 3-Clause License 12 | * 13 | * Copyright (c) 2020, SMRT-AIST 14 | * All rights reserved. 15 | * 16 | * Redistribution and use in source and binary forms, with or without 17 | * modification, are permitted provided that the following conditions are met: 18 | * 19 | * 1. Redistributions of source code must retain the above copyright notice, this 20 | * list of conditions and the following disclaimer. 21 | * 22 | * 2. Redistributions in binary form must reproduce the above copyright notice, 23 | * this list of conditions and the following disclaimer in the documentation 24 | * and/or other materials provided with the distribution. 25 | * 26 | * 3. Neither the name of the copyright holder nor the names of its 27 | * contributors may be used to endorse or promote products derived from 28 | * this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 31 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 32 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 33 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 34 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 35 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 36 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 37 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 38 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 39 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 40 | *************************************************************************/ 41 | 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | namespace nano_gicp { 48 | 49 | template 50 | LsqRegistration::LsqRegistration() { 51 | this->reg_name_ = "LsqRegistration"; 52 | max_iterations_ = 64; 53 | rotation_epsilon_ = 2e-3; 54 | transformation_epsilon_ = 5e-4; 55 | 56 | lsq_optimizer_type_ = LSQ_OPTIMIZER_TYPE::LevenbergMarquardt; 57 | lm_debug_print_ = false; 58 | lm_max_iterations_ = 10; 59 | lm_init_lambda_factor_ = 1e-9; 60 | lm_lambda_ = -1.0; 61 | 62 | final_hessian_.setIdentity(); 63 | } 64 | 65 | template 66 | LsqRegistration::~LsqRegistration() {} 67 | 68 | template 69 | void LsqRegistration::setRotationEpsilon(double eps) { 70 | rotation_epsilon_ = eps; 71 | } 72 | 73 | template 74 | void LsqRegistration::setInitialLambdaFactor(double init_lambda_factor) { 75 | lm_init_lambda_factor_ = init_lambda_factor; 76 | } 77 | 78 | template 79 | void LsqRegistration::setDebugPrint(bool lm_debug_print) { 80 | lm_debug_print_ = lm_debug_print; 81 | } 82 | 83 | template 84 | const Eigen::Matrix& LsqRegistration::getFinalHessian() const { 85 | return final_hessian_; 86 | } 87 | 88 | template 89 | void LsqRegistration::computeTransformation(PointCloudSource& output, const Matrix4& guess) { 90 | Eigen::Isometry3d x0 = Eigen::Isometry3d(guess.template cast()); 91 | 92 | lm_lambda_ = -1.0; 93 | converged_ = false; 94 | 95 | if (lm_debug_print_) { 96 | std::cout << "********************************************" << std::endl; 97 | std::cout << "***************** optimize *****************" << std::endl; 98 | std::cout << "********************************************" << std::endl; 99 | } 100 | 101 | for (int i = 0; i < max_iterations_ && !converged_; i++) { 102 | nr_iterations_ = i; 103 | 104 | Eigen::Isometry3d delta; 105 | if (!step_optimize(x0, delta)) { 106 | std::cerr << "lm not converged!!" << std::endl; 107 | break; 108 | } 109 | 110 | converged_ = is_converged(delta); 111 | } 112 | 113 | final_transformation_ = x0.cast().matrix(); 114 | pcl::transformPointCloud(*input_, output, final_transformation_); 115 | } 116 | 117 | template 118 | bool LsqRegistration::is_converged(const Eigen::Isometry3d& delta) const { 119 | double accum = 0.0; 120 | Eigen::Matrix3d R = delta.linear() - Eigen::Matrix3d::Identity(); 121 | Eigen::Vector3d t = delta.translation(); 122 | 123 | Eigen::Matrix3d r_delta = 1.0 / rotation_epsilon_ * R.array().abs(); 124 | Eigen::Vector3d t_delta = 1.0 / transformation_epsilon_ * t.array().abs(); 125 | 126 | return std::max(r_delta.maxCoeff(), t_delta.maxCoeff()) < 1; 127 | } 128 | 129 | template 130 | bool LsqRegistration::step_optimize(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) { 131 | switch (lsq_optimizer_type_) { 132 | case LSQ_OPTIMIZER_TYPE::LevenbergMarquardt: 133 | return step_lm(x0, delta); 134 | case LSQ_OPTIMIZER_TYPE::GaussNewton: 135 | return step_gn(x0, delta); 136 | } 137 | 138 | return step_lm(x0, delta); 139 | } 140 | 141 | template 142 | bool LsqRegistration::step_gn(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) { 143 | Eigen::Matrix H; 144 | Eigen::Matrix b; 145 | double y0 = linearize(x0, &H, &b); 146 | 147 | Eigen::LDLT> solver(H); 148 | Eigen::Matrix d = solver.solve(-b); 149 | 150 | delta.setIdentity(); 151 | delta.linear() = so3_exp(d.head<3>()).toRotationMatrix(); 152 | delta.translation() = d.tail<3>(); 153 | 154 | x0 = delta * x0; 155 | final_hessian_ = H; 156 | 157 | return true; 158 | } 159 | 160 | template 161 | bool LsqRegistration::step_lm(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) { 162 | Eigen::Matrix H; 163 | Eigen::Matrix b; 164 | double y0 = linearize(x0, &H, &b); 165 | 166 | if (lm_lambda_ < 0.0) { 167 | lm_lambda_ = lm_init_lambda_factor_ * H.diagonal().array().abs().maxCoeff(); 168 | } 169 | 170 | double nu = 2.0; 171 | for (int i = 0; i < lm_max_iterations_; i++) { 172 | Eigen::LDLT> solver(H + lm_lambda_ * Eigen::Matrix::Identity()); 173 | Eigen::Matrix d = solver.solve(-b); 174 | 175 | delta.setIdentity(); 176 | delta.linear() = so3_exp(d.head<3>()).toRotationMatrix(); 177 | delta.translation() = d.tail<3>(); 178 | 179 | Eigen::Isometry3d xi = delta * x0; 180 | double yi = compute_error(xi); 181 | double rho = (y0 - yi) / (d.dot(lm_lambda_ * d - b)); 182 | 183 | if (lm_debug_print_) { 184 | if (i == 0) { 185 | std::cout << boost::format("--- LM optimization ---\n%5s %15s %15s %15s %15s %15s %5s\n") % "i" % "y0" % "yi" % "rho" % "lambda" % "|delta|" % "dec"; 186 | } 187 | char dec = rho > 0.0 ? 'x' : ' '; 188 | std::cout << boost::format("%5d %15g %15g %15g %15g %15g %5c") % i % y0 % yi % rho % lm_lambda_ % d.norm() % dec << std::endl; 189 | } 190 | 191 | if (rho < 0) { 192 | if (is_converged(delta)) { 193 | return true; 194 | } 195 | 196 | lm_lambda_ = nu * lm_lambda_; 197 | nu = 2 * nu; 198 | continue; 199 | } 200 | 201 | x0 = xi; 202 | lm_lambda_ = lm_lambda_ * std::max(1.0 / 3.0, 1 - std::pow(2 * rho - 1, 3)); 203 | final_hessian_ = H; 204 | return true; 205 | } 206 | 207 | return false; 208 | } 209 | 210 | } // namespace nano_gicp -------------------------------------------------------------------------------- /include/nano_gicp/impl/nano_gicp_impl.hpp: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | /*********************************************************************** 11 | * BSD 3-Clause License 12 | * 13 | * Copyright (c) 2020, SMRT-AIST 14 | * All rights reserved. 15 | * 16 | * Redistribution and use in source and binary forms, with or without 17 | * modification, are permitted provided that the following conditions are met: 18 | * 19 | * 1. Redistributions of source code must retain the above copyright notice, this 20 | * list of conditions and the following disclaimer. 21 | * 22 | * 2. Redistributions in binary form must reproduce the above copyright notice, 23 | * this list of conditions and the following disclaimer in the documentation 24 | * and/or other materials provided with the distribution. 25 | * 26 | * 3. Neither the name of the copyright holder nor the names of its 27 | * contributors may be used to endorse or promote products derived from 28 | * this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 31 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 32 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 33 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 34 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 35 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 36 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 37 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 38 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 39 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 40 | *************************************************************************/ 41 | 42 | #ifndef NANO_GICP_NANO_GICP_IMPL_HPP 43 | #define NANO_GICP_NANO_GICP_IMPL_HPP 44 | 45 | #include 46 | 47 | namespace nano_gicp { 48 | 49 | template 50 | NanoGICP::NanoGICP() { 51 | #ifdef _OPENMP 52 | num_threads_ = omp_get_max_threads(); 53 | #else 54 | num_threads_ = 1; 55 | #endif 56 | 57 | k_correspondences_ = 20; 58 | reg_name_ = "NanoGICP"; 59 | corr_dist_threshold_ = std::numeric_limits::max(); 60 | 61 | regularization_method_ = RegularizationMethod::PLANE; 62 | source_kdtree_.reset(new nanoflann::KdTreeFLANN); 63 | target_kdtree_.reset(new nanoflann::KdTreeFLANN); 64 | } 65 | 66 | template 67 | NanoGICP::~NanoGICP() {} 68 | 69 | template 70 | void NanoGICP::setNumThreads(int n) { 71 | num_threads_ = n; 72 | 73 | #ifdef _OPENMP 74 | if (n == 0) { 75 | num_threads_ = omp_get_max_threads(); 76 | } 77 | #endif 78 | } 79 | 80 | template 81 | void NanoGICP::setCorrespondenceRandomness(int k) { 82 | k_correspondences_ = k; 83 | } 84 | 85 | template 86 | void NanoGICP::setRegularizationMethod(RegularizationMethod method) { 87 | regularization_method_ = method; 88 | } 89 | 90 | template 91 | void NanoGICP::swapSourceAndTarget() { 92 | input_.swap(target_); 93 | source_kdtree_.swap(target_kdtree_); 94 | source_covs_.swap(target_covs_); 95 | 96 | correspondences_.clear(); 97 | sq_distances_.clear(); 98 | } 99 | 100 | template 101 | void NanoGICP::clearSource() { 102 | input_.reset(); 103 | source_covs_.clear(); 104 | } 105 | 106 | template 107 | void NanoGICP::clearTarget() { 108 | target_.reset(); 109 | target_covs_.clear(); 110 | } 111 | 112 | template 113 | void NanoGICP::registerInputSource(const PointCloudSourceConstPtr& cloud) { 114 | if (input_ == cloud) { 115 | return; 116 | } 117 | pcl::Registration::setInputSource(cloud); 118 | } 119 | 120 | template 121 | void NanoGICP::setInputSource(const PointCloudSourceConstPtr& cloud) { 122 | if (input_ == cloud) { 123 | return; 124 | } 125 | 126 | pcl::Registration::setInputSource(cloud); 127 | source_kdtree_->setInputCloud(cloud); 128 | source_covs_.clear(); 129 | } 130 | 131 | template 132 | void NanoGICP::setInputTarget(const PointCloudTargetConstPtr& cloud) { 133 | if (target_ == cloud) { 134 | return; 135 | } 136 | pcl::Registration::setInputTarget(cloud); 137 | target_kdtree_->setInputCloud(cloud); 138 | target_covs_.clear(); 139 | } 140 | 141 | template 142 | void NanoGICP::setSourceCovariances(const std::vector>& covs) { 143 | source_covs_ = covs; 144 | } 145 | 146 | template 147 | void NanoGICP::setTargetCovariances(const std::vector>& covs) { 148 | target_covs_ = covs; 149 | } 150 | 151 | template 152 | bool NanoGICP::calculateSourceCovariances() { 153 | return calculate_covariances(input_, *source_kdtree_, source_covs_); 154 | } 155 | 156 | template 157 | bool NanoGICP::calculateTargetCovariances() { 158 | return calculate_covariances(target_, *target_kdtree_, target_covs_); 159 | } 160 | 161 | template 162 | void NanoGICP::computeTransformation(PointCloudSource& output, const Matrix4& guess) { 163 | if (source_covs_.size() != input_->size()) { 164 | calculateSourceCovariances(); 165 | } 166 | if (target_covs_.size() != target_->size()) { 167 | calculateTargetCovariances(); 168 | } 169 | 170 | LsqRegistration::computeTransformation(output, guess); 171 | } 172 | 173 | template 174 | void NanoGICP::update_correspondences(const Eigen::Isometry3d& trans) { 175 | assert(source_covs_.size() == input_->size()); 176 | assert(target_covs_.size() == target_->size()); 177 | 178 | Eigen::Isometry3f trans_f = trans.cast(); 179 | 180 | correspondences_.resize(input_->size()); 181 | sq_distances_.resize(input_->size()); 182 | mahalanobis_.resize(input_->size()); 183 | 184 | std::vector k_indices(1); 185 | std::vector k_sq_dists(1); 186 | 187 | #pragma omp parallel for num_threads(num_threads_) firstprivate(k_indices, k_sq_dists) schedule(guided, 8) 188 | for (int i = 0; i < input_->size(); i++) { 189 | PointTarget pt; 190 | pt.getVector4fMap() = trans_f * input_->at(i).getVector4fMap(); 191 | 192 | target_kdtree_->nearestKSearch(pt, 1, k_indices, k_sq_dists); 193 | 194 | sq_distances_[i] = k_sq_dists[0]; 195 | correspondences_[i] = k_sq_dists[0] < corr_dist_threshold_ * corr_dist_threshold_ ? k_indices[0] : -1; 196 | 197 | if (correspondences_[i] < 0) { 198 | continue; 199 | } 200 | 201 | const int target_index = correspondences_[i]; 202 | const auto& cov_A = source_covs_[i]; 203 | const auto& cov_B = target_covs_[target_index]; 204 | 205 | Eigen::Matrix4d RCR = cov_B + trans.matrix() * cov_A * trans.matrix().transpose(); 206 | RCR(3, 3) = 1.0; 207 | 208 | mahalanobis_[i] = RCR.inverse(); 209 | mahalanobis_[i](3, 3) = 0.0f; 210 | } 211 | } 212 | 213 | template 214 | double NanoGICP::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) { 215 | update_correspondences(trans); 216 | 217 | double sum_errors = 0.0; 218 | std::vector, Eigen::aligned_allocator>> Hs(num_threads_); 219 | std::vector, Eigen::aligned_allocator>> bs(num_threads_); 220 | for (int i = 0; i < num_threads_; i++) { 221 | Hs[i].setZero(); 222 | bs[i].setZero(); 223 | } 224 | 225 | #pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) schedule(guided, 8) 226 | for (int i = 0; i < input_->size(); i++) { 227 | int target_index = correspondences_[i]; 228 | if (target_index < 0) { 229 | continue; 230 | } 231 | 232 | const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast(); 233 | const auto& cov_A = source_covs_[i]; 234 | 235 | const Eigen::Vector4d mean_B = target_->at(target_index).getVector4fMap().template cast(); 236 | const auto& cov_B = target_covs_[target_index]; 237 | 238 | const Eigen::Vector4d transed_mean_A = trans * mean_A; 239 | const Eigen::Vector4d error = mean_B - transed_mean_A; 240 | 241 | sum_errors += error.transpose() * mahalanobis_[i] * error; 242 | 243 | if (H == nullptr || b == nullptr) { 244 | continue; 245 | } 246 | 247 | Eigen::Matrix dtdx0 = Eigen::Matrix::Zero(); 248 | dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>()); 249 | dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity(); 250 | 251 | Eigen::Matrix jlossexp = dtdx0; 252 | 253 | Eigen::Matrix Hi = jlossexp.transpose() * mahalanobis_[i] * jlossexp; 254 | Eigen::Matrix bi = jlossexp.transpose() * mahalanobis_[i] * error; 255 | 256 | Hs[omp_get_thread_num()] += Hi; 257 | bs[omp_get_thread_num()] += bi; 258 | } 259 | 260 | if (H && b) { 261 | H->setZero(); 262 | b->setZero(); 263 | for (int i = 0; i < num_threads_; i++) { 264 | (*H) += Hs[i]; 265 | (*b) += bs[i]; 266 | } 267 | } 268 | 269 | return sum_errors; 270 | } 271 | 272 | template 273 | double NanoGICP::compute_error(const Eigen::Isometry3d& trans) { 274 | double sum_errors = 0.0; 275 | 276 | #pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) schedule(guided, 8) 277 | for (int i = 0; i < input_->size(); i++) { 278 | int target_index = correspondences_[i]; 279 | if (target_index < 0) { 280 | continue; 281 | } 282 | 283 | const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast(); 284 | const auto& cov_A = source_covs_[i]; 285 | 286 | const Eigen::Vector4d mean_B = target_->at(target_index).getVector4fMap().template cast(); 287 | const auto& cov_B = target_covs_[target_index]; 288 | 289 | const Eigen::Vector4d transed_mean_A = trans * mean_A; 290 | const Eigen::Vector4d error = mean_B - transed_mean_A; 291 | 292 | sum_errors += error.transpose() * mahalanobis_[i] * error; 293 | } 294 | 295 | return sum_errors; 296 | } 297 | 298 | template 299 | template 300 | bool NanoGICP::calculate_covariances( 301 | const typename pcl::PointCloud::ConstPtr& cloud, 302 | nanoflann::KdTreeFLANN& kdtree, 303 | std::vector>& covariances) { 304 | if (kdtree.getInputCloud() != cloud) { 305 | kdtree.setInputCloud(cloud); 306 | } 307 | covariances.resize(cloud->size()); 308 | 309 | #pragma omp parallel for num_threads(num_threads_) schedule(guided, 8) 310 | for (int i = 0; i < cloud->size(); i++) { 311 | std::vector k_indices; 312 | std::vector k_sq_distances; 313 | kdtree.nearestKSearch(cloud->at(i), k_correspondences_, k_indices, k_sq_distances); 314 | 315 | Eigen::Matrix neighbors(4, k_correspondences_); 316 | for (int j = 0; j < k_indices.size(); j++) { 317 | neighbors.col(j) = cloud->at(k_indices[j]).getVector4fMap().template cast(); 318 | } 319 | 320 | neighbors.colwise() -= neighbors.rowwise().mean().eval(); 321 | Eigen::Matrix4d cov = neighbors * neighbors.transpose() / k_correspondences_; 322 | 323 | if (regularization_method_ == RegularizationMethod::NONE) { 324 | covariances[i] = cov; 325 | } else if (regularization_method_ == RegularizationMethod::FROBENIUS) { 326 | double lambda = 1e-3; 327 | Eigen::Matrix3d C = cov.block<3, 3>(0, 0).cast() + lambda * Eigen::Matrix3d::Identity(); 328 | Eigen::Matrix3d C_inv = C.inverse(); 329 | covariances[i].setZero(); 330 | covariances[i].template block<3, 3>(0, 0) = (C_inv / C_inv.norm()).inverse(); 331 | } else { 332 | Eigen::JacobiSVD svd(cov.block<3, 3>(0, 0), Eigen::ComputeFullU | Eigen::ComputeFullV); 333 | Eigen::Vector3d values; 334 | 335 | switch (regularization_method_) { 336 | default: 337 | std::cerr << "here must not be reached" << std::endl; 338 | abort(); 339 | case RegularizationMethod::PLANE: 340 | values = Eigen::Vector3d(1, 1, 1e-3); 341 | break; 342 | case RegularizationMethod::MIN_EIG: 343 | values = svd.singularValues().array().max(1e-3); 344 | break; 345 | case RegularizationMethod::NORMALIZED_MIN_EIG: 346 | values = svd.singularValues() / svd.singularValues().maxCoeff(); 347 | values = values.array().max(1e-3); 348 | break; 349 | } 350 | 351 | covariances[i].setZero(); 352 | covariances[i].template block<3, 3>(0, 0) = svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose(); 353 | } 354 | } 355 | 356 | return true; 357 | } 358 | 359 | } // namespace nano_gicp 360 | 361 | #endif 362 | -------------------------------------------------------------------------------- /include/nano_gicp/lsq_registration.hpp: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | /*********************************************************************** 11 | * BSD 3-Clause License 12 | * 13 | * Copyright (c) 2020, SMRT-AIST 14 | * All rights reserved. 15 | * 16 | * Redistribution and use in source and binary forms, with or without 17 | * modification, are permitted provided that the following conditions are met: 18 | * 19 | * 1. Redistributions of source code must retain the above copyright notice, this 20 | * list of conditions and the following disclaimer. 21 | * 22 | * 2. Redistributions in binary form must reproduce the above copyright notice, 23 | * this list of conditions and the following disclaimer in the documentation 24 | * and/or other materials provided with the distribution. 25 | * 26 | * 3. Neither the name of the copyright holder nor the names of its 27 | * contributors may be used to endorse or promote products derived from 28 | * this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 31 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 32 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 33 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 34 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 35 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 36 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 37 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 38 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 39 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 40 | *************************************************************************/ 41 | 42 | #ifndef NANO_GICP_LSQ_REGISTRATION_HPP 43 | #define NANO_GICP_LSQ_REGISTRATION_HPP 44 | 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | #include 51 | 52 | namespace nano_gicp { 53 | 54 | enum class LSQ_OPTIMIZER_TYPE { GaussNewton, LevenbergMarquardt }; 55 | 56 | template 57 | class LsqRegistration : public pcl::Registration { 58 | public: 59 | using Scalar = float; 60 | using Matrix4 = typename pcl::Registration::Matrix4; 61 | 62 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 63 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 64 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 65 | 66 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 67 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 68 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 69 | 70 | protected: 71 | using pcl::Registration::input_; 72 | using pcl::Registration::nr_iterations_; 73 | using pcl::Registration::max_iterations_; 74 | using pcl::Registration::final_transformation_; 75 | using pcl::Registration::transformation_epsilon_; 76 | using pcl::Registration::converged_; 77 | 78 | public: 79 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 80 | 81 | LsqRegistration(); 82 | virtual ~LsqRegistration(); 83 | 84 | void setRotationEpsilon(double eps); 85 | void setInitialLambdaFactor(double init_lambda_factor); 86 | void setDebugPrint(bool lm_debug_print); 87 | 88 | const Eigen::Matrix& getFinalHessian() const; 89 | 90 | virtual void swapSourceAndTarget() {} 91 | virtual void clearSource() {} 92 | virtual void clearTarget() {} 93 | 94 | protected: 95 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 96 | 97 | bool is_converged(const Eigen::Isometry3d& delta) const; 98 | 99 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) = 0; 100 | virtual double compute_error(const Eigen::Isometry3d& trans) = 0; 101 | 102 | bool step_optimize(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 103 | bool step_gn(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 104 | bool step_lm(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 105 | 106 | protected: 107 | double rotation_epsilon_; 108 | 109 | LSQ_OPTIMIZER_TYPE lsq_optimizer_type_; 110 | int lm_max_iterations_; 111 | double lm_init_lambda_factor_; 112 | double lm_lambda_; 113 | bool lm_debug_print_; 114 | 115 | Eigen::Matrix final_hessian_; 116 | }; 117 | } // namespace nano_gicp 118 | 119 | #endif -------------------------------------------------------------------------------- /include/nano_gicp/nano_gicp.hpp: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | /*********************************************************************** 11 | * BSD 3-Clause License 12 | * 13 | * Copyright (c) 2020, SMRT-AIST 14 | * All rights reserved. 15 | * 16 | * Redistribution and use in source and binary forms, with or without 17 | * modification, are permitted provided that the following conditions are met: 18 | * 19 | * 1. Redistributions of source code must retain the above copyright notice, this 20 | * list of conditions and the following disclaimer. 21 | * 22 | * 2. Redistributions in binary form must reproduce the above copyright notice, 23 | * this list of conditions and the following disclaimer in the documentation 24 | * and/or other materials provided with the distribution. 25 | * 26 | * 3. Neither the name of the copyright holder nor the names of its 27 | * contributors may be used to endorse or promote products derived from 28 | * this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 31 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 32 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 33 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 34 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 35 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 36 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 37 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 38 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 39 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 40 | *************************************************************************/ 41 | 42 | #ifndef NANO_GICP_NANO_GICP_HPP 43 | #define NANO_GICP_NANO_GICP_HPP 44 | 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | #include 51 | 52 | #include 53 | #include 54 | #include 55 | 56 | namespace nano_gicp { 57 | 58 | template 59 | class NanoGICP : public LsqRegistration { 60 | public: 61 | using Scalar = float; 62 | using Matrix4 = typename pcl::Registration::Matrix4; 63 | 64 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 65 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 66 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 67 | 68 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 69 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 70 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 71 | 72 | protected: 73 | using pcl::Registration::reg_name_; 74 | using pcl::Registration::input_; 75 | using pcl::Registration::target_; 76 | using pcl::Registration::corr_dist_threshold_; 77 | 78 | public: 79 | NanoGICP(); 80 | virtual ~NanoGICP() override; 81 | 82 | void setNumThreads(int n); 83 | void setCorrespondenceRandomness(int k); 84 | void setRegularizationMethod(RegularizationMethod method); 85 | 86 | virtual void swapSourceAndTarget() override; 87 | virtual void clearSource() override; 88 | virtual void clearTarget() override; 89 | 90 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 91 | virtual void setSourceCovariances(const std::vector>& covs); 92 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 93 | virtual void setTargetCovariances(const std::vector>& covs); 94 | 95 | virtual void registerInputSource(const PointCloudSourceConstPtr& cloud); 96 | 97 | virtual bool calculateSourceCovariances(); 98 | virtual bool calculateTargetCovariances(); 99 | 100 | const std::vector>& getSourceCovariances() const { 101 | return source_covs_; 102 | } 103 | 104 | const std::vector>& getTargetCovariances() const { 105 | return target_covs_; 106 | } 107 | 108 | protected: 109 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 110 | 111 | virtual void update_correspondences(const Eigen::Isometry3d& trans); 112 | 113 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) override; 114 | 115 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 116 | 117 | template 118 | bool calculate_covariances(const typename pcl::PointCloud::ConstPtr& cloud, nanoflann::KdTreeFLANN& kdtree, std::vector>& covariances); 119 | 120 | public: 121 | std::shared_ptr> source_kdtree_; 122 | std::shared_ptr> target_kdtree_; 123 | 124 | std::vector> source_covs_; 125 | std::vector> target_covs_; 126 | 127 | protected: 128 | int num_threads_; 129 | int k_correspondences_; 130 | 131 | RegularizationMethod regularization_method_; 132 | 133 | std::vector> mahalanobis_; 134 | 135 | std::vector correspondences_; 136 | std::vector sq_distances_; 137 | }; 138 | } // namespace nano_gicp 139 | 140 | #endif 141 | -------------------------------------------------------------------------------- /include/nano_gicp/nanoflann.hpp: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | /*********************************************************************** 11 | * Software License Agreement (BSD License) 12 | * 13 | * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. 14 | * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. 15 | * Copyright 2011-2021 Jose Luis Blanco (joseluisblancoc@gmail.com). 16 | * All rights reserved. 17 | * 18 | * THE BSD LICENSE 19 | * 20 | * Redistribution and use in source and binary forms, with or without 21 | * modification, are permitted provided that the following conditions 22 | * are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright 25 | * notice, this list of conditions and the following disclaimer. 26 | * 2. Redistributions in binary form must reproduce the above copyright 27 | * notice, this list of conditions and the following disclaimer in the 28 | * documentation and/or other materials provided with the distribution. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 31 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 32 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 33 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 34 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 35 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 36 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 37 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 38 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 39 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 40 | *************************************************************************/ 41 | 42 | #ifndef NANO_KDTREE_KDTREE_FLANN_H_ 43 | #define NANO_KDTREE_KDTREE_FLANN_H_ 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include "nano_gicp/impl/nanoflann_impl.hpp" 50 | 51 | namespace nanoflann 52 | { 53 | 54 | template 55 | class KdTreeFLANN 56 | { 57 | public: 58 | 59 | typedef typename pcl::PointCloud PointCloud; 60 | typedef typename pcl::PointCloud::Ptr PointCloudPtr; 61 | typedef typename pcl::PointCloud::ConstPtr PointCloudConstPtr; 62 | 63 | typedef boost::shared_ptr> Ptr; 64 | typedef boost::shared_ptr> ConstPtr; 65 | typedef boost::shared_ptr> IndicesPtr; 66 | typedef boost::shared_ptr> IndicesConstPtr; 67 | 68 | KdTreeFLANN (bool sorted = false); 69 | KdTreeFLANN (const KdTreeFLANN &k); 70 | 71 | void setEpsilon (float eps); 72 | 73 | void setSortedResults (bool sorted); 74 | 75 | inline Ptr makeShared () { return Ptr (new KdTreeFLANN (*this)); } 76 | 77 | void setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()); 78 | 79 | inline PointCloudConstPtr getInputCloud() const { return _adaptor.pcl; } 80 | 81 | int nearestKSearch (const PointT &point, int k, std::vector &k_indices, 82 | std::vector &k_sqr_distances) const; 83 | 84 | int radiusSearch (const PointT &point, double radius, std::vector &k_indices, 85 | std::vector &k_sqr_distances) const; 86 | 87 | protected: 88 | 89 | nanoflann::SearchParams _params; 90 | 91 | struct PointCloud_Adaptor 92 | { 93 | inline size_t kdtree_get_point_count() const; 94 | inline float kdtree_get_pt(const size_t idx, int dim) const; 95 | template bool kdtree_get_bbox(BBOX&) const { return false; } 96 | PointCloudConstPtr pcl; 97 | IndicesConstPtr indices; 98 | }; 99 | 100 | typedef nanoflann::KDTreeSingleIndexAdaptor< 101 | nanoflann::SO3_Adaptor , 102 | PointCloud_Adaptor, 3, int> KDTreeFlann_PCL_SO3; 103 | 104 | PointCloud_Adaptor _adaptor; 105 | 106 | KDTreeFlann_PCL_SO3 _kdtree; 107 | 108 | }; 109 | 110 | //---------- Definitions --------------------- 111 | 112 | template inline 113 | KdTreeFLANN::KdTreeFLANN(bool sorted): 114 | _kdtree(3,_adaptor, KDTreeSingleIndexAdaptorParams(100)) 115 | { 116 | _params.sorted = sorted; 117 | } 118 | 119 | template inline 120 | void KdTreeFLANN::setEpsilon(float eps) 121 | { 122 | _params.eps = eps; 123 | } 124 | 125 | template inline 126 | void KdTreeFLANN::setSortedResults(bool sorted) 127 | { 128 | _params.sorted = sorted; 129 | } 130 | 131 | template inline 132 | void KdTreeFLANN::setInputCloud(const KdTreeFLANN::PointCloudConstPtr &cloud, 133 | const IndicesConstPtr &indices) 134 | { 135 | _adaptor.pcl = cloud; 136 | _adaptor.indices = indices; 137 | _kdtree.buildIndex(); 138 | } 139 | 140 | template inline 141 | int KdTreeFLANN::nearestKSearch(const PointT &point, int num_closest, 142 | std::vector &k_indices, 143 | std::vector &k_sqr_distances) const 144 | { 145 | k_indices.resize(num_closest); 146 | k_sqr_distances.resize(num_closest); 147 | 148 | nanoflann::KNNResultSet resultSet(num_closest); 149 | resultSet.init( k_indices.data(), k_sqr_distances.data()); 150 | _kdtree.findNeighbors(resultSet, point.data, nanoflann::SearchParams() ); 151 | return resultSet.size(); 152 | } 153 | 154 | template inline 155 | int KdTreeFLANN::radiusSearch(const PointT &point, double radius, 156 | std::vector &k_indices, 157 | std::vector &k_sqr_distances) const 158 | { 159 | static std::vector > indices_dist; 160 | indices_dist.reserve( 128 ); 161 | 162 | RadiusResultSet resultSet(radius, indices_dist); 163 | const size_t nFound = _kdtree.findNeighbors(resultSet, point.data, _params); 164 | 165 | if (_params.sorted) 166 | std::sort(indices_dist.begin(), indices_dist.end(), IndexDist_Sorter() ); 167 | 168 | k_indices.resize(nFound); 169 | k_sqr_distances.resize(nFound); 170 | for(int i=0; i inline 178 | size_t KdTreeFLANN::PointCloud_Adaptor::kdtree_get_point_count() const { 179 | if( indices ) return indices->size(); 180 | if( pcl) return pcl->points.size(); 181 | return 0; 182 | } 183 | 184 | template inline 185 | float KdTreeFLANN::PointCloud_Adaptor::kdtree_get_pt(const size_t idx, int dim) const{ 186 | const PointT& p = ( indices ) ? pcl->points[(*indices)[idx]] : pcl->points[idx]; 187 | if (dim==0) return p.x; 188 | else if (dim==1) return p.y; 189 | else if (dim==2) return p.z; 190 | else return 0.0; 191 | } 192 | 193 | } 194 | 195 | 196 | #endif 197 | -------------------------------------------------------------------------------- /launch/dlo.launch: -------------------------------------------------------------------------------- 1 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /launch/dlo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Odometry1/Shape1 8 | - /Keyframes1/Shape1 9 | - /Map1/Autocompute Value Bounds1 10 | Splitter Ratio: 0.6235294342041016 11 | Tree Height: 978 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.4960629940032959 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: Map 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.25 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 100; 100; 100 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 1000 54 | Reference Frame: robot/odom 55 | Value: true 56 | - Alpha: 1 57 | Autocompute Intensity Bounds: true 58 | Autocompute Value Bounds: 59 | Max Value: 6.261447906494141 60 | Min Value: -0.6103848814964294 61 | Value: true 62 | Axis: Z 63 | Channel Name: intensity 64 | Class: rviz/PointCloud2 65 | Color: 255; 255; 255 66 | Color Transformer: Intensity 67 | Decay Time: 0 68 | Enabled: true 69 | Invert Rainbow: false 70 | Max Color: 255; 255; 255 71 | Min Color: 0; 0; 0 72 | Name: LiDAR 73 | Position Transformer: XYZ 74 | Queue Size: 10 75 | Selectable: true 76 | Size (Pixels): 2 77 | Size (m): 0.009999999776482582 78 | Style: Points 79 | Topic: /robot/lidar 80 | Unreliable: false 81 | Use Fixed Frame: true 82 | Use rainbow: true 83 | Value: true 84 | - Angle Tolerance: 0.009999999776482582 85 | Class: rviz/Odometry 86 | Covariance: 87 | Orientation: 88 | Alpha: 0.5 89 | Color: 255; 255; 127 90 | Color Style: Unique 91 | Frame: Local 92 | Offset: 1 93 | Scale: 1 94 | Value: true 95 | Position: 96 | Alpha: 0.30000001192092896 97 | Color: 204; 51; 204 98 | Scale: 1 99 | Value: true 100 | Value: false 101 | Enabled: true 102 | Keep: 1 103 | Name: Odometry 104 | Position Tolerance: 0.009999999776482582 105 | Queue Size: 1000 106 | Shape: 107 | Alpha: 1 108 | Axes Length: 1 109 | Axes Radius: 0.25 110 | Color: 255; 25; 0 111 | Head Length: 0.30000001192092896 112 | Head Radius: 0.10000000149011612 113 | Shaft Length: 1 114 | Shaft Radius: 0.05000000074505806 115 | Value: Axes 116 | Topic: /robot/dlo/odom_node/odom 117 | Unreliable: false 118 | Value: true 119 | - Angle Tolerance: 0.10000000149011612 120 | Class: rviz/Odometry 121 | Covariance: 122 | Orientation: 123 | Alpha: 0.5 124 | Color: 255; 255; 127 125 | Color Style: Unique 126 | Frame: Local 127 | Offset: 1 128 | Scale: 1 129 | Value: true 130 | Position: 131 | Alpha: 0.30000001192092896 132 | Color: 204; 51; 204 133 | Scale: 1 134 | Value: true 135 | Value: false 136 | Enabled: true 137 | Keep: 1000 138 | Name: Keyframes 139 | Position Tolerance: 0.10000000149011612 140 | Queue Size: 1000 141 | Shape: 142 | Alpha: 1 143 | Axes Length: 1 144 | Axes Radius: 0.20000000298023224 145 | Color: 255; 25; 0 146 | Head Length: 0.30000001192092896 147 | Head Radius: 0.10000000149011612 148 | Shaft Length: 1 149 | Shaft Radius: 0.05000000074505806 150 | Value: Axes 151 | Topic: /robot/dlo/odom_node/odom/keyframe 152 | Unreliable: false 153 | Value: true 154 | - Alpha: 1 155 | Autocompute Intensity Bounds: true 156 | Autocompute Value Bounds: 157 | Max Value: 6 158 | Min Value: 0 159 | Value: false 160 | Axis: Z 161 | Channel Name: intensity 162 | Class: rviz/PointCloud2 163 | Color: 0; 0; 128 164 | Color Transformer: AxisColor 165 | Decay Time: 0 166 | Enabled: true 167 | Invert Rainbow: false 168 | Max Color: 255; 255; 255 169 | Min Color: 0; 0; 0 170 | Name: Map 171 | Position Transformer: XYZ 172 | Queue Size: 1000 173 | Selectable: true 174 | Size (Pixels): 2 175 | Size (m): 0.05000000074505806 176 | Style: Points 177 | Topic: /robot/dlo/map_node/map 178 | Unreliable: false 179 | Use Fixed Frame: true 180 | Use rainbow: true 181 | Value: true 182 | Enabled: true 183 | Global Options: 184 | Background Color: 64; 64; 64 185 | Default Light: true 186 | Fixed Frame: robot/odom 187 | Frame Rate: 60 188 | Name: root 189 | Tools: 190 | - Class: rviz/Interact 191 | Hide Inactive Objects: true 192 | - Class: rviz/MoveCamera 193 | - Class: rviz/Select 194 | - Class: rviz/FocusCamera 195 | - Class: rviz/Measure 196 | - Class: rviz/SetInitialPose 197 | Theta std deviation: 0.2617993950843811 198 | Topic: /initialpose 199 | X std deviation: 0.5 200 | Y std deviation: 0.5 201 | - Class: rviz/SetGoal 202 | Topic: /move_base_simple/goal 203 | - Class: rviz/PublishPoint 204 | Single click: true 205 | Topic: /clicked_point 206 | Value: true 207 | Views: 208 | Current: 209 | Class: rviz/Orbit 210 | Distance: 46.69748306274414 211 | Enable Stereo Rendering: 212 | Stereo Eye Separation: 0.05999999865889549 213 | Stereo Focal Distance: 1 214 | Swap Stereo Eyes: false 215 | Value: false 216 | Field of View: 0.7853981852531433 217 | Focal Point: 218 | X: 0 219 | Y: 0 220 | Z: 0 221 | Focal Shape Fixed Size: false 222 | Focal Shape Size: 0.05000000074505806 223 | Invert Z Axis: false 224 | Name: Current View 225 | Near Clip Distance: 0.009999999776482582 226 | Pitch: 1.1997969150543213 227 | Target Frame: robot/base_link 228 | Yaw: 4.883605003356934 229 | Saved: ~ 230 | Window Geometry: 231 | Displays: 232 | collapsed: true 233 | Height: 1136 234 | Hide Left Dock: true 235 | Hide Right Dock: true 236 | QMainWindow State: 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 237 | Selection: 238 | collapsed: false 239 | Time: 240 | collapsed: false 241 | Tool Properties: 242 | collapsed: false 243 | Views: 244 | collapsed: true 245 | Width: 1848 246 | X: 72 247 | Y: 27 248 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | direct_lidar_odometry 6 | 1.4.3 7 | Direct LiDAR Odometry: Fast Localization with Dense Point Clouds 8 | 9 | Kenny J. Chen 10 | Brett T. Lopez 11 | 12 | Kenny J. Chen 13 | Brett T. Lopez 14 | 15 | MIT 16 | 17 | catkin 18 | 19 | message_generation 20 | message_runtime 21 | 22 | roscpp 23 | std_msgs 24 | sensor_msgs 25 | geometry_msgs 26 | pcl_ros 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /src/dlo/map.cc: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | #include "dlo/map.h" 11 | 12 | std::atomic dlo::MapNode::abort_(false); 13 | 14 | 15 | /** 16 | * Constructor 17 | **/ 18 | 19 | dlo::MapNode::MapNode(ros::NodeHandle node_handle) : nh(node_handle) { 20 | 21 | this->getParams(); 22 | 23 | this->abort_timer = this->nh.createTimer(ros::Duration(0.01), &dlo::MapNode::abortTimerCB, this); 24 | 25 | if (this->publish_full_map_){ 26 | this->publish_timer = this->nh.createTimer(ros::Duration(this->publish_freq_), &dlo::MapNode::publishTimerCB, this); 27 | } 28 | 29 | this->keyframe_sub = this->nh.subscribe("keyframes", 1, &dlo::MapNode::keyframeCB, this); 30 | this->map_pub = this->nh.advertise("map", 1); 31 | 32 | this->save_pcd_srv = this->nh.advertiseService("save_pcd", &dlo::MapNode::savePcd, this); 33 | 34 | // initialize map 35 | this->dlo_map = pcl::PointCloud::Ptr (new pcl::PointCloud); 36 | 37 | ROS_INFO("DLO Map Node Initialized"); 38 | 39 | } 40 | 41 | 42 | /** 43 | * Destructor 44 | **/ 45 | 46 | dlo::MapNode::~MapNode() {} 47 | 48 | 49 | /** 50 | * Get Params 51 | **/ 52 | 53 | void dlo::MapNode::getParams() { 54 | 55 | ros::param::param("~dlo/odomNode/odom_frame", this->odom_frame, "odom"); 56 | ros::param::param("~dlo/mapNode/publishFullMap", this->publish_full_map_, true); 57 | ros::param::param("~dlo/mapNode/publishFreq", this->publish_freq_, 1.0); 58 | ros::param::param("~dlo/mapNode/leafSize", this->leaf_size_, 0.5); 59 | 60 | // Get Node NS and Remove Leading Character 61 | std::string ns = ros::this_node::getNamespace(); 62 | ns.erase(0,1); 63 | 64 | // Concatenate Frame Name Strings 65 | this->odom_frame = ns + "/" + this->odom_frame; 66 | 67 | } 68 | 69 | 70 | /** 71 | * Start Map Node 72 | **/ 73 | 74 | void dlo::MapNode::start() { 75 | ROS_INFO("Starting DLO Map Node"); 76 | } 77 | 78 | 79 | /** 80 | * Stop Map Node 81 | **/ 82 | 83 | void dlo::MapNode::stop() { 84 | ROS_WARN("Stopping DLO Map Node"); 85 | 86 | // shutdown 87 | ros::shutdown(); 88 | } 89 | 90 | 91 | /** 92 | * Abort Timer Callback 93 | **/ 94 | 95 | void dlo::MapNode::abortTimerCB(const ros::TimerEvent& e) { 96 | if (abort_) { 97 | stop(); 98 | } 99 | } 100 | 101 | 102 | /** 103 | * Publish Timer Callback 104 | **/ 105 | 106 | void dlo::MapNode::publishTimerCB(const ros::TimerEvent& e) { 107 | 108 | if (this->dlo_map->points.size() == this->dlo_map->width * this->dlo_map->height) { 109 | sensor_msgs::PointCloud2 map_ros; 110 | pcl::toROSMsg(*this->dlo_map, map_ros); 111 | map_ros.header.stamp = ros::Time::now(); 112 | map_ros.header.frame_id = this->odom_frame; 113 | this->map_pub.publish(map_ros); 114 | } 115 | 116 | } 117 | 118 | 119 | /** 120 | * Node Callback 121 | **/ 122 | 123 | void dlo::MapNode::keyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe) { 124 | 125 | // convert scan to pcl format 126 | pcl::PointCloud::Ptr keyframe_pcl = pcl::PointCloud::Ptr (new pcl::PointCloud); 127 | pcl::fromROSMsg(*keyframe, *keyframe_pcl); 128 | 129 | // voxel filter 130 | this->voxelgrid.setLeafSize(this->leaf_size_, this->leaf_size_, this->leaf_size_); 131 | this->voxelgrid.setInputCloud(keyframe_pcl); 132 | this->voxelgrid.filter(*keyframe_pcl); 133 | 134 | // save keyframe to map 135 | this->map_stamp = keyframe->header.stamp; 136 | *this->dlo_map += *keyframe_pcl; 137 | 138 | if (!this->publish_full_map_) { 139 | if (keyframe_pcl->points.size() == keyframe_pcl->width * keyframe_pcl->height) { 140 | sensor_msgs::PointCloud2 map_ros; 141 | pcl::toROSMsg(*keyframe_pcl, map_ros); 142 | map_ros.header.stamp = ros::Time::now(); 143 | map_ros.header.frame_id = this->odom_frame; 144 | this->map_pub.publish(map_ros); 145 | } 146 | } 147 | 148 | } 149 | 150 | bool dlo::MapNode::savePcd(direct_lidar_odometry::save_pcd::Request& req, 151 | direct_lidar_odometry::save_pcd::Response& res) { 152 | 153 | pcl::PointCloud::Ptr m = 154 | pcl::PointCloud::Ptr (boost::make_shared>(*this->dlo_map)); 155 | 156 | float leaf_size = req.leaf_size; 157 | std::string p = req.save_path; 158 | 159 | std::cout << std::setprecision(2) << "Saving map to " << p + "/dlo_map.pcd" << "... "; std::cout.flush(); 160 | 161 | // voxelize map 162 | pcl::VoxelGrid vg; 163 | vg.setLeafSize(leaf_size, leaf_size, leaf_size); 164 | vg.setInputCloud(m); 165 | vg.filter(*m); 166 | 167 | // save map 168 | int ret = pcl::io::savePCDFileBinary(p + "/dlo_map.pcd", *m); 169 | res.success = ret == 0; 170 | 171 | if (res.success) { 172 | std::cout << "done" << std::endl; 173 | } else { 174 | std::cout << "failed" << std::endl; 175 | } 176 | 177 | return res.success; 178 | 179 | } 180 | -------------------------------------------------------------------------------- /src/dlo/map_node.cc: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | #include "dlo/map.h" 11 | 12 | void controlC(int sig) { 13 | 14 | dlo::MapNode::abort(); 15 | 16 | } 17 | 18 | int main(int argc, char** argv) { 19 | 20 | ros::init(argc, argv, "dlo_map_node"); 21 | ros::NodeHandle nh("~"); 22 | 23 | signal(SIGTERM, controlC); 24 | sleep(0.5); 25 | 26 | dlo::MapNode node(nh); 27 | ros::AsyncSpinner spinner(1); 28 | spinner.start(); 29 | node.start(); 30 | ros::waitForShutdown(); 31 | 32 | return 0; 33 | 34 | } 35 | -------------------------------------------------------------------------------- /src/dlo/odom.cc: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | #include "dlo/odom.h" 11 | 12 | std::atomic dlo::OdomNode::abort_(false); 13 | 14 | 15 | /** 16 | * Constructor 17 | **/ 18 | 19 | dlo::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { 20 | 21 | this->getParams(); 22 | 23 | this->stop_publish_thread = false; 24 | this->stop_publish_keyframe_thread = false; 25 | this->stop_metrics_thread = false; 26 | this->stop_debug_thread = false; 27 | 28 | this->dlo_initialized = false; 29 | this->imu_calibrated = false; 30 | 31 | this->icp_sub = this->nh.subscribe("pointcloud", 1, &dlo::OdomNode::icpCB, this); 32 | this->imu_sub = this->nh.subscribe("imu", 1, &dlo::OdomNode::imuCB, this); 33 | 34 | this->odom_pub = this->nh.advertise("odom", 1); 35 | this->pose_pub = this->nh.advertise("pose", 1); 36 | this->kf_pub = this->nh.advertise("kfs", 1, true); 37 | this->keyframe_pub = this->nh.advertise("keyframe", 1, true); 38 | this->save_traj_srv = this->nh.advertiseService("save_traj", &dlo::OdomNode::saveTrajectory, this); 39 | 40 | this->odom.pose.pose.position.x = 0.; 41 | this->odom.pose.pose.position.y = 0.; 42 | this->odom.pose.pose.position.z = 0.; 43 | this->odom.pose.pose.orientation.w = 1.; 44 | this->odom.pose.pose.orientation.x = 0.; 45 | this->odom.pose.pose.orientation.y = 0.; 46 | this->odom.pose.pose.orientation.z = 0.; 47 | this->odom.pose.covariance = {0.}; 48 | 49 | this->origin = Eigen::Vector3f(0., 0., 0.); 50 | 51 | this->T = Eigen::Matrix4f::Identity(); 52 | this->T_s2s = Eigen::Matrix4f::Identity(); 53 | this->T_s2s_prev = Eigen::Matrix4f::Identity(); 54 | 55 | this->pose_s2s = Eigen::Vector3f(0., 0., 0.); 56 | this->rotq_s2s = Eigen::Quaternionf(1., 0., 0., 0.); 57 | 58 | this->pose = Eigen::Vector3f(0., 0., 0.); 59 | this->rotq = Eigen::Quaternionf(1., 0., 0., 0.); 60 | 61 | this->imu_SE3 = Eigen::Matrix4f::Identity(); 62 | 63 | this->imu_bias.gyro.x = 0.; 64 | this->imu_bias.gyro.y = 0.; 65 | this->imu_bias.gyro.z = 0.; 66 | this->imu_bias.accel.x = 0.; 67 | this->imu_bias.accel.y = 0.; 68 | this->imu_bias.accel.z = 0.; 69 | 70 | this->imu_meas.stamp = 0.; 71 | this->imu_meas.ang_vel.x = 0.; 72 | this->imu_meas.ang_vel.y = 0.; 73 | this->imu_meas.ang_vel.z = 0.; 74 | this->imu_meas.lin_accel.x = 0.; 75 | this->imu_meas.lin_accel.y = 0.; 76 | this->imu_meas.lin_accel.z = 0.; 77 | 78 | this->imu_buffer.set_capacity(this->imu_buffer_size_); 79 | this->first_imu_time = 0.; 80 | 81 | this->original_scan = pcl::PointCloud::Ptr (new pcl::PointCloud); 82 | this->current_scan = pcl::PointCloud::Ptr (new pcl::PointCloud); 83 | this->current_scan_t = pcl::PointCloud::Ptr (new pcl::PointCloud); 84 | 85 | this->keyframe_cloud = pcl::PointCloud::Ptr (new pcl::PointCloud); 86 | this->keyframes_cloud = pcl::PointCloud::Ptr (new pcl::PointCloud); 87 | this->num_keyframes = 0; 88 | 89 | this->submap_cloud = pcl::PointCloud::Ptr (new pcl::PointCloud); 90 | this->submap_hasChanged = true; 91 | this->submap_kf_idx_prev.clear(); 92 | 93 | this->source_cloud = nullptr; 94 | this->target_cloud = nullptr; 95 | 96 | this->convex_hull.setDimension(3); 97 | this->concave_hull.setDimension(3); 98 | this->concave_hull.setAlpha(this->keyframe_thresh_dist_); 99 | this->concave_hull.setKeepInformation(true); 100 | 101 | this->gicp_s2s.setCorrespondenceRandomness(this->gicps2s_k_correspondences_); 102 | this->gicp_s2s.setMaxCorrespondenceDistance(this->gicps2s_max_corr_dist_); 103 | this->gicp_s2s.setMaximumIterations(this->gicps2s_max_iter_); 104 | this->gicp_s2s.setTransformationEpsilon(this->gicps2s_transformation_ep_); 105 | this->gicp_s2s.setEuclideanFitnessEpsilon(this->gicps2s_euclidean_fitness_ep_); 106 | this->gicp_s2s.setRANSACIterations(this->gicps2s_ransac_iter_); 107 | this->gicp_s2s.setRANSACOutlierRejectionThreshold(this->gicps2s_ransac_inlier_thresh_); 108 | 109 | this->gicp.setCorrespondenceRandomness(this->gicps2m_k_correspondences_); 110 | this->gicp.setMaxCorrespondenceDistance(this->gicps2m_max_corr_dist_); 111 | this->gicp.setMaximumIterations(this->gicps2m_max_iter_); 112 | this->gicp.setTransformationEpsilon(this->gicps2m_transformation_ep_); 113 | this->gicp.setEuclideanFitnessEpsilon(this->gicps2m_euclidean_fitness_ep_); 114 | this->gicp.setRANSACIterations(this->gicps2m_ransac_iter_); 115 | this->gicp.setRANSACOutlierRejectionThreshold(this->gicps2m_ransac_inlier_thresh_); 116 | 117 | pcl::Registration::KdTreeReciprocalPtr temp; 118 | this->gicp_s2s.setSearchMethodSource(temp, true); 119 | this->gicp_s2s.setSearchMethodTarget(temp, true); 120 | this->gicp.setSearchMethodSource(temp, true); 121 | this->gicp.setSearchMethodTarget(temp, true); 122 | 123 | this->crop.setNegative(true); 124 | this->crop.setMin(Eigen::Vector4f(-this->crop_size_, -this->crop_size_, -this->crop_size_, 1.0)); 125 | this->crop.setMax(Eigen::Vector4f(this->crop_size_, this->crop_size_, this->crop_size_, 1.0)); 126 | 127 | this->vf_scan.setLeafSize(this->vf_scan_res_, this->vf_scan_res_, this->vf_scan_res_); 128 | this->vf_submap.setLeafSize(this->vf_submap_res_, this->vf_submap_res_, this->vf_submap_res_); 129 | 130 | this->metrics.spaciousness.push_back(0.); 131 | 132 | // CPU Specs 133 | char CPUBrandString[0x40]; 134 | memset(CPUBrandString, 0, sizeof(CPUBrandString)); 135 | this->cpu_type = ""; 136 | 137 | #ifdef HAS_CPUID 138 | unsigned int CPUInfo[4] = {0,0,0,0}; 139 | __cpuid(0x80000000, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]); 140 | unsigned int nExIds = CPUInfo[0]; 141 | for (unsigned int i = 0x80000000; i <= nExIds; ++i) { 142 | __cpuid(i, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]); 143 | if (i == 0x80000002) 144 | memcpy(CPUBrandString, CPUInfo, sizeof(CPUInfo)); 145 | else if (i == 0x80000003) 146 | memcpy(CPUBrandString + 16, CPUInfo, sizeof(CPUInfo)); 147 | else if (i == 0x80000004) 148 | memcpy(CPUBrandString + 32, CPUInfo, sizeof(CPUInfo)); 149 | } 150 | 151 | this->cpu_type = CPUBrandString; 152 | boost::trim(this->cpu_type); 153 | #endif 154 | 155 | FILE* file; 156 | struct tms timeSample; 157 | char line[128]; 158 | 159 | this->lastCPU = times(&timeSample); 160 | this->lastSysCPU = timeSample.tms_stime; 161 | this->lastUserCPU = timeSample.tms_utime; 162 | 163 | file = fopen("/proc/cpuinfo", "r"); 164 | this->numProcessors = 0; 165 | while(fgets(line, 128, file) != NULL) { 166 | if (strncmp(line, "processor", 9) == 0) this->numProcessors++; 167 | } 168 | fclose(file); 169 | 170 | ROS_INFO("DLO Odom Node Initialized"); 171 | 172 | } 173 | 174 | 175 | /** 176 | * Destructor 177 | **/ 178 | 179 | dlo::OdomNode::~OdomNode() {} 180 | 181 | 182 | 183 | /** 184 | * Odom Node Parameters 185 | **/ 186 | 187 | void dlo::OdomNode::getParams() { 188 | 189 | // Version 190 | ros::param::param("~dlo/version", this->version_, "0.0.0"); 191 | 192 | // Frames 193 | ros::param::param("~dlo/odomNode/odom_frame", this->odom_frame, "odom"); 194 | ros::param::param("~dlo/odomNode/child_frame", this->child_frame, "base_link"); 195 | 196 | // Get Node NS and Remove Leading Character 197 | std::string ns = ros::this_node::getNamespace(); 198 | ns.erase(0,1); 199 | 200 | // Concatenate Frame Name Strings 201 | this->odom_frame = ns + "/" + this->odom_frame; 202 | this->child_frame = ns + "/" + this->child_frame; 203 | 204 | // Gravity alignment 205 | ros::param::param("~dlo/gravityAlign", this->gravity_align_, false); 206 | 207 | // Keyframe Threshold 208 | ros::param::param("~dlo/odomNode/keyframe/threshD", this->keyframe_thresh_dist_, 0.1); 209 | ros::param::param("~dlo/odomNode/keyframe/threshR", this->keyframe_thresh_rot_, 1.0); 210 | 211 | // Submap 212 | ros::param::param("~dlo/odomNode/submap/keyframe/knn", this->submap_knn_, 10); 213 | ros::param::param("~dlo/odomNode/submap/keyframe/kcv", this->submap_kcv_, 10); 214 | ros::param::param("~dlo/odomNode/submap/keyframe/kcc", this->submap_kcc_, 10); 215 | 216 | // Initial Position 217 | ros::param::param("~dlo/odomNode/initialPose/use", this->initial_pose_use_, false); 218 | 219 | double px, py, pz, qx, qy, qz, qw; 220 | ros::param::param("~dlo/odomNode/initialPose/position/x", px, 0.0); 221 | ros::param::param("~dlo/odomNode/initialPose/position/y", py, 0.0); 222 | ros::param::param("~dlo/odomNode/initialPose/position/z", pz, 0.0); 223 | ros::param::param("~dlo/odomNode/initialPose/orientation/w", qw, 1.0); 224 | ros::param::param("~dlo/odomNode/initialPose/orientation/x", qx, 0.0); 225 | ros::param::param("~dlo/odomNode/initialPose/orientation/y", qy, 0.0); 226 | ros::param::param("~dlo/odomNode/initialPose/orientation/z", qz, 0.0); 227 | this->initial_position_ = Eigen::Vector3f(px, py, pz); 228 | this->initial_orientation_ = Eigen::Quaternionf(qw, qx, qy, qz); 229 | 230 | // Crop Box Filter 231 | ros::param::param("~dlo/odomNode/preprocessing/cropBoxFilter/use", this->crop_use_, false); 232 | ros::param::param("~dlo/odomNode/preprocessing/cropBoxFilter/size", this->crop_size_, 1.0); 233 | 234 | // Voxel Grid Filter 235 | ros::param::param("~dlo/odomNode/preprocessing/voxelFilter/scan/use", this->vf_scan_use_, true); 236 | ros::param::param("~dlo/odomNode/preprocessing/voxelFilter/scan/res", this->vf_scan_res_, 0.05); 237 | ros::param::param("~dlo/odomNode/preprocessing/voxelFilter/submap/use", this->vf_submap_use_, false); 238 | ros::param::param("~dlo/odomNode/preprocessing/voxelFilter/submap/res", this->vf_submap_res_, 0.1); 239 | 240 | // Adaptive Parameters 241 | ros::param::param("~dlo/adaptiveParams", this->adaptive_params_use_, false); 242 | 243 | // IMU 244 | ros::param::param("~dlo/imu", this->imu_use_, false); 245 | ros::param::param("~dlo/odomNode/imu/calibTime", this->imu_calib_time_, 3); 246 | ros::param::param("~dlo/odomNode/imu/bufferSize", this->imu_buffer_size_, 2000); 247 | 248 | // GICP 249 | ros::param::param("~dlo/odomNode/gicp/minNumPoints", this->gicp_min_num_points_, 100); 250 | ros::param::param("~dlo/odomNode/gicp/s2s/kCorrespondences", this->gicps2s_k_correspondences_, 20); 251 | ros::param::param("~dlo/odomNode/gicp/s2s/maxCorrespondenceDistance", this->gicps2s_max_corr_dist_, std::sqrt(std::numeric_limits::max())); 252 | ros::param::param("~dlo/odomNode/gicp/s2s/maxIterations", this->gicps2s_max_iter_, 64); 253 | ros::param::param("~dlo/odomNode/gicp/s2s/transformationEpsilon", this->gicps2s_transformation_ep_, 0.0005); 254 | ros::param::param("~dlo/odomNode/gicp/s2s/euclideanFitnessEpsilon", this->gicps2s_euclidean_fitness_ep_, -std::numeric_limits::max()); 255 | ros::param::param("~dlo/odomNode/gicp/s2s/ransac/iterations", this->gicps2s_ransac_iter_, 0); 256 | ros::param::param("~dlo/odomNode/gicp/s2s/ransac/outlierRejectionThresh", this->gicps2s_ransac_inlier_thresh_, 0.05); 257 | ros::param::param("~dlo/odomNode/gicp/s2m/kCorrespondences", this->gicps2m_k_correspondences_, 20); 258 | ros::param::param("~dlo/odomNode/gicp/s2m/maxCorrespondenceDistance", this->gicps2m_max_corr_dist_, std::sqrt(std::numeric_limits::max())); 259 | ros::param::param("~dlo/odomNode/gicp/s2m/maxIterations", this->gicps2m_max_iter_, 64); 260 | ros::param::param("~dlo/odomNode/gicp/s2m/transformationEpsilon", this->gicps2m_transformation_ep_, 0.0005); 261 | ros::param::param("~dlo/odomNode/gicp/s2m/euclideanFitnessEpsilon", this->gicps2m_euclidean_fitness_ep_, -std::numeric_limits::max()); 262 | ros::param::param("~dlo/odomNode/gicp/s2m/ransac/iterations", this->gicps2m_ransac_iter_, 0); 263 | ros::param::param("~dlo/odomNode/gicp/s2m/ransac/outlierRejectionThresh", this->gicps2m_ransac_inlier_thresh_, 0.05); 264 | 265 | } 266 | 267 | 268 | /** 269 | * Start Odom Thread 270 | **/ 271 | 272 | void dlo::OdomNode::start() { 273 | ROS_INFO("Starting DLO Odometry Node"); 274 | 275 | printf("\033[2J\033[1;1H"); 276 | std::cout << std::endl << "==== Direct LiDAR Odometry v" << this->version_ << " ====" << std::endl << std::endl; 277 | 278 | } 279 | 280 | 281 | /** 282 | * Stop Odom Thread 283 | **/ 284 | 285 | void dlo::OdomNode::stop() { 286 | ROS_WARN("Stopping DLO Odometry Node"); 287 | 288 | this->stop_publish_thread = true; 289 | if (this->publish_thread.joinable()) { 290 | this->publish_thread.join(); 291 | } 292 | 293 | this->stop_publish_keyframe_thread = true; 294 | if (this->publish_keyframe_thread.joinable()) { 295 | this->publish_keyframe_thread.join(); 296 | } 297 | 298 | this->stop_metrics_thread = true; 299 | if (this->metrics_thread.joinable()) { 300 | this->metrics_thread.join(); 301 | } 302 | 303 | this->stop_debug_thread = true; 304 | if (this->debug_thread.joinable()) { 305 | this->debug_thread.join(); 306 | } 307 | 308 | ros::shutdown(); 309 | } 310 | 311 | 312 | /** 313 | * Abort Timer Callback 314 | **/ 315 | 316 | void dlo::OdomNode::abortTimerCB(const ros::TimerEvent& e) { 317 | if (abort_) { 318 | stop(); 319 | } 320 | } 321 | 322 | 323 | /** 324 | * Publish to ROS 325 | **/ 326 | 327 | void dlo::OdomNode::publishToROS() { 328 | this->publishPose(); 329 | this->publishTransform(); 330 | } 331 | 332 | 333 | /** 334 | * Publish Pose 335 | **/ 336 | 337 | void dlo::OdomNode::publishPose() { 338 | 339 | // Sign flip check 340 | static Eigen::Quaternionf q_diff{1., 0., 0., 0.}; 341 | static Eigen::Quaternionf q_last{1., 0., 0., 0.}; 342 | 343 | q_diff = q_last.conjugate()*this->rotq; 344 | 345 | // If q_diff has negative real part then there was a sign flip 346 | if (q_diff.w() < 0) { 347 | this->rotq.w() = -this->rotq.w(); 348 | this->rotq.vec() = -this->rotq.vec(); 349 | } 350 | 351 | q_last = this->rotq; 352 | 353 | this->odom.pose.pose.position.x = this->pose[0]; 354 | this->odom.pose.pose.position.y = this->pose[1]; 355 | this->odom.pose.pose.position.z = this->pose[2]; 356 | 357 | this->odom.pose.pose.orientation.w = this->rotq.w(); 358 | this->odom.pose.pose.orientation.x = this->rotq.x(); 359 | this->odom.pose.pose.orientation.y = this->rotq.y(); 360 | this->odom.pose.pose.orientation.z = this->rotq.z(); 361 | 362 | this->odom.header.stamp = this->scan_stamp; 363 | this->odom.header.frame_id = this->odom_frame; 364 | this->odom.child_frame_id = this->child_frame; 365 | this->odom_pub.publish(this->odom); 366 | 367 | this->pose_ros.header.stamp = this->scan_stamp; 368 | this->pose_ros.header.frame_id = this->odom_frame; 369 | 370 | this->pose_ros.pose.position.x = this->pose[0]; 371 | this->pose_ros.pose.position.y = this->pose[1]; 372 | this->pose_ros.pose.position.z = this->pose[2]; 373 | 374 | this->pose_ros.pose.orientation.w = this->rotq.w(); 375 | this->pose_ros.pose.orientation.x = this->rotq.x(); 376 | this->pose_ros.pose.orientation.y = this->rotq.y(); 377 | this->pose_ros.pose.orientation.z = this->rotq.z(); 378 | 379 | this->pose_pub.publish(this->pose_ros); 380 | } 381 | 382 | 383 | /** 384 | * Publish Transform 385 | **/ 386 | 387 | void dlo::OdomNode::publishTransform() { 388 | 389 | static tf2_ros::TransformBroadcaster br; 390 | geometry_msgs::TransformStamped transformStamped; 391 | 392 | transformStamped.header.stamp = this->scan_stamp; 393 | transformStamped.header.frame_id = this->odom_frame; 394 | transformStamped.child_frame_id = this->child_frame; 395 | 396 | transformStamped.transform.translation.x = this->pose[0]; 397 | transformStamped.transform.translation.y = this->pose[1]; 398 | transformStamped.transform.translation.z = this->pose[2]; 399 | 400 | transformStamped.transform.rotation.w = this->rotq.w(); 401 | transformStamped.transform.rotation.x = this->rotq.x(); 402 | transformStamped.transform.rotation.y = this->rotq.y(); 403 | transformStamped.transform.rotation.z = this->rotq.z(); 404 | 405 | br.sendTransform(transformStamped); 406 | 407 | } 408 | 409 | 410 | /** 411 | * Publish Keyframe Pose and Scan 412 | **/ 413 | 414 | void dlo::OdomNode::publishKeyframe() { 415 | 416 | // Publish keyframe pose 417 | this->kf.header.stamp = this->scan_stamp; 418 | this->kf.header.frame_id = this->odom_frame; 419 | this->kf.child_frame_id = this->child_frame; 420 | 421 | this->kf.pose.pose.position.x = this->pose[0]; 422 | this->kf.pose.pose.position.y = this->pose[1]; 423 | this->kf.pose.pose.position.z = this->pose[2]; 424 | 425 | this->kf.pose.pose.orientation.w = this->rotq.w(); 426 | this->kf.pose.pose.orientation.x = this->rotq.x(); 427 | this->kf.pose.pose.orientation.y = this->rotq.y(); 428 | this->kf.pose.pose.orientation.z = this->rotq.z(); 429 | 430 | this->kf_pub.publish(this->kf); 431 | 432 | // Publish keyframe scan 433 | if (this->keyframe_cloud->points.size() == this->keyframe_cloud->width * this->keyframe_cloud->height) { 434 | sensor_msgs::PointCloud2 keyframe_cloud_ros; 435 | pcl::toROSMsg(*this->keyframe_cloud, keyframe_cloud_ros); 436 | keyframe_cloud_ros.header.stamp = this->scan_stamp; 437 | keyframe_cloud_ros.header.frame_id = this->odom_frame; 438 | this->keyframe_pub.publish(keyframe_cloud_ros); 439 | } 440 | 441 | } 442 | 443 | 444 | /** 445 | * Preprocessing 446 | **/ 447 | 448 | void dlo::OdomNode::preprocessPoints() { 449 | 450 | // Original Scan 451 | *this->original_scan = *this->current_scan; 452 | 453 | // Remove NaNs 454 | std::vector idx; 455 | this->current_scan->is_dense = false; 456 | pcl::removeNaNFromPointCloud(*this->current_scan, *this->current_scan, idx); 457 | 458 | // Crop Box Filter 459 | if (this->crop_use_) { 460 | this->crop.setInputCloud(this->current_scan); 461 | this->crop.filter(*this->current_scan); 462 | } 463 | 464 | // Voxel Grid Filter 465 | if (this->vf_scan_use_) { 466 | this->vf_scan.setInputCloud(this->current_scan); 467 | this->vf_scan.filter(*this->current_scan); 468 | } 469 | 470 | } 471 | 472 | 473 | /** 474 | * Initialize Input Target 475 | **/ 476 | 477 | void dlo::OdomNode::initializeInputTarget() { 478 | 479 | this->prev_frame_stamp = this->curr_frame_stamp; 480 | 481 | // Convert ros message 482 | this->target_cloud = pcl::PointCloud::Ptr (new pcl::PointCloud); 483 | this->target_cloud = this->current_scan; 484 | this->gicp_s2s.setInputTarget(this->target_cloud); 485 | this->gicp_s2s.calculateTargetCovariances(); 486 | 487 | // initialize keyframes 488 | pcl::PointCloud::Ptr first_keyframe (new pcl::PointCloud); 489 | pcl::transformPointCloud (*this->target_cloud, *first_keyframe, this->T); 490 | 491 | // voxelization for submap 492 | if (this->vf_submap_use_) { 493 | this->vf_submap.setInputCloud(first_keyframe); 494 | this->vf_submap.filter(*first_keyframe); 495 | } 496 | 497 | // keep history of keyframes 498 | this->keyframes.push_back(std::make_pair(std::make_pair(this->pose, this->rotq), first_keyframe)); 499 | *this->keyframes_cloud += *first_keyframe; 500 | *this->keyframe_cloud = *first_keyframe; 501 | 502 | // compute kdtree and keyframe normals (use gicp_s2s input source as temporary storage because it will be overwritten by setInputSources()) 503 | this->gicp_s2s.setInputSource(this->keyframe_cloud); 504 | this->gicp_s2s.calculateSourceCovariances(); 505 | this->keyframe_normals.push_back(this->gicp_s2s.getSourceCovariances()); 506 | 507 | this->publish_keyframe_thread = std::thread( &dlo::OdomNode::publishKeyframe, this ); 508 | this->publish_keyframe_thread.detach(); 509 | 510 | ++this->num_keyframes; 511 | 512 | } 513 | 514 | 515 | /** 516 | * Set Input Sources 517 | **/ 518 | 519 | void dlo::OdomNode::setInputSources(){ 520 | 521 | // set the input source for the S2S gicp 522 | // this builds the KdTree of the source cloud 523 | // this does not build the KdTree for s2m because force_no_update is true 524 | this->gicp_s2s.setInputSource(this->current_scan); 525 | 526 | // set pcl::Registration input source for S2M gicp using custom NanoGICP function 527 | this->gicp.registerInputSource(this->current_scan); 528 | 529 | // now set the KdTree of S2M gicp using previously built KdTree 530 | this->gicp.source_kdtree_ = this->gicp_s2s.source_kdtree_; 531 | this->gicp.source_covs_.clear(); 532 | 533 | } 534 | 535 | 536 | /** 537 | * Gravity Alignment 538 | **/ 539 | 540 | void dlo::OdomNode::gravityAlign() { 541 | 542 | // get average acceleration vector for 1 second and normalize 543 | Eigen::Vector3f lin_accel = Eigen::Vector3f::Zero(); 544 | const double then = ros::Time::now().toSec(); 545 | int n=0; 546 | while ((ros::Time::now().toSec() - then) < 1.) { 547 | lin_accel[0] += this->imu_meas.lin_accel.x; 548 | lin_accel[1] += this->imu_meas.lin_accel.y; 549 | lin_accel[2] += this->imu_meas.lin_accel.z; 550 | ++n; 551 | } 552 | lin_accel[0] /= n; lin_accel[1] /= n; lin_accel[2] /= n; 553 | 554 | // normalize 555 | double lin_norm = sqrt(pow(lin_accel[0], 2) + pow(lin_accel[1], 2) + pow(lin_accel[2], 2)); 556 | lin_accel[0] /= lin_norm; lin_accel[1] /= lin_norm; lin_accel[2] /= lin_norm; 557 | 558 | // define gravity vector (assume point downwards) 559 | Eigen::Vector3f grav; 560 | grav << 0, 0, 1; 561 | 562 | // calculate angle between the two vectors 563 | Eigen::Quaternionf grav_q = Eigen::Quaternionf::FromTwoVectors(lin_accel, grav); 564 | 565 | // normalize 566 | double grav_norm = sqrt(grav_q.w()*grav_q.w() + grav_q.x()*grav_q.x() + grav_q.y()*grav_q.y() + grav_q.z()*grav_q.z()); 567 | grav_q.w() /= grav_norm; grav_q.x() /= grav_norm; grav_q.y() /= grav_norm; grav_q.z() /= grav_norm; 568 | 569 | // set gravity aligned orientation 570 | this->rotq = grav_q; 571 | this->T.block(0,0,3,3) = this->rotq.toRotationMatrix(); 572 | this->T_s2s.block(0,0,3,3) = this->rotq.toRotationMatrix(); 573 | this->T_s2s_prev.block(0,0,3,3) = this->rotq.toRotationMatrix(); 574 | 575 | // rpy 576 | auto euler = grav_q.toRotationMatrix().eulerAngles(2, 1, 0); 577 | double yaw = euler[0] * (180.0/M_PI); 578 | double pitch = euler[1] * (180.0/M_PI); 579 | double roll = euler[2] * (180.0/M_PI); 580 | 581 | std::cout << "done" << std::endl; 582 | std::cout << " Roll [deg]: " << roll << std::endl; 583 | std::cout << " Pitch [deg]: " << pitch << std::endl << std::endl; 584 | } 585 | 586 | 587 | /** 588 | * Initialize 6DOF 589 | **/ 590 | 591 | void dlo::OdomNode::initializeDLO() { 592 | 593 | // Calibrate IMU 594 | if (!this->imu_calibrated && this->imu_use_) { 595 | return; 596 | } 597 | 598 | // Gravity Align 599 | if (this->gravity_align_ && this->imu_use_ && this->imu_calibrated && !this->initial_pose_use_) { 600 | std::cout << "Aligning to gravity... "; std::cout.flush(); 601 | this->gravityAlign(); 602 | } 603 | 604 | // Use initial known pose 605 | if (this->initial_pose_use_) { 606 | std::cout << "Setting known initial pose... "; std::cout.flush(); 607 | 608 | // set known position 609 | this->pose = this->initial_position_; 610 | this->T.block(0,3,3,1) = this->pose; 611 | this->T_s2s.block(0,3,3,1) = this->pose; 612 | this->T_s2s_prev.block(0,3,3,1) = this->pose; 613 | this->origin = this->initial_position_; 614 | 615 | // set known orientation 616 | this->rotq = this->initial_orientation_; 617 | this->T.block(0,0,3,3) = this->rotq.toRotationMatrix(); 618 | this->T_s2s.block(0,0,3,3) = this->rotq.toRotationMatrix(); 619 | this->T_s2s_prev.block(0,0,3,3) = this->rotq.toRotationMatrix(); 620 | 621 | std::cout << "done" << std::endl << std::endl; 622 | } 623 | 624 | this->dlo_initialized = true; 625 | std::cout << "DLO initialized! Starting localization..." << std::endl; 626 | 627 | } 628 | 629 | 630 | /** 631 | * ICP Point Cloud Callback 632 | **/ 633 | 634 | void dlo::OdomNode::icpCB(const sensor_msgs::PointCloud2ConstPtr& pc) { 635 | 636 | double then = ros::Time::now().toSec(); 637 | this->scan_stamp = pc->header.stamp; 638 | this->curr_frame_stamp = pc->header.stamp.toSec(); 639 | 640 | // If there are too few points in the pointcloud, try again 641 | this->current_scan = pcl::PointCloud::Ptr (new pcl::PointCloud); 642 | pcl::fromROSMsg(*pc, *this->current_scan); 643 | if (this->current_scan->points.size() < this->gicp_min_num_points_) { 644 | ROS_WARN("Low number of points!"); 645 | return; 646 | } 647 | 648 | // DLO Initialization procedures (IMU calib, gravity align) 649 | if (!this->dlo_initialized) { 650 | this->initializeDLO(); 651 | return; 652 | } 653 | 654 | // Preprocess points 655 | this->preprocessPoints(); 656 | 657 | // Compute Metrics 658 | this->metrics_thread = std::thread( &dlo::OdomNode::computeMetrics, this ); 659 | this->metrics_thread.detach(); 660 | 661 | // Set Adaptive Parameters 662 | if (this->adaptive_params_use_){ 663 | this->setAdaptiveParams(); 664 | } 665 | 666 | // Set initial frame as target 667 | if(this->target_cloud == nullptr) { 668 | this->initializeInputTarget(); 669 | return; 670 | } 671 | 672 | // Set source frame 673 | this->source_cloud = pcl::PointCloud::Ptr (new pcl::PointCloud); 674 | this->source_cloud = this->current_scan; 675 | 676 | // Set new frame as input source for both gicp objects 677 | this->setInputSources(); 678 | 679 | // Get the next pose via IMU + S2S + S2M 680 | this->getNextPose(); 681 | 682 | // Update current keyframe poses and map 683 | this->updateKeyframes(); 684 | 685 | // Update trajectory 686 | this->trajectory.push_back( std::make_pair(this->pose, this->rotq) ); 687 | 688 | // Update next time stamp 689 | this->prev_frame_stamp = this->curr_frame_stamp; 690 | 691 | // Update some statistics 692 | this->comp_times.push_back(ros::Time::now().toSec() - then); 693 | 694 | // Publish stuff to ROS 695 | this->publish_thread = std::thread( &dlo::OdomNode::publishToROS, this ); 696 | this->publish_thread.detach(); 697 | 698 | // Debug statements and publish custom DLO message 699 | this->debug_thread = std::thread( &dlo::OdomNode::debug, this ); 700 | this->debug_thread.detach(); 701 | 702 | } 703 | 704 | 705 | /** 706 | * IMU Callback 707 | **/ 708 | 709 | void dlo::OdomNode::imuCB(const sensor_msgs::Imu::ConstPtr& imu) { 710 | 711 | if (!this->imu_use_) { 712 | return; 713 | } 714 | 715 | double ang_vel[3], lin_accel[3]; 716 | 717 | // Get IMU samples 718 | ang_vel[0] = imu->angular_velocity.x; 719 | ang_vel[1] = imu->angular_velocity.y; 720 | ang_vel[2] = imu->angular_velocity.z; 721 | 722 | lin_accel[0] = imu->linear_acceleration.x; 723 | lin_accel[1] = imu->linear_acceleration.y; 724 | lin_accel[2] = imu->linear_acceleration.z; 725 | 726 | if (this->first_imu_time == 0.) { 727 | this->first_imu_time = imu->header.stamp.toSec(); 728 | } 729 | 730 | // IMU calibration procedure - do for three seconds 731 | if (!this->imu_calibrated) { 732 | 733 | static int num_samples = 0; 734 | static bool print = true; 735 | 736 | if ((imu->header.stamp.toSec() - this->first_imu_time) < this->imu_calib_time_) { 737 | 738 | num_samples++; 739 | 740 | this->imu_bias.gyro.x += ang_vel[0]; 741 | this->imu_bias.gyro.y += ang_vel[1]; 742 | this->imu_bias.gyro.z += ang_vel[2]; 743 | 744 | this->imu_bias.accel.x += lin_accel[0]; 745 | this->imu_bias.accel.y += lin_accel[1]; 746 | this->imu_bias.accel.z += lin_accel[2]; 747 | 748 | if(print) { 749 | std::cout << "Calibrating IMU for " << this->imu_calib_time_ << " seconds... "; std::cout.flush(); 750 | print = false; 751 | } 752 | 753 | } else { 754 | 755 | this->imu_bias.gyro.x /= num_samples; 756 | this->imu_bias.gyro.y /= num_samples; 757 | this->imu_bias.gyro.z /= num_samples; 758 | 759 | this->imu_bias.accel.x /= num_samples; 760 | this->imu_bias.accel.y /= num_samples; 761 | this->imu_bias.accel.z /= num_samples; 762 | 763 | this->imu_calibrated = true; 764 | 765 | std::cout << "done" << std::endl; 766 | std::cout << " Gyro biases [xyz]: " << this->imu_bias.gyro.x << ", " << this->imu_bias.gyro.y << ", " << this->imu_bias.gyro.z << std::endl << std::endl; 767 | 768 | } 769 | 770 | } else { 771 | 772 | // Apply the calibrated bias to the new IMU measurements 773 | this->imu_meas.stamp = imu->header.stamp.toSec(); 774 | 775 | this->imu_meas.ang_vel.x = ang_vel[0] - this->imu_bias.gyro.x; 776 | this->imu_meas.ang_vel.y = ang_vel[1] - this->imu_bias.gyro.y; 777 | this->imu_meas.ang_vel.z = ang_vel[2] - this->imu_bias.gyro.z; 778 | 779 | this->imu_meas.lin_accel.x = lin_accel[0]; 780 | this->imu_meas.lin_accel.y = lin_accel[1]; 781 | this->imu_meas.lin_accel.z = lin_accel[2]; 782 | 783 | // Store into circular buffer 784 | this->mtx_imu.lock(); 785 | this->imu_buffer.push_front(this->imu_meas); 786 | this->mtx_imu.unlock(); 787 | 788 | } 789 | 790 | } 791 | 792 | 793 | /** 794 | * Get Next Pose 795 | **/ 796 | 797 | void dlo::OdomNode::getNextPose() { 798 | 799 | // 800 | // FRAME-TO-FRAME PROCEDURE 801 | // 802 | 803 | // Align using IMU prior if available 804 | pcl::PointCloud::Ptr aligned (new pcl::PointCloud); 805 | 806 | if (this->imu_use_) { 807 | this->integrateIMU(); 808 | this->gicp_s2s.align(*aligned, this->imu_SE3); 809 | } else { 810 | this->gicp_s2s.align(*aligned); 811 | } 812 | 813 | // Get the local S2S transform 814 | Eigen::Matrix4f T_S2S = this->gicp_s2s.getFinalTransformation(); 815 | 816 | // Get the global S2S transform 817 | this->propagateS2S(T_S2S); 818 | 819 | // reuse covariances from s2s for s2m 820 | this->gicp.source_covs_ = this->gicp_s2s.source_covs_; 821 | 822 | // Swap source and target (which also swaps KdTrees internally) for next S2S 823 | this->gicp_s2s.swapSourceAndTarget(); 824 | 825 | // 826 | // FRAME-TO-SUBMAP 827 | // 828 | 829 | // Get current global submap 830 | this->getSubmapKeyframes(); 831 | 832 | if (this->submap_hasChanged) { 833 | 834 | // Set the current global submap as the target cloud 835 | this->gicp.setInputTarget(this->submap_cloud); 836 | 837 | // Set target cloud's normals as submap normals 838 | this->gicp.setTargetCovariances( this->submap_normals ); 839 | } 840 | 841 | // Align with current submap with global S2S transformation as initial guess 842 | this->gicp.align(*aligned, this->T_s2s); 843 | 844 | // Get final transformation in global frame 845 | this->T = this->gicp.getFinalTransformation(); 846 | 847 | // Update the S2S transform for next propagation 848 | this->T_s2s_prev = this->T; 849 | 850 | // Update next global pose 851 | // Both source and target clouds are in the global frame now, so tranformation is global 852 | this->propagateS2M(); 853 | 854 | // Set next target cloud as current source cloud 855 | *this->target_cloud = *this->source_cloud; 856 | 857 | } 858 | 859 | 860 | /** 861 | * Integrate IMU 862 | **/ 863 | 864 | void dlo::OdomNode::integrateIMU() { 865 | 866 | // Extract IMU data between the two frames 867 | std::vector imu_frame; 868 | 869 | for (const auto& i : this->imu_buffer) { 870 | 871 | // IMU data between two frames is when: 872 | // current frame's timestamp minus imu timestamp is positive 873 | // previous frame's timestamp minus imu timestamp is negative 874 | double curr_frame_imu_dt = this->curr_frame_stamp - i.stamp; 875 | double prev_frame_imu_dt = this->prev_frame_stamp - i.stamp; 876 | 877 | if (curr_frame_imu_dt >= 0. && prev_frame_imu_dt <= 0.) { 878 | 879 | imu_frame.push_back(i); 880 | 881 | } 882 | 883 | } 884 | 885 | // Sort measurements by time 886 | std::sort(imu_frame.begin(), imu_frame.end(), this->comparatorImu); 887 | 888 | // Relative IMU integration of gyro and accelerometer 889 | double curr_imu_stamp = 0.; 890 | double prev_imu_stamp = 0.; 891 | double dt; 892 | 893 | Eigen::Quaternionf q = Eigen::Quaternionf::Identity(); 894 | 895 | for (uint32_t i = 0; i < imu_frame.size(); ++i) { 896 | 897 | if (prev_imu_stamp == 0.) { 898 | prev_imu_stamp = imu_frame[i].stamp; 899 | continue; 900 | } 901 | 902 | // Calculate difference in imu measurement times IN SECONDS 903 | curr_imu_stamp = imu_frame[i].stamp; 904 | dt = curr_imu_stamp - prev_imu_stamp; 905 | prev_imu_stamp = curr_imu_stamp; 906 | 907 | // Relative gyro propagation quaternion dynamics 908 | Eigen::Quaternionf qq = q; 909 | q.w() -= 0.5*( qq.x()*imu_frame[i].ang_vel.x + qq.y()*imu_frame[i].ang_vel.y + qq.z()*imu_frame[i].ang_vel.z ) * dt; 910 | q.x() += 0.5*( qq.w()*imu_frame[i].ang_vel.x - qq.z()*imu_frame[i].ang_vel.y + qq.y()*imu_frame[i].ang_vel.z ) * dt; 911 | q.y() += 0.5*( qq.z()*imu_frame[i].ang_vel.x + qq.w()*imu_frame[i].ang_vel.y - qq.x()*imu_frame[i].ang_vel.z ) * dt; 912 | q.z() += 0.5*( qq.x()*imu_frame[i].ang_vel.y - qq.y()*imu_frame[i].ang_vel.x + qq.w()*imu_frame[i].ang_vel.z ) * dt; 913 | 914 | } 915 | 916 | // Normalize quaternion 917 | double norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z()); 918 | q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm; 919 | 920 | // Store IMU guess 921 | this->imu_SE3 = Eigen::Matrix4f::Identity(); 922 | this->imu_SE3.block(0, 0, 3, 3) = q.toRotationMatrix(); 923 | 924 | } 925 | 926 | 927 | /** 928 | * Propagate S2S Alignment 929 | **/ 930 | 931 | void dlo::OdomNode::propagateS2S(Eigen::Matrix4f T) { 932 | 933 | this->T_s2s = this->T_s2s_prev * T; 934 | this->T_s2s_prev = this->T_s2s; 935 | 936 | this->pose_s2s << this->T_s2s(0,3), this->T_s2s(1,3), this->T_s2s(2,3); 937 | this->rotSO3_s2s << this->T_s2s(0,0), this->T_s2s(0,1), this->T_s2s(0,2), 938 | this->T_s2s(1,0), this->T_s2s(1,1), this->T_s2s(1,2), 939 | this->T_s2s(2,0), this->T_s2s(2,1), this->T_s2s(2,2); 940 | 941 | Eigen::Quaternionf q(this->rotSO3_s2s); 942 | 943 | // Normalize quaternion 944 | double norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z()); 945 | q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm; 946 | this->rotq_s2s = q; 947 | 948 | } 949 | 950 | 951 | /** 952 | * Propagate S2M Alignment 953 | **/ 954 | 955 | void dlo::OdomNode::propagateS2M() { 956 | 957 | this->pose << this->T(0,3), this->T(1,3), this->T(2,3); 958 | this->rotSO3 << this->T(0,0), this->T(0,1), this->T(0,2), 959 | this->T(1,0), this->T(1,1), this->T(1,2), 960 | this->T(2,0), this->T(2,1), this->T(2,2); 961 | 962 | Eigen::Quaternionf q(this->rotSO3); 963 | 964 | // Normalize quaternion 965 | double norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z()); 966 | q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm; 967 | this->rotq = q; 968 | 969 | } 970 | 971 | 972 | /** 973 | * Transform Current Scan 974 | **/ 975 | 976 | void dlo::OdomNode::transformCurrentScan() { 977 | this->current_scan_t = pcl::PointCloud::Ptr (new pcl::PointCloud); 978 | pcl::transformPointCloud (*this->current_scan, *this->current_scan_t, this->T); 979 | } 980 | 981 | 982 | /** 983 | * Compute Metrics 984 | **/ 985 | 986 | void dlo::OdomNode::computeMetrics() { 987 | this->computeSpaciousness(); 988 | } 989 | 990 | 991 | /** 992 | * Compute Spaciousness of Current Scan 993 | **/ 994 | 995 | void dlo::OdomNode::computeSpaciousness() { 996 | 997 | // compute range of points 998 | std::vector ds; 999 | 1000 | for (int i = 0; i <= this->current_scan->points.size(); i++) { 1001 | float d = std::sqrt(pow(this->current_scan->points[i].x, 2) + pow(this->current_scan->points[i].y, 2) + pow(this->current_scan->points[i].z, 2)); 1002 | ds.push_back(d); 1003 | } 1004 | 1005 | // median 1006 | std::nth_element(ds.begin(), ds.begin() + ds.size()/2, ds.end()); 1007 | float median_curr = ds[ds.size()/2]; 1008 | static float median_prev = median_curr; 1009 | float median_lpf = 0.95*median_prev + 0.05*median_curr; 1010 | median_prev = median_lpf; 1011 | 1012 | // push 1013 | this->metrics.spaciousness.push_back( median_lpf ); 1014 | 1015 | } 1016 | 1017 | 1018 | /** 1019 | * Convex Hull of Keyframes 1020 | **/ 1021 | 1022 | void dlo::OdomNode::computeConvexHull() { 1023 | 1024 | // at least 4 keyframes for convex hull 1025 | if (this->num_keyframes < 4) { 1026 | return; 1027 | } 1028 | 1029 | // create a pointcloud with points at keyframes 1030 | pcl::PointCloud::Ptr cloud = pcl::PointCloud::Ptr (new pcl::PointCloud); 1031 | 1032 | for (const auto& k : this->keyframes) { 1033 | PointType pt; 1034 | pt.x = k.first.first[0]; 1035 | pt.y = k.first.first[1]; 1036 | pt.z = k.first.first[2]; 1037 | cloud->push_back(pt); 1038 | } 1039 | 1040 | // calculate the convex hull of the point cloud 1041 | this->convex_hull.setInputCloud(cloud); 1042 | 1043 | // get the indices of the keyframes on the convex hull 1044 | pcl::PointCloud::Ptr convex_points = pcl::PointCloud::Ptr (new pcl::PointCloud); 1045 | this->convex_hull.reconstruct(*convex_points); 1046 | 1047 | pcl::PointIndices::Ptr convex_hull_point_idx = pcl::PointIndices::Ptr (new pcl::PointIndices); 1048 | this->convex_hull.getHullPointIndices(*convex_hull_point_idx); 1049 | 1050 | this->keyframe_convex.clear(); 1051 | for (int i=0; iindices.size(); ++i) { 1052 | this->keyframe_convex.push_back(convex_hull_point_idx->indices[i]); 1053 | } 1054 | 1055 | } 1056 | 1057 | 1058 | /** 1059 | * Concave Hull of Keyframes 1060 | **/ 1061 | 1062 | void dlo::OdomNode::computeConcaveHull() { 1063 | 1064 | // at least 5 keyframes for concave hull 1065 | if (this->num_keyframes < 5) { 1066 | return; 1067 | } 1068 | 1069 | // create a pointcloud with points at keyframes 1070 | pcl::PointCloud::Ptr cloud = pcl::PointCloud::Ptr (new pcl::PointCloud); 1071 | 1072 | for (const auto& k : this->keyframes) { 1073 | PointType pt; 1074 | pt.x = k.first.first[0]; 1075 | pt.y = k.first.first[1]; 1076 | pt.z = k.first.first[2]; 1077 | cloud->push_back(pt); 1078 | } 1079 | 1080 | // calculate the concave hull of the point cloud 1081 | this->concave_hull.setInputCloud(cloud); 1082 | 1083 | // get the indices of the keyframes on the concave hull 1084 | pcl::PointCloud::Ptr concave_points = pcl::PointCloud::Ptr (new pcl::PointCloud); 1085 | this->concave_hull.reconstruct(*concave_points); 1086 | 1087 | pcl::PointIndices::Ptr concave_hull_point_idx = pcl::PointIndices::Ptr (new pcl::PointIndices); 1088 | this->concave_hull.getHullPointIndices(*concave_hull_point_idx); 1089 | 1090 | this->keyframe_concave.clear(); 1091 | for (int i=0; iindices.size(); ++i) { 1092 | this->keyframe_concave.push_back(concave_hull_point_idx->indices[i]); 1093 | } 1094 | 1095 | } 1096 | 1097 | 1098 | /** 1099 | * Update keyframes 1100 | **/ 1101 | 1102 | void dlo::OdomNode::updateKeyframes() { 1103 | 1104 | // transform point cloud 1105 | this->transformCurrentScan(); 1106 | 1107 | // calculate difference in pose and rotation to all poses in trajectory 1108 | float closest_d = std::numeric_limits::infinity(); 1109 | int closest_idx = 0; 1110 | int keyframes_idx = 0; 1111 | 1112 | int num_nearby = 0; 1113 | 1114 | for (const auto& k : this->keyframes) { 1115 | 1116 | // calculate distance between current pose and pose in keyframes 1117 | float delta_d = sqrt( pow(this->pose[0] - k.first.first[0], 2) + pow(this->pose[1] - k.first.first[1], 2) + pow(this->pose[2] - k.first.first[2], 2) ); 1118 | 1119 | // count the number nearby current pose 1120 | if (delta_d <= this->keyframe_thresh_dist_ * 1.5){ 1121 | ++num_nearby; 1122 | } 1123 | 1124 | // store into variable 1125 | if (delta_d < closest_d) { 1126 | closest_d = delta_d; 1127 | closest_idx = keyframes_idx; 1128 | } 1129 | 1130 | keyframes_idx++; 1131 | 1132 | } 1133 | 1134 | // get closest pose and corresponding rotation 1135 | Eigen::Vector3f closest_pose = this->keyframes[closest_idx].first.first; 1136 | Eigen::Quaternionf closest_pose_r = this->keyframes[closest_idx].first.second; 1137 | 1138 | // calculate distance between current pose and closest pose from above 1139 | float dd = sqrt( pow(this->pose[0] - closest_pose[0], 2) + pow(this->pose[1] - closest_pose[1], 2) + pow(this->pose[2] - closest_pose[2], 2) ); 1140 | 1141 | // calculate difference in orientation 1142 | Eigen::Quaternionf dq = this->rotq * (closest_pose_r.inverse()); 1143 | 1144 | float theta_rad = 2. * atan2(sqrt( pow(dq.x(), 2) + pow(dq.y(), 2) + pow(dq.z(), 2) ), dq.w()); 1145 | float theta_deg = theta_rad * (180.0/M_PI); 1146 | 1147 | // update keyframe 1148 | bool newKeyframe = false; 1149 | 1150 | if (abs(dd) > this->keyframe_thresh_dist_ || abs(theta_deg) > this->keyframe_thresh_rot_) { 1151 | newKeyframe = true; 1152 | } 1153 | if (abs(dd) <= this->keyframe_thresh_dist_) { 1154 | newKeyframe = false; 1155 | } 1156 | if (abs(dd) <= this->keyframe_thresh_dist_ && abs(theta_deg) > this->keyframe_thresh_rot_ && num_nearby <= 1) { 1157 | newKeyframe = true; 1158 | } 1159 | 1160 | if (newKeyframe) { 1161 | 1162 | ++this->num_keyframes; 1163 | 1164 | // voxelization for submap 1165 | if (this->vf_submap_use_) { 1166 | this->vf_submap.setInputCloud(this->current_scan_t); 1167 | this->vf_submap.filter(*this->current_scan_t); 1168 | } 1169 | 1170 | // update keyframe vector 1171 | this->keyframes.push_back(std::make_pair(std::make_pair(this->pose, this->rotq), this->current_scan_t)); 1172 | 1173 | // compute kdtree and keyframe normals (use gicp_s2s input source as temporary storage because it will be overwritten by setInputSources()) 1174 | *this->keyframes_cloud += *this->current_scan_t; 1175 | *this->keyframe_cloud = *this->current_scan_t; 1176 | 1177 | this->gicp_s2s.setInputSource(this->keyframe_cloud); 1178 | this->gicp_s2s.calculateSourceCovariances(); 1179 | this->keyframe_normals.push_back(this->gicp_s2s.getSourceCovariances()); 1180 | 1181 | this->publish_keyframe_thread = std::thread( &dlo::OdomNode::publishKeyframe, this ); 1182 | this->publish_keyframe_thread.detach(); 1183 | 1184 | } 1185 | 1186 | } 1187 | 1188 | 1189 | /** 1190 | * Set Adaptive Parameters 1191 | **/ 1192 | 1193 | void dlo::OdomNode::setAdaptiveParams() { 1194 | 1195 | // Set Keyframe Thresh from Spaciousness Metric 1196 | if (this->metrics.spaciousness.back() > 20.0){ 1197 | this->keyframe_thresh_dist_ = 10.0; 1198 | } else if (this->metrics.spaciousness.back() > 10.0 && this->metrics.spaciousness.back() <= 20.0) { 1199 | this->keyframe_thresh_dist_ = 5.0; 1200 | } else if (this->metrics.spaciousness.back() > 5.0 && this->metrics.spaciousness.back() <= 10.0) { 1201 | this->keyframe_thresh_dist_ = 1.0; 1202 | } else if (this->metrics.spaciousness.back() <= 5.0) { 1203 | this->keyframe_thresh_dist_ = 0.5; 1204 | } 1205 | 1206 | // set concave hull alpha 1207 | this->concave_hull.setAlpha(this->keyframe_thresh_dist_); 1208 | 1209 | } 1210 | 1211 | 1212 | /** 1213 | * Push Submap Keyframe Indices 1214 | **/ 1215 | void dlo::OdomNode::pushSubmapIndices(std::vector dists, int k, std::vector frames) { 1216 | 1217 | // make sure dists is not empty 1218 | if (!dists.size()) { return; } 1219 | 1220 | // maintain max heap of at most k elements 1221 | std::priority_queue pq; 1222 | 1223 | for (auto d : dists) { 1224 | if (pq.size() >= k && pq.top() > d) { 1225 | pq.push(d); 1226 | pq.pop(); 1227 | } else if (pq.size() < k) { 1228 | pq.push(d); 1229 | } 1230 | } 1231 | 1232 | // get the kth smallest element, which should be at the top of the heap 1233 | float kth_element = pq.top(); 1234 | 1235 | // get all elements smaller or equal to the kth smallest element 1236 | for (int i = 0; i < dists.size(); ++i) { 1237 | if (dists[i] <= kth_element) 1238 | this->submap_kf_idx_curr.push_back(frames[i]); 1239 | } 1240 | 1241 | } 1242 | 1243 | 1244 | /** 1245 | * Get Submap using Nearest Neighbor Keyframes 1246 | **/ 1247 | 1248 | void dlo::OdomNode::getSubmapKeyframes() { 1249 | 1250 | // clear vector of keyframe indices to use for submap 1251 | this->submap_kf_idx_curr.clear(); 1252 | 1253 | // 1254 | // TOP K NEAREST NEIGHBORS FROM ALL KEYFRAMES 1255 | // 1256 | 1257 | // calculate distance between current pose and poses in keyframe set 1258 | std::vector ds; 1259 | std::vector keyframe_nn; int i=0; 1260 | Eigen::Vector3f curr_pose = this->T_s2s.block(0,3,3,1); 1261 | 1262 | for (const auto& k : this->keyframes) { 1263 | float d = sqrt( pow(curr_pose[0] - k.first.first[0], 2) + pow(curr_pose[1] - k.first.first[1], 2) + pow(curr_pose[2] - k.first.first[2], 2) ); 1264 | ds.push_back(d); 1265 | keyframe_nn.push_back(i); i++; 1266 | } 1267 | 1268 | // get indices for top K nearest neighbor keyframe poses 1269 | this->pushSubmapIndices(ds, this->submap_knn_, keyframe_nn); 1270 | 1271 | // 1272 | // TOP K NEAREST NEIGHBORS FROM CONVEX HULL 1273 | // 1274 | 1275 | // get convex hull indices 1276 | this->computeConvexHull(); 1277 | 1278 | // get distances for each keyframe on convex hull 1279 | std::vector convex_ds; 1280 | for (const auto& c : this->keyframe_convex) { 1281 | convex_ds.push_back(ds[c]); 1282 | } 1283 | 1284 | // get indicies for top kNN for convex hull 1285 | this->pushSubmapIndices(convex_ds, this->submap_kcv_, this->keyframe_convex); 1286 | 1287 | // 1288 | // TOP K NEAREST NEIGHBORS FROM CONCAVE HULL 1289 | // 1290 | 1291 | // get concave hull indices 1292 | this->computeConcaveHull(); 1293 | 1294 | // get distances for each keyframe on concave hull 1295 | std::vector concave_ds; 1296 | for (const auto& c : this->keyframe_concave) { 1297 | concave_ds.push_back(ds[c]); 1298 | } 1299 | 1300 | // get indicies for top kNN for convex hull 1301 | this->pushSubmapIndices(concave_ds, this->submap_kcc_, this->keyframe_concave); 1302 | 1303 | // 1304 | // BUILD SUBMAP 1305 | // 1306 | 1307 | // concatenate all submap clouds and normals 1308 | std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); 1309 | auto last = std::unique(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); 1310 | this->submap_kf_idx_curr.erase(last, this->submap_kf_idx_curr.end()); 1311 | 1312 | // sort current and previous submap kf list of indices 1313 | std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); 1314 | std::sort(this->submap_kf_idx_prev.begin(), this->submap_kf_idx_prev.end()); 1315 | 1316 | // check if submap has changed from previous iteration 1317 | if (this->submap_kf_idx_curr == this->submap_kf_idx_prev){ 1318 | this->submap_hasChanged = false; 1319 | } else { 1320 | this->submap_hasChanged = true; 1321 | 1322 | // reinitialize submap cloud, normals 1323 | pcl::PointCloud::Ptr submap_cloud_ (boost::make_shared>()); 1324 | this->submap_normals.clear(); 1325 | 1326 | for (auto k : this->submap_kf_idx_curr) { 1327 | 1328 | // create current submap cloud 1329 | *submap_cloud_ += *this->keyframes[k].second; 1330 | 1331 | // grab corresponding submap cloud's normals 1332 | this->submap_normals.insert( std::end(this->submap_normals), std::begin(this->keyframe_normals[k]), std::end(this->keyframe_normals[k]) ); 1333 | } 1334 | 1335 | this->submap_cloud = submap_cloud_; 1336 | this->submap_kf_idx_prev = this->submap_kf_idx_curr; 1337 | } 1338 | 1339 | } 1340 | 1341 | bool dlo::OdomNode::saveTrajectory(direct_lidar_odometry::save_traj::Request& req, 1342 | direct_lidar_odometry::save_traj::Response& res) { 1343 | std::string kittipath = req.save_path + "/kitti_traj.txt"; 1344 | std::ofstream out_kitti(kittipath); 1345 | 1346 | std::cout << std::setprecision(2) << "Saving KITTI trajectory to " << kittipath << "... "; std::cout.flush(); 1347 | 1348 | for (const auto& pose : this->trajectory) { 1349 | const auto& t = pose.first; 1350 | const auto& q = pose.second; 1351 | // Write to Kitti Format 1352 | auto R = q.normalized().toRotationMatrix(); 1353 | out_kitti << std::fixed << std::setprecision(9) 1354 | << R(0, 0) << " " << R(0, 1) << " " << R(0, 2) << " " << t.x() << " " 1355 | << R(1, 0) << " " << R(1, 1) << " " << R(1, 2) << " " << t.y() << " " 1356 | << R(2, 0) << " " << R(2, 1) << " " << R(2, 2) << " " << t.z() << "\n"; 1357 | } 1358 | 1359 | std::cout << "done" << std::endl; 1360 | res.success = true; 1361 | return res.success; 1362 | } 1363 | 1364 | /** 1365 | * Debug Statements 1366 | **/ 1367 | 1368 | void dlo::OdomNode::debug() { 1369 | 1370 | // Total length traversed 1371 | double length_traversed = 0.; 1372 | Eigen::Vector3f p_curr = Eigen::Vector3f(0., 0., 0.); 1373 | Eigen::Vector3f p_prev = Eigen::Vector3f(0., 0., 0.); 1374 | for (const auto& t : this->trajectory) { 1375 | if (p_prev == Eigen::Vector3f(0., 0., 0.)) { 1376 | p_prev = t.first; 1377 | continue; 1378 | } 1379 | p_curr = t.first; 1380 | double l = sqrt(pow(p_curr[0] - p_prev[0], 2) + pow(p_curr[1] - p_prev[1], 2) + pow(p_curr[2] - p_prev[2], 2)); 1381 | 1382 | if (l >= 0.05) { 1383 | length_traversed += l; 1384 | p_prev = p_curr; 1385 | } 1386 | } 1387 | 1388 | if (length_traversed == 0) { 1389 | this->publish_keyframe_thread = std::thread( &dlo::OdomNode::publishKeyframe, this ); 1390 | this->publish_keyframe_thread.detach(); 1391 | } 1392 | 1393 | // Average computation time 1394 | double avg_comp_time = std::accumulate(this->comp_times.begin(), this->comp_times.end(), 0.0) / this->comp_times.size(); 1395 | 1396 | // RAM Usage 1397 | double vm_usage = 0.0; 1398 | double resident_set = 0.0; 1399 | std::ifstream stat_stream("/proc/self/stat", std::ios_base::in); //get info from proc directory 1400 | std::string pid, comm, state, ppid, pgrp, session, tty_nr; 1401 | std::string tpgid, flags, minflt, cminflt, majflt, cmajflt; 1402 | std::string utime, stime, cutime, cstime, priority, nice; 1403 | std::string num_threads, itrealvalue, starttime; 1404 | unsigned long vsize; 1405 | long rss; 1406 | stat_stream >> pid >> comm >> state >> ppid >> pgrp >> session >> tty_nr 1407 | >> tpgid >> flags >> minflt >> cminflt >> majflt >> cmajflt 1408 | >> utime >> stime >> cutime >> cstime >> priority >> nice 1409 | >> num_threads >> itrealvalue >> starttime >> vsize >> rss; // don't care about the rest 1410 | stat_stream.close(); 1411 | long page_size_kb = sysconf(_SC_PAGE_SIZE) / 1024; // for x86-64 is configured to use 2MB pages 1412 | vm_usage = vsize / 1024.0; 1413 | resident_set = rss * page_size_kb; 1414 | 1415 | // CPU Usage 1416 | struct tms timeSample; 1417 | clock_t now; 1418 | double cpu_percent; 1419 | now = times(&timeSample); 1420 | if (now <= this->lastCPU || timeSample.tms_stime < this->lastSysCPU || 1421 | timeSample.tms_utime < this->lastUserCPU) { 1422 | cpu_percent = -1.0; 1423 | } else { 1424 | cpu_percent = (timeSample.tms_stime - this->lastSysCPU) + (timeSample.tms_utime - this->lastUserCPU); 1425 | cpu_percent /= (now - this->lastCPU); 1426 | cpu_percent /= this->numProcessors; 1427 | cpu_percent *= 100.; 1428 | } 1429 | this->lastCPU = now; 1430 | this->lastSysCPU = timeSample.tms_stime; 1431 | this->lastUserCPU = timeSample.tms_utime; 1432 | this->cpu_percents.push_back(cpu_percent); 1433 | double avg_cpu_usage = std::accumulate(this->cpu_percents.begin(), this->cpu_percents.end(), 0.0) / this->cpu_percents.size(); 1434 | 1435 | // Print to terminal 1436 | printf("\033[2J\033[1;1H"); 1437 | 1438 | std::cout << std::endl << "==== Direct LiDAR Odometry v" << this->version_ << " ====" << std::endl; 1439 | 1440 | if (!this->cpu_type.empty()) { 1441 | std::cout << std::endl << this->cpu_type << " x " << this->numProcessors << std::endl; 1442 | } 1443 | 1444 | std::cout << std::endl << std::setprecision(4) << std::fixed; 1445 | std::cout << "Position [xyz] :: " << this->pose[0] << " " << this->pose[1] << " " << this->pose[2] << std::endl; 1446 | std::cout << "Orientation [wxyz] :: " << this->rotq.w() << " " << this->rotq.x() << " " << this->rotq.y() << " " << this->rotq.z() << std::endl; 1447 | std::cout << "Distance Traveled :: " << length_traversed << " meters" << std::endl; 1448 | std::cout << "Distance to Origin :: " << sqrt(pow(this->pose[0]-this->origin[0],2) + pow(this->pose[1]-this->origin[1],2) + pow(this->pose[2]-this->origin[2],2)) << " meters" << std::endl; 1449 | 1450 | std::cout << std::endl << std::right << std::setprecision(2) << std::fixed; 1451 | std::cout << "Computation Time :: " << std::setfill(' ') << std::setw(6) << this->comp_times.back()*1000. << " ms // Avg: " << std::setw(5) << avg_comp_time*1000. << std::endl; 1452 | std::cout << "Cores Utilized :: " << std::setfill(' ') << std::setw(6) << (cpu_percent/100.) * this->numProcessors << " cores // Avg: " << std::setw(5) << (avg_cpu_usage/100.) * this->numProcessors << std::endl; 1453 | std::cout << "CPU Load :: " << std::setfill(' ') << std::setw(6) << cpu_percent << " % // Avg: " << std::setw(5) << avg_cpu_usage << std::endl; 1454 | std::cout << "RAM Allocation :: " << std::setfill(' ') << std::setw(6) << resident_set/1000. << " MB // VSZ: " << vm_usage/1000. << " MB" << std::endl; 1455 | 1456 | } 1457 | -------------------------------------------------------------------------------- /src/dlo/odom_node.cc: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | #include "dlo/odom.h" 11 | 12 | void controlC(int sig) { 13 | 14 | dlo::OdomNode::abort(); 15 | 16 | } 17 | 18 | int main(int argc, char** argv) { 19 | 20 | ros::init(argc, argv, "dlo_odom_node"); 21 | ros::NodeHandle nh("~"); 22 | 23 | signal(SIGTERM, controlC); 24 | sleep(0.5); 25 | 26 | dlo::OdomNode node(nh); 27 | ros::AsyncSpinner spinner(0); 28 | spinner.start(); 29 | node.start(); 30 | ros::waitForShutdown(); 31 | 32 | return 0; 33 | 34 | } 35 | -------------------------------------------------------------------------------- /src/nano_gicp/lsq_registration.cc: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | /*********************************************************************** 11 | * BSD 3-Clause License 12 | * 13 | * Copyright (c) 2020, SMRT-AIST 14 | * All rights reserved. 15 | * 16 | * Redistribution and use in source and binary forms, with or without 17 | * modification, are permitted provided that the following conditions are met: 18 | * 19 | * 1. Redistributions of source code must retain the above copyright notice, this 20 | * list of conditions and the following disclaimer. 21 | * 22 | * 2. Redistributions in binary form must reproduce the above copyright notice, 23 | * this list of conditions and the following disclaimer in the documentation 24 | * and/or other materials provided with the distribution. 25 | * 26 | * 3. Neither the name of the copyright holder nor the names of its 27 | * contributors may be used to endorse or promote products derived from 28 | * this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 31 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 32 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 33 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 34 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 35 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 36 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 37 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 38 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 39 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 40 | *************************************************************************/ 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | template class nano_gicp::LsqRegistration; 47 | -------------------------------------------------------------------------------- /src/nano_gicp/nano_gicp.cc: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | /*********************************************************************** 11 | * BSD 3-Clause License 12 | * 13 | * Copyright (c) 2020, SMRT-AIST 14 | * All rights reserved. 15 | * 16 | * Redistribution and use in source and binary forms, with or without 17 | * modification, are permitted provided that the following conditions are met: 18 | * 19 | * 1. Redistributions of source code must retain the above copyright notice, this 20 | * list of conditions and the following disclaimer. 21 | * 22 | * 2. Redistributions in binary form must reproduce the above copyright notice, 23 | * this list of conditions and the following disclaimer in the documentation 24 | * and/or other materials provided with the distribution. 25 | * 26 | * 3. Neither the name of the copyright holder nor the names of its 27 | * contributors may be used to endorse or promote products derived from 28 | * this software without specific prior written permission. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 31 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 32 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 33 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 34 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 35 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 36 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 37 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 38 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 39 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 40 | *************************************************************************/ 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | template class nano_gicp::NanoGICP; 47 | -------------------------------------------------------------------------------- /src/nano_gicp/nanoflann.cc: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * 3 | * Copyright (c) 2022, University of California, Los Angeles 4 | * 5 | * Authors: Kenny J. Chen, Brett T. Lopez 6 | * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu 7 | * 8 | ***********************************************************/ 9 | 10 | /*********************************************************************** 11 | * Software License Agreement (BSD License) 12 | * 13 | * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. 14 | * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. 15 | * Copyright 2011-2021 Jose Luis Blanco (joseluisblancoc@gmail.com). 16 | * All rights reserved. 17 | * 18 | * THE BSD LICENSE 19 | * 20 | * Redistribution and use in source and binary forms, with or without 21 | * modification, are permitted provided that the following conditions 22 | * are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright 25 | * notice, this list of conditions and the following disclaimer. 26 | * 2. Redistributions in binary form must reproduce the above copyright 27 | * notice, this list of conditions and the following disclaimer in the 28 | * documentation and/or other materials provided with the distribution. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 31 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 32 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 33 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 34 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 35 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 36 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 37 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 38 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 39 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 40 | *************************************************************************/ 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | template class nanoflann::KdTreeFLANN; 47 | -------------------------------------------------------------------------------- /srv/save_pcd.srv: -------------------------------------------------------------------------------- 1 | float32 leaf_size 2 | string save_path 3 | --- 4 | bool success -------------------------------------------------------------------------------- /srv/save_traj.srv: -------------------------------------------------------------------------------- 1 | string save_path 2 | --- 3 | bool success 4 | --------------------------------------------------------------------------------