├── CMakeLists.txt ├── README.md ├── include └── aloam_velodyne │ ├── common.h │ └── tic_toc.h ├── launch ├── aloam_velodyne_HDL_32.launch ├── aloam_velodyne_HDL_64.launch ├── aloam_velodyne_VLP_16.launch ├── kitti_helper.launch └── play_kitti_dataset.launch ├── package.xml ├── paramter_configuration_for_benchmarks.txt ├── rviz_cfg └── aloam_velodyne.rviz └── src ├── kittiHelper.cpp ├── laserMapping.cpp ├── laserOdometry.cpp ├── lidarFactor.hpp └── scanRegistration.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(light_loam) 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 | image_transport 17 | cv_bridge 18 | tf 19 | ) 20 | 21 | #find_package(Eigen3 REQUIRED) 22 | find_package(PCL 1.10 REQUIRED) 23 | find_package(OpenCV REQUIRED) 24 | find_package(Ceres 2.3 REQUIRED) 25 | FIND_PACKAGE(OpenMP REQUIRED) 26 | if(OPENMP_FOUND) 27 | message("OPENMP FOUND") 28 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 29 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 30 | endif() 31 | include_directories( 32 | include 33 | ${catkin_INCLUDE_DIRS} 34 | ${PCL_INCLUDE_DIRS} 35 | ${CERES_INCLUDE_DIRS} 36 | ${OpenCV_INCLUDE_DIRS} 37 | ${OpenMP_INCLUDE_DIR}) 38 | 39 | catkin_package( 40 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 41 | DEPENDS EIGEN3 PCL 42 | INCLUDE_DIRS include 43 | ) 44 | 45 | 46 | add_executable(ascanRegistration src/scanRegistration.cpp) 47 | target_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 48 | 49 | add_executable(alaserOdometry src/laserOdometry.cpp) 50 | target_link_libraries(alaserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} ${OpenMP_LIBRARY_DIRS}) 51 | 52 | add_executable(alaserMapping src/laserMapping.cpp) 53 | target_link_libraries(alaserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 54 | 55 | add_executable(kittiHelper src/kittiHelper.cpp) 56 | target_link_libraries(kittiHelper ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Light-LOAM 2 | News: Our paper has been accepted by the RA-L journal! 3 | This is the implementation for the Paper ``Light-LOAM: A Lightweight LiDAR Odometry and Mapping based on Graph-Matching''. This code is modified from [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM). 4 | 5 | ## Requirements 6 | * PCL 1.10 7 | * ROS 8 | * Ceres 1.14.x 9 | 10 | ## Introduction 11 | This is the beta version, and the final implementation code is coming soon. The research paper [Light-LOAM: A Lightweight LiDAR Odometry and Mapping based on Graph-Matching](https://arxiv.org/abs/2310.04162) is now availble on arXiv. 12 | 13 | ## Citation 14 | 15 | If you take pieces from our system in your research, please consider citing the [paper](https://arxiv.org/abs/2310.04162): 16 | 17 | ``` 18 | @ARTICLE{10439642, 19 | author={Yi, Shiquan and Lyu, Yang and Hua, Lin and Pan, Quan and Zhao, Chunhui}, 20 | journal={IEEE Robotics and Automation Letters}, 21 | title={Light-LOAM: A Lightweight LiDAR Odometry and Mapping Based on Graph-Matching}, 22 | year={2024}, 23 | volume={9}, 24 | number={4}, 25 | pages={3219-3226}, 26 | keywords={Simultaneous localization and mapping;Feature extraction;Point cloud compression;Reliability;Odometry;Laser radar;Robots;SLAM;localization;data association}, 27 | doi={10.1109/LRA.2024.3367268}} 28 | 29 | ``` 30 | --------- 31 | 32 | ## Acknowledgements 33 | Thanks for [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM). 34 | 35 | 36 | -------------------------------------------------------------------------------- /include/aloam_velodyne/common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | typedef pcl::PointXYZI PointType; 8 | 9 | inline double rad2deg(double radians) 10 | { 11 | return radians * 180.0 / M_PI; 12 | } 13 | 14 | inline double deg2rad(double degrees) 15 | { 16 | return degrees * M_PI / 180.0; 17 | } 18 | 19 | //new add 20 | typedef struct { 21 | int index; 22 | // int tgt_idx; 23 | pcl::PointXYZI src; 24 | pcl::PointXYZI tgt; 25 | // Eigen::Vector3f src_norm; 26 | // Eigen::Vector3f tgt_norm; 27 | // Eigen::Matrix3f covariance_src; 28 | // Eigen::Matrix3f covariance_tgt; 29 | float score; 30 | float s; // optimization weight 31 | }Corre_Match; 32 | 33 | typedef struct 34 | { 35 | int vertex_index; 36 | std::vector connected_vertex_index; 37 | int degree; 38 | }Vertex_Attribute; 39 | 40 | typedef struct{ 41 | int index; 42 | float score; 43 | }Vertex_Vote; 44 | 45 | // bool compare_degree(const Vertex_Attribute ob1, const Vertex_Attribute ob2) 46 | // { 47 | // return ob1.degree > ob2.degree; 48 | // } 49 | 50 | struct compare_score{ 51 | bool operator()(Vertex_Vote const &ob1, Vertex_Vote const &ob2){return ob1.score > ob2.score;} 52 | }; 53 | //new add 54 | -------------------------------------------------------------------------------- /include/aloam_velodyne/tic_toc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | class TicToc 11 | { 12 | public: 13 | TicToc() 14 | { 15 | tic(); 16 | } 17 | 18 | void tic() 19 | { 20 | start = std::chrono::system_clock::now(); 21 | } 22 | 23 | double toc() 24 | { 25 | end = std::chrono::system_clock::now(); 26 | std::chrono::duration elapsed_seconds = end - start; 27 | return elapsed_seconds.count() * 1000; 28 | } 29 | 30 | private: 31 | std::chrono::time_point start, end; 32 | }; 33 | -------------------------------------------------------------------------------- /launch/aloam_velodyne_HDL_32.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 | -------------------------------------------------------------------------------- /launch/aloam_velodyne_HDL_64.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 | -------------------------------------------------------------------------------- /launch/aloam_velodyne_VLP_16.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 | -------------------------------------------------------------------------------- /launch/kitti_helper.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /launch/play_kitti_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | light_loam 4 | 0.0.1 5 | The light version of LOAM 6 | BSD 7 | 8 | Ji Zhang 9 | Ahmet Ceyhun Bilir 10 | 11 | catkin 12 | geometry_msgs 13 | nav_msgs 14 | roscpp 15 | rospy 16 | std_msgs 17 | rosbag 18 | sensor_msgs 19 | tf 20 | image_transport 21 | 22 | geometry_msgs 23 | nav_msgs 24 | sensor_msgs 25 | roscpp 26 | rospy 27 | std_msgs 28 | rosbag 29 | tf 30 | image_transport 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /paramter_configuration_for_benchmarks.txt: -------------------------------------------------------------------------------- 1 | //For A-LOAM： 2 | 3 | //Modified code for KITTI dataset 4 | //Please comment out lines 189 to 199 in the scanRegistration.cpp file and then insert a new code segment in that space. 5 | 6 | /* 7 | if (angle >= -8.83) 8 | scanID = int((2 - angle) * 3.0 + 0.5); 9 | else 10 | scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5); 11 | 12 | // use [0 50] > 50 remove outlies 13 | if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0) 14 | { 15 | count--; 16 | continue; 17 | } 18 | 19 | */ 20 | float upBound = 2 21 | float lowerBound = -24.9; 22 | float _factor = (N_SCANS-1) / (upBound - lowerBound); 23 | scanID = int((angle - lowerBound) * _factor + 0.5); 24 | if (scanID >= N_SCANS || scanID < 0) 25 | { 26 | count--; 27 | continue; 28 | } 29 | 30 | //Modified code for M2DGR dataset 31 | //Please comment out line 180 in the scanRegistration.cpp file and then insert a new code segment in that space. 32 | // scanID = int((angle + 92.0/3.0) * 3.0 / 4.0); 33 | float upBound = 15 34 | float lowerBound = -25; 35 | float _factor = (N_SCANS-1) / (upBound - lowerBound); 36 | scanID = int((angle - lowerBound) * _factor + 0.5); 37 | 38 | 39 | //For LeGO-LOAM, reconmend 40 | //Reference code：https://github.com/Mitchell-Lee-93/kitti-lego-loam/tree/master 41 | 42 | //Modified code for KITTI dataset 43 | //Please comment out the original configuration for LIDAR in utility.h file and then insert a new code segment in that space. 44 | //Vel 64 45 | extern const int N_SCAN = 64; 46 | extern const int Horizon_SCAN = 1800; 47 | extern const float ang_res_x = 0.2; 48 | extern const float ang_res_y = 0.427; 49 | extern const float ang_bottom = 24.9; 50 | extern const int groundScanInd = 50; 51 | 52 | 53 | //Modified code for M2DGR dataset 54 | //Please comment out the original configuration for LIDAR in utility.h file and then insert a new code segment in that space. 55 | // VPL-32C 56 | extern const int N_SCAN = 32; 57 | extern const int Horizon_SCAN = 1800; 58 | extern const float ang_res_x = 360.0/float(Horizon_SCAN); 59 | extern const float ang_res_y = 40/float(N_SCAN-1); 60 | extern const float ang_bottom = 25; 61 | extern const int groundScanInd = 23; 62 | -------------------------------------------------------------------------------- /rviz_cfg/aloam_velodyne.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /odometry1/odomPath1 10 | - /odometry1/PointCloud21 11 | Splitter Ratio: 0.5 12 | Tree Height: 567 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.588679016 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: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: false 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 160 51 | Reference Frame: 52 | Value: false 53 | - Class: rviz/Axes 54 | Enabled: true 55 | Length: 1 56 | Name: Axes 57 | Radius: 0.100000001 58 | Reference Frame: 59 | Value: true 60 | - Class: rviz/Group 61 | Displays: 62 | - Alpha: 1 63 | Buffer Length: 1 64 | Class: rviz/Path 65 | Color: 255; 170; 1 66 | Enabled: false 67 | Head Diameter: 0.300000012 68 | Head Length: 0.200000003 69 | Length: 0.300000012 70 | Line Style: Lines 71 | Line Width: 0.0299999993 72 | Name: gtPathlc 73 | Offset: 74 | X: 0 75 | Y: 0 76 | Z: 0 77 | Pose Color: 255; 85; 255 78 | Pose Style: None 79 | Radius: 0.0299999993 80 | Shaft Diameter: 0.100000001 81 | Shaft Length: 0.100000001 82 | Topic: /path_gt 83 | Unreliable: false 84 | Value: false 85 | - Alpha: 1 86 | Buffer Length: 1 87 | Class: rviz/Path 88 | Color: 255; 170; 0 89 | Enabled: true 90 | Head Diameter: 0.300000012 91 | Head Length: 0.200000003 92 | Length: 0.300000012 93 | Line Style: Lines 94 | Line Width: 0.0299999993 95 | Name: gtPathLidar 96 | Offset: 97 | X: 0 98 | Y: 0 99 | Z: 0 100 | Pose Color: 255; 85; 255 101 | Pose Style: None 102 | Radius: 0.0299999993 103 | Shaft Diameter: 0.100000001 104 | Shaft Length: 0.100000001 105 | Topic: /path_gt_lidar 106 | Unreliable: false 107 | Value: true 108 | Enabled: false 109 | Name: GT 110 | - Class: rviz/Group 111 | Displays: 112 | - Alpha: 1 113 | Buffer Length: 1 114 | Class: rviz/Path 115 | Color: 255; 85; 0 116 | Enabled: true 117 | Head Diameter: 0.300000012 118 | Head Length: 0.200000003 119 | Length: 0.300000012 120 | Line Style: Lines 121 | Line Width: 0.100000001 122 | Name: odomPath 123 | Offset: 124 | X: 0 125 | Y: 0 126 | Z: 0 127 | Pose Color: 255; 85; 255 128 | Pose Style: None 129 | Radius: 0.0299999993 130 | Shaft Diameter: 0.100000001 131 | Shaft Length: 0.100000001 132 | Topic: /laser_odom_path 133 | Unreliable: false 134 | Value: true 135 | - Alpha: 1 136 | Autocompute Intensity Bounds: true 137 | Autocompute Value Bounds: 138 | Max Value: 10 139 | Min Value: -10 140 | Value: true 141 | Axis: Z 142 | Channel Name: intensity 143 | Class: rviz/PointCloud2 144 | Color: 255; 255; 255 145 | Color Transformer: FlatColor 146 | Decay Time: 0 147 | Enabled: false 148 | Invert Rainbow: false 149 | Max Color: 255; 255; 255 150 | Max Intensity: 63.1004791 151 | Min Color: 0; 0; 0 152 | Min Intensity: -0.00678629428 153 | Name: PointCloud2 154 | Position Transformer: XYZ 155 | Queue Size: 10 156 | Selectable: true 157 | Size (Pixels): 3 158 | Size (m): 0.0500000007 159 | Style: Flat Squares 160 | Topic: /velodyne_cloud_2 161 | Unreliable: false 162 | Use Fixed Frame: true 163 | Use rainbow: true 164 | Value: false 165 | Enabled: false 166 | Name: odometry 167 | - Class: rviz/Group 168 | Displays: 169 | - Alpha: 1 170 | Buffer Length: 1 171 | Class: rviz/Path 172 | Color: 25; 255; 0 173 | Enabled: true 174 | Head Diameter: 0.300000012 175 | Head Length: 0.200000003 176 | Length: 0.300000012 177 | Line Style: Lines 178 | Line Width: 0.0299999993 179 | Name: mappingPath 180 | Offset: 181 | X: 0 182 | Y: 0 183 | Z: 0 184 | Pose Color: 255; 85; 255 185 | Pose Style: None 186 | Radius: 0.0299999993 187 | Shaft Diameter: 0.100000001 188 | Shaft Length: 0.100000001 189 | Topic: /aft_mapped_path 190 | Unreliable: false 191 | Value: true 192 | - Alpha: 1 193 | Autocompute Intensity Bounds: true 194 | Autocompute Value Bounds: 195 | Max Value: 10 196 | Min Value: -10 197 | Value: true 198 | Axis: Z 199 | Channel Name: intensity 200 | Class: rviz/PointCloud2 201 | Color: 255; 255; 255 202 | Color Transformer: FlatColor 203 | Decay Time: 0 204 | Enabled: true 205 | Invert Rainbow: false 206 | Max Color: 255; 255; 255 207 | Max Intensity: -999999 208 | Min Color: 0; 0; 0 209 | Min Intensity: 999999 210 | Name: allMapCloud 211 | Position Transformer: XYZ 212 | Queue Size: 10 213 | Selectable: true 214 | Size (Pixels): 3 215 | Size (m): 0.0500000007 216 | Style: Flat Squares 217 | Topic: /laser_cloud_map 218 | Unreliable: false 219 | Use Fixed Frame: true 220 | Use rainbow: true 221 | Value: true 222 | - Alpha: 1 223 | Autocompute Intensity Bounds: true 224 | Autocompute Value Bounds: 225 | Max Value: 10 226 | Min Value: -10 227 | Value: true 228 | Axis: Z 229 | Channel Name: intensity 230 | Class: rviz/PointCloud2 231 | Color: 255; 255; 255 232 | Color Transformer: FlatColor 233 | Decay Time: 0 234 | Enabled: true 235 | Invert Rainbow: false 236 | Max Color: 255; 255; 255 237 | Max Intensity: 15 238 | Min Color: 0; 0; 0 239 | Min Intensity: 0 240 | Name: surround 241 | Position Transformer: XYZ 242 | Queue Size: 1 243 | Selectable: true 244 | Size (Pixels): 1 245 | Size (m): 0.0500000007 246 | Style: Squares 247 | Topic: /laser_cloud_surround 248 | Unreliable: false 249 | Use Fixed Frame: true 250 | Use rainbow: true 251 | Value: true 252 | - Alpha: 1 253 | Autocompute Intensity Bounds: true 254 | Autocompute Value Bounds: 255 | Max Value: 10 256 | Min Value: -10 257 | Value: true 258 | Axis: Z 259 | Channel Name: intensity 260 | Class: rviz/PointCloud2 261 | Color: 255; 255; 255 262 | Color Transformer: Intensity 263 | Decay Time: 0 264 | Enabled: true 265 | Invert Rainbow: false 266 | Max Color: 255; 255; 255 267 | Max Intensity: 15.1000004 268 | Min Color: 0; 0; 0 269 | Min Intensity: 0 270 | Name: currPoints 271 | Position Transformer: XYZ 272 | Queue Size: 10 273 | Selectable: true 274 | Size (Pixels): 2 275 | Size (m): 0.00999999978 276 | Style: Points 277 | Topic: /velodyne_cloud_registered 278 | Unreliable: false 279 | Use Fixed Frame: true 280 | Use rainbow: true 281 | Value: true 282 | - Alpha: 1 283 | Autocompute Intensity Bounds: true 284 | Autocompute Value Bounds: 285 | Max Value: 10 286 | Min Value: -10 287 | Value: true 288 | Axis: Z 289 | Channel Name: intensity 290 | Class: rviz/PointCloud2 291 | Color: 255; 0; 0 292 | Color Transformer: FlatColor 293 | Decay Time: 0 294 | Enabled: false 295 | Invert Rainbow: false 296 | Max Color: 255; 255; 255 297 | Max Intensity: 63.0384331 298 | Min Color: 255; 0; 0 299 | Min Intensity: -0.00643126154 300 | Name: corner 301 | Position Transformer: XYZ 302 | Queue Size: 10 303 | Selectable: true 304 | Size (Pixels): 3 305 | Size (m): 0.200000003 306 | Style: Flat Squares 307 | Topic: /mapping_corner 308 | Unreliable: false 309 | Use Fixed Frame: true 310 | Use rainbow: true 311 | Value: false 312 | - Alpha: 1 313 | Autocompute Intensity Bounds: true 314 | Autocompute Value Bounds: 315 | Max Value: 10 316 | Min Value: -10 317 | Value: true 318 | Axis: Z 319 | Channel Name: intensity 320 | Class: rviz/PointCloud2 321 | Color: 255; 0; 0 322 | Color Transformer: FlatColor 323 | Decay Time: 0 324 | Enabled: false 325 | Invert Rainbow: false 326 | Max Color: 255; 255; 255 327 | Max Intensity: 63.0818977 328 | Min Color: 0; 0; 0 329 | Min Intensity: -0.00524160406 330 | Name: surf 331 | Position Transformer: XYZ 332 | Queue Size: 10 333 | Selectable: true 334 | Size (Pixels): 3 335 | Size (m): 0.200000003 336 | Style: Flat Squares 337 | Topic: /mapping_surf 338 | Unreliable: false 339 | Use Fixed Frame: true 340 | Use rainbow: true 341 | Value: false 342 | - Alpha: 1 343 | Autocompute Intensity Bounds: true 344 | Autocompute Value Bounds: 345 | Max Value: 10 346 | Min Value: -10 347 | Value: true 348 | Axis: Z 349 | Channel Name: intensity 350 | Class: rviz/PointCloud2 351 | Color: 0; 255; 0 352 | Color Transformer: FlatColor 353 | Decay Time: 0 354 | Enabled: false 355 | Invert Rainbow: false 356 | Max Color: 255; 255; 255 357 | Max Intensity: 17.0880051 358 | Min Color: 0; 0; 0 359 | Min Intensity: -0.00679048989 360 | Name: used_corner 361 | Position Transformer: XYZ 362 | Queue Size: 10 363 | Selectable: true 364 | Size (Pixels): 3 365 | Size (m): 0.5 366 | Style: Flat Squares 367 | Topic: /mapping_used_corner 368 | Unreliable: false 369 | Use Fixed Frame: true 370 | Use rainbow: true 371 | Value: false 372 | - Alpha: 1 373 | Autocompute Intensity Bounds: true 374 | Autocompute Value Bounds: 375 | Max Value: 10 376 | Min Value: -10 377 | Value: true 378 | Axis: Z 379 | Channel Name: intensity 380 | Class: rviz/PointCloud2 381 | Color: 0; 255; 0 382 | Color Transformer: FlatColor 383 | Decay Time: 0 384 | Enabled: false 385 | Invert Rainbow: false 386 | Max Color: 0; 255; 0 387 | Max Intensity: 63.0818977 388 | Min Color: 0; 0; 0 389 | Min Intensity: -0.00524160406 390 | Name: used_surf 391 | Position Transformer: XYZ 392 | Queue Size: 10 393 | Selectable: true 394 | Size (Pixels): 3 395 | Size (m): 0.5 396 | Style: Flat Squares 397 | Topic: /mapping_used_surf 398 | Unreliable: false 399 | Use Fixed Frame: true 400 | Use rainbow: true 401 | Value: false 402 | - Alpha: 1 403 | Autocompute Intensity Bounds: true 404 | Autocompute Value Bounds: 405 | Max Value: 10 406 | Min Value: -10 407 | Value: true 408 | Axis: Z 409 | Channel Name: intensity 410 | Class: rviz/PointCloud2 411 | Color: 255; 255; 255 412 | Color Transformer: FlatColor 413 | Decay Time: 0 414 | Enabled: false 415 | Invert Rainbow: false 416 | Max Color: 255; 255; 255 417 | Max Intensity: 21.0628471 418 | Min Color: 0; 0; 0 419 | Min Intensity: -0.00662572403 420 | Name: map_corner 421 | Position Transformer: XYZ 422 | Queue Size: 10 423 | Selectable: true 424 | Size (Pixels): 3 425 | Size (m): 0.100000001 426 | Style: Flat Squares 427 | Topic: /mapping_map_corner 428 | Unreliable: false 429 | Use Fixed Frame: true 430 | Use rainbow: true 431 | Value: false 432 | - Alpha: 1 433 | Autocompute Intensity Bounds: true 434 | Autocompute Value Bounds: 435 | Max Value: 10 436 | Min Value: -10 437 | Value: true 438 | Axis: Z 439 | Channel Name: intensity 440 | Class: rviz/PointCloud2 441 | Color: 255; 255; 255 442 | Color Transformer: FlatColor 443 | Decay Time: 0 444 | Enabled: false 445 | Invert Rainbow: false 446 | Max Color: 255; 255; 255 447 | Max Intensity: 63.0839233 448 | Min Color: 0; 0; 0 449 | Min Intensity: -0.00876065157 450 | Name: map_surf 451 | Position Transformer: XYZ 452 | Queue Size: 10 453 | Selectable: true 454 | Size (Pixels): 3 455 | Size (m): 0.100000001 456 | Style: Flat Squares 457 | Topic: /mapping_map_surf 458 | Unreliable: false 459 | Use Fixed Frame: true 460 | Use rainbow: true 461 | Value: false 462 | - Alpha: 1 463 | Buffer Length: 1 464 | Class: rviz/Path 465 | Color: 25; 255; 0 466 | Enabled: false 467 | Head Diameter: 0.300000012 468 | Head Length: 0.200000003 469 | Length: 0.300000012 470 | Line Style: Lines 471 | Line Width: 0.0299999993 472 | Name: leftcameraPath 473 | Offset: 474 | X: 0 475 | Y: 0 476 | Z: 0 477 | Pose Color: 255; 85; 255 478 | Pose Style: None 479 | Radius: 0.0299999993 480 | Shaft Diameter: 0.100000001 481 | Shaft Length: 0.100000001 482 | Topic: /lc_path 483 | Unreliable: false 484 | Value: false 485 | Enabled: true 486 | Name: mapping 487 | - Class: rviz/Group 488 | Displays: 489 | - Alpha: 1 490 | Autocompute Intensity Bounds: true 491 | Autocompute Value Bounds: 492 | Max Value: 10 493 | Min Value: -10 494 | Value: true 495 | Axis: Z 496 | Channel Name: intensity 497 | Class: rviz/PointCloud2 498 | Color: 255; 255; 255 499 | Color Transformer: FlatColor 500 | Decay Time: 0 501 | Enabled: false 502 | Invert Rainbow: false 503 | Max Color: 255; 255; 255 504 | Max Intensity: 15 505 | Min Color: 0; 0; 0 506 | Min Intensity: 0 507 | Name: sharp 508 | Position Transformer: XYZ 509 | Queue Size: 10 510 | Selectable: true 511 | Size (Pixels): 3 512 | Size (m): 0.00999999978 513 | Style: Points 514 | Topic: /laser_cloud_sharp 515 | Unreliable: false 516 | Use Fixed Frame: true 517 | Use rainbow: true 518 | Value: false 519 | - Alpha: 1 520 | Autocompute Intensity Bounds: true 521 | Autocompute Value Bounds: 522 | Max Value: 10 523 | Min Value: -10 524 | Value: true 525 | Axis: Z 526 | Channel Name: intensity 527 | Class: rviz/PointCloud2 528 | Color: 255; 255; 255 529 | Color Transformer: Intensity 530 | Decay Time: 0 531 | Enabled: false 532 | Invert Rainbow: false 533 | Max Color: 255; 255; 255 534 | Max Intensity: 63.0398712 535 | Min Color: 0; 0; 0 536 | Min Intensity: 0.0263746977 537 | Name: flat 538 | Position Transformer: XYZ 539 | Queue Size: 10 540 | Selectable: true 541 | Size (Pixels): 3 542 | Size (m): 0.0500000007 543 | Style: Flat Squares 544 | Topic: /laser_cloud_flat 545 | Unreliable: false 546 | Use Fixed Frame: true 547 | Use rainbow: true 548 | Value: false 549 | - Alpha: 1 550 | Autocompute Intensity Bounds: true 551 | Autocompute Value Bounds: 552 | Max Value: 10 553 | Min Value: -10 554 | Value: true 555 | Axis: Z 556 | Channel Name: intensity 557 | Class: rviz/PointCloud2 558 | Color: 255; 255; 255 559 | Color Transformer: FlatColor 560 | Decay Time: 0 561 | Enabled: true 562 | Invert Rainbow: false 563 | Max Color: 255; 255; 255 564 | Max Intensity: 0.99000001 565 | Min Color: 0; 0; 0 566 | Min Intensity: 0 567 | Name: raw_data 568 | Position Transformer: XYZ 569 | Queue Size: 10 570 | Selectable: true 571 | Size (Pixels): 3 572 | Size (m): 0.0500000007 573 | Style: Flat Squares 574 | Topic: /velodyne_points 575 | Unreliable: false 576 | Use Fixed Frame: true 577 | Use rainbow: true 578 | Value: true 579 | - Alpha: 1 580 | Autocompute Intensity Bounds: true 581 | Autocompute Value Bounds: 582 | Max Value: 10 583 | Min Value: -10 584 | Value: true 585 | Axis: Z 586 | Channel Name: intensity 587 | Class: rviz/PointCloud2 588 | Color: 255; 0; 0 589 | Color Transformer: FlatColor 590 | Decay Time: 0 591 | Enabled: false 592 | Invert Rainbow: false 593 | Max Color: 255; 255; 255 594 | Max Intensity: 0.123617291 595 | Min Color: 0; 0; 0 596 | Min Intensity: -0.00711920578 597 | Name: scan 598 | Position Transformer: XYZ 599 | Queue Size: 10 600 | Selectable: true 601 | Size (Pixels): 3 602 | Size (m): 0.200000003 603 | Style: Flat Squares 604 | Topic: /laser_scanid_63 605 | Unreliable: false 606 | Use Fixed Frame: true 607 | Use rainbow: true 608 | Value: false 609 | - Alpha: 1 610 | Autocompute Intensity Bounds: true 611 | Autocompute Value Bounds: 612 | Max Value: 10 613 | Min Value: -10 614 | Value: true 615 | Axis: Z 616 | Channel Name: intensity 617 | Class: rviz/PointCloud2 618 | Color: 255; 85; 0 619 | Color Transformer: FlatColor 620 | Decay Time: 0 621 | Enabled: false 622 | Invert Rainbow: false 623 | Max Color: 255; 255; 255 624 | Max Intensity: 62.075676 625 | Min Color: 0; 0; 0 626 | Min Intensity: -0.00705419574 627 | Name: removPoints 628 | Position Transformer: XYZ 629 | Queue Size: 10 630 | Selectable: true 631 | Size (Pixels): 3 632 | Size (m): 0.200000003 633 | Style: Flat Squares 634 | Topic: /laser_remove_points 635 | Unreliable: false 636 | Use Fixed Frame: true 637 | Use rainbow: true 638 | Value: false 639 | Enabled: false 640 | Name: scan 641 | - Class: rviz/Image 642 | Enabled: true 643 | Image Topic: /kitti/image 644 | Max Value: 1 645 | Median window: 5 646 | Min Value: 0 647 | Name: Image 648 | Normalize Range: true 649 | Queue Size: 2 650 | Transport Hint: raw 651 | Unreliable: false 652 | Value: true 653 | - Class: rviz/TF 654 | Enabled: true 655 | Frame Timeout: 15 656 | Frames: 657 | All Enabled: true 658 | Marker Scale: 1 659 | Name: TF 660 | Show Arrows: true 661 | Show Axes: true 662 | Show Names: true 663 | Tree: 664 | {} 665 | Update Interval: 0 666 | Value: true 667 | Enabled: true 668 | Global Options: 669 | Background Color: 0; 0; 0 670 | Fixed Frame: camera_init 671 | Frame Rate: 30 672 | Name: root 673 | Tools: 674 | - Class: rviz/Interact 675 | Hide Inactive Objects: true 676 | - Class: rviz/MoveCamera 677 | - Class: rviz/Select 678 | - Class: rviz/FocusCamera 679 | - Class: rviz/Measure 680 | - Class: rviz/SetInitialPose 681 | Topic: /initialpose 682 | - Class: rviz/SetGoal 683 | Topic: /move_base_simple/goal 684 | - Class: rviz/PublishPoint 685 | Single click: true 686 | Topic: /clicked_point 687 | Value: true 688 | Views: 689 | Current: 690 | Class: rviz/XYOrbit 691 | Distance: 20.1141529 692 | Enable Stereo Rendering: 693 | Stereo Eye Separation: 0.0599999987 694 | Stereo Focal Distance: 1 695 | Swap Stereo Eyes: false 696 | Value: false 697 | Focal Point: 698 | X: 0.675796986 699 | Y: 1.08226371 700 | Z: -2.30979848 701 | Focal Shape Fixed Size: true 702 | Focal Shape Size: 0.0500000007 703 | Invert Z Axis: false 704 | Name: Current View 705 | Near Clip Distance: 0.00999999978 706 | Pitch: 0.484797001 707 | Target Frame: body 708 | Value: XYOrbit (rviz) 709 | Yaw: 4.52999926 710 | Saved: ~ 711 | Window Geometry: 712 | Displays: 713 | collapsed: false 714 | Height: 1056 715 | Hide Left Dock: false 716 | Hide Right Dock: true 717 | Image: 718 | collapsed: false 719 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c6000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002f4000000ca0000001600ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 720 | Selection: 721 | collapsed: false 722 | Time: 723 | collapsed: false 724 | Tool Properties: 725 | collapsed: false 726 | Views: 727 | collapsed: true 728 | Width: 1855 729 | X: 65 730 | Y: 24 731 | -------------------------------------------------------------------------------- /src/kittiHelper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | std::vector read_lidar_data(const std::string lidar_data_path) 23 | { 24 | std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary); 25 | lidar_data_file.seekg(0, std::ios::end); 26 | const size_t num_elements = lidar_data_file.tellg() / sizeof(float); 27 | lidar_data_file.seekg(0, std::ios::beg); 28 | 29 | std::vector lidar_data_buffer(num_elements); 30 | lidar_data_file.read(reinterpret_cast(&lidar_data_buffer[0]), num_elements*sizeof(float)); 31 | return lidar_data_buffer; 32 | } 33 | 34 | int main(int argc, char** argv) 35 | { 36 | ros::init(argc, argv, "kitti_helper"); 37 | ros::NodeHandle n("~"); 38 | std::string dataset_folder, sequence_number, output_bag_file; 39 | n.getParam("dataset_folder", dataset_folder); 40 | n.getParam("sequence_number", sequence_number); 41 | std::cout << "Reading sequence " << sequence_number << " from " << dataset_folder << '\n'; 42 | bool to_bag; 43 | n.getParam("to_bag", to_bag); 44 | if (to_bag) 45 | n.getParam("output_bag_file", output_bag_file); 46 | int publish_delay; 47 | n.getParam("publish_delay", publish_delay); 48 | publish_delay = publish_delay <= 0 ? 1 : publish_delay; 49 | 50 | ros::Publisher pub_laser_cloud = n.advertise("/velodyne_points", 2); 51 | 52 | image_transport::ImageTransport it(n); 53 | image_transport::Publisher pub_image_left = it.advertise("/image_left", 2); 54 | image_transport::Publisher pub_image_right = it.advertise("/image_right", 2); 55 | 56 | ros::Publisher pubOdomGT = n.advertise ("/odometry_gt", 5); 57 | nav_msgs::Odometry odomGT; 58 | odomGT.header.frame_id = "/camera_init"; 59 | odomGT.child_frame_id = "/ground_truth"; 60 | 61 | ros::Publisher pubPathGT = n.advertise ("/path_gt", 5); 62 | nav_msgs::Path pathGT; 63 | pathGT.header.frame_id = "/camera_init"; 64 | 65 | std::string timestamp_path = "sequences/" + sequence_number + "/times.txt"; 66 | std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in); 67 | 68 | std::string ground_truth_path = "results/" + sequence_number + ".txt"; 69 | std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in); 70 | 71 | rosbag::Bag bag_out; 72 | if (to_bag) 73 | bag_out.open(output_bag_file, rosbag::bagmode::Write); 74 | 75 | Eigen::Matrix3d R_transform; 76 | R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0; 77 | Eigen::Quaterniond q_transform(R_transform); 78 | 79 | std::string line; 80 | std::size_t line_num = 0; 81 | 82 | ros::Rate r(10.0 / publish_delay); 83 | while (std::getline(timestamp_file, line) && ros::ok()) 84 | { 85 | float timestamp = stof(line); 86 | std::stringstream left_image_path, right_image_path; 87 | left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png"; 88 | cv::Mat left_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE); 89 | right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png"; 90 | cv::Mat right_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE); 91 | 92 | std::getline(ground_truth_file, line); 93 | std::stringstream pose_stream(line); 94 | std::string s; 95 | Eigen::Matrix gt_pose; 96 | for (std::size_t i = 0; i < 3; ++i) 97 | { 98 | for (std::size_t j = 0; j < 4; ++j) 99 | { 100 | std::getline(pose_stream, s, ' '); 101 | gt_pose(i, j) = stof(s); 102 | } 103 | } 104 | 105 | Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>()); 106 | Eigen::Quaterniond q = q_transform * q_w_i; 107 | q.normalize(); 108 | Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>(); 109 | 110 | odomGT.header.stamp = ros::Time().fromSec(timestamp); 111 | odomGT.pose.pose.orientation.x = q.x(); 112 | odomGT.pose.pose.orientation.y = q.y(); 113 | odomGT.pose.pose.orientation.z = q.z(); 114 | odomGT.pose.pose.orientation.w = q.w(); 115 | odomGT.pose.pose.position.x = t(0); 116 | odomGT.pose.pose.position.y = t(1); 117 | odomGT.pose.pose.position.z = t(2); 118 | pubOdomGT.publish(odomGT); 119 | 120 | geometry_msgs::PoseStamped poseGT; 121 | poseGT.header = odomGT.header; 122 | poseGT.pose = odomGT.pose.pose; 123 | pathGT.header.stamp = odomGT.header.stamp; 124 | pathGT.poses.push_back(poseGT); 125 | pubPathGT.publish(pathGT); 126 | 127 | // read lidar point cloud 128 | std::stringstream lidar_data_path; 129 | lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/" 130 | << std::setfill('0') << std::setw(6) << line_num << ".bin"; 131 | std::vector lidar_data = read_lidar_data(lidar_data_path.str()); 132 | std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n"; 133 | 134 | std::vector lidar_points; 135 | std::vector lidar_intensities; 136 | pcl::PointCloud laser_cloud; 137 | for (std::size_t i = 0; i < lidar_data.size(); i += 4) 138 | { 139 | lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]); 140 | lidar_intensities.push_back(lidar_data[i+3]); 141 | 142 | pcl::PointXYZI point; 143 | point.x = lidar_data[i]; 144 | point.y = lidar_data[i + 1]; 145 | point.z = lidar_data[i + 2]; 146 | point.intensity = lidar_data[i + 3]; 147 | laser_cloud.push_back(point); 148 | } 149 | 150 | sensor_msgs::PointCloud2 laser_cloud_msg; 151 | pcl::toROSMsg(laser_cloud, laser_cloud_msg); 152 | laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp); 153 | laser_cloud_msg.header.frame_id = "/camera_init"; 154 | pub_laser_cloud.publish(laser_cloud_msg); 155 | 156 | sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", left_image).toImageMsg(); 157 | sensor_msgs::ImagePtr image_right_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", right_image).toImageMsg(); 158 | pub_image_left.publish(image_left_msg); 159 | pub_image_right.publish(image_right_msg); 160 | 161 | if (to_bag) 162 | { 163 | bag_out.write("/image_left", ros::Time::now(), image_left_msg); 164 | bag_out.write("/image_right", ros::Time::now(), image_right_msg); 165 | bag_out.write("/velodyne_points", ros::Time::now(), laser_cloud_msg); 166 | bag_out.write("/path_gt", ros::Time::now(), pathGT); 167 | bag_out.write("/odometry_gt", ros::Time::now(), odomGT); 168 | } 169 | 170 | line_num ++; 171 | r.sleep(); 172 | } 173 | bag_out.close(); 174 | std::cout << "Done \n"; 175 | 176 | 177 | return 0; 178 | } 179 | -------------------------------------------------------------------------------- /src/laserOdometry.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "omp.h" 19 | #include "aloam_velodyne/common.h" 20 | #include "aloam_velodyne/tic_toc.h" 21 | #include "lidarFactor.hpp" 22 | 23 | #define DISTORTION 0 24 | 25 | int N_SCANS = 0; 26 | int corner_correspondence = 0, plane_correspondence = 0; 27 | 28 | constexpr double SCAN_PERIOD = 0.1; 29 | constexpr double DISTANCE_SQ_THRESHOLD = 25; 30 | constexpr double NEARBY_SCAN = 2.5; 31 | 32 | int skipFrameNum = 5; 33 | bool systemInited = false; 34 | 35 | double timeCornerPointsSharp = 0; 36 | double timeCornerPointsLessSharp = 0; 37 | double timeSurfPointsFlat = 0; 38 | double timeSurfPointsLessFlat = 0; 39 | double timeLaserCloudFullRes = 0; 40 | 41 | pcl::KdTreeFLANN::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN()); 42 | pcl::KdTreeFLANN::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN()); 43 | 44 | pcl::PointCloud::Ptr cornerPointsSharp(new pcl::PointCloud()); 45 | pcl::PointCloud::Ptr cornerPointsLessSharp(new pcl::PointCloud()); 46 | pcl::PointCloud::Ptr surfPointsFlat(new pcl::PointCloud()); 47 | pcl::PointCloud::Ptr surfPointsLessFlat(new pcl::PointCloud()); 48 | 49 | pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud()); 50 | pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud()); 51 | pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); 52 | 53 | int laserCloudCornerLastNum = 0; 54 | int laserCloudSurfLastNum = 0; 55 | 56 | // Transformation from current frame to world frame 57 | Eigen::Quaterniond q_w_curr(1, 0, 0, 0); 58 | Eigen::Vector3d t_w_curr(0, 0, 0); 59 | 60 | // q_curr_last(x, y, z, w), t_curr_last 61 | double para_q[4] = {0, 0, 0, 1}; 62 | double para_t[3] = {0, 0, 0}; 63 | 64 | Eigen::Map q_last_curr(para_q); 65 | Eigen::Map t_last_curr(para_t); 66 | 67 | std::queue cornerSharpBuf; 68 | std::queue cornerLessSharpBuf; 69 | std::queue surfFlatBuf; 70 | std::queue surfLessFlatBuf; 71 | std::queue fullPointsBuf; 72 | std::mutex mBuf; 73 | 74 | 75 | 76 | // undistort lidar point 77 | void TransformToStart(PointType const *const pi, PointType *const po) 78 | { 79 | //interpolation ratio 80 | double s; 81 | if (DISTORTION) 82 | s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD; 83 | else 84 | s = 1.0; 85 | //s = 1; 86 | Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr); 87 | Eigen::Vector3d t_point_last = s * t_last_curr; 88 | Eigen::Vector3d point(pi->x, pi->y, pi->z); 89 | Eigen::Vector3d un_point = q_point_last * point + t_point_last; 90 | 91 | po->x = un_point.x(); 92 | po->y = un_point.y(); 93 | po->z = un_point.z(); 94 | po->intensity = pi->intensity; 95 | } 96 | 97 | // transform all lidar points to the start of the next frame 98 | 99 | void TransformToEnd(PointType const *const pi, PointType *const po) 100 | { 101 | // undistort point first 102 | pcl::PointXYZI un_point_tmp; 103 | TransformToStart(pi, &un_point_tmp); 104 | 105 | Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z); 106 | Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr); 107 | 108 | po->x = point_end.x(); 109 | po->y = point_end.y(); 110 | po->z = point_end.z(); 111 | 112 | //Remove distortion time info 113 | po->intensity = int(pi->intensity); 114 | } 115 | 116 | void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2) 117 | { 118 | mBuf.lock(); 119 | cornerSharpBuf.push(cornerPointsSharp2); 120 | mBuf.unlock(); 121 | } 122 | 123 | void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2) 124 | { 125 | mBuf.lock(); 126 | cornerLessSharpBuf.push(cornerPointsLessSharp2); 127 | mBuf.unlock(); 128 | } 129 | 130 | void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2) 131 | { 132 | mBuf.lock(); 133 | surfFlatBuf.push(surfPointsFlat2); 134 | mBuf.unlock(); 135 | } 136 | 137 | void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2) 138 | { 139 | mBuf.lock(); 140 | surfLessFlatBuf.push(surfPointsLessFlat2); 141 | mBuf.unlock(); 142 | } 143 | 144 | //receive all point cloud 145 | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2) 146 | { 147 | mBuf.lock(); 148 | fullPointsBuf.push(laserCloudFullRes2); 149 | mBuf.unlock(); 150 | } 151 | 152 | //new add 153 | float Distance(pcl::PointXYZI& a, pcl::PointXYZI& b) 154 | { 155 | float distance = -1; 156 | float dx, dy, dz; 157 | dx = a.x - b.x; 158 | dy = a.y - b.y; 159 | dz = a.z - b.z; 160 | distance = std::sqrt(dx*dx + dy*dy +dz*dz); 161 | return distance; 162 | } 163 | 164 | 165 | void graph_based_correspondence_vote_simple(std::vector &correspondences, bool corner_case, 166 | std::vector &selected_idx, 167 | Eigen::MatrixXi &graph_indexing, 168 | std::vector &feature_cur_point, 169 | std::vector &feature_last_point_a, 170 | std::vector &feature_last_point_b, 171 | std::vector &feature_last_point_c, 172 | std::vector &feature_s) 173 | { 174 | 175 | int cor_size_all = correspondences.size(); 176 | // int _horizontal_scans =2117; 177 | int number_of_region; 178 | float score_threshold; 179 | if(corner_case) 180 | { 181 | number_of_region = 5; 182 | score_threshold =0.96; 183 | } 184 | else 185 | { 186 | score_threshold = 0.96; 187 | number_of_region = 10; 188 | } 189 | 190 | 191 | 192 | std::chrono::steady_clock::time_point tic_graph_correspondence = std::chrono::steady_clock::now(); 193 | for(int num_region = 0; num_region < number_of_region; num_region++) 194 | { 195 | std::vector time_consuming; 196 | std::chrono::steady_clock::time_point tic_graph_loop8 = std::chrono::steady_clock::now(); 197 | // std::cout << "num_region: " << num_region << std::endl; 198 | 199 | std::chrono::steady_clock::time_point tic_graph_construction = std::chrono::steady_clock::now(); 200 | std::vector vertex_set_param; 201 | 202 | int initial_pos = cor_size_all/number_of_region*(num_region); 203 | int end_pos; 204 | if(num_region == number_of_region-1) 205 | { 206 | end_pos = cor_size_all; 207 | } 208 | else 209 | { 210 | end_pos = cor_size_all/number_of_region*(num_region+1); 211 | } 212 | std::vector correspondences_selected; 213 | correspondences_selected.reserve(cor_size_all/number_of_region*2); 214 | correspondences_selected.assign(correspondences.begin()+initial_pos, correspondences.begin()+end_pos); 215 | int cor_size = correspondences_selected.size(); 216 | Eigen::MatrixXf compatibility_matrix; 217 | // int size = correspondences.size(); 218 | compatibility_matrix.resize(correspondences_selected.size(), correspondences_selected.size()); 219 | compatibility_matrix.setZero(); 220 | Corre_Match c1, c2; 221 | float s1, s2, dis_gap, resolution, alpha_param, score; 222 | resolution = 1; //_ang_resolution_X; 223 | // alpha_param = resolution; // hyper parameter 224 | // std::vector> test_v(cor_size); 225 | std::vector vote_record(correspondences_selected.size(),{0,0.0}); 226 | // std::vector test(correspondences_selected.size(),0); 227 | // vote_record.reserve(correspondences_selected.size()); 228 | for(int i = 0; i < correspondences_selected.size(); i++) 229 | { 230 | int count =0; 231 | vote_record[i].index = i; 232 | c1 = correspondences_selected[i]; 233 | for(int j = i + 1; j < correspondences_selected.size(); j++) 234 | { 235 | c2 = correspondences_selected[j]; 236 | s1 = Distance(c1.src, c2.src); 237 | s2 = Distance(c1.tgt, c2.tgt); 238 | dis_gap = std::abs(s1 - s2); 239 | score = std::exp(-(dis_gap*dis_gap) / (resolution*resolution)); 240 | compatibility_matrix(i, j) = score; 241 | compatibility_matrix(j, i) = score; 242 | if(score < score_threshold) 243 | { 244 | count++; 245 | vote_record[j].score += 1; 246 | vote_record[i].score += 1; 247 | // test[i] +=1; 248 | // test[j] +=1; 249 | } 250 | } 251 | // std::cout<<"sample:" << i<< " , count:"<< count<<" , percent:"<< count*1.0/correspondence_partial.size()*100<<"%"<=0; i--) 267 | { 268 | if(i >= donot_num_selected) 269 | { 270 | Vertex_Vote obj; 271 | // int indx = correspondences_selected[voter_ordered[i].index].index; 272 | if(correspondences_selected[vote_record[i].index].index > correspondences.size()) continue; 273 | obj.index = correspondences_selected[vote_record[i].index].index; 274 | 275 | if(vote_record[i].score > num_selected) 276 | { 277 | 278 | obj.score =0; 279 | break; 280 | }else if(vote_record[i].score <=50){ 281 | // obj.score =0.2*vote_record[0].score/vote_record[i].score; 282 | obj.score =5.0; 283 | }else 284 | { 285 | obj.score =1; 286 | } 287 | 288 | 289 | 290 | selected_idx.push_back(obj);count_selected++; 291 | 292 | } 293 | } 294 | 295 | 296 | } 297 | else 298 | { 299 | float selected_ratio = 0.90; 300 | float num_selected = selected_ratio*cor_size; 301 | float selected_count_ratio = 1; 302 | int donot_num_selected = (1-selected_count_ratio)*cor_size; 303 | 304 | for(int i =cor_size-1; i >=0; i--) 305 | { 306 | if(i >= donot_num_selected) 307 | { 308 | Vertex_Vote obj; 309 | if(correspondences_selected[vote_record[i].index].index > correspondences.size()) continue; 310 | obj.index = correspondences_selected[vote_record[i].index].index; 311 | 312 | if(vote_record[i].score > num_selected) 313 | { 314 | 315 | obj.score =0; 316 | break; 317 | }else if(vote_record[i].score <=50){ 318 | obj.score =5.0; 319 | }else 320 | { 321 | obj.score =1; 322 | } 323 | 324 | 325 | 326 | selected_idx.push_back(obj);count_selected++; 327 | 328 | } 329 | } 330 | } 331 | vote_record.clear(); 332 | vote_record.shrink_to_fit(); 333 | 334 | 335 | 336 | } 337 | 338 | std::chrono::steady_clock::time_point toc_graph_correspondence = std::chrono::steady_clock::now(); 339 | std::chrono::duration time_used_init_graph_correspondence = std::chrono::duration_cast>(toc_graph_correspondence - tic_graph_correspondence); 340 | 341 | 342 | } 343 | 344 | //new add 345 | int main(int argc, char **argv) 346 | { 347 | ros::init(argc, argv, "laserOdometry"); 348 | ros::NodeHandle nh; 349 | nh.param("scan_line", N_SCANS, 16); 350 | nh.param("mapping_skip_frame", skipFrameNum, 2); 351 | 352 | //printf("Mapping %d Hz \n", 10 / skipFrameNum); 353 | 354 | ros::Subscriber subCornerPointsSharp = nh.subscribe("/laser_cloud_sharp", 100, laserCloudSharpHandler); 355 | 356 | ros::Subscriber subCornerPointsLessSharp = nh.subscribe("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler); 357 | 358 | ros::Subscriber subSurfPointsFlat = nh.subscribe("/laser_cloud_flat", 100, laserCloudFlatHandler); 359 | 360 | ros::Subscriber subSurfPointsLessFlat = nh.subscribe("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler); 361 | 362 | ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_2", 100, laserCloudFullResHandler); 363 | 364 | ros::Publisher pubLaserCloudCornerLast = nh.advertise("/laser_cloud_corner_last", 100); 365 | 366 | ros::Publisher pubLaserCloudSurfLast = nh.advertise("/laser_cloud_surf_last", 100); 367 | 368 | ros::Publisher pubLaserCloudFullRes = nh.advertise("/velodyne_cloud_3", 100); 369 | 370 | ros::Publisher pubLaserOdometry = nh.advertise("/laser_odom_to_init", 100); 371 | 372 | ros::Publisher pubLaserPath = nh.advertise("/laser_odom_path", 100); 373 | 374 | nav_msgs::Path laserPath; 375 | 376 | int frameCount = 0; 377 | int now_frame =0; 378 | ros::Rate rate(100); 379 | 380 | while (ros::ok()) 381 | { 382 | ros::spinOnce(); 383 | 384 | if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() && 385 | !surfFlatBuf.empty() && !surfLessFlatBuf.empty() && 386 | !fullPointsBuf.empty()) 387 | { 388 | timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec(); 389 | timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec(); 390 | timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec(); 391 | timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec(); 392 | timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec(); 393 | 394 | if (timeCornerPointsSharp != timeLaserCloudFullRes || 395 | timeCornerPointsLessSharp != timeLaserCloudFullRes || 396 | timeSurfPointsFlat != timeLaserCloudFullRes || 397 | timeSurfPointsLessFlat != timeLaserCloudFullRes) 398 | { 399 | //printf("unsync messeage!"); 400 | ROS_BREAK(); 401 | } 402 | 403 | mBuf.lock(); 404 | cornerPointsSharp->clear(); 405 | pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp); 406 | cornerSharpBuf.pop(); 407 | 408 | cornerPointsLessSharp->clear(); 409 | pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp); 410 | cornerLessSharpBuf.pop(); 411 | 412 | surfPointsFlat->clear(); 413 | pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat); 414 | surfFlatBuf.pop(); 415 | 416 | surfPointsLessFlat->clear(); 417 | pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat); 418 | surfLessFlatBuf.pop(); 419 | 420 | laserCloudFullRes->clear(); 421 | pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes); 422 | fullPointsBuf.pop(); 423 | mBuf.unlock(); 424 | 425 | TicToc t_whole; 426 | // initializing 427 | if (!systemInited) 428 | { 429 | systemInited = true; 430 | std::cout << "Initialization finished \n"; 431 | } 432 | else 433 | { 434 | int cornerPointsSharpNum = cornerPointsSharp->points.size(); 435 | int surfPointsFlatNum = surfPointsFlat->points.size(); 436 | std::cout<< "cornerPointsSharpNum:"< surfPointsFlatNum) ? cornerPointsSharpNum : surfPointsFlatNum; 443 | std::vector feature_cur_point; 444 | // std::cout<<"corner_cur_point_size:"< feature_last_point_a; 446 | std::vector feature_last_point_b; 447 | std::vector feature_last_point_c; 448 | std::vector feature_s; 449 | std::vector correspondences; 450 | std::vector selected_idx; 451 | feature_cur_point.reserve(intilize_num); 452 | feature_last_point_a.reserve(intilize_num); 453 | feature_last_point_b.reserve(intilize_num); 454 | feature_last_point_c.reserve(intilize_num); 455 | feature_s.reserve(intilize_num); 456 | correspondences.reserve(intilize_num); 457 | selected_idx.reserve(intilize_num); 458 | int index=0; 459 | int _horizontal_scans =2117; 460 | float _ang_resolution_X = (M_PI*2) / (_horizontal_scans);; 461 | Eigen::MatrixXi graph_indexing; 462 | graph_indexing.setOnes(N_SCANS, _horizontal_scans); 463 | graph_indexing = graph_indexing * -1; 464 | // float min_location; 465 | float vertical_angle_top = 2; 466 | float _ang_bottom=-24.9; 467 | float _ang_resolution_Y = M_PI / 180.0 *(vertical_angle_top - _ang_bottom) / float(N_SCANS-1); 468 | _ang_bottom = -( _ang_bottom - 0.1) * M_PI / 180.0; 469 | // new add 470 | 471 | corner_correspondence = 0; 472 | plane_correspondence = 0; 473 | 474 | //ceres::LossFunction *loss_function = NULL; 475 | ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); 476 | ceres::Manifold *q_parameterization = 477 | new ceres::EigenQuaternionManifold(); 478 | ceres::Problem::Options problem_options; 479 | 480 | ceres::Problem problem(problem_options); 481 | problem.AddParameterBlock(para_q, 4, q_parameterization); 482 | problem.AddParameterBlock(para_t, 3); 483 | 484 | pcl::PointXYZI pointSel; 485 | std::vector pointSearchInd; 486 | std::vector pointSearchSqDis; 487 | 488 | TicToc t_data; 489 | // find correspondence for corner features 490 | 491 | for (int i = 0; i < cornerPointsSharpNum; ++i) 492 | { 493 | TransformToStart(&(cornerPointsSharp->points[i]), &pointSel); 494 | kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); 495 | 496 | int closestPointInd = -1, minPointInd2 = -1; 497 | if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) 498 | { 499 | closestPointInd = pointSearchInd[0]; 500 | int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity); 501 | 502 | double minPointSqDis2 = DISTANCE_SQ_THRESHOLD; 503 | // search in the direction of increasing scan line 504 | for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j) 505 | { 506 | // if in the same scan line, continue 507 | if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID) 508 | continue; 509 | 510 | // if not in nearby scans, end the loop 511 | if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN)) 512 | break; 513 | 514 | double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 515 | (laserCloudCornerLast->points[j].x - pointSel.x) + 516 | (laserCloudCornerLast->points[j].y - pointSel.y) * 517 | (laserCloudCornerLast->points[j].y - pointSel.y) + 518 | (laserCloudCornerLast->points[j].z - pointSel.z) * 519 | (laserCloudCornerLast->points[j].z - pointSel.z); 520 | 521 | if (pointSqDis < minPointSqDis2) 522 | { 523 | // find nearer point 524 | minPointSqDis2 = pointSqDis; 525 | minPointInd2 = j; 526 | } 527 | } 528 | 529 | // search in the direction of decreasing scan line 530 | for (int j = closestPointInd - 1; j >= 0; --j) 531 | { 532 | // if in the same scan line, continue 533 | if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID) 534 | continue; 535 | 536 | // if not in nearby scans, end the loop 537 | if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN)) 538 | break; 539 | 540 | double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 541 | (laserCloudCornerLast->points[j].x - pointSel.x) + 542 | (laserCloudCornerLast->points[j].y - pointSel.y) * 543 | (laserCloudCornerLast->points[j].y - pointSel.y) + 544 | (laserCloudCornerLast->points[j].z - pointSel.z) * 545 | (laserCloudCornerLast->points[j].z - pointSel.z); 546 | 547 | if (pointSqDis < minPointSqDis2) 548 | { 549 | // find nearer point 550 | minPointSqDis2 = pointSqDis; 551 | minPointInd2 = j; 552 | } 553 | } 554 | } 555 | 556 | if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid 557 | { 558 | Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x, 559 | cornerPointsSharp->points[i].y, 560 | cornerPointsSharp->points[i].z); 561 | Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x, 562 | laserCloudCornerLast->points[closestPointInd].y, 563 | laserCloudCornerLast->points[closestPointInd].z); 564 | Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x, 565 | laserCloudCornerLast->points[minPointInd2].y, 566 | laserCloudCornerLast->points[minPointInd2].z); 567 | 568 | 569 | double s; 570 | if (DISTORTION) 571 | s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD; 572 | else 573 | s = 1.0; 574 | feature_cur_point.push_back(curr_point); 575 | feature_last_point_a.push_back(last_point_a); 576 | feature_last_point_b.push_back(last_point_b); 577 | feature_s.push_back(s); 578 | // new add 579 | Corre_Match cor; 580 | cor.index = index; 581 | cor.src = cornerPointsSharp->points[i]; 582 | cor.tgt = laserCloudCornerLast->points[closestPointInd]; 583 | cor.score = 0; 584 | cor.s = 1/s; 585 | correspondences.push_back(cor); 586 | // cor.tgt = 587 | // std::cout<<"i:" <points[i] col:"<< cornerlocation_mark[i]<< std::endl; 588 | // std::cout<< "cornerPointsSharp->points[i] col:"<< int((cornerlocation_mark[i]-min_location)*_horizontal_scans<< std::endl; 589 | //block the point 590 | // PointType thisPoint = cornerPointsSharp->points[i]; 591 | // float range = sqrt(thisPoint.x * thisPoint.x + 592 | // thisPoint.y * thisPoint.y + 593 | // thisPoint.z * thisPoint.z); 594 | // float verticalAngle = std::asin(thisPoint.z / range); 595 | // int rowIdn = (verticalAngle + _ang_bottom) / _ang_resolution_Y; 596 | // float horizonAngle = std::atan2(thisPoint.x, thisPoint.y); 597 | // int columnIdn = -round((horizonAngle - M_PI_2) / _ang_resolution_X) + _horizontal_scans * 0.5; 598 | 599 | // if (columnIdn >= _horizontal_scans){ 600 | // columnIdn -= _horizontal_scans; 601 | // } 602 | 603 | // // if (columnIdn < 0 || columnIdn >= _horizontal_scans){ 604 | // // continue; 605 | // // } 606 | 607 | // graph_indexing(rowIdn, columnIdn) = index; 608 | index++; 609 | // if(now_frame <= 5) 610 | // { 611 | // ceres::CostFunction *cost_function = LidarEdgeFactor_modify::Create(curr_point, last_point_a, last_point_b, s, 1); 612 | // problem.AddResidualBlock(cost_function, loss_function, para_q, para_t); 613 | 614 | // } 615 | ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s); 616 | problem.AddResidualBlock(cost_function, loss_function, para_q, para_t); 617 | corner_correspondence++; 618 | } 619 | 620 | } 621 | // std::cout<<"corner_correspondence:" < 5 ) 629 | // { 630 | // graph_based_correspondence_vote_simple(correspondences, true, selected_idx, graph_indexing,feature_cur_point, feature_last_point_a, feature_last_point_b, feature_last_point_c, feature_s); 631 | // for(int i = 0; i < selected_idx.size(); i++) 632 | // { 633 | 634 | // int idx = selected_idx[i].index; 635 | // // std::cout<<"feature_cur_point[idx]:"<points[i]), &pointSel); 656 | kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); 657 | 658 | int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1; 659 | if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) 660 | { 661 | closestPointInd = pointSearchInd[0]; 662 | 663 | // get closest point's scan ID 664 | int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity); 665 | double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD; 666 | 667 | // search in the direction of increasing scan line 668 | for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j) 669 | { 670 | // if not in nearby scans, end the loop 671 | if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN)) 672 | break; 673 | 674 | double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 675 | (laserCloudSurfLast->points[j].x - pointSel.x) + 676 | (laserCloudSurfLast->points[j].y - pointSel.y) * 677 | (laserCloudSurfLast->points[j].y - pointSel.y) + 678 | (laserCloudSurfLast->points[j].z - pointSel.z) * 679 | (laserCloudSurfLast->points[j].z - pointSel.z); 680 | 681 | // if in the same or lower scan line 682 | if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2) 683 | { 684 | minPointSqDis2 = pointSqDis; 685 | minPointInd2 = j; 686 | } 687 | // if in the higher scan line 688 | else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3) 689 | { 690 | minPointSqDis3 = pointSqDis; 691 | minPointInd3 = j; 692 | } 693 | } 694 | 695 | // search in the direction of decreasing scan line 696 | for (int j = closestPointInd - 1; j >= 0; --j) 697 | { 698 | // if not in nearby scans, end the loop 699 | if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN)) 700 | break; 701 | 702 | double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 703 | (laserCloudSurfLast->points[j].x - pointSel.x) + 704 | (laserCloudSurfLast->points[j].y - pointSel.y) * 705 | (laserCloudSurfLast->points[j].y - pointSel.y) + 706 | (laserCloudSurfLast->points[j].z - pointSel.z) * 707 | (laserCloudSurfLast->points[j].z - pointSel.z); 708 | 709 | // if in the same or higher scan line 710 | if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2) 711 | { 712 | minPointSqDis2 = pointSqDis; 713 | minPointInd2 = j; 714 | } 715 | else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3) 716 | { 717 | // find nearer point 718 | minPointSqDis3 = pointSqDis; 719 | minPointInd3 = j; 720 | } 721 | } 722 | 723 | if (minPointInd2 >= 0 && minPointInd3 >= 0) 724 | { 725 | 726 | Eigen::Vector3d curr_point(surfPointsFlat->points[i].x, 727 | surfPointsFlat->points[i].y, 728 | surfPointsFlat->points[i].z); 729 | Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x, 730 | laserCloudSurfLast->points[closestPointInd].y, 731 | laserCloudSurfLast->points[closestPointInd].z); 732 | Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x, 733 | laserCloudSurfLast->points[minPointInd2].y, 734 | laserCloudSurfLast->points[minPointInd2].z); 735 | Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x, 736 | laserCloudSurfLast->points[minPointInd3].y, 737 | laserCloudSurfLast->points[minPointInd3].z); 738 | 739 | double s; 740 | if (DISTORTION) 741 | s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD; 742 | else 743 | s = 1.0; 744 | 745 | feature_cur_point.push_back(curr_point); 746 | feature_last_point_a.push_back(last_point_a); 747 | feature_last_point_b.push_back(last_point_b); 748 | feature_last_point_c.push_back(last_point_c); 749 | feature_s.push_back(s); 750 | // new add 751 | Corre_Match cor; 752 | cor.index = index; 753 | cor.src = surfPointsFlat->points[i]; 754 | cor.tgt = laserCloudSurfLast->points[closestPointInd]; 755 | cor.score = 0; 756 | cor.s = 1/s; 757 | correspondences.push_back(cor); 758 | // cor.tgt = 759 | // std::cout<<"i:" <points[i] col:"<< cornerlocation_mark[i]<< std::endl; 760 | // std::cout<< "cornerPointsSharp->points[i] col:"<< int((cornerlocation_mark[i]-min_location)*_horizontal_scans<< std::endl; 761 | //block the point 762 | // PointType thisPoint = cornerPointsSharp->points[i]; 763 | // float range = sqrt(thisPoint.x * thisPoint.x + 764 | // thisPoint.y * thisPoint.y + 765 | // thisPoint.z * thisPoint.z); 766 | // float verticalAngle = std::asin(thisPoint.z / range); 767 | // int rowIdn = (verticalAngle + _ang_bottom) / _ang_resolution_Y; 768 | // float horizonAngle = std::atan2(thisPoint.x, thisPoint.y); 769 | // int columnIdn = -round((horizonAngle - M_PI_2) / _ang_resolution_X) + _horizontal_scans * 0.5; 770 | 771 | // if (columnIdn >= _horizontal_scans){ 772 | // columnIdn -= _horizontal_scans; 773 | // } 774 | 775 | // // if (columnIdn < 0 || columnIdn >= _horizontal_scans){ 776 | // // continue; 777 | // // } 778 | 779 | // graph_indexing(rowIdn, columnIdn) = index; 780 | index++; 781 | if(now_frame <= 5) //ori:20 782 | { 783 | ceres::CostFunction *cost_function = LidarPlaneFactor_modify::Create(curr_point, last_point_a, last_point_b, last_point_c, s, 1); 784 | // ceres::CostFunction *cost_function = LidarPlaneFactor_modify_test::Create(curr_point, last_point_a, last_point_b, last_point_c, s, 1); 785 | problem.AddResidualBlock(cost_function, loss_function, para_q, para_t); 786 | 787 | } 788 | // ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s); 789 | // problem.AddResidualBlock(cost_function, loss_function, para_q, para_t); 790 | plane_correspondence++; 791 | } 792 | } 793 | } 794 | if(now_frame > 5 ) 795 | { 796 | graph_based_correspondence_vote_simple(correspondences, false, selected_idx, graph_indexing, feature_cur_point, feature_last_point_a, feature_last_point_b, feature_last_point_c, feature_s); 797 | for(int i = 0; i < selected_idx.size(); i++) 798 | { 799 | 800 | int idx = selected_idx[i].index; 801 | // std::cout<<"feature_cur_point[idx]:"<points.size(); 864 | for (int i = 0; i < cornerPointsLessSharpNum; i++) 865 | { 866 | TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]); 867 | } 868 | 869 | int surfPointsLessFlatNum = surfPointsLessFlat->points.size(); 870 | for (int i = 0; i < surfPointsLessFlatNum; i++) 871 | { 872 | TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]); 873 | } 874 | 875 | int laserCloudFullResNum = laserCloudFullRes->points.size(); 876 | for (int i = 0; i < laserCloudFullResNum; i++) 877 | { 878 | TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]); 879 | } 880 | } 881 | 882 | pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp; 883 | cornerPointsLessSharp = laserCloudCornerLast; 884 | laserCloudCornerLast = laserCloudTemp; 885 | 886 | laserCloudTemp = surfPointsLessFlat; 887 | surfPointsLessFlat = laserCloudSurfLast; 888 | laserCloudSurfLast = laserCloudTemp; 889 | 890 | laserCloudCornerLastNum = laserCloudCornerLast->points.size(); 891 | laserCloudSurfLastNum = laserCloudSurfLast->points.size(); 892 | 893 | // std::cout << "the size of corner last is " << laserCloudCornerLastNum << ", and the size of surf last is " << laserCloudSurfLastNum << '\n'; 894 | 895 | kdtreeCornerLast->setInputCloud(laserCloudCornerLast); 896 | kdtreeSurfLast->setInputCloud(laserCloudSurfLast); 897 | 898 | if (frameCount % skipFrameNum == 0) 899 | { 900 | frameCount = 0; 901 | 902 | sensor_msgs::PointCloud2 laserCloudCornerLast2; 903 | pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); 904 | laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 905 | laserCloudCornerLast2.header.frame_id = "/camera"; 906 | pubLaserCloudCornerLast.publish(laserCloudCornerLast2); 907 | 908 | sensor_msgs::PointCloud2 laserCloudSurfLast2; 909 | pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); 910 | laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 911 | laserCloudSurfLast2.header.frame_id = "/camera"; 912 | pubLaserCloudSurfLast.publish(laserCloudSurfLast2); 913 | 914 | sensor_msgs::PointCloud2 laserCloudFullRes3; 915 | pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3); 916 | laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 917 | laserCloudFullRes3.header.frame_id = "/camera"; 918 | pubLaserCloudFullRes.publish(laserCloudFullRes3); 919 | } 920 | //printf("publication time %f ms \n", t_pub.toc()); 921 | printf("whole laserOdometry time %f ms \n \n", t_whole.toc()); 922 | if(t_whole.toc() > 100) 923 | ROS_WARN("odometry process over 100ms"); 924 | 925 | frameCount++; 926 | now_frame++; 927 | } 928 | rate.sleep(); 929 | } 930 | return 0; 931 | } 932 | -------------------------------------------------------------------------------- /src/lidarFactor.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | struct LidarEdgeFactor 10 | { 11 | LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, 12 | Eigen::Vector3d last_point_b_, double s_) 13 | : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {} 14 | 15 | template 16 | bool operator()(const T *q, const T *t, T *residual) const 17 | { 18 | 19 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 20 | Eigen::Matrix lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())}; 21 | Eigen::Matrix lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())}; 22 | 23 | //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; 24 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 25 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 26 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 27 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 28 | 29 | Eigen::Matrix lp; 30 | lp = q_last_curr * cp + t_last_curr; 31 | 32 | Eigen::Matrix nu = (lp - lpa).cross(lp - lpb); 33 | Eigen::Matrix de = lpa - lpb; 34 | 35 | residual[0] = nu.x() / de.norm(); 36 | residual[1] = nu.y() / de.norm(); 37 | residual[2] = nu.z() / de.norm(); 38 | 39 | return true; 40 | } 41 | 42 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_, 43 | const Eigen::Vector3d last_point_b_, const double s_) 44 | { 45 | return (new ceres::AutoDiffCostFunction< 46 | LidarEdgeFactor, 3, 4, 3>( 47 | new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_))); 48 | } 49 | 50 | Eigen::Vector3d curr_point, last_point_a, last_point_b; 51 | double s; 52 | }; 53 | 54 | struct LidarEdgeFactor_modify 55 | { 56 | LidarEdgeFactor_modify(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, 57 | Eigen::Vector3d last_point_b_, double s_, double weight_) 58 | : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_), weight(weight_){} 59 | 60 | template 61 | bool operator()(const T *q, const T *t, T *residual) const 62 | { 63 | 64 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 65 | Eigen::Matrix lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())}; 66 | Eigen::Matrix lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())}; 67 | 68 | //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; 69 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 70 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 71 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 72 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 73 | 74 | Eigen::Matrix lp; 75 | lp = q_last_curr * cp + t_last_curr; 76 | 77 | Eigen::Matrix nu = (lp - lpa).cross(lp - lpb); 78 | Eigen::Matrix de = lpa - lpb; 79 | 80 | // residual[0] = nu.x() / de.norm(); 81 | // residual[1] = nu.y() / de.norm(); 82 | // residual[2] = nu.z() / de.norm(); 83 | residual[0] = nu.norm() / de.norm(); 84 | residual[0] *= weight; 85 | 86 | return true; 87 | } 88 | 89 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_, 90 | const Eigen::Vector3d last_point_b_, const double s_, const double weight_) 91 | { 92 | return (new ceres::AutoDiffCostFunction< 93 | LidarEdgeFactor_modify, 1, 4, 3>( 94 | new LidarEdgeFactor_modify(curr_point_, last_point_a_, last_point_b_, s_, weight_))); 95 | } 96 | 97 | Eigen::Vector3d curr_point, last_point_a, last_point_b; 98 | double s; 99 | double weight; 100 | }; 101 | 102 | struct LidarPlaneFactor 103 | { 104 | LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_, 105 | Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_) 106 | : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), 107 | last_point_m(last_point_m_), s(s_) 108 | { 109 | ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m); 110 | ljm_norm.normalize(); 111 | } 112 | 113 | template 114 | bool operator()(const T *q, const T *t, T *residual) const 115 | { 116 | 117 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 118 | Eigen::Matrix lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())}; 119 | //Eigen::Matrix lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())}; 120 | //Eigen::Matrix lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())}; 121 | Eigen::Matrix ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())}; 122 | 123 | //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; 124 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 125 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 126 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 127 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 128 | 129 | Eigen::Matrix lp; 130 | lp = q_last_curr * cp + t_last_curr; 131 | 132 | residual[0] = (lp - lpj).dot(ljm); 133 | 134 | return true; 135 | } 136 | 137 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_, 138 | const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_, 139 | const double s_) 140 | { 141 | return (new ceres::AutoDiffCostFunction< 142 | LidarPlaneFactor, 1, 4, 3>( 143 | new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_))); 144 | } 145 | 146 | Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m; 147 | Eigen::Vector3d ljm_norm; 148 | double s; 149 | }; 150 | 151 | struct LidarPlaneFactor_modify_test 152 | { 153 | LidarPlaneFactor_modify_test(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_, 154 | Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_, double weight_) 155 | : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), 156 | last_point_m(last_point_m_), s(s_), weight(weight_) 157 | { 158 | ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m); 159 | ljm_norm.normalize(); 160 | } 161 | 162 | template 163 | bool operator()(const T *q, const T *t, T *residual) const 164 | { 165 | 166 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 167 | Eigen::Matrix lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())}; 168 | //Eigen::Matrix lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())}; 169 | //Eigen::Matrix lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())}; 170 | Eigen::Matrix ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())}; 171 | 172 | //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; 173 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 174 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 175 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 176 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 177 | 178 | Eigen::Matrix lp; 179 | lp = q_last_curr * cp + t_last_curr; 180 | Eigen::Matrix out; 181 | out = lp - lpj; 182 | residual[0] = out.x() * ljm.x() * weight;//(lp - lpj).dot(ljm); 183 | residual[1] = out.y() * ljm.y() * weight; 184 | residual[2] = out.z() * ljm.z() * weight *1.1; 185 | return true; 186 | } 187 | 188 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_, 189 | const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_, 190 | const double s_, const double weight_) 191 | { 192 | return (new ceres::AutoDiffCostFunction< 193 | LidarPlaneFactor_modify_test, 3, 4, 3>( 194 | new LidarPlaneFactor_modify_test(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_, weight_))); 195 | } 196 | 197 | Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m; 198 | Eigen::Vector3d ljm_norm; 199 | double s; 200 | double weight; 201 | }; 202 | 203 | struct LidarPlaneFactor_modify 204 | { 205 | LidarPlaneFactor_modify(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_, 206 | Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_, double weight_) 207 | : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), 208 | last_point_m(last_point_m_), s(s_), weight(weight_) 209 | { 210 | ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m); 211 | ljm_norm.normalize(); 212 | } 213 | 214 | template 215 | bool operator()(const T *q, const T *t, T *residual) const 216 | { 217 | 218 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 219 | Eigen::Matrix lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())}; 220 | //Eigen::Matrix lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())}; 221 | //Eigen::Matrix lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())}; 222 | Eigen::Matrix ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())}; 223 | 224 | //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; 225 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 226 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 227 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 228 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 229 | 230 | Eigen::Matrix lp; 231 | lp = q_last_curr * cp + t_last_curr; 232 | 233 | residual[0] = (lp - lpj).dot(ljm) * weight; 234 | 235 | return true; 236 | } 237 | 238 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_, 239 | const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_, 240 | const double s_, const double weight_) 241 | { 242 | return (new ceres::AutoDiffCostFunction< 243 | LidarPlaneFactor_modify, 1, 4, 3>( 244 | new LidarPlaneFactor_modify(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_, weight_))); 245 | } 246 | 247 | Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m; 248 | Eigen::Vector3d ljm_norm; 249 | double s; 250 | double weight; 251 | }; 252 | 253 | struct LidarPlaneNormFactor 254 | { 255 | 256 | LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, 257 | double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), 258 | negative_OA_dot_norm(negative_OA_dot_norm_) {} 259 | 260 | template 261 | bool operator()(const T *q, const T *t, T *residual) const 262 | { 263 | Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; 264 | Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; 265 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 266 | Eigen::Matrix point_w; 267 | point_w = q_w_curr * cp + t_w_curr; 268 | 269 | Eigen::Matrix norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z())); 270 | residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm); 271 | return true; 272 | } 273 | 274 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_, 275 | const double negative_OA_dot_norm_) 276 | { 277 | return (new ceres::AutoDiffCostFunction< 278 | LidarPlaneNormFactor, 1, 4, 3>( 279 | new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_))); 280 | } 281 | 282 | Eigen::Vector3d curr_point; 283 | Eigen::Vector3d plane_unit_norm; 284 | double negative_OA_dot_norm; 285 | }; 286 | 287 | 288 | struct LidarDistanceFactor 289 | { 290 | 291 | LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_) 292 | : curr_point(curr_point_), closed_point(closed_point_){} 293 | 294 | template 295 | bool operator()(const T *q, const T *t, T *residual) const 296 | { 297 | Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; 298 | Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; 299 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 300 | Eigen::Matrix point_w; 301 | point_w = q_w_curr * cp + t_w_curr; 302 | 303 | 304 | residual[0] = point_w.x() - T(closed_point.x()); 305 | residual[1] = point_w.y() - T(closed_point.y()); 306 | residual[2] = point_w.z() - T(closed_point.z()); 307 | return true; 308 | } 309 | 310 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d closed_point_) 311 | { 312 | return (new ceres::AutoDiffCostFunction< 313 | LidarDistanceFactor, 3, 4, 3>( 314 | new LidarDistanceFactor(curr_point_, closed_point_))); 315 | } 316 | 317 | Eigen::Vector3d curr_point; 318 | Eigen::Vector3d closed_point; 319 | }; 320 | -------------------------------------------------------------------------------- /src/scanRegistration.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "aloam_velodyne/common.h" 5 | #include "aloam_velodyne/tic_toc.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | using std::atan2; 20 | using std::cos; 21 | using std::sin; 22 | //new modify 23 | float lowerBound; 24 | float upBound; 25 | float _factor; 26 | // 27 | 28 | const double scanPeriod = 0.1; 29 | 30 | const int systemDelay = 0; 31 | int systemInitCount = 0; 32 | bool systemInited = false; 33 | int N_SCANS = 0; 34 | float cloudCurvature[400000]; 35 | float cloudCurvature_scaled[400000]; 36 | float angle_feature[400000]; 37 | float angle_std [400000]; 38 | int cloudSortInd[400000]; 39 | int cloudNeighborPicked[400000]; 40 | int cloudLabel[400000]; 41 | 42 | bool comp (int i,int j) { return (cloudCurvature[i] angle_feature[j]); } 45 | 46 | ros::Publisher pubLaserCloud; 47 | ros::Publisher pubCornerPointsSharp; 48 | ros::Publisher pubCornerPointsLessSharp; 49 | ros::Publisher pubSurfPointsFlat; 50 | ros::Publisher pubSurfPointsLessFlat; 51 | ros::Publisher pubRemovePoints; 52 | std::vector pubEachScan; 53 | 54 | bool PUB_EACH_LINE = false; 55 | 56 | double MINIMUM_RANGE = 0.1; 57 | 58 | template 59 | void removeClosedPointCloud(const pcl::PointCloud &cloud_in, 60 | pcl::PointCloud &cloud_out, float thres) 61 | { 62 | if (&cloud_in != &cloud_out) 63 | { 64 | cloud_out.header = cloud_in.header; 65 | cloud_out.points.resize(cloud_in.points.size()); 66 | } 67 | 68 | size_t j = 0; 69 | 70 | for (size_t i = 0; i < cloud_in.points.size(); ++i) 71 | { 72 | if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres) 73 | continue; 74 | cloud_out.points[j] = cloud_in.points[i]; 75 | j++; 76 | } 77 | if (j != cloud_in.points.size()) 78 | { 79 | cloud_out.points.resize(j); 80 | } 81 | 82 | cloud_out.height = 1; 83 | cloud_out.width = static_cast(j); 84 | cloud_out.is_dense = true; 85 | } 86 | 87 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 88 | { 89 | if (!systemInited) 90 | { 91 | systemInitCount++; 92 | if (systemInitCount >= systemDelay) 93 | { 94 | systemInited = true; 95 | } 96 | else 97 | return; 98 | } 99 | 100 | TicToc t_whole; 101 | TicToc t_prepare; 102 | std::vector scanStartInd(N_SCANS, 0); 103 | std::vector scanEndInd(N_SCANS, 0); 104 | 105 | pcl::PointCloud laserCloudIn; 106 | pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); 107 | std::vector indices; 108 | 109 | pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices); 110 | removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE); 111 | 112 | 113 | int cloudSize = laserCloudIn.points.size(); 114 | float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); 115 | float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, 116 | laserCloudIn.points[cloudSize - 1].x) + 117 | 2 * M_PI; 118 | 119 | if (endOri - startOri > 3 * M_PI) 120 | { 121 | endOri -= 2 * M_PI; 122 | } 123 | else if (endOri - startOri < M_PI) 124 | { 125 | endOri += 2 * M_PI; 126 | } 127 | ////printf("end Ori %f\n", endOri); 128 | 129 | bool halfPassed = false; 130 | int count = cloudSize; 131 | PointType point; 132 | std::vector> laserCloudScans(N_SCANS); 133 | for (int i = 0; i < cloudSize; i++) 134 | { 135 | point.x = laserCloudIn.points[i].x; 136 | point.y = laserCloudIn.points[i].y; 137 | point.z = laserCloudIn.points[i].z; 138 | 139 | float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI; 140 | int scanID = 0; 141 | 142 | if (N_SCANS == 16) 143 | { 144 | scanID = int((angle + 15) / 2 + 0.5); 145 | if (scanID > (N_SCANS - 1) || scanID < 0) 146 | { 147 | count--; 148 | continue; 149 | } 150 | } 151 | else if (N_SCANS == 32) 152 | { 153 | scanID = int((angle + 92.0/3.0) * 3.0 / 4.0); 154 | if (scanID > (N_SCANS - 1) || scanID < 0) 155 | { 156 | count--; 157 | continue; 158 | } 159 | } 160 | else if (N_SCANS == 64) 161 | { 162 | scanID = int((angle - lowerBound) * _factor + 0.5); 163 | 164 | if (scanID >= N_SCANS || scanID < 0) 165 | { 166 | count--; 167 | continue; 168 | } 169 | } 170 | else 171 | { 172 | //printf("wrong scan number\n"); 173 | ROS_BREAK(); 174 | } 175 | ////printf("angle %f scanID %d \n", angle, scanID); 176 | 177 | float ori = -atan2(point.y, point.x); 178 | if (!halfPassed) 179 | { 180 | if (ori < startOri - M_PI / 2) 181 | { 182 | ori += 2 * M_PI; 183 | } 184 | else if (ori > startOri + M_PI * 3 / 2) 185 | { 186 | ori -= 2 * M_PI; 187 | } 188 | 189 | if (ori - startOri > M_PI) 190 | { 191 | halfPassed = true; 192 | } 193 | } 194 | else 195 | { 196 | ori += 2 * M_PI; 197 | if (ori < endOri - M_PI * 3 / 2) 198 | { 199 | ori += 2 * M_PI; 200 | } 201 | else if (ori > endOri + M_PI / 2) 202 | { 203 | ori -= 2 * M_PI; 204 | } 205 | } 206 | 207 | float relTime = (ori - startOri) / (endOri - startOri); 208 | point.intensity = scanID + scanPeriod * relTime; 209 | laserCloudScans[scanID].push_back(point); 210 | } 211 | 212 | cloudSize = count; 213 | //printf("points size %d \n", cloudSize); 214 | 215 | pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); 216 | for (int i = 0; i < N_SCANS; i++) 217 | { 218 | scanStartInd[i] = laserCloud->size() + 5; 219 | *laserCloud += laserCloudScans[i]; 220 | scanEndInd[i] = laserCloud->size() - 6; 221 | } 222 | 223 | //printf("prepare time %f \n", t_prepare.toc()); 224 | 225 | for (int i = 5; i < cloudSize - 5; i++) 226 | { 227 | float weight = 1 + .5 + 1./3 + .25 + .2; 228 | float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x; 229 | float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y; 230 | float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z; 231 | cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ; 232 | cloudSortInd[i] = i; 233 | cloudNeighborPicked[i] = 0; 234 | cloudLabel[i] = 0; 235 | } 236 | 237 | 238 | TicToc t_pts; 239 | 240 | pcl::PointCloud cornerPointsSharp; 241 | pcl::PointCloud cornerPointsLessSharp; 242 | pcl::PointCloud surfPointsFlat; 243 | pcl::PointCloud surfPointsLessFlat; 244 | 245 | float t_q_sort = 0; 246 | for (int i = 0; i < N_SCANS; i++) 247 | { 248 | if( scanEndInd[i] - scanStartInd[i] < 6) 249 | continue; 250 | pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud); 251 | for (int j = 0; j < 6; j++) 252 | { 253 | int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; 254 | int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1; 255 | 256 | TicToc t_tmp; 257 | std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp); 258 | t_q_sort += t_tmp.toc(); 259 | 260 | int largestPickedNum = 0; 261 | for (int k = ep; k >= sp; k--) 262 | { 263 | int ind = cloudSortInd[k]; 264 | 265 | if (cloudNeighborPicked[ind] == 0 && 266 | cloudCurvature[ind] > 0.1) 267 | { 268 | 269 | largestPickedNum++; 270 | if (largestPickedNum <= 2) 271 | { 272 | cloudLabel[ind] = 2; 273 | cornerPointsSharp.push_back(laserCloud->points[ind]); 274 | cornerPointsLessSharp.push_back(laserCloud->points[ind]); 275 | } 276 | else if (largestPickedNum <= 20) 277 | { 278 | cloudLabel[ind] = 1; 279 | cornerPointsLessSharp.push_back(laserCloud->points[ind]); 280 | } 281 | else 282 | { 283 | break; 284 | } 285 | 286 | cloudNeighborPicked[ind] = 1; 287 | 288 | for (int l = 1; l <= 5; l++) 289 | { 290 | float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x; 291 | float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y; 292 | float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z; 293 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 294 | { 295 | break; 296 | } 297 | 298 | cloudNeighborPicked[ind + l] = 1; 299 | } 300 | for (int l = -1; l >= -5; l--) 301 | { 302 | float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x; 303 | float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y; 304 | float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z; 305 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 306 | { 307 | break; 308 | } 309 | 310 | cloudNeighborPicked[ind + l] = 1; 311 | } 312 | } 313 | } 314 | 315 | int smallestPickedNum = 0; 316 | for (int k = sp; k <= ep; k++) 317 | { 318 | int ind = cloudSortInd[k]; 319 | 320 | if (cloudNeighborPicked[ind] == 0 && 321 | cloudCurvature[ind] < 0.1) 322 | { 323 | 324 | cloudLabel[ind] = -1; 325 | surfPointsFlat.push_back(laserCloud->points[ind]); 326 | 327 | smallestPickedNum++; 328 | if (smallestPickedNum >= 4) 329 | { 330 | break; 331 | } 332 | 333 | cloudNeighborPicked[ind] = 1; 334 | for (int l = 1; l <= 5; l++) 335 | { 336 | float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x; 337 | float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y; 338 | float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z; 339 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 340 | { 341 | break; 342 | } 343 | 344 | cloudNeighborPicked[ind + l] = 1; 345 | } 346 | for (int l = -1; l >= -5; l--) 347 | { 348 | float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x; 349 | float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y; 350 | float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z; 351 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 352 | { 353 | break; 354 | } 355 | 356 | cloudNeighborPicked[ind + l] = 1; 357 | } 358 | } 359 | } 360 | 361 | for (int k = sp; k <= ep; k++) 362 | { 363 | if (cloudLabel[k] <= 0) 364 | { 365 | surfPointsLessFlatScan->push_back(laserCloud->points[k]); 366 | } 367 | } 368 | } 369 | 370 | pcl::PointCloud surfPointsLessFlatScanDS; 371 | pcl::VoxelGrid downSizeFilter; 372 | downSizeFilter.setInputCloud(surfPointsLessFlatScan); 373 | downSizeFilter.setLeafSize(0.2, 0.2, 0.2); 374 | downSizeFilter.filter(surfPointsLessFlatScanDS); 375 | 376 | surfPointsLessFlat += surfPointsLessFlatScanDS; 377 | } 378 | //printf("sort q time %f \n", t_q_sort); 379 | //printf("seperate points time %f \n", t_pts.toc()); 380 | 381 | 382 | sensor_msgs::PointCloud2 laserCloudOutMsg; 383 | pcl::toROSMsg(*laserCloud, laserCloudOutMsg); 384 | laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; 385 | laserCloudOutMsg.header.frame_id = laserCloudMsg->header.frame_id; 386 | pubLaserCloud.publish(laserCloudOutMsg); 387 | 388 | sensor_msgs::PointCloud2 cornerPointsSharpMsg; 389 | pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg); 390 | cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp; 391 | cornerPointsSharpMsg.header.frame_id = laserCloudMsg->header.frame_id; 392 | pubCornerPointsSharp.publish(cornerPointsSharpMsg); 393 | 394 | sensor_msgs::PointCloud2 cornerPointsLessSharpMsg; 395 | pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg); 396 | cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp; 397 | cornerPointsLessSharpMsg.header.frame_id = laserCloudMsg->header.frame_id; 398 | pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg); 399 | 400 | sensor_msgs::PointCloud2 surfPointsFlat2; 401 | pcl::toROSMsg(surfPointsFlat, surfPointsFlat2); 402 | surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp; 403 | surfPointsFlat2.header.frame_id = laserCloudMsg->header.frame_id; 404 | pubSurfPointsFlat.publish(surfPointsFlat2); 405 | 406 | sensor_msgs::PointCloud2 surfPointsLessFlat2; 407 | pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2); 408 | surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp; 409 | surfPointsLessFlat2.header.frame_id = laserCloudMsg->header.frame_id; 410 | pubSurfPointsLessFlat.publish(surfPointsLessFlat2); 411 | 412 | // pub each scam 413 | if(PUB_EACH_LINE) 414 | { 415 | for(int i = 0; i< N_SCANS; i++) 416 | { 417 | sensor_msgs::PointCloud2 scanMsg; 418 | pcl::toROSMsg(laserCloudScans[i], scanMsg); 419 | scanMsg.header.stamp = laserCloudMsg->header.stamp; 420 | scanMsg.header.frame_id = laserCloudMsg->header.frame_id; 421 | pubEachScan[i].publish(scanMsg); 422 | } 423 | } 424 | 425 | //printf("scan registration time %f ms *************\n", t_whole.toc()); 426 | if(t_whole.toc() > 100) 427 | ROS_WARN("scan registration process over 100ms"); 428 | } 429 | 430 | int main(int argc, char **argv) 431 | { 432 | ros::init(argc, argv, "scanRegistration"); 433 | ros::NodeHandle nh; 434 | 435 | nh.param("scan_line", N_SCANS, 16); 436 | 437 | // std::cout << "scan_line: " << N_SCANS << std::endl; 438 | nh.param("minimum_range", MINIMUM_RANGE, 0.1); 439 | nh.param("lowerBound", lowerBound, -24.9); 440 | nh.param("upBound", upBound, 2); 441 | _factor = (N_SCANS-1) / (upBound - lowerBound); 442 | 443 | nh.param("minimum_range", MINIMUM_RANGE, 0.1); 444 | 445 | //printf("scan line number %d \n", N_SCANS); 446 | 447 | if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64) 448 | { 449 | //printf("only support velodyne with 16, 32 or 64 scan line!"); 450 | return 0; 451 | } 452 | 453 | ros::Subscriber subLaserCloud = nh.subscribe("/rslidar_points", 100, laserCloudHandler); 454 | 455 | pubLaserCloud = nh.advertise("/velodyne_cloud_2", 100); 456 | 457 | pubCornerPointsSharp = nh.advertise("/laser_cloud_sharp", 100); 458 | 459 | pubCornerPointsLessSharp = nh.advertise("/laser_cloud_less_sharp", 100); 460 | 461 | pubSurfPointsFlat = nh.advertise("/laser_cloud_flat", 100); 462 | 463 | pubSurfPointsLessFlat = nh.advertise("/laser_cloud_less_flat", 100); 464 | 465 | pubRemovePoints = nh.advertise("/laser_remove_points", 100); 466 | 467 | if(PUB_EACH_LINE) 468 | { 469 | for(int i = 0; i < N_SCANS; i++) 470 | { 471 | ros::Publisher tmp = nh.advertise("/laser_scanid_" + std::to_string(i), 100); 472 | pubEachScan.push_back(tmp); 473 | } 474 | } 475 | ros::spin(); 476 | 477 | return 0; 478 | } 479 | --------------------------------------------------------------------------------