├── CMakeLists.txt ├── LICENSE ├── README.md ├── doc └── results │ ├── horizon_outdoor_01.png │ ├── horizon_parking.gif │ ├── horizon_parking_01.png │ ├── mid100_01.png │ ├── mid100_02.png │ ├── mid40_hall.gif │ ├── mid40_hall_01.png │ └── mid40_outdoor.png ├── launch ├── mapping_horizon.launch ├── mapping_mid.launch └── mapping_outdoor.launch ├── package.xml ├── rviz_cfg ├── .gitignore └── loam_livox.rviz └── src ├── laserMapping.cpp ├── livox_repub.cpp ├── scanRegistration.cpp └── scanRegistration_horizon.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(livox_mapping) 3 | 4 | SET(CMAKE_BUILD_TYPE "Debug") 5 | 6 | #set(CMAKE_CXX_COMPILER "/usr/bin/clang++") 7 | 8 | #set(CMAKE_C_COMPILER "/usr/bin/clang") 9 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions") 10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -pthread -std=c++0x -std=c++14 -fexceptions -Wno-unused-local-typedefs") 11 | 12 | find_package(OpenMP QUIET) 13 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 14 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 15 | 16 | find_package(catkin REQUIRED COMPONENTS 17 | geometry_msgs 18 | nav_msgs 19 | sensor_msgs 20 | roscpp 21 | rospy 22 | std_msgs 23 | pcl_ros 24 | tf 25 | livox_ros_driver 26 | ) 27 | 28 | find_package(Eigen3 REQUIRED) 29 | find_package(PCL REQUIRED) 30 | find_package(OpenCV REQUIRED) 31 | 32 | include_directories( 33 | ${catkin_INCLUDE_DIRS} 34 | ${EIGEN3_INCLUDE_DIR} 35 | ${PCL_INCLUDE_DIRS}) 36 | 37 | catkin_package( 38 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 39 | DEPENDS EIGEN3 PCL OpenCV 40 | INCLUDE_DIRS 41 | ) 42 | 43 | add_executable(livox_repub src/livox_repub.cpp) 44 | target_link_libraries(livox_repub ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 45 | 46 | add_executable(loam_scanRegistration src/scanRegistration.cpp) 47 | target_link_libraries(loam_scanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 48 | 49 | add_executable(loam_scanRegistration_horizon src/scanRegistration_horizon.cpp) 50 | target_link_libraries(loam_scanRegistration_horizon ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 51 | 52 | add_executable(loam_laserMapping src/laserMapping.cpp) 53 | target_link_libraries(loam_laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 54 | 55 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | This is an advanced implementation of the algorithm described in the following paper: 2 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | Modifier: livox dev@livoxtech.com 6 | 7 | Copyright 2013, Ji Zhang, Carnegie Mellon University 8 | Further contributions copyright (c) 2016, Southwest Research Institute 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | 1. Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 2. Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 3. Neither the name of the copyright holder nor the names of its contributors 20 | may be used to endorse or promote products derived from this software without 21 | specific prior written permission. 22 | 23 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 24 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 25 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 27 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,OR 31 | TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | 35 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Livox_mapping 2 | Livox_mapping is a mapping package for Livox LiDARs. 3 | The package currently contains the basic functions of low-speed mapping. 4 | 5 |
6 | 7 | 8 |
9 | 10 | Some key issues: 11 | 1. Support multiple livox lidar; 12 | 2. Different feature extraction; 13 | 3. Remove odometry for small FOV situation; 14 | 15 | In the development of our package, we reference to LOAM, LOAM_NOTED. 16 | ## 1. Prerequisites 17 | ### 1.1 **Ubuntu** and **ROS** 18 | Ubuntu 64-bit 16.04 or 18.04. 19 | ROS Kinetic or Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation) 20 | 21 | ### 1.2. **PCL && Eigen && openCV** 22 | Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html). 23 | Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page). 24 | Follow [openCV Installation](https://opencv.org/releases/). 25 | 26 | ### 1.3. **livox_ros_driver** 27 | Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver). 28 | 29 | 30 | ## 2. Build 31 | Clone the repository and catkin_make: 32 | 33 | ``` 34 | cd ~/catkin_ws/src 35 | git clone https://github.com/Livox-SDK/livox_mapping.git 36 | cd .. 37 | catkin_make 38 | source ~/catkin_ws/devel/setup.bash 39 | ``` 40 | 41 | *Remarks:* 42 | - If you want to save the pcd file please add map_file_path in launch file. 43 | ## 3. Directly run 44 | ### 3.1 Livox Mid-40 45 | Connect to your PC to Livox LiDAR (mid40) by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then 46 | ``` 47 | .... 48 | roslaunch livox_mapping mapping_mid.launch 49 | roslaunch livox_ros_driver livox_lidar.launch 50 | 51 | ``` 52 | ### 3.2 Livox Horizon 53 | Connect to your PC to Livox LiDAR (Horizon) by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then 54 | ``` 55 | .... 56 | roslaunch livox_mapping mapping_horizon.launch 57 | roslaunch livox_ros_driver livox_lidar_msg.launch 58 | 59 | ``` 60 | ## 4. Rosbag Example 61 | ### 4.1 Livox Mid-40 rosbag 62 | 63 |
64 | 65 |
66 | 67 | Download [mid40_hall_example](https://terra-1-g.djicdn.com/65c028cd298f4669a7f0e40e50ba1131/Showcase/mid40_hall_example.bag) or [mid40_outdoor](https://terra-1-g.djicdn.com/65c028cd298f4669a7f0e40e50ba1131/Showcase/mid40_outdoor.bag) 68 | and then 69 | ``` 70 | roslaunch livox_mapping mapping_mid.launch 71 | rosbag play YOUR_DOWNLOADED.bag 72 | ``` 73 | ### 4.2 Livox Mid-100 rosbag 74 | 75 |
76 | 77 |
78 | 79 | Download [mid100_example](https://terra-1-g.djicdn.com/65c028cd298f4669a7f0e40e50ba1131/Showcase/mid100_example.bag) and then 80 | ``` 81 | roslaunch livox_mapping mapping_mid.launch 82 | rosbag play YOUR_DOWNLOADED.bag 83 | ``` 84 | ### 4.3 Livox Horizon rosbag 85 | 86 |
87 | 88 |
89 | 90 | Download [horizon_parking](https://terra-1-g.djicdn.com/65c028cd298f4669a7f0e40e50ba1131/Showcase/horizon_parking.bag) or [horizon_outdoor](https://terra-1-g.djicdn.com/65c028cd298f4669a7f0e40e50ba1131/Showcase/horizon_outdoor.bag) 91 | and then 92 | ``` 93 | roslaunch livox_mapping mapping_horizon.launch 94 | rosbag play YOUR_DOWNLOADED.bag 95 | ``` 96 | ## 5.Acknowledgments 97 | Thanks for LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), [LOAM_NOTED](https://github.com/cuitaixiang/LOAM_NOTED). 98 | -------------------------------------------------------------------------------- /doc/results/horizon_outdoor_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Livox-SDK/livox_mapping/78b293893290f0611a727fc42e1dc1bed8e447a9/doc/results/horizon_outdoor_01.png -------------------------------------------------------------------------------- /doc/results/horizon_parking.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Livox-SDK/livox_mapping/78b293893290f0611a727fc42e1dc1bed8e447a9/doc/results/horizon_parking.gif -------------------------------------------------------------------------------- /doc/results/horizon_parking_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Livox-SDK/livox_mapping/78b293893290f0611a727fc42e1dc1bed8e447a9/doc/results/horizon_parking_01.png -------------------------------------------------------------------------------- /doc/results/mid100_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Livox-SDK/livox_mapping/78b293893290f0611a727fc42e1dc1bed8e447a9/doc/results/mid100_01.png -------------------------------------------------------------------------------- /doc/results/mid100_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Livox-SDK/livox_mapping/78b293893290f0611a727fc42e1dc1bed8e447a9/doc/results/mid100_02.png -------------------------------------------------------------------------------- /doc/results/mid40_hall.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Livox-SDK/livox_mapping/78b293893290f0611a727fc42e1dc1bed8e447a9/doc/results/mid40_hall.gif -------------------------------------------------------------------------------- /doc/results/mid40_hall_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Livox-SDK/livox_mapping/78b293893290f0611a727fc42e1dc1bed8e447a9/doc/results/mid40_hall_01.png -------------------------------------------------------------------------------- /doc/results/mid40_outdoor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Livox-SDK/livox_mapping/78b293893290f0611a727fc42e1dc1bed8e447a9/doc/results/mid40_outdoor.png -------------------------------------------------------------------------------- /launch/mapping_horizon.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /launch/mapping_mid.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /launch/mapping_outdoor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | livox_mapping 4 | 0.0.0 5 | 6 | 7 | This is a modified version of LOAM which is original algorithm 8 | is described in the following paper: 9 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 10 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 11 | 12 | 13 | claydergc 14 | 15 | BSD 16 | 17 | Ji Zhang 18 | 19 | catkin 20 | geometry_msgs 21 | nav_msgs 22 | roscpp 23 | rospy 24 | std_msgs 25 | sensor_msgs 26 | tf 27 | pcl_ros 28 | livox_ros_driver 29 | 30 | geometry_msgs 31 | nav_msgs 32 | sensor_msgs 33 | roscpp 34 | rospy 35 | std_msgs 36 | tf 37 | pcl_ros 38 | livox_ros_driver 39 | 40 | rostest 41 | rosbag 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /rviz_cfg/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Livox-SDK/livox_mapping/78b293893290f0611a727fc42e1dc1bed8e447a9/rviz_cfg/.gitignore -------------------------------------------------------------------------------- /rviz_cfg/loam_livox.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 | - /Axes1 10 | - /odometry1 11 | - /odometry1/odomPath1 12 | - /mapping1 13 | - /mapping1/mappingPath1 14 | - /mapping1/surround1 15 | - /mapping1/currPoints1 16 | - /Debug1/PointCloud21 17 | - /Debug1/PointCloud22 18 | - /Debug1/PointCloud23 19 | - /Odometry1 20 | - /Odometry1/Odometry1 21 | - /Odometry1/Odometry1/Shape1 22 | - /Odometry1/Odometry2 23 | - /Odometry1/Odometry2/Shape1 24 | - /Odometry1/Odometry3 25 | - /Odometry1/Odometry3/Shape1 26 | - /Features1 27 | - /Features1/PointCloud21 28 | - /Axes2 29 | Splitter Ratio: 0.5 30 | Tree Height: 755 31 | - Class: rviz/Selection 32 | Name: Selection 33 | - Class: rviz/Tool Properties 34 | Expanded: 35 | - /2D Pose Estimate1 36 | - /2D Nav Goal1 37 | - /Publish Point1 38 | Name: Tool Properties 39 | Splitter Ratio: 0.588679016 40 | - Class: rviz/Views 41 | Expanded: 42 | - /Current View1 43 | Name: Views 44 | Splitter Ratio: 0.5 45 | - Class: rviz/Time 46 | Experimental: false 47 | Name: Time 48 | SyncMode: 0 49 | SyncSource: currPoints 50 | Visualization Manager: 51 | Class: "" 52 | Displays: 53 | - Alpha: 0.5 54 | Cell Size: 1 55 | Class: rviz/Grid 56 | Color: 160; 160; 164 57 | Enabled: false 58 | Line Style: 59 | Line Width: 0.0299999993 60 | Value: Lines 61 | Name: Grid 62 | Normal Cell Count: 0 63 | Offset: 64 | X: 0 65 | Y: 0 66 | Z: 0 67 | Plane: XY 68 | Plane Cell Count: 160 69 | Reference Frame: 70 | Value: false 71 | - Class: rviz/Axes 72 | Enabled: true 73 | Length: 0.5 74 | Name: Axes 75 | Radius: 0.100000001 76 | Reference Frame: 77 | Value: true 78 | - Class: rviz/Group 79 | Displays: 80 | - Alpha: 1 81 | Buffer Length: 1 82 | Class: rviz/Path 83 | Color: 255; 170; 0 84 | Enabled: false 85 | Head Diameter: 0.300000012 86 | Head Length: 0.200000003 87 | Length: 0.300000012 88 | Line Style: Lines 89 | Line Width: 0.0299999993 90 | Name: gtPathlc 91 | Offset: 92 | X: 0 93 | Y: 0 94 | Z: 0 95 | Pose Color: 255; 85; 255 96 | Pose Style: None 97 | Radius: 0.0299999993 98 | Shaft Diameter: 0.100000001 99 | Shaft Length: 0.100000001 100 | Topic: /path_gt 101 | Unreliable: false 102 | Value: false 103 | - Alpha: 1 104 | Buffer Length: 1 105 | Class: rviz/Path 106 | Color: 255; 170; 0 107 | Enabled: true 108 | Head Diameter: 0.300000012 109 | Head Length: 0.200000003 110 | Length: 0.300000012 111 | Line Style: Lines 112 | Line Width: 0.0299999993 113 | Name: gtPathLidar 114 | Offset: 115 | X: 0 116 | Y: 0 117 | Z: 0 118 | Pose Color: 255; 85; 255 119 | Pose Style: None 120 | Radius: 0.0299999993 121 | Shaft Diameter: 0.100000001 122 | Shaft Length: 0.100000001 123 | Topic: /path_gt_lidar 124 | Unreliable: false 125 | Value: true 126 | Enabled: false 127 | Name: GT 128 | - Class: rviz/Group 129 | Displays: 130 | - Alpha: 1 131 | Buffer Length: 1 132 | Class: rviz/Path 133 | Color: 255; 85; 0 134 | Enabled: false 135 | Head Diameter: 0.300000012 136 | Head Length: 0.200000003 137 | Length: 0.300000012 138 | Line Style: Lines 139 | Line Width: 0.100000001 140 | Name: odomPath 141 | Offset: 142 | X: 0 143 | Y: 0 144 | Z: 0 145 | Pose Color: 255; 85; 255 146 | Pose Style: None 147 | Radius: 0.0299999993 148 | Shaft Diameter: 0.100000001 149 | Shaft Length: 0.100000001 150 | Topic: /laser_odom_path 151 | Unreliable: false 152 | Value: false 153 | Enabled: true 154 | Name: odometry 155 | - Class: rviz/Group 156 | Displays: 157 | - Alpha: 1 158 | Buffer Length: 1 159 | Class: rviz/Path 160 | Color: 255; 85; 0 161 | Enabled: false 162 | Head Diameter: 0.300000012 163 | Head Length: 0.200000003 164 | Length: 0.300000012 165 | Line Style: Lines 166 | Line Width: 0.0299999993 167 | Name: mappingPath 168 | Offset: 169 | X: 0 170 | Y: 0 171 | Z: 0 172 | Pose Color: 255; 85; 255 173 | Pose Style: None 174 | Radius: 0.0299999993 175 | Shaft Diameter: 0.100000001 176 | Shaft Length: 0.100000001 177 | Topic: /laser_odom_path 178 | Unreliable: false 179 | Value: false 180 | - Alpha: 1 181 | Autocompute Intensity Bounds: true 182 | Autocompute Value Bounds: 183 | Max Value: 10 184 | Min Value: -10 185 | Value: true 186 | Axis: Z 187 | Channel Name: intensity 188 | Class: rviz/PointCloud2 189 | Color: 255; 255; 255 190 | Color Transformer: FlatColor 191 | Decay Time: 0 192 | Enabled: true 193 | Invert Rainbow: false 194 | Max Color: 255; 255; 255 195 | Max Intensity: 15 196 | Min Color: 0; 0; 0 197 | Min Intensity: 0 198 | Name: surround 199 | Position Transformer: XYZ 200 | Queue Size: 1 201 | Selectable: false 202 | Size (Pixels): 3 203 | Size (m): 0.0500000007 204 | Style: Points 205 | Topic: /velodyne_cloud_registered 206 | Unreliable: false 207 | Use Fixed Frame: true 208 | Use rainbow: true 209 | Value: true 210 | - Alpha: 0.100000001 211 | Autocompute Intensity Bounds: true 212 | Autocompute Value Bounds: 213 | Max Value: 3.00941062 214 | Min Value: -3.43477297 215 | Value: true 216 | Axis: Z 217 | Channel Name: intensity 218 | Class: rviz/PointCloud2 219 | Color: 255; 255; 255 220 | Color Transformer: AxisColor 221 | Decay Time: 1e+09 222 | Enabled: true 223 | Invert Rainbow: true 224 | Max Color: 255; 255; 255 225 | Max Intensity: 90.0066299 226 | Min Color: 0; 0; 0 227 | Min Intensity: 0 228 | Name: currPoints 229 | Position Transformer: XYZ 230 | Queue Size: 100000 231 | Selectable: true 232 | Size (Pixels): 1 233 | Size (m): 0.00999999978 234 | Style: Points 235 | Topic: /velodyne_cloud_registered 236 | Unreliable: false 237 | Use Fixed Frame: true 238 | Use rainbow: true 239 | Value: true 240 | Enabled: true 241 | Name: mapping 242 | - Class: rviz/Group 243 | Displays: 244 | - Alpha: 1 245 | Autocompute Intensity Bounds: true 246 | Autocompute Value Bounds: 247 | Max Value: 10 248 | Min Value: -10 249 | Value: true 250 | Axis: Z 251 | Channel Name: intensity 252 | Class: rviz/PointCloud2 253 | Color: 255; 255; 255 254 | Color Transformer: FlatColor 255 | Decay Time: 100000 256 | Enabled: true 257 | Invert Rainbow: false 258 | Max Color: 255; 255; 255 259 | Max Intensity: 15.0027466 260 | Min Color: 0; 0; 0 261 | Min Intensity: -0.00289796153 262 | Name: PointCloud2 263 | Position Transformer: XYZ 264 | Queue Size: 10 265 | Selectable: true 266 | Size (Pixels): 3 267 | Size (m): 0.0500000007 268 | Style: Spheres 269 | Topic: /laser_cloud_sharp 270 | Unreliable: false 271 | Use Fixed Frame: true 272 | Use rainbow: true 273 | Value: true 274 | - Alpha: 1 275 | Autocompute Intensity Bounds: true 276 | Autocompute Value Bounds: 277 | Max Value: 2.30599999 278 | Min Value: -0.620000005 279 | Value: true 280 | Axis: Z 281 | Channel Name: intensity 282 | Class: rviz/PointCloud2 283 | Color: 98; 255; 245 284 | Color Transformer: FlatColor 285 | Decay Time: 1000000 286 | Enabled: true 287 | Invert Rainbow: false 288 | Max Color: 255; 255; 255 289 | Max Intensity: 353 290 | Min Color: 0; 0; 0 291 | Min Intensity: 2 292 | Name: PointCloud2 293 | Position Transformer: XYZ 294 | Queue Size: 10 295 | Selectable: true 296 | Size (Pixels): 3 297 | Size (m): 0.00999999978 298 | Style: Flat Squares 299 | Topic: /laser_cloud_flat 300 | Unreliable: false 301 | Use Fixed Frame: true 302 | Use rainbow: true 303 | Value: true 304 | - Alpha: 1 305 | Autocompute Intensity Bounds: true 306 | Autocompute Value Bounds: 307 | Max Value: 10 308 | Min Value: -10 309 | Value: true 310 | Axis: Z 311 | Channel Name: intensity 312 | Class: rviz/PointCloud2 313 | Color: 255; 255; 255 314 | Color Transformer: Intensity 315 | Decay Time: 100000 316 | Enabled: true 317 | Invert Rainbow: false 318 | Max Color: 255; 255; 255 319 | Max Intensity: 324 320 | Min Color: 0; 0; 0 321 | Min Intensity: 91 322 | Name: PointCloud2 323 | Position Transformer: XYZ 324 | Queue Size: 10 325 | Selectable: true 326 | Size (Pixels): 3 327 | Size (m): 0.00999999978 328 | Style: Flat Squares 329 | Topic: /laser_cloud_less_sharp 330 | Unreliable: false 331 | Use Fixed Frame: true 332 | Use rainbow: true 333 | Value: true 334 | Enabled: false 335 | Name: Debug 336 | - Class: rviz/Group 337 | Displays: 338 | - Angle Tolerance: 0.00999999978 339 | Class: rviz/Odometry 340 | Covariance: 341 | Orientation: 342 | Alpha: 0.5 343 | Color: 255; 255; 127 344 | Color Style: Unique 345 | Frame: Local 346 | Offset: 1 347 | Scale: 1 348 | Value: true 349 | Position: 350 | Alpha: 0.300000012 351 | Color: 204; 51; 204 352 | Scale: 1 353 | Value: true 354 | Value: true 355 | Enabled: true 356 | Keep: 1000 357 | Name: Odometry 358 | Position Tolerance: 0.00999999978 359 | Shape: 360 | Alpha: 1 361 | Axes Length: 0.5 362 | Axes Radius: 0.00999999978 363 | Color: 0; 255; 0 364 | Head Length: 0 365 | Head Radius: 0 366 | Shaft Length: 0.0500000007 367 | Shaft Radius: 0.0500000007 368 | Value: Arrow 369 | Topic: /re_local_pose 370 | Unreliable: false 371 | Value: true 372 | - Angle Tolerance: 0.00999999978 373 | Class: rviz/Odometry 374 | Covariance: 375 | Orientation: 376 | Alpha: 0.5 377 | Color: 255; 255; 127 378 | Color Style: Unique 379 | Frame: Local 380 | Offset: 1 381 | Scale: 1 382 | Value: true 383 | Position: 384 | Alpha: 0.300000012 385 | Color: 204; 51; 204 386 | Scale: 1 387 | Value: true 388 | Value: true 389 | Enabled: true 390 | Keep: 10000 391 | Name: Odometry 392 | Position Tolerance: 0.00100000005 393 | Shape: 394 | Alpha: 1 395 | Axes Length: 0.200000003 396 | Axes Radius: 0.0500000007 397 | Color: 255; 85; 0 398 | Head Length: 0 399 | Head Radius: 0 400 | Shaft Length: 0.0500000007 401 | Shaft Radius: 0.0500000007 402 | Value: Axes 403 | Topic: /aft_mapped_to_init 404 | Unreliable: false 405 | Value: true 406 | - Angle Tolerance: 0.100000001 407 | Class: rviz/Odometry 408 | Covariance: 409 | Orientation: 410 | Alpha: 0.5 411 | Color: 255; 255; 127 412 | Color Style: Unique 413 | Frame: Local 414 | Offset: 1 415 | Scale: 1 416 | Value: true 417 | Position: 418 | Alpha: 0.300000012 419 | Color: 204; 51; 204 420 | Scale: 1 421 | Value: true 422 | Value: true 423 | Enabled: true 424 | Keep: 1 425 | Name: Odometry 426 | Position Tolerance: 0.00100000005 427 | Shape: 428 | Alpha: 1 429 | Axes Length: 0.200000003 430 | Axes Radius: 0.0500000007 431 | Color: 255; 25; 0 432 | Head Length: 0.300000012 433 | Head Radius: 0.100000001 434 | Shaft Length: 1 435 | Shaft Radius: 0.0500000007 436 | Value: Axes 437 | Topic: /re_local_pose 438 | Unreliable: false 439 | Value: true 440 | Enabled: true 441 | Name: Odometry 442 | - Class: rviz/Group 443 | Displays: 444 | - Alpha: 1 445 | Autocompute Intensity Bounds: true 446 | Autocompute Value Bounds: 447 | Max Value: 10 448 | Min Value: -10 449 | Value: true 450 | Axis: Z 451 | Channel Name: intensity 452 | Class: rviz/PointCloud2 453 | Color: 255; 0; 0 454 | Color Transformer: FlatColor 455 | Decay Time: 0 456 | Enabled: false 457 | Invert Rainbow: false 458 | Max Color: 255; 255; 255 459 | Max Intensity: 38.0005989 460 | Min Color: 0; 0; 0 461 | Min Intensity: 1.00010002 462 | Name: PointCloud2 463 | Position Transformer: XYZ 464 | Queue Size: 10 465 | Selectable: true 466 | Size (Pixels): 3 467 | Size (m): 0.200000003 468 | Style: Points 469 | Topic: /laser_cloud_surround_corner 470 | Unreliable: false 471 | Use Fixed Frame: true 472 | Use rainbow: true 473 | Value: false 474 | Enabled: true 475 | Name: Features 476 | - Class: rviz/Axes 477 | Enabled: true 478 | Length: 1 479 | Name: Axes 480 | Radius: 0.100000001 481 | Reference Frame: aft_mapped 482 | Value: true 483 | Enabled: true 484 | Global Options: 485 | Background Color: 0; 0; 0 486 | Default Light: true 487 | Fixed Frame: /camera_init 488 | Frame Rate: 30 489 | Name: root 490 | Tools: 491 | - Class: rviz/Interact 492 | Hide Inactive Objects: true 493 | - Class: rviz/MoveCamera 494 | - Class: rviz/Select 495 | - Class: rviz/FocusCamera 496 | - Class: rviz/Measure 497 | - Class: rviz/SetInitialPose 498 | Topic: /initialpose 499 | - Class: rviz/SetGoal 500 | Topic: /move_base_simple/goal 501 | - Class: rviz/PublishPoint 502 | Single click: true 503 | Topic: /clicked_point 504 | Value: true 505 | Views: 506 | Current: 507 | Class: rviz/Orbit 508 | Distance: 49.6306572 509 | Enable Stereo Rendering: 510 | Stereo Eye Separation: 0.0599999987 511 | Stereo Focal Distance: 1 512 | Swap Stereo Eyes: false 513 | Value: false 514 | Focal Point: 515 | X: 27.9710369 516 | Y: -0.381309479 517 | Z: -4.58850336 518 | Focal Shape Fixed Size: true 519 | Focal Shape Size: 0.0500000007 520 | Invert Z Axis: false 521 | Name: Current View 522 | Near Clip Distance: 0.00999999978 523 | Pitch: 0.680201769 524 | Target Frame: 525 | Value: Orbit (rviz) 526 | Yaw: 2.38357234 527 | Saved: ~ 528 | Window Geometry: 529 | Displays: 530 | collapsed: false 531 | Height: 1056 532 | Hide Left Dock: false 533 | Hide Right Dock: true 534 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000382fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000382000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000618000001a80000000000000000000000010000015200000382fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000382000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000052fc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000038200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 535 | Selection: 536 | collapsed: false 537 | Time: 538 | collapsed: false 539 | Tool Properties: 540 | collapsed: false 541 | Views: 542 | collapsed: true 543 | Width: 1855 544 | X: 65 545 | Y: 24 546 | -------------------------------------------------------------------------------- /src/laserMapping.cpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 4 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 5 | 6 | // Modifier: Livox dev@livoxtech.com 7 | 8 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 9 | // Further contributions copyright (c) 2016, Southwest Research Institute 10 | // All rights reserved. 11 | // 12 | // Redistribution and use in source and binary forms, with or without 13 | // modification, are permitted provided that the following conditions are met: 14 | // 15 | // 1. Redistributions of source code must retain the above copyright notice, 16 | // this list of conditions and the following disclaimer. 17 | // 2. Redistributions in binary form must reproduce the above copyright notice, 18 | // this list of conditions and the following disclaimer in the documentation 19 | // and/or other materials provided with the distribution. 20 | // 3. Neither the name of the copyright holder nor the names of its 21 | // contributors may be used to endorse or promote products derived from this 22 | // software without specific prior written permission. 23 | // 24 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 28 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 29 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 30 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 31 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 32 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 33 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | // POSSIBILITY OF SUCH DAMAGE. 35 | 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | typedef pcl::PointXYZI PointType; 53 | 54 | int kfNum = 0; 55 | 56 | float timeLaserCloudCornerLast = 0; 57 | float timeLaserCloudSurfLast = 0; 58 | float timeLaserCloudFullRes = 0; 59 | 60 | bool newLaserCloudCornerLast = false; 61 | bool newLaserCloudSurfLast = false; 62 | bool newLaserCloudFullRes = false; 63 | 64 | int laserCloudCenWidth = 10; 65 | int laserCloudCenHeight = 5; 66 | int laserCloudCenDepth = 10; 67 | const int laserCloudWidth = 21; 68 | const int laserCloudHeight = 11; 69 | const int laserCloudDepth = 21; 70 | 71 | const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;//4851 72 | 73 | int laserCloudValidInd[125]; 74 | 75 | int laserCloudSurroundInd[125]; 76 | 77 | //corner feature 78 | pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud()); 79 | pcl::PointCloud::Ptr laserCloudCornerLast_down(new pcl::PointCloud()); 80 | //surf feature 81 | pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud()); 82 | pcl::PointCloud::Ptr laserCloudSurfLast_down(new pcl::PointCloud()); 83 | 84 | pcl::PointCloud::Ptr laserCloudOri(new pcl::PointCloud()); 85 | pcl::PointCloud::Ptr coeffSel(new pcl::PointCloud()); 86 | 87 | // pcl::PointCloud::Ptr laserCloudSurround(new pcl::PointCloud()); 88 | // pcl::PointCloud::Ptr laserCloudSurround_corner(new pcl::PointCloud()); 89 | 90 | pcl::PointCloud::Ptr laserCloudSurround2(new pcl::PointCloud()); 91 | pcl::PointCloud::Ptr laserCloudSurround2_corner(new pcl::PointCloud()); 92 | //corner feature in map 93 | pcl::PointCloud::Ptr laserCloudCornerFromMap(new pcl::PointCloud()); 94 | //surf feature in map 95 | pcl::PointCloud::Ptr laserCloudSurfFromMap(new pcl::PointCloud()); 96 | 97 | std::vector< Eigen::Matrix > keyframe_pose; 98 | std::vector< Eigen::Matrix4f > pose_map; 99 | //all points 100 | pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); 101 | pcl::PointCloud::Ptr laserCloudFullRes2(new pcl::PointCloud()); 102 | pcl::PointCloud::Ptr laserCloudFullResColor(new pcl::PointCloud()); 103 | pcl::PointCloud::Ptr laserCloudFullResColor_pcd(new pcl::PointCloud()); 104 | 105 | 106 | pcl::PointCloud::Ptr laserCloudCornerArray[laserCloudNum]; 107 | 108 | pcl::PointCloud::Ptr laserCloudSurfArray[laserCloudNum]; 109 | 110 | pcl::PointCloud::Ptr laserCloudCornerArray2[laserCloudNum]; 111 | 112 | pcl::PointCloud::Ptr laserCloudSurfArray2[laserCloudNum]; 113 | 114 | pcl::KdTreeFLANN::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN()); 115 | pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN()); 116 | 117 | //optimization states 118 | float transformTobeMapped[6] = {0}; 119 | //optimization states after mapping 120 | float transformAftMapped[6] = {0}; 121 | //last optimization states 122 | float transformLastMapped[6] = {0}; 123 | 124 | double rad2deg(double radians) 125 | { 126 | return radians * 180.0 / M_PI; 127 | } 128 | double deg2rad(double degrees) 129 | { 130 | return degrees * M_PI / 180.0; 131 | } 132 | Eigen::Matrix4f trans_euler_to_matrix(const float *trans) 133 | { 134 | Eigen::Matrix4f T = Eigen::Matrix4f::Identity(); 135 | Eigen::Matrix3f R; 136 | Eigen::AngleAxisf rollAngle(Eigen::AngleAxisf(trans[0],Eigen::Vector3f::UnitX())); 137 | Eigen::AngleAxisf pitchAngle(Eigen::AngleAxisf(trans[1],Eigen::Vector3f::UnitY())); 138 | Eigen::AngleAxisf yawAngle(Eigen::AngleAxisf(trans[2],Eigen::Vector3f::UnitZ())); 139 | R = pitchAngle * rollAngle * yawAngle; //zxy 140 | T.block<3,3>(0,0) = R; 141 | T.block<3,1>(0,3) = Eigen::Vector3f(trans[3],trans[4],trans[5]); 142 | 143 | return T; 144 | } 145 | void transformAssociateToMap() 146 | { 147 | Eigen::Matrix4f T_aft,T_last,T_predict; 148 | Eigen::Matrix3f R_predict; 149 | Eigen::Vector3f euler_predict,t_predict; 150 | 151 | T_aft = trans_euler_to_matrix(transformAftMapped); 152 | T_last = trans_euler_to_matrix(transformLastMapped); 153 | 154 | T_predict = T_aft * T_last.inverse() * T_aft; 155 | 156 | R_predict = T_predict.block<3,3>(0,0); 157 | euler_predict = R_predict.eulerAngles(1,0,2); 158 | 159 | t_predict = T_predict.block<3,1>(0,3); 160 | 161 | transformTobeMapped[0] = euler_predict[0]; 162 | transformTobeMapped[1] = euler_predict[1]; 163 | transformTobeMapped[2] = euler_predict[2]; 164 | transformTobeMapped[3] = t_predict[0]; 165 | transformTobeMapped[4] = t_predict[1]; 166 | transformTobeMapped[5] = t_predict[2]; 167 | 168 | std::cout<<"DEBUG transformAftMapped : "<x 186 | - sin(transformTobeMapped[2]) * pi->y; 187 | float y1 = sin(transformTobeMapped[2]) * pi->x 188 | + cos(transformTobeMapped[2]) * pi->y; 189 | float z1 = pi->z; 190 | 191 | //rot x(transformTobeMapped[0]) 192 | float x2 = x1; 193 | float y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1; 194 | float z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1; 195 | 196 | //rot y(transformTobeMapped[1])then add trans 197 | po->x = cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2 198 | + transformTobeMapped[3]; 199 | po->y = y2 + transformTobeMapped[4]; 200 | po->z = -sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2 201 | + transformTobeMapped[5]; 202 | po->intensity = pi->intensity; 203 | } 204 | //lidar coordinate sys to world coordinate sys USE S 205 | void pointAssociateToMap_all(PointType const * const pi, PointType * const po) 206 | { 207 | // Eigen::Matrix4f T_aft,T_last,delta_T; 208 | 209 | // Eigen::Matrix3f R_aft,R_last; 210 | // Eigen::Quaternionf Q_aft,Q_last; 211 | // Eigen::Vector3f t_aft,t_last; 212 | 213 | // T_aft = trans_euler_to_matrix(transformAftMapped); 214 | // T_last = trans_euler_to_matrix(transformLastMapped); 215 | 216 | // R_aft = T_aft.block<3,3>(0,0); 217 | // R_last = T_last.block<3,3>(0,0); 218 | 219 | // Q_aft = R_aft; 220 | // Q_last = R_last; 221 | 222 | double s; 223 | s = pi->intensity - int(pi->intensity); 224 | 225 | //std::cout<<"DEBUG pointAssociateToMap_all s: "<intensity<x 241 | - sin(rz) * pi->y; 242 | float y1 = sin(rz) * pi->x 243 | + cos(rz) * pi->y; 244 | float z1 = pi->z; 245 | 246 | //rot x(transformTobeMapped[0]) 247 | float x2 = x1; 248 | float y2 = cos(rx) * y1 - sin(rx) * z1; 249 | float z2 = sin(rx) * y1 + cos(rx) * z1; 250 | 251 | //rot y(transformTobeMapped[1])then add trans 252 | po->x = cos(ry) * x2 + sin(ry) * z2 + tx; 253 | po->y = y2 + ty; 254 | po->z = -sin(ry) * x2 + cos(ry) * z2 + tz; 255 | po->intensity = pi->intensity; 256 | } 257 | void RGBpointAssociateToMap(PointType const * const pi, pcl::PointXYZRGB * const po) 258 | { 259 | double s; 260 | s = pi->intensity - int(pi->intensity); 261 | 262 | // float rx = (1-s)*transformLastMapped[0] + s * transformAftMapped[0]; 263 | // float ry = (1-s)*transformLastMapped[1] + s * transformAftMapped[1]; 264 | // float rz = (1-s)*transformLastMapped[2] + s * transformAftMapped[2]; 265 | // float tx = (1-s)*transformLastMapped[3] + s * transformAftMapped[3]; 266 | // float ty = (1-s)*transformLastMapped[4] + s * transformAftMapped[4]; 267 | // float tz = (1-s)*transformLastMapped[5] + s * transformAftMapped[5]; 268 | float rx = transformAftMapped[0]; 269 | float ry = transformAftMapped[1]; 270 | float rz = transformAftMapped[2]; 271 | float tx = transformAftMapped[3]; 272 | float ty = transformAftMapped[4]; 273 | float tz = transformAftMapped[5]; 274 | //rot z(transformTobeMapped[2]) 275 | float x1 = cos(rz) * pi->x 276 | - sin(rz) * pi->y; 277 | float y1 = sin(rz) * pi->x 278 | + cos(rz) * pi->y; 279 | float z1 = pi->z; 280 | 281 | //rot x(transformTobeMapped[0]) 282 | float x2 = x1; 283 | float y2 = cos(rx) * y1 - sin(rx) * z1; 284 | float z2 = sin(rx) * y1 + cos(rx) * z1; 285 | 286 | //rot y(transformTobeMapped[1])then add trans 287 | po->x = cos(ry) * x2 + sin(ry) * z2 + tx; 288 | po->y = y2 + ty; 289 | po->z = -sin(ry) * x2 + cos(ry) * z2 + tz; 290 | //po->intensity = pi->intensity; 291 | 292 | float intensity = pi->intensity; 293 | intensity = intensity - std::floor(intensity); 294 | 295 | int reflection_map = intensity*10000; 296 | 297 | //std::cout<<"DEBUG reflection_map "<r = 0; 303 | po->g = green & 0xff; 304 | po->b = 0xff; 305 | } 306 | else if (reflection_map < 90) 307 | { 308 | int blue = (((90 - reflection_map) * 255) / 60); 309 | po->r = 0x0; 310 | po->g = 0xff; 311 | po->b = blue & 0xff; 312 | } 313 | else if (reflection_map < 150) 314 | { 315 | int red = ((reflection_map-90) * 255 / 60); 316 | po->r = red & 0xff; 317 | po->g = 0xff; 318 | po->b = 0x0; 319 | } 320 | else 321 | { 322 | int green = (((255-reflection_map) * 255) / (255-150)); 323 | po->r = 0xff; 324 | po->g = green & 0xff; 325 | po->b = 0; 326 | } 327 | } 328 | 329 | void pointAssociateTobeMapped(PointType const * const pi, PointType * const po) 330 | { 331 | //add trans then rot y 332 | float x1 = cos(transformTobeMapped[1]) * (pi->x - transformTobeMapped[3]) 333 | - sin(transformTobeMapped[1]) * (pi->z - transformTobeMapped[5]); 334 | float y1 = pi->y - transformTobeMapped[4]; 335 | float z1 = sin(transformTobeMapped[1]) * (pi->x - transformTobeMapped[3]) 336 | + cos(transformTobeMapped[1]) * (pi->z - transformTobeMapped[5]); 337 | //rot x 338 | float x2 = x1; 339 | float y2 = cos(transformTobeMapped[0]) * y1 + sin(transformTobeMapped[0]) * z1; 340 | float z2 = -sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1; 341 | //rot z 342 | po->x = cos(transformTobeMapped[2]) * x2 343 | + sin(transformTobeMapped[2]) * y2; 344 | po->y = -sin(transformTobeMapped[2]) * x2 345 | + cos(transformTobeMapped[2]) * y2; 346 | po->z = z2; 347 | po->intensity = pi->intensity; 348 | } 349 | 350 | void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudCornerLast2) 351 | { 352 | timeLaserCloudCornerLast = laserCloudCornerLast2->header.stamp.toSec(); 353 | 354 | laserCloudCornerLast->clear(); 355 | pcl::fromROSMsg(*laserCloudCornerLast2, *laserCloudCornerLast); 356 | 357 | newLaserCloudCornerLast = true; 358 | } 359 | 360 | void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudSurfLast2) 361 | { 362 | timeLaserCloudSurfLast = laserCloudSurfLast2->header.stamp.toSec(); 363 | 364 | laserCloudSurfLast->clear(); 365 | pcl::fromROSMsg(*laserCloudSurfLast2, *laserCloudSurfLast); 366 | 367 | newLaserCloudSurfLast = true; 368 | } 369 | 370 | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2) 371 | { 372 | timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec(); 373 | 374 | laserCloudFullRes->clear(); 375 | laserCloudFullResColor->clear(); 376 | pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes); 377 | 378 | newLaserCloudFullRes = true; 379 | } 380 | 381 | int main(int argc, char** argv) 382 | { 383 | ros::init(argc, argv, "laserMapping"); 384 | ros::NodeHandle nh; 385 | 386 | ros::Subscriber subLaserCloudCornerLast = nh.subscribe 387 | ("/laser_cloud_sharp", 100, laserCloudCornerLastHandler); 388 | 389 | ros::Subscriber subLaserCloudSurfLast = nh.subscribe 390 | ("/laser_cloud_flat", 100, laserCloudSurfLastHandler); 391 | 392 | ros::Subscriber subLaserCloudFullRes = nh.subscribe 393 | ("/livox_cloud", 100, laserCloudFullResHandler); 394 | 395 | ros::Publisher pubLaserCloudSurround = nh.advertise 396 | ("/laser_cloud_surround", 100); 397 | ros::Publisher pubLaserCloudSurround_corner = nh.advertise 398 | ("/laser_cloud_surround_corner", 100); 399 | 400 | ros::Publisher pubLaserCloudFullRes = nh.advertise 401 | ("/velodyne_cloud_registered", 100); 402 | 403 | ros::Publisher pubOdomAftMapped = nh.advertise ("/aft_mapped_to_init", 1); 404 | nav_msgs::Odometry odomAftMapped; 405 | odomAftMapped.header.frame_id = "/camera_init"; 406 | odomAftMapped.child_frame_id = "/aft_mapped"; 407 | 408 | std::string map_file_path; 409 | ros::param::get("~map_file_path",map_file_path); 410 | double filter_parameter_corner; 411 | ros::param::get("~filter_parameter_corner",filter_parameter_corner); 412 | double filter_parameter_surf; 413 | ros::param::get("~filter_parameter_surf",filter_parameter_surf); 414 | 415 | std::vector pointSearchInd; 416 | std::vector pointSearchSqDis; 417 | PointType pointOri, pointSel, coeff; 418 | 419 | cv::Mat matA0(10, 3, CV_32F, cv::Scalar::all(0)); 420 | cv::Mat matB0(10, 1, CV_32F, cv::Scalar::all(-1)); 421 | cv::Mat matX0(10, 1, CV_32F, cv::Scalar::all(0)); 422 | 423 | cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0)); 424 | cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0)); 425 | cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0)); 426 | 427 | bool isDegenerate = false; 428 | cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0)); 429 | //VoxelGrid 430 | pcl::VoxelGrid downSizeFilterCorner; 431 | 432 | downSizeFilterCorner.setLeafSize(filter_parameter_corner, filter_parameter_corner, filter_parameter_corner); 433 | 434 | pcl::VoxelGrid downSizeFilterSurf; 435 | downSizeFilterSurf.setLeafSize(filter_parameter_surf, filter_parameter_surf, filter_parameter_surf); 436 | 437 | // pcl::VoxelGrid downSizeFilterFull; 438 | // downSizeFilterFull.setLeafSize(0.15, 0.15, 0.15); 439 | 440 | for (int i = 0; i < laserCloudNum; i++) { 441 | laserCloudCornerArray[i].reset(new pcl::PointCloud()); 442 | laserCloudSurfArray[i].reset(new pcl::PointCloud()); 443 | laserCloudCornerArray2[i].reset(new pcl::PointCloud()); 444 | laserCloudSurfArray2[i].reset(new pcl::PointCloud()); 445 | } 446 | 447 | //------------------------------------------------------------------------------------------------------ 448 | ros::Rate rate(100); 449 | bool status = ros::ok(); 450 | while (status) { 451 | ros::spinOnce(); 452 | 453 | if (newLaserCloudCornerLast && newLaserCloudSurfLast && newLaserCloudFullRes && 454 | fabs(timeLaserCloudSurfLast - timeLaserCloudCornerLast) < 0.005 && 455 | fabs(timeLaserCloudFullRes - timeLaserCloudCornerLast) < 0.005) { 456 | 457 | clock_t t1,t2,t3,t4; 458 | 459 | t1 = clock(); 460 | 461 | newLaserCloudCornerLast = false; 462 | newLaserCloudSurfLast = false; 463 | newLaserCloudFullRes = false; 464 | 465 | //transformAssociateToMap(); 466 | std::cout<<"DEBUG mapping start "<::Ptr laserCloudCubeCornerPointer = 489 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 490 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer = 491 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 492 | 493 | for (; i >= 1; i--) { 494 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 495 | laserCloudCornerArray[i - 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k]; 496 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 497 | laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 498 | } 499 | 500 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 501 | laserCloudCubeCornerPointer; 502 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 503 | laserCloudCubeSurfPointer; 504 | laserCloudCubeCornerPointer->clear(); 505 | laserCloudCubeSurfPointer->clear(); 506 | } 507 | } 508 | 509 | centerCubeI++; 510 | laserCloudCenWidth++; 511 | } 512 | 513 | while (centerCubeI >= laserCloudWidth - 3) { 514 | for (int j = 0; j < laserCloudHeight; j++) { 515 | for (int k = 0; k < laserCloudDepth; k++) { 516 | int i = 0; 517 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer = 518 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 519 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer = 520 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 521 | 522 | for (; i < laserCloudWidth - 1; i++) { 523 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 524 | laserCloudCornerArray[i + 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k]; 525 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 526 | laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 527 | } 528 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 529 | laserCloudCubeCornerPointer; 530 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 531 | laserCloudCubeSurfPointer; 532 | laserCloudCubeCornerPointer->clear(); 533 | laserCloudCubeSurfPointer->clear(); 534 | } 535 | } 536 | 537 | centerCubeI--; 538 | laserCloudCenWidth--; 539 | } 540 | 541 | while (centerCubeJ < 3) { 542 | for (int i = 0; i < laserCloudWidth; i++) { 543 | for (int k = 0; k < laserCloudDepth; k++) { 544 | int j = laserCloudHeight - 1; 545 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer = 546 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 547 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer = 548 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 549 | 550 | for (; j >= 1; j--) { 551 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 552 | laserCloudCornerArray[i + laserCloudWidth*(j - 1) + laserCloudWidth * laserCloudHeight*k]; 553 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 554 | laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight*k]; 555 | } 556 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 557 | laserCloudCubeCornerPointer; 558 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 559 | laserCloudCubeSurfPointer; 560 | laserCloudCubeCornerPointer->clear(); 561 | laserCloudCubeSurfPointer->clear(); 562 | } 563 | } 564 | 565 | centerCubeJ++; 566 | laserCloudCenHeight++; 567 | } 568 | 569 | while (centerCubeJ >= laserCloudHeight - 3) { 570 | for (int i = 0; i < laserCloudWidth; i++) { 571 | for (int k = 0; k < laserCloudDepth; k++) { 572 | int j = 0; 573 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer = 574 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 575 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer = 576 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 577 | 578 | for (; j < laserCloudHeight - 1; j++) { 579 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 580 | laserCloudCornerArray[i + laserCloudWidth*(j + 1) + laserCloudWidth * laserCloudHeight*k]; 581 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 582 | laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight*k]; 583 | } 584 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 585 | laserCloudCubeCornerPointer; 586 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 587 | laserCloudCubeSurfPointer; 588 | laserCloudCubeCornerPointer->clear(); 589 | laserCloudCubeSurfPointer->clear(); 590 | } 591 | } 592 | 593 | centerCubeJ--; 594 | laserCloudCenHeight--; 595 | } 596 | 597 | while (centerCubeK < 3) { 598 | for (int i = 0; i < laserCloudWidth; i++) { 599 | for (int j = 0; j < laserCloudHeight; j++) { 600 | int k = laserCloudDepth - 1; 601 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer = 602 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 603 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer = 604 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 605 | 606 | for (; k >= 1; k--) { 607 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 608 | laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k - 1)]; 609 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 610 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k - 1)]; 611 | } 612 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 613 | laserCloudCubeCornerPointer; 614 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 615 | laserCloudCubeSurfPointer; 616 | laserCloudCubeCornerPointer->clear(); 617 | laserCloudCubeSurfPointer->clear(); 618 | } 619 | } 620 | 621 | centerCubeK++; 622 | laserCloudCenDepth++; 623 | } 624 | 625 | while (centerCubeK >= laserCloudDepth - 3) { 626 | for (int i = 0; i < laserCloudWidth; i++) { 627 | for (int j = 0; j < laserCloudHeight; j++) { 628 | int k = 0; 629 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer = 630 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 631 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer = 632 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 633 | 634 | for (; k < laserCloudDepth - 1; k++) { 635 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 636 | laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k + 1)]; 637 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 638 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k + 1)]; 639 | } 640 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 641 | laserCloudCubeCornerPointer; 642 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 643 | laserCloudCubeSurfPointer; 644 | laserCloudCubeCornerPointer->clear(); 645 | laserCloudCubeSurfPointer->clear(); 646 | } 647 | } 648 | 649 | centerCubeK--; 650 | laserCloudCenDepth--; 651 | } 652 | 653 | int laserCloudValidNum = 0; 654 | int laserCloudSurroundNum = 0; 655 | 656 | for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) { 657 | for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) { 658 | for (int k = centerCubeK - 2; k <= centerCubeK + 2; k++) { 659 | if (i >= 0 && i < laserCloudWidth && 660 | j >= 0 && j < laserCloudHeight && 661 | k >= 0 && k < laserCloudDepth) { 662 | 663 | float centerX = 50.0 * (i - laserCloudCenWidth); 664 | float centerY = 50.0 * (j - laserCloudCenHeight); 665 | float centerZ = 50.0 * (k - laserCloudCenDepth); 666 | 667 | bool isInLaserFOV = false; 668 | for (int ii = -1; ii <= 1; ii += 2) { 669 | for (int jj = -1; jj <= 1; jj += 2) { 670 | for (int kk = -1; kk <= 1; kk += 2) { 671 | 672 | float cornerX = centerX + 25.0 * ii; 673 | float cornerY = centerY + 25.0 * jj; 674 | float cornerZ = centerZ + 25.0 * kk; 675 | 676 | float squaredSide1 = (transformTobeMapped[3] - cornerX) 677 | * (transformTobeMapped[3] - cornerX) 678 | + (transformTobeMapped[4] - cornerY) 679 | * (transformTobeMapped[4] - cornerY) 680 | + (transformTobeMapped[5] - cornerZ) 681 | * (transformTobeMapped[5] - cornerZ); 682 | 683 | float squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX) 684 | + (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY) 685 | + (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ); 686 | 687 | float check1 = 100.0 + squaredSide1 - squaredSide2 688 | - 10.0 * sqrt(3.0) * sqrt(squaredSide1); 689 | 690 | float check2 = 100.0 + squaredSide1 - squaredSide2 691 | + 10.0 * sqrt(3.0) * sqrt(squaredSide1); 692 | 693 | if (check1 < 0 && check2 > 0) { 694 | isInLaserFOV = true; 695 | } 696 | } 697 | } 698 | } 699 | 700 | if (isInLaserFOV) { 701 | laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j 702 | + laserCloudWidth * laserCloudHeight * k; 703 | laserCloudValidNum++; 704 | } 705 | laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j 706 | + laserCloudWidth * laserCloudHeight * k; 707 | laserCloudSurroundNum++; 708 | } 709 | } 710 | } 711 | } 712 | 713 | laserCloudCornerFromMap->clear(); 714 | laserCloudSurfFromMap->clear(); 715 | 716 | for (int i = 0; i < laserCloudValidNum; i++) { 717 | *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]]; 718 | *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]]; 719 | } 720 | int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size(); 721 | int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size(); 722 | 723 | laserCloudCornerLast_down->clear(); 724 | downSizeFilterCorner.setInputCloud(laserCloudCornerLast); 725 | downSizeFilterCorner.filter(*laserCloudCornerLast_down); 726 | int laserCloudCornerLast_downNum = laserCloudCornerLast_down->points.size(); 727 | 728 | laserCloudSurfLast_down->clear(); 729 | downSizeFilterSurf.setInputCloud(laserCloudSurfLast); 730 | downSizeFilterSurf.filter(*laserCloudSurfLast_down); 731 | int laserCloudSurfLast_downNum = laserCloudSurfLast_down->points.size(); 732 | 733 | std::cout<<"DEBUG MAPPING laserCloudCornerLast_down : "<points.size()<<" laserCloudSurfLast_down : " 734 | <points.size()<points.size()<<" laserCloudSurfLast : " 736 | <points.size()< 10 && laserCloudSurfFromMapNum > 100) { 742 | //if (laserCloudSurfFromMapNum > 100) { 743 | kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap); 744 | kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap); 745 | 746 | int num_temp = 0; 747 | 748 | for (int iterCount = 0; iterCount < 20; iterCount++) { 749 | num_temp++; 750 | laserCloudOri->clear(); 751 | coeffSel->clear(); 752 | for (int i = 0; i < laserCloudCornerLast->points.size(); i++) { 753 | pointOri = laserCloudCornerLast->points[i]; 754 | 755 | pointAssociateToMap(&pointOri, &pointSel); 756 | //find the closest 5 points 757 | kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); 758 | 759 | if (pointSearchSqDis[4] < 1.5) { 760 | float cx = 0; 761 | float cy = 0; 762 | float cz = 0; 763 | for (int j = 0; j < 5; j++) { 764 | cx += laserCloudCornerFromMap->points[pointSearchInd[j]].x; 765 | cy += laserCloudCornerFromMap->points[pointSearchInd[j]].y; 766 | cz += laserCloudCornerFromMap->points[pointSearchInd[j]].z; 767 | } 768 | cx /= 5; 769 | cy /= 5; 770 | cz /= 5; 771 | //mean square error 772 | float a11 = 0; 773 | float a12 = 0; 774 | float a13 = 0; 775 | float a22 = 0; 776 | float a23 = 0; 777 | float a33 = 0; 778 | for (int j = 0; j < 5; j++) { 779 | float ax = laserCloudCornerFromMap->points[pointSearchInd[j]].x - cx; 780 | float ay = laserCloudCornerFromMap->points[pointSearchInd[j]].y - cy; 781 | float az = laserCloudCornerFromMap->points[pointSearchInd[j]].z - cz; 782 | 783 | a11 += ax * ax; 784 | a12 += ax * ay; 785 | a13 += ax * az; 786 | a22 += ay * ay; 787 | a23 += ay * az; 788 | a33 += az * az; 789 | } 790 | a11 /= 5; 791 | a12 /= 5; 792 | a13 /= 5; 793 | a22 /= 5; 794 | a23 /= 5; 795 | a33 /= 5; 796 | 797 | matA1.at(0, 0) = a11; 798 | matA1.at(0, 1) = a12; 799 | matA1.at(0, 2) = a13; 800 | matA1.at(1, 0) = a12; 801 | matA1.at(1, 1) = a22; 802 | matA1.at(1, 2) = a23; 803 | matA1.at(2, 0) = a13; 804 | matA1.at(2, 1) = a23; 805 | matA1.at(2, 2) = a33; 806 | 807 | cv::eigen(matA1, matD1, matV1); 808 | 809 | if (matD1.at(0, 0) > 3 * matD1.at(0, 1)) { 810 | 811 | float x0 = pointSel.x; 812 | float y0 = pointSel.y; 813 | float z0 = pointSel.z; 814 | float x1 = cx + 0.1 * matV1.at(0, 0); 815 | float y1 = cy + 0.1 * matV1.at(0, 1); 816 | float z1 = cz + 0.1 * matV1.at(0, 2); 817 | float x2 = cx - 0.1 * matV1.at(0, 0); 818 | float y2 = cy - 0.1 * matV1.at(0, 1); 819 | float z2 = cz - 0.1 * matV1.at(0, 2); 820 | 821 | //OA = (x0 - x1, y0 - y1, z0 - z1),OB = (x0 - x2, y0 - y2, z0 - z2),AB = (x1 - x2, y1 - y2, z1 - z2) 822 | //cross: 823 | //| i j k | 824 | //|x0-x1 y0-y1 z0-z1| 825 | //|x0-x2 y0-y2 z0-z2| 826 | float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 827 | * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 828 | + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 829 | * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 830 | + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) 831 | * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); 832 | 833 | float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); 834 | 835 | float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 836 | + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12; 837 | 838 | float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 839 | - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; 840 | 841 | float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 842 | + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; 843 | 844 | float ld2 = a012 / l12; 845 | //if(fabs(ld2) > 1) continue; 846 | 847 | float s = 1 - 0.9 * fabs(ld2); 848 | 849 | coeff.x = s * la; 850 | coeff.y = s * lb; 851 | coeff.z = s * lc; 852 | coeff.intensity = s * ld2; 853 | 854 | if (s > 0.1) { 855 | laserCloudOri->push_back(pointOri); 856 | coeffSel->push_back(coeff); 857 | } 858 | } 859 | } 860 | } 861 | //std::cout <<"DEBUG mapping select corner points : " << coeffSel->size() << std::endl; 862 | 863 | for (int i = 0; i < laserCloudSurfLast_down->points.size(); i++) { 864 | pointOri = laserCloudSurfLast_down->points[i]; 865 | 866 | pointAssociateToMap(&pointOri, &pointSel); 867 | kdtreeSurfFromMap->nearestKSearch(pointSel, 8, pointSearchInd, pointSearchSqDis); 868 | 869 | if (pointSearchSqDis[7] < 5.0) { 870 | 871 | for (int j = 0; j < 8; j++) { 872 | matA0.at(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x; 873 | matA0.at(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y; 874 | matA0.at(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z; 875 | } 876 | //matA0*matX0=matB0 877 | //AX+BY+CZ+D = 0 <=> AX+BY+CZ=-D <=> (A/D)X+(B/D)Y+(C/D)Z = -1 878 | //(X,Y,Z)<=>mat_a0 879 | //A/D, B/D, C/D <=> mat_x0 880 | 881 | cv::solve(matA0, matB0, matX0, cv::DECOMP_QR); //TODO 882 | 883 | float pa = matX0.at(0, 0); 884 | float pb = matX0.at(1, 0); 885 | float pc = matX0.at(2, 0); 886 | float pd = 1; 887 | 888 | //ps is the norm of the plane normal vector 889 | //pd is the distance from point to plane 890 | float ps = sqrt(pa * pa + pb * pb + pc * pc); 891 | pa /= ps; 892 | pb /= ps; 893 | pc /= ps; 894 | pd /= ps; 895 | 896 | bool planeValid = true; 897 | for (int j = 0; j < 8; j++) { 898 | if (fabs(pa * laserCloudSurfFromMap->points[pointSearchInd[j]].x + 899 | pb * laserCloudSurfFromMap->points[pointSearchInd[j]].y + 900 | pc * laserCloudSurfFromMap->points[pointSearchInd[j]].z + pd) > 0.2) { 901 | planeValid = false; 902 | break; 903 | } 904 | } 905 | 906 | if (planeValid) { 907 | //loss fuction 908 | float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; 909 | 910 | //if(fabs(pd2) > 0.1) continue; 911 | 912 | float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x + pointSel.y * pointSel.y + pointSel.z * pointSel.z)); 913 | 914 | coeff.x = s * pa; 915 | coeff.y = s * pb; 916 | coeff.z = s * pc; 917 | coeff.intensity = s * pd2; 918 | 919 | if (s > 0.1) { 920 | laserCloudOri->push_back(pointOri); 921 | coeffSel->push_back(coeff); 922 | } 923 | } 924 | } 925 | } 926 | //std::cout <<"DEBUG mapping select all points : " << coeffSel->size() << std::endl; 927 | 928 | float srx = sin(transformTobeMapped[0]); 929 | float crx = cos(transformTobeMapped[0]); 930 | float sry = sin(transformTobeMapped[1]); 931 | float cry = cos(transformTobeMapped[1]); 932 | float srz = sin(transformTobeMapped[2]); 933 | float crz = cos(transformTobeMapped[2]); 934 | 935 | int laserCloudSelNum = laserCloudOri->points.size(); 936 | if (laserCloudSelNum < 50) { 937 | continue; 938 | } 939 | 940 | 941 | //|c1c3+s1s2s3 c3s1s2-c1s3 c2s1| 942 | //| c2s3 c2c3 -s2| 943 | //|c1s2s3-c3s1 c1c3s2+s1s3 c1c2| 944 | //AT*A*x = AT*b 945 | cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0)); 946 | cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0)); 947 | cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0)); 948 | cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0)); 949 | cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0)); 950 | cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0)); 951 | float debug_distance = 0; 952 | for (int i = 0; i < laserCloudSelNum; i++) { 953 | pointOri = laserCloudOri->points[i]; 954 | coeff = coeffSel->points[i]; 955 | 956 | float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x 957 | + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y 958 | + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z; 959 | 960 | float ary = ((cry*srx*srz - crz*sry)*pointOri.x 961 | + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x 962 | + ((-cry*crz - srx*sry*srz)*pointOri.x 963 | + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z; 964 | 965 | float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x 966 | + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y 967 | + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z; 968 | 969 | matA.at(i, 0) = arx; 970 | matA.at(i, 1) = ary; 971 | matA.at(i, 2) = arz; 972 | //TODO: the partial derivative 973 | matA.at(i, 3) = coeff.x; 974 | matA.at(i, 4) = coeff.y; 975 | matA.at(i, 5) = coeff.z; 976 | matB.at(i, 0) = -coeff.intensity; 977 | 978 | debug_distance += fabs(coeff.intensity); 979 | } 980 | cv::transpose(matA, matAt); 981 | matAtA = matAt * matA; 982 | matAtB = matAt * matB; 983 | cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR); 984 | 985 | //Deterioration judgment 986 | if (iterCount == 0) { 987 | cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0)); 988 | cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0)); 989 | cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0)); 990 | 991 | cv::eigen(matAtA, matE, matV); 992 | matV.copyTo(matV2); 993 | 994 | isDegenerate = false; 995 | float eignThre[6] = {1, 1, 1, 1, 1, 1}; 996 | for (int i = 5; i >= 0; i--) { 997 | if (matE.at(0, i) < eignThre[i]) { 998 | for (int j = 0; j < 6; j++) { 999 | matV2.at(i, j) = 0; 1000 | } 1001 | isDegenerate = true; 1002 | } else { 1003 | break; 1004 | } 1005 | } 1006 | matP = matV.inv() * matV2; 1007 | } 1008 | 1009 | if (isDegenerate) { 1010 | cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0)); 1011 | matX.copyTo(matX2); 1012 | matX = matP * matX2; 1013 | } 1014 | 1015 | transformTobeMapped[0] += matX.at(0, 0); 1016 | transformTobeMapped[1] += matX.at(1, 0); 1017 | transformTobeMapped[2] += matX.at(2, 0); 1018 | transformTobeMapped[3] += matX.at(3, 0); 1019 | transformTobeMapped[4] += matX.at(4, 0); 1020 | transformTobeMapped[5] += matX.at(5, 0); 1021 | 1022 | float deltaR = sqrt( 1023 | pow(rad2deg(matX.at(0, 0)), 2) + 1024 | pow(rad2deg(matX.at(1, 0)), 2) + 1025 | pow(rad2deg(matX.at(2, 0)), 2)); 1026 | float deltaT = sqrt( 1027 | pow(matX.at(3, 0) * 100, 2) + 1028 | pow(matX.at(4, 0) * 100, 2) + 1029 | pow(matX.at(5, 0) * 100, 2)); 1030 | 1031 | if (deltaR < 0.05 && deltaT < 0.05) { 1032 | break; 1033 | } 1034 | } 1035 | std::cout<<"DEBUG num_temp: "<points.size(); i++) { 1043 | pointAssociateToMap(&laserCloudCornerLast->points[i], &pointSel); 1044 | 1045 | int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth; 1046 | int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight; 1047 | int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth; 1048 | 1049 | if (pointSel.x + 25.0 < 0) cubeI--; 1050 | if (pointSel.y + 25.0 < 0) cubeJ--; 1051 | if (pointSel.z + 25.0 < 0) cubeK--; 1052 | 1053 | if (cubeI >= 0 && cubeI < laserCloudWidth && 1054 | cubeJ >= 0 && cubeJ < laserCloudHeight && 1055 | cubeK >= 0 && cubeK < laserCloudDepth) { 1056 | 1057 | int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; 1058 | laserCloudCornerArray[cubeInd]->push_back(pointSel); 1059 | } 1060 | } 1061 | 1062 | for (int i = 0; i < laserCloudSurfLast_down->points.size(); i++) { 1063 | pointAssociateToMap(&laserCloudSurfLast_down->points[i], &pointSel); 1064 | 1065 | int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth; 1066 | int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight; 1067 | int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth; 1068 | 1069 | if (pointSel.x + 25.0 < 0) cubeI--; 1070 | if (pointSel.y + 25.0 < 0) cubeJ--; 1071 | if (pointSel.z + 25.0 < 0) cubeK--; 1072 | 1073 | if (cubeI >= 0 && cubeI < laserCloudWidth && 1074 | cubeJ >= 0 && cubeJ < laserCloudHeight && 1075 | cubeK >= 0 && cubeK < laserCloudDepth) { 1076 | int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; 1077 | laserCloudSurfArray[cubeInd]->push_back(pointSel); 1078 | } 1079 | } 1080 | 1081 | for (int i = 0; i < laserCloudValidNum; i++) { 1082 | int ind = laserCloudValidInd[i]; 1083 | 1084 | laserCloudCornerArray2[ind]->clear(); 1085 | downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]); 1086 | downSizeFilterCorner.filter(*laserCloudCornerArray2[ind]); 1087 | 1088 | laserCloudSurfArray2[ind]->clear(); 1089 | downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]); 1090 | downSizeFilterSurf.filter(*laserCloudSurfArray2[ind]); 1091 | 1092 | pcl::PointCloud::Ptr laserCloudTemp = laserCloudCornerArray[ind]; 1093 | laserCloudCornerArray[ind] = laserCloudCornerArray2[ind]; 1094 | laserCloudCornerArray2[ind] = laserCloudTemp; 1095 | 1096 | laserCloudTemp = laserCloudSurfArray[ind]; 1097 | laserCloudSurfArray[ind] = laserCloudSurfArray2[ind]; 1098 | laserCloudSurfArray2[ind] = laserCloudTemp; 1099 | } 1100 | 1101 | laserCloudSurround2->clear(); 1102 | laserCloudSurround2_corner->clear(); 1103 | for (int i = 0; i < laserCloudSurroundNum; i++) { 1104 | int ind = laserCloudSurroundInd[i]; 1105 | *laserCloudSurround2_corner += *laserCloudCornerArray[ind]; 1106 | *laserCloudSurround2 += *laserCloudSurfArray[ind]; 1107 | } 1108 | 1109 | // laserCloudSurround->clear(); 1110 | // downSizeFilterSurf.setInputCloud(laserCloudSurround2); 1111 | // downSizeFilterSurf.filter(*laserCloudSurround); 1112 | 1113 | // laserCloudSurround_corner->clear(); 1114 | // downSizeFilterCorner.setInputCloud(laserCloudSurround2_corner); 1115 | // downSizeFilterCorner.filter(*laserCloudSurround_corner); 1116 | 1117 | sensor_msgs::PointCloud2 laserCloudSurround3; 1118 | pcl::toROSMsg(*laserCloudSurround2, laserCloudSurround3); 1119 | laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserCloudCornerLast); 1120 | laserCloudSurround3.header.frame_id = "/camera_init"; 1121 | pubLaserCloudSurround.publish(laserCloudSurround3); 1122 | 1123 | sensor_msgs::PointCloud2 laserCloudSurround3_corner; 1124 | pcl::toROSMsg(*laserCloudSurround2_corner, laserCloudSurround3_corner); 1125 | laserCloudSurround3_corner.header.stamp = ros::Time().fromSec(timeLaserCloudCornerLast); 1126 | laserCloudSurround3_corner.header.frame_id = "/camera_init"; 1127 | pubLaserCloudSurround_corner.publish(laserCloudSurround3_corner); 1128 | 1129 | 1130 | laserCloudFullRes2->clear(); 1131 | *laserCloudFullRes2 = *laserCloudFullRes; 1132 | 1133 | int laserCloudFullResNum = laserCloudFullRes2->points.size(); 1134 | for (int i = 0; i < laserCloudFullResNum; i++) { 1135 | 1136 | pcl::PointXYZRGB temp_point; 1137 | RGBpointAssociateToMap(&laserCloudFullRes2->points[i], &temp_point); 1138 | laserCloudFullResColor->push_back(temp_point); 1139 | } 1140 | 1141 | sensor_msgs::PointCloud2 laserCloudFullRes3; 1142 | pcl::toROSMsg(*laserCloudFullResColor, laserCloudFullRes3); 1143 | laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserCloudCornerLast); 1144 | laserCloudFullRes3.header.frame_id = "/camera_init"; 1145 | pubLaserCloudFullRes.publish(laserCloudFullRes3); 1146 | 1147 | *laserCloudFullResColor_pcd += *laserCloudFullResColor; 1148 | 1149 | geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw 1150 | (transformAftMapped[2], - transformAftMapped[0], - transformAftMapped[1]); 1151 | 1152 | odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserCloudCornerLast); 1153 | odomAftMapped.pose.pose.orientation.x = -geoQuat.y; 1154 | odomAftMapped.pose.pose.orientation.y = -geoQuat.z; 1155 | odomAftMapped.pose.pose.orientation.z = geoQuat.x; 1156 | odomAftMapped.pose.pose.orientation.w = geoQuat.w; 1157 | odomAftMapped.pose.pose.position.x = transformAftMapped[3]; 1158 | odomAftMapped.pose.pose.position.y = transformAftMapped[4]; 1159 | odomAftMapped.pose.pose.position.z = transformAftMapped[5]; 1160 | 1161 | pubOdomAftMapped.publish(odomAftMapped); 1162 | 1163 | static tf::TransformBroadcaster br; 1164 | tf::Transform transform; 1165 | tf::Quaternion q; 1166 | transform.setOrigin( tf::Vector3( odomAftMapped.pose.pose.position.x, 1167 | odomAftMapped.pose.pose.position.y, 1168 | odomAftMapped.pose.pose.position.z ) ); 1169 | q.setW( odomAftMapped.pose.pose.orientation.w ); 1170 | q.setX( odomAftMapped.pose.pose.orientation.x ); 1171 | q.setY( odomAftMapped.pose.pose.orientation.y ); 1172 | q.setZ( odomAftMapped.pose.pose.orientation.z ); 1173 | transform.setRotation( q ); 1174 | br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped" ) ); 1175 | 1176 | kfNum++; 1177 | 1178 | if(kfNum >= 20){ 1179 | Eigen::Matrix kf_pose; 1180 | kf_pose << -geoQuat.y,-geoQuat.z,geoQuat.x,geoQuat.w,transformAftMapped[3],transformAftMapped[4],transformAftMapped[5]; 1181 | keyframe_pose.push_back(kf_pose); 1182 | kfNum = 0; 1183 | } 1184 | 1185 | t4 = clock(); 1186 | 1187 | std::cout<<"mapping time : "< surf_points, corner_points; 1206 | surf_points = *laserCloudSurfFromMap; 1207 | corner_points = *laserCloudCornerFromMap; 1208 | if (surf_points.size() > 0 && corner_points.size() > 0) { 1209 | pcl::PCDWriter pcd_writer; 1210 | std::cout << "saving..."; 1211 | pcd_writer.writeBinary(surf_filename, surf_points); 1212 | pcd_writer.writeBinary(corner_filename, corner_points); 1213 | pcd_writer.writeBinary(all_points_filename, *laserCloudFullResColor_pcd); 1214 | } else { 1215 | std::cout << "no points saved"; 1216 | } 1217 | //-------------------------- 1218 | // loss_output.close(); 1219 | 1220 | 1221 | return 0; 1222 | } 1223 | 1224 | -------------------------------------------------------------------------------- /src/livox_repub.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "livox_ros_driver/CustomMsg.h" 4 | 5 | typedef pcl::PointXYZINormal PointType; 6 | typedef pcl::PointCloud PointCloudXYZI; 7 | 8 | ros::Publisher pub_pcl_out0, pub_pcl_out1; 9 | uint64_t TO_MERGE_CNT = 1; 10 | constexpr bool b_dbg_line = false; 11 | std::vector livox_data; 12 | void LivoxMsgCbk1(const livox_ros_driver::CustomMsgConstPtr& livox_msg_in) { 13 | livox_data.push_back(livox_msg_in); 14 | if (livox_data.size() < TO_MERGE_CNT) return; 15 | 16 | pcl::PointCloud pcl_in; 17 | 18 | for (size_t j = 0; j < livox_data.size(); j++) { 19 | auto& livox_msg = livox_data[j]; 20 | auto time_end = livox_msg->points.back().offset_time; 21 | for (unsigned int i = 0; i < livox_msg->point_num; ++i) { 22 | PointType pt; 23 | pt.x = livox_msg->points[i].x; 24 | pt.y = livox_msg->points[i].y; 25 | pt.z = livox_msg->points[i].z; 26 | float s = livox_msg->points[i].offset_time / (float)time_end; 27 | 28 | pt.intensity = livox_msg->points[i].line +livox_msg->points[i].reflectivity /10000.0 ; // The integer part is line number and the decimal part is timestamp 29 | pt.curvature = s*0.1; 30 | pcl_in.push_back(pt); 31 | } 32 | } 33 | 34 | unsigned long timebase_ns = livox_data[0]->timebase; 35 | ros::Time timestamp; 36 | timestamp.fromNSec(timebase_ns); 37 | 38 | sensor_msgs::PointCloud2 pcl_ros_msg; 39 | pcl::toROSMsg(pcl_in, pcl_ros_msg); 40 | pcl_ros_msg.header.stamp.fromNSec(timebase_ns); 41 | pcl_ros_msg.header.frame_id = "/livox"; 42 | pub_pcl_out1.publish(pcl_ros_msg); 43 | livox_data.clear(); 44 | } 45 | 46 | int main(int argc, char** argv) { 47 | ros::init(argc, argv, "livox_repub"); 48 | ros::NodeHandle nh; 49 | 50 | ROS_INFO("start livox_repub"); 51 | 52 | ros::Subscriber sub_livox_msg1 = nh.subscribe( 53 | "/livox/lidar", 100, LivoxMsgCbk1); 54 | pub_pcl_out1 = nh.advertise("/livox_pcl0", 100); 55 | 56 | ros::spin(); 57 | } 58 | -------------------------------------------------------------------------------- /src/scanRegistration.cpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 4 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 5 | 6 | // Modifier: Livox dev@livoxtech.com 7 | 8 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 9 | // Further contributions copyright (c) 2016, Southwest Research Institute 10 | // All rights reserved. 11 | // 12 | // Redistribution and use in source and binary forms, with or without 13 | // modification, are permitted provided that the following conditions are met: 14 | // 15 | // 1. Redistributions of source code must retain the above copyright notice, 16 | // this list of conditions and the following disclaimer. 17 | // 2. Redistributions in binary form must reproduce the above copyright notice, 18 | // this list of conditions and the following disclaimer in the documentation 19 | // and/or other materials provided with the distribution. 20 | // 3. Neither the name of the copyright holder nor the names of its 21 | // contributors may be used to endorse or promote products derived from this 22 | // software without specific prior written permission. 23 | // 24 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 28 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 29 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 30 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 31 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 32 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 33 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | // POSSIBILITY OF SUCH DAMAGE. 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | 50 | typedef pcl::PointXYZI PointType; 51 | int scanID; 52 | 53 | int CloudFeatureFlag[32000]; 54 | 55 | ros::Publisher pubLaserCloud; 56 | ros::Publisher pubCornerPointsSharp; 57 | ros::Publisher pubSurfPointsFlat; 58 | ros::Publisher pubLaserCloud_temp; 59 | std::vector msg_window; 60 | cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0)); 61 | cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0)); 62 | cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0)); 63 | 64 | bool plane_judge(const std::vector& point_list,const int plane_threshold) 65 | { 66 | int num = point_list.size(); 67 | float cx = 0; 68 | float cy = 0; 69 | float cz = 0; 70 | for (int j = 0; j < num; j++) { 71 | cx += point_list[j].x; 72 | cy += point_list[j].y; 73 | cz += point_list[j].z; 74 | } 75 | cx /= num; 76 | cy /= num; 77 | cz /= num; 78 | //mean square error 79 | float a11 = 0; 80 | float a12 = 0; 81 | float a13 = 0; 82 | float a22 = 0; 83 | float a23 = 0; 84 | float a33 = 0; 85 | for (int j = 0; j < num; j++) { 86 | float ax = point_list[j].x - cx; 87 | float ay = point_list[j].y - cy; 88 | float az = point_list[j].z - cz; 89 | 90 | a11 += ax * ax; 91 | a12 += ax * ay; 92 | a13 += ax * az; 93 | a22 += ay * ay; 94 | a23 += ay * az; 95 | a33 += az * az; 96 | } 97 | a11 /= num; 98 | a12 /= num; 99 | a13 /= num; 100 | a22 /= num; 101 | a23 /= num; 102 | a33 /= num; 103 | 104 | matA1.at(0, 0) = a11; 105 | matA1.at(0, 1) = a12; 106 | matA1.at(0, 2) = a13; 107 | matA1.at(1, 0) = a12; 108 | matA1.at(1, 1) = a22; 109 | matA1.at(1, 2) = a23; 110 | matA1.at(2, 0) = a13; 111 | matA1.at(2, 1) = a23; 112 | matA1.at(2, 2) = a33; 113 | 114 | cv::eigen(matA1, matD1, matV1); 115 | if (matD1.at(0, 0) > plane_threshold * matD1.at(0, 1)) { 116 | return true; 117 | } 118 | else{ 119 | return false; 120 | } 121 | } 122 | void laserCloudHandler_temp(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) //for hkmars data 123 | { 124 | 125 | pcl::PointCloud::Ptr laserCloudIn(new pcl::PointCloud()); 126 | 127 | if(msg_window.size() < 2){ 128 | msg_window.push_back(laserCloudMsg); 129 | } 130 | else{ 131 | msg_window.erase(msg_window.begin()); 132 | msg_window.push_back(laserCloudMsg); 133 | } 134 | 135 | for(int i = 0; i < msg_window.size();i++){ 136 | pcl::PointCloud temp; 137 | pcl::fromROSMsg(*msg_window[i], temp); 138 | *laserCloudIn += temp; 139 | } 140 | sensor_msgs::PointCloud2 laserCloudOutMsg; 141 | pcl::toROSMsg(*laserCloudIn, laserCloudOutMsg); 142 | laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; 143 | laserCloudOutMsg.header.frame_id = "/livox"; 144 | pubLaserCloud_temp.publish(laserCloudOutMsg); 145 | 146 | } 147 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) 148 | { 149 | pcl::PointCloud laserCloudIn; 150 | pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); 151 | 152 | int cloudSize = laserCloudIn.points.size(); 153 | 154 | if(cloudSize > 32000) cloudSize = 32000; 155 | 156 | int count = cloudSize; 157 | PointType point; 158 | pcl::PointCloud Allpoints; 159 | 160 | for (int i = 0; i < cloudSize; i++) { 161 | 162 | point.x = laserCloudIn.points[i].x; 163 | point.y = laserCloudIn.points[i].y; 164 | point.z = laserCloudIn.points[i].z; 165 | double theta = std::atan2(laserCloudIn.points[i].y,laserCloudIn.points[i].z) / M_PI * 180 + 180; 166 | 167 | scanID = std::floor(theta / 9); 168 | float dis = point.x * point.x 169 | + point.y * point.y 170 | + point.z * point.z; 171 | 172 | double dis2 = laserCloudIn.points[i].z * laserCloudIn.points[i].z + laserCloudIn.points[i].y * laserCloudIn.points[i].y; 173 | double theta2 = std::asin(sqrt(dis2/dis)) / M_PI * 180; 174 | 175 | point.intensity = scanID+(laserCloudIn.points[i].intensity/10000); 176 | //point.intensity = scanID+(double(i)/cloudSize); 177 | 178 | if (!pcl_isfinite(point.x) || 179 | !pcl_isfinite(point.y) || 180 | !pcl_isfinite(point.z)) { 181 | continue; 182 | } 183 | 184 | Allpoints.push_back(point); 185 | } 186 | 187 | pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); 188 | *laserCloud += Allpoints; 189 | cloudSize = laserCloud->size(); 190 | 191 | for (int i = 0; i < cloudSize; i++) { 192 | CloudFeatureFlag[i] = 0; 193 | } 194 | 195 | pcl::PointCloud cornerPointsSharp; 196 | 197 | pcl::PointCloud surfPointsFlat; 198 | 199 | pcl::PointCloud laserCloudFull; 200 | 201 | int debugnum1 = 0; 202 | int debugnum2 = 0; 203 | int debugnum3 = 0; 204 | int debugnum4 = 0; 205 | int debugnum5 = 0; 206 | 207 | int count_num = 1; 208 | bool left_surf_flag = false; 209 | bool right_surf_flag = false; 210 | Eigen::Vector3d surf_vector_current(0,0,0); 211 | Eigen::Vector3d surf_vector_last(0,0,0); 212 | int last_surf_position = 0; 213 | double depth_threshold = 0.1; 214 | //******************************************************************************************************************************************** 215 | for (int i = 5; i < cloudSize - 5; i += count_num ) { 216 | float depth = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + 217 | laserCloud->points[i].y * laserCloud->points[i].y + 218 | laserCloud->points[i].z * laserCloud->points[i].z); 219 | 220 | // if(depth < 2) depth_threshold = 0.05; 221 | // if(depth > 30) depth_threshold = 0.1; 222 | //left curvature 223 | float ldiffX = 224 | laserCloud->points[i - 4].x + laserCloud->points[i - 3].x 225 | - 4 * laserCloud->points[i - 2].x 226 | + laserCloud->points[i - 1].x + laserCloud->points[i].x; 227 | 228 | float ldiffY = 229 | laserCloud->points[i - 4].y + laserCloud->points[i - 3].y 230 | - 4 * laserCloud->points[i - 2].y 231 | + laserCloud->points[i - 1].y + laserCloud->points[i].y; 232 | 233 | float ldiffZ = 234 | laserCloud->points[i - 4].z + laserCloud->points[i - 3].z 235 | - 4 * laserCloud->points[i - 2].z 236 | + laserCloud->points[i - 1].z + laserCloud->points[i].z; 237 | 238 | float left_curvature = ldiffX * ldiffX + ldiffY * ldiffY + ldiffZ * ldiffZ; 239 | 240 | if(left_curvature < 0.01){ 241 | 242 | std::vector left_list; 243 | 244 | for(int j = -4; j < 0; j++){ 245 | left_list.push_back(laserCloud->points[i+j]); 246 | } 247 | 248 | if( left_curvature < 0.001) CloudFeatureFlag[i-2] = 1; //surf point flag && plane_judge(left_list,1000) 249 | 250 | left_surf_flag = true; 251 | } 252 | else{ 253 | left_surf_flag = false; 254 | } 255 | 256 | //right curvature 257 | float rdiffX = 258 | laserCloud->points[i + 4].x + laserCloud->points[i + 3].x 259 | - 4 * laserCloud->points[i + 2].x 260 | + laserCloud->points[i + 1].x + laserCloud->points[i].x; 261 | 262 | float rdiffY = 263 | laserCloud->points[i + 4].y + laserCloud->points[i + 3].y 264 | - 4 * laserCloud->points[i + 2].y 265 | + laserCloud->points[i + 1].y + laserCloud->points[i].y; 266 | 267 | float rdiffZ = 268 | laserCloud->points[i + 4].z + laserCloud->points[i + 3].z 269 | - 4 * laserCloud->points[i + 2].z 270 | + laserCloud->points[i + 1].z + laserCloud->points[i].z; 271 | 272 | float right_curvature = rdiffX * rdiffX + rdiffY * rdiffY + rdiffZ * rdiffZ; 273 | 274 | if(right_curvature < 0.01){ 275 | std::vector right_list; 276 | 277 | for(int j = 1; j < 5; j++){ 278 | right_list.push_back(laserCloud->points[i+j]); 279 | } 280 | if(right_curvature < 0.001 ) CloudFeatureFlag[i+2] = 1; //surf point flag && plane_judge(right_list,1000) 281 | 282 | 283 | count_num = 4; 284 | right_surf_flag = true; 285 | } 286 | else{ 287 | count_num = 1; 288 | right_surf_flag = false; 289 | } 290 | 291 | //surf-surf corner feature 292 | if(left_surf_flag && right_surf_flag){ 293 | debugnum4 ++; 294 | 295 | Eigen::Vector3d norm_left(0,0,0); 296 | Eigen::Vector3d norm_right(0,0,0); 297 | for(int k = 1;k<5;k++){ 298 | Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i-k].x-laserCloud->points[i].x, 299 | laserCloud->points[i-k].y-laserCloud->points[i].y, 300 | laserCloud->points[i-k].z-laserCloud->points[i].z); 301 | tmp.normalize(); 302 | norm_left += (k/10.0)* tmp; 303 | } 304 | for(int k = 1;k<5;k++){ 305 | Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i+k].x-laserCloud->points[i].x, 306 | laserCloud->points[i+k].y-laserCloud->points[i].y, 307 | laserCloud->points[i+k].z-laserCloud->points[i].z); 308 | tmp.normalize(); 309 | norm_right += (k/10.0)* tmp; 310 | } 311 | 312 | //calculate the angle between this group and the previous group 313 | double cc = fabs( norm_left.dot(norm_right) / (norm_left.norm()*norm_right.norm()) ); 314 | //calculate the maximum distance, the distance cannot be too small 315 | Eigen::Vector3d last_tmp = Eigen::Vector3d(laserCloud->points[i-4].x-laserCloud->points[i].x, 316 | laserCloud->points[i-4].y-laserCloud->points[i].y, 317 | laserCloud->points[i-4].z-laserCloud->points[i].z); 318 | Eigen::Vector3d current_tmp = Eigen::Vector3d(laserCloud->points[i+4].x-laserCloud->points[i].x, 319 | laserCloud->points[i+4].y-laserCloud->points[i].y, 320 | laserCloud->points[i+4].z-laserCloud->points[i].z); 321 | double last_dis = last_tmp.norm(); 322 | double current_dis = current_tmp.norm(); 323 | 324 | if(cc < 0.5 && last_dis > 0.05 && current_dis > 0.05 ){ // 325 | debugnum5 ++; 326 | CloudFeatureFlag[i] = 150; 327 | } 328 | } 329 | } 330 | for(int i = 5; i < cloudSize - 5; i ++){ 331 | float diff_left[2]; 332 | float diff_right[2]; 333 | float depth = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + 334 | laserCloud->points[i].y * laserCloud->points[i].y + 335 | laserCloud->points[i].z * laserCloud->points[i].z); 336 | 337 | for(int count = 1; count < 3; count++ ){ 338 | float diffX1 = laserCloud->points[i + count].x - laserCloud->points[i].x; 339 | float diffY1 = laserCloud->points[i + count].y - laserCloud->points[i].y; 340 | float diffZ1 = laserCloud->points[i + count].z - laserCloud->points[i].z; 341 | diff_right[count - 1] = sqrt(diffX1 * diffX1 + diffY1 * diffY1 + diffZ1 * diffZ1); 342 | 343 | float diffX2 = laserCloud->points[i - count].x - laserCloud->points[i].x; 344 | float diffY2 = laserCloud->points[i - count].y - laserCloud->points[i].y; 345 | float diffZ2 = laserCloud->points[i - count].z - laserCloud->points[i].z; 346 | diff_left[count - 1] = sqrt(diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2); 347 | } 348 | 349 | float depth_right = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + 350 | laserCloud->points[i + 1].y * laserCloud->points[i + 1].y + 351 | laserCloud->points[i + 1].z * laserCloud->points[i + 1].z); 352 | float depth_left = sqrt(laserCloud->points[i - 1].x * laserCloud->points[i - 1].x + 353 | laserCloud->points[i - 1].y * laserCloud->points[i - 1].y + 354 | laserCloud->points[i - 1].z * laserCloud->points[i - 1].z); 355 | 356 | //outliers 357 | if( (diff_right[0] > 0.1*depth && diff_left[0] > 0.1*depth) ){ 358 | debugnum1 ++; 359 | CloudFeatureFlag[i] = 250; 360 | continue; 361 | } 362 | 363 | //break points 364 | if(fabs(diff_right[0] - diff_left[0])>0.1){ 365 | if(diff_right[0] > diff_left[0]){ 366 | 367 | Eigen::Vector3d surf_vector = Eigen::Vector3d(laserCloud->points[i-4].x-laserCloud->points[i].x, 368 | laserCloud->points[i-4].y-laserCloud->points[i].y, 369 | laserCloud->points[i-4].z-laserCloud->points[i].z); 370 | Eigen::Vector3d lidar_vector = Eigen::Vector3d(laserCloud->points[i].x, 371 | laserCloud->points[i].y, 372 | laserCloud->points[i].z); 373 | double left_surf_dis = surf_vector.norm(); 374 | //calculate the angle between the laser direction and the surface 375 | double cc = fabs( surf_vector.dot(lidar_vector) / (surf_vector.norm()*lidar_vector.norm()) ); 376 | 377 | std::vector left_list; 378 | double min_dis = 10000; 379 | double max_dis = 0; 380 | for(int j = 0; j < 4; j++){ //TODO: change the plane window size and add thin rod support 381 | left_list.push_back(laserCloud->points[i-j]); 382 | Eigen::Vector3d temp_vector = Eigen::Vector3d(laserCloud->points[i-j].x-laserCloud->points[i-j-1].x, 383 | laserCloud->points[i-j].y-laserCloud->points[i-j-1].y, 384 | laserCloud->points[i-j].z-laserCloud->points[i-j-1].z); 385 | 386 | if(j == 3) break; 387 | double temp_dis = temp_vector.norm(); 388 | if(temp_dis < min_dis) min_dis = temp_dis; 389 | if(temp_dis > max_dis) max_dis = temp_dis; 390 | } 391 | bool left_is_plane = plane_judge(left_list,100); 392 | 393 | if(left_is_plane && (max_dis < 2*min_dis) && left_surf_dis < 0.05 * depth && cc < 0.8){// 394 | if(depth_right > depth_left){ 395 | CloudFeatureFlag[i] = 100; 396 | } 397 | else{ 398 | if(depth_right == 0) CloudFeatureFlag[i] = 100; 399 | } 400 | } 401 | } 402 | else{ 403 | 404 | Eigen::Vector3d surf_vector = Eigen::Vector3d(laserCloud->points[i+4].x-laserCloud->points[i].x, 405 | laserCloud->points[i+4].y-laserCloud->points[i].y, 406 | laserCloud->points[i+4].z-laserCloud->points[i].z); 407 | Eigen::Vector3d lidar_vector = Eigen::Vector3d(laserCloud->points[i].x, 408 | laserCloud->points[i].y, 409 | laserCloud->points[i].z); 410 | double right_surf_dis = surf_vector.norm(); 411 | //calculate the angle between the laser direction and the surface 412 | double cc = fabs( surf_vector.dot(lidar_vector) / (surf_vector.norm()*lidar_vector.norm()) ); 413 | 414 | std::vector right_list; 415 | double min_dis = 10000; 416 | double max_dis = 0; 417 | for(int j = 0; j < 4; j++){ //TODO: change the plane window size and add thin rod support 418 | right_list.push_back(laserCloud->points[i-j]); 419 | Eigen::Vector3d temp_vector = Eigen::Vector3d(laserCloud->points[i+j].x-laserCloud->points[i+j+1].x, 420 | laserCloud->points[i+j].y-laserCloud->points[i+j+1].y, 421 | laserCloud->points[i+j].z-laserCloud->points[i+j+1].z); 422 | 423 | if(j == 3) break; 424 | double temp_dis = temp_vector.norm(); 425 | if(temp_dis < min_dis) min_dis = temp_dis; 426 | if(temp_dis > max_dis) max_dis = temp_dis; 427 | } 428 | bool right_is_plane = plane_judge(right_list,100); 429 | 430 | if(right_is_plane && (max_dis < 2*min_dis) && right_surf_dis < 0.05 * depth && cc < 0.8){ // 431 | 432 | if(depth_right < depth_left){ 433 | CloudFeatureFlag[i] = 100; 434 | } 435 | else{ 436 | if(depth_left == 0) CloudFeatureFlag[i] = 100; 437 | } 438 | } 439 | } 440 | } 441 | 442 | // break point select 443 | if(CloudFeatureFlag[i] == 100){ 444 | debugnum2++; 445 | std::vector front_norms; 446 | Eigen::Vector3d norm_front(0,0,0); 447 | Eigen::Vector3d norm_back(0,0,0); 448 | for(int k = 1;k<4;k++){ 449 | Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i-k].x-laserCloud->points[i].x, 450 | laserCloud->points[i-k].y-laserCloud->points[i].y, 451 | laserCloud->points[i-k].z-laserCloud->points[i].z); 452 | tmp.normalize(); 453 | front_norms.push_back(tmp); 454 | norm_front += (k/6.0)* tmp; 455 | } 456 | std::vector back_norms; 457 | for(int k = 1;k<4;k++){ 458 | Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i+k].x-laserCloud->points[i].x, 459 | laserCloud->points[i+k].y-laserCloud->points[i].y, 460 | laserCloud->points[i+k].z-laserCloud->points[i].z); 461 | tmp.normalize(); 462 | back_norms.push_back(tmp); 463 | norm_back += (k/6.0)* tmp; 464 | } 465 | double cc = fabs( norm_front.dot(norm_back) / (norm_front.norm()*norm_back.norm()) ); 466 | if(cc < 0.8){ 467 | debugnum3++; 468 | }else{ 469 | CloudFeatureFlag[i] = 0; 470 | } 471 | 472 | continue; 473 | 474 | } 475 | } 476 | 477 | //push_back feature 478 | for(int i = 0; i < cloudSize; i++){ 479 | //laserCloud->points[i].intensity = double(CloudFeatureFlag[i]) / 10000; 480 | float dis = laserCloud->points[i].x * laserCloud->points[i].x 481 | + laserCloud->points[i].y * laserCloud->points[i].y 482 | + laserCloud->points[i].z * laserCloud->points[i].z; 483 | float dis2 = laserCloud->points[i].y * laserCloud->points[i].y + laserCloud->points[i].z * laserCloud->points[i].z; 484 | float theta2 = std::asin(sqrt(dis2/dis)) / M_PI * 180; 485 | //std::cout<<"DEBUG theta "< 34.2 || theta2 < 1){ 487 | // continue; 488 | // } 489 | //if(dis > 30*30) continue; 490 | 491 | if(CloudFeatureFlag[i] == 1){ 492 | surfPointsFlat.push_back(laserCloud->points[i]); 493 | continue; 494 | } 495 | 496 | if(CloudFeatureFlag[i] == 100 || CloudFeatureFlag[i] == 150){ 497 | cornerPointsSharp.push_back(laserCloud->points[i]); 498 | } 499 | } 500 | 501 | 502 | std::cout<<"ALL point: "<header.stamp; 509 | laserCloudOutMsg.header.frame_id = "/livox"; 510 | pubLaserCloud.publish(laserCloudOutMsg); 511 | 512 | sensor_msgs::PointCloud2 cornerPointsSharpMsg; 513 | pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg); 514 | cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp; 515 | cornerPointsSharpMsg.header.frame_id = "/livox"; 516 | pubCornerPointsSharp.publish(cornerPointsSharpMsg); 517 | 518 | sensor_msgs::PointCloud2 surfPointsFlat2; 519 | pcl::toROSMsg(surfPointsFlat, surfPointsFlat2); 520 | surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp; 521 | surfPointsFlat2.header.frame_id = "/livox"; 522 | pubSurfPointsFlat.publish(surfPointsFlat2); 523 | 524 | } 525 | 526 | int main(int argc, char** argv) 527 | { 528 | ros::init(argc, argv, "scanRegistration"); 529 | ros::NodeHandle nh; 530 | 531 | // ros::Subscriber subLaserCloud_for_hk = nh.subscribe 532 | // ("/livox/lidar", 2, laserCloudHandler_temp); 533 | // pubLaserCloud_for_hk = nh.advertise 534 | // ("/livox/lidar_temp", 2); 535 | 536 | ros::Subscriber subLaserCloud = nh.subscribe 537 | ("/livox/lidar", 100, laserCloudHandler); 538 | pubLaserCloud = nh.advertise 539 | ("/livox_cloud", 20); 540 | 541 | pubCornerPointsSharp = nh.advertise 542 | ("/laser_cloud_sharp", 20); 543 | 544 | 545 | pubSurfPointsFlat = nh.advertise 546 | ("/laser_cloud_flat", 20); 547 | 548 | 549 | ros::spin(); 550 | 551 | return 0; 552 | } 553 | -------------------------------------------------------------------------------- /src/scanRegistration_horizon.cpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 4 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 5 | 6 | // Modifier: Livox dev@livoxtech.com 7 | 8 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 9 | // Further contributions copyright (c) 2016, Southwest Research Institute 10 | // All rights reserved. 11 | // 12 | // Redistribution and use in source and binary forms, with or without 13 | // modification, are permitted provided that the following conditions are met: 14 | // 15 | // 1. Redistributions of source code must retain the above copyright notice, 16 | // this list of conditions and the following disclaimer. 17 | // 2. Redistributions in binary form must reproduce the above copyright notice, 18 | // this list of conditions and the following disclaimer in the documentation 19 | // and/or other materials provided with the distribution. 20 | // 3. Neither the name of the copyright holder nor the names of its 21 | // contributors may be used to endorse or promote products derived from this 22 | // software without specific prior written permission. 23 | // 24 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 28 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 29 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 30 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 31 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 32 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 33 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | // POSSIBILITY OF SUCH DAMAGE. 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | 50 | typedef pcl::PointXYZINormal PointType; 51 | int scanID; 52 | int N_SCANS = 6; 53 | int CloudFeatureFlag[32000]; 54 | 55 | ros::Publisher pubLaserCloud; 56 | ros::Publisher pubCornerPointsSharp; 57 | ros::Publisher pubSurfPointsFlat; 58 | ros::Publisher pubLaserCloud_temp; 59 | std::vector msg_window; 60 | cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0)); 61 | cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0)); 62 | cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0)); 63 | 64 | bool plane_judge(const std::vector& point_list,const int plane_threshold) 65 | { 66 | int num = point_list.size(); 67 | float cx = 0; 68 | float cy = 0; 69 | float cz = 0; 70 | for (int j = 0; j < num; j++) { 71 | cx += point_list[j].x; 72 | cy += point_list[j].y; 73 | cz += point_list[j].z; 74 | } 75 | cx /= num; 76 | cy /= num; 77 | cz /= num; 78 | //mean square error 79 | float a11 = 0; 80 | float a12 = 0; 81 | float a13 = 0; 82 | float a22 = 0; 83 | float a23 = 0; 84 | float a33 = 0; 85 | for (int j = 0; j < num; j++) { 86 | float ax = point_list[j].x - cx; 87 | float ay = point_list[j].y - cy; 88 | float az = point_list[j].z - cz; 89 | 90 | a11 += ax * ax; 91 | a12 += ax * ay; 92 | a13 += ax * az; 93 | a22 += ay * ay; 94 | a23 += ay * az; 95 | a33 += az * az; 96 | } 97 | a11 /= num; 98 | a12 /= num; 99 | a13 /= num; 100 | a22 /= num; 101 | a23 /= num; 102 | a33 /= num; 103 | 104 | matA1.at(0, 0) = a11; 105 | matA1.at(0, 1) = a12; 106 | matA1.at(0, 2) = a13; 107 | matA1.at(1, 0) = a12; 108 | matA1.at(1, 1) = a22; 109 | matA1.at(1, 2) = a23; 110 | matA1.at(2, 0) = a13; 111 | matA1.at(2, 1) = a23; 112 | matA1.at(2, 2) = a33; 113 | 114 | cv::eigen(matA1, matD1, matV1); 115 | if (matD1.at(0, 0) > plane_threshold * matD1.at(0, 1)) { 116 | return true; 117 | } 118 | else{ 119 | return false; 120 | } 121 | } 122 | void laserCloudHandler_temp(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) //for hkmars data 123 | { 124 | 125 | pcl::PointCloud::Ptr laserCloudIn(new pcl::PointCloud()); 126 | 127 | if(msg_window.size() < 2){ 128 | msg_window.push_back(laserCloudMsg); 129 | } 130 | else{ 131 | msg_window.erase(msg_window.begin()); 132 | msg_window.push_back(laserCloudMsg); 133 | } 134 | 135 | for(int i = 0; i < msg_window.size();i++){ 136 | pcl::PointCloud temp; 137 | pcl::fromROSMsg(*msg_window[i], temp); 138 | *laserCloudIn += temp; 139 | } 140 | sensor_msgs::PointCloud2 laserCloudOutMsg; 141 | pcl::toROSMsg(*laserCloudIn, laserCloudOutMsg); 142 | laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; 143 | laserCloudOutMsg.header.frame_id = "/livox"; 144 | pubLaserCloud_temp.publish(laserCloudOutMsg); 145 | 146 | } 147 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) 148 | { 149 | pcl::PointCloud laserCloudIn; 150 | pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); 151 | 152 | int cloudSize = laserCloudIn.points.size(); 153 | 154 | std::cout<<"DEBUG first cloudSize "< 32000) cloudSize = 32000; 157 | 158 | int count = cloudSize; 159 | 160 | PointType point; 161 | std::vector> laserCloudScans(N_SCANS); 162 | for (int i = 0; i < cloudSize; i++) { 163 | point.x = laserCloudIn.points[i].x; 164 | point.y = laserCloudIn.points[i].y; 165 | point.z = laserCloudIn.points[i].z; 166 | point.intensity = laserCloudIn.points[i].intensity; 167 | point.curvature = laserCloudIn.points[i].curvature; 168 | int scanID = 0; 169 | if (N_SCANS == 6) { 170 | scanID = (int)point.intensity; 171 | } 172 | laserCloudScans[scanID].push_back(point); 173 | } 174 | 175 | pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); 176 | 177 | for (int i = 0; i < N_SCANS; i++) { 178 | *laserCloud += laserCloudScans[i]; 179 | } 180 | 181 | cloudSize = laserCloud->size(); 182 | 183 | for (int i = 0; i < cloudSize; i++) { 184 | CloudFeatureFlag[i] = 0; 185 | } 186 | 187 | pcl::PointCloud cornerPointsSharp; 188 | 189 | pcl::PointCloud surfPointsFlat; 190 | 191 | pcl::PointCloud laserCloudFull; 192 | 193 | int debugnum1 = 0; 194 | int debugnum2 = 0; 195 | int debugnum3 = 0; 196 | int debugnum4 = 0; 197 | int debugnum5 = 0; 198 | 199 | int count_num = 1; 200 | bool left_surf_flag = false; 201 | bool right_surf_flag = false; 202 | Eigen::Vector3d surf_vector_current(0,0,0); 203 | Eigen::Vector3d surf_vector_last(0,0,0); 204 | int last_surf_position = 0; 205 | double depth_threshold = 0.1; 206 | 207 | 208 | //******************************************************************************************************************************************** 209 | for (int i = 5; i < cloudSize - 5; i += count_num ) { 210 | float depth = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + 211 | laserCloud->points[i].y * laserCloud->points[i].y + 212 | laserCloud->points[i].z * laserCloud->points[i].z); 213 | 214 | // if(depth < 2) depth_threshold = 0.05; 215 | // if(depth > 30) depth_threshold = 0.1; 216 | //left curvature 217 | float ldiffX = 218 | laserCloud->points[i - 4].x + laserCloud->points[i - 3].x 219 | - 4 * laserCloud->points[i - 2].x 220 | + laserCloud->points[i - 1].x + laserCloud->points[i].x; 221 | 222 | float ldiffY = 223 | laserCloud->points[i - 4].y + laserCloud->points[i - 3].y 224 | - 4 * laserCloud->points[i - 2].y 225 | + laserCloud->points[i - 1].y + laserCloud->points[i].y; 226 | 227 | float ldiffZ = 228 | laserCloud->points[i - 4].z + laserCloud->points[i - 3].z 229 | - 4 * laserCloud->points[i - 2].z 230 | + laserCloud->points[i - 1].z + laserCloud->points[i].z; 231 | 232 | float left_curvature = ldiffX * ldiffX + ldiffY * ldiffY + ldiffZ * ldiffZ; 233 | 234 | if(left_curvature < 0.01){ 235 | 236 | std::vector left_list; 237 | 238 | for(int j = -4; j < 0; j++){ 239 | left_list.push_back(laserCloud->points[i+j]); 240 | } 241 | 242 | if( left_curvature < 0.001) CloudFeatureFlag[i-2] = 1; //surf point flag && plane_judge(left_list,1000) 243 | 244 | left_surf_flag = true; 245 | } 246 | else{ 247 | left_surf_flag = false; 248 | } 249 | 250 | //right curvature 251 | float rdiffX = 252 | laserCloud->points[i + 4].x + laserCloud->points[i + 3].x 253 | - 4 * laserCloud->points[i + 2].x 254 | + laserCloud->points[i + 1].x + laserCloud->points[i].x; 255 | 256 | float rdiffY = 257 | laserCloud->points[i + 4].y + laserCloud->points[i + 3].y 258 | - 4 * laserCloud->points[i + 2].y 259 | + laserCloud->points[i + 1].y + laserCloud->points[i].y; 260 | 261 | float rdiffZ = 262 | laserCloud->points[i + 4].z + laserCloud->points[i + 3].z 263 | - 4 * laserCloud->points[i + 2].z 264 | + laserCloud->points[i + 1].z + laserCloud->points[i].z; 265 | 266 | float right_curvature = rdiffX * rdiffX + rdiffY * rdiffY + rdiffZ * rdiffZ; 267 | 268 | if(right_curvature < 0.01){ 269 | std::vector right_list; 270 | 271 | for(int j = 1; j < 5; j++){ 272 | right_list.push_back(laserCloud->points[i+j]); 273 | } 274 | if(right_curvature < 0.001 ) CloudFeatureFlag[i+2] = 1; //surf point flag && plane_judge(right_list,1000) 275 | 276 | 277 | count_num = 4; 278 | right_surf_flag = true; 279 | } 280 | else{ 281 | count_num = 1; 282 | right_surf_flag = false; 283 | } 284 | 285 | //surf-surf corner feature 286 | if(left_surf_flag && right_surf_flag){ 287 | debugnum4 ++; 288 | 289 | Eigen::Vector3d norm_left(0,0,0); 290 | Eigen::Vector3d norm_right(0,0,0); 291 | for(int k = 1;k<5;k++){ 292 | Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i-k].x-laserCloud->points[i].x, 293 | laserCloud->points[i-k].y-laserCloud->points[i].y, 294 | laserCloud->points[i-k].z-laserCloud->points[i].z); 295 | tmp.normalize(); 296 | norm_left += (k/10.0)* tmp; 297 | } 298 | for(int k = 1;k<5;k++){ 299 | Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i+k].x-laserCloud->points[i].x, 300 | laserCloud->points[i+k].y-laserCloud->points[i].y, 301 | laserCloud->points[i+k].z-laserCloud->points[i].z); 302 | tmp.normalize(); 303 | norm_right += (k/10.0)* tmp; 304 | } 305 | 306 | //calculate the angle between this group and the previous group 307 | double cc = fabs( norm_left.dot(norm_right) / (norm_left.norm()*norm_right.norm()) ); 308 | //calculate the maximum distance, the distance cannot be too small 309 | Eigen::Vector3d last_tmp = Eigen::Vector3d(laserCloud->points[i-4].x-laserCloud->points[i].x, 310 | laserCloud->points[i-4].y-laserCloud->points[i].y, 311 | laserCloud->points[i-4].z-laserCloud->points[i].z); 312 | Eigen::Vector3d current_tmp = Eigen::Vector3d(laserCloud->points[i+4].x-laserCloud->points[i].x, 313 | laserCloud->points[i+4].y-laserCloud->points[i].y, 314 | laserCloud->points[i+4].z-laserCloud->points[i].z); 315 | double last_dis = last_tmp.norm(); 316 | double current_dis = current_tmp.norm(); 317 | 318 | if(cc < 0.5 && last_dis > 0.05 && current_dis > 0.05 ){ // 319 | debugnum5 ++; 320 | CloudFeatureFlag[i] = 150; 321 | } 322 | } 323 | } 324 | for(int i = 5; i < cloudSize - 5; i ++){ 325 | float diff_left[2]; 326 | float diff_right[2]; 327 | float depth = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + 328 | laserCloud->points[i].y * laserCloud->points[i].y + 329 | laserCloud->points[i].z * laserCloud->points[i].z); 330 | 331 | for(int count = 1; count < 3; count++ ){ 332 | float diffX1 = laserCloud->points[i + count].x - laserCloud->points[i].x; 333 | float diffY1 = laserCloud->points[i + count].y - laserCloud->points[i].y; 334 | float diffZ1 = laserCloud->points[i + count].z - laserCloud->points[i].z; 335 | diff_right[count - 1] = sqrt(diffX1 * diffX1 + diffY1 * diffY1 + diffZ1 * diffZ1); 336 | 337 | float diffX2 = laserCloud->points[i - count].x - laserCloud->points[i].x; 338 | float diffY2 = laserCloud->points[i - count].y - laserCloud->points[i].y; 339 | float diffZ2 = laserCloud->points[i - count].z - laserCloud->points[i].z; 340 | diff_left[count - 1] = sqrt(diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2); 341 | } 342 | 343 | float depth_right = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + 344 | laserCloud->points[i + 1].y * laserCloud->points[i + 1].y + 345 | laserCloud->points[i + 1].z * laserCloud->points[i + 1].z); 346 | float depth_left = sqrt(laserCloud->points[i - 1].x * laserCloud->points[i - 1].x + 347 | laserCloud->points[i - 1].y * laserCloud->points[i - 1].y + 348 | laserCloud->points[i - 1].z * laserCloud->points[i - 1].z); 349 | 350 | //outliers 351 | if( (diff_right[0] > 0.1*depth && diff_left[0] > 0.1*depth) ){ 352 | debugnum1 ++; 353 | CloudFeatureFlag[i] = 250; 354 | continue; 355 | } 356 | 357 | //break points 358 | if(fabs(diff_right[0] - diff_left[0])>0.1){ 359 | if(diff_right[0] > diff_left[0]){ 360 | 361 | Eigen::Vector3d surf_vector = Eigen::Vector3d(laserCloud->points[i-4].x-laserCloud->points[i].x, 362 | laserCloud->points[i-4].y-laserCloud->points[i].y, 363 | laserCloud->points[i-4].z-laserCloud->points[i].z); 364 | Eigen::Vector3d lidar_vector = Eigen::Vector3d(laserCloud->points[i].x, 365 | laserCloud->points[i].y, 366 | laserCloud->points[i].z); 367 | double left_surf_dis = surf_vector.norm(); 368 | //calculate the angle between the laser direction and the surface 369 | double cc = fabs( surf_vector.dot(lidar_vector) / (surf_vector.norm()*lidar_vector.norm()) ); 370 | 371 | std::vector left_list; 372 | double min_dis = 10000; 373 | double max_dis = 0; 374 | for(int j = 0; j < 4; j++){ //TODO: change the plane window size and add thin rod support 375 | left_list.push_back(laserCloud->points[i-j]); 376 | Eigen::Vector3d temp_vector = Eigen::Vector3d(laserCloud->points[i-j].x-laserCloud->points[i-j-1].x, 377 | laserCloud->points[i-j].y-laserCloud->points[i-j-1].y, 378 | laserCloud->points[i-j].z-laserCloud->points[i-j-1].z); 379 | 380 | if(j == 3) break; 381 | double temp_dis = temp_vector.norm(); 382 | if(temp_dis < min_dis) min_dis = temp_dis; 383 | if(temp_dis > max_dis) max_dis = temp_dis; 384 | } 385 | bool left_is_plane = plane_judge(left_list,100); 386 | 387 | if(left_is_plane && (max_dis < 2*min_dis) && left_surf_dis < 0.05 * depth && cc < 0.8){// 388 | if(depth_right > depth_left){ 389 | CloudFeatureFlag[i] = 100; 390 | } 391 | else{ 392 | if(depth_right == 0) CloudFeatureFlag[i] = 100; 393 | } 394 | } 395 | } 396 | else{ 397 | 398 | Eigen::Vector3d surf_vector = Eigen::Vector3d(laserCloud->points[i+4].x-laserCloud->points[i].x, 399 | laserCloud->points[i+4].y-laserCloud->points[i].y, 400 | laserCloud->points[i+4].z-laserCloud->points[i].z); 401 | Eigen::Vector3d lidar_vector = Eigen::Vector3d(laserCloud->points[i].x, 402 | laserCloud->points[i].y, 403 | laserCloud->points[i].z); 404 | double right_surf_dis = surf_vector.norm(); 405 | //calculate the angle between the laser direction and the surface 406 | double cc = fabs( surf_vector.dot(lidar_vector) / (surf_vector.norm()*lidar_vector.norm()) ); 407 | 408 | std::vector right_list; 409 | double min_dis = 10000; 410 | double max_dis = 0; 411 | for(int j = 0; j < 4; j++){ //TODO: change the plane window size and add thin rod support 412 | right_list.push_back(laserCloud->points[i-j]); 413 | Eigen::Vector3d temp_vector = Eigen::Vector3d(laserCloud->points[i+j].x-laserCloud->points[i+j+1].x, 414 | laserCloud->points[i+j].y-laserCloud->points[i+j+1].y, 415 | laserCloud->points[i+j].z-laserCloud->points[i+j+1].z); 416 | 417 | if(j == 3) break; 418 | double temp_dis = temp_vector.norm(); 419 | if(temp_dis < min_dis) min_dis = temp_dis; 420 | if(temp_dis > max_dis) max_dis = temp_dis; 421 | } 422 | bool right_is_plane = plane_judge(right_list,100); 423 | 424 | if(right_is_plane && (max_dis < 2*min_dis) && right_surf_dis < 0.05 * depth && cc < 0.8){ // 425 | 426 | if(depth_right < depth_left){ 427 | CloudFeatureFlag[i] = 100; 428 | } 429 | else{ 430 | if(depth_left == 0) CloudFeatureFlag[i] = 100; 431 | } 432 | } 433 | } 434 | } 435 | 436 | // break point select 437 | if(CloudFeatureFlag[i] == 100){ 438 | debugnum2++; 439 | std::vector front_norms; 440 | Eigen::Vector3d norm_front(0,0,0); 441 | Eigen::Vector3d norm_back(0,0,0); 442 | for(int k = 1;k<4;k++){ 443 | Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i-k].x-laserCloud->points[i].x, 444 | laserCloud->points[i-k].y-laserCloud->points[i].y, 445 | laserCloud->points[i-k].z-laserCloud->points[i].z); 446 | tmp.normalize(); 447 | front_norms.push_back(tmp); 448 | norm_front += (k/6.0)* tmp; 449 | } 450 | std::vector back_norms; 451 | for(int k = 1;k<4;k++){ 452 | Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i+k].x-laserCloud->points[i].x, 453 | laserCloud->points[i+k].y-laserCloud->points[i].y, 454 | laserCloud->points[i+k].z-laserCloud->points[i].z); 455 | tmp.normalize(); 456 | back_norms.push_back(tmp); 457 | norm_back += (k/6.0)* tmp; 458 | } 459 | double cc = fabs( norm_front.dot(norm_back) / (norm_front.norm()*norm_back.norm()) ); 460 | if(cc < 0.8){ 461 | debugnum3++; 462 | }else{ 463 | CloudFeatureFlag[i] = 0; 464 | } 465 | 466 | continue; 467 | 468 | } 469 | } 470 | 471 | //push_back feature 472 | for(int i = 0; i < cloudSize; i++){ 473 | //laserCloud->points[i].intensity = double(CloudFeatureFlag[i]) / 10000; 474 | float dis = laserCloud->points[i].x * laserCloud->points[i].x 475 | + laserCloud->points[i].y * laserCloud->points[i].y 476 | + laserCloud->points[i].z * laserCloud->points[i].z; 477 | float dis2 = laserCloud->points[i].y * laserCloud->points[i].y + laserCloud->points[i].z * laserCloud->points[i].z; 478 | float theta2 = std::asin(sqrt(dis2/dis)) / M_PI * 180; 479 | //std::cout<<"DEBUG theta "< 34.2 || theta2 < 1){ 481 | // continue; 482 | // } 483 | //if(dis > 30*30) continue; 484 | 485 | if(CloudFeatureFlag[i] == 1){ 486 | surfPointsFlat.push_back(laserCloud->points[i]); 487 | continue; 488 | } 489 | 490 | if(CloudFeatureFlag[i] == 100 || CloudFeatureFlag[i] == 150){ 491 | cornerPointsSharp.push_back(laserCloud->points[i]); 492 | } 493 | } 494 | 495 | 496 | std::cout<<"ALL point: "<header.stamp; 503 | laserCloudOutMsg.header.frame_id = "/livox"; 504 | pubLaserCloud.publish(laserCloudOutMsg); 505 | 506 | sensor_msgs::PointCloud2 cornerPointsSharpMsg; 507 | pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg); 508 | cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp; 509 | cornerPointsSharpMsg.header.frame_id = "/livox"; 510 | pubCornerPointsSharp.publish(cornerPointsSharpMsg); 511 | 512 | sensor_msgs::PointCloud2 surfPointsFlat2; 513 | pcl::toROSMsg(surfPointsFlat, surfPointsFlat2); 514 | surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp; 515 | surfPointsFlat2.header.frame_id = "/livox"; 516 | pubSurfPointsFlat.publish(surfPointsFlat2); 517 | 518 | } 519 | 520 | int main(int argc, char** argv) 521 | { 522 | ros::init(argc, argv, "scanRegistration"); 523 | ros::NodeHandle nh; 524 | 525 | // ros::Subscriber subLaserCloud_for_hk = nh.subscribe 526 | // ("/livox/lidar", 2, laserCloudHandler_temp); 527 | // pubLaserCloud_for_hk = nh.advertise 528 | // ("/livox/lidar_temp", 2); 529 | 530 | ros::Subscriber subLaserCloud = nh.subscribe 531 | ("/livox_pcl0", 100, laserCloudHandler); 532 | pubLaserCloud = nh.advertise 533 | ("/livox_cloud", 100); 534 | 535 | pubCornerPointsSharp = nh.advertise 536 | ("/laser_cloud_sharp", 100); 537 | 538 | 539 | pubSurfPointsFlat = nh.advertise 540 | ("/laser_cloud_flat", 100); 541 | 542 | 543 | ros::spin(); 544 | 545 | return 0; 546 | } 547 | --------------------------------------------------------------------------------