├── 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 |
--------------------------------------------------------------------------------