├── .gitignore ├── CMakeLists.txt ├── Dockerfile ├── LICENSE ├── README.md ├── config ├── orbvoc.dbow3 ├── spot.png ├── spot.stl └── spot.yaml ├── include ├── KDTreeVectorOfVectorsAdaptor.h ├── Scancontext.h ├── nanoflann.hpp └── tictoc.h ├── launch ├── rviz_setting.rviz └── spot.launch ├── package.xml └── src ├── Scancontext.cpp ├── ikd-Tree ├── ikd_Tree.cpp └── ikd_Tree.h ├── image_handler.h ├── intensity_feature_tracker.cpp ├── intensity_feature_tracker.h ├── intensity_feature_tracker_node.cpp ├── keyframe.h ├── laserMapping.cpp ├── laserOdometry.cpp ├── lidarFeaturePointsFunction.hpp ├── loop_closure_handler.cpp ├── loop_closure_handler.h ├── mapOptimization.cpp ├── mapOptimization.hpp ├── mapOptimizationNode.cpp ├── odom_handler_node.cpp ├── parameters.h ├── scanRegistration.cpp └── tic_toc.h /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/* 2 | build 3 | .vscode 4 | .vscode/c_cpp_properties.json 5 | resultsOutput/ 6 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(intensity_feature_tracker) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | set(CMAKE_CXX_FLAGS_RELEASE " -O3 -Wall -g -pthread -DROSCONSOLE_MIN_SEVERITY=5") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | geometry_msgs 10 | nav_msgs 11 | cv_bridge 12 | roscpp 13 | roslib 14 | sensor_msgs 15 | std_msgs 16 | pcl_conversions 17 | tf 18 | tf2 19 | tf2_ros 20 | message_filters 21 | ) 22 | find_package(OpenMP REQUIRED) 23 | find_package(PCL REQUIRED) 24 | find_package(OpenCV REQUIRED) 25 | find_package(Eigen3 REQUIRED) 26 | find_package(GTSAM REQUIRED QUIET) 27 | find_package(Ceres REQUIRED) 28 | 29 | catkin_package( 30 | INCLUDE_DIRS include 31 | CATKIN_DEPENDS roscpp roslib message_runtime geometry_msgs nav_msgs std_msgs 32 | DEPENDS PCL EIGEN3 33 | INCLUDE_DIRS include 34 | ) 35 | set( DBoW3_INCLUDE_DIRS "/usr/local/include" ) 36 | set( DBoW3_LIBS "/usr/local/lib/libDBoW3.so" ) 37 | 38 | include_directories( 39 | SYSTEM 40 | include 41 | ${catkin_INCLUDE_DIRS} 42 | ${PCL_INCLUDE_DIRS} 43 | ${OpenCV_INCLUDE_DIRS} 44 | ${GTSAM_INCLUDE_DIR} 45 | ${CERES_INCLUDE_DIRS} 46 | ) 47 | link_directories( 48 | inlcude 49 | ${PCL_LIBRARY_DIRS} 50 | ${OpenCV_LIBRARY_DIRS} 51 | ${GTSAM_LIBRARY_DIRS} 52 | ) 53 | 54 | file(GLOB source_files 55 | src/intensity_feature_tracker_node.cpp 56 | src/intensity_feature_tracker.cpp 57 | src/loop_closure_handler.cpp 58 | src/Scancontext.cpp 59 | src/parameters.h 60 | ) 61 | 62 | add_executable(${PROJECT_NAME}_node ${source_files}) 63 | target_compile_options(${PROJECT_NAME}_node PRIVATE ${OpenMP_CXX_FLAGS}) 64 | target_link_libraries(${PROJECT_NAME}_node ${CERES_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} ${DBoW3_LIBS} Eigen3::Eigen gtsam) 65 | 66 | file(GLOB map_source_files 67 | src/mapOptimizationNode.cpp 68 | src/mapOptimization.cpp 69 | src/image_handler.h 70 | src/ikd-Tree/ikd_Tree.cpp 71 | ikd-Tree/ikd_Tree.h 72 | src/parameters.h 73 | 74 | ) 75 | add_executable(${PROJECT_NAME}_mapping_node ${map_source_files}) 76 | target_compile_options(${PROJECT_NAME}_mapping_node PRIVATE ${OpenMP_CXX_FLAGS}) 77 | target_link_libraries(${PROJECT_NAME}_mapping_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} Eigen3::Eigen gtsam ${CERES_LIBRARIES} ${OpenMP_LIBS}) 78 | 79 | add_executable(ascanRegistration src/scanRegistration.cpp src/intensity_feature_tracker.cpp 80 | src/loop_closure_handler.cpp 81 | src/Scancontext.cpp 82 | src/parameters.h) 83 | target_compile_options(ascanRegistration PRIVATE ${OpenMP_CXX_FLAGS}) 84 | target_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${CERES_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} Eigen3::Eigen ${DBoW3_LIBS} gtsam ) 85 | 86 | add_executable(alaserMapping_node src/laserMapping.cpp) 87 | target_link_libraries(alaserMapping_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 88 | 89 | add_executable(alaserOdometry_node src/laserOdometry.cpp) 90 | target_link_libraries(alaserOdometry_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 91 | 92 | add_executable(odomHandler_node src/odom_handler_node.cpp) 93 | target_link_libraries(odomHandler_node ${catkin_LIBRARIES}) 94 | 95 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic-ros-base 2 | 3 | LABEL Maintainer="Wenqiang Du " 4 | LABEL intensitySLAM.version="0.1" 5 | 6 | RUN apt-get update 7 | 8 | RUN apt-get install -y --fix-missing --no-install-recommends git ros-noetic-cv-bridge ros-noetic-image-transport ros-noetic-pcl-ros libpcl-dev wget unzip 9 | 10 | RUN mkdir -p /dependencies_ws &&\ 11 | cd /dependencies_ws &&\ 12 | wget -O /dependencies_ws/ceres.zip https://github.com/ceres-solver/ceres-solver/archive/1.14.0.zip &&\ 13 | apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev unzip libsuitesparse-dev &&\ 14 | cd /dependencies_ws/ && unzip ceres.zip -d /dependencies_ws/ &&\ 15 | cd /dependencies_ws/ceres-solver-1.14.0 &&\ 16 | mkdir ceres-bin && cd ceres-bin &&\ 17 | cmake .. &&\ 18 | make install -j4 19 | 20 | RUN apt update && apt install -y --force-yes apt-transport-https && \ 21 | wget https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS-2019.PUB && \ 22 | apt-key add GPG-PUB-KEY-INTEL-SW-PRODUCTS-2019.PUB && \ 23 | sh -c 'echo deb https://apt.repos.intel.com/mkl all main > /etc/apt/sources.list.d/intel-mkl.list' && \ 24 | apt-get update && DEBIAN_FRONTEND=noninteractive apt-get -y install cpio intel-mkl-64bit-2018.2-046 25 | 26 | RUN cd /dependencies_ws/ &&\ 27 | wget -O gtsam.zip https://github.com/borglab/gtsam/archive/4.1.0.zip &&\ 28 | unzip gtsam.zip -d /dependencies_ws/ &&\ 29 | cd /dependencies_ws/gtsam-4.1.0 &&\ 30 | mkdir build && cd build &&\ 31 | cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF .. &&\ 32 | make install -j4 33 | 34 | 35 | RUN cd /dependencies_ws/ &&\ 36 | git clone https://github.com/rmsalinas/DBow3.git &&\ 37 | cd /dependencies_ws/DBow3 &&\ 38 | mkdir build && cd build &&\ 39 | cmake .. &&\ 40 | make &&\ 41 | make install -j4 42 | RUN cp /usr/local/lib/libmetis-gtsam.so /usr/lib/ 43 | 44 | RUN mkdir -p /intensitySLAM/src &&\ 45 | cd /intensitySLAM/src &&\ 46 | git clone https://github.com/SnowCarter/Intensity_based_LiDAR_SLAM.git &&\ 47 | cd /intensitySLAM &&\ 48 | . /opt/ros/${ROS_DISTRO}/setup.sh &&\ 49 | catkin_make 50 | RUN echo "source /intensitySLAM/devel/setup.bash" >> ~/.bashrc 51 | 52 | RUN rm -rf /dependencies_ws 53 | WORKDIR /intensitySLAM -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Wenqiang Du 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 | 2 | # Real-Time Simultaneous Localization and Mapping with LiDAR Intensity 3 | 4 | The utilization of LiDAR's intensity image for scan matching addresses the issue of degeneracy. 5 | 6 | [![IMAGE ALT TEXT HERE](./config/spot.png)](https://youtu.be/B_JX0q5Cdeg) 7 | 8 | 9 | 10 | ## Paper 11 | Thank you for citing our [paper](https://arxiv.org/abs/2301.09257) if you use any of this code or datasets. 12 | ``` 13 | @inproceedings{du2023real, 14 | title={Real-Time Simultaneous Localization and Mapping with LiDAR intensity}, 15 | author={Du, Wenqiang and Beltrame, Giovanni}, 16 | booktitle={2023 International Conference on Robotics and Automation (ICRA)}, 17 | pages={4164--4170}, 18 | year={2023}, 19 | organization={IEEE} 20 | } 21 | ``` 22 | ## Authors 23 | 24 | - [@Wenqiang Du](http://wenqiangdu.me/) 25 | - [@Linkedin](https://www.linkedin.com/in/wenqiangdu/) 26 | - [@Github](https://github.com/SnowCarter) 27 | - [@Giovanni Beltrame](https://mistlab.ca/people/beltrame/) 28 | - [@Linkedin](https://www.linkedin.com/in/gbeltrame/) 29 | 30 | 31 | ## Installation 32 | ### Dependency 33 | 34 | * [ROS noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) 35 | * [ceres-solver](https://github.com/ceres-solver/ceres-solver) 36 | ```bash 37 | $ mkdir ~/dependencies_ws 38 | $ cd ~/dependencies_ws/ 39 | $ wget -O ~/dependencies_ws/ceres.zip https://github.com/ceres-solver/ceres-solver/archive/1.14.0.zip 40 | $ apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev unzip libsuitesparse-dev 41 | $ cd ~/dependencies_ws/ && unzip ceres.zip -d ~/dependencies_ws/ 42 | $ cd /dependencies_ws/ceres-solver-1.14.0 43 | $ mkdir ceres-bin && cd ceres-bin 44 | $ cmake .. 45 | $ sudo make install -j4 46 | ``` 47 | * [GTSAM](https://github.com/borglab/gtsam) 48 | ``` 49 | $ cd ~/dependencies_ws/ 50 | $ wget -O gtsam.zip https://github.com/borglab/gtsam/archive/4.1.0.zip 51 | $ unzip gtsam.zip -d ~/dependencies_ws/ 52 | $ cd ~/dependencies_ws/gtsam-4.1.0 53 | $ mkdir build && cd build 54 | $ cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF .. 55 | $ sudo make install -j4 56 | ``` 57 | * [DBOW3](https://github.com/rmsalinas/DBow3) 58 | ``` 59 | $ cd ~/dependencies_ws/ 60 | $ git clone https://github.com/rmsalinas/DBow3.git 61 | $ cd DBow3/ && mkdir build && cd build 62 | $ cmake .. 63 | $ make 64 | $ sudo make install 65 | ``` 66 | ### Compile and Launch 67 | - Create workspace 68 | ``` 69 | mkdir -p ~/catkin_ws/src 70 | ``` 71 | 72 | - Clone the project 73 | 74 | ```bash 75 | cd ~/catkin_ws/src/ 76 | git clone https://github.com/SnowCarter/Intensity_based_LiDAR_SLAM.git 77 | ``` 78 | 79 | - Go to the project workspace directory 80 | 81 | ```bash 82 | cd ../ 83 | ``` 84 | 85 | - Compile 86 | ```bash 87 | source /opt/ros/noetic/setup.bash 88 | catkin_make 89 | ``` 90 | 91 | - Launch 92 | 93 | ```bash 94 | source devel/setup.bash 95 | roslaunch intensity_feature_tracker spot.launch rviz:=1 96 | ``` 97 | - Play bag file 98 | ``` 99 | rosbag play xxx.bag --clock 100 | ``` 101 | 102 | ## [Docker](https://www.docker.com/) support 103 | #### Building 104 | - Create workspace 105 | 106 | ``` 107 | mkdir -p ~/catkin_ws/src 108 | ``` 109 | 110 | - Clone the project 111 | 112 | ```bash 113 | cd ~/catkin_ws/src/ 114 | git clone https://github.com/SnowCarter/Intensity_based_LiDAR_SLAM.git 115 | ``` 116 | 117 | - Go to the project directory 118 | 119 | ```bash 120 | cd Intensity_based_LiDAR_SLAM/ 121 | ``` 122 | - Build docker image 123 | ``` 124 | docker build . -t intensity_lidar_slam 125 | ``` 126 | 127 | #### Launch 128 | - Launch docker images with intensity Lidar SLAM (Terminal 1) 129 | ``` 130 | docker run --rm -it --net=host --privileged intensity_lidar_slam:latest /bin/bash -i -c 'roslaunch intensity_feature_tracker spot.launch rviz:=0' 131 | ``` 132 | - Play bag file (Terminal 2) 133 | ``` 134 | rosbag play xxx.bag --clock 135 | ``` 136 | - RVIZ (Terminal 3) 137 | ``` 138 | rosrun rviz rviz -d ~/catkin_ws/src/Intensity_based_LiDAR_SLAM/launch/rviz_setting.rviz 139 | ``` 140 | - Show spot mesh (optional): 141 | - rviz need to find the mesh file according to the package name, so we need to make sure this package is compiled and sourced, even you can't compile it successfully. 142 | ``` 143 | cd ~/catkin_ws/ 144 | catkin_make 145 | ``` 146 | - in Terminal 3: 147 | ``` 148 | source ~/catkin_ws/devel/setup.bash 149 | rosrun rviz rviz -d ~/catkin_ws/src/Intensity_based_LiDAR_SLAM/launch/rviz_setting.rviz 150 | ``` 151 | 152 | ## Example Bag file 153 | * [Onedrive](https://polymtlca0-my.sharepoint.com/:u:/g/personal/wenqiang_du_polymtlus_ca/EUn0TO3TMLdFvBvRiVFroIUB9ggQNb3dUfYqoe073WD7hA?e=cJao7W): Ouster (OS0-64) 154 | ``` 155 | path: spot_corridor.bag 156 | version: 2.0 157 | duration: 8:46s (526s) 158 | start: Aug 12 2022 16:25:24.06 (1660335924.06) 159 | end: Aug 12 2022 16:34:10.95 (1660336450.95) 160 | size: 8.2 GB 161 | messages: 57957 162 | compression: lz4 [5270/5270 chunks; 53.28%] 163 | uncompressed: 15.5 GB @ 30.0 MB/s 164 | compressed: 8.2 GB @ 16.0 MB/s (53.28%) 165 | types: sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2] 166 | sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181] 167 | topics: /os_cloud_node/imu 52688 msgs : sensor_msgs/Imu 168 | /os_cloud_node/points 5269 msgs : sensor_msgs/PointCloud2 169 | ``` 170 | 171 | ## Contributing 172 | 173 | Contributions are always welcome! 174 | 175 | ## License 176 | [![MIT License](https://img.shields.io/badge/License-MIT-green.svg)](https://choosealicense.com/licenses/mit/) 177 | 178 | 179 | ## Acknowledgements 180 | - [Robust Place Recognition using an Imaging Lidar](https://github.com/TixiaoShan/imaging_lidar_place_recognition) 181 | - [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM) 182 | - [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono) 183 | - [Scan Context](https://github.com/irapkaist/scancontext) 184 | - [LVI-SAM](https://github.com/TixiaoShan/LVI-SAM) 185 | - [ikd-Tree](https://github.com/hku-mars/ikd-Tree) 186 | ## 🚀 About Me 187 | I'm a Ph.D. candidate in [MISTLab](https://mistlab.ca/). 188 | 189 | 190 | ## Support 191 | 192 | For support, email snowdwq@gmail.com. 193 | 194 | ![Logo](https://mistlab.ca/images/mistlogo.png) 195 | -------------------------------------------------------------------------------- /config/orbvoc.dbow3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MISTLab/Intensity_based_LiDAR_SLAM/785a5dc037169d5369590a9fed0346c9f6ee4b2d/config/orbvoc.dbow3 -------------------------------------------------------------------------------- /config/spot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MISTLab/Intensity_based_LiDAR_SLAM/785a5dc037169d5369590a9fed0346c9f6ee4b2d/config/spot.png -------------------------------------------------------------------------------- /config/spot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MISTLab/Intensity_based_LiDAR_SLAM/785a5dc037169d5369590a9fed0346c9f6ee4b2d/config/spot.stl -------------------------------------------------------------------------------- /config/spot.yaml: -------------------------------------------------------------------------------- 1 | intensity_feature_tracker: 2 | # Project 3 | project_name: "intensity_feature_tracker" 4 | 5 | # Cloud parameters 6 | cloud_topic: "/os_cloud_node/points" # point cloud input topic 7 | image_width: 1024 # image horitontal resolution 8 | image_height: 64 # image vertical resolution 9 | image_crop: 3 # mask for blocking feature extraction; indoor/handheld=196-256; jackal=0 10 | hand_held_flag: true # mask blocking for peoplse shadow. 11 | 12 | # Feature matching 13 | use_orb: 1 # enabling flag for ORB feature 14 | num_orb_features: 1000 # feature num for ORB 500 1500 3000 2000 15 | 16 | 17 | # CPU 18 | skip_time: 0.15 # point cloud processing time interval 19 | num_threads: 16 # CPU thread number 20 | 21 | # scan registration in loops( only one registration parameter can be set to true here) 22 | use_teaser: false # USE_TEASER 23 | use_pnpransac: false # USE_PNPRANSAC 24 | use_icp: true # USE_ICP ICP is the best one here. 25 | 26 | loop_closure_parameters: 27 | use_crop: false #indoor: true; outdoor: false 28 | crop_size: 200 # for ICP scan matching: Crop the cloud and only process the 200x200x200m^3 area indoor: 1 29 | 30 | use_voxel_downsample: true # usually true 31 | vf_scan_res: 0.25 #outdoor: 0.4; indoor: 0.25 32 | 33 | icp_fitness_score: 0.5 #0.6 34 | 35 | keyframe_time_intervals: 0.3 36 | keyframe_distance_intervals: 0.3 #indoor: 0.3; outdoor: 3 37 | 38 | min_loop_bow_threshold: 0.013 # 0.018 39 | min_loop_search_gap: 20 40 | skiped_frames: 10 #normal:5 41 | 42 | 43 | record_ground_truth: false 44 | 45 | map_optimization_parameters: 46 | sliding_window_size: 0 # sliding window size for keyframes map optimization: 10 or 5 47 | ground_plane_window_size: 2 # sliding window size for ground plane optimization 10 frames: ~110ms; 100frames: best; 1000 frames: ~350ms 48 | crop_size: 5 # only processing the 5x5x5m^3 area indoor: 1 49 | remove_radius: 0.3 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /include/KDTreeVectorOfVectorsAdaptor.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com). 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in the 15 | * documentation and/or other materials provided with the distribution. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 18 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 19 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 20 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 22 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 26 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | *************************************************************************/ 28 | 29 | #pragma once 30 | 31 | #include 32 | 33 | #include 34 | 35 | // ===== This example shows how to use nanoflann with these types of containers: ======= 36 | //typedef std::vector > my_vector_of_vectors_t; 37 | //typedef std::vector my_vector_of_vectors_t; // This requires #include 38 | // ===================================================================================== 39 | 40 | 41 | /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage. 42 | * The i'th vector represents a point in the state space. 43 | * 44 | * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations. 45 | * \tparam num_t The type of the point coordinates (typically, double or float). 46 | * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. 47 | * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int) 48 | */ 49 | template 50 | struct KDTreeVectorOfVectorsAdaptor 51 | { 52 | typedef KDTreeVectorOfVectorsAdaptor self_t; 53 | typedef typename Distance::template traits::distance_t metric_t; 54 | typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t; 55 | 56 | index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index. 57 | 58 | /// Constructor: takes a const ref to the vector of vectors object with the data points 59 | KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat) 60 | { 61 | assert(mat.size() != 0 && mat[0].size() != 0); 62 | const size_t dims = mat[0].size(); 63 | if (DIM>0 && static_cast(dims) != DIM) 64 | throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument"); 65 | index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) ); 66 | index->buildIndex(); 67 | } 68 | 69 | ~KDTreeVectorOfVectorsAdaptor() { 70 | delete index; 71 | } 72 | 73 | const VectorOfVectorsType &m_data; 74 | 75 | /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). 76 | * Note that this is a short-cut method for index->findNeighbors(). 77 | * The user can also call index->... methods as desired. 78 | * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. 79 | */ 80 | inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const 81 | { 82 | nanoflann::KNNResultSet resultSet(num_closest); 83 | resultSet.init(out_indices, out_distances_sq); 84 | index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); 85 | } 86 | 87 | /** @name Interface expected by KDTreeSingleIndexAdaptor 88 | * @{ */ 89 | 90 | const self_t & derived() const { 91 | return *this; 92 | } 93 | self_t & derived() { 94 | return *this; 95 | } 96 | 97 | // Must return the number of data points 98 | inline size_t kdtree_get_point_count() const { 99 | return m_data.size(); 100 | } 101 | 102 | // Returns the dim'th component of the idx'th point in the class: 103 | inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const { 104 | return m_data[idx][dim]; 105 | } 106 | 107 | // Optional bounding-box computation: return false to default to a standard bbox computation loop. 108 | // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. 109 | // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) 110 | template 111 | bool kdtree_get_bbox(BBOX & /*bb*/) const { 112 | return false; 113 | } 114 | 115 | /** @} */ 116 | 117 | }; // end of KDTreeVectorOfVectorsAdaptor 118 | -------------------------------------------------------------------------------- /include/Scancontext.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "nanoflann.hpp" 26 | #include "KDTreeVectorOfVectorsAdaptor.h" 27 | 28 | #include "tictoc.h" 29 | 30 | using namespace Eigen; 31 | using namespace nanoflann; 32 | 33 | using std::cout; 34 | using std::endl; 35 | using std::make_pair; 36 | 37 | using std::atan2; 38 | using std::cos; 39 | using std::sin; 40 | 41 | using SCPointType = pcl::PointXYZI; // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context) 42 | using KeyMat = std::vector >; 43 | using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >; 44 | 45 | 46 | // namespace SC2 47 | // { 48 | 49 | void coreImportTest ( void ); 50 | 51 | 52 | // sc param-independent helper functions 53 | float xy2theta( const float & _x, const float & _y ); 54 | MatrixXd circshift( MatrixXd &_mat, int _num_shift ); 55 | std::vector eig2stdvec( MatrixXd _eigmat ); 56 | 57 | 58 | class SCManager 59 | { 60 | public: 61 | SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care. 62 | 63 | Eigen::MatrixXd makeScancontext( pcl::PointCloud & _scan_down ); 64 | Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ); 65 | Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ); 66 | 67 | int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 ); 68 | double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18) 69 | std::pair distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18) 70 | 71 | // User-side API 72 | void makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ); 73 | std::pair detectLoopClosureID( void ); // int: nearest node index, float: relative yaw 74 | 75 | public: 76 | // hyper parameters () 77 | const double LIDAR_HEIGHT = 2.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0. 78 | 79 | const int PC_NUM_RING = 20; // 20 in the original paper (IROS 18) 80 | const int PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18) 81 | const double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18) 82 | const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR); 83 | const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING); 84 | 85 | // tree 86 | const int NUM_EXCLUDE_RECENT = 50; // simply just keyframe gap, but node position distance-based exclusion is ok. 87 | const int NUM_CANDIDATES_FROM_TREE = 10; // 10 is enough. (refer the IROS 18 paper) 88 | 89 | // loop thres 90 | const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver. 91 | const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness) 92 | // const double SC_DIST_THRES = 0.5; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15 93 | 94 | // config 95 | const int TREE_MAKING_PERIOD_ = 50; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / if you want to find a very recent revisits use small value of it (it is enough fast ~ 5-50ms wrt N.). 96 | int tree_making_period_conter = 0; 97 | 98 | // data 99 | std::vector polarcontexts_timestamp_; // optional. 100 | std::vector polarcontexts_; 101 | std::vector polarcontext_invkeys_; 102 | std::vector polarcontext_vkeys_; 103 | 104 | KeyMat polarcontext_invkeys_mat_; 105 | KeyMat polarcontext_invkeys_to_search_; 106 | std::unique_ptr polarcontext_tree_; 107 | 108 | }; // SCManager 109 | 110 | // } // namespace SC2 111 | -------------------------------------------------------------------------------- /include/tictoc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class TicToc_SC 13 | { 14 | public: 15 | TicToc_SC() 16 | { 17 | tic(); 18 | } 19 | 20 | TicToc_SC( bool _disp ) 21 | { 22 | disp_ = _disp; 23 | tic(); 24 | } 25 | 26 | void tic() 27 | { 28 | start = std::chrono::system_clock::now(); 29 | } 30 | 31 | void toc( std::string _about_task ) 32 | { 33 | end = std::chrono::system_clock::now(); 34 | std::chrono::duration elapsed_seconds = end - start; 35 | double elapsed_ms = elapsed_seconds.count() * 1000; 36 | 37 | if( disp_ ) 38 | { 39 | std::cout.precision(3); // 10 for sec, 3 for ms 40 | std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl; 41 | } 42 | } 43 | 44 | private: 45 | std::chrono::time_point start, end; 46 | bool disp_ = false; 47 | }; 48 | -------------------------------------------------------------------------------- /launch/rviz_setting.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Image1 8 | - /Odometry1 9 | - /Odometry1/Shape1 10 | - /PointCloud1 11 | Splitter Ratio: 0.5051546096801758 12 | Tree Height: 460 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: Image 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: false 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: 50 54 | Reference Frame: 55 | Value: false 56 | - Class: rviz/TF 57 | Enabled: false 58 | Filter (blacklist): "" 59 | Filter (whitelist): "" 60 | Frame Timeout: 15 61 | Frames: 62 | All Enabled: true 63 | Marker Alpha: 1 64 | Marker Scale: 1 65 | Name: TF 66 | Show Arrows: true 67 | Show Axes: true 68 | Show Names: true 69 | Tree: 70 | {} 71 | Update Interval: 0 72 | Value: false 73 | - Alpha: 1 74 | Buffer Length: 1 75 | Class: rviz/Path 76 | Color: 25; 255; 0 77 | Enabled: true 78 | Head Diameter: 0.30000001192092896 79 | Head Length: 0.20000000298023224 80 | Length: 0.30000001192092896 81 | Line Style: Lines 82 | Line Width: 0.029999999329447746 83 | Name: Path 84 | Offset: 85 | X: 0 86 | Y: 0 87 | Z: 0 88 | Pose Color: 255; 85; 255 89 | Pose Style: None 90 | Queue Size: 10 91 | Radius: 0.029999999329447746 92 | Shaft Diameter: 0.10000000149011612 93 | Shaft Length: 0.10000000149011612 94 | Topic: /aft_mapped_path 95 | Unreliable: false 96 | Value: true 97 | - Class: rviz/Marker 98 | Enabled: true 99 | Marker Topic: /car_model_Marker_array 100 | Name: Marker 101 | Namespaces: 102 | {} 103 | Queue Size: 100 104 | Value: true 105 | - Class: rviz/Marker 106 | Enabled: false 107 | Marker Topic: /matched_lines 108 | Name: Marker 109 | Namespaces: 110 | {} 111 | Queue Size: 100 112 | Value: false 113 | - Class: rviz/Image 114 | Enabled: true 115 | Image Topic: /front_matched_keypoints_img 116 | Max Value: 1 117 | Median window: 5 118 | Min Value: 0 119 | Name: Image 120 | Normalize Range: true 121 | Queue Size: 2 122 | Transport Hint: raw 123 | Unreliable: false 124 | Value: true 125 | - Angle Tolerance: 0.10000000149011612 126 | Class: rviz/Odometry 127 | Covariance: 128 | Orientation: 129 | Alpha: 0.5 130 | Color: 255; 255; 127 131 | Color Style: Unique 132 | Frame: Local 133 | Offset: 1 134 | Scale: 1 135 | Value: true 136 | Position: 137 | Alpha: 0.30000001192092896 138 | Color: 204; 51; 204 139 | Scale: 1 140 | Value: true 141 | Value: true 142 | Enabled: false 143 | Keep: 3000 144 | Name: Odometry 145 | Position Tolerance: 0.10000000149011612 146 | Queue Size: 10 147 | Shape: 148 | Alpha: 1 149 | Axes Length: 0.5 150 | Axes Radius: 0.05000000074505806 151 | Color: 255; 25; 0 152 | Head Length: 0.30000001192092896 153 | Head Radius: 0.10000000149011612 154 | Shaft Length: 1 155 | Shaft Radius: 0.05000000074505806 156 | Value: Axes 157 | Topic: /aft_mapped_to_init_high_frec 158 | Unreliable: false 159 | Value: false 160 | - Class: rviz/Group 161 | Displays: 162 | - Alpha: 1 163 | Autocompute Intensity Bounds: true 164 | Autocompute Value Bounds: 165 | Max Value: 10.331636428833008 166 | Min Value: -1.6375482082366943 167 | Value: true 168 | Axis: Z 169 | Channel Name: intensity 170 | Class: rviz/PointCloud2 171 | Color: 255; 255; 255 172 | Color Transformer: Intensity 173 | Decay Time: 0 174 | Enabled: false 175 | Invert Rainbow: false 176 | Max Color: 255; 255; 255 177 | Min Color: 0; 0; 0 178 | Name: PointCloud2 179 | Position Transformer: XYZ 180 | Queue Size: 10 181 | Selectable: true 182 | Size (Pixels): 2 183 | Size (m): 0.05000000074505806 184 | Style: Points 185 | Topic: /laser_cloud_less_flat 186 | Unreliable: false 187 | Use Fixed Frame: true 188 | Use rainbow: true 189 | Value: false 190 | - Alpha: 1 191 | Autocompute Intensity Bounds: true 192 | Autocompute Value Bounds: 193 | Max Value: 10 194 | Min Value: -10 195 | Value: true 196 | Axis: Z 197 | Channel Name: intensity 198 | Class: rviz/PointCloud2 199 | Color: 255; 255; 255 200 | Color Transformer: FlatColor 201 | Decay Time: 0 202 | Enabled: false 203 | Invert Rainbow: false 204 | Max Color: 255; 255; 255 205 | Min Color: 0; 0; 0 206 | Name: PointCloud2 207 | Position Transformer: XYZ 208 | Queue Size: 10 209 | Selectable: true 210 | Size (Pixels): 3 211 | Size (m): 0.05000000074505806 212 | Style: Flat Squares 213 | Topic: /laser_cloud_less_sharp 214 | Unreliable: false 215 | Use Fixed Frame: true 216 | Use rainbow: true 217 | Value: false 218 | - Alpha: 1 219 | Autocompute Intensity Bounds: true 220 | Autocompute Value Bounds: 221 | Max Value: 3.9330835342407227 222 | Min Value: -4.237585544586182 223 | Value: true 224 | Axis: Z 225 | Channel Name: intensity 226 | Class: rviz/PointCloud2 227 | Color: 255; 255; 255 228 | Color Transformer: AxisColor 229 | Decay Time: 0 230 | Enabled: true 231 | Invert Rainbow: false 232 | Max Color: 255; 255; 255 233 | Min Color: 0; 0; 0 234 | Name: ikd-tree map 235 | Position Transformer: XYZ 236 | Queue Size: 10 237 | Selectable: true 238 | Size (Pixels): 2 239 | Size (m): 0.10000000149011612 240 | Style: Points 241 | Topic: /ground_points 242 | Unreliable: false 243 | Use Fixed Frame: true 244 | Use rainbow: true 245 | Value: true 246 | - Alpha: 1 247 | Autocompute Intensity Bounds: true 248 | Autocompute Value Bounds: 249 | Max Value: 10 250 | Min Value: -10 251 | Value: true 252 | Axis: Z 253 | Channel Name: intensity 254 | Class: rviz/PointCloud2 255 | Color: 255; 255; 255 256 | Color Transformer: Intensity 257 | Decay Time: 0 258 | Enabled: true 259 | Invert Rainbow: false 260 | Max Color: 255; 255; 255 261 | Min Color: 0; 0; 0 262 | Name: loambackMap 263 | Position Transformer: XYZ 264 | Queue Size: 10 265 | Selectable: true 266 | Size (Pixels): 2 267 | Size (m): 0.5 268 | Style: Points 269 | Topic: /laser_cloud_map 270 | Unreliable: false 271 | Use Fixed Frame: true 272 | Use rainbow: true 273 | Value: true 274 | - Alpha: 1 275 | Autocompute Intensity Bounds: true 276 | Autocompute Value Bounds: 277 | Max Value: 10 278 | Min Value: -10 279 | Value: true 280 | Axis: Z 281 | Channel Name: intensity 282 | Class: rviz/PointCloud2 283 | Color: 255; 255; 255 284 | Color Transformer: Intensity 285 | Decay Time: 0 286 | Enabled: true 287 | Invert Rainbow: false 288 | Max Color: 255; 255; 255 289 | Min Color: 0; 0; 0 290 | Name: keyPoses 291 | Position Transformer: XYZ 292 | Queue Size: 10 293 | Selectable: true 294 | Size (Pixels): 10 295 | Size (m): 0.5 296 | Style: Spheres 297 | Topic: /key_pose_origin 298 | Unreliable: false 299 | Use Fixed Frame: true 300 | Use rainbow: true 301 | Value: true 302 | - Alpha: 1 303 | Autocompute Intensity Bounds: true 304 | Autocompute Value Bounds: 305 | Max Value: -0.5435317158699036 306 | Min Value: -0.9303632974624634 307 | Value: true 308 | Axis: Z 309 | Channel Name: intensity 310 | Class: rviz/PointCloud2 311 | Color: 115; 210; 22 312 | Color Transformer: FlatColor 313 | Decay Time: 0 314 | Enabled: false 315 | Invert Rainbow: false 316 | Max Color: 255; 255; 255 317 | Min Color: 0; 0; 0 318 | Name: groundPoints 319 | Position Transformer: XYZ 320 | Queue Size: 10 321 | Selectable: true 322 | Size (Pixels): 2 323 | Size (m): 0.009999999776482582 324 | Style: Points 325 | Topic: /ground_points 326 | Unreliable: false 327 | Use Fixed Frame: true 328 | Use rainbow: true 329 | Value: false 330 | - Alpha: 1 331 | Autocompute Intensity Bounds: true 332 | Autocompute Value Bounds: 333 | Max Value: 10 334 | Min Value: -10 335 | Value: true 336 | Axis: Z 337 | Channel Name: intensity 338 | Class: rviz/PointCloud2 339 | Color: 114; 159; 207 340 | Color Transformer: FlatColor 341 | Decay Time: 0 342 | Enabled: false 343 | Invert Rainbow: false 344 | Max Color: 255; 255; 255 345 | Min Color: 0; 0; 0 346 | Name: loopScan 347 | Position Transformer: XYZ 348 | Queue Size: 10 349 | Selectable: true 350 | Size (Pixels): 5 351 | Size (m): 0.009999999776482582 352 | Style: Points 353 | Topic: /loop_scan 354 | Unreliable: false 355 | Use Fixed Frame: true 356 | Use rainbow: true 357 | Value: false 358 | - Alpha: 1 359 | Autocompute Intensity Bounds: true 360 | Autocompute Value Bounds: 361 | Max Value: 2.9068737030029297 362 | Min Value: -1.5247312784194946 363 | Value: true 364 | Axis: Z 365 | Channel Name: intensity 366 | Class: rviz/PointCloud2 367 | Color: 239; 41; 41 368 | Color Transformer: RGB8 369 | Decay Time: 0 370 | Enabled: true 371 | Invert Rainbow: false 372 | Max Color: 255; 255; 255 373 | Min Color: 0; 0; 0 374 | Name: matched Points 375 | Position Transformer: XYZ 376 | Queue Size: 10 377 | Selectable: true 378 | Size (Pixels): 5 379 | Size (m): 0.10000000149011612 380 | Style: Spheres 381 | Topic: /matched_points 382 | Unreliable: false 383 | Use Fixed Frame: true 384 | Use rainbow: true 385 | Value: true 386 | Enabled: true 387 | Name: PointCloud 388 | - Alpha: 1 389 | Buffer Length: 1 390 | Class: rviz/Path 391 | Color: 25; 255; 0 392 | Enabled: false 393 | Head Diameter: 0.30000001192092896 394 | Head Length: 0.20000000298023224 395 | Length: 0.30000001192092896 396 | Line Style: Lines 397 | Line Width: 0.029999999329447746 398 | Name: Path 399 | Offset: 400 | X: 0 401 | Y: 0 402 | Z: 0 403 | Pose Color: 255; 85; 255 404 | Pose Style: None 405 | Queue Size: 10 406 | Radius: 0.029999999329447746 407 | Shaft Diameter: 0.10000000149011612 408 | Shaft Length: 0.10000000149011612 409 | Topic: /laser_odom_path 410 | Unreliable: false 411 | Value: false 412 | - Alpha: 1 413 | Buffer Length: 1 414 | Class: rviz/Path 415 | Color: 25; 255; 0 416 | Enabled: true 417 | Head Diameter: 0.30000001192092896 418 | Head Length: 0.20000000298023224 419 | Length: 0.30000001192092896 420 | Line Style: Lines 421 | Line Width: 0.029999999329447746 422 | Name: Path 423 | Offset: 424 | X: 0 425 | Y: 0 426 | Z: 0 427 | Pose Color: 255; 85; 255 428 | Pose Style: None 429 | Queue Size: 10 430 | Radius: 0.029999999329447746 431 | Shaft Diameter: 0.10000000149011612 432 | Shaft Length: 0.10000000149011612 433 | Topic: /lio_sam/mapping/path 434 | Unreliable: false 435 | Value: true 436 | - Alpha: 1 437 | Autocompute Intensity Bounds: true 438 | Autocompute Value Bounds: 439 | Max Value: 10 440 | Min Value: -10 441 | Value: true 442 | Axis: Z 443 | Channel Name: intensity 444 | Class: rviz/PointCloud2 445 | Color: 255; 255; 255 446 | Color Transformer: Intensity 447 | Decay Time: 0 448 | Enabled: false 449 | Invert Rainbow: false 450 | Max Color: 255; 255; 255 451 | Min Color: 0; 0; 0 452 | Name: PointCloud2 453 | Position Transformer: XYZ 454 | Queue Size: 10 455 | Selectable: true 456 | Size (Pixels): 3 457 | Size (m): 0.009999999776482582 458 | Style: Flat Squares 459 | Topic: /laser_cloud_surround 460 | Unreliable: false 461 | Use Fixed Frame: true 462 | Use rainbow: true 463 | Value: false 464 | Enabled: true 465 | Global Options: 466 | Background Color: 48; 48; 48 467 | Default Light: true 468 | Fixed Frame: map 469 | Frame Rate: 30 470 | Name: root 471 | Tools: 472 | - Class: rviz/Interact 473 | Hide Inactive Objects: true 474 | - Class: rviz/MoveCamera 475 | - Class: rviz/Select 476 | - Class: rviz/FocusCamera 477 | - Class: rviz/Measure 478 | - Class: rviz/SetInitialPose 479 | Theta std deviation: 0.2617993950843811 480 | Topic: /initialpose 481 | X std deviation: 0.5 482 | Y std deviation: 0.5 483 | - Class: rviz/SetGoal 484 | Topic: /move_base_simple/goal 485 | - Class: rviz/PublishPoint 486 | Single click: true 487 | Topic: /clicked_point 488 | Value: true 489 | Views: 490 | Current: 491 | Class: rviz/Orbit 492 | Distance: 31.244979858398438 493 | Enable Stereo Rendering: 494 | Stereo Eye Separation: 0.05999999865889549 495 | Stereo Focal Distance: 1 496 | Swap Stereo Eyes: false 497 | Value: false 498 | Field of View: 0.7853981852531433 499 | Focal Point: 500 | X: -6.5753254890441895 501 | Y: 4.13983678817749 502 | Z: -2.2634100914001465 503 | Focal Shape Fixed Size: false 504 | Focal Shape Size: 0.05000000074505806 505 | Invert Z Axis: false 506 | Name: Current View 507 | Near Clip Distance: 0.009999999776482582 508 | Pitch: 1.5097967386245728 509 | Target Frame: 510 | Yaw: 5.7121758460998535 511 | Saved: ~ 512 | Window Geometry: 513 | Displays: 514 | collapsed: false 515 | Height: 1043 516 | Hide Left Dock: false 517 | Hide Right Dock: true 518 | Image: 519 | collapsed: false 520 | QMainWindow State: 000000ff00000000fd00000004000000000000018100000257fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000015b00000257000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000078000000118fc0100000002fb0000000a0049006d0061006700650100000000000007800000005e00fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005f90000025700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 521 | Selection: 522 | collapsed: false 523 | Time: 524 | collapsed: false 525 | Tool Properties: 526 | collapsed: false 527 | Views: 528 | collapsed: true 529 | Width: 1920 530 | X: 0 531 | Y: 0 532 | -------------------------------------------------------------------------------- /launch/spot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 24 | 25 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | intensity_feature_tracker 4 | 0.0.0 5 | The intensity_feature_tracker package 6 | 7 | 8 | 9 | 10 | Wenqiang Du 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | catkin 21 | cv_bridge 22 | roscpp 23 | roslib 24 | sensor_msgs 25 | std_msgs 26 | pcl_conversions 27 | tf2 28 | tf2_ros 29 | message_filters 30 | geometry_msgs 31 | nav_msgs 32 | 33 | cv_bridge 34 | roscpp 35 | roslib 36 | sensor_msgs 37 | std_msgs 38 | pcl_conversions 39 | tf2 40 | tf2_ros 41 | message_filters 42 | geometry_msgs 43 | nav_msgs 44 | 45 | cv_bridge 46 | roscpp 47 | roslib 48 | sensor_msgs 49 | std_msgs 50 | pcl_conversions 51 | tf2 52 | tf2_ros 53 | message_filters 54 | message_runtime 55 | geometry_msgs 56 | nav_msgs 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /src/Scancontext.cpp: -------------------------------------------------------------------------------- 1 | // Modified from https://github.com/irapkaist/scancontext 2 | #include "Scancontext.h" 3 | 4 | // namespace SC2 5 | // { 6 | 7 | void coreImportTest (void) 8 | { 9 | cout << "scancontext lib is successfully imported." << endl; 10 | } // coreImportTest 11 | 12 | 13 | float rad2deg(float radians) 14 | { 15 | return radians * 180.0 / M_PI; 16 | } 17 | 18 | float deg2rad(float degrees) 19 | { 20 | return degrees * M_PI / 180.0; 21 | } 22 | 23 | 24 | float xy2theta( const float & _x, const float & _y ) 25 | { 26 | if ( _x >= 0 & _y >= 0) 27 | return (180/M_PI) * atan(_y / _x); 28 | 29 | if ( _x < 0 & _y >= 0) 30 | return 180 - ( (180/M_PI) * atan(_y / (-_x)) ); 31 | 32 | if ( _x < 0 & _y < 0) 33 | return 180 + ( (180/M_PI) * atan(_y / _x) ); 34 | 35 | if ( _x >= 0 & _y < 0) 36 | return 360 - ( (180/M_PI) * atan((-_y) / _x) ); 37 | } // xy2theta 38 | 39 | 40 | MatrixXd circshift( MatrixXd &_mat, int _num_shift ) 41 | { 42 | // shift columns to right direction 43 | assert(_num_shift >= 0); 44 | 45 | if( _num_shift == 0 ) 46 | { 47 | MatrixXd shifted_mat( _mat ); 48 | return shifted_mat; // Early return 49 | } 50 | 51 | MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() ); 52 | for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ ) 53 | { 54 | int new_location = (col_idx + _num_shift) % _mat.cols(); 55 | shifted_mat.col(new_location) = _mat.col(col_idx); 56 | } 57 | 58 | return shifted_mat; 59 | 60 | } // circshift 61 | 62 | 63 | std::vector eig2stdvec( MatrixXd _eigmat ) 64 | { 65 | std::vector vec( _eigmat.data(), _eigmat.data() + _eigmat.size() ); 66 | return vec; 67 | } // eig2stdvec 68 | 69 | 70 | double SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ) 71 | { 72 | int num_eff_cols = 0; // i.e., to exclude all-nonzero sector 73 | double sum_sector_similarity = 0; 74 | for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ ) 75 | { 76 | VectorXd col_sc1 = _sc1.col(col_idx); 77 | VectorXd col_sc2 = _sc2.col(col_idx); 78 | 79 | if( col_sc1.norm() == 0 | col_sc2.norm() == 0 ) 80 | continue; // don't count this sector pair. 81 | 82 | double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm()); 83 | 84 | sum_sector_similarity = sum_sector_similarity + sector_similarity; 85 | num_eff_cols = num_eff_cols + 1; 86 | } 87 | 88 | double sc_sim = sum_sector_similarity / num_eff_cols; 89 | return 1.0 - sc_sim; 90 | 91 | } // distDirectSC 92 | 93 | 94 | int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2) 95 | { 96 | int argmin_vkey_shift = 0; 97 | double min_veky_diff_norm = 10000000; 98 | for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ ) 99 | { 100 | MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx); 101 | 102 | MatrixXd vkey_diff = _vkey1 - vkey2_shifted; 103 | 104 | double cur_diff_norm = vkey_diff.norm(); 105 | if( cur_diff_norm < min_veky_diff_norm ) 106 | { 107 | argmin_vkey_shift = shift_idx; 108 | min_veky_diff_norm = cur_diff_norm; 109 | } 110 | } 111 | 112 | return argmin_vkey_shift; 113 | 114 | } // fastAlignUsingVkey 115 | 116 | 117 | std::pair SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 ) 118 | { 119 | // 1. fast align using variant key (not in original IROS18) 120 | MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 ); 121 | MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 ); 122 | int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 ); 123 | 124 | const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range 125 | std::vector shift_idx_search_space { argmin_vkey_shift }; 126 | for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ ) 127 | { 128 | shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() ); 129 | shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() ); 130 | } 131 | std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end()); 132 | 133 | // 2. fast columnwise diff 134 | int argmin_shift = 0; 135 | double min_sc_dist = 10000000; 136 | for ( int num_shift: shift_idx_search_space ) 137 | { 138 | MatrixXd sc2_shifted = circshift(_sc2, num_shift); 139 | double cur_sc_dist = distDirectSC( _sc1, sc2_shifted ); 140 | if( cur_sc_dist < min_sc_dist ) 141 | { 142 | argmin_shift = num_shift; 143 | min_sc_dist = cur_sc_dist; 144 | } 145 | } 146 | 147 | return make_pair(min_sc_dist, argmin_shift); 148 | 149 | } // distanceBtnScanContext 150 | 151 | 152 | MatrixXd SCManager::makeScancontext( pcl::PointCloud & _scan_down ) 153 | { 154 | TicToc_SC t_making_desc; 155 | 156 | int num_pts_scan_down = _scan_down.points.size(); 157 | 158 | // main 159 | const int NO_POINT = -1000; 160 | MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR); 161 | 162 | SCPointType pt; 163 | float azim_angle, azim_range; // wihtin 2d plane 164 | int ring_idx, sctor_idx; 165 | for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++) 166 | { 167 | pt.x = _scan_down.points[pt_idx].x; 168 | pt.y = _scan_down.points[pt_idx].y; 169 | pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0). 170 | 171 | // xyz to ring, sector 172 | azim_range = sqrt(pt.x * pt.x + pt.y * pt.y); 173 | azim_angle = xy2theta(pt.x, pt.y); 174 | 175 | // if range is out of roi, pass 176 | if( azim_range > PC_MAX_RADIUS ) 177 | continue; 178 | 179 | ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 ); 180 | sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 ); 181 | 182 | // taking maximum z 183 | if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0 184 | desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin 185 | } 186 | 187 | // reset no points to zero (for cosine dist later) 188 | for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ ) 189 | for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ ) 190 | if( desc(row_idx, col_idx) == NO_POINT ) 191 | desc(row_idx, col_idx) = 0; 192 | 193 | t_making_desc.toc("PolarContext making"); 194 | 195 | return desc; 196 | } // SCManager::makeScancontext 197 | 198 | 199 | MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ) 200 | { 201 | /* 202 | * summary: rowwise mean vector 203 | */ 204 | Eigen::MatrixXd invariant_key(_desc.rows(), 1); 205 | for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ ) 206 | { 207 | Eigen::MatrixXd curr_row = _desc.row(row_idx); 208 | invariant_key(row_idx, 0) = curr_row.mean(); 209 | } 210 | 211 | return invariant_key; 212 | } // SCManager::makeRingkeyFromScancontext 213 | 214 | 215 | MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ) 216 | { 217 | /* 218 | * summary: columnwise mean vector 219 | */ 220 | Eigen::MatrixXd variant_key(1, _desc.cols()); 221 | for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ ) 222 | { 223 | Eigen::MatrixXd curr_col = _desc.col(col_idx); 224 | variant_key(0, col_idx) = curr_col.mean(); 225 | } 226 | 227 | return variant_key; 228 | } // SCManager::makeSectorkeyFromScancontext 229 | 230 | 231 | void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ) 232 | { 233 | Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 234 | Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc ); 235 | Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc ); 236 | std::vector polarcontext_invkey_vec = eig2stdvec( ringkey ); 237 | 238 | polarcontexts_.push_back( sc ); 239 | polarcontext_invkeys_.push_back( ringkey ); 240 | polarcontext_vkeys_.push_back( sectorkey ); 241 | polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec ); 242 | 243 | // cout < SCManager::detectLoopClosureID ( void ) 249 | { 250 | int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID") 251 | 252 | auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query) 253 | auto curr_desc = polarcontexts_.back(); // current observation (query) 254 | 255 | /* 256 | * step 1: candidates from ringkey tree_ 257 | */ 258 | if( polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1) 259 | { 260 | std::pair result {loop_id, 0.0}; 261 | return result; // Early return 262 | } 263 | 264 | // tree_ reconstruction (not mandatory to make everytime) 265 | if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost 266 | { 267 | TicToc_SC t_tree_construction; 268 | 269 | polarcontext_invkeys_to_search_.clear(); 270 | polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ; 271 | 272 | polarcontext_tree_.reset(); 273 | polarcontext_tree_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ ); 274 | // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor) 275 | t_tree_construction.toc("Tree construction"); 276 | } 277 | tree_making_period_conter = tree_making_period_conter + 1; 278 | 279 | double min_dist = 10000000; // init with somthing large 280 | int nn_align = 0; 281 | int nn_idx = 0; 282 | 283 | // knn search 284 | std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE ); 285 | std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE ); 286 | 287 | TicToc_SC t_tree_search; 288 | nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE ); 289 | knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] ); 290 | polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); 291 | t_tree_search.toc("Tree search"); 292 | 293 | /* 294 | * step 2: pairwise distance (find optimal columnwise best-fit using cosine distance) 295 | */ 296 | TicToc_SC t_calc_dist; 297 | for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ ) 298 | { 299 | MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ]; 300 | std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); 301 | 302 | double candidate_dist = sc_dist_result.first; 303 | int candidate_align = sc_dist_result.second; 304 | 305 | if( candidate_dist < min_dist ) 306 | { 307 | min_dist = candidate_dist; 308 | nn_align = candidate_align; 309 | 310 | nn_idx = candidate_indexes[candidate_iter_idx]; 311 | } 312 | } 313 | t_calc_dist.toc("Distance calc"); 314 | 315 | /* 316 | * loop threshold check 317 | */ 318 | if( min_dist < SC_DIST_THRES ) 319 | { 320 | loop_id = nn_idx; 321 | 322 | // std::cout.precision(3); 323 | cout << "[Loop found] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl; 324 | cout << "[Loop found] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl; 325 | } 326 | else 327 | { 328 | std::cout.precision(3); 329 | cout << "[Not loop] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl; 330 | cout << "[Not loop] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl; 331 | } 332 | 333 | // To do: return also nn_align (i.e., yaw diff) 334 | float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE); 335 | std::pair result {loop_id, yaw_diff_rad}; 336 | 337 | return result; 338 | 339 | } // SCManager::detectLoopClosureID 340 | 341 | // } // namespace SC2 -------------------------------------------------------------------------------- /src/ikd-Tree/ikd_Tree.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define EPSS 1e-6 14 | #define Minimal_Unbalanced_Tree_Size 10 15 | #define Multi_Thread_Rebuild_Point_Num 1500 16 | #define DOWNSAMPLE_SWITCH true 17 | #define ForceRebuildPercentage 0.2 18 | #define Q_LEN 1000000 19 | 20 | using namespace std; 21 | 22 | struct ikdTree_PointType 23 | { 24 | float x,y,z; 25 | ikdTree_PointType (float px = 0.0f, float py = 0.0f, float pz = 0.0f){ 26 | x = px; 27 | y = py; 28 | z = pz; 29 | } 30 | }; 31 | 32 | struct BoxPointType{ 33 | float vertex_min[3]; 34 | float vertex_max[3]; 35 | }; 36 | 37 | enum operation_set {ADD_POINT, DELETE_POINT, DELETE_BOX, ADD_BOX, DOWNSAMPLE_DELETE, PUSH_DOWN}; 38 | 39 | enum delete_point_storage_set {NOT_RECORD, DELETE_POINTS_REC, MULTI_THREAD_REC}; 40 | 41 | template 42 | class MANUAL_Q{ 43 | private: 44 | int head = 0,tail = 0, counter = 0; 45 | T q[Q_LEN]; 46 | bool is_empty; 47 | public: 48 | void pop(); 49 | T front(); 50 | T back(); 51 | void clear(); 52 | void push(T op); 53 | bool empty(); 54 | int size(); 55 | }; 56 | 57 | 58 | 59 | template 60 | class KD_TREE{ 61 | public: 62 | using PointVector = vector>; 63 | using Ptr = shared_ptr>; 64 | struct KD_TREE_NODE{ 65 | PointType point; 66 | uint8_t division_axis; 67 | int TreeSize = 1; 68 | int invalid_point_num = 0; 69 | int down_del_num = 0; 70 | bool point_deleted = false; 71 | bool tree_deleted = false; 72 | bool point_downsample_deleted = false; 73 | bool tree_downsample_deleted = false; 74 | bool need_push_down_to_left = false; 75 | bool need_push_down_to_right = false; 76 | bool working_flag = false; 77 | float radius_sq; 78 | pthread_mutex_t push_down_mutex_lock; 79 | float node_range_x[2], node_range_y[2], node_range_z[2]; 80 | KD_TREE_NODE *left_son_ptr = nullptr; 81 | KD_TREE_NODE *right_son_ptr = nullptr; 82 | KD_TREE_NODE *father_ptr = nullptr; 83 | // For paper data record 84 | float alpha_del; 85 | float alpha_bal; 86 | }; 87 | 88 | struct Operation_Logger_Type{ 89 | PointType point; 90 | BoxPointType boxpoint; 91 | bool tree_deleted, tree_downsample_deleted; 92 | operation_set op; 93 | }; 94 | 95 | struct PointType_CMP{ 96 | PointType point; 97 | float dist = 0.0; 98 | PointType_CMP (PointType p = PointType(), float d = INFINITY){ 99 | this->point = p; 100 | this->dist = d; 101 | }; 102 | bool operator < (const PointType_CMP &a)const{ 103 | if (fabs(dist - a.dist) < 1e-10) return point.x < a.point.x; 104 | else return dist < a.dist; 105 | } 106 | }; 107 | 108 | class MANUAL_HEAP{ 109 | public: 110 | MANUAL_HEAP(int max_capacity = 100){ 111 | cap = max_capacity; 112 | heap = new PointType_CMP[max_capacity]; 113 | heap_size = 0; 114 | } 115 | 116 | ~MANUAL_HEAP(){ delete[] heap;} 117 | 118 | void pop(){ 119 | if (heap_size == 0) return; 120 | heap[0] = heap[heap_size-1]; 121 | heap_size--; 122 | MoveDown(0); 123 | return; 124 | } 125 | 126 | PointType_CMP top(){ return heap[0];} 127 | 128 | void push(PointType_CMP point){ 129 | if (heap_size >= cap) return; 130 | heap[heap_size] = point; 131 | FloatUp(heap_size); 132 | heap_size++; 133 | return; 134 | } 135 | 136 | int size(){ return heap_size;} 137 | 138 | void clear(){ heap_size = 0;} 139 | private: 140 | int heap_size = 0; 141 | int cap = 0; 142 | PointType_CMP * heap; 143 | void MoveDown(int heap_index){ 144 | int l = heap_index * 2 + 1; 145 | PointType_CMP tmp = heap[heap_index]; 146 | while (l < heap_size){ 147 | if (l + 1 < heap_size && heap[l] < heap[l+1]) l++; 148 | if (tmp < heap[l]){ 149 | heap[heap_index] = heap[l]; 150 | heap_index = l; 151 | l = heap_index * 2 + 1; 152 | } else break; 153 | } 154 | heap[heap_index] = tmp; 155 | return; 156 | } 157 | 158 | void FloatUp(int heap_index){ 159 | int ancestor = (heap_index-1)/2; 160 | PointType_CMP tmp = heap[heap_index]; 161 | while (heap_index > 0){ 162 | if (heap[ancestor] < tmp){ 163 | heap[heap_index] = heap[ancestor]; 164 | heap_index = ancestor; 165 | ancestor = (heap_index-1)/2; 166 | } else break; 167 | } 168 | heap[heap_index] = tmp; 169 | return; 170 | } 171 | 172 | }; 173 | 174 | private: 175 | // Multi-thread Tree Rebuild 176 | bool termination_flag = false; 177 | bool rebuild_flag = false; 178 | pthread_t rebuild_thread; 179 | pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; 180 | pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; 181 | // queue Rebuild_Logger; 182 | MANUAL_Q Rebuild_Logger; 183 | PointVector Rebuild_PCL_Storage; 184 | KD_TREE_NODE ** Rebuild_Ptr = nullptr; 185 | int search_mutex_counter = 0; 186 | static void * multi_thread_ptr(void *arg); 187 | void multi_thread_rebuild(); 188 | void start_thread(); 189 | void stop_thread(); 190 | void run_operation(KD_TREE_NODE ** root, Operation_Logger_Type operation); 191 | // KD Tree Functions and augmented variables 192 | int Treesize_tmp = 0, Validnum_tmp = 0; 193 | float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; 194 | float delete_criterion_param = 0.5f; 195 | float balance_criterion_param = 0.7f; 196 | float downsample_size = 0.2f; 197 | bool Delete_Storage_Disabled = false; 198 | KD_TREE_NODE * STATIC_ROOT_NODE = nullptr; 199 | PointVector Points_deleted; 200 | PointVector Downsample_Storage; 201 | PointVector Multithread_Points_deleted; 202 | void InitTreeNode(KD_TREE_NODE * root); 203 | void Test_Lock_States(KD_TREE_NODE *root); 204 | void BuildTree(KD_TREE_NODE ** root, int l, int r, PointVector & Storage); 205 | void Rebuild(KD_TREE_NODE ** root); 206 | int Delete_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample); 207 | void Delete_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild); 208 | void Add_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild, int father_axis); 209 | void Add_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild); 210 | void Search(KD_TREE_NODE * root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist);//priority_queue 211 | void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage); 212 | void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage); 213 | bool Criterion_Check(KD_TREE_NODE * root); 214 | void Push_Down(KD_TREE_NODE * root); 215 | void Update(KD_TREE_NODE * root); 216 | void delete_tree_nodes(KD_TREE_NODE ** root); 217 | void downsample(KD_TREE_NODE ** root); 218 | bool same_point(PointType a, PointType b); 219 | float calc_dist(PointType a, PointType b); 220 | float calc_box_dist(KD_TREE_NODE * node, PointType point); 221 | static bool point_cmp_x(PointType a, PointType b); 222 | static bool point_cmp_y(PointType a, PointType b); 223 | static bool point_cmp_z(PointType a, PointType b); 224 | 225 | public: 226 | KD_TREE(float delete_param = 0.5, float balance_param = 0.6 , float box_length = 0.2); 227 | ~KD_TREE(); 228 | void Set_delete_criterion_param(float delete_param); 229 | void Set_balance_criterion_param(float balance_param); 230 | void set_downsample_param(float box_length); 231 | void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2); 232 | int size(); 233 | int validnum(); 234 | void root_alpha(float &alpha_bal, float &alpha_del); 235 | void Build(PointVector point_cloud); 236 | void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector & Point_Distance, double max_dist = INFINITY); 237 | void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage); 238 | void Radius_Search(PointType point, const float radius, PointVector &Storage); 239 | int Add_Points(PointVector & PointToAdd, bool downsample_on); 240 | void Add_Point_Boxes(vector & BoxPoints); 241 | void Delete_Points(PointVector & PointToDel); 242 | int Delete_Point_Boxes(vector & BoxPoints); 243 | void flatten(KD_TREE_NODE * root, PointVector &Storage, delete_point_storage_set storage_type); 244 | void acquire_removed_points(PointVector & removed_points); 245 | BoxPointType tree_range(); 246 | PointVector PCL_Storage; 247 | KD_TREE_NODE * Root_Node = nullptr; 248 | int max_queue_size = 0; 249 | }; 250 | 251 | 252 | -------------------------------------------------------------------------------- /src/image_handler.h: -------------------------------------------------------------------------------- 1 | // Modified from https://github.com/TixiaoShan/imaging_lidar_place_recognition 2 | #pragma once 3 | #include "parameters.h" 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace intensity_slam{ 13 | class ImageHandler 14 | { 15 | public: 16 | 17 | ros::NodeHandle nh; 18 | 19 | ros::Publisher pub_image; 20 | ros::Publisher groundPoint_pub; 21 | 22 | cv::Mat image_range; 23 | cv::Mat image_ambient; 24 | cv::Mat image_intensity; 25 | int IMAGE_HEIGHT, IMAGE_WIDTH, NUM_THREADS; 26 | 27 | pcl::PointCloud::Ptr cloud_track; 28 | pcl::PointCloud::Ptr GroundPointOut; 29 | ImageHandler(int height=128, int width=1024, int threadNum=6) 30 | { 31 | IMAGE_HEIGHT = height; 32 | IMAGE_WIDTH = width; 33 | NUM_THREADS = threadNum; 34 | cloud_track.reset(new pcl::PointCloud()); 35 | cloud_track->resize(IMAGE_HEIGHT * IMAGE_WIDTH); 36 | GroundPointOut.reset(new pcl::PointCloud); 37 | groundPoint_pub = nh.advertise("/ransac_groundPoint",1000); 38 | 39 | } 40 | 41 | void groundPlaneExtraction(const sensor_msgs::PointCloud2ConstPtr &cloud_msg){ 42 | GroundPointOut->points.clear(); 43 | pcl::PointCloud::Ptr point_cloud_original_ptr(new pcl::PointCloud); 44 | pcl::fromROSMsg(*cloud_msg, *point_cloud_original_ptr); 45 | 46 | assert((int)point_cloud_original_ptr->size() % IMAGE_HEIGHT * IMAGE_WIDTH == 0); 47 | pcl::PointCloud::Ptr point_cloud_screening_ptr(new pcl::PointCloud); 48 | #pragma omp parallel for num_threads(NUM_THREADS) 49 | for (long unsigned int i = 0; i < point_cloud_original_ptr->points.size(); i++) 50 | { 51 | 52 | 53 | if (point_cloud_original_ptr->points[i].z >= -2.0 && point_cloud_original_ptr->points[i].z <= -0.45) 54 | { 55 | point_cloud_screening_ptr->points.push_back(point_cloud_original_ptr->points[i]); 56 | } 57 | } 58 | pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 59 | pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 60 | pcl::SACSegmentation seg; 61 | seg.setOptimizeCoefficients (true); 62 | seg.setModelType (pcl::SACMODEL_PLANE); 63 | seg.setMethodType (pcl::SAC_RANSAC); 64 | seg.setDistanceThreshold (0.01); 65 | seg.setAxis(Eigen::Vector3f(0,0,1)); 66 | seg.setEpsAngle(15 *M_PI/180); 67 | seg.setNumberOfThreads(2*NUM_THREADS); 68 | seg.setInputCloud (point_cloud_screening_ptr); 69 | seg.segment (*inliers, *coefficients); 70 | pcl::PointCloud::Ptr GroundPoint_ptr(new pcl::PointCloud); 71 | 72 | double A = coefficients->values[0]; 73 | double B = coefficients->values[1]; 74 | double C = coefficients->values[2]; 75 | double D = coefficients->values[3]; 76 | 77 | Eigen::Vector3f plane_normal(A, B, C); 78 | Eigen::Vector3f z_normal(0, 0, 1); 79 | if (plane_normal.dot(z_normal) > cos(15 *M_PI/180)) 80 | { 81 | #pragma omp parallel for num_threads(NUM_THREADS) 82 | for (long unsigned int i = 0; i < point_cloud_original_ptr->points.size(); i++) 83 | { 84 | double X = point_cloud_original_ptr->points[i].x; 85 | double Y = point_cloud_original_ptr->points[i].y; 86 | double Z = point_cloud_original_ptr->points[i].z; 87 | double height = abs(A*X + B*Y + C*Z + D) / sqrt(A*A + B*B + C*C); 88 | 89 | if ((height <= 0.03) && (point_cloud_original_ptr->points[i].z < -0.0) ) 90 | { 91 | GroundPoint_ptr->points.push_back(point_cloud_original_ptr->points[i]); 92 | } 93 | } 94 | 95 | GroundPointOut = GroundPoint_ptr; 96 | if(groundPoint_pub.getNumSubscribers() != 0){ 97 | sensor_msgs::PointCloud2 groundPoint_cloud_msg; 98 | pcl::toROSMsg(*GroundPoint_ptr, groundPoint_cloud_msg); 99 | groundPoint_cloud_msg.header = cloud_msg->header; 100 | groundPoint_pub.publish(groundPoint_cloud_msg); 101 | } 102 | } 103 | } 104 | void cloud_handler(const sensor_msgs::PointCloud2ConstPtr &cloud_msg) 105 | { 106 | pcl::PointCloud::Ptr laser_cloud(new pcl::PointCloud()); 107 | pcl::fromROSMsg(*cloud_msg, *laser_cloud); 108 | assert((int)laser_cloud->size() % IMAGE_HEIGHT * IMAGE_WIDTH == 0); 109 | image_range = cv::Mat(IMAGE_HEIGHT, IMAGE_WIDTH, CV_8UC1, cv::Scalar(0)); 110 | image_ambient = cv::Mat(IMAGE_HEIGHT, IMAGE_WIDTH, CV_8UC1, cv::Scalar(0)); 111 | image_intensity = cv::Mat(IMAGE_HEIGHT, IMAGE_WIDTH, CV_8UC1, cv::Scalar(0)); 112 | 113 | #pragma omp parallel for num_threads(NUM_THREADS) 114 | for (int u = 0; u < IMAGE_HEIGHT; u++) 115 | { 116 | for (int v = 0; v < IMAGE_WIDTH; v++) 117 | { 118 | const auto& pt = laser_cloud->points[u * IMAGE_WIDTH + v]; 119 | float range = std::sqrt(pt.x*pt.x + pt.y*pt.y + pt.z*pt.z); 120 | float intensity = pt.intensity; 121 | 122 | intensity = std::min(intensity, 255.0f); 123 | image_range.at(u, v) = std::min(range * 20, 255.0f); 124 | image_intensity.at(u, v) = intensity; 125 | PointType* p = &cloud_track->points[u * IMAGE_WIDTH + v]; 126 | 127 | if (range >= 0.1) 128 | { 129 | p->x = pt.x; 130 | p->y = pt.y; 131 | p->z = pt.z; 132 | p->intensity = intensity; 133 | 134 | } 135 | else 136 | { 137 | p->x = p->y = p->z = 0; 138 | 139 | p->intensity = 0; 140 | } 141 | } 142 | } 143 | 144 | } 145 | 146 | 147 | }; 148 | 149 | }//namespace 150 | -------------------------------------------------------------------------------- /src/intensity_feature_tracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "parameters.h" 3 | #include 4 | #include 5 | #include 6 | #include "tic_toc.h" 7 | #include 8 | #include 9 | #include "keyframe.h" 10 | #include "loop_closure_handler.h" 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | 35 | namespace intensity_slam{ 36 | 37 | class feature_tracker{ 38 | public: 39 | feature_tracker(ros::NodeHandle& n); 40 | ~feature_tracker(); 41 | void readParameters(); 42 | void detectfeatures(ros::Time &time, 43 | const cv::Mat &image_intensity, 44 | const pcl::PointCloud::Ptr cloud_track, 45 | const pcl::PointCloud::Ptr groundPoint_); 46 | void detectcorners( ros::Time &time, 47 | const cv::Mat &image_intensity, 48 | const pcl::PointCloud::Ptr cloud_track); 49 | void setMask(); 50 | void keypoint2uv(); 51 | std::vector keypoint2uv(std::vector cur_keypoints); 52 | void extractPointsAndFilterZeroValue(); 53 | void extractPointsAndFilterZeroValue(std::vector cur_orb_point_2d_uv, pcl::PointCloud::Ptr cloudTrack, std::vector &cur_out_point3d, std::vector &status); 54 | 55 | void image_show(std::vector &matches, std::string& detectTime); 56 | void image_show(std::vector &matches, std::string& detectTime, cv::Mat prev_img_, cv::Mat cur_img_, std::vector cur_orb_point_2d_uv_, std::vector prev_orb_point_2d_uv_); 57 | void extractMatchedPoints(std::vector &matches, std::vector &prev_points, std::vector &cur_points, std::vector &prev_out_point3d, std::vector &cur_out_point3d); 58 | Eigen::Matrix4d p2p_calculateRandT(std::vector &src_cloud, std::vector &dst_cloud); 59 | void tfBroadcast(); 60 | void getKeyframe(double time, std::vector &featurMatchedpoints); 61 | void loopClosureThread(); 62 | void mapOdomHandle(); 63 | void factorGraphThread(); 64 | void icpThread(loopClosureProcessor::Keyframe keyframeNew); 65 | void updatePoses(); 66 | void writeTF2file(ros::Time &time); 67 | void getSubmapOfhistory(pcl::PointCloud::Ptr cloud_submap); 68 | void tranformCurrentScanToMap(pcl::PointCloud::Ptr cloud_current_scan, size_t index, loopClosureProcessor::Keyframe keyframe_new); 69 | Eigen::Matrix4d getTransformMatrix(size_t index); 70 | 71 | 72 | std::string PROJECT_NAME; 73 | std::string CLOUD_TOPIC; 74 | int IMAGE_WIDTH; 75 | int IMAGE_HEIGHT; 76 | int IMAGE_CROP; 77 | int USE_ORB; 78 | int NUM_ORB_FEATURES; 79 | double SKIP_TIME; 80 | int NUM_THREADS; 81 | bool HAND_HELD_FLAG; 82 | bool USE_TEASER; 83 | bool USE_PNPRANSAC; 84 | bool USE_ICP; 85 | bool USE_CROP; 86 | bool USE_DOWNSAMPLE; 87 | double CROP_SIZE; 88 | double VOXEL_SIZE; 89 | double FITNESS_SCORE; 90 | double KEYFRAME_TIME_INTERVAL, KEYFRAME_DISTANCE_INTERVAL; 91 | 92 | cv::Mat MASK; 93 | std::vector cur_orb_point_2d_uv_, prev_orb_point_2d_uv_; 94 | pcl::PointCloud::Ptr cloudTrack_; 95 | pcl::PointCloud cur_cloud_, prev_cloud_; 96 | pcl::PointCloud ground_cloud_; 97 | std::vector status_; 98 | std::vector cur_out_point3d_, prev_out_point3d_; 99 | double first_frame_time_; 100 | ros::Time time_stamp_; 101 | int frequency_; 102 | size_t keyframeId_; 103 | loopClosureHandler loopClosureProcessor_; 104 | std::thread icpProcessThread_; 105 | bool getLoop_; 106 | pcl::PointCloud::Ptr cloudKeyPoses3D_; 107 | pcl::PointCloud::Ptr cloudKeyPoses6D_; 108 | loopClosureProcessor::Keyframe keyframe_; 109 | // std::deque factorGraphNode_; 110 | // std::deque keyframesQueue_; 111 | 112 | std::deque> factorGraphNode_; 113 | std::deque> keyframesQueue_; 114 | std::queue mapOdomQueue_; 115 | ros::Publisher pubLaserOdometry_; 116 | ros::Publisher matched_keypoints_img_pub_front_end; 117 | 118 | 119 | // std::deque> mavros_to_batman_msg_; 120 | pcl::PointCloud::Ptr current_scan_; 121 | pcl::PointCloud::Ptr loop_scan_; 122 | bool skip_intensity_; 123 | double feature_extraction_time_; 124 | double scan_matching_time_; 125 | 126 | 127 | 128 | 129 | 130 | 131 | private: 132 | ros::NodeHandle nh_; 133 | ros::Publisher pubKeyPoses_; 134 | ros::Publisher pubLoopScan_, pubCurrentScan_; 135 | cv::Mat prev_img_, cur_img_;//intensity image 136 | std::vector prev_keypoints_, cur_keypoints_; 137 | cv::Mat prev_descriptors_, cur_descriptors_; 138 | boost::shared_ptr viewer_; 139 | Eigen::Matrix4d T_s2s_, T_s2m_, T_s2pgo_;//T_s2s_: local scan to scan transform R+T; T_s2m_: global transformation matrix; T_s2pgo_: global transformation matrix after pgo 140 | Eigen::Vector3d cur_position_, prev_position_; 141 | Eigen::Matrix3d cur_rotation_, prev_rotation_; 142 | std::vector> corres_; 143 | 144 | gtsam::NonlinearFactorGraph gtSAMgraph_; 145 | gtsam::Values initialEstimate_; 146 | gtsam::Values optimizedEstimate_; 147 | gtsam::ISAM2 *isam_; 148 | gtsam::Values isamCurrentEstimate_; 149 | 150 | gtsam::noiseModel::Diagonal::shared_ptr priorNoise_; 151 | gtsam::noiseModel::Diagonal::shared_ptr odometryNoise_; 152 | gtsam::noiseModel::Diagonal::shared_ptr constraintNoise_; 153 | gtsam::noiseModel::Base::shared_ptr robustNoiseModel_; 154 | std::mutex factorMutex; 155 | 156 | Eigen::Quaterniond graph_prev_rotation_; 157 | Eigen::Vector3d graph_prev_translation_; 158 | int loop_index_kdtree_; 159 | bool skip_flag_; 160 | double pgo_time_; 161 | double loop_time_; 162 | 163 | 164 | 165 | 166 | }; 167 | } -------------------------------------------------------------------------------- /src/intensity_feature_tracker_node.cpp: -------------------------------------------------------------------------------- 1 | #include "intensity_feature_tracker.h" 2 | #include "image_handler.h" 3 | #include "tic_toc.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | intensity_slam::ImageHandler *image_handler; 14 | intensity_slam::feature_tracker *feature_tracker; 15 | std::mutex mapOdomMutex; 16 | void map_odom_callback(const nav_msgs::Odometry::ConstPtr &mapOdometry){ 17 | mapOdomMutex.lock(); 18 | feature_tracker->mapOdomQueue_.push(mapOdometry); 19 | mapOdomMutex.unlock(); 20 | } 21 | 22 | void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg) 23 | { 24 | TicToc covert_pointcloud2Intensity_time; 25 | 26 | #pragma omp parallel sections num_threads(4) 27 | { 28 | #pragma omp section 29 | image_handler->cloud_handler(cloud_msg); //5ms 30 | } 31 | ros::Time cloud_time = cloud_msg->header.stamp; 32 | TicToc detectFeatures_time; 33 | feature_tracker->detectfeatures(cloud_time, image_handler->image_intensity, image_handler->cloud_track, image_handler->GroundPointOut); 34 | } 35 | 36 | int main(int argc, char **argv){ 37 | ros::init(argc, argv, "intensity_feature_tracker"); 38 | ros::NodeHandle n("~"); 39 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); 40 | feature_tracker = new intensity_slam::feature_tracker(n); 41 | 42 | // Load params 43 | std::cout << "read parameters" << std::endl; 44 | feature_tracker->readParameters(); 45 | 46 | image_handler = new intensity_slam::ImageHandler(feature_tracker->IMAGE_HEIGHT, 47 | feature_tracker->IMAGE_WIDTH, 48 | feature_tracker->NUM_THREADS); 49 | if(true){ 50 | std::cout << "start subscribe topic: "<< feature_tracker->CLOUD_TOPIC << std::endl; 51 | ros::Subscriber sub_point = n.subscribe(feature_tracker->CLOUD_TOPIC, 1, cloud_callback); 52 | ros::Subscriber sub_map_path = n.subscribe("/aft_mapped_to_init_high_frec", 100, map_odom_callback); 53 | ros::MultiThreadedSpinner spinner(8); 54 | spinner.spin(); 55 | } 56 | 57 | return 0; 58 | } 59 | 60 | -------------------------------------------------------------------------------- /src/keyframe.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "parameters.h" 7 | 8 | namespace mapProcessor{ 9 | struct Keyframe 10 | { 11 | 12 | size_t keyframeId; 13 | pcl::PointCloud cloud_track_; 14 | pcl::PointCloud ground_cloud_; 15 | Eigen::Quaterniond q_map_cur_k_; 16 | Eigen::Vector3d t_map_cur_k_; 17 | 18 | 19 | Keyframe(){} 20 | 21 | Keyframe( 22 | size_t keyframeIdTmp, 23 | pcl::PointCloud cloud, 24 | pcl::PointCloud cloud_ground, 25 | Eigen::Quaterniond q_map_cur_k, 26 | Eigen::Vector3d t_map_cur_k): 27 | keyframeId{keyframeIdTmp}, 28 | cloud_track_{cloud}, ground_cloud_{cloud_ground}, q_map_cur_k_{q_map_cur_k}, t_map_cur_k_{t_map_cur_k} 29 | {} 30 | }; 31 | struct SlideWindowKeyframe 32 | { 33 | 34 | cv::Mat descriptors; // for detecting matches between two keyframes 35 | cv::Mat image_intensity; // for visualization 36 | std::vector orb_point_2d_uv;// for visualization 37 | Eigen::Quaterniond q_map_cur_tk; // for convert point cloud to map frame 38 | Eigen::Vector3d t_map_cur_tk; // for convert point cloud to map frame 39 | std::vector cur_point3d_wrt_orb_features; // 3D point cloud in lidar frame 40 | 41 | SlideWindowKeyframe(){} 42 | SlideWindowKeyframe(cv::Mat descriptorsTmp, cv::Mat image_intensityTmp, std::vector orb_point_2d_uvTmp, Eigen::Quaterniond q_map_cur_tkTmp, Eigen::Vector3d t_map_cur_tkTmp, std::vector cur_point3d_wrt_orb_featuresTmp): 43 | descriptors{descriptorsTmp}, image_intensity{image_intensityTmp}, orb_point_2d_uv{orb_point_2d_uvTmp}, q_map_cur_tk{q_map_cur_tkTmp}, t_map_cur_tk{t_map_cur_tkTmp}, cur_point3d_wrt_orb_features{cur_point3d_wrt_orb_featuresTmp} 44 | {} 45 | 46 | }; 47 | } 48 | 49 | 50 | namespace loopClosureProcessor{ 51 | struct Keyframe 52 | { 53 | double time; 54 | size_t keyframeId; 55 | cv::Mat image; 56 | std::vector keypoints; 57 | cv::Mat descriptors; 58 | std::vector featureMatched3DPoints; 59 | Eigen::Vector3d cur_position_; 60 | Eigen::Vector3d prev_position_; 61 | Eigen::Matrix3d cur_rotation_; 62 | Eigen::Matrix3d prev_rotation_; 63 | pcl::PointCloud cloud_track_; 64 | 65 | Keyframe(){} 66 | 67 | Keyframe( double timeTmp, 68 | size_t keyframeIdTmp, 69 | cv::Mat imageTmp, 70 | std::vector keypointsTmp, 71 | cv::Mat descriptorsTmp, 72 | std::vector featureMatched3DPointsTmp, 73 | Eigen::Vector3d cur_position_Tmp, 74 | Eigen::Vector3d prev_position_Tmp, 75 | Eigen::Matrix3d cur_rotation_Tmp, 76 | Eigen::Matrix3d prev_rotation_Tmp, 77 | pcl::PointCloud cloud): 78 | time{timeTmp}, keyframeId{keyframeIdTmp}, image{imageTmp}, keypoints{keypointsTmp}, descriptors{descriptorsTmp}, featureMatched3DPoints{featureMatched3DPointsTmp}, cur_position_{cur_position_Tmp}, prev_position_{prev_position_Tmp}, cur_rotation_{cur_rotation_Tmp}, prev_rotation_{prev_rotation_Tmp}, 79 | cloud_track_{cloud} 80 | {} 81 | 82 | 83 | 84 | }; 85 | 86 | struct FactorGraphNode 87 | { 88 | double time; 89 | size_t keyframeId; 90 | Eigen::Vector3d cur_position_; 91 | Eigen::Vector3d prev_position_; 92 | Eigen::Matrix3d cur_rotation_; 93 | Eigen::Matrix3d prev_rotation_; 94 | 95 | // construction function 96 | FactorGraphNode( double timeTmp, 97 | size_t keyframeIdTmp, 98 | Eigen::Vector3d cur_position_Tmp, 99 | Eigen::Vector3d prev_position_Tmp, 100 | Eigen::Matrix3d cur_rotation_Tmp, 101 | Eigen::Matrix3d prev_rotation_Tmp): 102 | 103 | time{timeTmp}, keyframeId{keyframeIdTmp}, cur_position_{cur_position_Tmp},prev_position_{prev_position_Tmp}, cur_rotation_{cur_rotation_Tmp}, prev_rotation_{prev_rotation_Tmp} 104 | {} 105 | FactorGraphNode(){} 106 | 107 | 108 | 109 | 110 | }; 111 | 112 | 113 | } -------------------------------------------------------------------------------- /src/laserOdometry.cpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the following paper: 2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | // Modifier: Tong Qin qintonguav@gmail.com 6 | // Shaozu Cao saozu.cao@connect.ust.hk 7 | 8 | 9 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 10 | // Further contributions copyright (c) 2016, Southwest Research Institute 11 | // All rights reserved. 12 | // 13 | // Redistribution and use in source and binary forms, with or without 14 | // modification, are permitted provided that the following conditions are met: 15 | // 16 | // 1. Redistributions of source code must retain the above copyright notice, 17 | // this list of conditions and the following disclaimer. 18 | // 2. Redistributions in binary form must reproduce the above copyright notice, 19 | // this list of conditions and the following disclaimer in the documentation 20 | // and/or other materials provided with the distribution. 21 | // 3. Neither the name of the copyright holder nor the names of its 22 | // contributors may be used to endorse or promote products derived from this 23 | // software without specific prior written permission. 24 | // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 28 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 29 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | // POSSIBILITY OF SUCH DAMAGE. 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | #include "parameters.h" 56 | #include "lidarFeaturePointsFunction.hpp" 57 | #include "tic_toc.h" 58 | // #include "intensity_feature_tracker.h" 59 | // #include "image_handler.h" 60 | 61 | #define DISTORTION 0 62 | 63 | 64 | int corner_correspondence = 0, plane_correspondence = 0; 65 | 66 | constexpr double SCAN_PERIOD = 0.1; 67 | constexpr double DISTANCE_SQ_THRESHOLD = 25; 68 | constexpr double NEARBY_SCAN = 2.5; 69 | 70 | int skipFrameNum = 5; 71 | bool systemInited = false; 72 | 73 | double timeCornerPointsSharp = 0; 74 | double timeCornerPointsLessSharp = 0; 75 | double timeSurfPointsFlat = 0; 76 | double timeSurfPointsLessFlat = 0; 77 | double timeLaserCloudFullRes = 0; 78 | 79 | pcl::KdTreeFLANN::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN()); 80 | pcl::KdTreeFLANN::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN()); 81 | 82 | std::string skip_flag; 83 | pcl::PointCloud::Ptr cornerPointsSharp(new pcl::PointCloud()); 84 | pcl::PointCloud::Ptr cornerPointsLessSharp(new pcl::PointCloud()); 85 | pcl::PointCloud::Ptr surfPointsFlat(new pcl::PointCloud()); 86 | pcl::PointCloud::Ptr surfPointsLessFlat(new pcl::PointCloud()); 87 | 88 | pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud()); 89 | pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud()); 90 | pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); 91 | 92 | int laserCloudCornerLastNum = 0; 93 | int laserCloudSurfLastNum = 0; 94 | 95 | // Transformation from current frame to world frame 96 | Eigen::Quaterniond q_w_curr(1, 0, 0, 0); 97 | Eigen::Vector3d t_w_curr(0, 0, 0); 98 | 99 | // q_curr_last(x, y, z, w), t_curr_last 100 | double para_q[4] = {0, 0, 0, 1}; 101 | double para_t[3] = {0, 0, 0}; 102 | 103 | Eigen::Map q_last_curr(para_q); 104 | Eigen::Map t_last_curr(para_t); 105 | 106 | std::queue cornerSharpBuf; 107 | std::queue cornerLessSharpBuf; 108 | std::queue surfFlatBuf; 109 | std::queue surfLessFlatBuf; 110 | std::queue fullPointsBuf; 111 | std::mutex mBuf; 112 | 113 | // undistort lidar point 114 | void TransformToStart(PointType const *const pi, PointType *const po) 115 | { 116 | //interpolation ratio 117 | double s; 118 | if (DISTORTION) 119 | s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD; 120 | else 121 | s = 1.0; 122 | //s = 1; 123 | Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr); 124 | Eigen::Vector3d t_point_last = s * t_last_curr; 125 | Eigen::Vector3d point(pi->x, pi->y, pi->z); 126 | Eigen::Vector3d un_point = q_point_last * point + t_point_last; 127 | 128 | po->x = un_point.x(); 129 | po->y = un_point.y(); 130 | po->z = un_point.z(); 131 | po->intensity = pi->intensity; 132 | } 133 | 134 | // transform all lidar points to the start of the next frame 135 | 136 | void TransformToEnd(PointType const *const pi, PointType *const po) 137 | { 138 | // undistort point first 139 | pcl::PointXYZI un_point_tmp; 140 | TransformToStart(pi, &un_point_tmp); 141 | 142 | Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z); 143 | Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr); 144 | 145 | po->x = point_end.x(); 146 | po->y = point_end.y(); 147 | po->z = point_end.z(); 148 | 149 | //Remove distortion time info 150 | po->intensity = int(pi->intensity); 151 | } 152 | 153 | void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2) 154 | { 155 | mBuf.lock(); 156 | cornerSharpBuf.push(cornerPointsSharp2); 157 | mBuf.unlock(); 158 | } 159 | 160 | void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2) 161 | { 162 | mBuf.lock(); 163 | cornerLessSharpBuf.push(cornerPointsLessSharp2); 164 | mBuf.unlock(); 165 | } 166 | 167 | void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2) 168 | { 169 | mBuf.lock(); 170 | surfFlatBuf.push(surfPointsFlat2); 171 | mBuf.unlock(); 172 | } 173 | 174 | void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2) 175 | { 176 | mBuf.lock(); 177 | surfLessFlatBuf.push(surfPointsLessFlat2); 178 | mBuf.unlock(); 179 | } 180 | 181 | //receive all point cloud 182 | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2) 183 | { 184 | mBuf.lock(); 185 | fullPointsBuf.push(laserCloudFullRes2); 186 | mBuf.unlock(); 187 | } 188 | 189 | int main(int argc, char **argv) 190 | { 191 | ros::init(argc, argv, "laserOdometry"); 192 | ros::NodeHandle nh; 193 | 194 | nh.param("mapping_skip_frame", skipFrameNum, 1); 195 | 196 | // printf("Mapping %d Hz \n", 10 / skipFrameNum); 197 | 198 | ros::Subscriber subCornerPointsSharp = nh.subscribe("/laser_cloud_sharp", 100, laserCloudSharpHandler); 199 | 200 | ros::Subscriber subCornerPointsLessSharp = nh.subscribe("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler); 201 | 202 | ros::Subscriber subSurfPointsFlat = nh.subscribe("/laser_cloud_flat", 100, laserCloudFlatHandler); 203 | 204 | ros::Subscriber subSurfPointsLessFlat = nh.subscribe("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler); 205 | 206 | ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_2", 100, laserCloudFullResHandler); 207 | 208 | ros::Publisher pubLaserCloudCornerLast = nh.advertise("/laser_cloud_corner_last", 100); 209 | 210 | ros::Publisher pubLaserCloudSurfLast = nh.advertise("/laser_cloud_surf_last", 100); 211 | 212 | ros::Publisher pubLaserCloudFullRes = nh.advertise("/velodyne_cloud_3", 100); 213 | 214 | ros::Publisher pubLaserOdometry = nh.advertise("/laser_odom_to_init_aloam", 100); 215 | 216 | ros::Publisher pubLaserPath = nh.advertise("/laser_odom_path", 100); 217 | 218 | nav_msgs::Path laserPath; 219 | 220 | int frameCount = 0; 221 | ros::Rate rate(100); 222 | 223 | while (ros::ok()) 224 | { 225 | ros::spinOnce(); 226 | 227 | if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() && 228 | !surfFlatBuf.empty() && !surfLessFlatBuf.empty() && 229 | !fullPointsBuf.empty()) 230 | { 231 | timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec(); 232 | timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec(); 233 | timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec(); 234 | timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec(); 235 | timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec(); 236 | 237 | if (timeCornerPointsSharp != timeLaserCloudFullRes || 238 | timeCornerPointsLessSharp != timeLaserCloudFullRes || 239 | timeSurfPointsFlat != timeLaserCloudFullRes || 240 | timeSurfPointsLessFlat != timeLaserCloudFullRes) 241 | { 242 | printf("unsync messeage!"); 243 | ROS_BREAK(); 244 | } 245 | 246 | mBuf.lock(); 247 | cornerPointsSharp->clear(); 248 | pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp); 249 | skip_flag = cornerSharpBuf.front()->header.frame_id; 250 | ROS_INFO("skip_flag: %s", skip_flag.c_str()); 251 | cornerSharpBuf.pop(); 252 | 253 | cornerPointsLessSharp->clear(); 254 | pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp); 255 | cornerLessSharpBuf.pop(); 256 | 257 | surfPointsFlat->clear(); 258 | pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat); 259 | surfFlatBuf.pop(); 260 | 261 | surfPointsLessFlat->clear(); 262 | pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat); 263 | surfLessFlatBuf.pop(); 264 | 265 | laserCloudFullRes->clear(); 266 | pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes); 267 | fullPointsBuf.pop(); 268 | mBuf.unlock(); 269 | 270 | TicToc t_whole; 271 | // initializing 272 | if (!systemInited) 273 | { 274 | systemInited = true; 275 | std::cout << "Initialization finished \n"; 276 | } 277 | else 278 | { 279 | int cornerPointsSharpNum = cornerPointsSharp->points.size(); 280 | int surfPointsFlatNum = surfPointsFlat->points.size(); 281 | 282 | TicToc t_opt; 283 | bool use_aloam; 284 | if(skip_flag == "skip_intensity"){ 285 | use_aloam = true; 286 | 287 | } 288 | else{ 289 | use_aloam = false; 290 | } 291 | for (size_t opti_counter = 0; opti_counter < 2 && use_aloam; ++opti_counter) 292 | { 293 | corner_correspondence = 0; 294 | plane_correspondence = 0; 295 | 296 | //ceres::LossFunction *loss_function = NULL; 297 | ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); 298 | ceres::LocalParameterization *q_parameterization = 299 | new ceres::EigenQuaternionParameterization(); 300 | ceres::Problem::Options problem_options; 301 | 302 | ceres::Problem problem(problem_options); 303 | problem.AddParameterBlock(para_q, 4, q_parameterization); 304 | problem.AddParameterBlock(para_t, 3); 305 | 306 | pcl::PointXYZI pointSel; 307 | std::vector pointSearchInd; 308 | std::vector pointSearchSqDis; 309 | 310 | TicToc t_data; 311 | // find correspondence for corner features 312 | for (int i = 0; i < cornerPointsSharpNum; ++i) 313 | { 314 | TransformToStart(&(cornerPointsSharp->points[i]), &pointSel); 315 | kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); 316 | 317 | int closestPointInd = -1, minPointInd2 = -1; 318 | if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) 319 | { 320 | closestPointInd = pointSearchInd[0]; 321 | int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity); 322 | 323 | double minPointSqDis2 = DISTANCE_SQ_THRESHOLD; 324 | // search in the direction of increasing scan line 325 | for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j) 326 | { 327 | // if in the same scan line, continue 328 | if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID) 329 | continue; 330 | 331 | // if not in nearby scans, end the loop 332 | if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN)) 333 | break; 334 | 335 | double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 336 | (laserCloudCornerLast->points[j].x - pointSel.x) + 337 | (laserCloudCornerLast->points[j].y - pointSel.y) * 338 | (laserCloudCornerLast->points[j].y - pointSel.y) + 339 | (laserCloudCornerLast->points[j].z - pointSel.z) * 340 | (laserCloudCornerLast->points[j].z - pointSel.z); 341 | 342 | if (pointSqDis < minPointSqDis2) 343 | { 344 | // find nearer point 345 | minPointSqDis2 = pointSqDis; 346 | minPointInd2 = j; 347 | } 348 | } 349 | 350 | // search in the direction of decreasing scan line 351 | for (int j = closestPointInd - 1; j >= 0; --j) 352 | { 353 | // if in the same scan line, continue 354 | if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID) 355 | continue; 356 | 357 | // if not in nearby scans, end the loop 358 | if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN)) 359 | break; 360 | 361 | double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 362 | (laserCloudCornerLast->points[j].x - pointSel.x) + 363 | (laserCloudCornerLast->points[j].y - pointSel.y) * 364 | (laserCloudCornerLast->points[j].y - pointSel.y) + 365 | (laserCloudCornerLast->points[j].z - pointSel.z) * 366 | (laserCloudCornerLast->points[j].z - pointSel.z); 367 | 368 | if (pointSqDis < minPointSqDis2) 369 | { 370 | // find nearer point 371 | minPointSqDis2 = pointSqDis; 372 | minPointInd2 = j; 373 | } 374 | } 375 | } 376 | if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid 377 | { 378 | Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x, 379 | cornerPointsSharp->points[i].y, 380 | cornerPointsSharp->points[i].z); 381 | Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x, 382 | laserCloudCornerLast->points[closestPointInd].y, 383 | laserCloudCornerLast->points[closestPointInd].z); 384 | Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x, 385 | laserCloudCornerLast->points[minPointInd2].y, 386 | laserCloudCornerLast->points[minPointInd2].z); 387 | 388 | double s; 389 | if (DISTORTION) 390 | s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD; 391 | else 392 | s = 1.0; 393 | ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s); 394 | problem.AddResidualBlock(cost_function, loss_function, para_q, para_t); 395 | corner_correspondence++; 396 | } 397 | } 398 | 399 | // find correspondence for plane features 400 | for (int i = 0; i < surfPointsFlatNum; ++i) 401 | { 402 | TransformToStart(&(surfPointsFlat->points[i]), &pointSel); 403 | kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); 404 | 405 | int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1; 406 | if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) 407 | { 408 | closestPointInd = pointSearchInd[0]; 409 | 410 | // get closest point's scan ID 411 | int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity); 412 | double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD; 413 | 414 | // search in the direction of increasing scan line 415 | for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j) 416 | { 417 | // if not in nearby scans, end the loop 418 | if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN)) 419 | break; 420 | 421 | double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 422 | (laserCloudSurfLast->points[j].x - pointSel.x) + 423 | (laserCloudSurfLast->points[j].y - pointSel.y) * 424 | (laserCloudSurfLast->points[j].y - pointSel.y) + 425 | (laserCloudSurfLast->points[j].z - pointSel.z) * 426 | (laserCloudSurfLast->points[j].z - pointSel.z); 427 | 428 | // if in the same or lower scan line 429 | if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2) 430 | { 431 | minPointSqDis2 = pointSqDis; 432 | minPointInd2 = j; 433 | } 434 | // if in the higher scan line 435 | else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3) 436 | { 437 | minPointSqDis3 = pointSqDis; 438 | minPointInd3 = j; 439 | } 440 | } 441 | 442 | // search in the direction of decreasing scan line 443 | for (int j = closestPointInd - 1; j >= 0; --j) 444 | { 445 | // if not in nearby scans, end the loop 446 | if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN)) 447 | break; 448 | 449 | double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 450 | (laserCloudSurfLast->points[j].x - pointSel.x) + 451 | (laserCloudSurfLast->points[j].y - pointSel.y) * 452 | (laserCloudSurfLast->points[j].y - pointSel.y) + 453 | (laserCloudSurfLast->points[j].z - pointSel.z) * 454 | (laserCloudSurfLast->points[j].z - pointSel.z); 455 | 456 | // if in the same or higher scan line 457 | if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2) 458 | { 459 | minPointSqDis2 = pointSqDis; 460 | minPointInd2 = j; 461 | } 462 | else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3) 463 | { 464 | // find nearer point 465 | minPointSqDis3 = pointSqDis; 466 | minPointInd3 = j; 467 | } 468 | } 469 | 470 | if (minPointInd2 >= 0 && minPointInd3 >= 0) 471 | { 472 | 473 | Eigen::Vector3d curr_point(surfPointsFlat->points[i].x, 474 | surfPointsFlat->points[i].y, 475 | surfPointsFlat->points[i].z); 476 | Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x, 477 | laserCloudSurfLast->points[closestPointInd].y, 478 | laserCloudSurfLast->points[closestPointInd].z); 479 | Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x, 480 | laserCloudSurfLast->points[minPointInd2].y, 481 | laserCloudSurfLast->points[minPointInd2].z); 482 | Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x, 483 | laserCloudSurfLast->points[minPointInd3].y, 484 | laserCloudSurfLast->points[minPointInd3].z); 485 | 486 | double s; 487 | if (DISTORTION) 488 | s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD; 489 | else 490 | s = 1.0; 491 | ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s); 492 | problem.AddResidualBlock(cost_function, loss_function, para_q, para_t); 493 | plane_correspondence++; 494 | } 495 | } 496 | } 497 | 498 | //printf("coner_correspondance %d, plane_correspondence %d \n", corner_correspondence, plane_correspondence); 499 | // printf("data association time %f ms \n", t_data.toc()); 500 | 501 | if ((corner_correspondence + plane_correspondence) < 10) 502 | { 503 | // printf("less correspondence! *************************************************\n"); 504 | } 505 | 506 | TicToc t_solver; 507 | ceres::Solver::Options options; 508 | options.linear_solver_type = ceres::DENSE_QR; 509 | options.max_num_iterations = 4; 510 | options.minimizer_progress_to_stdout = false; 511 | ceres::Solver::Summary summary; 512 | ceres::Solve(options, &problem, &summary); 513 | printf("geometry features solver time %f ms \n", t_solver.toc()); 514 | } 515 | // printf("optimization twice time %f \n", t_opt.toc()); 516 | 517 | t_w_curr = t_w_curr + q_w_curr * t_last_curr; 518 | q_w_curr = q_w_curr * q_last_curr; 519 | } 520 | 521 | TicToc t_pub; 522 | 523 | // publish odometry 524 | nav_msgs::Odometry laserOdometry; 525 | laserOdometry.header.frame_id = "camera_init"; 526 | laserOdometry.child_frame_id = "/laser_odom"; 527 | laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 528 | laserOdometry.pose.pose.orientation.x = q_w_curr.x(); 529 | laserOdometry.pose.pose.orientation.y = q_w_curr.y(); 530 | laserOdometry.pose.pose.orientation.z = q_w_curr.z(); 531 | laserOdometry.pose.pose.orientation.w = q_w_curr.w(); 532 | laserOdometry.pose.pose.position.x = t_w_curr.x(); 533 | laserOdometry.pose.pose.position.y = t_w_curr.y(); 534 | laserOdometry.pose.pose.position.z = t_w_curr.z(); 535 | pubLaserOdometry.publish(laserOdometry); 536 | 537 | geometry_msgs::PoseStamped laserPose; 538 | laserPose.header = laserOdometry.header; 539 | laserPose.pose = laserOdometry.pose.pose; 540 | laserPath.header.stamp = laserOdometry.header.stamp; 541 | laserPath.poses.push_back(laserPose); 542 | laserPath.header.frame_id = "camera_init"; 543 | pubLaserPath.publish(laserPath); 544 | 545 | // transform corner features and plane features to the scan end point 546 | if (0) 547 | { 548 | int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size(); 549 | for (int i = 0; i < cornerPointsLessSharpNum; i++) 550 | { 551 | TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]); 552 | } 553 | 554 | int surfPointsLessFlatNum = surfPointsLessFlat->points.size(); 555 | for (int i = 0; i < surfPointsLessFlatNum; i++) 556 | { 557 | TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]); 558 | } 559 | 560 | int laserCloudFullResNum = laserCloudFullRes->points.size(); 561 | for (int i = 0; i < laserCloudFullResNum; i++) 562 | { 563 | TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]); 564 | } 565 | } 566 | 567 | pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp; 568 | cornerPointsLessSharp = laserCloudCornerLast; 569 | laserCloudCornerLast = laserCloudTemp; 570 | 571 | laserCloudTemp = surfPointsLessFlat; 572 | surfPointsLessFlat = laserCloudSurfLast; 573 | laserCloudSurfLast = laserCloudTemp; 574 | 575 | laserCloudCornerLastNum = laserCloudCornerLast->points.size(); 576 | laserCloudSurfLastNum = laserCloudSurfLast->points.size(); 577 | 578 | // std::cout << "the size of corner last is " << laserCloudCornerLastNum << ", and the size of surf last is " << laserCloudSurfLastNum << '\n'; 579 | 580 | kdtreeCornerLast->setInputCloud(laserCloudCornerLast); 581 | kdtreeSurfLast->setInputCloud(laserCloudSurfLast); 582 | 583 | if (frameCount % skipFrameNum == 0) 584 | { 585 | frameCount = 0; 586 | 587 | sensor_msgs::PointCloud2 laserCloudCornerLast2; 588 | pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); 589 | laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 590 | laserCloudCornerLast2.header.frame_id = "/camera"; 591 | pubLaserCloudCornerLast.publish(laserCloudCornerLast2); 592 | 593 | sensor_msgs::PointCloud2 laserCloudSurfLast2; 594 | pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); 595 | laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 596 | laserCloudSurfLast2.header.frame_id = "/camera"; 597 | pubLaserCloudSurfLast.publish(laserCloudSurfLast2); 598 | 599 | sensor_msgs::PointCloud2 laserCloudFullRes3; 600 | pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3); 601 | laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 602 | laserCloudFullRes3.header.frame_id = "/camera"; 603 | pubLaserCloudFullRes.publish(laserCloudFullRes3); 604 | } 605 | // printf("publication time %f ms \n", t_pub.toc()); 606 | // printf("whole laserOdometry time %f ms \n \n", t_whole.toc()); 607 | // if(t_whole.toc() > 100) 608 | // ROS_WARN("odometry process over 100ms"); 609 | 610 | frameCount++; 611 | } 612 | rate.sleep(); 613 | } 614 | return 0; 615 | } -------------------------------------------------------------------------------- /src/lidarFeaturePointsFunction.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | // front end optimization for matched feature points 10 | struct front_end_residual 11 | { 12 | front_end_residual(Eigen::Vector3d src_point_, Eigen::Vector3d dst_point_) 13 | : src_point(src_point_), dst_point(dst_point_){} 14 | template 15 | bool operator()(const T *q, const T *t, T *residual) const 16 | { 17 | Eigen::Quaternion q_src2dest{q[3], q[0], q[1], q[2]}; 18 | Eigen::Matrix t_src2dest{t[0], t[1], t[2]}; 19 | Eigen::Matrix cp{T(src_point.x()), T(src_point.y()), T(src_point.z())}; 20 | Eigen::Matrix point_in_dist_coordinate_sys; 21 | point_in_dist_coordinate_sys = q_src2dest * cp + t_src2dest; 22 | 23 | residual[0] = point_in_dist_coordinate_sys.x() - T(dst_point.x()); 24 | residual[1] = point_in_dist_coordinate_sys.y() - T(dst_point.y()); 25 | residual[2] = point_in_dist_coordinate_sys.z() - T(dst_point.z()); 26 | return true; 27 | } 28 | static ceres::CostFunction *Create(const Eigen::Vector3d src_point_, const Eigen::Vector3d dst_point_) 29 | { 30 | return (new ceres::AutoDiffCostFunction< 31 | front_end_residual, 3, 4, 3>( 32 | new front_end_residual(src_point_, dst_point_))); 33 | } 34 | 35 | Eigen::Vector3d src_point, dst_point; 36 | }; 37 | 38 | 39 | struct FeatureMatchingResidual 40 | { 41 | FeatureMatchingResidual(Eigen::Vector3d curr_point_, Eigen::Vector3d prev_point_) 42 | : curr_point(curr_point_), prev_point(prev_point_){} 43 | template 44 | bool operator()(const T *q, const T *t, T *residual) const 45 | { 46 | Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; 47 | Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; 48 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 49 | Eigen::Matrix point_w; 50 | point_w = q_w_curr * cp + t_w_curr; 51 | residual[0] = point_w.x() - T(prev_point.x()); 52 | residual[1] = point_w.y() - T(prev_point.y()); 53 | residual[2] = point_w.z() - T(prev_point.z()); 54 | return true; 55 | } 56 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d prev_point_) 57 | { 58 | return (new ceres::AutoDiffCostFunction< 59 | FeatureMatchingResidual, 3, 4, 3>( 60 | new FeatureMatchingResidual(curr_point_, prev_point_))); 61 | } 62 | 63 | Eigen::Vector3d curr_point, prev_point; 64 | }; 65 | 66 | struct LidarGroundPlaneNormFactor 67 | { 68 | 69 | LidarGroundPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, 70 | double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), 71 | negative_OA_dot_norm(negative_OA_dot_norm_) {} 72 | 73 | template 74 | bool operator()(const T *q, T *residual) const 75 | { 76 | Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; 77 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 78 | Eigen::Matrix point_w; 79 | point_w = q_w_curr * cp; 80 | 81 | Eigen::Matrix norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z())); 82 | residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm); 83 | return true; 84 | } 85 | 86 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_, 87 | const double negative_OA_dot_norm_) 88 | { 89 | return (new ceres::AutoDiffCostFunction< 90 | LidarGroundPlaneNormFactor, 1, 4>( 91 | new LidarGroundPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_))); 92 | } 93 | 94 | Eigen::Vector3d curr_point; 95 | Eigen::Vector3d plane_unit_norm; 96 | double negative_OA_dot_norm; 97 | }; 98 | struct LidarPlaneFactor 99 | { 100 | LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_, 101 | Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_) 102 | : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), 103 | last_point_m(last_point_m_), s(s_) 104 | { 105 | ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m); 106 | ljm_norm.normalize(); 107 | } 108 | 109 | template 110 | bool operator()(const T *q, const T *t, T *residual) const 111 | { 112 | 113 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 114 | Eigen::Matrix lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())}; 115 | 116 | Eigen::Matrix ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())}; 117 | 118 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 119 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 120 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 121 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 122 | 123 | Eigen::Matrix lp; 124 | lp = q_last_curr * cp + t_last_curr; 125 | 126 | residual[0] = (lp - lpj).dot(ljm); 127 | 128 | return true; 129 | } 130 | 131 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_, 132 | const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_, 133 | const double s_) 134 | { 135 | return (new ceres::AutoDiffCostFunction< 136 | LidarPlaneFactor, 1, 4, 3>( 137 | new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_))); 138 | } 139 | 140 | Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m; 141 | Eigen::Vector3d ljm_norm; 142 | double s; 143 | }; 144 | 145 | struct LidarPlaneNormFactor 146 | { 147 | 148 | LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, 149 | double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), 150 | negative_OA_dot_norm(negative_OA_dot_norm_) {} 151 | 152 | template 153 | bool operator()(const T *q, const T *t, T *residual) const 154 | { 155 | Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; 156 | Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; 157 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 158 | Eigen::Matrix point_w; 159 | point_w = q_w_curr * cp + t_w_curr; 160 | 161 | Eigen::Matrix norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z())); 162 | residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm); 163 | return true; 164 | } 165 | 166 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_, 167 | const double negative_OA_dot_norm_) 168 | { 169 | return (new ceres::AutoDiffCostFunction< 170 | LidarPlaneNormFactor, 1, 4, 3>( 171 | new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_))); 172 | } 173 | 174 | Eigen::Vector3d curr_point; 175 | Eigen::Vector3d plane_unit_norm; 176 | double negative_OA_dot_norm; 177 | }; 178 | 179 | struct LidarEdgeFactor 180 | { 181 | LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, 182 | Eigen::Vector3d last_point_b_, double s_) 183 | : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {} 184 | 185 | template 186 | bool operator()(const T *q, const T *t, T *residual) const 187 | { 188 | 189 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 190 | Eigen::Matrix lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())}; 191 | Eigen::Matrix lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())}; 192 | 193 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 194 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 195 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 196 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 197 | 198 | Eigen::Matrix lp; 199 | lp = q_last_curr * cp + t_last_curr; 200 | 201 | Eigen::Matrix nu = (lp - lpa).cross(lp - lpb); 202 | Eigen::Matrix de = lpa - lpb; 203 | 204 | residual[0] = nu.x() / de.norm(); 205 | residual[1] = nu.y() / de.norm(); 206 | residual[2] = nu.z() / de.norm(); 207 | 208 | return true; 209 | } 210 | 211 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_, 212 | const Eigen::Vector3d last_point_b_, const double s_) 213 | { 214 | return (new ceres::AutoDiffCostFunction< 215 | LidarEdgeFactor, 3, 4, 3>( 216 | new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_))); 217 | } 218 | 219 | Eigen::Vector3d curr_point, last_point_a, last_point_b; 220 | double s; 221 | }; -------------------------------------------------------------------------------- /src/loop_closure_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "loop_closure_handler.h" 2 | 3 | loopClosureHandler::loopClosureHandler(/* args */) 4 | { 5 | std::string PROJECT_NAME("intensity_feature_tracker"); 6 | std::string pkg_path = ros::package::getPath(PROJECT_NAME); 7 | std::string vocabulary_file("/config/orbvoc.dbow3"); 8 | vocabulary_file = pkg_path + vocabulary_file; 9 | voc_= new DBoW3::Vocabulary(vocabulary_file); 10 | db_.setVocabulary(*voc_, false, 0); 11 | loop_index_ = -1; 12 | kdtreeKeyframeNearSearch_.reset(new pcl::KdTreeFLANN()); 13 | } 14 | 15 | loopClosureHandler::~loopClosureHandler() 16 | { 17 | } 18 | 19 | sensor_msgs::ImagePtr loopClosureHandler::cvMat2Image(std_msgs::Header header, cv::Mat & image){ 20 | static cv_bridge::CvImage outImg; 21 | outImg.header = header; 22 | outImg.encoding = "bgr8"; 23 | outImg.image = image; 24 | auto imageOut = outImg.toImageMsg(); 25 | return imageOut; 26 | } 27 | 28 | void loopClosureHandler::keyframeProcessor(loopClosureProcessor::Keyframe keyframe, pcl::PointCloud::Ptr cloud_keyPose3D){ 29 | keyframe_pool_[keyframe.keyframeId] = keyframe; 30 | loop_index_ = -1; 31 | // ROS_INFO cloud_keyPose3D->size(), cloud_keyPose6D->size(); 32 | if(cloud_keyPose3D->size() == 0){ 33 | return; 34 | } 35 | // ROS_INFO("size of keyframe: %li", cloud_keyPose3D->size()); 36 | PointType cur_pose; 37 | { 38 | std::lock_guard lock(kdtreeKeyframeNearSearch_mutex_); 39 | kdtreeKeyframeNearSearch_->setInputCloud(cloud_keyPose3D); 40 | cur_pose = cloud_keyPose3D->points[cloud_keyPose3D->size()-1]; 41 | } 42 | 43 | 44 | std::vector keyframe_near_search_indices; 45 | std::vector keyframe_near_search_distances; 46 | kdtreeKeyframeNearSearch_->radiusSearch(cur_pose, 7, keyframe_near_search_indices, keyframe_near_search_distances); 47 | // ROS_INFO("size of keyframe_near_search_indices: %li", keyframe_near_search_indices.size()); 48 | for(size_t i = 0; i< keyframe_near_search_indices.size(); i++){ 49 | int id_tmp = keyframe_near_search_indices[i]; 50 | auto loop_keyframe = keyframe_pool_.find(id_tmp); 51 | // ROS_INFO("index: %i, distance: %f, interval time: %f", keyframe_near_search_indices[i], keyframe_near_search_distances[i], loop_keyframe->second.time - keyframe.time); 52 | if(loop_keyframe != keyframe_pool_.end()){ 53 | if(abs(loop_keyframe->second.time - keyframe.time) > 40){ // loop closure time threshold 15 seconds 54 | loop_index_ = id_tmp; 55 | ROS_INFO("loop index: %i, cur index: %li", loop_index_, keyframe.keyframeId); 56 | break; 57 | } 58 | } 59 | 60 | } 61 | 62 | 63 | } 64 | 65 | void loopClosureHandler::keyframeProcessor(loopClosureProcessor::Keyframe keyframe){ 66 | keyframe_pool_[keyframe.keyframeId] = keyframe; 67 | loop_index_ = -1; 68 | 69 | bool USE_SCANCONTEXT = false; 70 | bool USE_KDTREE = false; 71 | bool USE_ORBLOOP = true; 72 | // scan context 73 | if(USE_SCANCONTEXT){ 74 | scManager.makeAndSaveScancontextAndKeys(keyframe.cloud_track_); 75 | auto detectResult = scManager.detectLoopClosureID(); 76 | loop_index_ = detectResult.first; 77 | } 78 | 79 | if(USE_ORBLOOP){ 80 | loop_index_ = detectLoop(keyframe.image, keyframe.descriptors, keyframe.keyframeId);//false: -1; detect loop: loop_index >=1 81 | } 82 | 83 | if(USE_KDTREE){ 84 | // kdtree based loop detection 85 | 86 | 87 | } 88 | 89 | 90 | 91 | 92 | 93 | 94 | if(loop_index_ >= 0){ 95 | std::cout << "\n Current id:" << keyframe.keyframeId << ", candidate id: " << loop_index_ << std::endl; 96 | } 97 | 98 | 99 | } 100 | 101 | 102 | int loopClosureHandler::detectLoop(cv::Mat & image, cv::Mat & descriptors, int frame_index){ 103 | DBoW3::QueryResults ret; 104 | int MIN_LOOP_SEARCH_GAP = 50; 105 | double MIN_LOOP_BOW_TH = 0.015; 106 | int SKIPED_FRAMES = 5; 107 | 108 | ros::NodeHandle nh; 109 | nh.param("/loop_closure_parameters/min_loop_bow_threshold", MIN_LOOP_BOW_TH, 0.0155); 110 | nh.param("/loop_closure_parameters/min_loop_search_gap", MIN_LOOP_SEARCH_GAP, 50); 111 | nh.param("/loop_closure_parameters/skiped_frames", SKIPED_FRAMES, 5); 112 | 113 | db_.query(descriptors, ret, 4, frame_index - MIN_LOOP_SEARCH_GAP); 114 | db_.add(descriptors); 115 | 116 | image_pool_[frame_index] = image.clone(); 117 | cv::Mat bow_images = image.clone(); 118 | 119 | if (frame_index - MIN_LOOP_SEARCH_GAP < 0) 120 | return -1; 121 | 122 | bool find_loop = false; 123 | if (ret.size() >= 1 && ret[0].Score > MIN_LOOP_BOW_TH) 124 | { 125 | for (unsigned int i = 1; i < ret.size(); i++) 126 | { 127 | ROS_INFO("ret [%i] score is %f", i, ret[i].Score); 128 | if (ret[i].Score > MIN_LOOP_BOW_TH) 129 | { 130 | find_loop = true; 131 | } 132 | } 133 | } 134 | 135 | if (find_loop && frame_index > 5) 136 | { 137 | int min_index = -1; 138 | for (unsigned int i = 0; i < ret.size(); i++) 139 | { 140 | if (min_index == -1 || ((int)ret[i].Id < min_index && ret[i].Score > 0.015 )) 141 | min_index = ret[i].Id; 142 | } 143 | ROS_INFO("find loop: %i", min_index); 144 | if(min_index < 6){ 145 | return min_index; 146 | } 147 | else{ 148 | return -1; 149 | } 150 | 151 | 152 | } 153 | else 154 | return -1; 155 | 156 | } -------------------------------------------------------------------------------- /src/loop_closure_handler.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include 6 | // #include 7 | #include 8 | 9 | // #include 10 | // #include 11 | // #include 12 | 13 | #include 14 | // #include 15 | // #include 16 | #include 17 | 18 | #include "keyframe.h" 19 | #include "parameters.h" 20 | #include "Scancontext.h" 21 | // #include "ouster_ros/ros.h" 22 | 23 | class loopClosureHandler 24 | { 25 | private: 26 | /* data */ 27 | 28 | public: 29 | loopClosureHandler(/* args */); 30 | ~loopClosureHandler(); 31 | void keyframeProcessor(loopClosureProcessor::Keyframe keyframe); 32 | void keyframeProcessor(loopClosureProcessor::Keyframe keyframe, pcl::PointCloud::Ptr cloud_keyPose3D); 33 | int detectLoop(cv::Mat & image, cv::Mat& descriptors, int frame_index); 34 | sensor_msgs::ImagePtr cvMat2Image(std_msgs::Header header, cv::Mat & image); 35 | SCManager scManager; 36 | 37 | 38 | 39 | public: 40 | DBoW3::Database db_; 41 | DBoW3::Vocabulary* voc_; 42 | std::vector bow_descriptors_; 43 | std::map image_pool_; 44 | std::map keyframe_pool_; 45 | int loop_index_; 46 | pcl::KdTreeFLANN::Ptr kdtreeKeyframeNearSearch_; 47 | std::mutex kdtreeKeyframeNearSearch_mutex_; 48 | // cv::Mat MASK; 49 | }; 50 | 51 | -------------------------------------------------------------------------------- /src/mapOptimization.cpp: -------------------------------------------------------------------------------- 1 | #include "mapOptimization.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "lidarFeaturePointsFunction.hpp" 7 | void mapOptimization::laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) 8 | { 9 | TicToc laserOdomHandler_time; 10 | Eigen::Quaterniond q_wodom_curr; 11 | Eigen::Vector3d t_wodom_curr; 12 | q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x; 13 | q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y; 14 | q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z; 15 | q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w; 16 | t_wodom_curr.x() = laserOdometry->pose.pose.position.x; 17 | t_wodom_curr.y() = laserOdometry->pose.pose.position.y; 18 | t_wodom_curr.z() = laserOdometry->pose.pose.position.z; 19 | 20 | Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr; 21 | Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; 22 | 23 | nav_msgs::Odometry odomAftMapped; 24 | odomAftMapped.header.frame_id = "map"; 25 | odomAftMapped.child_frame_id = "high_freq_odom"; 26 | odomAftMapped.header.stamp = laserOdometry->header.stamp; 27 | odomAftMapped.pose.pose.orientation.x = q_w_curr.x(); 28 | odomAftMapped.pose.pose.orientation.y = q_w_curr.y(); 29 | odomAftMapped.pose.pose.orientation.z = q_w_curr.z(); 30 | odomAftMapped.pose.pose.orientation.w = q_w_curr.w(); 31 | odomAftMapped.pose.pose.position.x = t_w_curr.x(); 32 | odomAftMapped.pose.pose.position.y = t_w_curr.y(); 33 | odomAftMapped.pose.pose.position.z = t_w_curr.z(); 34 | pubOdomAftMappedHighFrec.publish(odomAftMapped); 35 | 36 | //robot model 37 | // Create a mesh marker for the spot 38 | visualization_msgs::Marker mesh_marker; 39 | mesh_marker.header.frame_id = "map"; 40 | mesh_marker.header.stamp = laserOdometry->header.stamp; 41 | mesh_marker.ns = "spot"; 42 | mesh_marker.id = 0; 43 | mesh_marker.type = visualization_msgs::Marker::MESH_RESOURCE; 44 | mesh_marker.action = visualization_msgs::Marker::ADD; 45 | mesh_marker.pose.position.x = t_w_curr.x(); 46 | mesh_marker.pose.position.y = t_w_curr.y(); 47 | mesh_marker.pose.position.z = t_w_curr.z(); 48 | mesh_marker.pose.orientation.x = q_w_curr.x(); 49 | mesh_marker.pose.orientation.y = q_w_curr.y(); 50 | mesh_marker.pose.orientation.z = q_w_curr.z(); 51 | mesh_marker.pose.orientation.w = q_w_curr.w(); 52 | mesh_marker.scale.x = 1; 53 | mesh_marker.scale.y = 1; 54 | mesh_marker.scale.z = 1; 55 | mesh_marker.color.r = 1; 56 | mesh_marker.color.g = 1; 57 | mesh_marker.color.b = 1; 58 | mesh_marker.color.a = 1.0; 59 | mesh_marker.lifetime = ros::Duration(0.5); 60 | std::string PROJECT_NAME("intensity_feature_tracker"); 61 | std::string pkg_path = ros::package::getPath(PROJECT_NAME); 62 | std::string spot_mesh_file("/config/spot.stl"); 63 | std::string prefix_str("package://"); 64 | std::string mesh_path_str = prefix_str + PROJECT_NAME + spot_mesh_file; 65 | mesh_marker.mesh_resource = mesh_path_str; 66 | mesh_marker.mesh_use_embedded_materials = true; 67 | robot_marker_pub.publish(mesh_marker); 68 | 69 | } 70 | void mapOptimization::filterNaNPoints(){ 71 | 72 | } 73 | 74 | void mapOptimization::mapOptimizationCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg, const nav_msgs::Odometry::ConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& plane_cloud_msg, const sensor_msgs::PointCloud2::ConstPtr& pc_corner_msg){ 75 | TicToc map_optimization_time; 76 | q_wodom_curr.x() = odom_msg->pose.pose.orientation.x; 77 | q_wodom_curr.y() = odom_msg->pose.pose.orientation.y; 78 | q_wodom_curr.z() = odom_msg->pose.pose.orientation.z; 79 | q_wodom_curr.w() = odom_msg->pose.pose.orientation.w; 80 | t_wodom_curr.x() = odom_msg->pose.pose.position.x; 81 | t_wodom_curr.y() = odom_msg->pose.pose.position.y; 82 | t_wodom_curr.z() = odom_msg->pose.pose.position.z; 83 | transformAssociateToMap(); 84 | 85 | Eigen::Map q_w_curr(parameters_); 86 | Eigen::Map t_w_curr(parameters_ + 4); 87 | 88 | Eigen::Quaterniond q_w_curr_original(q_w_curr); 89 | Eigen::Vector3d t_w_curr_original(t_w_curr); 90 | 91 | TicToc tic_toc_pc_processing; 92 | pcl::PointCloud pc_plane; 93 | pcl::PointCloud pc_corner; 94 | 95 | image_handler_->GroundPointOut->points.clear(); 96 | #pragma omp parallel sections num_threads(4) //~20ms 97 | { 98 | #pragma omp section 99 | image_handler_->groundPlaneExtraction(cloud_msg); 100 | 101 | #pragma omp section 102 | pcl::fromROSMsg(*plane_cloud_msg, pc_plane); // read the plane pointcloud 103 | 104 | #pragma omp section 105 | pcl::fromROSMsg(*pc_corner_msg, pc_corner); // read edge points 106 | 107 | #pragma omp section 108 | image_handler_->cloud_handler(cloud_msg); //~5ms processing intensity image 109 | } 110 | 111 | 112 | *image_handler_->GroundPointOut += pc_plane; 113 | std::vector idx; 114 | pcl::removeNaNFromPointCloud(*image_handler_->GroundPointOut, *image_handler_->GroundPointOut, idx); 115 | pcl::removeNaNFromPointCloud(pc_corner, pc_corner, idx); 116 | 117 | cv::Mat image_intensity = image_handler_->image_intensity; 118 | cv::Ptr detector = cv::ORB::create(NUM_ORB_FEATURES*2, 1.2f, 8, 1); 119 | detector->detect(image_intensity, cur_keypoints_, MASK); 120 | keypoint2uv(cur_keypoints_, cur_orb_point_2d_uv_); 121 | extractPointsAndFilterZeroValue(cur_orb_point_2d_uv_, image_handler_->cloud_track, cur_out_point3d_, status_); 122 | reduceVector(cur_out_point3d_, status_); 123 | reduceVector(cur_orb_point_2d_uv_, status_); 124 | reduceVector(cur_keypoints_, status_); 125 | detector->compute(image_intensity, cur_keypoints_, cur_descriptors_); 126 | 127 | std::shared_ptr mapKeyframeNew( new mapProcessor::SlideWindowKeyframe(cur_descriptors_, image_intensity, cur_orb_point_2d_uv_, q_w_curr, t_w_curr, cur_out_point3d_)); 128 | 129 | 130 | 131 | //match the features with previous keyframe, initialize parameters 132 | std::vector matches, good_matches; 133 | if(prev_keyframe_img.empty()){ 134 | prev_keyframe_img = image_intensity; 135 | prev_keypoints_ = cur_keypoints_; 136 | prev_descriptors_ = cur_descriptors_; 137 | prev_orb_point_2d_uv_ = cur_orb_point_2d_uv_; 138 | prev_out_point3d_ = cur_out_point3d_; 139 | keyframeId_ = 0; 140 | 141 | std::shared_ptr keyframetmp (new mapProcessor::Keyframe(keyframeId_, *image_handler_->cloud_track, *image_handler_->GroundPointOut, q_w_curr, t_w_curr)); 142 | prev_keyframe = *keyframetmp; 143 | 144 | ground_plane_cloud_ = *image_handler_->GroundPointOut; 145 | Eigen::Matrix4d cur_transform = Eigen::Matrix4d::Identity(); 146 | cur_transform.block<3,3>(0,0) = q_w_curr.toRotationMatrix(); 147 | cur_transform.block<3,1>(0,3) = t_w_curr; 148 | pcl::PointCloud::Ptr tmp_cloud(new pcl::PointCloud()); 149 | pcl::transformPointCloud(ground_plane_cloud_, *tmp_cloud, cur_transform); 150 | ikdtree->Build((*tmp_cloud).points); 151 | tmp_cloud->clear(); 152 | pcl::transformPointCloud(pc_corner, *tmp_cloud, cur_transform); 153 | corner_ikdtree_->Build((*tmp_cloud).points); 154 | 155 | 156 | } 157 | else{ 158 | matcher_.match(cur_descriptors_, prev_descriptors_, matches); 159 | std::sort(matches.begin(), matches.end()); 160 | for (size_t i = 0; i < matches.size()*0.2; ++i) 161 | { 162 | good_matches.push_back(matches[i]); 163 | } 164 | 165 | 166 | 167 | if(1){ 168 | keyframe_flag_ = true; 169 | keyframeId_++; 170 | extractMatchedPoints(good_matches, prev_matched_points3d_, prev_keyframe.q_map_cur_k_, prev_keyframe.t_map_cur_k_, cur_matched_points3d_, q_w_curr, t_w_curr, prev_out_point3d_, cur_out_point3d_, cloud_msg->header); 171 | } 172 | std::shared_ptr keyframetmp (new mapProcessor::Keyframe(keyframeId_, *image_handler_->cloud_track, *image_handler_->GroundPointOut, q_w_curr, t_w_curr)); 173 | cur_keyframe = *keyframetmp; 174 | 175 | 176 | } 177 | 178 | if(keyframe_flag_ == true){ 179 | keyframe_flag_ = false; 180 | if(ground_points_pub.getNumSubscribers() != 0){ 181 | pcl::PointCloud::Ptr ikdtree_points(new pcl::PointCloud()); 182 | corner_ikdtree_->flatten(ikdtree->Root_Node, ikdtree->PCL_Storage, NOT_RECORD); 183 | ikdtree_points->points = ikdtree->PCL_Storage; 184 | sensor_msgs::PointCloud2 groundPlaneMapCloud_msg; 185 | pcl::toROSMsg(*ikdtree_points, groundPlaneMapCloud_msg); 186 | groundPlaneMapCloud_msg.header.frame_id = "map"; 187 | groundPlaneMapCloud_msg.header.stamp = cloud_msg->header.stamp; 188 | ground_points_pub.publish(groundPlaneMapCloud_msg); 189 | } 190 | 191 | 192 | 193 | 194 | ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); 195 | ceres::LocalParameterization *q_parameterization = 196 | new ceres::EigenQuaternionParameterization(); 197 | ceres::Problem::Options problem_options; 198 | 199 | ceres::Problem problem(problem_options); 200 | problem.AddParameterBlock(parameters_, 4, q_parameterization); 201 | problem.AddParameterBlock(parameters_ + 4, 3); 202 | 203 | size_t num_good_matches = 0; 204 | 205 | //publish line list for visualization 206 | initialLineList(line_list ,cloud_msg->header); 207 | if(prev_keypoints_.size() != cur_keypoints_.size() && good_matches.size() >=4 && good_matches.size()!= matches.size()){ 208 | for(size_t i = 0; i < prev_matched_points3d_.size() && false; i++){ 209 | Eigen::Vector3d prev_point3d = prev_keyframe.q_map_cur_k_ * Eigen::Vector3d(prev_matched_points3d_[i].x, prev_matched_points3d_[i].y, prev_matched_points3d_[i].z) + prev_keyframe.t_map_cur_k_; 210 | Eigen::Vector3d cur_point3d = cur_keyframe.q_map_cur_k_ * Eigen::Vector3d(cur_matched_points3d_[i].x, cur_matched_points3d_[i].y, cur_matched_points3d_[i].z) + cur_keyframe.t_map_cur_k_; 211 | geometry_msgs::Point prev_point, cur_point; 212 | prev_point.x = prev_point3d(0); 213 | prev_point.y = prev_point3d(1); 214 | prev_point.z = prev_point3d(2); 215 | cur_point.x = cur_point3d(0); 216 | cur_point.y = cur_point3d(1); 217 | cur_point.z = cur_point3d(2); 218 | { 219 | std::lock_guard lock(map_mutex_); 220 | appendLines(line_list, prev_point, cur_point); 221 | } 222 | 223 | double distance = (prev_point3d - cur_point3d).norm(); 224 | if(distance > 0.5){ 225 | continue; 226 | } 227 | num_good_matches++; 228 | 229 | Eigen::Vector3d cur_point3d_vector3d(cur_matched_points3d_[i].x, cur_matched_points3d_[i].y, cur_matched_points3d_[i].z); 230 | Eigen::Vector3d point_w; 231 | point_w = q_w_curr * cur_point3d_vector3d + t_w_curr; 232 | // BA residual 233 | ceres::CostFunction* cost_function = 234 | new ceres::AutoDiffCostFunction( 235 | new FeatureMatchingResidual(cur_point3d_vector3d, prev_point3d)); 236 | { 237 | std::lock_guard lock(map_mutex_); 238 | problem.AddResidualBlock(cost_function, loss_function, parameters_, parameters_ + 4); 239 | } 240 | } 241 | } 242 | if(matched_lines_pub.getNumSubscribers() !=0){ 243 | matched_lines_pub.publish(line_list); 244 | } 245 | if(keyframe_sliding_window_.size() >= 1 && true) 246 | { 247 | 248 | auto cur_keyframe_from_SW = mapKeyframeNew; 249 | #pragma omp parallel for num_threads(NUM_THREADS) 250 | for (auto it = keyframe_sliding_window_.rbegin(); it != keyframe_sliding_window_.rend(); ++it){ 251 | auto prev_keyframe_from_SW = *it; 252 | cv::BFMatcher matcher_tmp; 253 | std::vector matches_tmp, good_matches_tmp; 254 | matcher_tmp.match(cur_keyframe_from_SW->descriptors, prev_keyframe_from_SW->descriptors, matches_tmp); 255 | if(matches_tmp.size() > 100 ){ 256 | std::sort(matches_tmp.begin(), matches_tmp.end()); 257 | for (size_t i = 0; i < matches_tmp.size()*0.2; ++i) 258 | { 259 | good_matches_tmp.push_back(matches_tmp[i]); 260 | } 261 | { 262 | std::string detectTime = std::to_string(tic_toc_pc_processing.toc()); 263 | image_show(good_matches_tmp, detectTime, prev_keyframe_from_SW->image_intensity, cur_keyframe_from_SW->image_intensity, cur_keyframe_from_SW->orb_point_2d_uv, prev_keyframe_from_SW->orb_point_2d_uv); 264 | } 265 | std::vector prev_matched_points3d_tmp, cur_matched_points3d_tmp; 266 | if(good_matches_tmp.size() > 50 && good_matches_tmp.size() != matches_tmp.size() && prev_keyframe_from_SW->descriptors.size() != cur_keyframe_from_SW->descriptors.size()){ 267 | extractMatchedPoints(good_matches_tmp, prev_matched_points3d_tmp, cur_matched_points3d_tmp, prev_keyframe_from_SW->cur_point3d_wrt_orb_features, cur_keyframe_from_SW->cur_point3d_wrt_orb_features); 268 | if(prev_matched_points3d_tmp.size() > 0 && cur_matched_points3d_tmp.size() > 0){ 269 | #pragma omp parallel for num_threads(NUM_THREADS) 270 | for(size_t i = 0; i < prev_matched_points3d_tmp.size(); i++){ 271 | Eigen::Vector3d prev_point3d = prev_keyframe_from_SW->q_map_cur_tk * Eigen::Vector3d(prev_matched_points3d_tmp[i].x, prev_matched_points3d_tmp[i].y, prev_matched_points3d_tmp[i].z) + prev_keyframe_from_SW->t_map_cur_tk; 272 | Eigen::Vector3d cur_point3d = cur_keyframe_from_SW->q_map_cur_tk * Eigen::Vector3d(cur_matched_points3d_tmp[i].x, cur_matched_points3d_tmp[i].y, cur_matched_points3d_tmp[i].z) + cur_keyframe_from_SW->t_map_cur_tk; 273 | 274 | double distance = (prev_point3d - cur_point3d).norm(); 275 | if(distance > 0.3){ 276 | continue; 277 | } 278 | Eigen::Vector3d cur_point3d_vector3d = Eigen::Vector3d(cur_matched_points3d_tmp[i].x, cur_matched_points3d_tmp[i].y, cur_matched_points3d_tmp[i].z); 279 | 280 | ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction( new FeatureMatchingResidual(cur_point3d_vector3d, prev_point3d)); 281 | { 282 | std::lock_guard lock(map_mutex_); 283 | problem.AddResidualBlock(cost_function, loss_function, parameters_, parameters_ + 4); 284 | } 285 | 286 | } 287 | } 288 | } 289 | } 290 | } 291 | } 292 | if(true){ 293 | pcl::PointCloud::Ptr PlaneCloudDS(new pcl::PointCloud()); 294 | voxel_grid_.setInputCloud(image_handler_->GroundPointOut); 295 | voxel_grid_.filter(*image_handler_->GroundPointOut); 296 | 297 | 298 | for(size_t i=0; i < image_handler_->GroundPointOut->points.size(); i++){ 299 | ground_point_sensor_ = image_handler_->GroundPointOut->points[i]; 300 | Eigen::Vector3d point_curr(ground_point_sensor_.x, ground_point_sensor_.y, ground_point_sensor_.z); 301 | Eigen::Vector3d point_w_tmp = q_w_curr * point_curr + t_w_curr; 302 | ground_point_world_.x = point_w_tmp.x(); 303 | ground_point_world_.y = point_w_tmp.y(); 304 | ground_point_world_.z = point_w_tmp.z(); 305 | KD_TREE::PointVector searchResults; 306 | std::vector pointSearchSqDis; 307 | ikdtree->Nearest_Search(ground_point_world_, 5, searchResults, pointSearchSqDis); 308 | Eigen::Matrix matA0; 309 | Eigen::Matrix matB0 = -1 * Eigen::Matrix::Ones(); 310 | if (pointSearchSqDis[4] < 1.0) 311 | { 312 | 313 | for (int j = 0; j < 5; j++) 314 | { 315 | matA0(j, 0) = searchResults[j].x; 316 | matA0(j, 1) = searchResults[j].y; 317 | matA0(j, 2) = searchResults[j].z; 318 | } 319 | Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0); 320 | double negative_OA_dot_norm = 1 / norm.norm(); 321 | norm.normalize(); 322 | bool planeValid = true; 323 | for (int j = 0; j < 5; j++) 324 | { 325 | if (fabs(norm(0) * searchResults[j].x + 326 | norm(1) * searchResults[j].y + 327 | norm(2) * searchResults[j].z + negative_OA_dot_norm) > 0.2) 328 | { 329 | planeValid = false; 330 | break; 331 | } 332 | } 333 | Eigen::Vector3d curr_point(ground_point_sensor_.x, ground_point_sensor_.y, ground_point_sensor_.z); 334 | if (planeValid) 335 | { 336 | ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm); 337 | problem.AddResidualBlock(cost_function, loss_function, parameters_, parameters_ + 4); 338 | 339 | } 340 | } 341 | } 342 | 343 | } 344 | 345 | ceres::Solver::Options options; 346 | options.linear_solver_type = ceres::DENSE_QR; 347 | options.max_num_iterations = 10; 348 | options.minimizer_progress_to_stdout = false; 349 | options.check_gradients = false; 350 | options.gradient_check_relative_precision = 1e-4; 351 | ceres::Solver::Summary summary; 352 | ceres::Solve(options, &problem, &summary); 353 | ROS_INFO("%s", summary.BriefReport().c_str()); 354 | if(summary.termination_type == ceres::CONVERGENCE){ 355 | transformUpdate(); 356 | } 357 | 358 | if(true){ 359 | if(summary.termination_type == ceres::CONVERGENCE){ 360 | cur_keyframe.q_map_cur_k_ = q_w_curr; 361 | cur_keyframe.t_map_cur_k_ = t_w_curr; 362 | } 363 | 364 | prev_keyframe = cur_keyframe; 365 | prev_keyframe_img = image_intensity; 366 | prev_keypoints_ = cur_keypoints_; 367 | prev_descriptors_ = cur_descriptors_; 368 | prev_orb_point_2d_uv_ = cur_orb_point_2d_uv_; 369 | prev_out_point3d_ = cur_out_point3d_; 370 | pcl::PointCloud ground_plane_cloud_; 371 | ground_plane_cloud_ = *image_handler_->GroundPointOut; 372 | Eigen::Matrix4d cur_transform = Eigen::Matrix4d::Identity(); 373 | cur_transform.block<3,3>(0,0) = cur_keyframe.q_map_cur_k_.toRotationMatrix(); 374 | cur_transform.block<3,1>(0,3) = cur_keyframe.t_map_cur_k_; 375 | pcl::PointCloud::Ptr tmp_cloud(new pcl::PointCloud()); 376 | pcl::transformPointCloud(ground_plane_cloud_, *tmp_cloud, cur_transform); 377 | ikdtree->Add_Points((*tmp_cloud).points, true); 378 | 379 | tmp_cloud->clear(); 380 | pcl::transformPointCloud(pc_corner, *tmp_cloud, cur_transform); 381 | corner_ikdtree_->Add_Points((*tmp_cloud).points, true); 382 | if(summary.termination_type == ceres::CONVERGENCE){ 383 | mapKeyframeNew->q_map_cur_tk = q_w_curr; 384 | mapKeyframeNew->t_map_cur_tk = t_w_curr; 385 | 386 | } 387 | keyframe_sliding_window_.emplace_back(mapKeyframeNew); 388 | 389 | if(keyframe_sliding_window_.size() > (size_t)SLIDING_WINDOW_SIZE){ 390 | keyframe_sliding_window_.pop_front(); 391 | } 392 | } 393 | } 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | } 426 | 427 | //mapOptimization() function 428 | mapOptimization::mapOptimization(): 429 | ikdtree(new KD_TREE(0.3, 0.6, 0.4)),//0.4 best 430 | corner_ikdtree_(new KD_TREE(0.3, 0.6, 0.8)) 431 | { 432 | Eigen::Map q_w_curr(parameters_); 433 | Eigen::Map t_w_curr(parameters_ + 4); 434 | //print q_w_curr and t_w_curr 435 | // ROS_INFO("q_w_curr: %f, %f, %f, %f", q_w_curr.x(), q_w_curr.y(), q_w_curr.z(), q_w_curr.w()); 436 | // ROS_INFO("t_w_curr: %f, %f, %f", t_w_curr.x(), t_w_curr.y(), t_w_curr.z()); 437 | //print the address of parameters_ 438 | // ROS_INFO("parameters_ address: %p", parameters_); 439 | // ROS_INFO("parameters_+4 address: %p", parameters_+4); 440 | 441 | //ros node handle 442 | ros::NodeHandle nh; 443 | nh.getParam("/intensity_feature_tracker/image_width", IMAGE_WIDTH); 444 | nh.getParam("/intensity_feature_tracker/image_height", IMAGE_HEIGHT); 445 | //IMAGE_CROP 446 | nh.getParam("/intensity_feature_tracker/image_crop", IMAGE_CROP); 447 | //num_threads 448 | nh.getParam("/intensity_feature_tracker/num_threads", NUM_THREADS); 449 | // NUM_ORB_FEATURES 450 | nh.getParam("/intensity_feature_tracker/num_orb_features", NUM_ORB_FEATURES); 451 | nh.getParam("/intensity_feature_tracker/hand_held_flag", HAND_HELD_FLAG); 452 | nh.getParam("/map_optimization_parameters/sliding_window_size", SLIDING_WINDOW_SIZE); 453 | // GROUND_PLANE_WINDOW_SIZE; ground_plane_window_size 454 | nh.getParam("/map_optimization_parameters/ground_plane_window_size", GROUND_PLANE_WINDOW_SIZE); 455 | 456 | 457 | image_handler_ = new intensity_slam::ImageHandler(IMAGE_HEIGHT, 458 | IMAGE_WIDTH, 459 | NUM_THREADS); 460 | //initial MASK 461 | if(HAND_HELD_FLAG) MASK = setMask(IMAGE_HEIGHT, IMAGE_WIDTH, IMAGE_CROP); 462 | else MASK = setMask(IMAGE_HEIGHT, IMAGE_WIDTH, 0); 463 | matcher_ = cv::BFMatcher(cv::NORM_HAMMING, true); 464 | keyframe_flag_ = false; 465 | 466 | 467 | 468 | 469 | q_wmap_wodom = Eigen::Quaterniond(1, 0, 0, 0); 470 | t_wmap_wodom = Eigen::Vector3d(0, 0, 0); 471 | pubOdomAftMappedHighFrec = nh.advertise("/aft_mapped_to_init_high_frec", 100);//disable factor node 472 | robot_marker_pub = nh.advertise("/car_model_Marker_array", 10); 473 | matched_points_pub = nh.advertise("/matched_points", 10); 474 | ground_points_pub = nh.advertise("/ground_points", 10); 475 | //pub_matched_lines 476 | matched_lines_pub = nh.advertise("/matched_lines", 10); 477 | // publish image 478 | matched_keypoints_img_pub = nh.advertise("/matched_keypoints_img", 10); 479 | voxel_grid_.setLeafSize(0.8, 0.8, 0.8); 480 | corner_voxel_grid_.setLeafSize(0.4, 0.4, 0.4); 481 | 482 | } 483 | mapOptimization::~mapOptimization(){} 484 | 485 | // setmask 486 | cv::Mat mapOptimization::setMask(int height, int width, int crop){ 487 | auto mask = cv::Mat(height, width, CV_8UC1, cv::Scalar(255)); 488 | for (int i = 0; i < height; ++i) 489 | for (int j = 0; j < width; ++j) 490 | if (j < crop || j > width - crop) 491 | mask.at(i,j) = 0; 492 | return mask; 493 | } 494 | //void keypoint2uv(const std::vector& keypoints, std::vector& uv); 495 | void mapOptimization::keypoint2uv(const std::vector& keypoints, std::vector& uv){ 496 | uv.clear(); 497 | for (long unsigned int i = 0; i < keypoints.size(); ++i) 498 | uv.push_back(keypoints[i].pt); 499 | } 500 | 501 | // void extractPointsAndFilterZeroValue 502 | void mapOptimization::extractPointsAndFilterZeroValue(const std::vector& cur_orb_point_2d_uv, const pcl::PointCloud::Ptr& cloudTrack, std::vector& cur_out_point3d, std::vector& status){ 503 | cur_out_point3d.clear(); 504 | status.clear(); 505 | cur_out_point3d.resize(cur_orb_point_2d_uv.size()); 506 | status.resize(cur_orb_point_2d_uv.size()); 507 | 508 | #pragma omp parallel for num_threads(NUM_THREADS) 509 | for(size_t i=0; i< cur_orb_point_2d_uv.size(); i++){ 510 | int col_id = cvRound(cur_orb_point_2d_uv[i].x); 511 | int row_id = cvRound(cur_orb_point_2d_uv[i].y); 512 | int index = row_id * IMAGE_WIDTH + col_id; 513 | 514 | PointType *point_i = &cloudTrack->points[index]; 515 | 516 | cv::Point3f p_3d(0.0f, 0.0f, 0.0f); 517 | 518 | if(abs(point_i->x) < 0.01 && abs(point_i->y) < 0.01 && abs(point_i->z) < 0.01){ 519 | status[i] = 0; //filter points if there are no x, y, z values for this pixel. 520 | } 521 | else{ 522 | status[i] = 1; 523 | p_3d.x = point_i->x; 524 | p_3d.y = point_i->y; 525 | p_3d.z = point_i->z; 526 | } 527 | 528 | cur_out_point3d[i] = p_3d; 529 | } 530 | } 531 | 532 | 533 | // @in: prev_out_point3d_, cur_out_point3d_ 534 | // @out: prev_matched_points3d_, cur_matched_points3d_ 535 | void mapOptimization::extractMatchedPoints(std::vector &matches, std::vector &prev_matched_points3d, Eigen::Quaterniond prev_q, Eigen::Vector3d prev_t, std::vector &cur_matched_points3d, Eigen::Quaterniond cur_q, Eigen::Vector3d cur_t, std::vector &prev_out_point3d, std::vector &cur_out_point3d, std_msgs::Header header){ 536 | prev_matched_points3d.clear(); 537 | cur_matched_points3d.clear(); 538 | prev_matched_points3d.resize(matches.size()); 539 | cur_matched_points3d.resize(matches.size()); 540 | 541 | #pragma omp parallel for num_threads(NUM_THREADS) 542 | for(size_t i=0; i 0 && cur_matched_points3d.size() > 0){ 551 | // create PointcloudXYZRGB for cur_matched_points3d and prev_matched_points3d, set the color to red and green respectively. 552 | pcl::PointCloud::Ptr cur_matched_points3d_cloud(new pcl::PointCloud); 553 | pcl::PointCloud::Ptr prev_matched_points3d_cloud(new pcl::PointCloud); 554 | cur_matched_points3d_cloud->points.resize(cur_matched_points3d.size()); 555 | prev_matched_points3d_cloud->points.resize(prev_matched_points3d.size()); 556 | #pragma omp parallel for num_threads(NUM_THREADS) 557 | for(size_t i=0; ipoints[i].x = cur_matched_points3d[i].x; 559 | cur_matched_points3d_cloud->points[i].y = cur_matched_points3d[i].y; 560 | cur_matched_points3d_cloud->points[i].z = cur_matched_points3d[i].z; 561 | cur_matched_points3d_cloud->points[i].r = 255; 562 | cur_matched_points3d_cloud->points[i].g = 0; 563 | cur_matched_points3d_cloud->points[i].b = 0; 564 | } 565 | #pragma omp parallel for num_threads(NUM_THREADS) 566 | for(size_t i=0; ipoints[i].x = prev_matched_points3d[i].x; 568 | prev_matched_points3d_cloud->points[i].y = prev_matched_points3d[i].y; 569 | prev_matched_points3d_cloud->points[i].z = prev_matched_points3d[i].z; 570 | prev_matched_points3d_cloud->points[i].r = 0; 571 | prev_matched_points3d_cloud->points[i].g = 255; 572 | prev_matched_points3d_cloud->points[i].b = 0; 573 | } 574 | 575 | //transform cur_matched_points3d_cloud to map 576 | pcl::PointCloud::Ptr cur_matched(new pcl::PointCloud()); 577 | Eigen::Matrix4d cur_transform = Eigen::Matrix4d::Identity(); 578 | cur_transform.block<3,3>(0,0) = cur_q.toRotationMatrix(); 579 | cur_transform.block<3,1>(0,3) = cur_t; 580 | pcl::transformPointCloud(*cur_matched_points3d_cloud, *cur_matched, cur_transform); 581 | 582 | 583 | //transform prev_matched_points3d_cloud to map 584 | pcl::PointCloud::Ptr prev_matched(new pcl::PointCloud); 585 | Eigen::Matrix4d prev_transform = Eigen::Matrix4d::Identity(); 586 | prev_transform.block<3,3>(0,0) = prev_q.toRotationMatrix(); 587 | prev_transform.block<3,1>(0,3) = prev_t; 588 | pcl::transformPointCloud(*prev_matched_points3d_cloud, *prev_matched, prev_transform); 589 | 590 | // put cur_matched_points3d_cloud and prev_matched_points3d_cloud together. 591 | pcl::PointCloud::Ptr matched_points3d_cloud(new pcl::PointCloud); 592 | *matched_points3d_cloud = *cur_matched + *prev_matched; 593 | // publish the matched points3d_cloud. 594 | if(matched_points_pub.getNumSubscribers() != 0){ 595 | sensor_msgs::PointCloud2 matched_points3d_cloud_msg; 596 | pcl::toROSMsg(*matched_points3d_cloud, matched_points3d_cloud_msg); 597 | matched_points3d_cloud_msg.header = header; 598 | matched_points3d_cloud_msg.header.frame_id = "map"; 599 | matched_points_pub.publish(matched_points3d_cloud_msg); 600 | } 601 | 602 | } 603 | } 604 | 605 | // @in: prev_out_point3d_, cur_out_point3d_ 606 | // @out: prev_matched_points3d_, cur_matched_points3d_ 607 | void mapOptimization::extractMatchedPoints(std::vector &matches, std::vector &prev_matched_points3d, std::vector &cur_matched_points3d, std::vector &prev_out_point3d, std::vector &cur_out_point3d){ 608 | prev_matched_points3d.clear(); 609 | cur_matched_points3d.clear(); 610 | prev_matched_points3d.resize(matches.size()); 611 | cur_matched_points3d.resize(matches.size()); 612 | 613 | #pragma omp parallel for num_threads(NUM_THREADS) 614 | for(size_t i=0; i q_w_curr(parameters_); 627 | Eigen::Map t_w_curr(parameters_ + 4); 628 | q_w_curr = q_wmap_wodom * q_wodom_curr; 629 | t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; 630 | } 631 | 632 | void mapOptimization::transformUpdate() 633 | { 634 | Eigen::Map q_w_curr(parameters_); 635 | Eigen::Map t_w_curr(parameters_ + 4); 636 | q_wmap_wodom = q_w_curr * q_wodom_curr.inverse(); 637 | t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr; 638 | } 639 | void mapOptimization::calculateAverageDistance(double &avg_distance, std::vector prev_matched_points3d, Eigen::Quaterniond prev_q, Eigen::Vector3d prev_t, std::vector cur_matched_points3d, Eigen::Quaterniond cur_q, Eigen::Vector3d cur_t){ 640 | double sum_distance = 0; 641 | double min_distance = std::numeric_limits::max(); 642 | double max_distance = std::numeric_limits::min(); 643 | for(size_t i=0; i max_distance) 651 | max_distance = distance; 652 | } 653 | avg_distance = sum_distance / prev_matched_points3d.size(); 654 | 655 | 656 | // //ros info the min and max distance 657 | // ROS_INFO("min_distance: %f", min_distance); 658 | // ROS_INFO("max_distance: %f", max_distance); 659 | // ROS_INFO("avg_distance: %f", avg_distance); 660 | 661 | } 662 | 663 | void mapOptimization::appendLines(visualization_msgs::Marker &line_list, geometry_msgs::Point p1, geometry_msgs::Point p2){ 664 | line_list.points.push_back(p1); 665 | line_list.points.push_back(p2); 666 | } 667 | void mapOptimization::initialLineList(visualization_msgs::Marker &line_list, std_msgs::Header header){ 668 | // clear line_list 669 | line_list.points.clear(); 670 | line_list.header = header; 671 | line_list.header.frame_id = "map"; 672 | line_list.ns = "basic_line"; 673 | line_list.id = 0; 674 | line_list.type = visualization_msgs::Marker::LINE_LIST; 675 | line_list.action = visualization_msgs::Marker::ADD; 676 | line_list.scale.x = 0.005; 677 | line_list.scale.y = 0.005; 678 | line_list.scale.z = 0.005; 679 | line_list.color.a = 1.0; 680 | line_list.color.r = 0.0; 681 | line_list.color.g = 0.0; 682 | line_list.color.b = 1.0; 683 | line_list.pose.orientation.w = 1.0; 684 | 685 | 686 | } 687 | void mapOptimization::image_show(std::vector &matches, std::string& detectTime, cv::Mat prev_img, cv::Mat cur_img, std::vector cur_orb_point_2d_uv_, std::vector prev_orb_point_2d_uv_){ 688 | int gap =10; 689 | cv::Mat gap_image(gap, prev_img.size().width, CV_8UC1, cv::Scalar(255,255,255)); 690 | cv::Mat img_show; 691 | 692 | cv::vconcat(cur_img, gap_image, img_show); 693 | cv::vconcat(img_show, prev_img, img_show); 694 | cv::cvtColor(img_show, img_show, cv::COLOR_GRAY2RGB); 695 | 696 | // draw keypoints in current frame 697 | for(size_t i = 0; i< (size_t)cur_orb_point_2d_uv_.size(); i++){ 698 | cv::Point2f cur_pt = cur_orb_point_2d_uv_[i]; 699 | cv::circle(img_show, cur_pt, 5, cv::Scalar(0,255,0)); 700 | } 701 | // draw keypoints in previous frame 702 | for(size_t i = 0; i< (size_t)prev_orb_point_2d_uv_.size(); i++){ 703 | cv::Point2f prev_pt = prev_orb_point_2d_uv_[i]; 704 | prev_pt.y += cur_img.size().height + gap; 705 | cv::circle(img_show, prev_pt, 5, cv::Scalar(0,0,255)); 706 | } 707 | 708 | //draw lines for matches 709 | for(size_t i = 0; i< (size_t)matches.size(); i++){ 710 | int cur_pt_index = matches[i].queryIdx; 711 | cv::Point2f cur_pt = cur_orb_point_2d_uv_[cur_pt_index]; 712 | int prev_pt_index = matches[i].trainIdx; 713 | cv::Point2f prev_pt = prev_orb_point_2d_uv_[prev_pt_index]; 714 | prev_pt.y += cur_img.size().height + gap; 715 | 716 | cv::line(img_show, cur_pt, prev_pt, cv::Scalar(0,255,0), 2, 8, 0); 717 | } 718 | 719 | std::string keypoint_cur_imgtext("cur_img, time cost ms:"); 720 | keypoint_cur_imgtext.append(detectTime); 721 | 722 | std::string match_num("Match num:"); 723 | int match_size = (int)matches.size(); 724 | match_num += std::to_string(match_size); 725 | 726 | 727 | cv::putText(img_show, keypoint_cur_imgtext, cv::Point2f(5, 20 + IMAGE_HEIGHT*0), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(255,0,255), 2); 728 | 729 | cv::putText(img_show, match_num, cv::Point2f(5, 60 + IMAGE_HEIGHT*0), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(255,0,255), 2); 730 | 731 | cv::putText(img_show, "prev_img", cv::Point2f(5, 20 + IMAGE_HEIGHT*1 + gap), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(255,0,255), 2); 732 | 733 | if(matched_keypoints_img_pub.getNumSubscribers() > 0){ 734 | cv_bridge::CvImage output_image; 735 | output_image.header.frame_id = "map"; 736 | output_image.encoding = sensor_msgs::image_encodings::TYPE_8UC3; 737 | output_image.image = img_show; 738 | matched_keypoints_img_pub.publish(output_image); 739 | } 740 | } -------------------------------------------------------------------------------- /src/mapOptimization.hpp: -------------------------------------------------------------------------------- 1 | //include sensor_msgs pointcloud2 2 | #pragma once 3 | #include 4 | #include 5 | #include 6 | // include eigen library 7 | // #include 8 | #include 9 | #include 10 | #include "image_handler.h" 11 | #include "tic_toc.h" 12 | #include "keyframe.h" 13 | #include 14 | #include 15 | #include "ikd-Tree/ikd_Tree.h" 16 | #define PCL_NO_PRECOMPILE 17 | 18 | template 19 | static void reduceVector(std::vector &v, std::vector status) 20 | { 21 | int j = 0; 22 | for (int i = 0; i < int(v.size()); i++) 23 | if (status[i]) 24 | v[j++] = v[i]; 25 | v.resize(j); 26 | } 27 | 28 | class mapOptimization 29 | { 30 | public: 31 | mapOptimization(); 32 | ~mapOptimization(); 33 | void mapOptimizationCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg, const nav_msgs::Odometry::ConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& plane_cloud_msg, const sensor_msgs::PointCloud2::ConstPtr& pc_corner_msg); 34 | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry); 35 | cv::Mat setMask(int height, int width, int crop); 36 | void keypoint2uv(const std::vector& keypoints, std::vector& uv); 37 | void extractPointsAndFilterZeroValue(const std::vector& cur_orb_point_2d_uv_, const pcl::PointCloud::Ptr& cloudTrack_, std::vector& cur_out_point3d_, std::vector& status_); 38 | void extractMatchedPoints(std::vector &matches, std::vector &prev_matched_points3d, Eigen::Quaterniond prev_q, Eigen::Vector3d prev_t, std::vector &cur_matched_points3d, Eigen::Quaterniond cur_q, Eigen::Vector3d cur_t, std::vector &prev_out_point3d, std::vector &cur_out_point3d, std_msgs::Header header); 39 | void extractMatchedPoints(std::vector &matches, std::vector &prev_matched_points3d, std::vector &cur_matched_points3d, std::vector &prev_out_point3d, std::vector &cur_out_point3d); 40 | 41 | void transformUpdate(); 42 | void transformAssociateToMap(); 43 | void calculateAverageDistance(double &avg_distance, std::vector prev_matched_points3d, Eigen::Quaterniond prev_q, Eigen::Vector3d prev_t, std::vector cur_matched_points3d, Eigen::Quaterniond cur_q, Eigen::Vector3d cur_t); 44 | void appendLines(visualization_msgs::Marker &line_list, geometry_msgs::Point p1, geometry_msgs::Point p2); 45 | void initialLineList(visualization_msgs::Marker &line_list, std_msgs::Header header); 46 | void image_show(std::vector &matches, std::string& detectTime, cv::Mat prev_img, cv::Mat cur_img, std::vector cur_orb_point_2d_uv_, std::vector prev_orb_point_2d_uv_); 47 | void filterNaNPoints(); 48 | intensity_slam::ImageHandler *image_handler_; 49 | std::map keyframe_pool_; 50 | std::deque> keyframe_sliding_window_; 51 | mapProcessor::Keyframe prev_keyframe, cur_keyframe; 52 | 53 | private: 54 | size_t keyframeId_; 55 | int IMAGE_WIDTH; 56 | int IMAGE_HEIGHT; 57 | int IMAGE_CROP; 58 | int NUM_THREADS; 59 | int NUM_ORB_FEATURES; 60 | bool HAND_HELD_FLAG; 61 | int SLIDING_WINDOW_SIZE; 62 | int GROUND_PLANE_WINDOW_SIZE; 63 | Eigen::Quaterniond q_wmap_wodom; 64 | Eigen::Vector3d t_wmap_wodom; 65 | Eigen::Quaterniond q_wodom_curr; 66 | Eigen::Vector3d t_wodom_curr; 67 | ros::Publisher pubOdomAftMappedHighFrec; 68 | ros::Publisher robot_marker_pub; 69 | ros::Publisher matched_points_pub; 70 | std::vector prev_keypoints_, cur_keypoints_, cur_keypoints_tmp_; 71 | std::vector prev_orb_point_2d_uv_, cur_orb_point_2d_uv_, cur_orb_point_2d_uv_tmp_; 72 | std::vector prev_out_point3d_, cur_out_point3d_, cur_out_point3d_tmp_; 73 | std::vector status_; 74 | cv::Mat MASK; 75 | cv::Mat prev_descriptors_, cur_descriptors_, cur_descriptors_tmp_; 76 | cv::Mat prev_keyframe_img, cur_keyframe_img; 77 | cv::BFMatcher matcher_; 78 | bool keyframe_flag_; 79 | double parameters_[7] = {0, 0, 0, 1, 0, 0, 0}; 80 | std::vector prev_matched_points3d_, cur_matched_points3d_; 81 | visualization_msgs::Marker line_list; 82 | ros::Publisher matched_lines_pub; 83 | ros::Publisher matched_keypoints_img_pub, ground_points_pub; 84 | pcl::VoxelGrid voxel_grid_, corner_voxel_grid_; 85 | pcl::PointXYZ ground_point_sensor_, ground_point_world_; 86 | std::mutex map_mutex_; 87 | KD_TREE::Ptr ikdtree, corner_ikdtree_; 88 | pcl::PointCloud ground_plane_cloud_; 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | }; 103 | 104 | -------------------------------------------------------------------------------- /src/mapOptimizationNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "lidarFeaturePointsFunction.hpp" 12 | #include "mapOptimization.hpp" 13 | #include 14 | #include 15 | 16 | int main(int argc, char **argv) 17 | { 18 | 19 | ros::init(argc, argv, "map_optimization"); 20 | ros::NodeHandle nh; 21 | std::string CLOUD_TOPIC; // full cloud topic name 22 | nh.getParam("/intensity_feature_tracker/cloud_topic", CLOUD_TOPIC); 23 | mapOptimization mapOptimization_; 24 | message_filters::Subscriber cloud_sub(nh, CLOUD_TOPIC, 20); 25 | message_filters::Subscriber odom_sub(nh, "/laser_odom_to_init", 20); 26 | message_filters::Subscriber plane_sub(nh, "/laser_cloud_less_flat", 20); 27 | message_filters::Subscriber plane_search_sub(nh, "/laser_cloud_less_sharp", 20); 28 | typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; 29 | message_filters::Synchronizer sync(MySyncPolicy(10), cloud_sub, odom_sub, plane_sub, plane_search_sub); 30 | sync.registerCallback(boost::bind(&mapOptimization::mapOptimizationCallback, &mapOptimization_, _1, _2, _3, _4)); 31 | ros::Subscriber subLaserOdometry = nh.subscribe("/laser_odom_to_init", 20, &mapOptimization::laserOdometryHandler, &mapOptimization_); 32 | ros::Publisher pub_aft_mapped_to_init = nh.advertise("/aft_mapped_to_init", 100); 33 | ros::MultiThreadedSpinner spinner(8); 34 | spinner.spin(); 35 | 36 | return 0; 37 | } -------------------------------------------------------------------------------- /src/odom_handler_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | ros::Publisher pubMergedOdometry; 11 | Eigen::Quaterniond aloam_q_wodom_prev, intensity_q_wodom_prev, q_odom; 12 | Eigen::Vector3d aloam_t_wodom_prev, intensity_t_wodom_prev, t_odom; 13 | Eigen::Matrix4d odom_cur, aloam_odom_cur, intensity_odom_cur, aloam_prev, intensity_prev; 14 | bool aloam_odom_init = false, intensity_odom_init = false; 15 | std::string skip_flag; 16 | 17 | void callback(const nav_msgs::Odometry::ConstPtr& aloam_odom, const nav_msgs::Odometry::ConstPtr& intensity_odom) 18 | { 19 | Eigen::Quaterniond aloam_q_wodom_curr, intensity_q_wodom_curr; 20 | Eigen::Vector3d aloam_t_wodom_curr, intensity_t_wodom_curr; 21 | aloam_q_wodom_curr.x() = aloam_odom->pose.pose.orientation.x; 22 | aloam_q_wodom_curr.y() = aloam_odom->pose.pose.orientation.y; 23 | aloam_q_wodom_curr.z() = aloam_odom->pose.pose.orientation.z; 24 | aloam_q_wodom_curr.w() = aloam_odom->pose.pose.orientation.w; 25 | aloam_t_wodom_curr.x() = aloam_odom->pose.pose.position.x; 26 | aloam_t_wodom_curr.y() = aloam_odom->pose.pose.position.y; 27 | aloam_t_wodom_curr.z() = aloam_odom->pose.pose.position.z; 28 | aloam_odom_cur.block<3,3>(0,0) = aloam_q_wodom_curr.toRotationMatrix(); 29 | aloam_odom_cur.block<3,1>(0,3) = aloam_t_wodom_curr; 30 | aloam_odom_cur.block(3,0,1,4) << 0, 0, 0, 1; 31 | 32 | intensity_q_wodom_curr.x() = intensity_odom->pose.pose.orientation.x; 33 | intensity_q_wodom_curr.y() = intensity_odom->pose.pose.orientation.y; 34 | intensity_q_wodom_curr.z() = intensity_odom->pose.pose.orientation.z; 35 | intensity_q_wodom_curr.w() = intensity_odom->pose.pose.orientation.w; 36 | intensity_t_wodom_curr.x() = intensity_odom->pose.pose.position.x; 37 | intensity_t_wodom_curr.y() = intensity_odom->pose.pose.position.y; 38 | intensity_t_wodom_curr.z() = intensity_odom->pose.pose.position.z; 39 | skip_flag = intensity_odom->child_frame_id; 40 | intensity_odom_cur.block<3,3>(0,0) = intensity_q_wodom_curr.toRotationMatrix(); 41 | intensity_odom_cur.block<3,1>(0,3) = intensity_t_wodom_curr; 42 | intensity_odom_cur.block(3,0,1,4) << 0, 0, 0, 1; 43 | 44 | if(!aloam_odom_init && !intensity_odom_init){ 45 | aloam_prev = aloam_odom_cur; 46 | intensity_prev = intensity_odom_cur; 47 | odom_cur = intensity_odom_cur; 48 | aloam_odom_init = true; 49 | intensity_odom_init = true; 50 | } 51 | else{ 52 | Eigen::Matrix4d intensity_odom_diff = intensity_prev.inverse() * intensity_odom_cur; 53 | Eigen::Matrix4d aloam_odom_diff = aloam_prev.inverse() * aloam_odom_cur; 54 | if( skip_flag == "/odom_skip"){ 55 | std::cout << "intensity_odom_diff: " << intensity_odom_diff << std::endl; 56 | odom_cur = odom_cur * aloam_odom_diff; 57 | } 58 | else{ 59 | odom_cur = odom_cur * intensity_odom_diff; 60 | } 61 | aloam_prev = aloam_odom_cur; 62 | intensity_prev = intensity_odom_cur; 63 | 64 | } 65 | Eigen::Matrix3d rot_cur = odom_cur.block(0,0,3,3); 66 | Eigen::Vector3d t_cur = odom_cur.block(0,3,3,1); 67 | Eigen::Quaterniond q_cur(rot_cur); 68 | nav_msgs::Odometry odom; 69 | odom.header.stamp = aloam_odom->header.stamp; 70 | odom.header.frame_id = "map"; 71 | odom.child_frame_id = "laser_odom"; 72 | odom.pose.pose.orientation.x = q_cur.x(); 73 | odom.pose.pose.orientation.y = q_cur.y(); 74 | odom.pose.pose.orientation.z = q_cur.z(); 75 | odom.pose.pose.orientation.w = q_cur.w(); 76 | odom.pose.pose.position.x = t_cur.x(); 77 | odom.pose.pose.position.y = t_cur.y(); 78 | odom.pose.pose.position.z = t_cur.z(); 79 | pubMergedOdometry.publish(odom); 80 | } 81 | 82 | int main(int argc, char **argv) 83 | { 84 | 85 | ros::init(argc, argv, "map_optimization"); 86 | ros::NodeHandle nh; 87 | pubMergedOdometry = nh.advertise("/laser_odom_to_init", 100); 88 | 89 | message_filters::Subscriber aloam_odom_sub(nh, "/laser_odom_to_init_aloam", 10); 90 | message_filters::Subscriber intensity_odom_sub(nh, "/laser_odom_to_init_intensity", 10); 91 | 92 | typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; 93 | message_filters::Synchronizer sync(MySyncPolicy(100), aloam_odom_sub, intensity_odom_sub); 94 | sync.registerCallback(boost::bind(&callback, _1, _2)); 95 | ros::spin(); 96 | return 0; 97 | } -------------------------------------------------------------------------------- /src/parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | // #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | 62 | 63 | #include "DBoW3/DBoW3.h" 64 | typedef pcl::PointXYZI PointType; 65 | typedef pcl::PointXYZ GroundPlanePointType; 66 | 67 | //pointXYZIR 68 | struct PointXYZIR { 69 | PCL_ADD_POINT4D; 70 | float intensity; 71 | std::uint8_t ring; 72 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 73 | }EIGEN_ALIGN16; 74 | 75 | POINT_CLOUD_REGISTER_POINT_STRUCT( 76 | PointXYZIR, 77 | (float, x, x) 78 | (float, y, y) 79 | (float, z, z) 80 | (float, intensity, intensity) 81 | (std::uint8_t, ring, ring) 82 | ) 83 | 84 | struct PointXYZIRPYT 85 | { 86 | PCL_ADD_POINT4D 87 | PCL_ADD_INTENSITY; 88 | float roll; 89 | float pitch; 90 | float yaw; 91 | double time; 92 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 93 | } EIGEN_ALIGN16; 94 | 95 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT, 96 | (float, x, x) (float, y, y) 97 | (float, z, z) (float, intensity, intensity) 98 | (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw) 99 | (double, time, time) 100 | ) 101 | 102 | typedef PointXYZIRPYT PointTypePose; 103 | typedef pcl::PointXYZI PointOuster; 104 | -------------------------------------------------------------------------------- /src/scanRegistration.cpp: -------------------------------------------------------------------------------- 1 | // Merge intensitySLAM framework with ALOAM and use ALOAM's odometry when intensity features are not enough, usually 2 or 3 frames 2 | // Modified by: Wenqiang Du snowdwq@gmail.com 3 | 4 | 5 | // This is an advanced implementation of the algorithm described in the following paper: 6 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 7 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 8 | 9 | // Modifier: Tong Qin qintonguav@gmail.com 10 | // Shaozu Cao saozu.cao@connect.ust.hk 11 | 12 | 13 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 14 | // Further contributions copyright (c) 2016, Southwest Research Institute 15 | // All rights reserved. 16 | // 17 | // Redistribution and use in source and binary forms, with or without 18 | // modification, are permitted provided that the following conditions are met: 19 | // 20 | // 1. Redistributions of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 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 | // 3. Neither the name of the copyright holder nor the names of its 26 | // contributors may be used to endorse or promote products derived from this 27 | // software without specific prior written permission. 28 | // 29 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 30 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 31 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 32 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 33 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 34 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 35 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 36 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 37 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 38 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 39 | // POSSIBILITY OF SUCH DAMAGE. 40 | 41 | 42 | #include 43 | #include 44 | #include 45 | #include "tic_toc.h" 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include "parameters.h" 59 | #include "intensity_feature_tracker.h" 60 | #include "image_handler.h" 61 | 62 | using std::atan2; 63 | using std::cos; 64 | using std::sin; 65 | uint32_t skipFrameNum = 0; 66 | 67 | const double scanPeriod = 0.1; 68 | 69 | const int systemDelay = 0; 70 | int systemInitCount = 0; 71 | bool systemInited = false; 72 | int N_SCANS = 0; 73 | float cloudCurvature[400000]; 74 | int cloudSortInd[400000]; 75 | int cloudNeighborPicked[400000]; 76 | int cloudLabel[400000]; 77 | 78 | intensity_slam::ImageHandler *image_handler; 79 | intensity_slam::feature_tracker *feature_tracker; 80 | 81 | bool comp (int i,int j) { return (cloudCurvature[i] pubEachScan; 90 | 91 | bool PUB_EACH_LINE = false; 92 | 93 | double MINIMUM_RANGE = 0.1; 94 | 95 | template 96 | void removeClosedPointCloud(const pcl::PointCloud &cloud_in, 97 | pcl::PointCloud &cloud_out, float thres) 98 | { 99 | if (&cloud_in != &cloud_out) 100 | { 101 | cloud_out.header = cloud_in.header; 102 | cloud_out.points.resize(cloud_in.points.size()); 103 | } 104 | 105 | size_t j = 0; 106 | 107 | for (size_t i = 0; i < cloud_in.points.size(); ++i) 108 | { 109 | if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres) 110 | continue; 111 | cloud_out.points[j] = cloud_in.points[i]; 112 | j++; 113 | } 114 | if (j != cloud_in.points.size()) 115 | { 116 | cloud_out.points.resize(j); 117 | } 118 | 119 | cloud_out.height = 1; 120 | cloud_out.width = static_cast(j); 121 | cloud_out.is_dense = true; 122 | } 123 | 124 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 125 | { 126 | TicToc t_whole; 127 | image_handler->cloud_handler(laserCloudMsg); 128 | ros::Time cloud_time = laserCloudMsg->header.stamp; 129 | TicToc detectFeatures_time; 130 | double cloudHandler_time = t_whole.toc(); 131 | feature_tracker->detectfeatures(cloud_time, image_handler->image_intensity, image_handler->cloud_track, image_handler->GroundPointOut); 132 | if (!systemInited) 133 | { 134 | systemInitCount++; 135 | if (systemInitCount >= systemDelay) 136 | { 137 | systemInited = true; 138 | } 139 | else 140 | return; 141 | } 142 | 143 | 144 | TicToc t_prepare; 145 | std::vector scanStartInd(N_SCANS, 0); 146 | std::vector scanEndInd(N_SCANS, 0); 147 | 148 | pcl::PointCloud laserCloudIn; 149 | pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); 150 | std::vector indices; 151 | removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE); 152 | 153 | 154 | int cloudSize = laserCloudIn.points.size(); 155 | float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); 156 | float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, 157 | laserCloudIn.points[cloudSize - 1].x) + 158 | 2 * M_PI; 159 | 160 | if (endOri - startOri > 3 * M_PI) 161 | { 162 | endOri -= 2 * M_PI; 163 | } 164 | else if (endOri - startOri < M_PI) 165 | { 166 | endOri += 2 * M_PI; 167 | } 168 | 169 | bool halfPassed = false; 170 | int count = cloudSize; 171 | PointType point; 172 | std::vector> laserCloudScans(N_SCANS); 173 | for (int i = 0; i < cloudSize; i++) 174 | { 175 | point.x = laserCloudIn.points[i].x; 176 | point.y = laserCloudIn.points[i].y; 177 | point.z = laserCloudIn.points[i].z; 178 | 179 | float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI; 180 | int scanID = 0; 181 | 182 | if (N_SCANS == 16) 183 | { 184 | scanID = int((angle + 15) / 2 + 0.5); 185 | if (scanID > (N_SCANS - 1) || scanID < 0) 186 | { 187 | count--; 188 | continue; 189 | } 190 | } 191 | else if (N_SCANS == 32) 192 | { 193 | scanID = int((angle + 92.0/3.0) * 3.0 / 4.0); 194 | if (scanID > (N_SCANS - 1) || scanID < 0) 195 | { 196 | count--; 197 | continue; 198 | } 199 | } 200 | else if (N_SCANS == 64) 201 | { 202 | scanID = int((angle + 22.5) * 1.41 + 0.5)-1; 203 | if (scanID > (N_SCANS - 1) || scanID < 0) 204 | { 205 | count--; 206 | continue; 207 | } 208 | } 209 | else if (N_SCANS == 128) 210 | { 211 | scanID = int((angle + 22.5) * 2.83 + 0.5)-1; 212 | if (scanID > (N_SCANS - 1) || scanID < 0) 213 | { 214 | count--; 215 | continue; 216 | } 217 | } 218 | else 219 | { 220 | printf("wrong scan number\n"); 221 | ROS_BREAK(); 222 | } 223 | 224 | float ori = -atan2(point.y, point.x); 225 | if (!halfPassed) 226 | { 227 | if (ori < startOri - M_PI / 2) 228 | { 229 | ori += 2 * M_PI; 230 | } 231 | else if (ori > startOri + M_PI * 3 / 2) 232 | { 233 | ori -= 2 * M_PI; 234 | } 235 | 236 | if (ori - startOri > M_PI) 237 | { 238 | halfPassed = true; 239 | } 240 | } 241 | else 242 | { 243 | ori += 2 * M_PI; 244 | if (ori < endOri - M_PI * 3 / 2) 245 | { 246 | ori += 2 * M_PI; 247 | } 248 | else if (ori > endOri + M_PI / 2) 249 | { 250 | ori -= 2 * M_PI; 251 | } 252 | } 253 | 254 | float relTime = (ori - startOri) / (endOri - startOri); 255 | point.intensity = scanID + scanPeriod * relTime; 256 | laserCloudScans[scanID].push_back(point); 257 | } 258 | 259 | cloudSize = count; 260 | pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); 261 | for (int i = 0; i < N_SCANS; i++) 262 | { 263 | scanStartInd[i] = laserCloud->size() + 5; 264 | *laserCloud += laserCloudScans[i]; 265 | scanEndInd[i] = laserCloud->size() - 6; 266 | } 267 | 268 | for (int i = 5; i < cloudSize - 5; i++) 269 | { 270 | float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x; 271 | float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y; 272 | float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z; 273 | 274 | cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ; 275 | cloudSortInd[i] = i; 276 | cloudNeighborPicked[i] = 0; 277 | cloudLabel[i] = 0; 278 | } 279 | 280 | 281 | TicToc t_pts; 282 | 283 | pcl::PointCloud cornerPointsSharp; 284 | pcl::PointCloud cornerPointsLessSharp; 285 | pcl::PointCloud surfPointsFlat; 286 | pcl::PointCloud surfPointsLessFlat; 287 | 288 | float t_q_sort = 0; 289 | for (int i = 0; i < N_SCANS; i++) 290 | { 291 | if( scanEndInd[i] - scanStartInd[i] < 6) 292 | continue; 293 | pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud); 294 | for (int j = 0; j < 6; j++) 295 | { 296 | int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; 297 | int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1; 298 | 299 | TicToc t_tmp; 300 | std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp); 301 | t_q_sort += t_tmp.toc(); 302 | 303 | int largestPickedNum = 0; 304 | for (int k = ep; k >= sp; k--) 305 | { 306 | int ind = cloudSortInd[k]; 307 | 308 | if (cloudNeighborPicked[ind] == 0 && 309 | cloudCurvature[ind] > 0.1) 310 | { 311 | 312 | largestPickedNum++; 313 | if (largestPickedNum <= 2) 314 | { 315 | cloudLabel[ind] = 2; 316 | cornerPointsSharp.push_back(laserCloud->points[ind]); 317 | cornerPointsLessSharp.push_back(laserCloud->points[ind]); 318 | } 319 | else if (largestPickedNum <= 20) 320 | { 321 | cloudLabel[ind] = 1; 322 | cornerPointsLessSharp.push_back(laserCloud->points[ind]); 323 | } 324 | else 325 | { 326 | break; 327 | } 328 | 329 | cloudNeighborPicked[ind] = 1; 330 | 331 | for (int l = 1; l <= 5; l++) 332 | { 333 | float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x; 334 | float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y; 335 | float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z; 336 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 337 | { 338 | break; 339 | } 340 | 341 | cloudNeighborPicked[ind + l] = 1; 342 | } 343 | for (int l = -1; l >= -5; l--) 344 | { 345 | float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x; 346 | float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y; 347 | float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z; 348 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 349 | { 350 | break; 351 | } 352 | 353 | cloudNeighborPicked[ind + l] = 1; 354 | } 355 | } 356 | } 357 | 358 | int smallestPickedNum = 0; 359 | for (int k = sp; k <= ep; k++) 360 | { 361 | int ind = cloudSortInd[k]; 362 | 363 | if (cloudNeighborPicked[ind] == 0 && 364 | cloudCurvature[ind] < 0.1) 365 | { 366 | 367 | cloudLabel[ind] = -1; 368 | surfPointsFlat.push_back(laserCloud->points[ind]); 369 | 370 | smallestPickedNum++; 371 | if (smallestPickedNum >= 4) 372 | { 373 | break; 374 | } 375 | 376 | cloudNeighborPicked[ind] = 1; 377 | for (int l = 1; l <= 5; l++) 378 | { 379 | float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x; 380 | float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y; 381 | float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z; 382 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 383 | { 384 | break; 385 | } 386 | 387 | cloudNeighborPicked[ind + l] = 1; 388 | } 389 | for (int l = -1; l >= -5; l--) 390 | { 391 | float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x; 392 | float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y; 393 | float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z; 394 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 395 | { 396 | break; 397 | } 398 | 399 | cloudNeighborPicked[ind + l] = 1; 400 | } 401 | } 402 | } 403 | 404 | for (int k = sp; k <= ep; k++) 405 | { 406 | if (cloudLabel[k] <= 0) 407 | { 408 | surfPointsLessFlatScan->push_back(laserCloud->points[k]); 409 | } 410 | } 411 | } 412 | 413 | pcl::PointCloud surfPointsLessFlatScanDS; 414 | pcl::VoxelGrid downSizeFilter; 415 | downSizeFilter.setInputCloud(surfPointsLessFlatScan); 416 | downSizeFilter.setLeafSize(0.2, 0.2, 0.2); 417 | downSizeFilter.filter(surfPointsLessFlatScanDS); 418 | 419 | surfPointsLessFlat += surfPointsLessFlatScanDS; 420 | } 421 | 422 | sensor_msgs::PointCloud2 laserCloudOutMsg; 423 | pcl::toROSMsg(*laserCloud, laserCloudOutMsg); 424 | laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; 425 | laserCloudOutMsg.header.frame_id = "os_sensor"; 426 | pubLaserCloud.publish(laserCloudOutMsg); 427 | 428 | sensor_msgs::PointCloud2 cornerPointsSharpMsg; 429 | pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg); 430 | cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp; 431 | if(feature_tracker->skip_intensity_){ 432 | cornerPointsSharpMsg.header.frame_id = "skip_intensity"; 433 | skipFrameNum ++; 434 | std::cout << "skipFrameNum: " << skipFrameNum << std::endl; 435 | ROS_ERROR("intensity image is hard to match, skipped!") 436 | feature_tracker->skip_intensity_ = false; 437 | 438 | } 439 | else{ 440 | cornerPointsSharpMsg.header.frame_id = "os_sensor"; 441 | } 442 | 443 | pubCornerPointsSharp.publish(cornerPointsSharpMsg); 444 | 445 | sensor_msgs::PointCloud2 cornerPointsLessSharpMsg; 446 | pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg); 447 | cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp; 448 | cornerPointsLessSharpMsg.header.frame_id = "os_sensor"; 449 | pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg); 450 | 451 | sensor_msgs::PointCloud2 surfPointsFlat2; 452 | pcl::toROSMsg(surfPointsFlat, surfPointsFlat2); 453 | surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp; 454 | surfPointsFlat2.header.frame_id = "os_sensor"; 455 | pubSurfPointsFlat.publish(surfPointsFlat2); 456 | 457 | sensor_msgs::PointCloud2 surfPointsLessFlat2; 458 | pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2); 459 | surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp; 460 | surfPointsLessFlat2.header.frame_id = "os_sensor"; 461 | pubSurfPointsLessFlat.publish(surfPointsLessFlat2); 462 | 463 | if(PUB_EACH_LINE) 464 | { 465 | for(int i = 0; i< N_SCANS; i++) 466 | { 467 | sensor_msgs::PointCloud2 scanMsg; 468 | pcl::toROSMsg(laserCloudScans[i], scanMsg); 469 | scanMsg.header.stamp = laserCloudMsg->header.stamp; 470 | scanMsg.header.frame_id = "os_sensor"; 471 | pubEachScan[i].publish(scanMsg); 472 | } 473 | } 474 | } 475 | 476 | std::mutex mapOdomMutex; 477 | void map_odom_callback(const nav_msgs::Odometry::ConstPtr &mapOdometry){ 478 | mapOdomMutex.lock(); 479 | feature_tracker->mapOdomQueue_.push(mapOdometry); 480 | mapOdomMutex.unlock(); 481 | } 482 | 483 | int main(int argc, char **argv) 484 | { 485 | ros::init(argc, argv, "scanRegistration"); 486 | ros::NodeHandle nh; 487 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); 488 | feature_tracker = new intensity_slam::feature_tracker(nh); 489 | std::cout << "read parameters" << std::endl; 490 | feature_tracker->readParameters(); 491 | 492 | image_handler = new intensity_slam::ImageHandler(feature_tracker->IMAGE_HEIGHT, 493 | feature_tracker->IMAGE_WIDTH, 494 | feature_tracker->NUM_THREADS); 495 | 496 | 497 | nh.param("/intensity_feature_tracker/image_height", N_SCANS, 64); 498 | 499 | nh.param("/map_optimization_parameters/remove_radius", MINIMUM_RANGE, 0.3); 500 | 501 | printf("scan line number %d \n", N_SCANS); 502 | 503 | if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64 && N_SCANS != 128) 504 | { 505 | printf("only support velodyne with 16, 32, 64, 128 scan line!"); 506 | return 0; 507 | } 508 | std::string CLOUD_TOPIC; 509 | nh.getParam("/intensity_feature_tracker/cloud_topic", CLOUD_TOPIC); 510 | 511 | ros::Subscriber subLaserCloud = nh.subscribe(CLOUD_TOPIC, 100, laserCloudHandler); 512 | ros::Subscriber sub_map_path = nh.subscribe("/aft_mapped_to_init_high_frec", 100, map_odom_callback); 513 | pubLaserCloud = nh.advertise("/velodyne_cloud_2", 100); 514 | pubCornerPointsSharp = nh.advertise("/laser_cloud_sharp", 100); 515 | pubCornerPointsLessSharp = nh.advertise("/laser_cloud_less_sharp", 100); 516 | pubSurfPointsFlat = nh.advertise("/laser_cloud_flat", 100); 517 | pubSurfPointsLessFlat = nh.advertise("/laser_cloud_less_flat", 100); 518 | pubRemovePoints = nh.advertise("/laser_remove_points", 100); 519 | if(PUB_EACH_LINE) 520 | { 521 | for(int i = 0; i < N_SCANS; i++) 522 | { 523 | ros::Publisher tmp = nh.advertise("/laser_scanid_" + std::to_string(i), 100); 524 | pubEachScan.push_back(tmp); 525 | } 526 | } 527 | std::thread loopClosureThread_(&intensity_slam::feature_tracker::loopClosureThread, feature_tracker); 528 | std::thread factorGraphThread_(&intensity_slam::feature_tracker::factorGraphThread, feature_tracker); 529 | ros::MultiThreadedSpinner spinner(8); 530 | spinner.spin(); 531 | factorGraphThread_.join(); 532 | loopClosureThread_.join(); 533 | 534 | return 0; 535 | } 536 | -------------------------------------------------------------------------------- /src/tic_toc.h: -------------------------------------------------------------------------------- 1 | // from https://github.com/HKUST-Aerial-Robotics/VINS-Mono 2 | #pragma once 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | class TicToc 10 | { 11 | public: 12 | TicToc() 13 | { 14 | tic(); 15 | } 16 | 17 | void tic() 18 | { 19 | start = std::chrono::system_clock::now(); 20 | } 21 | 22 | double toc() 23 | { 24 | end = std::chrono::system_clock::now(); 25 | std::chrono::duration elapsed_seconds = end - start; 26 | return elapsed_seconds.count() * 1000; 27 | } 28 | 29 | private: 30 | std::chrono::time_point start, end; 31 | }; 32 | --------------------------------------------------------------------------------