├── CMakeLists.txt ├── LICENSE ├── README.md ├── img ├── 00.png ├── 03.png ├── 05.png ├── 07.png ├── iscloam_kitti.gif ├── iscloam_kitti_mapping.gif └── iscloam_title.gif ├── include ├── iscGenerationClass.h ├── iscOptimizationClass.h ├── laserMappingClass.h ├── laserProcessingClass.h ├── lidar.h ├── lidarOptimization.h └── odomEstimationClass.h ├── launch ├── iscloam.launch ├── iscloam_mapping.launch └── iscloam_velodyne.launch ├── msg └── LoopInfo.msg ├── package.xml ├── rviz ├── iscloam.rviz ├── iscloam_mapping.rviz └── iscloam_velodyne.rviz └── src ├── iscGenerationClass.cpp ├── iscGenerationNode.cpp ├── iscOptimizationClass.cpp ├── iscOptimizationNode.cpp ├── laserMappingClass.cpp ├── laserMappingNode.cpp ├── laserProcessingClass.cpp ├── laserProcessingNode.cpp ├── lidar.cpp ├── lidarOptimization.cpp ├── odomEstimationClass.cpp └── odomEstimationNode.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(iscloam) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | geometry_msgs 10 | nav_msgs 11 | sensor_msgs 12 | roscpp 13 | rospy 14 | rosbag 15 | std_msgs 16 | tf 17 | eigen_conversions 18 | message_generation 19 | cv_bridge 20 | image_transport 21 | ) 22 | 23 | find_package(Eigen3) 24 | if(NOT EIGEN3_FOUND) 25 | # Fallback to cmake_modules 26 | find_package(cmake_modules REQUIRED) 27 | find_package(Eigen REQUIRED) 28 | set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 29 | set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only 30 | # Possibly map additional variables to the EIGEN3_ prefix. 31 | else() 32 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 33 | endif() 34 | find_package(Boost REQUIRED COMPONENTS timer thread) 35 | find_package(PCL REQUIRED) 36 | find_package(Ceres REQUIRED) 37 | find_package(GTSAM REQUIRED) 38 | find_package(OpenCV REQUIRED) 39 | 40 | include_directories( 41 | include 42 | ${catkin_INCLUDE_DIRS} 43 | ${PCL_INCLUDE_DIRS} 44 | ${CERES_INCLUDE_DIRS} 45 | ${EIGEN3_INCLUDE_DIRS} 46 | ${OpenCV_INCLUDE_DIRS} 47 | ${GTSAM_INCLUDE_DIR} 48 | ) 49 | 50 | link_directories( 51 | include 52 | ${PCL_LIBRARY_DIRS} 53 | ${CERES_LIBRARY_DIRS} 54 | ${GTSAM_LIBRARY_DIRS} 55 | ${OpenCV_LIBRARY_DIRS} 56 | ) 57 | 58 | add_message_files( 59 | FILES 60 | LoopInfo.msg 61 | ) 62 | 63 | generate_messages( 64 | DEPENDENCIES 65 | std_msgs 66 | ) 67 | 68 | 69 | catkin_package( 70 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime 71 | DEPENDS EIGEN3 PCL Ceres GTSAM 72 | INCLUDE_DIRS include 73 | ) 74 | 75 | add_executable(iscloam_laser_processing_node src/laserProcessingNode.cpp src/laserProcessingClass.cpp src/lidar.cpp) 76 | target_link_libraries(iscloam_laser_processing_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} ${OpenCV_LIBRARIES}) 77 | 78 | add_executable(iscloam_odom_estimation_node src/odomEstimationNode.cpp src/lidarOptimization.cpp src/lidar.cpp src/odomEstimationClass.cpp) 79 | target_link_libraries(iscloam_odom_estimation_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} ${OpenCV_LIBRARIES}) 80 | 81 | add_executable(iscloam_isc_generation_node src/iscGenerationNode.cpp src/iscGenerationClass.cpp src/lidar.cpp) 82 | target_link_libraries(iscloam_isc_generation_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} ${OpenCV_LIBRARIES}) 83 | add_dependencies(iscloam_isc_generation_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) 84 | 85 | add_executable(iscloam_isc_optimization_node src/iscOptimizationNode.cpp src/iscOptimizationClass.cpp src/lidarOptimization.cpp src/lidar.cpp) 86 | target_link_libraries(iscloam_isc_optimization_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} gtsam) 87 | add_dependencies(iscloam_isc_optimization_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) 88 | 89 | add_executable(iscloam_laser_mapping_node src/laserMappingNode.cpp src/laserMappingClass.cpp src/lidar.cpp) 90 | target_link_libraries(iscloam_laser_mapping_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 91 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | This is an full slam implemation for autonomous drive 2 | Author 3 | Wang Han 4 | Nanyang Technological University, Singapore 5 | Email: wh200720041@gmail.com 6 | Homepage: https://wanghan.pro 7 | 8 | 9 | The back-end slam is implemented based on Intensity Scan Context: 10 | Intensity Scan Context: Coding Intensity and Geometry Relations for Loop Closure Detection (ICRA 2020) 11 | 12 | 13 | The front-end slam is implemented based on LOAM, ALOAM and FLOAM: 14 | 15 | LOAM 16 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 17 | Copyright 2013, Ji Zhang, Carnegie Mellon University 18 | Further contributions copyright (c) 2016, Southwest Research Institute 19 | All rights reserved. 20 | 21 | Advanced implementation of LOAM (A-LOAM) 22 | Tong Qin qintonguav@gmail.com 23 | Shaozu Cao saozu.cao@connect.ust.hk 24 | 25 | FLOAM 26 | Wang Han wh200720041@gmail.com 27 | 28 | Redistribution and use in source and binary forms, with or without 29 | modification, are permitted provided that the following conditions are met: 30 | 31 | 1. Redistributions of source code must retain the above copyright notice, this 32 | list of conditions and the following disclaimer. 33 | 2. Redistributions in binary form must reproduce the above copyright notice, 34 | this list of conditions and the following disclaimer in the documentation 35 | and/or other materials provided with the distribution. 36 | 3. Neither the name of the copyright holder nor the names of its contributors 37 | may be used to endorse or promote products derived from this software without 38 | specific prior written permission. 39 | 40 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 41 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 42 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 43 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 44 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 45 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 46 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 47 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,OR 48 | TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 49 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 50 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ISCLOAM 2 | ## Intensity Scan Context based Full SLAM Implementation (ISC-LOAM) 3 | 4 | This work is an implementation of paper "Intensity Scan Context: Coding Intensity and Geometry Relations for Loop Closure Detection" in IEEE International Conference on Robotics and Automation 2020 (ICRA) [paper](https://arxiv.org/pdf/2003.05656.pdf) 5 | This work is 3D lidar based Simultaneous Localization And Mapping (SLAM), including both front-end and back-end SLAM, at 20Hz. 6 | 7 | **Author:** [Wang Han](http://wanghan.pro), Nanyang Technological University, Singapore 8 | 9 | For front-end only odometry, you may visit [FLOAM (fast lidar odometry and mapping)](https://github.com/wh200720041/floam) 10 | ## 1. Evaluation 11 | ### 1.1. Demo 12 | Watch our demo at [Video Link](https://youtu.be/Kfi6CFK4Ke4) 13 |

14 | 15 | 16 | 17 |

18 | 19 | ### 1.2. Mapping Example 20 |

21 | 22 | 23 | 24 |

25 | 26 | ### 1.3. Localization Example 27 |

28 | 29 |

30 | 31 | 32 | ### 1.4. Ground Truth Comparison 33 | Green: ISCLOAM Red: Ground Truth 34 |

35 | 36 | 37 |

38 | 39 | KITTI sequence 00 KITTI sequence 05 40 | 41 | ### 1.5. Localization error 42 | 43 | Platform: Intel® Core™ i7-8700 CPU @ 3.20GHz 44 | 45 | Average translation error : 1.08% 46 | 47 | Average rotation error : 0.000073 48 | 49 | ### 1.6. Comparison 50 | | Dataset | ISCLOAM | FLOAM | 51 | |----------------------------------------------|----------------------------|------------------------| 52 | | `KITTI sequence 00` | 0.24% | 0.51% | 53 | | `KITTI sequence 05` | 0.22% | 0.93% | 54 | 55 | ## 2. Prerequisites 56 | ### 2.1 **Ubuntu** and **ROS** 57 | Ubuntu 64-bit 20.04. 58 | 59 | ROS Noetic. [ROS Installation](http://wiki.ros.org/ROS/Installation) 60 | 61 | ### 2.2. **Ceres Solver** 62 | Follow [Ceres Installation](http://ceres-solver.org/installation.html). 63 | 64 | ### 2.3. **PCL** 65 | Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html). 66 | 67 | ### 2.3. **GTSAM** 68 | Follow [GTSAM Installation](https://gtsam.org/get_started/). 69 | 70 | ### 2.3. **OPENCV** 71 | Follow [OPENCV Installation](https://opencv.org/releases/). 72 | 73 | ### 2.4. **Trajectory visualization** 74 | For visualization purpose, this package uses hector trajectory sever, you may install the package by 75 | ``` 76 | sudo apt-get install ros-noetic-hector-trajectory-server 77 | ``` 78 | Alternatively, you may remove the hector trajectory server node if trajectory visualization is not needed 79 | 80 | ## 3. Build 81 | ### 3.1 Clone repository: 82 | ``` 83 | cd ~/catkin_ws/src 84 | git clone https://github.com/wh200720041/iscloam.git 85 | cd .. 86 | catkin_make -j1 87 | source ~/catkin_ws/devel/setup.bash 88 | ``` 89 | 90 | 91 | ### 3.2 Download test rosbag 92 | Download [KITTI sequence 05](https://drive.google.com/file/d/1eyO0Io3lX2z-yYsfGHawMKZa5Z0uYJ0W/view?usp=sharing) (10GB) or [KITTI sequence 07](https://drive.google.com/file/d/1_qUfwUw88rEKitUpt1kjswv7Cv4GPs0b/view?usp=sharing) (4GB) 93 | 94 | Unzip compressed file 2011_09_30_0018.zip. If your system does not have unzip. please install unzip by 95 | ``` 96 | sudo apt-get install unzip 97 | ``` 98 | 99 | This may take a few minutes to unzip the file, by default the file location should be /home/user/Downloads/2011_09_30_0018.bag 100 | ``` 101 | cd ~/Downloads 102 | unzip ~/Downloads/2011_09_30_0018.zip 103 | ``` 104 | 105 | ### 3.3 Launch ROS 106 | ``` 107 | roslaunch iscloam iscloam.launch 108 | ``` 109 | 110 | ### 3.4 Mapping Node 111 | if you would like to generate the map of environment at the same time, you can run 112 | ``` 113 | roslaunch iscloam iscloam_mapping.launch 114 | ``` 115 | Note that the global map can be very large, so it may takes a while to perform global optimization, some lag is expected between trajectory and map since they are running in separate thread. More CPU usage will happen when loop closure is identified. 116 | 117 | ## 4. Test other sequence 118 | To generate rosbag file of kitti dataset, you may use the tools provided by 119 | [kitti_to_rosbag](https://github.com/ethz-asl/kitti_to_rosbag) or [kitti2bag](https://github.com/tomas789/kitti2bag) 120 | 121 | ## 5. Other Velodyne sensor 122 | You may use iscloam_velodyne.launch for your own velodyne sensor, such as Velodyne VLP-16. 123 | 124 | 125 | ## 6. Citation 126 | If you use this work for your research, you may want to cite the paper below, your citation will be appreciated 127 | ``` 128 | @inproceedings{wang2020intensity, 129 | author={H. {Wang} and C. {Wang} and L. {Xie}}, 130 | booktitle={2020 IEEE International Conference on Robotics and Automation (ICRA)}, 131 | title={Intensity Scan Context: Coding Intensity and Geometry Relations for Loop Closure Detection}, 132 | year={2020}, 133 | volume={}, 134 | number={}, 135 | pages={2095-2101}, 136 | doi={10.1109/ICRA40945.2020.9196764} 137 | } 138 | ``` 139 | 140 | ## 7.Acknowledgements 141 | Thanks for [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM) and LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time) and [LOAM_NOTED](https://github.com/cuitaixiang/LOAM_NOTED). 142 | 143 | 144 | -------------------------------------------------------------------------------- /img/00.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wh200720041/iscloam/074d9634abe938887e93660d71264afb2c7a21d9/img/00.png -------------------------------------------------------------------------------- /img/03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wh200720041/iscloam/074d9634abe938887e93660d71264afb2c7a21d9/img/03.png -------------------------------------------------------------------------------- /img/05.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wh200720041/iscloam/074d9634abe938887e93660d71264afb2c7a21d9/img/05.png -------------------------------------------------------------------------------- /img/07.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wh200720041/iscloam/074d9634abe938887e93660d71264afb2c7a21d9/img/07.png -------------------------------------------------------------------------------- /img/iscloam_kitti.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wh200720041/iscloam/074d9634abe938887e93660d71264afb2c7a21d9/img/iscloam_kitti.gif -------------------------------------------------------------------------------- /img/iscloam_kitti_mapping.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wh200720041/iscloam/074d9634abe938887e93660d71264afb2c7a21d9/img/iscloam_kitti_mapping.gif -------------------------------------------------------------------------------- /img/iscloam_title.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wh200720041/iscloam/074d9634abe938887e93660d71264afb2c7a21d9/img/iscloam_title.gif -------------------------------------------------------------------------------- /include/iscGenerationClass.h: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #ifndef _ISC_GENERATION_CLASS_H_ 6 | #define _ISC_GENERATION_CLASS_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | //PCL 17 | #include 18 | #include 19 | 20 | //opencv 21 | #include 22 | #include 23 | 24 | //ros 25 | #include 26 | 27 | //pcl 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | 41 | //IF TRAVELLED DISTANCE IS LESS THAN THIS VALUE, SKIP FOR PLACE RECOGNTION 42 | #define SKIP_NEIBOUR_DISTANCE 20.0 43 | //how much error will odom generate per frame 44 | #define INFLATION_COVARIANCE 0.03 45 | 46 | //define threshold for loop closure detection 47 | #define GEOMETRY_THRESHOLD 0.67 48 | #define INTENSITY_THRESHOLD 0.91 49 | 50 | typedef cv::Mat ISCDescriptor; 51 | 52 | 53 | class ISCGenerationClass 54 | { 55 | public: 56 | ISCGenerationClass(); 57 | void init_param(int rings_in, int sectors_in, double max_dis_in); 58 | 59 | ISCDescriptor getLastISCMONO(void); 60 | ISCDescriptor getLastISCRGB(void); 61 | void loopDetection(const pcl::PointCloud::Ptr& current_pc, Eigen::Isometry3d& odom); 62 | 63 | int current_frame_id; 64 | std::vector matched_frame_id; 65 | private: 66 | int rings = 20; 67 | int sectors = 90; 68 | double ring_step=0.0; 69 | double sector_step=0.0; 70 | double max_dis = 60; 71 | 72 | std::vector color_projection; 73 | 74 | 75 | pcl::PointCloud::Ptr current_point_cloud; 76 | pcl::PointCloud::Ptr test_pc; 77 | 78 | std::vector pos_arr; 79 | std::vector travel_distance_arr; 80 | std::vector isc_arr; 81 | 82 | void init_color(void); 83 | void print_param(void); 84 | bool is_loop_pair(ISCDescriptor& desc1, ISCDescriptor& desc2, double& geo_score, double& inten_score); 85 | ISCDescriptor calculate_isc(const pcl::PointCloud::Ptr filtered_pointcloud); 86 | double calculate_geometry_dis(const ISCDescriptor& desc1, const ISCDescriptor& desc2, int& angle); 87 | double calculate_intensity_dis(const ISCDescriptor& desc1, const ISCDescriptor& desc2, int& angle); 88 | void ground_filter(const pcl::PointCloud::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out); 89 | }; 90 | 91 | 92 | 93 | 94 | #endif // _ISC_GENERATION_CLASS_H_ 95 | 96 | -------------------------------------------------------------------------------- /include/iscOptimizationClass.h: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #ifndef _ISC_OPTIMIZATION_CLASS_H_ 6 | #define _ISC_OPTIMIZATION_CLASS_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | //Eigen 14 | #include 15 | #include 16 | #include 17 | 18 | //PCL 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | //GTSAM 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | //ros 42 | #include 43 | 44 | //local lib 45 | #include "lidarOptimization.h" 46 | 47 | #define LOOPCLOSURE_THRESHOLD 41 48 | //stop loop check for the next N frames if loop is identified 49 | #define STOP_LOOP_CHECK_COUNTER 40 50 | class ISCOptimizationClass 51 | { 52 | public: 53 | ISCOptimizationClass(); 54 | 55 | void init(void); 56 | 57 | //return true if global optimized 58 | bool addPoseToGraph(const pcl::PointCloud::Ptr& pointcloud_edge_in, const pcl::PointCloud::Ptr& pointcloud_surf_in, std::vector& matched_frame_id, Eigen::Isometry3d& odom_in); 59 | 60 | Eigen::Isometry3d getLastPose(void); 61 | 62 | Eigen::Isometry3d getPose(int frame_num); 63 | 64 | std::vector::Ptr> pointcloud_arr; 65 | 66 | std::vector::Ptr> pointcloud_surf_arr; 67 | std::vector::Ptr> pointcloud_edge_arr; 68 | 69 | pcl::PointCloud::Ptr loop_candidate_pc = pcl::PointCloud::Ptr(new pcl::PointCloud()); 70 | pcl::PointCloud::Ptr loop_map_pc = pcl::PointCloud::Ptr(new pcl::PointCloud()); 71 | pcl::PointCloud::Ptr map = pcl::PointCloud::Ptr(new pcl::PointCloud()); 72 | 73 | private: 74 | std::vector pose_optimized_arr; 75 | std::vector odom_original_arr; 76 | gtsam::Pose3 last_pose3; 77 | 78 | gtsam::NonlinearFactorGraph graph; 79 | gtsam::Values initials; 80 | 81 | gtsam::noiseModel::Diagonal::shared_ptr priorModel; 82 | 83 | gtsam::noiseModel::Diagonal::shared_ptr odomModel; 84 | 85 | gtsam::noiseModel::Diagonal::shared_ptr loopModel; 86 | 87 | int stop_check_loop_count = 0; 88 | 89 | pcl::VoxelGrid downSizeFilter; 90 | 91 | Eigen::Isometry3d pose3ToEigen(const gtsam::Pose3& pose3); 92 | 93 | gtsam::Pose3 eigenToPose3(const Eigen::Isometry3d& pose_eigen); 94 | 95 | void globalOptimization(void); 96 | 97 | bool geometryConsistencyVerification(int current_id, int matched_id, Eigen::Isometry3d& transform); 98 | 99 | bool updateStates(gtsam::Values& result, int matched_id, int current_id); 100 | 101 | double estimateOdom(const pcl::PointCloud::Ptr& pc_source_edge, const pcl::PointCloud::Ptr& pc_source_surf, const pcl::PointCloud::Ptr& pc_target_edge, const pcl::PointCloud::Ptr& pc_target_surf, Eigen::Isometry3d& transform); 102 | 103 | }; 104 | 105 | 106 | #endif // _ISC_OPTIMIZATION_CLASS_H_ 107 | 108 | -------------------------------------------------------------------------------- /include/laserMappingClass.h: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #ifndef _LASER_MAPPING_H_ 6 | #define _LASER_MAPPING_H_ 7 | 8 | //PCL lib 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | //eigen lib 17 | #include 18 | #include 19 | 20 | //c++ lib 21 | #include 22 | #include 23 | #include 24 | 25 | 26 | #define LASER_CELL_WIDTH 50.0 27 | #define LASER_CELL_HEIGHT 50.0 28 | #define LASER_CELL_DEPTH 50.0 29 | 30 | //separate map as many sub point clouds 31 | 32 | #define LASER_CELL_RANGE_HORIZONTAL 2 33 | #define LASER_CELL_RANGE_VERTICAL 2 34 | 35 | 36 | class LaserMappingClass 37 | { 38 | 39 | public: 40 | LaserMappingClass(); 41 | void init(double map_resolution_); 42 | void resetMap(std::vector& path); 43 | void updateCurrentPointsToMap(const pcl::PointCloud::Ptr& pc_in, const Eigen::Isometry3d& pose_current); 44 | pcl::PointCloud::Ptr getMap(void); 45 | std::vector::Ptr> point_cloud_arr; 46 | Eigen::Isometry3d last_pose; 47 | 48 | private: 49 | int origin_in_map_x; 50 | int origin_in_map_y; 51 | int origin_in_map_z; 52 | int map_width; 53 | int map_height; 54 | int map_depth; 55 | std::vector::Ptr>>> map; 56 | pcl::VoxelGrid downSizeFilter; 57 | double map_resolution = 1.0; 58 | void addWidthCellNegative(void); 59 | void addWidthCellPositive(void); 60 | void addHeightCellNegative(void); 61 | void addHeightCellPositive(void); 62 | void addDepthCellNegative(void); 63 | void addDepthCellPositive(void); 64 | void checkPoints(int& x, int& y, int& z); 65 | 66 | }; 67 | 68 | 69 | #endif // _LASER_MAPPING_H_ 70 | 71 | -------------------------------------------------------------------------------- /include/laserProcessingClass.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _LASER_PROCESSING_CLASS_H_ 5 | #define _LASER_PROCESSING_CLASS_H_ 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "lidar.h" 17 | 18 | //points covariance class 19 | class Double2d{ 20 | public: 21 | int id; 22 | double value; 23 | Double2d(int id_in, double value_in); 24 | }; 25 | //points info class 26 | class PointsInfo{ 27 | public: 28 | int layer; 29 | double time; 30 | PointsInfo(int layer_in, double time_in); 31 | }; 32 | 33 | 34 | class LaserProcessingClass 35 | { 36 | public: 37 | LaserProcessingClass(); 38 | void init(lidar::Lidar lidar_param_in); 39 | void featureExtraction(const pcl::PointCloud::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf); 40 | void featureExtractionFromSector(const pcl::PointCloud::Ptr& pc_in, std::vector& cloudCurvature, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf); 41 | private: 42 | lidar::Lidar lidar_param; 43 | }; 44 | 45 | 46 | 47 | #endif // _LASER_PROCESSING_CLASS_H_ 48 | 49 | -------------------------------------------------------------------------------- /include/lidar.h: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #ifndef _LIDAR_H_ 6 | #define _LIDAR_H_ 7 | 8 | //define lidar parameter 9 | 10 | namespace lidar{ 11 | 12 | class Lidar 13 | { 14 | public: 15 | Lidar(); 16 | 17 | void setScanPeriod(double scan_period_in); 18 | void setLines(double num_lines_in); 19 | void setVerticalAngle(double vertical_angle_in); 20 | void setVerticalResolution(double vertical_angle_resolution_in); 21 | //by default is 100. pls do not change 22 | void setMaxDistance(double max_distance_in); 23 | void setMinDistance(double min_distance_in); 24 | 25 | double max_distance; 26 | double min_distance; 27 | int num_lines; 28 | double scan_period; 29 | int points_per_line; 30 | double horizontal_angle_resolution; 31 | double horizontal_angle; 32 | double vertical_angle_resolution; 33 | double vertical_angle; 34 | }; 35 | 36 | 37 | } 38 | 39 | 40 | #endif // _LIDAR_H_ 41 | 42 | -------------------------------------------------------------------------------- /include/lidarOptimization.h: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _LIDAR_OPTIMIZATION_ANALYTIC_H_ 5 | #define _LIDAR_OPTIMIZATION_ANALYTIC_H_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #if CERES_VERSION_MAJOR >= 3 || (CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1) 14 | #include 15 | #endif 16 | 17 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t); 18 | 19 | Eigen::Matrix3d skew(Eigen::Vector3d& mat_in); 20 | 21 | class EdgeAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> { 22 | public: 23 | 24 | EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_); 25 | virtual ~EdgeAnalyticCostFunction() {} 26 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 27 | 28 | Eigen::Vector3d curr_point; 29 | Eigen::Vector3d last_point_a; 30 | Eigen::Vector3d last_point_b; 31 | }; 32 | 33 | class SurfNormAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> { 34 | public: 35 | SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_); 36 | virtual ~SurfNormAnalyticCostFunction() {} 37 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 38 | 39 | Eigen::Vector3d curr_point; 40 | Eigen::Vector3d plane_unit_norm; 41 | double negative_OA_dot_norm; 42 | }; 43 | 44 | #if CERES_VERSION_MAJOR >= 3 || (CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1) 45 | class PoseSE3Parameterization : public ceres::Manifold { 46 | public: 47 | 48 | PoseSE3Parameterization() {} 49 | virtual ~PoseSE3Parameterization() {} 50 | virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const; 51 | virtual bool PlusJacobian(const double* x, double* jacobian) const; 52 | virtual bool Minus(const double* x, const double* delta, double* x_minus_delta) const; 53 | virtual bool MinusJacobian(const double* x, double* jacobian) const; 54 | virtual int AmbientSize() const { return 7; } 55 | virtual int TangentSize() const { return 6; } 56 | }; 57 | #else 58 | class PoseSE3Parameterization : public ceres::LocalParameterization { 59 | public: 60 | 61 | PoseSE3Parameterization() {} 62 | virtual ~PoseSE3Parameterization() {} 63 | virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const; 64 | virtual bool ComputeJacobian(const double* x, double* jacobian) const; 65 | virtual int GlobalSize() const { return 7; } 66 | virtual int LocalSize() const { return 6; } 67 | }; 68 | #endif 69 | #endif // _LIDAR_OPTIMIZATION_ANALYTIC_H_ 70 | 71 | -------------------------------------------------------------------------------- /include/odomEstimationClass.h: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #ifndef _ODOM_ESTIMATION_CLASS_H_ 6 | #define _ODOM_ESTIMATION_CLASS_H_ 7 | 8 | //std lib 9 | #include 10 | #include 11 | #include 12 | 13 | //PCL 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | //ceres 25 | #include 26 | #include 27 | 28 | //eigen 29 | #include 30 | #include 31 | 32 | //LOCAL LIB 33 | #include "lidar.h" 34 | #include "lidarOptimization.h" 35 | #include 36 | 37 | class OdomEstimationClass 38 | { 39 | 40 | public: 41 | OdomEstimationClass(); 42 | 43 | void init(lidar::Lidar lidar_param, double map_resolution); 44 | void initMapWithPoints(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in); 45 | void updatePointsToMap(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in); 46 | void getMap(pcl::PointCloud::Ptr& laserCloudMap); 47 | 48 | Eigen::Isometry3d odom; 49 | pcl::PointCloud::Ptr laserCloudCornerMap; 50 | pcl::PointCloud::Ptr laserCloudSurfMap; 51 | private: 52 | //optimization variable 53 | double parameters[7] = {0, 0, 0, 1, 0, 0, 0}; 54 | Eigen::Map q_w_curr = Eigen::Map(parameters); 55 | Eigen::Map t_w_curr = Eigen::Map(parameters + 4); 56 | 57 | Eigen::Isometry3d last_odom; 58 | 59 | //kd-tree 60 | pcl::KdTreeFLANN::Ptr kdtreeEdgeMap; 61 | pcl::KdTreeFLANN::Ptr kdtreeSurfMap; 62 | 63 | //points downsampling before add to map 64 | pcl::VoxelGrid downSizeFilterEdge; 65 | pcl::VoxelGrid downSizeFilterSurf; 66 | 67 | //local map 68 | pcl::CropBox cropBoxFilter; 69 | 70 | //optimization count 71 | int optimization_count; 72 | 73 | //function 74 | void addEdgeCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function); 75 | void addSurfCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function); 76 | void addPointsToMap(const pcl::PointCloud::Ptr& downsampledEdgeCloud, const pcl::PointCloud::Ptr& downsampledSurfCloud); 77 | void pointAssociateToMap(pcl::PointXYZI const *const pi, pcl::PointXYZI *const po); 78 | void downSamplingToMap(const pcl::PointCloud::Ptr& edge_pc_in, pcl::PointCloud::Ptr& edge_pc_out, const pcl::PointCloud::Ptr& surf_pc_in, pcl::PointCloud::Ptr& surf_pc_out); 79 | }; 80 | 81 | #endif // _ODOM_ESTIMATION_CLASS_H_ 82 | 83 | -------------------------------------------------------------------------------- /launch/iscloam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /launch/iscloam_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /launch/iscloam_velodyne.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /msg/LoopInfo.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int32 current_id 3 | int32[] matched_id -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | iscloam 4 | 0.0.0 5 | 6 | 7 | This work is an implmentation of paper 8 | Intensity Scan Context: Coding Intensity and Geometry Relations for Loop Closure Detection (ICRA 2020) 9 | 10 | 11 | Han Wang 12 | 13 | BSD 14 | https://wanghan.pro 15 | Han Wang 16 | 17 | catkin 18 | geometry_msgs 19 | nav_msgs 20 | roscpp 21 | rospy 22 | std_msgs 23 | rosbag 24 | sensor_msgs 25 | tf 26 | image_transport 27 | eigen_conversions 28 | message_generation 29 | message_runtime 30 | geometry_msgs 31 | nav_msgs 32 | sensor_msgs 33 | roscpp 34 | rospy 35 | std_msgs 36 | rosbag 37 | tf 38 | eigen_conversions 39 | image_transport 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /rviz/iscloam.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /iscloam_result1 9 | Splitter Ratio: 0.5 10 | Tree Height: 430 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: Camera_View 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 50 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 15 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/Image 56 | Enabled: true 57 | Image Topic: /cam03/image_raw 58 | Max Value: 1 59 | Median window: 5 60 | Min Value: 0 61 | Name: Camera_View 62 | Normalize Range: true 63 | Queue Size: 2 64 | Transport Hint: raw 65 | Unreliable: false 66 | Value: true 67 | - Class: rviz/Image 68 | Enabled: false 69 | Image Topic: /isc 70 | Max Value: 1 71 | Median window: 5 72 | Min Value: 0 73 | Name: ISC_Image 74 | Normalize Range: true 75 | Queue Size: 2 76 | Transport Hint: raw 77 | Unreliable: false 78 | Value: false 79 | - Class: rviz/TF 80 | Enabled: true 81 | Frame Timeout: 15 82 | Frames: 83 | All Enabled: false 84 | aft_mapped: 85 | Value: true 86 | cam00: 87 | Value: false 88 | cam01: 89 | Value: false 90 | cam02: 91 | Value: false 92 | cam03: 93 | Value: false 94 | imu: 95 | Value: false 96 | map: 97 | Value: false 98 | odom_final: 99 | Value: true 100 | velodyne: 101 | Value: false 102 | world: 103 | Value: true 104 | Marker Scale: 5 105 | Name: TF 106 | Show Arrows: false 107 | Show Axes: true 108 | Show Names: true 109 | Tree: 110 | world: 111 | imu: 112 | cam00: 113 | {} 114 | cam01: 115 | {} 116 | cam02: 117 | {} 118 | cam03: 119 | {} 120 | velodyne: 121 | {} 122 | map: 123 | aft_mapped: 124 | {} 125 | odom_final: 126 | {} 127 | Update Interval: 0 128 | Value: true 129 | - Alpha: 1 130 | Autocompute Intensity Bounds: true 131 | Autocompute Value Bounds: 132 | Max Value: 10 133 | Min Value: -10 134 | Value: true 135 | Axis: Z 136 | Channel Name: intensity 137 | Class: rviz/PointCloud2 138 | Color: 255; 255; 255 139 | Color Transformer: Intensity 140 | Decay Time: 0 141 | Enabled: true 142 | Invert Rainbow: false 143 | Max Color: 255; 255; 255 144 | Max Intensity: 0.9900000095367432 145 | Min Color: 0; 0; 0 146 | Min Intensity: 0 147 | Name: raw_data 148 | Position Transformer: XYZ 149 | Queue Size: 10 150 | Selectable: true 151 | Size (Pixels): 1 152 | Size (m): 0.009999999776482582 153 | Style: Points 154 | Topic: /velodyne_points 155 | Unreliable: false 156 | Use Fixed Frame: true 157 | Use rainbow: true 158 | Value: true 159 | - Alpha: 1 160 | Buffer Length: 1 161 | Class: rviz/Path 162 | Color: 239; 41; 41 163 | Enabled: true 164 | Head Diameter: 0.30000001192092896 165 | Head Length: 0.20000000298023224 166 | Length: 0.30000001192092896 167 | Line Style: Lines 168 | Line Width: 0.029999999329447746 169 | Name: ground_truth 170 | Offset: 171 | X: 0 172 | Y: 0 173 | Z: 0 174 | Pose Color: 255; 85; 255 175 | Pose Style: None 176 | Radius: 0.029999999329447746 177 | Shaft Diameter: 0.10000000149011612 178 | Shaft Length: 0.10000000149011612 179 | Topic: /gt/trajectory 180 | Unreliable: false 181 | Value: true 182 | - Alpha: 1 183 | Buffer Length: 1 184 | Class: rviz/Path 185 | Color: 245; 121; 0 186 | Enabled: false 187 | Head Diameter: 0.30000001192092896 188 | Head Length: 0.20000000298023224 189 | Length: 0.30000001192092896 190 | Line Style: Lines 191 | Line Width: 0.029999999329447746 192 | Name: Odom 193 | Offset: 194 | X: 0 195 | Y: 0 196 | Z: 0 197 | Pose Color: 255; 85; 255 198 | Pose Style: None 199 | Radius: 0.029999999329447746 200 | Shaft Diameter: 0.10000000149011612 201 | Shaft Length: 0.10000000149011612 202 | Topic: /odom_final/trajectory 203 | Unreliable: false 204 | Value: false 205 | - Alpha: 1 206 | Buffer Length: 1 207 | Class: rviz/Path 208 | Color: 25; 255; 0 209 | Enabled: true 210 | Head Diameter: 0.30000001192092896 211 | Head Length: 0.20000000298023224 212 | Length: 0.30000001192092896 213 | Line Style: Lines 214 | Line Width: 0.029999999329447746 215 | Name: iscloam_result 216 | Offset: 217 | X: 0 218 | Y: 0 219 | Z: 0 220 | Pose Color: 255; 85; 255 221 | Pose Style: None 222 | Radius: 0.029999999329447746 223 | Shaft Diameter: 0.10000000149011612 224 | Shaft Length: 0.10000000149011612 225 | Topic: /final_path 226 | Unreliable: false 227 | Value: true 228 | Enabled: true 229 | Global Options: 230 | Background Color: 0; 0; 0 231 | Default Light: true 232 | Fixed Frame: world 233 | Frame Rate: 30 234 | Name: root 235 | Tools: 236 | - Class: rviz/Interact 237 | Hide Inactive Objects: true 238 | - Class: rviz/MoveCamera 239 | - Class: rviz/Select 240 | - Class: rviz/FocusCamera 241 | - Class: rviz/Measure 242 | - Class: rviz/SetInitialPose 243 | Theta std deviation: 0.2617993950843811 244 | Topic: /initialpose 245 | X std deviation: 0.5 246 | Y std deviation: 0.5 247 | - Class: rviz/SetGoal 248 | Topic: /move_base_simple/goal 249 | - Class: rviz/PublishPoint 250 | Single click: true 251 | Topic: /clicked_point 252 | Value: true 253 | Views: 254 | Current: 255 | Class: rviz/XYOrbit 256 | Distance: 166.35633850097656 257 | Enable Stereo Rendering: 258 | Stereo Eye Separation: 0.05999999865889549 259 | Stereo Focal Distance: 1 260 | Swap Stereo Eyes: false 261 | Value: false 262 | Focal Point: 263 | X: 30.172157287597656 264 | Y: -19.642745971679688 265 | Z: 0 266 | Focal Shape Fixed Size: true 267 | Focal Shape Size: 0.05000000074505806 268 | Invert Z Axis: false 269 | Name: Current View 270 | Near Clip Distance: 0.009999999776482582 271 | Pitch: 1.0253976583480835 272 | Target Frame: body 273 | Value: XYOrbit (rviz) 274 | Yaw: 3.0853984355926514 275 | Saved: ~ 276 | Window Geometry: 277 | Camera_View: 278 | collapsed: false 279 | Displays: 280 | collapsed: false 281 | Height: 1063 282 | Hide Left Dock: false 283 | Hide Right Dock: false 284 | ISC_Image: 285 | collapsed: false 286 | QMainWindow State: 000000ff00000000fd0000000400000000000001ef0000038efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000231000000c900fffffffb00000012004900530043005f0049006d00610067006500000001dc000000ef0000001600fffffffb0000001600430061006d006500720061005f00560069006500770100000274000001570000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000600490053004300000002d9000000c70000000000000000000000010000010f0000038efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000038e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000083d00000039fc0100000002fb0000000800540069006d006501000000000000083d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005330000038e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 287 | Selection: 288 | collapsed: false 289 | Time: 290 | collapsed: false 291 | Tool Properties: 292 | collapsed: false 293 | Views: 294 | collapsed: false 295 | Width: 2109 296 | X: 539 297 | Y: 27 298 | -------------------------------------------------------------------------------- /rviz/iscloam_mapping.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1 9 | - /TF1/Frames1 10 | - /map1 11 | Splitter Ratio: 0.5 12 | Tree Height: 435 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 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: Camera_View 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 50 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 15 55 | Reference Frame: 56 | Value: true 57 | - Class: rviz/Image 58 | Enabled: true 59 | Image Topic: /cam03/image_raw 60 | Max Value: 1 61 | Median window: 5 62 | Min Value: 0 63 | Name: Camera_View 64 | Normalize Range: true 65 | Queue Size: 2 66 | Transport Hint: raw 67 | Unreliable: false 68 | Value: true 69 | - Class: rviz/Image 70 | Enabled: false 71 | Image Topic: /isc 72 | Max Value: 1 73 | Median window: 5 74 | Min Value: 0 75 | Name: ISC_Image 76 | Normalize Range: true 77 | Queue Size: 2 78 | Transport Hint: raw 79 | Unreliable: false 80 | Value: false 81 | - Class: rviz/TF 82 | Enabled: true 83 | Frame Timeout: 15 84 | Frames: 85 | All Enabled: false 86 | aft_mapped: 87 | Value: false 88 | cam00: 89 | Value: false 90 | cam01: 91 | Value: false 92 | cam02: 93 | Value: false 94 | cam03: 95 | Value: false 96 | imu: 97 | Value: false 98 | map: 99 | Value: false 100 | odom_final: 101 | Value: true 102 | velodyne: 103 | Value: false 104 | world: 105 | Value: true 106 | Marker Scale: 100 107 | Name: TF 108 | Show Arrows: false 109 | Show Axes: true 110 | Show Names: true 111 | Tree: 112 | world: 113 | imu: 114 | cam00: 115 | {} 116 | cam01: 117 | {} 118 | cam02: 119 | {} 120 | cam03: 121 | {} 122 | velodyne: 123 | {} 124 | map: 125 | aft_mapped: 126 | {} 127 | odom_final: 128 | {} 129 | Update Interval: 0 130 | Value: true 131 | - Alpha: 1 132 | Autocompute Intensity Bounds: true 133 | Autocompute Value Bounds: 134 | Max Value: 10 135 | Min Value: -10 136 | Value: true 137 | Axis: Z 138 | Channel Name: intensity 139 | Class: rviz/PointCloud2 140 | Color: 255; 255; 255 141 | Color Transformer: Intensity 142 | Decay Time: 0 143 | Enabled: false 144 | Invert Rainbow: false 145 | Max Color: 255; 255; 255 146 | Max Intensity: 0.9900000095367432 147 | Min Color: 0; 0; 0 148 | Min Intensity: 0 149 | Name: raw_data 150 | Position Transformer: XYZ 151 | Queue Size: 10 152 | Selectable: true 153 | Size (Pixels): 1 154 | Size (m): 0.009999999776482582 155 | Style: Points 156 | Topic: /velodyne_points 157 | Unreliable: false 158 | Use Fixed Frame: true 159 | Use rainbow: true 160 | Value: false 161 | - Alpha: 1 162 | Buffer Length: 1 163 | Class: rviz/Path 164 | Color: 239; 41; 41 165 | Enabled: true 166 | Head Diameter: 0.30000001192092896 167 | Head Length: 0.20000000298023224 168 | Length: 0.30000001192092896 169 | Line Style: Lines 170 | Line Width: 0.029999999329447746 171 | Name: ground_truth 172 | Offset: 173 | X: 0 174 | Y: 0 175 | Z: 0 176 | Pose Color: 255; 85; 255 177 | Pose Style: None 178 | Radius: 0.029999999329447746 179 | Shaft Diameter: 0.10000000149011612 180 | Shaft Length: 0.10000000149011612 181 | Topic: /gt/trajectory 182 | Unreliable: false 183 | Value: true 184 | - Alpha: 1 185 | Buffer Length: 1 186 | Class: rviz/Path 187 | Color: 245; 121; 0 188 | Enabled: false 189 | Head Diameter: 0.30000001192092896 190 | Head Length: 0.20000000298023224 191 | Length: 0.30000001192092896 192 | Line Style: Lines 193 | Line Width: 0.029999999329447746 194 | Name: Odom 195 | Offset: 196 | X: 0 197 | Y: 0 198 | Z: 0 199 | Pose Color: 255; 85; 255 200 | Pose Style: None 201 | Radius: 0.029999999329447746 202 | Shaft Diameter: 0.10000000149011612 203 | Shaft Length: 0.10000000149011612 204 | Topic: /odom_final/trajectory 205 | Unreliable: false 206 | Value: false 207 | - Alpha: 1 208 | Buffer Length: 1 209 | Class: rviz/Path 210 | Color: 25; 255; 0 211 | Enabled: true 212 | Head Diameter: 0.30000001192092896 213 | Head Length: 0.20000000298023224 214 | Length: 0.30000001192092896 215 | Line Style: Lines 216 | Line Width: 0.029999999329447746 217 | Name: iscloam_result 218 | Offset: 219 | X: 0 220 | Y: 0 221 | Z: 0 222 | Pose Color: 255; 85; 255 223 | Pose Style: None 224 | Radius: 0.029999999329447746 225 | Shaft Diameter: 0.10000000149011612 226 | Shaft Length: 0.10000000149011612 227 | Topic: /final_path 228 | Unreliable: false 229 | Value: true 230 | - Alpha: 0.699999988079071 231 | Autocompute Intensity Bounds: true 232 | Autocompute Value Bounds: 233 | Max Value: 11.07189655303955 234 | Min Value: -20.349206924438477 235 | Value: true 236 | Axis: Z 237 | Channel Name: intensity 238 | Class: rviz/PointCloud2 239 | Color: 255; 255; 255 240 | Color Transformer: AxisColor 241 | Decay Time: 0 242 | Enabled: true 243 | Invert Rainbow: false 244 | Max Color: 0; 0; 0 245 | Max Intensity: 0.9900000095367432 246 | Min Color: 238; 238; 236 247 | Min Intensity: 0 248 | Name: map 249 | Position Transformer: XYZ 250 | Queue Size: 10 251 | Selectable: true 252 | Size (Pixels): 5 253 | Size (m): 0.699999988079071 254 | Style: Boxes 255 | Topic: /map 256 | Unreliable: false 257 | Use Fixed Frame: true 258 | Use rainbow: true 259 | Value: true 260 | Enabled: true 261 | Global Options: 262 | Background Color: 136; 138; 133 263 | Default Light: true 264 | Fixed Frame: world 265 | Frame Rate: 30 266 | Name: root 267 | Tools: 268 | - Class: rviz/Interact 269 | Hide Inactive Objects: true 270 | - Class: rviz/MoveCamera 271 | - Class: rviz/Select 272 | - Class: rviz/FocusCamera 273 | - Class: rviz/Measure 274 | - Class: rviz/SetInitialPose 275 | Theta std deviation: 0.2617993950843811 276 | Topic: /initialpose 277 | X std deviation: 0.5 278 | Y std deviation: 0.5 279 | - Class: rviz/SetGoal 280 | Topic: /move_base_simple/goal 281 | - Class: rviz/PublishPoint 282 | Single click: true 283 | Topic: /clicked_point 284 | Value: true 285 | Views: 286 | Current: 287 | Angle: 4.700019359588623 288 | Class: rviz/TopDownOrtho 289 | Enable Stereo Rendering: 290 | Stereo Eye Separation: 0.05999999865889549 291 | Stereo Focal Distance: 1 292 | Swap Stereo Eyes: false 293 | Value: false 294 | Invert Z Axis: false 295 | Name: Current View 296 | Near Clip Distance: 0.009999999776482582 297 | Scale: 1.6136374473571777 298 | Target Frame: 299 | Value: TopDownOrtho (rviz) 300 | X: 109.91279602050781 301 | Y: -27.7003116607666 302 | Saved: ~ 303 | Window Geometry: 304 | Camera_View: 305 | collapsed: false 306 | Displays: 307 | collapsed: false 308 | Height: 956 309 | Hide Left Dock: false 310 | Hide Right Dock: false 311 | ISC_Image: 312 | collapsed: false 313 | QMainWindow State: 000000ff00000000fd0000000400000000000001ef00000323fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000236000000c900fffffffb00000012004900530043005f0049006d00610067006500000001ee000000be0000001600fffffffb0000001600430061006d006500720061005f00560069006500770100000279000000e70000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000600490053004300000002d9000000c70000000000000000000000010000010f00000323fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000323000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000083d00000039fc0100000002fb0000000800540069006d006501000000000000083d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005330000032300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 314 | Selection: 315 | collapsed: false 316 | Time: 317 | collapsed: false 318 | Tool Properties: 319 | collapsed: false 320 | Views: 321 | collapsed: false 322 | Width: 2109 323 | X: 539 324 | Y: 27 325 | -------------------------------------------------------------------------------- /rviz/iscloam_velodyne.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1 9 | - /TF1/Frames1 10 | - /map1 11 | Splitter Ratio: 0.5 12 | Tree Height: 435 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 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: Camera_View 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 50 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 15 55 | Reference Frame: 56 | Value: true 57 | - Class: rviz/Image 58 | Enabled: true 59 | Image Topic: /cam03/image_raw 60 | Max Value: 1 61 | Median window: 5 62 | Min Value: 0 63 | Name: Camera_View 64 | Normalize Range: true 65 | Queue Size: 2 66 | Transport Hint: raw 67 | Unreliable: false 68 | Value: true 69 | - Class: rviz/Image 70 | Enabled: false 71 | Image Topic: /isc 72 | Max Value: 1 73 | Median window: 5 74 | Min Value: 0 75 | Name: ISC_Image 76 | Normalize Range: true 77 | Queue Size: 2 78 | Transport Hint: raw 79 | Unreliable: false 80 | Value: false 81 | - Class: rviz/TF 82 | Enabled: true 83 | Frame Timeout: 15 84 | Frames: 85 | All Enabled: false 86 | aft_mapped: 87 | Value: false 88 | cam00: 89 | Value: false 90 | cam01: 91 | Value: false 92 | cam02: 93 | Value: false 94 | cam03: 95 | Value: false 96 | imu: 97 | Value: false 98 | map: 99 | Value: false 100 | odom_final: 101 | Value: true 102 | velodyne: 103 | Value: false 104 | world: 105 | Value: true 106 | Marker Scale: 100 107 | Name: TF 108 | Show Arrows: false 109 | Show Axes: true 110 | Show Names: true 111 | Tree: 112 | world: 113 | imu: 114 | cam00: 115 | {} 116 | cam01: 117 | {} 118 | cam02: 119 | {} 120 | cam03: 121 | {} 122 | velodyne: 123 | {} 124 | map: 125 | aft_mapped: 126 | {} 127 | odom_final: 128 | {} 129 | Update Interval: 0 130 | Value: true 131 | - Alpha: 1 132 | Autocompute Intensity Bounds: true 133 | Autocompute Value Bounds: 134 | Max Value: 10 135 | Min Value: -10 136 | Value: true 137 | Axis: Z 138 | Channel Name: intensity 139 | Class: rviz/PointCloud2 140 | Color: 255; 255; 255 141 | Color Transformer: Intensity 142 | Decay Time: 0 143 | Enabled: false 144 | Invert Rainbow: false 145 | Max Color: 255; 255; 255 146 | Max Intensity: 0.9900000095367432 147 | Min Color: 0; 0; 0 148 | Min Intensity: 0 149 | Name: raw_data 150 | Position Transformer: XYZ 151 | Queue Size: 10 152 | Selectable: true 153 | Size (Pixels): 1 154 | Size (m): 0.009999999776482582 155 | Style: Points 156 | Topic: /velodyne_points 157 | Unreliable: false 158 | Use Fixed Frame: true 159 | Use rainbow: true 160 | Value: false 161 | - Alpha: 1 162 | Buffer Length: 1 163 | Class: rviz/Path 164 | Color: 239; 41; 41 165 | Enabled: true 166 | Head Diameter: 0.30000001192092896 167 | Head Length: 0.20000000298023224 168 | Length: 0.30000001192092896 169 | Line Style: Lines 170 | Line Width: 0.029999999329447746 171 | Name: ground_truth 172 | Offset: 173 | X: 0 174 | Y: 0 175 | Z: 0 176 | Pose Color: 255; 85; 255 177 | Pose Style: None 178 | Radius: 0.029999999329447746 179 | Shaft Diameter: 0.10000000149011612 180 | Shaft Length: 0.10000000149011612 181 | Topic: /gt/trajectory 182 | Unreliable: false 183 | Value: true 184 | - Alpha: 1 185 | Buffer Length: 1 186 | Class: rviz/Path 187 | Color: 245; 121; 0 188 | Enabled: false 189 | Head Diameter: 0.30000001192092896 190 | Head Length: 0.20000000298023224 191 | Length: 0.30000001192092896 192 | Line Style: Lines 193 | Line Width: 0.029999999329447746 194 | Name: Odom 195 | Offset: 196 | X: 0 197 | Y: 0 198 | Z: 0 199 | Pose Color: 255; 85; 255 200 | Pose Style: None 201 | Radius: 0.029999999329447746 202 | Shaft Diameter: 0.10000000149011612 203 | Shaft Length: 0.10000000149011612 204 | Topic: /odom_final/trajectory 205 | Unreliable: false 206 | Value: false 207 | - Alpha: 1 208 | Buffer Length: 1 209 | Class: rviz/Path 210 | Color: 25; 255; 0 211 | Enabled: true 212 | Head Diameter: 0.30000001192092896 213 | Head Length: 0.20000000298023224 214 | Length: 0.30000001192092896 215 | Line Style: Lines 216 | Line Width: 0.029999999329447746 217 | Name: iscloam_result 218 | Offset: 219 | X: 0 220 | Y: 0 221 | Z: 0 222 | Pose Color: 255; 85; 255 223 | Pose Style: None 224 | Radius: 0.029999999329447746 225 | Shaft Diameter: 0.10000000149011612 226 | Shaft Length: 0.10000000149011612 227 | Topic: /final_path 228 | Unreliable: false 229 | Value: true 230 | - Alpha: 0.699999988079071 231 | Autocompute Intensity Bounds: true 232 | Autocompute Value Bounds: 233 | Max Value: 11.07189655303955 234 | Min Value: -20.349206924438477 235 | Value: true 236 | Axis: Z 237 | Channel Name: intensity 238 | Class: rviz/PointCloud2 239 | Color: 255; 255; 255 240 | Color Transformer: AxisColor 241 | Decay Time: 0 242 | Enabled: true 243 | Invert Rainbow: false 244 | Max Color: 0; 0; 0 245 | Max Intensity: 0.9900000095367432 246 | Min Color: 238; 238; 236 247 | Min Intensity: 0 248 | Name: map 249 | Position Transformer: XYZ 250 | Queue Size: 10 251 | Selectable: true 252 | Size (Pixels): 5 253 | Size (m): 0.699999988079071 254 | Style: Boxes 255 | Topic: /map 256 | Unreliable: false 257 | Use Fixed Frame: true 258 | Use rainbow: true 259 | Value: true 260 | Enabled: true 261 | Global Options: 262 | Background Color: 136; 138; 133 263 | Default Light: true 264 | Fixed Frame: world 265 | Frame Rate: 30 266 | Name: root 267 | Tools: 268 | - Class: rviz/Interact 269 | Hide Inactive Objects: true 270 | - Class: rviz/MoveCamera 271 | - Class: rviz/Select 272 | - Class: rviz/FocusCamera 273 | - Class: rviz/Measure 274 | - Class: rviz/SetInitialPose 275 | Theta std deviation: 0.2617993950843811 276 | Topic: /initialpose 277 | X std deviation: 0.5 278 | Y std deviation: 0.5 279 | - Class: rviz/SetGoal 280 | Topic: /move_base_simple/goal 281 | - Class: rviz/PublishPoint 282 | Single click: true 283 | Topic: /clicked_point 284 | Value: true 285 | Views: 286 | Current: 287 | Angle: 4.700019359588623 288 | Class: rviz/TopDownOrtho 289 | Enable Stereo Rendering: 290 | Stereo Eye Separation: 0.05999999865889549 291 | Stereo Focal Distance: 1 292 | Swap Stereo Eyes: false 293 | Value: false 294 | Invert Z Axis: false 295 | Name: Current View 296 | Near Clip Distance: 0.009999999776482582 297 | Scale: 1.6136374473571777 298 | Target Frame: 299 | Value: TopDownOrtho (rviz) 300 | X: 109.91279602050781 301 | Y: -27.7003116607666 302 | Saved: ~ 303 | Window Geometry: 304 | Camera_View: 305 | collapsed: false 306 | Displays: 307 | collapsed: false 308 | Height: 956 309 | Hide Left Dock: false 310 | Hide Right Dock: false 311 | ISC_Image: 312 | collapsed: false 313 | QMainWindow State: 000000ff00000000fd0000000400000000000001ef00000323fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000236000000c900fffffffb00000012004900530043005f0049006d00610067006500000001ee000000be0000001600fffffffb0000001600430061006d006500720061005f00560069006500770100000279000000e70000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000600490053004300000002d9000000c70000000000000000000000010000010f00000323fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000323000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000083d00000039fc0100000002fb0000000800540069006d006501000000000000083d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005330000032300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 314 | Selection: 315 | collapsed: false 316 | Time: 317 | collapsed: false 318 | Tool Properties: 319 | collapsed: false 320 | Views: 321 | collapsed: false 322 | Width: 2109 323 | X: 539 324 | Y: 27 325 | -------------------------------------------------------------------------------- /src/iscGenerationClass.cpp: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #include "iscGenerationClass.h" 6 | //#define INTEGER_INTENSITY 7 | ISCGenerationClass::ISCGenerationClass() 8 | { 9 | 10 | } 11 | 12 | void ISCGenerationClass::init_param(int rings_in, int sectors_in, double max_dis_in){ 13 | rings = rings_in; 14 | sectors = sectors_in; 15 | max_dis = max_dis_in; 16 | ring_step = max_dis/rings; 17 | sector_step = 2*M_PI/sectors; 18 | print_param(); 19 | init_color(); 20 | 21 | current_point_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud()); 22 | } 23 | 24 | void ISCGenerationClass::init_color(void){ 25 | for(int i=0;i<1;i++){//RGB format 26 | color_projection.push_back(cv::Vec3b(0,i*16,255)); 27 | } 28 | for(int i=0;i<15;i++){//RGB format 29 | color_projection.push_back(cv::Vec3b(0,i*16,255)); 30 | } 31 | for(int i=0;i<16;i++){//RGB format 32 | color_projection.push_back(cv::Vec3b(0,255,255-i*16)); 33 | } 34 | for(int i=0;i<32;i++){//RGB format 35 | color_projection.push_back(cv::Vec3b(i*32,255,0)); 36 | } 37 | for(int i=0;i<16;i++){//RGB format 38 | color_projection.push_back(cv::Vec3b(255,255-i*16,0)); 39 | } 40 | for(int i=0;i<64;i++){//RGB format 41 | color_projection.push_back(cv::Vec3b(i*4,255,0)); 42 | } 43 | for(int i=0;i<64;i++){//RGB format 44 | color_projection.push_back(cv::Vec3b(255,255-i*4,0)); 45 | } 46 | for(int i=0;i<64;i++){//RGB format 47 | color_projection.push_back(cv::Vec3b(255,i*4,i*4)); 48 | } 49 | } 50 | 51 | void ISCGenerationClass::print_param(){ 52 | std::cout << "The ISC parameters are:"<::Ptr filtered_pointcloud){ 59 | ISCDescriptor isc = cv::Mat::zeros(cv::Size(sectors,rings), CV_8U); 60 | 61 | for(int i=0;i<(int)filtered_pointcloud->points.size();i++){ 62 | ROS_WARN_ONCE("intensity is %f, if intensity showed here is integer format between 1-255, please uncomment #define INTEGER_INTENSITY in iscGenerationClass.cpp and recompile", (double) filtered_pointcloud->points[i].intensity); 63 | double distance = std::sqrt(filtered_pointcloud->points[i].x * filtered_pointcloud->points[i].x + filtered_pointcloud->points[i].y * filtered_pointcloud->points[i].y); 64 | if(distance>=max_dis) 65 | continue; 66 | double angle = M_PI + std::atan2(filtered_pointcloud->points[i].y,filtered_pointcloud->points[i].x); 67 | int ring_id = std::floor(distance/ring_step); 68 | int sector_id = std::floor(angle/sector_step); 69 | if(ring_id>=rings) 70 | continue; 71 | if(sector_id>=sectors) 72 | continue; 73 | #ifndef INTEGER_INTENSITY 74 | int intensity_temp = (int) (255*filtered_pointcloud->points[i].intensity); 75 | #else 76 | int intensity_temp = (int) (filtered_pointcloud->points[i].intensity); 77 | #endif 78 | if(isc.at(ring_id,sector_id)(ring_id,sector_id)=intensity_temp; 80 | 81 | } 82 | 83 | return isc; 84 | 85 | } 86 | 87 | 88 | ISCDescriptor ISCGenerationClass::getLastISCMONO(void){ 89 | return isc_arr.back(); 90 | 91 | } 92 | 93 | ISCDescriptor ISCGenerationClass::getLastISCRGB(void){ 94 | //ISCDescriptor isc = isc_arr.back(); 95 | ISCDescriptor isc_color = cv::Mat::zeros(cv::Size(sectors,rings), CV_8UC3); 96 | for (int i = 0;i < isc_arr.back().rows;i++) { 97 | for (int j = 0;j < isc_arr.back().cols;j++) { 98 | isc_color.at(i, j) = color_projection[isc_arr.back().at(i,j)]; 99 | 100 | } 101 | } 102 | return isc_color; 103 | } 104 | 105 | void ISCGenerationClass::loopDetection(const pcl::PointCloud::Ptr& current_pc, Eigen::Isometry3d& odom){ 106 | 107 | pcl::PointCloud::Ptr pc_filtered(new pcl::PointCloud()); 108 | ground_filter(current_pc, pc_filtered); 109 | ISCDescriptor desc = calculate_isc(pc_filtered); 110 | Eigen::Vector3d current_t = odom.translation(); 111 | //dont change push_back sequence 112 | if(travel_distance_arr.size()==0){ 113 | travel_distance_arr.push_back(0); 114 | }else{ 115 | double dis_temp = travel_distance_arr.back()+std::sqrt((pos_arr.back()-current_t).array().square().sum()); 116 | travel_distance_arr.push_back(dis_temp); 117 | } 118 | pos_arr.push_back(current_t); 119 | isc_arr.push_back(desc); 120 | 121 | current_frame_id = pos_arr.size()-1; 122 | matched_frame_id.clear(); 123 | //search for the near neibourgh pos 124 | int best_matched_id=0; 125 | double best_score=0.0; 126 | for(int i = 0; i< (int)pos_arr.size(); i++){ 127 | double delta_travel_distance = travel_distance_arr.back()- travel_distance_arr[i]; 128 | double pos_distance = std::sqrt((pos_arr[i]-pos_arr.back()).array().square().sum()); 129 | if(delta_travel_distance > SKIP_NEIBOUR_DISTANCE && pos_distancebest_score){ 134 | best_score = geo_score+inten_score; 135 | best_matched_id = i; 136 | } 137 | } 138 | 139 | } 140 | } 141 | if(best_matched_id!=0){ 142 | matched_frame_id.push_back(best_matched_id); 143 | //ROS_INFO("received loop closure candidate: current: %d, history %d, total_score%f",current_frame_id,best_matched_id,best_score); 144 | } 145 | 146 | 147 | } 148 | 149 | bool ISCGenerationClass::is_loop_pair(ISCDescriptor& desc1, ISCDescriptor& desc2, double& geo_score, double& inten_score){ 150 | int angle =0; 151 | geo_score = calculate_geometry_dis(desc1,desc2,angle); 152 | if(geo_score>GEOMETRY_THRESHOLD){ 153 | inten_score = calculate_intensity_dis(desc1,desc2,angle); 154 | if(inten_score>INTENSITY_THRESHOLD){ 155 | return true; 156 | } 157 | } 158 | return false; 159 | } 160 | 161 | double ISCGenerationClass::calculate_geometry_dis(const ISCDescriptor& desc1, const ISCDescriptor& desc2, int& angle){ 162 | double similarity = 0.0; 163 | 164 | for(int i=0;i=sectors?p+i-sectors:p+i; 168 | for(int q=0;q(q,p)== true && desc2.at(q,new_col)== true) || (desc1.at(q,p)== false && desc2.at(q,new_col)== false)){ 170 | match_count++; 171 | } 172 | 173 | } 174 | } 175 | if(match_count>similarity){ 176 | similarity=match_count; 177 | angle = i; 178 | } 179 | 180 | } 181 | return similarity/(sectors*rings); 182 | 183 | } 184 | double ISCGenerationClass::calculate_intensity_dis(const ISCDescriptor& desc1, const ISCDescriptor& desc2, int& angle){ 185 | double difference = 1.0; 186 | double angle_temp = angle; 187 | for(int i=angle_temp-10;i=sectors) 194 | new_col = new_col-sectors; 195 | if(new_col<0) 196 | new_col = new_col+sectors; 197 | for(int q=0;q(q,p)-desc2.at(q,new_col)); 199 | total_points++; 200 | } 201 | 202 | } 203 | double diff_temp = ((double)match_count)/(sectors*rings*255); 204 | if(diff_temp::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out){ 212 | pcl::PassThrough pass; 213 | pass.setInputCloud (pc_in); 214 | pass.setFilterFieldName ("z"); 215 | pass.setFilterLimits (-0.9, 30.0); 216 | pass.filter (*pc_out); 217 | 218 | } -------------------------------------------------------------------------------- /src/iscGenerationNode.cpp: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | //std lib 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | //ros 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | //PCL 21 | #include 22 | #include 23 | #include 24 | 25 | //opencv 26 | #include 27 | #include 28 | #include 29 | 30 | //local library 31 | #include "iscGenerationClass.h" 32 | #include 33 | 34 | ros::Publisher isc_pub; 35 | ros::Publisher loop_info_pub; 36 | 37 | 38 | std::queue pointCloudBuf; 39 | std::queue odometryBuf; 40 | std::mutex mutex_lock; 41 | 42 | ISCGenerationClass iscGeneration; 43 | 44 | double scan_period= 0.1; 45 | 46 | //receive odomtry 47 | void odomCallback(const nav_msgs::Odometry::ConstPtr &msg) 48 | { 49 | mutex_lock.lock(); 50 | odometryBuf.push(msg); 51 | mutex_lock.unlock(); 52 | 53 | } 54 | 55 | //receive all point cloud 56 | void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &msg) 57 | { 58 | mutex_lock.lock(); 59 | pointCloudBuf.push(msg); 60 | mutex_lock.unlock(); 61 | } 62 | 63 | void loop_closure_detection(){ 64 | while(1){ 65 | if(!pointCloudBuf.empty() && !odometryBuf.empty()){ 66 | //align time stamp 67 | mutex_lock.lock(); 68 | if(!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < pointCloudBuf.front()->header.stamp.toSec() - 0.5 * scan_period){ 69 | ROS_WARN("isc_generation: time stamp unaligned error and odom discarded, pls check your data; odom time %f, pc time %f",odometryBuf.front()->header.stamp.toSec(),pointCloudBuf.front()->header.stamp.toSec()); 70 | odometryBuf.pop(); 71 | mutex_lock.unlock(); 72 | continue; 73 | } 74 | 75 | if(!pointCloudBuf.empty() && pointCloudBuf.front()->header.stamp.toSec() < odometryBuf.front()->header.stamp.toSec() - 0.5 * scan_period){ 76 | ROS_WARN("isc_generation: time stamp unaligned error and odom discarded, pls check your data; odom time %f, pc time %f",odometryBuf.front()->header.stamp.toSec(),pointCloudBuf.front()->header.stamp.toSec()); 77 | pointCloudBuf.pop(); 78 | mutex_lock.unlock(); 79 | continue; 80 | } 81 | 82 | pcl::PointCloud::Ptr pointcloud_in(new pcl::PointCloud()); 83 | pcl::fromROSMsg(*pointCloudBuf.front(), *pointcloud_in); 84 | ros::Time pointcloud_time = (pointCloudBuf.front())->header.stamp; 85 | Eigen::Isometry3d odom_in = Eigen::Isometry3d::Identity(); 86 | odom_in.rotate(Eigen::Quaterniond(odometryBuf.front()->pose.pose.orientation.w,odometryBuf.front()->pose.pose.orientation.x,odometryBuf.front()->pose.pose.orientation.y,odometryBuf.front()->pose.pose.orientation.z)); 87 | odom_in.pretranslate(Eigen::Vector3d(odometryBuf.front()->pose.pose.position.x,odometryBuf.front()->pose.pose.position.y,odometryBuf.front()->pose.pose.position.z)); 88 | odometryBuf.pop(); 89 | pointCloudBuf.pop(); 90 | mutex_lock.unlock(); 91 | 92 | 93 | iscGeneration.loopDetection(pointcloud_in, odom_in); 94 | 95 | cv_bridge::CvImage out_msg; 96 | out_msg.header.frame_id = "velodyne"; 97 | out_msg.header.stamp = pointcloud_time; 98 | out_msg.encoding = sensor_msgs::image_encodings::RGB8; 99 | out_msg.image = iscGeneration.getLastISCRGB(); 100 | isc_pub.publish(out_msg.toImageMsg()); 101 | 102 | iscloam::LoopInfo loop; 103 | loop.header.stamp = pointcloud_time; 104 | loop.header.frame_id = "velodyne"; 105 | loop.current_id = iscGeneration.current_frame_id; 106 | for(int i=0;i<(int)iscGeneration.matched_frame_id.size();i++){ 107 | loop.matched_id.push_back(iscGeneration.matched_frame_id[i]); 108 | } 109 | loop_info_pub.publish(loop); 110 | 111 | } 112 | //sleep 2 ms every time 113 | std::chrono::milliseconds dura(2); 114 | std::this_thread::sleep_for(dura); 115 | }//end of while(1) 116 | } 117 | 118 | 119 | 120 | int main(int argc, char **argv) 121 | { 122 | 123 | ros::init(argc, argv, "isc_gen"); 124 | 125 | ros::NodeHandle nh("~"); 126 | 127 | //rosnode init 128 | ros::Subscriber velodyne_sub= nh.subscribe("/velodyne_points_filtered", 10, pointCloudCallback); 129 | ros::Subscriber odom_sub= nh.subscribe("/odom", 10, odomCallback); 130 | 131 | loop_info_pub = nh.advertise("/loop_closure", 100); 132 | isc_pub = nh.advertise("/isc", 100); 133 | 134 | 135 | //read parameter 136 | int sector_width = 60; 137 | int ring_height = 60; 138 | double max_dis= 40.0; 139 | 140 | nh.getParam("/sector_width", sector_width); 141 | nh.getParam("/ring_height", ring_height); 142 | //nh.getParam("/max_dis", max_dis); 143 | nh.getParam("/scan_period", scan_period); 144 | 145 | //init ISC 146 | iscGeneration.init_param(ring_height,sector_width,max_dis); 147 | 148 | //open thread 149 | std::thread loop_closure_detection_process{loop_closure_detection}; 150 | 151 | ros::spin(); 152 | 153 | return 0; 154 | } 155 | -------------------------------------------------------------------------------- /src/iscOptimizationClass.cpp: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #include "iscOptimizationClass.h" 6 | 7 | ISCOptimizationClass::ISCOptimizationClass() 8 | { 9 | 10 | } 11 | 12 | void ISCOptimizationClass::init(void){ 13 | 14 | pointcloud_arr.clear(); 15 | 16 | // A prior factor consists of a mean value and a noise model (covariance matrix) 17 | priorModel = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6).finished()); 18 | 19 | // odometry measurement noise model (covariance matrix) 20 | odomModel = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.10, 0.10, 0.10, 0.10, 0.10, 0.10).finished()); 21 | 22 | //loop noise model 23 | loopModel = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.10, 0.10, 0.10, 0.10, 0.10, 0.10).finished()); 24 | //attention, here is x y z r p y order 25 | stop_check_loop_count=0; 26 | 27 | downSizeFilter.setLeafSize(0.8, 0.8, 0.8); 28 | } 29 | 30 | bool ISCOptimizationClass::addPoseToGraph(const pcl::PointCloud::Ptr& pointcloud_edge_in, const pcl::PointCloud::Ptr& pointcloud_surf_in, std::vector& matched_frame_id, Eigen::Isometry3d& odom_in){ 31 | 32 | //pointcloud_arr.push_back(pointcloud_in); 33 | pcl::VoxelGrid downSizeEdgeFilter; 34 | pcl::VoxelGrid downSizeSurfFilter; 35 | downSizeSurfFilter.setLeafSize(0.8, 0.8, 0.8); 36 | downSizeEdgeFilter.setLeafSize(0.4, 0.4, 0.4); 37 | downSizeEdgeFilter.setInputCloud(pointcloud_edge_in); 38 | downSizeSurfFilter.setInputCloud(pointcloud_surf_in); 39 | pcl::PointCloud::Ptr filtered_edge_in(new pcl::PointCloud()); 40 | pcl::PointCloud::Ptr filtered_surf_in(new pcl::PointCloud()); 41 | downSizeEdgeFilter.filter(*filtered_edge_in); 42 | downSizeSurfFilter.filter(*filtered_surf_in); 43 | pointcloud_edge_arr.push_back(filtered_edge_in); 44 | pointcloud_surf_arr.push_back(filtered_surf_in); 45 | 46 | //ROS_INFO("input pc size %d %d",(int)filtered_edge_in->points.size(),(int)filtered_surf_in->points.size()); 47 | gtsam::Pose3 pose3_current = eigenToPose3(odom_in); 48 | 49 | if(pointcloud_edge_arr.size()<=1){ 50 | //if first time 51 | pose_optimized_arr.push_back(pose3_current); 52 | graph.push_back(gtsam::PriorFactor(gtsam::Symbol('x', 1), pose_optimized_arr.front(), priorModel)); 53 | initials.insert(gtsam::Symbol('x', 1), pose_optimized_arr.back()); 54 | last_pose3 = pose3_current; 55 | return false; 56 | } 57 | 58 | odom_original_arr.push_back(last_pose3.between(pose3_current)); 59 | pose_optimized_arr.push_back(pose_optimized_arr.back() * odom_original_arr.back()); 60 | last_pose3 = pose3_current; 61 | initials.insert(gtsam::Symbol('x', pointcloud_edge_arr.size()), pose_optimized_arr.back()); 62 | graph.push_back(gtsam::BetweenFactor(gtsam::Symbol('x', pointcloud_edge_arr.size()-1), gtsam::Symbol('x', pointcloud_edge_arr.size()), odom_original_arr.back(), odomModel)); 63 | 64 | if(stop_check_loop_count>0){ 65 | stop_check_loop_count--; 66 | return false; 67 | } 68 | if(matched_frame_id.size()!=0){ 69 | //if loop closure detected 70 | for(int i=0;i<(int)matched_frame_id.size();i++){ 71 | //get initial guess 72 | gtsam::Pose3 transform_pose3 = pose_optimized_arr[matched_frame_id[i]].between(pose_optimized_arr.back()); 73 | //ROS_WARN("pose %f,%f,%f, [%f,%f,%f,%f]",transform_pose3.translation().x(),transform_pose3.translation().y(),transform_pose3.translation().z(),transform_pose3.rotation().toQuaternion().w(),transform_pose3.rotation().toQuaternion().x(),transform_pose3.rotation().toQuaternion().y(),transform_pose3.rotation().toQuaternion().z()); 74 | Eigen::Isometry3d transform = pose3ToEigen(pose_optimized_arr[matched_frame_id[i]].between(pose_optimized_arr.back())); 75 | //ROS_WARN("flag 1 %d,%d",matched_frame_id[i],pointcloud_edge_arr.size()); 76 | if(geometryConsistencyVerification(pointcloud_edge_arr.size()-1, matched_frame_id[i], transform)){ 77 | gtsam::Pose3 loop_temp = eigenToPose3(transform); 78 | graph.push_back(gtsam::BetweenFactor(gtsam::Symbol('x', matched_frame_id[i]+1), gtsam::Symbol('x', pointcloud_edge_arr.size()), loop_temp, loopModel)); 79 | gtsam::Values result = gtsam::LevenbergMarquardtOptimizer(graph, initials).optimize(); 80 | if(updateStates(result, matched_frame_id[i], pointcloud_edge_arr.size()-1)){ 81 | stop_check_loop_count=STOP_LOOP_CHECK_COUNTER; 82 | ROS_WARN("global optimization finished%d,%d with tranform %f,%f,%f, [%f,%f,%f,%f]",pointcloud_edge_arr.size()-1, matched_frame_id[i],loop_temp.translation().x(),loop_temp.translation().y(),loop_temp.translation().z(),loop_temp.rotation().toQuaternion().w(),loop_temp.rotation().toQuaternion().x(),loop_temp.rotation().toQuaternion().y(),loop_temp.rotation().toQuaternion().z()); 83 | 84 | return true; 85 | }else{ 86 | stop_check_loop_count=2; 87 | } 88 | 89 | }else{ 90 | stop_check_loop_count=2; 91 | } 92 | } 93 | } 94 | return false; 95 | } 96 | 97 | Eigen::Isometry3d ISCOptimizationClass::pose3ToEigen(const gtsam::Pose3& pose3){ 98 | Eigen::Isometry3d pose_eigen = Eigen::Isometry3d::Identity(); 99 | gtsam::Quaternion q_temp = pose3.rotation().toQuaternion(); 100 | pose_eigen.rotate(Eigen::Quaterniond(q_temp.w(),q_temp.x(),q_temp.y(),q_temp.z())); 101 | pose_eigen.pretranslate(Eigen::Vector3d(pose3.translation().x(),pose3.translation().y(),pose3.translation().z())); 102 | return pose_eigen; 103 | 104 | } 105 | 106 | gtsam::Pose3 ISCOptimizationClass::eigenToPose3(const Eigen::Isometry3d& pose_eigen){ 107 | Eigen::Quaterniond q(pose_eigen.rotation()); 108 | return gtsam::Pose3(gtsam::Rot3::Quaternion(q.w(), q.x(), q.y(), q.z()), gtsam::Point3(pose_eigen.translation().x(), pose_eigen.translation().y(), pose_eigen.translation().z())); 109 | } 110 | 111 | bool ISCOptimizationClass::updateStates(gtsam::Values& result, int matched_id, int current_id){ 112 | //verify states first 113 | double sum_residual_q = 0.0; 114 | double sum_residual_t = 0.0; 115 | int total_counter=0; 116 | for(int i =current_id-STOP_LOOP_CHECK_COUNTER-10;i(gtsam::Symbol('x',current_id+1)); 120 | gtsam::Pose3 pose_temp2= result.at(gtsam::Symbol('x',i+1)); 121 | gtsam::Pose3 tranform1 = pose_temp2.between(pose_temp1); 122 | gtsam::Pose3 tranform2 = pose_optimized_arr[i].between(pose_optimized_arr[current_id]); 123 | gtsam::Pose3 tranform = tranform1.between(tranform2); 124 | sum_residual_t += std::abs(tranform.translation().x())+std::abs(tranform.translation().y())+std::abs(tranform.translation().z()); 125 | sum_residual_q += std::abs(tranform.rotation().toQuaternion().w()-1)+std::abs(tranform.rotation().toQuaternion().x())+std::abs(tranform.rotation().toQuaternion().y())+std::abs(tranform.rotation().toQuaternion().z()); 126 | } 127 | for(int i =matched_id-STOP_LOOP_CHECK_COUNTER-10;i(gtsam::Symbol('x',matched_id+1)); 131 | gtsam::Pose3 pose_temp2= result.at(gtsam::Symbol('x',i+1)); 132 | gtsam::Pose3 tranform1 = pose_temp2.between(pose_temp1); 133 | gtsam::Pose3 tranform2 = pose_optimized_arr[i].between(pose_optimized_arr[matched_id]); 134 | gtsam::Pose3 tranform = tranform1.between(tranform2); 135 | sum_residual_t += std::abs(tranform.translation().x())+std::abs(tranform.translation().y())+std::abs(tranform.translation().z()); 136 | sum_residual_q += std::abs(tranform.rotation().toQuaternion().w()-1)+std::abs(tranform.rotation().toQuaternion().x())+std::abs(tranform.rotation().toQuaternion().y())+std::abs(tranform.rotation().toQuaternion().z()); 137 | } 138 | sum_residual_q = sum_residual_q / total_counter; 139 | sum_residual_t = sum_residual_t / total_counter; 140 | //ROS_INFO("optimization discard due to frame unaligned, residual_q:%f, residual_t:%f",sum_residual_q,sum_residual_t); 141 | 142 | if(sum_residual_q>0.02 || sum_residual_t>0.5){ 143 | ROS_INFO("optimization discard due to frame unaligned, residual_q:%f, residual_t:%f",sum_residual_q,sum_residual_t); 144 | //graph.pop_back(); 145 | graph.remove(graph.size()-1); 146 | return false; 147 | } 148 | //update states 149 | initials.clear(); 150 | for(int i =0;i<(int)result.size();i++){ 151 | pose_optimized_arr[i]= result.at(gtsam::Symbol('x',i+1)); 152 | initials.insert(gtsam::Symbol('x', i+1), pose_optimized_arr[i]); 153 | } 154 | return true; 155 | 156 | 157 | } 158 | bool ISCOptimizationClass::geometryConsistencyVerification(int current_id, int matched_id, Eigen::Isometry3d& transform){ 159 | pcl::PointCloud::Ptr map_surf_temp(new pcl::PointCloud()); 160 | pcl::PointCloud::Ptr map_edge_temp(new pcl::PointCloud()); 161 | pcl::PointCloud::Ptr map_surf(new pcl::PointCloud()); 162 | pcl::PointCloud::Ptr map_edge(new pcl::PointCloud()); 163 | 164 | for(int i = -10; i <=10; i=i+5){ 165 | if(matched_id+i>= current_id || matched_id+i<0) 166 | continue; 167 | Eigen::Isometry3d transform_pose = pose3ToEigen(pose_optimized_arr[matched_id+i]); 168 | pcl::PointCloud::Ptr transformed_temp(new pcl::PointCloud()); 169 | pcl::transformPointCloud(*pointcloud_surf_arr[matched_id+i], *transformed_temp, transform_pose.cast()); 170 | *map_surf_temp+=*transformed_temp; 171 | pcl::PointCloud::Ptr transformed_temp2(new pcl::PointCloud()); 172 | pcl::transformPointCloud(*pointcloud_edge_arr[matched_id+i], *transformed_temp2, transform_pose.cast()); 173 | *map_edge_temp+=*transformed_temp2; 174 | } 175 | 176 | Eigen::Isometry3d transform_pose = pose3ToEigen(pose_optimized_arr[matched_id]); 177 | pcl::transformPointCloud(*map_edge_temp, *map_edge, transform_pose.cast().inverse()); 178 | pcl::transformPointCloud(*map_surf_temp, *map_surf, transform_pose.cast().inverse()); 179 | //ROS_INFO("tag31"); 180 | pcl::PointCloud::Ptr current_scan_surf_temp(new pcl::PointCloud()); 181 | pcl::PointCloud::Ptr current_scan_edge_temp(new pcl::PointCloud()); 182 | pcl::PointCloud::Ptr current_scan_surf(new pcl::PointCloud()); 183 | pcl::PointCloud::Ptr current_scan_edge(new pcl::PointCloud()); 184 | for(int i = 0; i <=0; i=i+3){ 185 | if(current_id-i<0) 186 | continue; 187 | Eigen::Isometry3d transform_pose = pose3ToEigen(pose_optimized_arr[current_id+i]); 188 | pcl::PointCloud::Ptr transformed_temp(new pcl::PointCloud()); 189 | pcl::transformPointCloud(*pointcloud_surf_arr[current_id-i], *transformed_temp, transform_pose.cast()); 190 | *current_scan_surf_temp+=*transformed_temp; 191 | pcl::PointCloud::Ptr transformed_temp2(new pcl::PointCloud()); 192 | pcl::transformPointCloud(*pointcloud_edge_arr[current_id-i], *transformed_temp2, transform_pose.cast()); 193 | *current_scan_edge_temp+=*transformed_temp2; 194 | } 195 | 196 | transform_pose = pose3ToEigen(pose_optimized_arr[current_id]); 197 | pcl::transformPointCloud(*current_scan_edge_temp, *current_scan_edge, transform_pose.cast().inverse()); 198 | pcl::transformPointCloud(*current_scan_surf_temp, *current_scan_surf, transform_pose.cast().inverse()); 199 | 200 | //this is for visualization only 201 | loop_candidate_pc->clear(); 202 | loop_map_pc->clear(); 203 | transform_pose = transform; 204 | transform_pose.translation() = Eigen::Vector3d(0,0,10); 205 | pcl::PointCloud::Ptr cloud_temp(new pcl::PointCloud()); 206 | pcl::transformPointCloud(*current_scan_surf, *loop_candidate_pc, transform_pose.cast()); 207 | pcl::transformPointCloud(*current_scan_edge, *cloud_temp, transform_pose.cast()); 208 | *loop_candidate_pc+=*cloud_temp; 209 | *loop_map_pc += *map_surf; 210 | *loop_map_pc += *map_edge; 211 | double match_score = estimateOdom(map_edge,map_surf,current_scan_edge,current_scan_surf,transform); 212 | //ROS_WARN("matched score %f",match_score); 213 | 214 | if(match_score < LOOPCLOSURE_THRESHOLD){ 215 | return true; 216 | } 217 | else{ 218 | //ROS_INFO("loop rejected due to geometry verification current_id%d matched_id %d, score: %f",current_id,matched_id,match_score); 219 | return false; 220 | } 221 | return false; 222 | } 223 | 224 | Eigen::Isometry3d ISCOptimizationClass::getLastPose(void){ 225 | return pose3ToEigen(pose_optimized_arr.back()); 226 | } 227 | 228 | Eigen::Isometry3d ISCOptimizationClass::getPose(int frame_num){ 229 | 230 | return pose3ToEigen(pose_optimized_arr[frame_num]); 231 | } 232 | 233 | double ISCOptimizationClass::estimateOdom(const pcl::PointCloud::Ptr& pc_source_edge, const pcl::PointCloud::Ptr& pc_source_surf, const pcl::PointCloud::Ptr& pc_target_edge, const pcl::PointCloud::Ptr& pc_target_surf, Eigen::Isometry3d& transform){ 234 | Eigen::Quaterniond init_q(transform.rotation()); 235 | Eigen::Vector3d init_t(0,0,0); 236 | double parameters[7] = {init_q.x(), init_q.y(), init_q.z(), init_q.w(),init_t.x(), init_t.y(), init_t.z()}; 237 | Eigen::Map q_temp = Eigen::Map(parameters); 238 | Eigen::Map t_temp = Eigen::Map(parameters + 4); 239 | pcl::KdTreeFLANN::Ptr kdtreeCorner = pcl::KdTreeFLANN::Ptr(new pcl::KdTreeFLANN()); 240 | pcl::KdTreeFLANN::Ptr kdtreeSurf = pcl::KdTreeFLANN::Ptr(new pcl::KdTreeFLANN()); 241 | kdtreeCorner->setInputCloud(pc_source_edge); 242 | kdtreeSurf->setInputCloud(pc_source_surf); 243 | double total_cost = 300; 244 | for (int opti_counter = 0; opti_counter < 10; opti_counter++) 245 | { 246 | ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); 247 | ceres::Problem::Options problem_options; 248 | 249 | ceres::Problem problem(problem_options); 250 | problem.AddParameterBlock(parameters, 7, new PoseSE3Parameterization()); 251 | 252 | Eigen::Affine3d transform = Eigen::Affine3d::Identity(); 253 | transform.translation() = t_temp; 254 | transform.linear() = q_temp.toRotationMatrix(); 255 | //add edge cost factor 256 | int corner_num=0; 257 | pcl::PointCloud::Ptr tranformed_edge(new pcl::PointCloud()); 258 | pcl::transformPointCloud(*pc_target_edge, *tranformed_edge, transform); 259 | for (int i = 0; i < (int) tranformed_edge->points.size(); i++) 260 | { 261 | std::vector pointSearchInd; 262 | std::vector pointSearchSqDis; 263 | kdtreeCorner->nearestKSearch(tranformed_edge->points[i], 5, pointSearchInd, pointSearchSqDis); 264 | if (pointSearchSqDis[4] < 2.0) 265 | { 266 | std::vector nearCorners; 267 | Eigen::Vector3d center(0, 0, 0); 268 | for (int j = 0; j < 5; j++) 269 | { 270 | Eigen::Vector3d tmp(pc_source_edge->points[pointSearchInd[j]].x, 271 | pc_source_edge->points[pointSearchInd[j]].y, 272 | pc_source_edge->points[pointSearchInd[j]].z); 273 | center = center + tmp; 274 | nearCorners.push_back(tmp); 275 | } 276 | center = center / 5.0; 277 | 278 | Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero(); 279 | for (int j = 0; j < 5; j++) 280 | { 281 | Eigen::Matrix tmpZeroMean = nearCorners[j] - center; 282 | covMat = covMat + tmpZeroMean * tmpZeroMean.transpose(); 283 | } 284 | 285 | Eigen::SelfAdjointEigenSolver saes(covMat); 286 | 287 | Eigen::Vector3d unit_direction = saes.eigenvectors().col(2); 288 | Eigen::Vector3d curr_point(pc_target_edge->points[i].x, pc_target_edge->points[i].y, pc_target_edge->points[i].z); 289 | if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1]) 290 | { 291 | Eigen::Vector3d point_on_line = center; 292 | Eigen::Vector3d point_a, point_b; 293 | point_a = 0.1 * unit_direction + point_on_line; 294 | point_b = -0.1 * unit_direction + point_on_line; 295 | 296 | ceres::CostFunction *cost_function = new EdgeAnalyticCostFunction(curr_point, point_a, point_b); 297 | problem.AddResidualBlock(cost_function, loss_function, parameters); 298 | corner_num++; 299 | } 300 | } 301 | } 302 | if(corner_num<20){ 303 | ROS_INFO("not enough corresponding points"); 304 | return 300.0; 305 | } 306 | //add surf cost factor 307 | int surf_num=0; 308 | pcl::PointCloud::Ptr tranformed_surf(new pcl::PointCloud()); 309 | pcl::transformPointCloud(*pc_target_surf, *tranformed_surf, transform); 310 | // find correspondence for plane features 311 | for (int i = 0; i <(int) tranformed_surf->points.size(); ++i) 312 | { 313 | std::vector pointSearchInd; 314 | std::vector pointSearchSqDis; 315 | kdtreeSurf->nearestKSearch(tranformed_surf->points[i], 5, pointSearchInd, pointSearchSqDis); 316 | Eigen::Matrix matA0; 317 | Eigen::Matrix matB0 = -1 * Eigen::Matrix::Ones(); 318 | if (pointSearchSqDis[4] < 2.0) 319 | { 320 | for (int j = 0; j < 5; j++) 321 | { 322 | matA0(j, 0) = pc_source_surf->points[pointSearchInd[j]].x; 323 | matA0(j, 1) = pc_source_surf->points[pointSearchInd[j]].y; 324 | matA0(j, 2) = pc_source_surf->points[pointSearchInd[j]].z; 325 | } 326 | 327 | Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0); 328 | double negative_OA_dot_norm = 1 / norm.norm(); 329 | norm.normalize(); 330 | bool planeValid = true; 331 | 332 | for (int j = 0; j < 5; j++) 333 | { 334 | 335 | if (fabs(norm(0) * pc_source_surf->points[pointSearchInd[j]].x + 336 | norm(1) * pc_source_surf->points[pointSearchInd[j]].y + 337 | norm(2) * pc_source_surf->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2) 338 | { 339 | planeValid = false; 340 | break; 341 | } 342 | } 343 | 344 | Eigen::Vector3d curr_point(pc_target_surf->points[i].x, pc_target_surf->points[i].y, pc_target_surf->points[i].z); 345 | if (planeValid) 346 | { 347 | ceres::CostFunction *cost_function = new SurfNormAnalyticCostFunction(curr_point, norm, negative_OA_dot_norm); 348 | problem.AddResidualBlock(cost_function, loss_function, parameters); 349 | surf_num++; 350 | } 351 | 352 | } 353 | } 354 | 355 | if(surf_num<20){ 356 | ROS_INFO("not enough corresponding points"); 357 | return 300.0; 358 | } 359 | ceres::Solver::Options options; 360 | options.linear_solver_type = ceres::DENSE_QR; 361 | options.max_num_iterations = 4; 362 | options.minimizer_progress_to_stdout = false; 363 | ceres::Solver::Summary summary; 364 | ceres::Solve(options, &problem, &summary); 365 | if(summary.final_cost 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | //ros 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | //PCL 21 | #include 22 | #include 23 | #include 24 | 25 | //opencv 26 | 27 | //my library 28 | #include "iscOptimizationClass.h" 29 | #include 30 | 31 | ros::Publisher odom_pub; 32 | //ros::Publisher map_pub; 33 | ros::Publisher path_pub; 34 | ros::Publisher loop_map_pub; 35 | ros::Publisher loop_candidate_pub; 36 | std::mutex mutex_lock; 37 | 38 | //queue for loop closure detection 39 | std::queue pointCloudBuf; 40 | std::queue pointCloudSurfBuf; 41 | std::queue pointCloudEdgeBuf; 42 | std::queue odometryBuf; 43 | std::queue loopInfoBuf; 44 | 45 | ISCOptimizationClass iscOptimization; 46 | 47 | double scan_period= 0.1; 48 | 49 | 50 | Eigen::Isometry3d w_odom_curr= Eigen::Isometry3d::Identity(); 51 | nav_msgs::Path path_optimized; 52 | int current_frame_id; 53 | std::vector matched_frame_id; 54 | //receive odomtry 55 | void odomCallback(const nav_msgs::Odometry::ConstPtr &msg) 56 | { 57 | mutex_lock.lock(); 58 | odometryBuf.push(msg); 59 | mutex_lock.unlock(); 60 | 61 | // high frequence publish 62 | Eigen::Isometry3d odom_to_base= Eigen::Isometry3d::Identity(); 63 | odom_to_base.rotate(Eigen::Quaterniond(msg->pose.pose.orientation.w,msg->pose.pose.orientation.x,msg->pose.pose.orientation.y,msg->pose.pose.orientation.z)); 64 | odom_to_base.pretranslate(Eigen::Vector3d(msg->pose.pose.position.x,msg->pose.pose.position.y,msg->pose.pose.position.z)); 65 | Eigen::Isometry3d w_curr = w_odom_curr*odom_to_base; 66 | Eigen::Quaterniond q_temp(w_curr.rotation()); 67 | 68 | static tf::TransformBroadcaster br_realtime; 69 | tf::Transform transform; 70 | transform.setOrigin( tf::Vector3(w_curr.translation().x(), w_curr.translation().y(), w_curr.translation().z()) ); 71 | tf::Quaternion q(q_temp.x(),q_temp.y(),q_temp.z(),q_temp.w()); 72 | transform.setRotation(q); 73 | br_realtime.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "odom_final")); 74 | 75 | // publish odometry 76 | nav_msgs::Odometry laserOdometry; 77 | laserOdometry.header.frame_id = "map"; //world 78 | laserOdometry.child_frame_id = "odom_final"; //odom 79 | laserOdometry.header.stamp = msg->header.stamp; 80 | laserOdometry.pose.pose.orientation.x = q_temp.x(); 81 | laserOdometry.pose.pose.orientation.y = q_temp.y(); 82 | laserOdometry.pose.pose.orientation.z = q_temp.z(); 83 | laserOdometry.pose.pose.orientation.w = q_temp.w(); 84 | laserOdometry.pose.pose.position.x = w_curr.translation().x(); 85 | laserOdometry.pose.pose.position.y = w_curr.translation().y(); 86 | laserOdometry.pose.pose.position.z = w_curr.translation().z(); 87 | odom_pub.publish(laserOdometry); 88 | } 89 | 90 | //receive all point cloud 91 | void pointCloudSurfCallback(const sensor_msgs::PointCloud2ConstPtr &msg) 92 | { 93 | mutex_lock.lock(); 94 | pointCloudSurfBuf.push(msg); 95 | mutex_lock.unlock(); 96 | } 97 | //receive all point cloud 98 | void pointCloudEdgeCallback(const sensor_msgs::PointCloud2ConstPtr &msg) 99 | { 100 | mutex_lock.lock(); 101 | pointCloudEdgeBuf.push(msg); 102 | mutex_lock.unlock(); 103 | } 104 | 105 | //receive all point cloud 106 | void loopClosureCallback(const iscloam::LoopInfoConstPtr &msg) 107 | { 108 | mutex_lock.lock(); 109 | loopInfoBuf.push(msg); 110 | mutex_lock.unlock(); 111 | } 112 | 113 | void global_optimization(){ 114 | while(1){ 115 | if(!loopInfoBuf.empty()&& !pointCloudSurfBuf.empty() && !pointCloudEdgeBuf.empty() && !odometryBuf.empty()){ 116 | 117 | mutex_lock.lock(); 118 | 119 | double time1 = loopInfoBuf.front()->header.stamp.toSec(); 120 | double time2 = odometryBuf.front()->header.stamp.toSec(); 121 | double time3 = pointCloudEdgeBuf.front()->header.stamp.toSec(); 122 | double time4 = pointCloudSurfBuf.front()->header.stamp.toSec(); 123 | if(!loopInfoBuf.empty() && (time1 isc optimization"); 125 | loopInfoBuf.pop(); 126 | mutex_lock.unlock(); 127 | continue; 128 | } 129 | if(!odometryBuf.empty() && (time2 isc optimization"); 131 | odometryBuf.pop(); 132 | mutex_lock.unlock(); 133 | continue; 134 | } 135 | if(!pointCloudEdgeBuf.empty() && (time3 isc optimization"); 137 | pointCloudEdgeBuf.pop(); 138 | mutex_lock.unlock(); 139 | continue; 140 | } 141 | if(!pointCloudSurfBuf.empty() && (time4 isc optimization"); 143 | pointCloudSurfBuf.pop(); 144 | mutex_lock.unlock(); 145 | continue; 146 | } 147 | 148 | 149 | pcl::PointCloud::Ptr pointcloud_edge_in(new pcl::PointCloud()); 150 | pcl::PointCloud::Ptr pointcloud_surf_in(new pcl::PointCloud()); 151 | pcl::fromROSMsg(*pointCloudEdgeBuf.front(), *pointcloud_edge_in); 152 | pcl::fromROSMsg(*pointCloudSurfBuf.front(), *pointcloud_surf_in); 153 | ros::Time pointcloud_time = (odometryBuf.front())->header.stamp; 154 | Eigen::Isometry3d odom_in = Eigen::Isometry3d::Identity(); 155 | odom_in.rotate(Eigen::Quaterniond(odometryBuf.front()->pose.pose.orientation.w,odometryBuf.front()->pose.pose.orientation.x,odometryBuf.front()->pose.pose.orientation.y,odometryBuf.front()->pose.pose.orientation.z)); 156 | odom_in.pretranslate(Eigen::Vector3d(odometryBuf.front()->pose.pose.position.x,odometryBuf.front()->pose.pose.position.y,odometryBuf.front()->pose.pose.position.z)); 157 | current_frame_id = loopInfoBuf.front()->current_id; 158 | matched_frame_id.clear(); 159 | for(int i=0;i<(int)loopInfoBuf.front()->matched_id.size();i++){ 160 | matched_frame_id.push_back(loopInfoBuf.front()->matched_id[i]); 161 | } 162 | pointCloudEdgeBuf.pop(); 163 | pointCloudSurfBuf.pop(); 164 | odometryBuf.pop(); 165 | loopInfoBuf.pop(); 166 | mutex_lock.unlock(); 167 | 168 | if(current_frame_id!= (int)iscOptimization.pointcloud_surf_arr.size()){ 169 | ROS_WARN_ONCE("graph optimization frame not aligned,pls check your data"); 170 | } 171 | if(iscOptimization.addPoseToGraph(pointcloud_edge_in, pointcloud_surf_in,matched_frame_id, odom_in)){ 172 | //update path 173 | path_optimized.poses.clear(); 174 | for(int i=0;i<(int)iscOptimization.pointcloud_surf_arr.size()-1;i++){ 175 | Eigen::Isometry3d pose = iscOptimization.getPose(i); 176 | Eigen::Quaterniond q_temp(pose.rotation()); 177 | 178 | geometry_msgs::PoseStamped pose_temp; 179 | pose_temp.pose.position.x = pose.translation().x(); 180 | pose_temp.pose.position.y = pose.translation().y(); 181 | pose_temp.pose.position.z = pose.translation().z(); 182 | pose_temp.pose.orientation.w = q_temp.w(); 183 | pose_temp.pose.orientation.x = q_temp.x(); 184 | pose_temp.pose.orientation.y = q_temp.y(); 185 | pose_temp.pose.orientation.z = q_temp.z(); 186 | pose_temp.header.stamp = ros::Time::now(); 187 | pose_temp.header.frame_id = "map"; 188 | path_optimized.poses.push_back(pose_temp); 189 | } 190 | } 191 | 192 | path_optimized.header.seq = (int)iscOptimization.pointcloud_surf_arr.size(); 193 | path_optimized.header.stamp = pointcloud_time; 194 | Eigen::Isometry3d pose_current = iscOptimization.getLastPose(); 195 | Eigen::Quaterniond q_current(pose_current.rotation()); 196 | Eigen::Vector3d t_current = pose_current.translation(); 197 | w_odom_curr = pose_current * odom_in.inverse(); 198 | 199 | geometry_msgs::PoseStamped pose_temp; 200 | pose_temp.pose.position.x = t_current.x(); 201 | pose_temp.pose.position.y = t_current.y(); 202 | pose_temp.pose.position.z = t_current.z(); 203 | pose_temp.pose.orientation.w = q_current.w(); 204 | pose_temp.pose.orientation.x = q_current.x(); 205 | pose_temp.pose.orientation.y = q_current.y(); 206 | pose_temp.pose.orientation.z = q_current.z(); 207 | pose_temp.header.stamp = ros::Time::now(); 208 | pose_temp.header.frame_id = "map"; 209 | path_optimized.poses.push_back(pose_temp); 210 | path_pub.publish(path_optimized); 211 | 212 | /* loop closure visualization 213 | sensor_msgs::PointCloud2 PointsMsg; 214 | pcl::toROSMsg(*iscOptimization.loop_map_pc, PointsMsg); 215 | PointsMsg.header.stamp = pointcloud_time; 216 | PointsMsg.header.frame_id = "/map"; 217 | loop_map_pub.publish(PointsMsg); 218 | 219 | sensor_msgs::PointCloud2 PointsMsg2; 220 | pcl::toROSMsg(*iscOptimization.loop_candidate_pc, PointsMsg2); 221 | PointsMsg2.header.stamp = pointcloud_time; 222 | PointsMsg2.header.frame_id = "/map"; 223 | loop_candidate_pub.publish(PointsMsg2); 224 | 225 | 226 | sensor_msgs::PointCloud2 PointsMsg3; 227 | pcl::toROSMsg(*iscOptimization.map, PointsMsg3); 228 | PointsMsg2.header.stamp = pointcloud_time; 229 | PointsMsg2.header.frame_id = "/map"; 230 | map_pub.publish(PointsMsg3); 231 | */ 232 | 233 | } 234 | //sleep 2 ms every time 235 | std::chrono::milliseconds dura(2); 236 | std::this_thread::sleep_for(dura); 237 | }//end of while(1) 238 | } 239 | 240 | int main(int argc, char **argv) 241 | { 242 | 243 | ros::init(argc, argv, "iscOptimization"); 244 | ros::NodeHandle nh("~"); 245 | 246 | ros::Subscriber velodyne_edge_sub= nh.subscribe("/laser_cloud_edge", 10, pointCloudEdgeCallback); 247 | ros::Subscriber velodyne_surf_sub= nh.subscribe("/laser_cloud_surf", 10, pointCloudSurfCallback); 248 | ros::Subscriber odom_sub= nh.subscribe("/odom", 10, odomCallback); 249 | ros::Subscriber loop_sub= nh.subscribe("/loop_closure", 10, loopClosureCallback); 250 | 251 | odom_pub = nh.advertise("/odom_final", 100); 252 | //map_pub = nh.advertise("/map", 100); 253 | path_pub = nh.advertise("/final_path", 100); 254 | // loop_map_pub = nh.advertise("/loop_map", 100); 255 | // loop_candidate_pub = nh.advertise("/loop_candidate", 100); 256 | 257 | path_optimized.header.frame_id = "map"; 258 | path_optimized.header.stamp = ros::Time::now(); 259 | //read parameter 260 | int sector_width =60; 261 | int ring_height = 60; 262 | double max_distance= 40.0; 263 | 264 | nh.getParam("/sector_width", sector_width); 265 | nh.getParam("/ring_height", ring_height); 266 | nh.getParam("/max_distance", max_distance); 267 | nh.getParam("/scan_period", scan_period); 268 | 269 | 270 | //init ISC 271 | iscOptimization.init(); 272 | 273 | //open thread 274 | std::thread global_optimization_process{global_optimization}; 275 | 276 | ros::spin(); 277 | 278 | return 0; 279 | } 280 | -------------------------------------------------------------------------------- /src/laserMappingClass.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #include "laserMappingClass.h" 6 | 7 | void LaserMappingClass::init(double map_resolution_){ 8 | //init map 9 | //init can have real object, but future added block does not need 10 | for(int i=0;i::Ptr>> map_height_temp; 12 | for(int j=0;j::Ptr> map_depth_temp; 14 | for(int k=0;k::Ptr point_cloud_temp(new pcl::PointCloud); 16 | map_depth_temp.push_back(point_cloud_temp); 17 | } 18 | map_height_temp.push_back(map_depth_temp); 19 | } 20 | map.push_back(map_height_temp); 21 | } 22 | map_resolution = map_resolution_; 23 | origin_in_map_x = LASER_CELL_RANGE_HORIZONTAL; 24 | origin_in_map_y = LASER_CELL_RANGE_HORIZONTAL; 25 | origin_in_map_z = LASER_CELL_RANGE_VERTICAL; 26 | map_width = LASER_CELL_RANGE_HORIZONTAL*2+1; 27 | map_height = LASER_CELL_RANGE_HORIZONTAL*2+1; 28 | map_depth = LASER_CELL_RANGE_HORIZONTAL*2+1; 29 | 30 | //downsampling size 31 | downSizeFilter.setLeafSize(map_resolution* 2.5, map_resolution* 2.5, map_resolution* 2.5); 32 | last_pose = Eigen::Isometry3d::Identity(); 33 | } 34 | 35 | void LaserMappingClass::addWidthCellNegative(void){ 36 | std::vector::Ptr>> map_height_temp; 37 | for(int j=0; j < map_height;j++){ 38 | std::vector::Ptr> map_depth_temp; 39 | for(int k=0;k< map_depth;k++){ 40 | pcl::PointCloud::Ptr point_cloud_temp; 41 | map_depth_temp.push_back(point_cloud_temp); 42 | } 43 | map_height_temp.push_back(map_depth_temp); 44 | } 45 | map.insert(map.begin(), map_height_temp); 46 | 47 | origin_in_map_x++; 48 | map_width++; 49 | } 50 | void LaserMappingClass::addWidthCellPositive(void){ 51 | std::vector::Ptr>> map_height_temp; 52 | for(int j=0; j < map_height;j++){ 53 | std::vector::Ptr> map_depth_temp; 54 | for(int k=0;k< map_depth;k++){ 55 | pcl::PointCloud::Ptr point_cloud_temp; 56 | map_depth_temp.push_back(point_cloud_temp); 57 | } 58 | map_height_temp.push_back(map_depth_temp); 59 | } 60 | map.push_back(map_height_temp); 61 | map_width++; 62 | } 63 | void LaserMappingClass::addHeightCellNegative(void){ 64 | for(int i=0; i < map_width;i++){ 65 | std::vector::Ptr> map_depth_temp; 66 | for(int k=0;k::Ptr point_cloud_temp; 68 | map_depth_temp.push_back(point_cloud_temp); 69 | } 70 | map[i].insert(map[i].begin(), map_depth_temp); 71 | } 72 | origin_in_map_y++; 73 | map_height++; 74 | } 75 | void LaserMappingClass::addHeightCellPositive(void){ 76 | for(int i=0; i < map_width;i++){ 77 | std::vector::Ptr> map_depth_temp; 78 | for(int k=0;k::Ptr point_cloud_temp; 80 | map_depth_temp.push_back(point_cloud_temp); 81 | } 82 | map[i].push_back(map_depth_temp); 83 | } 84 | map_height++; 85 | } 86 | void LaserMappingClass::addDepthCellNegative(void){ 87 | for(int i=0; i < map_width;i++){ 88 | for(int j=0;j< map_height;j++){ 89 | pcl::PointCloud::Ptr point_cloud_temp; 90 | map[i][j].insert(map[i][j].begin(), point_cloud_temp); 91 | } 92 | } 93 | origin_in_map_z++; 94 | map_depth++; 95 | } 96 | void LaserMappingClass::addDepthCellPositive(void){ 97 | for(int i=0; i < map_width;i++){ 98 | for(int j=0;j< map_height;j++){ 99 | pcl::PointCloud::Ptr point_cloud_temp; 100 | map[i][j].push_back(point_cloud_temp); 101 | } 102 | } 103 | map_depth++; 104 | } 105 | 106 | //extend map is points exceed size 107 | void LaserMappingClass::checkPoints(int& x, int& y, int& z){ 108 | 109 | while(x + LASER_CELL_RANGE_HORIZONTAL> map_width-1){ 110 | addWidthCellPositive(); 111 | } 112 | while(x-LASER_CELL_RANGE_HORIZONTAL<0){ 113 | addWidthCellNegative(); 114 | x++; 115 | } 116 | while(y + LASER_CELL_RANGE_HORIZONTAL> map_height-1){ 117 | addHeightCellPositive(); 118 | } 119 | while(y-LASER_CELL_RANGE_HORIZONTAL<0){ 120 | addHeightCellNegative(); 121 | y++; 122 | } 123 | while(z + LASER_CELL_RANGE_VERTICAL> map_depth-1){ 124 | addDepthCellPositive(); 125 | } 126 | while(z-LASER_CELL_RANGE_VERTICAL<0){ 127 | addDepthCellNegative(); 128 | z++; 129 | } 130 | 131 | //initialize 132 | //create object if area is null 133 | for(int i=x-LASER_CELL_RANGE_HORIZONTAL;i::Ptr point_cloud_temp(new pcl::PointCloud()); 138 | map[i][j][k] = point_cloud_temp; 139 | } 140 | 141 | } 142 | 143 | } 144 | 145 | } 146 | } 147 | 148 | void LaserMappingClass::resetMap(std::vector& path){ 149 | map.clear(); 150 | init(map_resolution); 151 | 152 | for(int i=0;i::Ptr transformed_pc(new pcl::PointCloud()); 161 | pcl::transformPointCloud(*point_cloud_arr[i], *transformed_pc, path[i].cast()); 162 | 163 | 164 | for (int j = 0; j < (int)transformed_pc->points.size(); j++) 165 | { 166 | pcl::PointXYZI point_temp = transformed_pc->points[j]; 167 | //for visualization only 168 | //point_temp.intensity = std::min(1.0 , std::max(point_cloud_arr[i]->points[j].z+2.0, 0.0) / 4); 169 | 170 | int currentPointIdX = int(std::floor(point_temp.x / LASER_CELL_WIDTH + 0.5)) + origin_in_map_x; 171 | int currentPointIdY = int(std::floor(point_temp.y / LASER_CELL_HEIGHT + 0.5)) + origin_in_map_y; 172 | int currentPointIdZ = int(std::floor(point_temp.z / LASER_CELL_DEPTH + 0.5)) + origin_in_map_z; 173 | 174 | map[currentPointIdX][currentPointIdY][currentPointIdZ]->push_back(point_temp); 175 | 176 | } 177 | 178 | //filtering points 179 | for(int i=currentPosIdX-LASER_CELL_RANGE_HORIZONTAL;i::Ptr& pc_in, const Eigen::Isometry3d& pose_current){ 197 | 198 | int currentPosIdX = int(std::floor(pose_current.translation().x() / LASER_CELL_WIDTH + 0.5)) + origin_in_map_x; 199 | int currentPosIdY = int(std::floor(pose_current.translation().y() / LASER_CELL_HEIGHT + 0.5)) + origin_in_map_y; 200 | int currentPosIdZ = int(std::floor(pose_current.translation().z() / LASER_CELL_DEPTH + 0.5)) + origin_in_map_z; 201 | 202 | //check is submap is null 203 | checkPoints(currentPosIdX,currentPosIdY,currentPosIdZ); 204 | 205 | pcl::PointCloud::Ptr transformed_pc(new pcl::PointCloud()); 206 | pcl::transformPointCloud(*pc_in, *transformed_pc, pose_current.cast()); 207 | 208 | //储存点 209 | for (int i = 0; i < (int)transformed_pc->points.size(); i++) 210 | { 211 | pcl::PointXYZI point_temp = transformed_pc->points[i]; 212 | //for visualization only 213 | //point_temp.intensity = std::min(1.0 , std::max(pc_in->points[i].z+2.0, 0.0) / 4); 214 | 215 | int currentPointIdX = int(std::floor(point_temp.x / LASER_CELL_WIDTH + 0.5)) + origin_in_map_x; 216 | int currentPointIdY = int(std::floor(point_temp.y / LASER_CELL_HEIGHT + 0.5)) + origin_in_map_y; 217 | int currentPointIdZ = int(std::floor(point_temp.z / LASER_CELL_DEPTH + 0.5)) + origin_in_map_z; 218 | 219 | map[currentPointIdX][currentPointIdY][currentPointIdZ]->push_back(point_temp); 220 | 221 | } 222 | 223 | //filtering points 224 | for(int i=currentPosIdX-LASER_CELL_RANGE_HORIZONTAL;i::Ptr LaserMappingClass::getMap(void){ 240 | pcl::PointCloud::Ptr laserCloudMap = pcl::PointCloud::Ptr(new pcl::PointCloud()); 241 | for (int i = 0; i < map_width; i++){ 242 | for (int j = 0; j < map_height; j++){ 243 | for (int k = 0; k < map_depth; k++){ 244 | if(map[i][j][k]!=NULL){ 245 | *laserCloudMap += *(map[i][j][k]); 246 | } 247 | } 248 | } 249 | } 250 | return laserCloudMap; 251 | } 252 | 253 | LaserMappingClass::LaserMappingClass(){ 254 | 255 | } 256 | -------------------------------------------------------------------------------- /src/laserMappingNode.cpp: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | //c++ lib 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | //ros lib 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | //pcl lib 22 | #include 23 | #include 24 | #include 25 | 26 | //local lib 27 | #include "laserMappingClass.h" 28 | #include "lidar.h" 29 | 30 | LaserMappingClass laserMapping; 31 | lidar::Lidar lidar_param; 32 | std::mutex mutex_lock; 33 | std::queue pathBuf; 34 | std::queue pointCloudBuf; 35 | 36 | ros::Publisher map_pub; 37 | void pathHandler(const nav_msgs::PathConstPtr &pathMsg) 38 | { 39 | mutex_lock.lock(); 40 | pathBuf.push(pathMsg); 41 | mutex_lock.unlock(); 42 | } 43 | 44 | void velodyneHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 45 | { 46 | mutex_lock.lock(); 47 | pointCloudBuf.push(laserCloudMsg); 48 | mutex_lock.unlock(); 49 | } 50 | 51 | Eigen::Isometry3d geometryToEigen(geometry_msgs::PoseStamped& pose_geo){ 52 | Eigen::Isometry3d pose_eigen = Eigen::Isometry3d::Identity(); 53 | pose_eigen.rotate(Eigen::Quaterniond(pose_geo.pose.orientation.w,pose_geo.pose.orientation.x,pose_geo.pose.orientation.y,pose_geo.pose.orientation.z)); 54 | pose_eigen.pretranslate(Eigen::Vector3d(pose_geo.pose.position.x,pose_geo.pose.position.y,pose_geo.pose.position.z)); 55 | return pose_eigen; 56 | } 57 | 58 | void laser_mapping(){ 59 | while(1){ 60 | if(!pathBuf.empty() && !pointCloudBuf.empty()){ 61 | 62 | //read data 63 | mutex_lock.lock(); 64 | if(!pointCloudBuf.empty() && pointCloudBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period){ 65 | ROS_WARN("time stamp unaligned error and pointcloud discarded, pls check your data --> laser mapping node"); 66 | pointCloudBuf.pop(); 67 | mutex_lock.unlock(); 68 | continue; 69 | } 70 | 71 | if(!pathBuf.empty() && pathBuf.front()->header.stamp.toSec() < pointCloudBuf.front()->header.stamp.toSec()-0.5*lidar_param.scan_period){ 72 | pathBuf.pop(); 73 | ROS_INFO("time stamp unaligned with path final, pls check your data --> laser mapping node"); 74 | mutex_lock.unlock(); 75 | continue; 76 | } 77 | 78 | //if time aligned 79 | pcl::PointCloud::Ptr pointcloud_in(new pcl::PointCloud()); 80 | pcl::fromROSMsg(*pointCloudBuf.front(), *pointcloud_in); 81 | ros::Time pointcloud_time = (pointCloudBuf.front())->header.stamp; 82 | nav_msgs::Path path = *(pathBuf.front()); 83 | pointCloudBuf.pop(); 84 | pathBuf.pop(); 85 | mutex_lock.unlock(); 86 | 87 | int path_size = path.poses.size(); 88 | if(path.poses.size()!=laserMapping.point_cloud_arr.size()+1){ 89 | ROS_WARN("path size %d and point cloud size %d not aligned, laser mapping", path_size, laserMapping.point_cloud_arr.size()+1); 90 | } 91 | 92 | //check is the path deviates from last path by checking the last odom 93 | if(path.poses.size()>2){ 94 | Eigen::Isometry3d pose_temp = geometryToEigen(path.poses[path_size-2]); 95 | Eigen::Isometry3d pose_error = laserMapping.last_pose * pose_temp.inverse(); 96 | 97 | double translational_error = abs(pose_error.translation().x()) + abs(pose_error.translation().y()) + abs(pose_error.translation().z()); 98 | if(translational_error>0.1){ 99 | //if global optimization is applied 100 | std::vector path_eigen; 101 | for(int i=0;i::Ptr pc_map = laserMapping.getMap(); 120 | sensor_msgs::PointCloud2 PointsMsg; 121 | pcl::toROSMsg(*pc_map, PointsMsg); 122 | PointsMsg.header.stamp = pointcloud_time; 123 | PointsMsg.header.frame_id = "map"; 124 | map_pub.publish(PointsMsg); 125 | } 126 | 127 | 128 | } 129 | //sleep 2 ms every time 130 | std::chrono::milliseconds dura(2); 131 | std::this_thread::sleep_for(dura); 132 | } 133 | } 134 | 135 | int main(int argc, char **argv) 136 | { 137 | ros::init(argc, argv, "main"); 138 | ros::NodeHandle nh; 139 | 140 | int scan_line = 64; 141 | double vertical_angle = 2.0; 142 | double scan_period= 0.1; 143 | double max_dis = 60.0; 144 | double min_dis = 2.0; 145 | double map_resolution = 0.4; 146 | nh.getParam("/scan_period", scan_period); 147 | nh.getParam("/vertical_angle", vertical_angle); 148 | nh.getParam("/max_dis", max_dis); 149 | nh.getParam("/min_dis", min_dis); 150 | nh.getParam("/scan_line", scan_line); 151 | nh.getParam("/map_resolution", map_resolution); 152 | 153 | lidar_param.setScanPeriod(scan_period); 154 | lidar_param.setVerticalAngle(vertical_angle); 155 | lidar_param.setLines(scan_line); 156 | lidar_param.setMaxDistance(max_dis); 157 | lidar_param.setMinDistance(min_dis); 158 | 159 | laserMapping.init(map_resolution); 160 | ros::Subscriber subLaserCloud = nh.subscribe("/velodyne_points_filtered", 100, velodyneHandler); 161 | ros::Subscriber subEdgeLaserCloud = nh.subscribe("/final_path", 100, pathHandler); 162 | 163 | map_pub = nh.advertise("/map", 100); 164 | std::thread laser_mapping_process{laser_mapping}; 165 | 166 | ros::spin(); 167 | 168 | return 0; 169 | } 170 | -------------------------------------------------------------------------------- /src/laserProcessingClass.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #include "laserProcessingClass.h" 5 | 6 | void LaserProcessingClass::init(lidar::Lidar lidar_param_in){ 7 | 8 | lidar_param = lidar_param_in; 9 | 10 | } 11 | 12 | void LaserProcessingClass::featureExtraction(const pcl::PointCloud::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf){ 13 | 14 | std::vector indices; 15 | pcl::removeNaNFromPointCloud(*pc_in, indices); 16 | 17 | 18 | int N_SCANS = lidar_param.num_lines; 19 | std::vector::Ptr> laserCloudScans; 20 | for(int i=0;i::Ptr(new pcl::PointCloud())); 22 | } 23 | 24 | for (int i = 0; i < (int) pc_in->points.size(); i++) 25 | { 26 | int scanID=0; 27 | double distance = sqrt(pc_in->points[i].x * pc_in->points[i].x + pc_in->points[i].y * pc_in->points[i].y); 28 | if(distancelidar_param.max_distance) 29 | continue; 30 | double angle = atan(pc_in->points[i].z / distance) * 180 / M_PI; 31 | 32 | if (N_SCANS == 16) 33 | { 34 | scanID = int((angle + 15) / 2 + 0.5); 35 | if (scanID > (N_SCANS - 1) || scanID < 0) 36 | { 37 | continue; 38 | } 39 | } 40 | else if (N_SCANS == 32) 41 | { 42 | scanID = int((angle + 92.0/3.0) * 3.0 / 4.0); 43 | if (scanID > (N_SCANS - 1) || scanID < 0) 44 | { 45 | continue; 46 | } 47 | } 48 | else if (N_SCANS == 64) 49 | { 50 | if (angle >= -8.83) 51 | scanID = int((2 - angle) * 3.0 + 0.5); 52 | else 53 | scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5); 54 | 55 | if (angle > 2 || angle < -24.33 || scanID > 63 || scanID < 0) 56 | { 57 | continue; 58 | } 59 | } 60 | else 61 | { 62 | printf("wrong scan number\n"); 63 | } 64 | laserCloudScans[scanID]->push_back(pc_in->points[i]); 65 | 66 | } 67 | 68 | for(int i = 0; i < N_SCANS; i++){ 69 | if(laserCloudScans[i]->points.size()<131){ 70 | continue; 71 | } 72 | 73 | std::vector cloudCurvature; 74 | int total_points = laserCloudScans[i]->points.size()-10; 75 | for(int j = 5; j < (int)laserCloudScans[i]->points.size() - 5; j++){ 76 | double diffX = laserCloudScans[i]->points[j - 5].x + laserCloudScans[i]->points[j - 4].x + laserCloudScans[i]->points[j - 3].x + laserCloudScans[i]->points[j - 2].x + laserCloudScans[i]->points[j - 1].x - 10 * laserCloudScans[i]->points[j].x + laserCloudScans[i]->points[j + 1].x + laserCloudScans[i]->points[j + 2].x + laserCloudScans[i]->points[j + 3].x + laserCloudScans[i]->points[j + 4].x + laserCloudScans[i]->points[j + 5].x; 77 | double diffY = laserCloudScans[i]->points[j - 5].y + laserCloudScans[i]->points[j - 4].y + laserCloudScans[i]->points[j - 3].y + laserCloudScans[i]->points[j - 2].y + laserCloudScans[i]->points[j - 1].y - 10 * laserCloudScans[i]->points[j].y + laserCloudScans[i]->points[j + 1].y + laserCloudScans[i]->points[j + 2].y + laserCloudScans[i]->points[j + 3].y + laserCloudScans[i]->points[j + 4].y + laserCloudScans[i]->points[j + 5].y; 78 | double diffZ = laserCloudScans[i]->points[j - 5].z + laserCloudScans[i]->points[j - 4].z + laserCloudScans[i]->points[j - 3].z + laserCloudScans[i]->points[j - 2].z + laserCloudScans[i]->points[j - 1].z - 10 * laserCloudScans[i]->points[j].z + laserCloudScans[i]->points[j + 1].z + laserCloudScans[i]->points[j + 2].z + laserCloudScans[i]->points[j + 3].z + laserCloudScans[i]->points[j + 4].z + laserCloudScans[i]->points[j + 5].z; 79 | Double2d distance(j,diffX * diffX + diffY * diffY + diffZ * diffZ); 80 | cloudCurvature.push_back(distance); 81 | 82 | } 83 | for(int j=0;j<6;j++){ 84 | int sector_length = (int)(total_points/6); 85 | int sector_start = sector_length *j; 86 | int sector_end = sector_length *(j+1)-1; 87 | if (j==5){ 88 | sector_end = total_points - 1; 89 | } 90 | std::vector subCloudCurvature(cloudCurvature.begin()+sector_start,cloudCurvature.begin()+sector_end); 91 | 92 | featureExtractionFromSector(laserCloudScans[i],subCloudCurvature, pc_out_edge, pc_out_surf); 93 | 94 | } 95 | 96 | } 97 | 98 | } 99 | 100 | 101 | void LaserProcessingClass::featureExtractionFromSector(const pcl::PointCloud::Ptr& pc_in, std::vector& cloudCurvature, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf){ 102 | 103 | std::sort(cloudCurvature.begin(), cloudCurvature.end(), [](const Double2d & a, const Double2d & b) 104 | { 105 | return a.value < b.value; 106 | }); 107 | 108 | 109 | int largestPickedNum = 0; 110 | std::vector picked_points; 111 | int point_info_count =0; 112 | for (int i = cloudCurvature.size()-1; i >= 0; i--) 113 | { 114 | int ind = cloudCurvature[i].id; 115 | if(std::find(picked_points.begin(), picked_points.end(), ind)==picked_points.end()){ 116 | if(cloudCurvature[i].value <= 0.1){ 117 | break; 118 | } 119 | 120 | largestPickedNum++; 121 | picked_points.push_back(ind); 122 | 123 | if (largestPickedNum <= 20){ 124 | pc_out_edge->push_back(pc_in->points[ind]); 125 | point_info_count++; 126 | }else{ 127 | break; 128 | } 129 | 130 | for(int k=1;k<=5;k++){ 131 | double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k - 1].x; 132 | double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k - 1].y; 133 | double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k - 1].z; 134 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){ 135 | break; 136 | } 137 | picked_points.push_back(ind+k); 138 | } 139 | for(int k=-1;k>=-5;k--){ 140 | double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k + 1].x; 141 | double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k + 1].y; 142 | double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k + 1].z; 143 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){ 144 | break; 145 | } 146 | picked_points.push_back(ind+k); 147 | } 148 | 149 | } 150 | } 151 | 152 | //find flat points 153 | // point_info_count =0; 154 | // int smallestPickedNum = 0; 155 | 156 | // for (int i = 0; i <= (int)cloudCurvature.size()-1; i++) 157 | // { 158 | // int ind = cloudCurvature[i].id; 159 | 160 | // if( std::find(picked_points.begin(), picked_points.end(), ind)==picked_points.end()){ 161 | // if(cloudCurvature[i].value > 0.1){ 162 | // //ROS_WARN("extracted feature not qualified, please check lidar"); 163 | // break; 164 | // } 165 | // smallestPickedNum++; 166 | // picked_points.push_back(ind); 167 | 168 | // if(smallestPickedNum <= 4){ 169 | // //find all points 170 | // pc_surf_flat->push_back(pc_in->points[ind]); 171 | // pc_surf_lessFlat->push_back(pc_in->points[ind]); 172 | // point_info_count++; 173 | // } 174 | // else{ 175 | // break; 176 | // } 177 | 178 | // for(int k=1;k<=5;k++){ 179 | // double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k - 1].x; 180 | // double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k - 1].y; 181 | // double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k - 1].z; 182 | // if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){ 183 | // break; 184 | // } 185 | // picked_points.push_back(ind+k); 186 | // } 187 | // for(int k=-1;k>=-5;k--){ 188 | // double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k + 1].x; 189 | // double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k + 1].y; 190 | // double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k + 1].z; 191 | // if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){ 192 | // break; 193 | // } 194 | // picked_points.push_back(ind+k); 195 | // } 196 | 197 | // } 198 | // } 199 | 200 | for (int i = 0; i <= (int)cloudCurvature.size()-1; i++) 201 | { 202 | int ind = cloudCurvature[i].id; 203 | if( std::find(picked_points.begin(), picked_points.end(), ind)==picked_points.end()) 204 | { 205 | pc_out_surf->push_back(pc_in->points[ind]); 206 | } 207 | } 208 | 209 | 210 | 211 | } 212 | LaserProcessingClass::LaserProcessingClass(){ 213 | 214 | } 215 | 216 | Double2d::Double2d(int id_in, double value_in){ 217 | id = id_in; 218 | value =value_in; 219 | }; 220 | 221 | PointsInfo::PointsInfo(int layer_in, double time_in){ 222 | layer = layer_in; 223 | time = time_in; 224 | }; 225 | -------------------------------------------------------------------------------- /src/laserProcessingNode.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | //c++ lib 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | //ros lib 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | //pcl lib 22 | #include 23 | #include 24 | #include 25 | 26 | //local lib 27 | #include "lidar.h" 28 | #include "laserProcessingClass.h" 29 | 30 | 31 | LaserProcessingClass laserProcessing; 32 | std::mutex mutex_lock; 33 | std::queue pointCloudBuf; 34 | lidar::Lidar lidar_param; 35 | 36 | ros::Publisher pubEdgePoints; 37 | ros::Publisher pubSurfPoints; 38 | ros::Publisher pubLaserCloudFiltered; 39 | 40 | void velodyneHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 41 | { 42 | mutex_lock.lock(); 43 | pointCloudBuf.push(laserCloudMsg); 44 | mutex_lock.unlock(); 45 | 46 | } 47 | 48 | double total_time =0; 49 | int total_frame=0; 50 | 51 | void laser_processing(){ 52 | while(1){ 53 | if(!pointCloudBuf.empty()){ 54 | //read data 55 | mutex_lock.lock(); 56 | pcl::PointCloud::Ptr pointcloud_in(new pcl::PointCloud()); 57 | pcl::fromROSMsg(*pointCloudBuf.front(), *pointcloud_in); 58 | ros::Time pointcloud_time = (pointCloudBuf.front())->header.stamp; 59 | pointCloudBuf.pop(); 60 | mutex_lock.unlock(); 61 | 62 | pcl::PointCloud::Ptr pointcloud_edge(new pcl::PointCloud()); 63 | pcl::PointCloud::Ptr pointcloud_surf(new pcl::PointCloud()); 64 | 65 | std::chrono::time_point start, end; 66 | start = std::chrono::system_clock::now(); 67 | laserProcessing.featureExtraction(pointcloud_in,pointcloud_edge,pointcloud_surf); 68 | end = std::chrono::system_clock::now(); 69 | std::chrono::duration elapsed_seconds = end - start; 70 | total_frame++; 71 | float time_temp = elapsed_seconds.count() * 1000; 72 | total_time+=time_temp; 73 | //ROS_INFO("average laser processing time %f ms \n \n", total_time/total_frame); 74 | 75 | sensor_msgs::PointCloud2 laserCloudFilteredMsg; 76 | pcl::PointCloud::Ptr pointcloud_filtered(new pcl::PointCloud()); 77 | *pointcloud_filtered+=*pointcloud_edge; 78 | *pointcloud_filtered+=*pointcloud_surf; 79 | pcl::toROSMsg(*pointcloud_filtered, laserCloudFilteredMsg); 80 | laserCloudFilteredMsg.header.stamp = pointcloud_time; 81 | laserCloudFilteredMsg.header.frame_id = "velodyne"; 82 | pubLaserCloudFiltered.publish(laserCloudFilteredMsg); 83 | 84 | sensor_msgs::PointCloud2 edgePointsMsg; 85 | pcl::toROSMsg(*pointcloud_edge, edgePointsMsg); 86 | edgePointsMsg.header.stamp = pointcloud_time; 87 | edgePointsMsg.header.frame_id = "velodyne"; 88 | pubEdgePoints.publish(edgePointsMsg); 89 | 90 | 91 | sensor_msgs::PointCloud2 surfPointsMsg; 92 | pcl::toROSMsg(*pointcloud_surf, surfPointsMsg); 93 | surfPointsMsg.header.stamp = pointcloud_time; 94 | surfPointsMsg.header.frame_id = "velodyne"; 95 | pubSurfPoints.publish(surfPointsMsg); 96 | 97 | } 98 | //sleep 2 ms every time 99 | std::chrono::milliseconds dura(2); 100 | std::this_thread::sleep_for(dura); 101 | } 102 | } 103 | 104 | int main(int argc, char **argv) 105 | { 106 | ros::init(argc, argv, "main"); 107 | ros::NodeHandle nh; 108 | 109 | int scan_line = 64; 110 | double vertical_angle = 2.0; 111 | double scan_period= 0.1; 112 | double max_dis = 60.0; 113 | double min_dis = 2.0; 114 | 115 | nh.getParam("/scan_period", scan_period); 116 | nh.getParam("/vertical_angle", vertical_angle); 117 | nh.getParam("/max_dis", max_dis); 118 | nh.getParam("/min_dis", min_dis); 119 | nh.getParam("/scan_line", scan_line); 120 | 121 | lidar_param.setScanPeriod(scan_period); 122 | lidar_param.setVerticalAngle(vertical_angle); 123 | lidar_param.setLines(scan_line); 124 | lidar_param.setMaxDistance(max_dis); 125 | lidar_param.setMinDistance(min_dis); 126 | 127 | laserProcessing.init(lidar_param); 128 | 129 | ros::Subscriber subLaserCloud = nh.subscribe("/velodyne_points", 100, velodyneHandler); 130 | 131 | pubLaserCloudFiltered = nh.advertise("/velodyne_points_filtered", 100); 132 | 133 | pubEdgePoints = nh.advertise("/laser_cloud_edge", 100); 134 | 135 | pubSurfPoints = nh.advertise("/laser_cloud_surf", 100); 136 | 137 | std::thread laser_processing_process{laser_processing}; 138 | 139 | ros::spin(); 140 | 141 | return 0; 142 | } 143 | -------------------------------------------------------------------------------- /src/lidar.cpp: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #include "lidar.h" 6 | 7 | 8 | lidar::Lidar::Lidar(){ 9 | 10 | } 11 | 12 | 13 | void lidar::Lidar::setLines(double num_lines_in){ 14 | num_lines=num_lines_in; 15 | } 16 | 17 | 18 | void lidar::Lidar::setVerticalAngle(double vertical_angle_in){ 19 | vertical_angle = vertical_angle_in; 20 | } 21 | 22 | 23 | void lidar::Lidar::setVerticalResolution(double vertical_angle_resolution_in){ 24 | vertical_angle_resolution = vertical_angle_resolution_in; 25 | } 26 | 27 | 28 | void lidar::Lidar::setScanPeriod(double scan_period_in){ 29 | scan_period = scan_period_in; 30 | } 31 | 32 | 33 | void lidar::Lidar::setMaxDistance(double max_distance_in){ 34 | max_distance = max_distance_in; 35 | } 36 | 37 | void lidar::Lidar::setMinDistance(double min_distance_in){ 38 | min_distance = min_distance_in; 39 | } -------------------------------------------------------------------------------- /src/lidarOptimization.cpp: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #include "lidarOptimization.h" 6 | 7 | EdgeAnalyticCostFunction::EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_) 8 | : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_){ 9 | 10 | } 11 | 12 | bool EdgeAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 13 | { 14 | 15 | Eigen::Map q_last_curr(parameters[0]); 16 | Eigen::Map t_last_curr(parameters[0] + 4); 17 | Eigen::Vector3d lp; 18 | lp = q_last_curr * curr_point + t_last_curr; 19 | 20 | Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b); 21 | Eigen::Vector3d de = last_point_a - last_point_b; 22 | double de_norm = de.norm(); 23 | residuals[0] = nu.norm()/de_norm; 24 | 25 | if(jacobians != NULL) 26 | { 27 | if(jacobians[0] != NULL) 28 | { 29 | Eigen::Matrix3d skew_lp = skew(lp); 30 | Eigen::Matrix dp_by_se3; 31 | dp_by_se3.block<3,3>(0,0) = -skew_lp; 32 | (dp_by_se3.block<3,3>(0, 3)).setIdentity(); 33 | Eigen::Map > J_se3(jacobians[0]); 34 | J_se3.setZero(); 35 | Eigen::Matrix3d skew_de = skew(de); 36 | J_se3.block<1,6>(0,0) = - nu.transpose() / nu.norm() * skew_de * dp_by_se3/de_norm; 37 | 38 | } 39 | } 40 | 41 | return true; 42 | 43 | } 44 | 45 | 46 | SurfNormAnalyticCostFunction::SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_) 47 | : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), negative_OA_dot_norm(negative_OA_dot_norm_){ 48 | 49 | } 50 | 51 | bool SurfNormAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 52 | { 53 | Eigen::Map q_w_curr(parameters[0]); 54 | Eigen::Map t_w_curr(parameters[0] + 4); 55 | Eigen::Vector3d point_w = q_w_curr * curr_point + t_w_curr; 56 | residuals[0] = plane_unit_norm.dot(point_w) + negative_OA_dot_norm; 57 | 58 | if(jacobians != NULL) 59 | { 60 | if(jacobians[0] != NULL) 61 | { 62 | Eigen::Matrix3d skew_point_w = skew(point_w); 63 | Eigen::Matrix dp_by_se3; 64 | dp_by_se3.block<3,3>(0,0) = -skew_point_w; 65 | (dp_by_se3.block<3,3>(0, 3)).setIdentity(); 66 | Eigen::Map > J_se3(jacobians[0]); 67 | J_se3.setZero(); 68 | J_se3.block<1,6>(0,0) = plane_unit_norm.transpose() * dp_by_se3; 69 | 70 | } 71 | } 72 | return true; 73 | 74 | } 75 | 76 | #if CERES_VERSION_MAJOR >= 3 || (CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1) 77 | bool PoseSE3Parameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 78 | { 79 | Eigen::Map trans(x + 4); 80 | 81 | Eigen::Quaterniond delta_q; 82 | Eigen::Vector3d delta_t; 83 | getTransformFromSe3(Eigen::Map>(delta), delta_q, delta_t); 84 | Eigen::Map quater(x); 85 | Eigen::Map quater_plus(x_plus_delta); 86 | Eigen::Map trans_plus(x_plus_delta + 4); 87 | 88 | quater_plus = delta_q * quater; 89 | trans_plus = delta_q * trans + delta_t; 90 | return true; 91 | } 92 | 93 | bool PoseSE3Parameterization::Minus(const double *x, const double *delta, double *x_minus_delta) const 94 | { 95 | Eigen::Map trans(x + 4); 96 | 97 | Eigen::Quaterniond delta_q; 98 | Eigen::Vector3d delta_t; 99 | getTransformFromSe3(Eigen::Map>(delta), delta_q, delta_t); 100 | Eigen::Map quater(x); 101 | Eigen::Map quater_minus(x_minus_delta); 102 | Eigen::Map trans_minus(x_minus_delta + 4); 103 | 104 | quater_minus = delta_q.inverse() * quater; 105 | trans_minus = delta_q.inverse() * trans - delta_t; 106 | return true; 107 | } 108 | 109 | bool PoseSE3Parameterization::PlusJacobian(const double *x, double *jacobian) const 110 | { 111 | Eigen::Map> j(jacobian); 112 | (j.topRows(6)).setIdentity(); 113 | (j.bottomRows(1)).setZero(); 114 | return true; 115 | } 116 | 117 | bool PoseSE3Parameterization::MinusJacobian(const double *x, double *jacobian) const 118 | { 119 | Eigen::Map> j(jacobian); 120 | (j.topRows(6)).setIdentity(); 121 | (j.bottomRows(1)).setZero(); 122 | return true; 123 | } 124 | 125 | #else 126 | 127 | bool PoseSE3Parameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 128 | { 129 | Eigen::Map trans(x + 4); 130 | 131 | Eigen::Quaterniond delta_q; 132 | Eigen::Vector3d delta_t; 133 | getTransformFromSe3(Eigen::Map>(delta), delta_q, delta_t); 134 | Eigen::Map quater(x); 135 | Eigen::Map quater_plus(x_plus_delta); 136 | Eigen::Map trans_plus(x_plus_delta + 4); 137 | 138 | quater_plus = delta_q * quater; 139 | trans_plus = delta_q * trans + delta_t; 140 | 141 | return true; 142 | } 143 | 144 | bool PoseSE3Parameterization::ComputeJacobian(const double *x, double *jacobian) const 145 | { 146 | Eigen::Map> j(jacobian); 147 | (j.topRows(6)).setIdentity(); 148 | (j.bottomRows(1)).setZero(); 149 | 150 | return true; 151 | } 152 | 153 | #endif 154 | 155 | 156 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t){ 157 | Eigen::Vector3d omega(se3.data()); 158 | Eigen::Vector3d upsilon(se3.data()+3); 159 | Eigen::Matrix3d Omega = skew(omega); 160 | 161 | double theta = omega.norm(); 162 | double half_theta = 0.5*theta; 163 | 164 | double imag_factor; 165 | double real_factor = cos(half_theta); 166 | if(theta<1e-10) 167 | { 168 | double theta_sq = theta*theta; 169 | double theta_po4 = theta_sq*theta_sq; 170 | imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4; 171 | } 172 | else 173 | { 174 | double sin_half_theta = sin(half_theta); 175 | imag_factor = sin_half_theta/theta; 176 | } 177 | 178 | q = Eigen::Quaterniond(real_factor, imag_factor*omega.x(), imag_factor*omega.y(), imag_factor*omega.z()); 179 | 180 | 181 | Eigen::Matrix3d J; 182 | if (theta<1e-10) 183 | { 184 | J = q.matrix(); 185 | } 186 | else 187 | { 188 | Eigen::Matrix3d Omega2 = Omega*Omega; 189 | J = (Eigen::Matrix3d::Identity() + (1-cos(theta))/(theta*theta)*Omega + (theta-sin(theta))/(pow(theta,3))*Omega2); 190 | } 191 | 192 | t = J*upsilon; 193 | } 194 | 195 | Eigen::Matrix skew(Eigen::Matrix& mat_in){ 196 | Eigen::Matrix skew_mat; 197 | skew_mat.setZero(); 198 | skew_mat(0,1) = -mat_in(2); 199 | skew_mat(0,2) = mat_in(1); 200 | skew_mat(1,2) = -mat_in(0); 201 | skew_mat(1,0) = mat_in(2); 202 | skew_mat(2,0) = -mat_in(1); 203 | skew_mat(2,1) = mat_in(0); 204 | return skew_mat; 205 | } -------------------------------------------------------------------------------- /src/odomEstimationClass.cpp: -------------------------------------------------------------------------------- 1 | // Author of ISCLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #include "odomEstimationClass.h" 6 | 7 | void OdomEstimationClass::init(lidar::Lidar lidar_param, double map_resolution){ 8 | //init local map 9 | laserCloudCornerMap = pcl::PointCloud::Ptr(new pcl::PointCloud()); 10 | laserCloudSurfMap = pcl::PointCloud::Ptr(new pcl::PointCloud()); 11 | 12 | //downsampling size 13 | downSizeFilterEdge.setLeafSize(map_resolution, map_resolution, map_resolution); 14 | downSizeFilterSurf.setLeafSize(map_resolution * 2, map_resolution * 2, map_resolution * 2); 15 | 16 | //kd-tree 17 | kdtreeEdgeMap = pcl::KdTreeFLANN::Ptr(new pcl::KdTreeFLANN()); 18 | kdtreeSurfMap = pcl::KdTreeFLANN::Ptr(new pcl::KdTreeFLANN()); 19 | 20 | odom = Eigen::Isometry3d::Identity(); 21 | last_odom = Eigen::Isometry3d::Identity(); 22 | optimization_count=2; 23 | } 24 | 25 | void OdomEstimationClass::initMapWithPoints(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in){ 26 | *laserCloudCornerMap += *edge_in; 27 | *laserCloudSurfMap += *surf_in; 28 | optimization_count=12; 29 | } 30 | 31 | 32 | void OdomEstimationClass::updatePointsToMap(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in){ 33 | 34 | if(optimization_count>2) 35 | optimization_count--; 36 | 37 | Eigen::Isometry3d odom_prediction = odom * (last_odom.inverse() * odom); 38 | last_odom = odom; 39 | odom = odom_prediction; 40 | 41 | q_w_curr = Eigen::Quaterniond(odom.rotation()); 42 | t_w_curr = odom.translation(); 43 | 44 | pcl::PointCloud::Ptr downsampledEdgeCloud(new pcl::PointCloud()); 45 | pcl::PointCloud::Ptr downsampledSurfCloud(new pcl::PointCloud()); 46 | downSamplingToMap(edge_in,downsampledEdgeCloud,surf_in,downsampledSurfCloud); 47 | //ROS_WARN("point nyum%d,%d",(int)downsampledEdgeCloud->points.size(), (int)downsampledSurfCloud->points.size()); 48 | if(laserCloudCornerMap->points.size()>10 && laserCloudSurfMap->points.size()>50){ 49 | kdtreeEdgeMap->setInputCloud(laserCloudCornerMap); 50 | kdtreeSurfMap->setInputCloud(laserCloudSurfMap); 51 | 52 | for (int iterCount = 0; iterCount < optimization_count; iterCount++){ 53 | ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); 54 | ceres::Problem::Options problem_options; 55 | ceres::Problem problem(problem_options); 56 | 57 | problem.AddParameterBlock(parameters, 7, new PoseSE3Parameterization()); 58 | 59 | addEdgeCostFactor(downsampledEdgeCloud,laserCloudCornerMap,problem,loss_function); 60 | addSurfCostFactor(downsampledSurfCloud,laserCloudSurfMap,problem,loss_function); 61 | 62 | ceres::Solver::Options options; 63 | options.linear_solver_type = ceres::DENSE_QR; 64 | options.max_num_iterations = 4; 65 | options.minimizer_progress_to_stdout = false; 66 | options.check_gradients = false; 67 | options.gradient_check_relative_precision = 1e-4; 68 | ceres::Solver::Summary summary; 69 | 70 | ceres::Solve(options, &problem, &summary); 71 | 72 | } 73 | }else{ 74 | printf("not enough points in map to associate, map error"); 75 | } 76 | odom = Eigen::Isometry3d::Identity(); 77 | odom.linear() = q_w_curr.toRotationMatrix(); 78 | odom.translation() = t_w_curr; 79 | addPointsToMap(downsampledEdgeCloud,downsampledSurfCloud); 80 | 81 | } 82 | 83 | void OdomEstimationClass::pointAssociateToMap(pcl::PointXYZI const *const pi, pcl::PointXYZI *const po) 84 | { 85 | Eigen::Vector3d point_curr(pi->x, pi->y, pi->z); 86 | Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr; 87 | po->x = point_w.x(); 88 | po->y = point_w.y(); 89 | po->z = point_w.z(); 90 | po->intensity = pi->intensity; 91 | //po->intensity = 1.0; 92 | } 93 | 94 | void OdomEstimationClass::downSamplingToMap(const pcl::PointCloud::Ptr& edge_pc_in, pcl::PointCloud::Ptr& edge_pc_out, const pcl::PointCloud::Ptr& surf_pc_in, pcl::PointCloud::Ptr& surf_pc_out){ 95 | downSizeFilterEdge.setInputCloud(edge_pc_in); 96 | downSizeFilterEdge.filter(*edge_pc_out); 97 | downSizeFilterSurf.setInputCloud(surf_pc_in); 98 | downSizeFilterSurf.filter(*surf_pc_out); 99 | } 100 | 101 | void OdomEstimationClass::addEdgeCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function){ 102 | int corner_num=0; 103 | for (int i = 0; i < (int)pc_in->points.size(); i++) 104 | { 105 | pcl::PointXYZI point_temp; 106 | pointAssociateToMap(&(pc_in->points[i]), &point_temp); 107 | 108 | std::vector pointSearchInd; 109 | std::vector pointSearchSqDis; 110 | kdtreeEdgeMap->nearestKSearch(point_temp, 5, pointSearchInd, pointSearchSqDis); 111 | if (pointSearchSqDis[4] < 1.0) 112 | { 113 | std::vector nearCorners; 114 | Eigen::Vector3d center(0, 0, 0); 115 | for (int j = 0; j < 5; j++) 116 | { 117 | Eigen::Vector3d tmp(map_in->points[pointSearchInd[j]].x, 118 | map_in->points[pointSearchInd[j]].y, 119 | map_in->points[pointSearchInd[j]].z); 120 | center = center + tmp; 121 | nearCorners.push_back(tmp); 122 | } 123 | center = center / 5.0; 124 | 125 | Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero(); 126 | for (int j = 0; j < 5; j++) 127 | { 128 | Eigen::Matrix tmpZeroMean = nearCorners[j] - center; 129 | covMat = covMat + tmpZeroMean * tmpZeroMean.transpose(); 130 | } 131 | 132 | Eigen::SelfAdjointEigenSolver saes(covMat); 133 | 134 | Eigen::Vector3d unit_direction = saes.eigenvectors().col(2); 135 | Eigen::Vector3d curr_point(pc_in->points[i].x, pc_in->points[i].y, pc_in->points[i].z); 136 | if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1]) 137 | { 138 | Eigen::Vector3d point_on_line = center; 139 | Eigen::Vector3d point_a, point_b; 140 | point_a = 0.1 * unit_direction + point_on_line; 141 | point_b = -0.1 * unit_direction + point_on_line; 142 | 143 | ceres::CostFunction *cost_function = new EdgeAnalyticCostFunction(curr_point, point_a, point_b); 144 | problem.AddResidualBlock(cost_function, loss_function, parameters); 145 | corner_num++; 146 | } 147 | } 148 | } 149 | if(corner_num<20){ 150 | printf("not enough correct points"); 151 | } 152 | 153 | } 154 | 155 | void OdomEstimationClass::addSurfCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function){ 156 | int surf_num=0; 157 | for (int i = 0; i < (int)pc_in->points.size(); i++) 158 | { 159 | pcl::PointXYZI point_temp; 160 | pointAssociateToMap(&(pc_in->points[i]), &point_temp); 161 | std::vector pointSearchInd; 162 | std::vector pointSearchSqDis; 163 | kdtreeSurfMap->nearestKSearch(point_temp, 5, pointSearchInd, pointSearchSqDis); 164 | 165 | Eigen::Matrix matA0; 166 | Eigen::Matrix matB0 = -1 * Eigen::Matrix::Ones(); 167 | if (pointSearchSqDis[4] < 1.0) 168 | { 169 | 170 | for (int j = 0; j < 5; j++) 171 | { 172 | matA0(j, 0) = map_in->points[pointSearchInd[j]].x; 173 | matA0(j, 1) = map_in->points[pointSearchInd[j]].y; 174 | matA0(j, 2) = map_in->points[pointSearchInd[j]].z; 175 | } 176 | // find the norm of plane 177 | Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0); 178 | double negative_OA_dot_norm = 1 / norm.norm(); 179 | norm.normalize(); 180 | 181 | bool planeValid = true; 182 | for (int j = 0; j < 5; j++) 183 | { 184 | // if OX * n > 0.2, then plane is not fit well 185 | if (fabs(norm(0) * map_in->points[pointSearchInd[j]].x + 186 | norm(1) * map_in->points[pointSearchInd[j]].y + 187 | norm(2) * map_in->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2) 188 | { 189 | planeValid = false; 190 | break; 191 | } 192 | } 193 | Eigen::Vector3d curr_point(pc_in->points[i].x, pc_in->points[i].y, pc_in->points[i].z); 194 | if (planeValid) 195 | { 196 | ceres::CostFunction *cost_function = new SurfNormAnalyticCostFunction(curr_point, norm, negative_OA_dot_norm); 197 | problem.AddResidualBlock(cost_function, loss_function, parameters); 198 | 199 | surf_num++; 200 | } 201 | } 202 | 203 | } 204 | if(surf_num<20){ 205 | printf("not enough correct points"); 206 | } 207 | 208 | } 209 | 210 | void OdomEstimationClass::addPointsToMap(const pcl::PointCloud::Ptr& downsampledEdgeCloud, const pcl::PointCloud::Ptr& downsampledSurfCloud){ 211 | //储存点 212 | for (int i = 0; i < (int)downsampledEdgeCloud->points.size(); i++) 213 | { 214 | pcl::PointXYZI point_temp; 215 | pointAssociateToMap(&downsampledEdgeCloud->points[i], &point_temp); 216 | laserCloudCornerMap->push_back(point_temp); 217 | } 218 | 219 | for (int i = 0; i < (int)downsampledSurfCloud->points.size(); i++) 220 | { 221 | pcl::PointXYZI point_temp; 222 | pointAssociateToMap(&downsampledSurfCloud->points[i], &point_temp); 223 | laserCloudSurfMap->push_back(point_temp); 224 | } 225 | 226 | double x_min = +odom.translation().x()-100; 227 | double y_min = +odom.translation().y()-100; 228 | double z_min = +odom.translation().z()-100; 229 | double x_max = +odom.translation().x()+100; 230 | double y_max = +odom.translation().y()+100; 231 | double z_max = +odom.translation().z()+100; 232 | 233 | //ROS_INFO("size : %f,%f,%f,%f,%f,%f", x_min, y_min, z_min,x_max, y_max, z_max); 234 | cropBoxFilter.setMin(Eigen::Vector4f(x_min, y_min, z_min, 1.0)); 235 | cropBoxFilter.setMax(Eigen::Vector4f(x_max, y_max, z_max, 1.0)); 236 | cropBoxFilter.setNegative(false); 237 | 238 | pcl::PointCloud::Ptr tmpCorner(new pcl::PointCloud()); 239 | pcl::PointCloud::Ptr tmpSurf(new pcl::PointCloud()); 240 | cropBoxFilter.setInputCloud(laserCloudSurfMap); 241 | cropBoxFilter.filter(*tmpSurf); 242 | cropBoxFilter.setInputCloud(laserCloudCornerMap); 243 | cropBoxFilter.filter(*tmpCorner); 244 | 245 | downSizeFilterSurf.setInputCloud(tmpSurf); 246 | downSizeFilterSurf.filter(*laserCloudSurfMap); 247 | downSizeFilterEdge.setInputCloud(tmpCorner); 248 | downSizeFilterEdge.filter(*laserCloudCornerMap); 249 | 250 | } 251 | 252 | void OdomEstimationClass::getMap(pcl::PointCloud::Ptr& laserCloudMap){ 253 | 254 | *laserCloudMap += *laserCloudSurfMap; 255 | *laserCloudMap += *laserCloudCornerMap; 256 | } 257 | 258 | OdomEstimationClass::OdomEstimationClass(){ 259 | 260 | } -------------------------------------------------------------------------------- /src/odomEstimationNode.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | //c++ lib 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | //ros lib 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | //pcl lib 21 | #include 22 | #include 23 | #include 24 | 25 | //local lib 26 | #include "lidar.h" 27 | #include "odomEstimationClass.h" 28 | 29 | OdomEstimationClass odomEstimation; 30 | std::mutex mutex_lock; 31 | std::queue pointCloudEdgeBuf; 32 | std::queue pointCloudSurfBuf; 33 | std::queue pointCloudBuf; 34 | lidar::Lidar lidar_param; 35 | 36 | ros::Publisher pubLaserOdometry; 37 | void velodyneSurfHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 38 | { 39 | mutex_lock.lock(); 40 | pointCloudSurfBuf.push(laserCloudMsg); 41 | mutex_lock.unlock(); 42 | } 43 | void velodyneEdgeHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 44 | { 45 | mutex_lock.lock(); 46 | pointCloudEdgeBuf.push(laserCloudMsg); 47 | mutex_lock.unlock(); 48 | } 49 | void velodyneHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 50 | { 51 | mutex_lock.lock(); 52 | pointCloudBuf.push(laserCloudMsg); 53 | mutex_lock.unlock(); 54 | } 55 | 56 | bool is_odom_inited = false; 57 | double total_time =0; 58 | int total_frame=0; 59 | void odom_estimation(){ 60 | while(1){ 61 | if(!pointCloudEdgeBuf.empty() && !pointCloudSurfBuf.empty()&& !pointCloudBuf.empty()){ 62 | 63 | //read data 64 | mutex_lock.lock(); 65 | if(!pointCloudBuf.empty() && (pointCloudBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period || pointCloudBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period)){ 66 | ROS_WARN("time stamp unaligned error and odom discarded, pls check your data --> odom correction"); 67 | pointCloudBuf.pop(); 68 | mutex_lock.unlock(); 69 | continue; 70 | } 71 | 72 | if(!pointCloudSurfBuf.empty() && (pointCloudSurfBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period || pointCloudSurfBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period)){ 73 | pointCloudSurfBuf.pop(); 74 | ROS_INFO("time stamp unaligned with extra point cloud, pls check your data --> odom correction"); 75 | mutex_lock.unlock(); 76 | continue; 77 | } 78 | 79 | if(!pointCloudEdgeBuf.empty() && (pointCloudEdgeBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period || pointCloudEdgeBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period)){ 80 | pointCloudEdgeBuf.pop(); 81 | ROS_INFO("time stamp unaligned with extra point cloud, pls check your data --> odom correction"); 82 | mutex_lock.unlock(); 83 | continue; 84 | } 85 | //if time aligned 86 | 87 | pcl::PointCloud::Ptr pointcloud_surf_in(new pcl::PointCloud()); 88 | pcl::PointCloud::Ptr pointcloud_edge_in(new pcl::PointCloud()); 89 | pcl::PointCloud::Ptr pointcloud_in(new pcl::PointCloud()); 90 | pcl::fromROSMsg(*pointCloudEdgeBuf.front(), *pointcloud_edge_in); 91 | pcl::fromROSMsg(*pointCloudSurfBuf.front(), *pointcloud_surf_in); 92 | pcl::fromROSMsg(*pointCloudBuf.front(), *pointcloud_in); 93 | ros::Time pointcloud_time = (pointCloudBuf.front())->header.stamp; 94 | pointCloudEdgeBuf.pop(); 95 | pointCloudSurfBuf.pop(); 96 | pointCloudBuf.pop(); 97 | mutex_lock.unlock(); 98 | 99 | if(is_odom_inited == false){ 100 | odomEstimation.initMapWithPoints(pointcloud_edge_in, pointcloud_surf_in); 101 | is_odom_inited = true; 102 | ROS_INFO("odom inited"); 103 | }else{ 104 | std::chrono::time_point start, end; 105 | start = std::chrono::system_clock::now(); 106 | odomEstimation.updatePointsToMap(pointcloud_edge_in, pointcloud_surf_in); 107 | end = std::chrono::system_clock::now(); 108 | std::chrono::duration elapsed_seconds = end - start; 109 | total_frame++; 110 | float time_temp = elapsed_seconds.count() * 1000; 111 | total_time+=time_temp; 112 | //ROS_INFO("average odom estimation time %f ms \n \n", total_time/total_frame); 113 | } 114 | 115 | 116 | 117 | Eigen::Quaterniond q_current(odomEstimation.odom.rotation()); 118 | //q_current.normalize(); 119 | Eigen::Vector3d t_current = odomEstimation.odom.translation(); 120 | 121 | static tf::TransformBroadcaster br; 122 | tf::Transform transform; 123 | transform.setOrigin( tf::Vector3(t_current.x(), t_current.y(), t_current.z()) ); 124 | tf::Quaternion q(q_current.x(),q_current.y(),q_current.z(),q_current.w()); 125 | transform.setRotation(q); 126 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "aft_mapped")); 127 | 128 | // publish odometry 129 | nav_msgs::Odometry laserOdometry; 130 | laserOdometry.header.frame_id = "world"; 131 | laserOdometry.child_frame_id = "aft_mapped"; 132 | laserOdometry.header.stamp = pointcloud_time; 133 | laserOdometry.pose.pose.orientation.x = q_current.x(); 134 | laserOdometry.pose.pose.orientation.y = q_current.y(); 135 | laserOdometry.pose.pose.orientation.z = q_current.z(); 136 | laserOdometry.pose.pose.orientation.w = q_current.w(); 137 | laserOdometry.pose.pose.position.x = t_current.x(); 138 | laserOdometry.pose.pose.position.y = t_current.y(); 139 | laserOdometry.pose.pose.position.z = t_current.z(); 140 | pubLaserOdometry.publish(laserOdometry); 141 | 142 | } 143 | //sleep 2 ms every time 144 | std::chrono::milliseconds dura(2); 145 | std::this_thread::sleep_for(dura); 146 | } 147 | } 148 | 149 | int main(int argc, char **argv) 150 | { 151 | ros::init(argc, argv, "main"); 152 | ros::NodeHandle nh; 153 | 154 | int scan_line = 64; 155 | double vertical_angle = 2.0; 156 | double scan_period= 0.1; 157 | double max_dis = 60.0; 158 | double min_dis = 2.0; 159 | double map_resolution = 0.4; 160 | 161 | nh.getParam("/scan_period", scan_period); 162 | nh.getParam("/vertical_angle", vertical_angle); 163 | nh.getParam("/max_dis", max_dis); 164 | nh.getParam("/min_dis", min_dis); 165 | nh.getParam("/scan_line", scan_line); 166 | nh.getParam("/map_resolution", map_resolution); 167 | 168 | lidar_param.setScanPeriod(scan_period); 169 | lidar_param.setVerticalAngle(vertical_angle); 170 | lidar_param.setLines(scan_line); 171 | lidar_param.setMaxDistance(max_dis); 172 | lidar_param.setMinDistance(min_dis); 173 | 174 | odomEstimation.init(lidar_param, map_resolution); 175 | ros::Subscriber subLaserCloud = nh.subscribe("/velodyne_points_filtered", 100, velodyneHandler); 176 | ros::Subscriber subEdgeLaserCloud = nh.subscribe("/laser_cloud_edge", 100, velodyneEdgeHandler); 177 | ros::Subscriber subSurfLaserCloud = nh.subscribe("/laser_cloud_surf", 100, velodyneSurfHandler); 178 | 179 | pubLaserOdometry = nh.advertise("/odom", 100); 180 | std::thread odom_estimation_process{odom_estimation}; 181 | 182 | ros::spin(); 183 | 184 | return 0; 185 | } 186 | --------------------------------------------------------------------------------