├── .github └── stale.yml ├── LICENSE ├── LeGO-LOAM ├── CMakeLists.txt ├── include │ └── utility.h ├── launch │ ├── block.png │ ├── dataset-demo.gif │ ├── demo.gif │ ├── google-earth.png │ ├── jackal-label.jpg │ ├── odometry.jpg │ ├── run.launch │ ├── seg-total.jpg │ └── test.rviz ├── package.xml └── src │ ├── featureAssociation.cpp │ ├── imageProjection.cpp │ ├── mapOptmization.cpp │ └── transformFusion.cpp ├── README.md ├── Shan_Englot_IROS_2018_Preprint.pdf └── cloud_msgs ├── CMakeLists.txt ├── msg └── cloud_info.msg └── package.xml /.github/stale.yml: -------------------------------------------------------------------------------- 1 | # Number of days of inactivity before an issue becomes stale 2 | daysUntilStale: 30 3 | # Number of days of inactivity before a stale issue is closed 4 | daysUntilClose: 7 5 | # Issues with these labels will never be considered stale 6 | exemptLabels: 7 | - pinned 8 | - security 9 | # Label to use when marking an issue as stale 10 | staleLabel: wontfix 11 | # Comment to post when marking an issue as stale. Set to `false` to disable 12 | markComment: > 13 | This issue has been automatically marked as stale because it has not had 14 | recent activity. It will be closed if no further activity occurs. Thank you 15 | for your contributions. 16 | # Comment to post when closing a stale issue. Set to `false` to disable 17 | closeComment: false 18 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, Robust Field Autonomy Lab 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /LeGO-LOAM/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lego_loam) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | tf 8 | roscpp 9 | rospy 10 | cv_bridge 11 | image_transport 12 | 13 | pcl_ros 14 | pcl_conversions 15 | 16 | std_msgs 17 | sensor_msgs 18 | geometry_msgs 19 | nav_msgs 20 | cloud_msgs 21 | ) 22 | 23 | find_package(GTSAM REQUIRED QUIET) 24 | find_package(PCL REQUIRED QUIET) 25 | find_package(OpenCV REQUIRED QUIET) 26 | 27 | catkin_package( 28 | INCLUDE_DIRS include 29 | CATKIN_DEPENDS cloud_msgs 30 | DEPENDS PCL 31 | ) 32 | 33 | include_directories( 34 | include 35 | ${catkin_INCLUDE_DIRS} 36 | ${PCL_INCLUDE_DIRS} 37 | ${OpenCV_INCLUDE_DIRS} 38 | ${GTSAM_INCLUDE_DIR} 39 | ) 40 | 41 | link_directories( 42 | include 43 | ${OpenCV_LIBRARY_DIRS} 44 | ${PCL_LIBRARY_DIRS} 45 | ${GTSAM_LIBRARY_DIRS} 46 | ) 47 | 48 | add_executable(imageProjection src/imageProjection.cpp) 49 | add_dependencies(imageProjection ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp) 50 | target_link_libraries(imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 51 | 52 | add_executable(featureAssociation src/featureAssociation.cpp) 53 | add_dependencies(featureAssociation ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp) 54 | target_link_libraries(featureAssociation ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 55 | 56 | add_executable(mapOptmization src/mapOptmization.cpp) 57 | target_link_libraries(mapOptmization ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam) 58 | 59 | add_executable(transformFusion src/transformFusion.cpp) 60 | target_link_libraries(transformFusion ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) -------------------------------------------------------------------------------- /LeGO-LOAM/include/utility.h: -------------------------------------------------------------------------------- 1 | #ifndef _UTILITY_LIDAR_ODOMETRY_H_ 2 | #define _UTILITY_LIDAR_ODOMETRY_H_ 3 | 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "cloud_msgs/cloud_info.h" 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #define PI 3.14159265 48 | 49 | using namespace std; 50 | 51 | typedef pcl::PointXYZI PointType; 52 | 53 | extern const string pointCloudTopic = "/velodyne_points"; 54 | extern const string imuTopic = "/imu/data"; 55 | 56 | // Save pcd 57 | extern const string fileDirectory = "/tmp/"; 58 | 59 | // Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below) 60 | extern const bool useCloudRing = true; // if true, ang_res_y and ang_bottom are not used 61 | 62 | // VLP-16 63 | extern const int N_SCAN = 16; 64 | extern const int Horizon_SCAN = 1800; 65 | extern const float ang_res_x = 0.2; 66 | extern const float ang_res_y = 2.0; 67 | extern const float ang_bottom = 15.0+0.1; 68 | extern const int groundScanInd = 7; 69 | 70 | // HDL-32E 71 | // extern const int N_SCAN = 32; 72 | // extern const int Horizon_SCAN = 1800; 73 | // extern const float ang_res_x = 360.0/float(Horizon_SCAN); 74 | // extern const float ang_res_y = 41.33/float(N_SCAN-1); 75 | // extern const float ang_bottom = 30.67; 76 | // extern const int groundScanInd = 20; 77 | 78 | // VLS-128 79 | // extern const int N_SCAN = 128; 80 | // extern const int Horizon_SCAN = 1800; 81 | // extern const float ang_res_x = 0.2; 82 | // extern const float ang_res_y = 0.3; 83 | // extern const float ang_bottom = 25.0; 84 | // extern const int groundScanInd = 10; 85 | 86 | // Ouster users may need to uncomment line 159 in imageProjection.cpp 87 | // Usage of Ouster imu data is not fully supported yet (LeGO-LOAM needs 9-DOF IMU), please just publish point cloud data 88 | // Ouster OS1-16 89 | // extern const int N_SCAN = 16; 90 | // extern const int Horizon_SCAN = 1024; 91 | // extern const float ang_res_x = 360.0/float(Horizon_SCAN); 92 | // extern const float ang_res_y = 33.2/float(N_SCAN-1); 93 | // extern const float ang_bottom = 16.6+0.1; 94 | // extern const int groundScanInd = 7; 95 | 96 | // Ouster OS1-64 97 | // extern const int N_SCAN = 64; 98 | // extern const int Horizon_SCAN = 1024; 99 | // extern const float ang_res_x = 360.0/float(Horizon_SCAN); 100 | // extern const float ang_res_y = 33.2/float(N_SCAN-1); 101 | // extern const float ang_bottom = 16.6+0.1; 102 | // extern const int groundScanInd = 15; 103 | 104 | extern const bool loopClosureEnableFlag = false; 105 | extern const double mappingProcessInterval = 0.3; 106 | 107 | extern const float scanPeriod = 0.1; 108 | extern const int systemDelay = 0; 109 | extern const int imuQueLength = 200; 110 | 111 | extern const float sensorMinimumRange = 1.0; 112 | extern const float sensorMountAngle = 0.0; 113 | extern const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy 114 | extern const int segmentValidPointNum = 5; 115 | extern const int segmentValidLineNum = 3; 116 | extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI; 117 | extern const float segmentAlphaY = ang_res_y / 180.0 * M_PI; 118 | 119 | 120 | extern const int edgeFeatureNum = 2; 121 | extern const int surfFeatureNum = 4; 122 | extern const int sectionsTotal = 6; 123 | extern const float edgeThreshold = 0.1; 124 | extern const float surfThreshold = 0.1; 125 | extern const float nearestFeatureSearchSqDist = 25; 126 | 127 | 128 | // Mapping Params 129 | extern const float surroundingKeyframeSearchRadius = 50.0; // key frame that is within n meters from current pose will be considerd for scan-to-map optimization (when loop closure disabled) 130 | extern const int surroundingKeyframeSearchNum = 50; // submap size (when loop closure enabled) 131 | // history key frames (history submap for loop closure) 132 | extern const float historyKeyframeSearchRadius = 7.0; // key frame that is within n meters from current pose will be considerd for loop closure 133 | extern const int historyKeyframeSearchNum = 25; // 2n+1 number of hostory key frames will be fused into a submap for loop closure 134 | extern const float historyKeyframeFitnessScore = 0.3; // the smaller the better alignment 135 | 136 | extern const float globalMapVisualizationSearchRadius = 500.0; // key frames with in n meters will be visualized 137 | 138 | 139 | struct smoothness_t{ 140 | float value; 141 | size_t ind; 142 | }; 143 | 144 | struct by_value{ 145 | bool operator()(smoothness_t const &left, smoothness_t const &right) { 146 | return left.value < right.value; 147 | } 148 | }; 149 | 150 | /* 151 | * A point cloud type that has "ring" channel 152 | */ 153 | struct PointXYZIR 154 | { 155 | PCL_ADD_POINT4D 156 | PCL_ADD_INTENSITY; 157 | uint16_t ring; 158 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 159 | } EIGEN_ALIGN16; 160 | 161 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR, 162 | (float, x, x) (float, y, y) 163 | (float, z, z) (float, intensity, intensity) 164 | (uint16_t, ring, ring) 165 | ) 166 | 167 | /* 168 | * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp) 169 | */ 170 | struct PointXYZIRPYT 171 | { 172 | PCL_ADD_POINT4D 173 | PCL_ADD_INTENSITY; 174 | float roll; 175 | float pitch; 176 | float yaw; 177 | double time; 178 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 179 | } EIGEN_ALIGN16; 180 | 181 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT, 182 | (float, x, x) (float, y, y) 183 | (float, z, z) (float, intensity, intensity) 184 | (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw) 185 | (double, time, time) 186 | ) 187 | 188 | typedef PointXYZIRPYT PointTypePose; 189 | 190 | #endif 191 | -------------------------------------------------------------------------------- /LeGO-LOAM/launch/block.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/LeGO-LOAM/896a7a95a8bc510b76819d4cc48707e344bad621/LeGO-LOAM/launch/block.png -------------------------------------------------------------------------------- /LeGO-LOAM/launch/dataset-demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/LeGO-LOAM/896a7a95a8bc510b76819d4cc48707e344bad621/LeGO-LOAM/launch/dataset-demo.gif -------------------------------------------------------------------------------- /LeGO-LOAM/launch/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/LeGO-LOAM/896a7a95a8bc510b76819d4cc48707e344bad621/LeGO-LOAM/launch/demo.gif -------------------------------------------------------------------------------- /LeGO-LOAM/launch/google-earth.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/LeGO-LOAM/896a7a95a8bc510b76819d4cc48707e344bad621/LeGO-LOAM/launch/google-earth.png -------------------------------------------------------------------------------- /LeGO-LOAM/launch/jackal-label.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/LeGO-LOAM/896a7a95a8bc510b76819d4cc48707e344bad621/LeGO-LOAM/launch/jackal-label.jpg -------------------------------------------------------------------------------- /LeGO-LOAM/launch/odometry.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/LeGO-LOAM/896a7a95a8bc510b76819d4cc48707e344bad621/LeGO-LOAM/launch/odometry.jpg -------------------------------------------------------------------------------- /LeGO-LOAM/launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /LeGO-LOAM/launch/seg-total.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/LeGO-LOAM/896a7a95a8bc510b76819d4cc48707e344bad621/LeGO-LOAM/launch/seg-total.jpg -------------------------------------------------------------------------------- /LeGO-LOAM/launch/test.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 171 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | Splitter Ratio: 0.703999996 9 | Tree Height: 707 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: ~ 14 | Name: Tool Properties 15 | Splitter Ratio: 0.588679016 16 | - Class: rviz/Views 17 | Expanded: 18 | - /Current View1 19 | Name: Views 20 | Splitter Ratio: 0.5 21 | - Class: rviz/Time 22 | Experimental: false 23 | Name: Time 24 | SyncMode: 0 25 | SyncSource: Edge Features (green) 26 | - Class: rviz/Help 27 | Name: Help 28 | Toolbars: 29 | toolButtonStyle: 2 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Class: rviz/TF 34 | Enabled: false 35 | Frame Timeout: 15 36 | Frames: 37 | All Enabled: false 38 | Marker Scale: 10 39 | Name: TF 40 | Show Arrows: true 41 | Show Axes: true 42 | Show Names: true 43 | Tree: 44 | {} 45 | Update Interval: 0 46 | Value: false 47 | - Class: rviz/Axes 48 | Enabled: true 49 | Length: 2 50 | Name: map 51 | Radius: 0.300000012 52 | Reference Frame: map 53 | Value: true 54 | - Class: rviz/Axes 55 | Enabled: true 56 | Length: 1 57 | Name: base_link 58 | Radius: 0.5 59 | Reference Frame: base_link 60 | Value: true 61 | - Alpha: 1 62 | Autocompute Intensity Bounds: true 63 | Autocompute Value Bounds: 64 | Max Value: 17.0059433 65 | Min Value: -1.04788589 66 | Value: true 67 | Axis: Z 68 | Channel Name: intensity 69 | Class: rviz/PointCloud2 70 | Color: 255; 0; 0 71 | Color Transformer: AxisColor 72 | Decay Time: 0 73 | Enabled: false 74 | Invert Rainbow: false 75 | Max Color: 255; 255; 255 76 | Max Intensity: 183 77 | Min Color: 0; 0; 0 78 | Min Intensity: 1 79 | Name: Velodyne 80 | Position Transformer: XYZ 81 | Queue Size: 2 82 | Selectable: true 83 | Size (Pixels): 3 84 | Size (m): 0.00999999978 85 | Style: Points 86 | Topic: /full_cloud_projected 87 | Unreliable: false 88 | Use Fixed Frame: true 89 | Use rainbow: true 90 | Value: false 91 | - Alpha: 1 92 | Autocompute Intensity Bounds: true 93 | Autocompute Value Bounds: 94 | Max Value: 20.4566002 95 | Min Value: -3.58307004 96 | Value: true 97 | Axis: Z 98 | Channel Name: intensity 99 | Class: rviz/PointCloud2 100 | Color: 255; 255; 0 101 | Color Transformer: FlatColor 102 | Decay Time: 0 103 | Enabled: false 104 | Invert Rainbow: false 105 | Max Color: 255; 255; 255 106 | Max Intensity: 96 107 | Min Color: 0; 0; 0 108 | Min Intensity: 1 109 | Name: Surface (yellow) 110 | Position Transformer: XYZ 111 | Queue Size: 2 112 | Selectable: true 113 | Size (Pixels): 8 114 | Size (m): 0.00999999978 115 | Style: Points 116 | Topic: /laser_cloud_flat 117 | Unreliable: false 118 | Use Fixed Frame: true 119 | Use rainbow: true 120 | Value: false 121 | - Alpha: 1 122 | Autocompute Intensity Bounds: true 123 | Autocompute Value Bounds: 124 | Max Value: 20.4566002 125 | Min Value: -3.58307004 126 | Value: true 127 | Axis: Z 128 | Channel Name: intensity 129 | Class: rviz/PointCloud2 130 | Color: 110; 132; 255 131 | Color Transformer: FlatColor 132 | Decay Time: 0 133 | Enabled: false 134 | Invert Rainbow: false 135 | Max Color: 255; 255; 255 136 | Max Intensity: 96 137 | Min Color: 0; 0; 0 138 | Min Intensity: 1 139 | Name: Edge Sharp (blue) 140 | Position Transformer: XYZ 141 | Queue Size: 2 142 | Selectable: true 143 | Size (Pixels): 8 144 | Size (m): 0.00999999978 145 | Style: Points 146 | Topic: /laser_cloud_sharp 147 | Unreliable: false 148 | Use Fixed Frame: true 149 | Use rainbow: true 150 | Value: false 151 | - Alpha: 1 152 | Autocompute Intensity Bounds: true 153 | Autocompute Value Bounds: 154 | Max Value: 20.4566002 155 | Min Value: -3.58307004 156 | Value: true 157 | Axis: Z 158 | Channel Name: intensity 159 | Class: rviz/PointCloud2 160 | Color: 0; 255; 0 161 | Color Transformer: FlatColor 162 | Decay Time: 0 163 | Enabled: true 164 | Invert Rainbow: false 165 | Max Color: 255; 255; 255 166 | Max Intensity: 96 167 | Min Color: 0; 0; 0 168 | Min Intensity: 1 169 | Name: Edge Features (green) 170 | Position Transformer: XYZ 171 | Queue Size: 2 172 | Selectable: true 173 | Size (Pixels): 5 174 | Size (m): 0.00999999978 175 | Style: Points 176 | Topic: /laser_cloud_less_sharp 177 | Unreliable: false 178 | Use Fixed Frame: true 179 | Use rainbow: true 180 | Value: true 181 | - Alpha: 1 182 | Autocompute Intensity Bounds: true 183 | Autocompute Value Bounds: 184 | Max Value: 20.4566002 185 | Min Value: -3.58307004 186 | Value: true 187 | Axis: Z 188 | Channel Name: intensity 189 | Class: rviz/PointCloud2 190 | Color: 255; 170; 255 191 | Color Transformer: FlatColor 192 | Decay Time: 0 193 | Enabled: true 194 | Invert Rainbow: false 195 | Max Color: 255; 255; 255 196 | Max Intensity: 96 197 | Min Color: 0; 0; 0 198 | Min Intensity: 1 199 | Name: Surface Features (pink) 200 | Position Transformer: XYZ 201 | Queue Size: 2 202 | Selectable: true 203 | Size (Pixels): 3 204 | Size (m): 0.00999999978 205 | Style: Points 206 | Topic: /laser_cloud_less_flat 207 | Unreliable: false 208 | Use Fixed Frame: true 209 | Use rainbow: true 210 | Value: true 211 | - Alpha: 1 212 | Autocompute Intensity Bounds: true 213 | Autocompute Value Bounds: 214 | Max Value: 10.7254 215 | Min Value: -6.34819984 216 | Value: true 217 | Axis: Z 218 | Channel Name: intensity 219 | Class: rviz/PointCloud2 220 | Color: 255; 255; 255 221 | Color Transformer: FlatColor 222 | Decay Time: 0 223 | Enabled: false 224 | Invert Rainbow: false 225 | Max Color: 255; 255; 255 226 | Max Intensity: 15.1604996 227 | Min Color: 0; 0; 0 228 | Min Intensity: 8.00349998 229 | Name: Outlier Cloud 230 | Position Transformer: XYZ 231 | Queue Size: 2 232 | Selectable: true 233 | Size (Pixels): 3 234 | Size (m): 0.00999999978 235 | Style: Points 236 | Topic: /outlier_cloud 237 | Unreliable: false 238 | Use Fixed Frame: true 239 | Use rainbow: true 240 | Value: false 241 | - Alpha: 1 242 | Autocompute Intensity Bounds: true 243 | Autocompute Value Bounds: 244 | Max Value: 10 245 | Min Value: -10 246 | Value: true 247 | Axis: Z 248 | Channel Name: intensity 249 | Class: rviz/PointCloud2 250 | Color: 0; 0; 255 251 | Color Transformer: Intensity 252 | Decay Time: 0 253 | Enabled: false 254 | Invert Rainbow: false 255 | Max Color: 255; 255; 255 256 | Max Intensity: 15.1787996 257 | Min Color: 0; 0; 0 258 | Min Intensity: 0 259 | Name: Segmentation Full 260 | Position Transformer: XYZ 261 | Queue Size: 10 262 | Selectable: true 263 | Size (Pixels): 3 264 | Size (m): 0.00999999978 265 | Style: Points 266 | Topic: /segmented_cloud 267 | Unreliable: false 268 | Use Fixed Frame: true 269 | Use rainbow: true 270 | Value: false 271 | - Alpha: 1 272 | Autocompute Intensity Bounds: true 273 | Autocompute Value Bounds: 274 | Max Value: 10 275 | Min Value: -10 276 | Value: true 277 | Axis: Z 278 | Channel Name: intensity 279 | Class: rviz/PointCloud2 280 | Color: 255; 255; 255 281 | Color Transformer: Intensity 282 | Decay Time: 0 283 | Enabled: false 284 | Invert Rainbow: false 285 | Max Color: 255; 255; 255 286 | Max Intensity: 241 287 | Min Color: 0; 0; 0 288 | Min Intensity: 1 289 | Name: Segmentation Pure 290 | Position Transformer: XYZ 291 | Queue Size: 10 292 | Selectable: true 293 | Size (Pixels): 5 294 | Size (m): 0.00999999978 295 | Style: Points 296 | Topic: /segmented_cloud_pure 297 | Unreliable: false 298 | Use Fixed Frame: true 299 | Use rainbow: true 300 | Value: false 301 | - Alpha: 1 302 | Autocompute Intensity Bounds: true 303 | Autocompute Value Bounds: 304 | Max Value: 18.9591999 305 | Min Value: 0.0404983014 306 | Value: true 307 | Axis: Z 308 | Channel Name: intensity 309 | Class: rviz/PointCloud2 310 | Color: 255; 0; 127 311 | Color Transformer: FlatColor 312 | Decay Time: 0 313 | Enabled: false 314 | Invert Rainbow: false 315 | Max Color: 255; 255; 255 316 | Max Intensity: 15 317 | Min Color: 0; 0; 0 318 | Min Intensity: 0 319 | Name: Ground Cloud 320 | Position Transformer: XYZ 321 | Queue Size: 10 322 | Selectable: true 323 | Size (Pixels): 3 324 | Size (m): 0.00999999978 325 | Style: Points 326 | Topic: /ground_cloud 327 | Unreliable: false 328 | Use Fixed Frame: true 329 | Use rainbow: true 330 | Value: false 331 | - Alpha: 1 332 | Autocompute Intensity Bounds: true 333 | Autocompute Value Bounds: 334 | Max Value: 4.57089996 335 | Min Value: 0.300000012 336 | Value: true 337 | Axis: Z 338 | Channel Name: intensity 339 | Class: rviz/PointCloud2 340 | Color: 255; 255; 255 341 | Color Transformer: Intensity 342 | Decay Time: 0 343 | Enabled: true 344 | Invert Rainbow: false 345 | Max Color: 255; 255; 255 346 | Max Intensity: 42 347 | Min Color: 0; 0; 0 348 | Min Intensity: 0 349 | Name: Trajectory 350 | Position Transformer: XYZ 351 | Queue Size: 1 352 | Selectable: true 353 | Size (Pixels): 3 354 | Size (m): 0.300000012 355 | Style: Flat Squares 356 | Topic: /key_pose_origin 357 | Unreliable: false 358 | Use Fixed Frame: true 359 | Use rainbow: true 360 | Value: true 361 | - Alpha: 1 362 | Autocompute Intensity Bounds: true 363 | Autocompute Value Bounds: 364 | Max Value: 10 365 | Min Value: -10 366 | Value: true 367 | Axis: Z 368 | Channel Name: intensity 369 | Class: rviz/PointCloud2 370 | Color: 255; 255; 255 371 | Color Transformer: Intensity 372 | Decay Time: 0 373 | Enabled: false 374 | Invert Rainbow: false 375 | Max Color: 255; 255; 255 376 | Max Intensity: 15 377 | Min Color: 0; 0; 0 378 | Min Intensity: 0 379 | Name: ICP cloud 380 | Position Transformer: XYZ 381 | Queue Size: 10 382 | Selectable: true 383 | Size (Pixels): 5 384 | Size (m): 0.00999999978 385 | Style: Points 386 | Topic: /corrected_cloud 387 | Unreliable: false 388 | Use Fixed Frame: true 389 | Use rainbow: true 390 | Value: false 391 | - Alpha: 1 392 | Autocompute Intensity Bounds: true 393 | Autocompute Value Bounds: 394 | Max Value: 10 395 | Min Value: -10 396 | Value: true 397 | Axis: Z 398 | Channel Name: intensity 399 | Class: rviz/PointCloud2 400 | Color: 255; 255; 255 401 | Color Transformer: FlatColor 402 | Decay Time: 0 403 | Enabled: false 404 | Invert Rainbow: false 405 | Max Color: 255; 255; 255 406 | Max Intensity: 15 407 | Min Color: 0; 0; 0 408 | Min Intensity: 0 409 | Name: History Key Frames 410 | Position Transformer: XYZ 411 | Queue Size: 10 412 | Selectable: true 413 | Size (Pixels): 2 414 | Size (m): 0.00999999978 415 | Style: Points 416 | Topic: /history_cloud 417 | Unreliable: false 418 | Use Fixed Frame: true 419 | Use rainbow: true 420 | Value: false 421 | - Alpha: 1 422 | Autocompute Intensity Bounds: true 423 | Autocompute Value Bounds: 424 | Max Value: 23.0833607 425 | Min Value: -3.98265076 426 | Value: true 427 | Axis: Y 428 | Channel Name: intensity 429 | Class: rviz/PointCloud2 430 | Color: 255; 255; 255 431 | Color Transformer: AxisColor 432 | Decay Time: 0 433 | Enabled: false 434 | Invert Rainbow: false 435 | Max Color: 255; 255; 255 436 | Max Intensity: 15 437 | Min Color: 0; 0; 0 438 | Min Intensity: 0 439 | Name: Map Cloud 440 | Position Transformer: XYZ 441 | Queue Size: 10 442 | Selectable: true 443 | Size (Pixels): 1 444 | Size (m): 0.00999999978 445 | Style: Points 446 | Topic: /laser_cloud_surround 447 | Unreliable: false 448 | Use Fixed Frame: false 449 | Use rainbow: false 450 | Value: false 451 | - Alpha: 0.5 452 | Autocompute Intensity Bounds: true 453 | Autocompute Value Bounds: 454 | Max Value: 52.9331703 455 | Min Value: -7.77158022 456 | Value: true 457 | Axis: Y 458 | Channel Name: intensity 459 | Class: rviz/PointCloud2 460 | Color: 255; 255; 255 461 | Color Transformer: AxisColor 462 | Decay Time: 300 463 | Enabled: true 464 | Invert Rainbow: false 465 | Max Color: 255; 255; 255 466 | Max Intensity: 15 467 | Min Color: 0; 0; 0 468 | Min Intensity: 0 469 | Name: Map Cloud (stack) 470 | Position Transformer: XYZ 471 | Queue Size: 10 472 | Selectable: true 473 | Size (Pixels): 2 474 | Size (m): 0.00999999978 475 | Style: Points 476 | Topic: /registered_cloud 477 | Unreliable: false 478 | Use Fixed Frame: false 479 | Use rainbow: false 480 | Value: true 481 | - Alpha: 1 482 | Autocompute Intensity Bounds: true 483 | Autocompute Value Bounds: 484 | Max Value: 20.2114735 485 | Min Value: -2.70486069 486 | Value: true 487 | Axis: Z 488 | Channel Name: intensity 489 | Class: rviz/PointCloud2 490 | Color: 170; 255; 255 491 | Color Transformer: AxisColor 492 | Decay Time: 0 493 | Enabled: false 494 | Invert Rainbow: false 495 | Max Color: 255; 255; 255 496 | Max Intensity: 15 497 | Min Color: 0; 0; 0 498 | Min Intensity: 0 499 | Name: Surround Cloud 500 | Position Transformer: XYZ 501 | Queue Size: 10 502 | Selectable: true 503 | Size (Pixels): 2 504 | Size (m): 0.00999999978 505 | Style: Points 506 | Topic: /recent_cloud 507 | Unreliable: false 508 | Use Fixed Frame: true 509 | Use rainbow: true 510 | Value: false 511 | Enabled: true 512 | Global Options: 513 | Background Color: 0; 0; 0 514 | Default Light: true 515 | Fixed Frame: map 516 | Frame Rate: 30 517 | Name: root 518 | Tools: 519 | - Class: rviz/Interact 520 | Hide Inactive Objects: true 521 | - Class: rviz/FocusCamera 522 | - Class: rviz/Measure 523 | Value: true 524 | Views: 525 | Current: 526 | Class: rviz/Orbit 527 | Distance: 86.4874191 528 | Enable Stereo Rendering: 529 | Stereo Eye Separation: 0.0599999987 530 | Stereo Focal Distance: 1 531 | Swap Stereo Eyes: false 532 | Value: false 533 | Focal Point: 534 | X: 0 535 | Y: 0 536 | Z: 0 537 | Focal Shape Fixed Size: false 538 | Focal Shape Size: 0.0500000007 539 | Invert Z Axis: false 540 | Name: Current View 541 | Near Clip Distance: 0.00999999978 542 | Pitch: 0.829797029 543 | Target Frame: base_link 544 | Value: Orbit (rviz) 545 | Yaw: 0.395514786 546 | Saved: ~ 547 | Window Geometry: 548 | Displays: 549 | collapsed: false 550 | Height: 1028 551 | Help: 552 | collapsed: false 553 | Hide Left Dock: false 554 | Hide Right Dock: true 555 | QMainWindow State: 000000ff00000000fd000000040000000000000179000003b5fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002e000003b5000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000800480065006c0070000000032c000000ba0000007900ffffff000000010000010f000003befc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003be000000b700fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073f00000039fc0100000003fb0000001600520061006e0067006500200049006d00610067006500000000000000073f0000000000000000fb0000000a0049006d0061006700650000000000000006100000000000000000fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073e0000003efc0100000002fb0000000800540069006d006500000000000000073e0000038300fffffffb0000000800540069006d0065010000000000000450000000000000000000000600000003b500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 556 | Selection: 557 | collapsed: false 558 | Time: 559 | collapsed: false 560 | Tool Properties: 561 | collapsed: false 562 | Views: 563 | collapsed: true 564 | Width: 1920 565 | X: 637 566 | Y: 227 567 | -------------------------------------------------------------------------------- /LeGO-LOAM/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lego_loam 4 | 1.0.0 5 | LeGO-LOAM from IROS 2018 6 | 7 | Tixiao Shan 8 | 9 | BSD 10 | 11 | Tixiao Shan 12 | 13 | catkin 14 | 15 | roscpp 16 | roscpp 17 | rospy 18 | rospy 19 | ros_bridge 20 | ros_bridge 21 | 22 | tf 23 | tf 24 | 25 | pcl_ros 26 | pcl_ros 27 | pcl_conversions 28 | pcl_conversions 29 | 30 | cv_bridge 31 | cv_bridge 32 | 33 | std_msgs 34 | std_msgs 35 | cloud_msgs 36 | cloud_msgs 37 | sensors_msgs 38 | sensors_msgs 39 | geometry_msgs 40 | geometry_msgs 41 | nav_msgs 42 | nav_msgs 43 | 44 | image_transport 45 | image_transport 46 | 47 | gtsam 48 | gtsam 49 | 50 | 51 | -------------------------------------------------------------------------------- /LeGO-LOAM/src/imageProjection.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 2 | // Further contributions copyright (c) 2016, Southwest Research Institute 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // 1. Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived from this 15 | // software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // This is an implementation of the algorithm described in the following papers: 30 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 31 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 32 | // T. Shan and B. Englot. LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain 33 | // IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). October 2018. 34 | 35 | #include "utility.h" 36 | 37 | class ImageProjection{ 38 | private: 39 | 40 | ros::NodeHandle nh; 41 | 42 | ros::Subscriber subLaserCloud; 43 | 44 | ros::Publisher pubFullCloud; 45 | ros::Publisher pubFullInfoCloud; 46 | 47 | ros::Publisher pubGroundCloud; 48 | ros::Publisher pubSegmentedCloud; 49 | ros::Publisher pubSegmentedCloudPure; 50 | ros::Publisher pubSegmentedCloudInfo; 51 | ros::Publisher pubOutlierCloud; 52 | 53 | pcl::PointCloud::Ptr laserCloudIn; 54 | pcl::PointCloud::Ptr laserCloudInRing; 55 | 56 | pcl::PointCloud::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix 57 | pcl::PointCloud::Ptr fullInfoCloud; // same as fullCloud, but with intensity - range 58 | 59 | pcl::PointCloud::Ptr groundCloud; 60 | pcl::PointCloud::Ptr segmentedCloud; 61 | pcl::PointCloud::Ptr segmentedCloudPure; 62 | pcl::PointCloud::Ptr outlierCloud; 63 | 64 | PointType nanPoint; // fill in fullCloud at each iteration 65 | 66 | cv::Mat rangeMat; // range matrix for range image 67 | cv::Mat labelMat; // label matrix for segmentaiton marking 68 | cv::Mat groundMat; // ground matrix for ground cloud marking 69 | int labelCount; 70 | 71 | float startOrientation; 72 | float endOrientation; 73 | 74 | cloud_msgs::cloud_info segMsg; // info of segmented cloud 75 | std_msgs::Header cloudHeader; 76 | 77 | std::vector > neighborIterator; // neighbor iterator for segmentaiton process 78 | 79 | uint16_t *allPushedIndX; // array for tracking points of a segmented object 80 | uint16_t *allPushedIndY; 81 | 82 | uint16_t *queueIndX; // array for breadth-first search process of segmentation, for speed 83 | uint16_t *queueIndY; 84 | 85 | public: 86 | ImageProjection(): 87 | nh("~"){ 88 | 89 | subLaserCloud = nh.subscribe(pointCloudTopic, 1, &ImageProjection::cloudHandler, this); 90 | 91 | pubFullCloud = nh.advertise ("/full_cloud_projected", 1); 92 | pubFullInfoCloud = nh.advertise ("/full_cloud_info", 1); 93 | 94 | pubGroundCloud = nh.advertise ("/ground_cloud", 1); 95 | pubSegmentedCloud = nh.advertise ("/segmented_cloud", 1); 96 | pubSegmentedCloudPure = nh.advertise ("/segmented_cloud_pure", 1); 97 | pubSegmentedCloudInfo = nh.advertise ("/segmented_cloud_info", 1); 98 | pubOutlierCloud = nh.advertise ("/outlier_cloud", 1); 99 | 100 | nanPoint.x = std::numeric_limits::quiet_NaN(); 101 | nanPoint.y = std::numeric_limits::quiet_NaN(); 102 | nanPoint.z = std::numeric_limits::quiet_NaN(); 103 | nanPoint.intensity = -1; 104 | 105 | allocateMemory(); 106 | resetParameters(); 107 | } 108 | 109 | void allocateMemory(){ 110 | 111 | laserCloudIn.reset(new pcl::PointCloud()); 112 | laserCloudInRing.reset(new pcl::PointCloud()); 113 | 114 | fullCloud.reset(new pcl::PointCloud()); 115 | fullInfoCloud.reset(new pcl::PointCloud()); 116 | 117 | groundCloud.reset(new pcl::PointCloud()); 118 | segmentedCloud.reset(new pcl::PointCloud()); 119 | segmentedCloudPure.reset(new pcl::PointCloud()); 120 | outlierCloud.reset(new pcl::PointCloud()); 121 | 122 | fullCloud->points.resize(N_SCAN*Horizon_SCAN); 123 | fullInfoCloud->points.resize(N_SCAN*Horizon_SCAN); 124 | 125 | segMsg.startRingIndex.assign(N_SCAN, 0); 126 | segMsg.endRingIndex.assign(N_SCAN, 0); 127 | 128 | segMsg.segmentedCloudGroundFlag.assign(N_SCAN*Horizon_SCAN, false); 129 | segMsg.segmentedCloudColInd.assign(N_SCAN*Horizon_SCAN, 0); 130 | segMsg.segmentedCloudRange.assign(N_SCAN*Horizon_SCAN, 0); 131 | 132 | std::pair neighbor; 133 | neighbor.first = -1; neighbor.second = 0; neighborIterator.push_back(neighbor); 134 | neighbor.first = 0; neighbor.second = 1; neighborIterator.push_back(neighbor); 135 | neighbor.first = 0; neighbor.second = -1; neighborIterator.push_back(neighbor); 136 | neighbor.first = 1; neighbor.second = 0; neighborIterator.push_back(neighbor); 137 | 138 | allPushedIndX = new uint16_t[N_SCAN*Horizon_SCAN]; 139 | allPushedIndY = new uint16_t[N_SCAN*Horizon_SCAN]; 140 | 141 | queueIndX = new uint16_t[N_SCAN*Horizon_SCAN]; 142 | queueIndY = new uint16_t[N_SCAN*Horizon_SCAN]; 143 | } 144 | 145 | void resetParameters(){ 146 | laserCloudIn->clear(); 147 | groundCloud->clear(); 148 | segmentedCloud->clear(); 149 | segmentedCloudPure->clear(); 150 | outlierCloud->clear(); 151 | 152 | rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX)); 153 | groundMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_8S, cv::Scalar::all(0)); 154 | labelMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32S, cv::Scalar::all(0)); 155 | labelCount = 1; 156 | 157 | std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint); 158 | std::fill(fullInfoCloud->points.begin(), fullInfoCloud->points.end(), nanPoint); 159 | } 160 | 161 | ~ImageProjection(){} 162 | 163 | void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){ 164 | 165 | cloudHeader = laserCloudMsg->header; 166 | // cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line 167 | pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn); 168 | // Remove Nan points 169 | std::vector indices; 170 | pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices); 171 | // have "ring" channel in the cloud 172 | if (useCloudRing == true){ 173 | pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing); 174 | if (laserCloudInRing->is_dense == false) { 175 | ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!"); 176 | ros::shutdown(); 177 | } 178 | } 179 | } 180 | 181 | void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){ 182 | 183 | // 1. Convert ros message to pcl point cloud 184 | copyPointCloud(laserCloudMsg); 185 | // 2. Start and end angle of a scan 186 | findStartEndAngle(); 187 | // 3. Range image projection 188 | projectPointCloud(); 189 | // 4. Mark ground points 190 | groundRemoval(); 191 | // 5. Point cloud segmentation 192 | cloudSegmentation(); 193 | // 6. Publish all clouds 194 | publishCloud(); 195 | // 7. Reset parameters for next iteration 196 | resetParameters(); 197 | } 198 | 199 | void findStartEndAngle(){ 200 | // start and end orientation of this cloud 201 | segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x); 202 | segMsg.endOrientation = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y, 203 | laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI; 204 | if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) { 205 | segMsg.endOrientation -= 2 * M_PI; 206 | } else if (segMsg.endOrientation - segMsg.startOrientation < M_PI) 207 | segMsg.endOrientation += 2 * M_PI; 208 | segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation; 209 | } 210 | 211 | void projectPointCloud(){ 212 | // range image projection 213 | float verticalAngle, horizonAngle, range; 214 | size_t rowIdn, columnIdn, index, cloudSize; 215 | PointType thisPoint; 216 | 217 | cloudSize = laserCloudIn->points.size(); 218 | 219 | for (size_t i = 0; i < cloudSize; ++i){ 220 | 221 | thisPoint.x = laserCloudIn->points[i].x; 222 | thisPoint.y = laserCloudIn->points[i].y; 223 | thisPoint.z = laserCloudIn->points[i].z; 224 | // find the row and column index in the iamge for this point 225 | if (useCloudRing == true){ 226 | rowIdn = laserCloudInRing->points[i].ring; 227 | } 228 | else{ 229 | verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI; 230 | rowIdn = (verticalAngle + ang_bottom) / ang_res_y; 231 | } 232 | if (rowIdn < 0 || rowIdn >= N_SCAN) 233 | continue; 234 | 235 | horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; 236 | 237 | columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2; 238 | if (columnIdn >= Horizon_SCAN) 239 | columnIdn -= Horizon_SCAN; 240 | 241 | if (columnIdn < 0 || columnIdn >= Horizon_SCAN) 242 | continue; 243 | 244 | range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z); 245 | if (range < sensorMinimumRange) 246 | continue; 247 | 248 | rangeMat.at(rowIdn, columnIdn) = range; 249 | 250 | thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0; 251 | 252 | index = columnIdn + rowIdn * Horizon_SCAN; 253 | fullCloud->points[index] = thisPoint; 254 | fullInfoCloud->points[index] = thisPoint; 255 | fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity" 256 | } 257 | } 258 | 259 | 260 | void groundRemoval(){ 261 | size_t lowerInd, upperInd; 262 | float diffX, diffY, diffZ, angle; 263 | // groundMat 264 | // -1, no valid info to check if ground of not 265 | // 0, initial value, after validation, means not ground 266 | // 1, ground 267 | for (size_t j = 0; j < Horizon_SCAN; ++j){ 268 | for (size_t i = 0; i < groundScanInd; ++i){ 269 | 270 | lowerInd = j + ( i )*Horizon_SCAN; 271 | upperInd = j + (i+1)*Horizon_SCAN; 272 | 273 | if (fullCloud->points[lowerInd].intensity == -1 || 274 | fullCloud->points[upperInd].intensity == -1){ 275 | // no info to check, invalid points 276 | groundMat.at(i,j) = -1; 277 | continue; 278 | } 279 | 280 | diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x; 281 | diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y; 282 | diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z; 283 | 284 | angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI; 285 | 286 | if (abs(angle - sensorMountAngle) <= 10){ 287 | groundMat.at(i,j) = 1; 288 | groundMat.at(i+1,j) = 1; 289 | } 290 | } 291 | } 292 | // extract ground cloud (groundMat == 1) 293 | // mark entry that doesn't need to label (ground and invalid point) for segmentation 294 | // note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan 295 | for (size_t i = 0; i < N_SCAN; ++i){ 296 | for (size_t j = 0; j < Horizon_SCAN; ++j){ 297 | if (groundMat.at(i,j) == 1 || rangeMat.at(i,j) == FLT_MAX){ 298 | labelMat.at(i,j) = -1; 299 | } 300 | } 301 | } 302 | if (pubGroundCloud.getNumSubscribers() != 0){ 303 | for (size_t i = 0; i <= groundScanInd; ++i){ 304 | for (size_t j = 0; j < Horizon_SCAN; ++j){ 305 | if (groundMat.at(i,j) == 1) 306 | groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]); 307 | } 308 | } 309 | } 310 | } 311 | 312 | void cloudSegmentation(){ 313 | // segmentation process 314 | for (size_t i = 0; i < N_SCAN; ++i) 315 | for (size_t j = 0; j < Horizon_SCAN; ++j) 316 | if (labelMat.at(i,j) == 0) 317 | labelComponents(i, j); 318 | 319 | int sizeOfSegCloud = 0; 320 | // extract segmented cloud for lidar odometry 321 | for (size_t i = 0; i < N_SCAN; ++i) { 322 | 323 | segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5; 324 | 325 | for (size_t j = 0; j < Horizon_SCAN; ++j) { 326 | if (labelMat.at(i,j) > 0 || groundMat.at(i,j) == 1){ 327 | // outliers that will not be used for optimization (always continue) 328 | if (labelMat.at(i,j) == 999999){ 329 | if (i > groundScanInd && j % 5 == 0){ 330 | outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]); 331 | continue; 332 | }else{ 333 | continue; 334 | } 335 | } 336 | // majority of ground points are skipped 337 | if (groundMat.at(i,j) == 1){ 338 | if (j%5!=0 && j>5 && j(i,j) == 1); 343 | // mark the points' column index for marking occlusion later 344 | segMsg.segmentedCloudColInd[sizeOfSegCloud] = j; 345 | // save range info 346 | segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at(i,j); 347 | // save seg cloud 348 | segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]); 349 | // size of seg cloud 350 | ++sizeOfSegCloud; 351 | } 352 | } 353 | 354 | segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5; 355 | } 356 | 357 | // extract segmented cloud for visualization 358 | if (pubSegmentedCloudPure.getNumSubscribers() != 0){ 359 | for (size_t i = 0; i < N_SCAN; ++i){ 360 | for (size_t j = 0; j < Horizon_SCAN; ++j){ 361 | if (labelMat.at(i,j) > 0 && labelMat.at(i,j) != 999999){ 362 | segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]); 363 | segmentedCloudPure->points.back().intensity = labelMat.at(i,j); 364 | } 365 | } 366 | } 367 | } 368 | } 369 | 370 | void labelComponents(int row, int col){ 371 | // use std::queue std::vector std::deque will slow the program down greatly 372 | float d1, d2, alpha, angle; 373 | int fromIndX, fromIndY, thisIndX, thisIndY; 374 | bool lineCountFlag[N_SCAN] = {false}; 375 | 376 | queueIndX[0] = row; 377 | queueIndY[0] = col; 378 | int queueSize = 1; 379 | int queueStartInd = 0; 380 | int queueEndInd = 1; 381 | 382 | allPushedIndX[0] = row; 383 | allPushedIndY[0] = col; 384 | int allPushedIndSize = 1; 385 | 386 | while(queueSize > 0){ 387 | // Pop point 388 | fromIndX = queueIndX[queueStartInd]; 389 | fromIndY = queueIndY[queueStartInd]; 390 | --queueSize; 391 | ++queueStartInd; 392 | // Mark popped point 393 | labelMat.at(fromIndX, fromIndY) = labelCount; 394 | // Loop through all the neighboring grids of popped grid 395 | for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){ 396 | // new index 397 | thisIndX = fromIndX + (*iter).first; 398 | thisIndY = fromIndY + (*iter).second; 399 | // index should be within the boundary 400 | if (thisIndX < 0 || thisIndX >= N_SCAN) 401 | continue; 402 | // at range image margin (left or right side) 403 | if (thisIndY < 0) 404 | thisIndY = Horizon_SCAN - 1; 405 | if (thisIndY >= Horizon_SCAN) 406 | thisIndY = 0; 407 | // prevent infinite loop (caused by put already examined point back) 408 | if (labelMat.at(thisIndX, thisIndY) != 0) 409 | continue; 410 | 411 | d1 = std::max(rangeMat.at(fromIndX, fromIndY), 412 | rangeMat.at(thisIndX, thisIndY)); 413 | d2 = std::min(rangeMat.at(fromIndX, fromIndY), 414 | rangeMat.at(thisIndX, thisIndY)); 415 | 416 | if ((*iter).first == 0) 417 | alpha = segmentAlphaX; 418 | else 419 | alpha = segmentAlphaY; 420 | 421 | angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha))); 422 | 423 | if (angle > segmentTheta){ 424 | 425 | queueIndX[queueEndInd] = thisIndX; 426 | queueIndY[queueEndInd] = thisIndY; 427 | ++queueSize; 428 | ++queueEndInd; 429 | 430 | labelMat.at(thisIndX, thisIndY) = labelCount; 431 | lineCountFlag[thisIndX] = true; 432 | 433 | allPushedIndX[allPushedIndSize] = thisIndX; 434 | allPushedIndY[allPushedIndSize] = thisIndY; 435 | ++allPushedIndSize; 436 | } 437 | } 438 | } 439 | 440 | // check if this segment is valid 441 | bool feasibleSegment = false; 442 | if (allPushedIndSize >= 30) 443 | feasibleSegment = true; 444 | else if (allPushedIndSize >= segmentValidPointNum){ 445 | int lineCount = 0; 446 | for (size_t i = 0; i < N_SCAN; ++i) 447 | if (lineCountFlag[i] == true) 448 | ++lineCount; 449 | if (lineCount >= segmentValidLineNum) 450 | feasibleSegment = true; 451 | } 452 | // segment is valid, mark these points 453 | if (feasibleSegment == true){ 454 | ++labelCount; 455 | }else{ // segment is invalid, mark these points 456 | for (size_t i = 0; i < allPushedIndSize; ++i){ 457 | labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999; 458 | } 459 | } 460 | } 461 | 462 | 463 | void publishCloud(){ 464 | // 1. Publish Seg Cloud Info 465 | segMsg.header = cloudHeader; 466 | pubSegmentedCloudInfo.publish(segMsg); 467 | // 2. Publish clouds 468 | sensor_msgs::PointCloud2 laserCloudTemp; 469 | 470 | pcl::toROSMsg(*outlierCloud, laserCloudTemp); 471 | laserCloudTemp.header.stamp = cloudHeader.stamp; 472 | laserCloudTemp.header.frame_id = "base_link"; 473 | pubOutlierCloud.publish(laserCloudTemp); 474 | // segmented cloud with ground 475 | pcl::toROSMsg(*segmentedCloud, laserCloudTemp); 476 | laserCloudTemp.header.stamp = cloudHeader.stamp; 477 | laserCloudTemp.header.frame_id = "base_link"; 478 | pubSegmentedCloud.publish(laserCloudTemp); 479 | // projected full cloud 480 | if (pubFullCloud.getNumSubscribers() != 0){ 481 | pcl::toROSMsg(*fullCloud, laserCloudTemp); 482 | laserCloudTemp.header.stamp = cloudHeader.stamp; 483 | laserCloudTemp.header.frame_id = "base_link"; 484 | pubFullCloud.publish(laserCloudTemp); 485 | } 486 | // original dense ground cloud 487 | if (pubGroundCloud.getNumSubscribers() != 0){ 488 | pcl::toROSMsg(*groundCloud, laserCloudTemp); 489 | laserCloudTemp.header.stamp = cloudHeader.stamp; 490 | laserCloudTemp.header.frame_id = "base_link"; 491 | pubGroundCloud.publish(laserCloudTemp); 492 | } 493 | // segmented cloud without ground 494 | if (pubSegmentedCloudPure.getNumSubscribers() != 0){ 495 | pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp); 496 | laserCloudTemp.header.stamp = cloudHeader.stamp; 497 | laserCloudTemp.header.frame_id = "base_link"; 498 | pubSegmentedCloudPure.publish(laserCloudTemp); 499 | } 500 | // projected full cloud info 501 | if (pubFullInfoCloud.getNumSubscribers() != 0){ 502 | pcl::toROSMsg(*fullInfoCloud, laserCloudTemp); 503 | laserCloudTemp.header.stamp = cloudHeader.stamp; 504 | laserCloudTemp.header.frame_id = "base_link"; 505 | pubFullInfoCloud.publish(laserCloudTemp); 506 | } 507 | } 508 | }; 509 | 510 | 511 | 512 | 513 | int main(int argc, char** argv){ 514 | 515 | ros::init(argc, argv, "lego_loam"); 516 | 517 | ImageProjection IP; 518 | 519 | ROS_INFO("\033[1;32m---->\033[0m Image Projection Started."); 520 | 521 | ros::spin(); 522 | return 0; 523 | } 524 | -------------------------------------------------------------------------------- /LeGO-LOAM/src/mapOptmization.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 2 | // Further contributions copyright (c) 2016, Southwest Research Institute 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // 1. Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived from this 15 | // software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // This is an implementation of the algorithm described in the following paper: 30 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 31 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 32 | // T. Shan and B. Englot. LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain 33 | // IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). October 2018. 34 | #include "utility.h" 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | using namespace gtsam; 48 | 49 | class mapOptimization{ 50 | 51 | private: 52 | 53 | NonlinearFactorGraph gtSAMgraph; 54 | Values initialEstimate; 55 | Values optimizedEstimate; 56 | ISAM2 *isam; 57 | Values isamCurrentEstimate; 58 | 59 | noiseModel::Diagonal::shared_ptr priorNoise; 60 | noiseModel::Diagonal::shared_ptr odometryNoise; 61 | noiseModel::Diagonal::shared_ptr constraintNoise; 62 | 63 | ros::NodeHandle nh; 64 | 65 | ros::Publisher pubLaserCloudSurround; 66 | ros::Publisher pubOdomAftMapped; 67 | ros::Publisher pubKeyPoses; 68 | 69 | ros::Publisher pubHistoryKeyFrames; 70 | ros::Publisher pubIcpKeyFrames; 71 | ros::Publisher pubRecentKeyFrames; 72 | ros::Publisher pubRegisteredCloud; 73 | 74 | ros::Subscriber subLaserCloudCornerLast; 75 | ros::Subscriber subLaserCloudSurfLast; 76 | ros::Subscriber subOutlierCloudLast; 77 | ros::Subscriber subLaserOdometry; 78 | ros::Subscriber subImu; 79 | 80 | nav_msgs::Odometry odomAftMapped; 81 | tf::StampedTransform aftMappedTrans; 82 | tf::TransformBroadcaster tfBroadcaster; 83 | 84 | vector::Ptr> cornerCloudKeyFrames; 85 | vector::Ptr> surfCloudKeyFrames; 86 | vector::Ptr> outlierCloudKeyFrames; 87 | 88 | deque::Ptr> recentCornerCloudKeyFrames; 89 | deque::Ptr> recentSurfCloudKeyFrames; 90 | deque::Ptr> recentOutlierCloudKeyFrames; 91 | int latestFrameID; 92 | 93 | vector surroundingExistingKeyPosesID; 94 | deque::Ptr> surroundingCornerCloudKeyFrames; 95 | deque::Ptr> surroundingSurfCloudKeyFrames; 96 | deque::Ptr> surroundingOutlierCloudKeyFrames; 97 | 98 | PointType previousRobotPosPoint; 99 | PointType currentRobotPosPoint; 100 | 101 | pcl::PointCloud::Ptr cloudKeyPoses3D; 102 | pcl::PointCloud::Ptr cloudKeyPoses6D; 103 | 104 | 105 | 106 | pcl::PointCloud::Ptr surroundingKeyPoses; 107 | pcl::PointCloud::Ptr surroundingKeyPosesDS; 108 | 109 | pcl::PointCloud::Ptr laserCloudCornerLast; // corner feature set from odoOptimization 110 | pcl::PointCloud::Ptr laserCloudSurfLast; // surf feature set from odoOptimization 111 | pcl::PointCloud::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization 112 | pcl::PointCloud::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization 113 | 114 | pcl::PointCloud::Ptr laserCloudOutlierLast; // corner feature set from odoOptimization 115 | pcl::PointCloud::Ptr laserCloudOutlierLastDS; // corner feature set from odoOptimization 116 | 117 | pcl::PointCloud::Ptr laserCloudSurfTotalLast; // surf feature set from odoOptimization 118 | pcl::PointCloud::Ptr laserCloudSurfTotalLastDS; // downsampled corner featuer set from odoOptimization 119 | 120 | pcl::PointCloud::Ptr laserCloudOri; 121 | pcl::PointCloud::Ptr coeffSel; 122 | 123 | pcl::PointCloud::Ptr laserCloudCornerFromMap; 124 | pcl::PointCloud::Ptr laserCloudSurfFromMap; 125 | pcl::PointCloud::Ptr laserCloudCornerFromMapDS; 126 | pcl::PointCloud::Ptr laserCloudSurfFromMapDS; 127 | 128 | pcl::KdTreeFLANN::Ptr kdtreeCornerFromMap; 129 | pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap; 130 | 131 | pcl::KdTreeFLANN::Ptr kdtreeSurroundingKeyPoses; 132 | pcl::KdTreeFLANN::Ptr kdtreeHistoryKeyPoses; 133 | 134 | 135 | pcl::PointCloud::Ptr nearHistoryCornerKeyFrameCloud; 136 | pcl::PointCloud::Ptr nearHistoryCornerKeyFrameCloudDS; 137 | pcl::PointCloud::Ptr nearHistorySurfKeyFrameCloud; 138 | pcl::PointCloud::Ptr nearHistorySurfKeyFrameCloudDS; 139 | 140 | pcl::PointCloud::Ptr latestCornerKeyFrameCloud; 141 | pcl::PointCloud::Ptr latestSurfKeyFrameCloud; 142 | pcl::PointCloud::Ptr latestSurfKeyFrameCloudDS; 143 | 144 | pcl::KdTreeFLANN::Ptr kdtreeGlobalMap; 145 | pcl::PointCloud::Ptr globalMapKeyPoses; 146 | pcl::PointCloud::Ptr globalMapKeyPosesDS; 147 | pcl::PointCloud::Ptr globalMapKeyFrames; 148 | pcl::PointCloud::Ptr globalMapKeyFramesDS; 149 | 150 | std::vector pointSearchInd; 151 | std::vector pointSearchSqDis; 152 | 153 | pcl::VoxelGrid downSizeFilterCorner; 154 | pcl::VoxelGrid downSizeFilterSurf; 155 | pcl::VoxelGrid downSizeFilterOutlier; 156 | pcl::VoxelGrid downSizeFilterHistoryKeyFrames; // for histor key frames of loop closure 157 | pcl::VoxelGrid downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization 158 | pcl::VoxelGrid downSizeFilterGlobalMapKeyPoses; // for global map visualization 159 | pcl::VoxelGrid downSizeFilterGlobalMapKeyFrames; // for global map visualization 160 | 161 | double timeLaserCloudCornerLast; 162 | double timeLaserCloudSurfLast; 163 | double timeLaserOdometry; 164 | double timeLaserCloudOutlierLast; 165 | double timeLastGloalMapPublish; 166 | 167 | bool newLaserCloudCornerLast; 168 | bool newLaserCloudSurfLast; 169 | bool newLaserOdometry; 170 | bool newLaserCloudOutlierLast; 171 | 172 | 173 | float transformLast[6]; 174 | float transformSum[6]; 175 | float transformIncre[6]; 176 | float transformTobeMapped[6]; 177 | float transformBefMapped[6]; 178 | float transformAftMapped[6]; 179 | 180 | 181 | int imuPointerFront; 182 | int imuPointerLast; 183 | 184 | double imuTime[imuQueLength]; 185 | float imuRoll[imuQueLength]; 186 | float imuPitch[imuQueLength]; 187 | 188 | std::mutex mtx; 189 | 190 | double timeLastProcessing; 191 | 192 | PointType pointOri, pointSel, pointProj, coeff; 193 | 194 | cv::Mat matA0; 195 | cv::Mat matB0; 196 | cv::Mat matX0; 197 | 198 | cv::Mat matA1; 199 | cv::Mat matD1; 200 | cv::Mat matV1; 201 | 202 | bool isDegenerate; 203 | cv::Mat matP; 204 | 205 | int laserCloudCornerFromMapDSNum; 206 | int laserCloudSurfFromMapDSNum; 207 | int laserCloudCornerLastDSNum; 208 | int laserCloudSurfLastDSNum; 209 | int laserCloudOutlierLastDSNum; 210 | int laserCloudSurfTotalLastDSNum; 211 | 212 | bool potentialLoopFlag; 213 | double timeSaveFirstCurrentScanForLoopClosure; 214 | int closestHistoryFrameID; 215 | int latestFrameIDLoopCloure; 216 | 217 | bool aLoopIsClosed; 218 | 219 | float cRoll, sRoll, cPitch, sPitch, cYaw, sYaw, tX, tY, tZ; 220 | float ctRoll, stRoll, ctPitch, stPitch, ctYaw, stYaw, tInX, tInY, tInZ; 221 | 222 | public: 223 | 224 | 225 | 226 | mapOptimization(): 227 | nh("~") 228 | { 229 | ISAM2Params parameters; 230 | parameters.relinearizeThreshold = 0.01; 231 | parameters.relinearizeSkip = 1; 232 | isam = new ISAM2(parameters); 233 | 234 | pubKeyPoses = nh.advertise("/key_pose_origin", 2); 235 | pubLaserCloudSurround = nh.advertise("/laser_cloud_surround", 2); 236 | pubOdomAftMapped = nh.advertise ("/aft_mapped_to_init", 5); 237 | 238 | subLaserCloudCornerLast = nh.subscribe("/laser_cloud_corner_last", 2, &mapOptimization::laserCloudCornerLastHandler, this); 239 | subLaserCloudSurfLast = nh.subscribe("/laser_cloud_surf_last", 2, &mapOptimization::laserCloudSurfLastHandler, this); 240 | subOutlierCloudLast = nh.subscribe("/outlier_cloud_last", 2, &mapOptimization::laserCloudOutlierLastHandler, this); 241 | subLaserOdometry = nh.subscribe("/laser_odom_to_init", 5, &mapOptimization::laserOdometryHandler, this); 242 | subImu = nh.subscribe (imuTopic, 50, &mapOptimization::imuHandler, this); 243 | 244 | pubHistoryKeyFrames = nh.advertise("/history_cloud", 2); 245 | pubIcpKeyFrames = nh.advertise("/corrected_cloud", 2); 246 | pubRecentKeyFrames = nh.advertise("/recent_cloud", 2); 247 | pubRegisteredCloud = nh.advertise("/registered_cloud", 2); 248 | 249 | downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2); 250 | downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4); 251 | downSizeFilterOutlier.setLeafSize(0.4, 0.4, 0.4); 252 | 253 | downSizeFilterHistoryKeyFrames.setLeafSize(0.4, 0.4, 0.4); // for histor key frames of loop closure 254 | downSizeFilterSurroundingKeyPoses.setLeafSize(1.0, 1.0, 1.0); // for surrounding key poses of scan-to-map optimization 255 | 256 | downSizeFilterGlobalMapKeyPoses.setLeafSize(1.0, 1.0, 1.0); // for global map visualization 257 | downSizeFilterGlobalMapKeyFrames.setLeafSize(0.4, 0.4, 0.4); // for global map visualization 258 | 259 | odomAftMapped.header.frame_id = "/camera_init"; 260 | odomAftMapped.child_frame_id = "/aft_mapped"; 261 | 262 | aftMappedTrans.frame_id_ = "/camera_init"; 263 | aftMappedTrans.child_frame_id_ = "/aft_mapped"; 264 | 265 | allocateMemory(); 266 | } 267 | 268 | void allocateMemory(){ 269 | 270 | cloudKeyPoses3D.reset(new pcl::PointCloud()); 271 | cloudKeyPoses6D.reset(new pcl::PointCloud()); 272 | 273 | kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN()); 274 | kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN()); 275 | 276 | surroundingKeyPoses.reset(new pcl::PointCloud()); 277 | surroundingKeyPosesDS.reset(new pcl::PointCloud()); 278 | 279 | laserCloudCornerLast.reset(new pcl::PointCloud()); // corner feature set from odoOptimization 280 | laserCloudSurfLast.reset(new pcl::PointCloud()); // surf feature set from odoOptimization 281 | laserCloudCornerLastDS.reset(new pcl::PointCloud()); // downsampled corner featuer set from odoOptimization 282 | laserCloudSurfLastDS.reset(new pcl::PointCloud()); // downsampled surf featuer set from odoOptimization 283 | laserCloudOutlierLast.reset(new pcl::PointCloud()); // corner feature set from odoOptimization 284 | laserCloudOutlierLastDS.reset(new pcl::PointCloud()); // downsampled corner feature set from odoOptimization 285 | laserCloudSurfTotalLast.reset(new pcl::PointCloud()); // surf feature set from odoOptimization 286 | laserCloudSurfTotalLastDS.reset(new pcl::PointCloud()); // downsampled surf featuer set from odoOptimization 287 | 288 | laserCloudOri.reset(new pcl::PointCloud()); 289 | coeffSel.reset(new pcl::PointCloud()); 290 | 291 | laserCloudCornerFromMap.reset(new pcl::PointCloud()); 292 | laserCloudSurfFromMap.reset(new pcl::PointCloud()); 293 | laserCloudCornerFromMapDS.reset(new pcl::PointCloud()); 294 | laserCloudSurfFromMapDS.reset(new pcl::PointCloud()); 295 | 296 | kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN()); 297 | kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN()); 298 | 299 | 300 | nearHistoryCornerKeyFrameCloud.reset(new pcl::PointCloud()); 301 | nearHistoryCornerKeyFrameCloudDS.reset(new pcl::PointCloud()); 302 | nearHistorySurfKeyFrameCloud.reset(new pcl::PointCloud()); 303 | nearHistorySurfKeyFrameCloudDS.reset(new pcl::PointCloud()); 304 | 305 | latestCornerKeyFrameCloud.reset(new pcl::PointCloud()); 306 | latestSurfKeyFrameCloud.reset(new pcl::PointCloud()); 307 | latestSurfKeyFrameCloudDS.reset(new pcl::PointCloud()); 308 | 309 | kdtreeGlobalMap.reset(new pcl::KdTreeFLANN()); 310 | globalMapKeyPoses.reset(new pcl::PointCloud()); 311 | globalMapKeyPosesDS.reset(new pcl::PointCloud()); 312 | globalMapKeyFrames.reset(new pcl::PointCloud()); 313 | globalMapKeyFramesDS.reset(new pcl::PointCloud()); 314 | 315 | timeLaserCloudCornerLast = 0; 316 | timeLaserCloudSurfLast = 0; 317 | timeLaserOdometry = 0; 318 | timeLaserCloudOutlierLast = 0; 319 | timeLastGloalMapPublish = 0; 320 | 321 | timeLastProcessing = -1; 322 | 323 | newLaserCloudCornerLast = false; 324 | newLaserCloudSurfLast = false; 325 | 326 | newLaserOdometry = false; 327 | newLaserCloudOutlierLast = false; 328 | 329 | for (int i = 0; i < 6; ++i){ 330 | transformLast[i] = 0; 331 | transformSum[i] = 0; 332 | transformIncre[i] = 0; 333 | transformTobeMapped[i] = 0; 334 | transformBefMapped[i] = 0; 335 | transformAftMapped[i] = 0; 336 | } 337 | 338 | imuPointerFront = 0; 339 | imuPointerLast = -1; 340 | 341 | for (int i = 0; i < imuQueLength; ++i){ 342 | imuTime[i] = 0; 343 | imuRoll[i] = 0; 344 | imuPitch[i] = 0; 345 | } 346 | 347 | gtsam::Vector Vector6(6); 348 | Vector6 << 1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-6; 349 | priorNoise = noiseModel::Diagonal::Variances(Vector6); 350 | odometryNoise = noiseModel::Diagonal::Variances(Vector6); 351 | 352 | matA0 = cv::Mat (5, 3, CV_32F, cv::Scalar::all(0)); 353 | matB0 = cv::Mat (5, 1, CV_32F, cv::Scalar::all(-1)); 354 | matX0 = cv::Mat (3, 1, CV_32F, cv::Scalar::all(0)); 355 | 356 | matA1 = cv::Mat (3, 3, CV_32F, cv::Scalar::all(0)); 357 | matD1 = cv::Mat (1, 3, CV_32F, cv::Scalar::all(0)); 358 | matV1 = cv::Mat (3, 3, CV_32F, cv::Scalar::all(0)); 359 | 360 | isDegenerate = false; 361 | matP = cv::Mat (6, 6, CV_32F, cv::Scalar::all(0)); 362 | 363 | laserCloudCornerFromMapDSNum = 0; 364 | laserCloudSurfFromMapDSNum = 0; 365 | laserCloudCornerLastDSNum = 0; 366 | laserCloudSurfLastDSNum = 0; 367 | laserCloudOutlierLastDSNum = 0; 368 | laserCloudSurfTotalLastDSNum = 0; 369 | 370 | potentialLoopFlag = false; 371 | aLoopIsClosed = false; 372 | 373 | latestFrameID = 0; 374 | } 375 | 376 | void transformAssociateToMap() 377 | { 378 | float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 379 | - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 380 | float y1 = transformBefMapped[4] - transformSum[4]; 381 | float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 382 | + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 383 | 384 | float x2 = x1; 385 | float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1; 386 | float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1; 387 | 388 | transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2; 389 | transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2; 390 | transformIncre[5] = z2; 391 | 392 | float sbcx = sin(transformSum[0]); 393 | float cbcx = cos(transformSum[0]); 394 | float sbcy = sin(transformSum[1]); 395 | float cbcy = cos(transformSum[1]); 396 | float sbcz = sin(transformSum[2]); 397 | float cbcz = cos(transformSum[2]); 398 | 399 | float sblx = sin(transformBefMapped[0]); 400 | float cblx = cos(transformBefMapped[0]); 401 | float sbly = sin(transformBefMapped[1]); 402 | float cbly = cos(transformBefMapped[1]); 403 | float sblz = sin(transformBefMapped[2]); 404 | float cblz = cos(transformBefMapped[2]); 405 | 406 | float salx = sin(transformAftMapped[0]); 407 | float calx = cos(transformAftMapped[0]); 408 | float saly = sin(transformAftMapped[1]); 409 | float caly = cos(transformAftMapped[1]); 410 | float salz = sin(transformAftMapped[2]); 411 | float calz = cos(transformAftMapped[2]); 412 | 413 | float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz) 414 | - cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly) 415 | - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) 416 | - cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) 417 | - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx); 418 | transformTobeMapped[0] = -asin(srx); 419 | 420 | float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly) 421 | - cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx) 422 | - cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz) 423 | + (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly) 424 | + cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) 425 | + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly); 426 | float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz) 427 | - cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx) 428 | + cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 429 | + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly) 430 | - cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly) 431 | + (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly); 432 | transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]), 433 | crycrx / cos(transformTobeMapped[0])); 434 | 435 | float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) 436 | - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) 437 | - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) 438 | - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) 439 | + cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); 440 | float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) 441 | - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) 442 | - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) 443 | - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) 444 | + cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); 445 | transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]), 446 | crzcrx / cos(transformTobeMapped[0])); 447 | 448 | x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4]; 449 | y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2]) * transformIncre[4]; 450 | z1 = transformIncre[5]; 451 | 452 | x2 = x1; 453 | y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1; 454 | z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1; 455 | 456 | transformTobeMapped[3] = transformAftMapped[3] 457 | - (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2); 458 | transformTobeMapped[4] = transformAftMapped[4] - y2; 459 | transformTobeMapped[5] = transformAftMapped[5] 460 | - (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2); 461 | } 462 | 463 | void transformUpdate() 464 | { 465 | if (imuPointerLast >= 0) { 466 | float imuRollLast = 0, imuPitchLast = 0; 467 | while (imuPointerFront != imuPointerLast) { 468 | if (timeLaserOdometry + scanPeriod < imuTime[imuPointerFront]) { 469 | break; 470 | } 471 | imuPointerFront = (imuPointerFront + 1) % imuQueLength; 472 | } 473 | 474 | if (timeLaserOdometry + scanPeriod > imuTime[imuPointerFront]) { 475 | imuRollLast = imuRoll[imuPointerFront]; 476 | imuPitchLast = imuPitch[imuPointerFront]; 477 | } else { 478 | int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength; 479 | float ratioFront = (timeLaserOdometry + scanPeriod - imuTime[imuPointerBack]) 480 | / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); 481 | float ratioBack = (imuTime[imuPointerFront] - timeLaserOdometry - scanPeriod) 482 | / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); 483 | 484 | imuRollLast = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack; 485 | imuPitchLast = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack; 486 | } 487 | 488 | transformTobeMapped[0] = 0.998 * transformTobeMapped[0] + 0.002 * imuPitchLast; 489 | transformTobeMapped[2] = 0.998 * transformTobeMapped[2] + 0.002 * imuRollLast; 490 | } 491 | 492 | for (int i = 0; i < 6; i++) { 493 | transformBefMapped[i] = transformSum[i]; 494 | transformAftMapped[i] = transformTobeMapped[i]; 495 | } 496 | } 497 | 498 | void updatePointAssociateToMapSinCos(){ 499 | cRoll = cos(transformTobeMapped[0]); 500 | sRoll = sin(transformTobeMapped[0]); 501 | 502 | cPitch = cos(transformTobeMapped[1]); 503 | sPitch = sin(transformTobeMapped[1]); 504 | 505 | cYaw = cos(transformTobeMapped[2]); 506 | sYaw = sin(transformTobeMapped[2]); 507 | 508 | tX = transformTobeMapped[3]; 509 | tY = transformTobeMapped[4]; 510 | tZ = transformTobeMapped[5]; 511 | } 512 | 513 | void pointAssociateToMap(PointType const * const pi, PointType * const po) 514 | { 515 | float x1 = cYaw * pi->x - sYaw * pi->y; 516 | float y1 = sYaw * pi->x + cYaw * pi->y; 517 | float z1 = pi->z; 518 | 519 | float x2 = x1; 520 | float y2 = cRoll * y1 - sRoll * z1; 521 | float z2 = sRoll * y1 + cRoll * z1; 522 | 523 | po->x = cPitch * x2 + sPitch * z2 + tX; 524 | po->y = y2 + tY; 525 | po->z = -sPitch * x2 + cPitch * z2 + tZ; 526 | po->intensity = pi->intensity; 527 | } 528 | 529 | void updateTransformPointCloudSinCos(PointTypePose *tIn){ 530 | 531 | ctRoll = cos(tIn->roll); 532 | stRoll = sin(tIn->roll); 533 | 534 | ctPitch = cos(tIn->pitch); 535 | stPitch = sin(tIn->pitch); 536 | 537 | ctYaw = cos(tIn->yaw); 538 | stYaw = sin(tIn->yaw); 539 | 540 | tInX = tIn->x; 541 | tInY = tIn->y; 542 | tInZ = tIn->z; 543 | } 544 | 545 | pcl::PointCloud::Ptr transformPointCloud(pcl::PointCloud::Ptr cloudIn){ 546 | // !!! DO NOT use pcl for point cloud transformation, results are not accurate 547 | // Reason: unkown 548 | pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); 549 | 550 | PointType *pointFrom; 551 | PointType pointTo; 552 | 553 | int cloudSize = cloudIn->points.size(); 554 | cloudOut->resize(cloudSize); 555 | 556 | for (int i = 0; i < cloudSize; ++i){ 557 | 558 | pointFrom = &cloudIn->points[i]; 559 | float x1 = ctYaw * pointFrom->x - stYaw * pointFrom->y; 560 | float y1 = stYaw * pointFrom->x + ctYaw* pointFrom->y; 561 | float z1 = pointFrom->z; 562 | 563 | float x2 = x1; 564 | float y2 = ctRoll * y1 - stRoll * z1; 565 | float z2 = stRoll * y1 + ctRoll* z1; 566 | 567 | pointTo.x = ctPitch * x2 + stPitch * z2 + tInX; 568 | pointTo.y = y2 + tInY; 569 | pointTo.z = -stPitch * x2 + ctPitch * z2 + tInZ; 570 | pointTo.intensity = pointFrom->intensity; 571 | 572 | cloudOut->points[i] = pointTo; 573 | } 574 | return cloudOut; 575 | } 576 | 577 | pcl::PointCloud::Ptr transformPointCloud(pcl::PointCloud::Ptr cloudIn, PointTypePose* transformIn){ 578 | 579 | pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); 580 | 581 | PointType *pointFrom; 582 | PointType pointTo; 583 | 584 | int cloudSize = cloudIn->points.size(); 585 | cloudOut->resize(cloudSize); 586 | 587 | for (int i = 0; i < cloudSize; ++i){ 588 | 589 | pointFrom = &cloudIn->points[i]; 590 | float x1 = cos(transformIn->yaw) * pointFrom->x - sin(transformIn->yaw) * pointFrom->y; 591 | float y1 = sin(transformIn->yaw) * pointFrom->x + cos(transformIn->yaw)* pointFrom->y; 592 | float z1 = pointFrom->z; 593 | 594 | float x2 = x1; 595 | float y2 = cos(transformIn->roll) * y1 - sin(transformIn->roll) * z1; 596 | float z2 = sin(transformIn->roll) * y1 + cos(transformIn->roll)* z1; 597 | 598 | pointTo.x = cos(transformIn->pitch) * x2 + sin(transformIn->pitch) * z2 + transformIn->x; 599 | pointTo.y = y2 + transformIn->y; 600 | pointTo.z = -sin(transformIn->pitch) * x2 + cos(transformIn->pitch) * z2 + transformIn->z; 601 | pointTo.intensity = pointFrom->intensity; 602 | 603 | cloudOut->points[i] = pointTo; 604 | } 605 | return cloudOut; 606 | } 607 | 608 | void laserCloudOutlierLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){ 609 | timeLaserCloudOutlierLast = msg->header.stamp.toSec(); 610 | laserCloudOutlierLast->clear(); 611 | pcl::fromROSMsg(*msg, *laserCloudOutlierLast); 612 | newLaserCloudOutlierLast = true; 613 | } 614 | 615 | void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){ 616 | timeLaserCloudCornerLast = msg->header.stamp.toSec(); 617 | laserCloudCornerLast->clear(); 618 | pcl::fromROSMsg(*msg, *laserCloudCornerLast); 619 | newLaserCloudCornerLast = true; 620 | } 621 | 622 | void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){ 623 | timeLaserCloudSurfLast = msg->header.stamp.toSec(); 624 | laserCloudSurfLast->clear(); 625 | pcl::fromROSMsg(*msg, *laserCloudSurfLast); 626 | newLaserCloudSurfLast = true; 627 | } 628 | 629 | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry){ 630 | timeLaserOdometry = laserOdometry->header.stamp.toSec(); 631 | double roll, pitch, yaw; 632 | geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; 633 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 634 | transformSum[0] = -pitch; 635 | transformSum[1] = -yaw; 636 | transformSum[2] = roll; 637 | transformSum[3] = laserOdometry->pose.pose.position.x; 638 | transformSum[4] = laserOdometry->pose.pose.position.y; 639 | transformSum[5] = laserOdometry->pose.pose.position.z; 640 | newLaserOdometry = true; 641 | } 642 | 643 | void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn){ 644 | double roll, pitch, yaw; 645 | tf::Quaternion orientation; 646 | tf::quaternionMsgToTF(imuIn->orientation, orientation); 647 | tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); 648 | imuPointerLast = (imuPointerLast + 1) % imuQueLength; 649 | imuTime[imuPointerLast] = imuIn->header.stamp.toSec(); 650 | imuRoll[imuPointerLast] = roll; 651 | imuPitch[imuPointerLast] = pitch; 652 | } 653 | 654 | void publishTF(){ 655 | 656 | geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw 657 | (transformAftMapped[2], -transformAftMapped[0], -transformAftMapped[1]); 658 | 659 | odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry); 660 | odomAftMapped.pose.pose.orientation.x = -geoQuat.y; 661 | odomAftMapped.pose.pose.orientation.y = -geoQuat.z; 662 | odomAftMapped.pose.pose.orientation.z = geoQuat.x; 663 | odomAftMapped.pose.pose.orientation.w = geoQuat.w; 664 | odomAftMapped.pose.pose.position.x = transformAftMapped[3]; 665 | odomAftMapped.pose.pose.position.y = transformAftMapped[4]; 666 | odomAftMapped.pose.pose.position.z = transformAftMapped[5]; 667 | odomAftMapped.twist.twist.angular.x = transformBefMapped[0]; 668 | odomAftMapped.twist.twist.angular.y = transformBefMapped[1]; 669 | odomAftMapped.twist.twist.angular.z = transformBefMapped[2]; 670 | odomAftMapped.twist.twist.linear.x = transformBefMapped[3]; 671 | odomAftMapped.twist.twist.linear.y = transformBefMapped[4]; 672 | odomAftMapped.twist.twist.linear.z = transformBefMapped[5]; 673 | pubOdomAftMapped.publish(odomAftMapped); 674 | 675 | aftMappedTrans.stamp_ = ros::Time().fromSec(timeLaserOdometry); 676 | aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 677 | aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped[3], transformAftMapped[4], transformAftMapped[5])); 678 | tfBroadcaster.sendTransform(aftMappedTrans); 679 | } 680 | 681 | PointTypePose trans2PointTypePose(float transformIn[]){ 682 | PointTypePose thisPose6D; 683 | thisPose6D.x = transformIn[3]; 684 | thisPose6D.y = transformIn[4]; 685 | thisPose6D.z = transformIn[5]; 686 | thisPose6D.roll = transformIn[0]; 687 | thisPose6D.pitch = transformIn[1]; 688 | thisPose6D.yaw = transformIn[2]; 689 | return thisPose6D; 690 | } 691 | 692 | void publishKeyPosesAndFrames(){ 693 | 694 | if (pubKeyPoses.getNumSubscribers() != 0){ 695 | sensor_msgs::PointCloud2 cloudMsgTemp; 696 | pcl::toROSMsg(*cloudKeyPoses3D, cloudMsgTemp); 697 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 698 | cloudMsgTemp.header.frame_id = "/camera_init"; 699 | pubKeyPoses.publish(cloudMsgTemp); 700 | } 701 | 702 | if (pubRecentKeyFrames.getNumSubscribers() != 0){ 703 | sensor_msgs::PointCloud2 cloudMsgTemp; 704 | pcl::toROSMsg(*laserCloudSurfFromMapDS, cloudMsgTemp); 705 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 706 | cloudMsgTemp.header.frame_id = "/camera_init"; 707 | pubRecentKeyFrames.publish(cloudMsgTemp); 708 | } 709 | 710 | if (pubRegisteredCloud.getNumSubscribers() != 0){ 711 | pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); 712 | PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped); 713 | *cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D); 714 | *cloudOut += *transformPointCloud(laserCloudSurfTotalLast, &thisPose6D); 715 | 716 | sensor_msgs::PointCloud2 cloudMsgTemp; 717 | pcl::toROSMsg(*cloudOut, cloudMsgTemp); 718 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 719 | cloudMsgTemp.header.frame_id = "/camera_init"; 720 | pubRegisteredCloud.publish(cloudMsgTemp); 721 | } 722 | } 723 | 724 | void visualizeGlobalMapThread(){ 725 | ros::Rate rate(0.2); 726 | while (ros::ok()){ 727 | rate.sleep(); 728 | publishGlobalMap(); 729 | } 730 | // save final point cloud 731 | pcl::io::savePCDFileASCII(fileDirectory+"finalCloud.pcd", *globalMapKeyFramesDS); 732 | 733 | string cornerMapString = "/tmp/cornerMap.pcd"; 734 | string surfaceMapString = "/tmp/surfaceMap.pcd"; 735 | string trajectoryString = "/tmp/trajectory.pcd"; 736 | 737 | pcl::PointCloud::Ptr cornerMapCloud(new pcl::PointCloud()); 738 | pcl::PointCloud::Ptr cornerMapCloudDS(new pcl::PointCloud()); 739 | pcl::PointCloud::Ptr surfaceMapCloud(new pcl::PointCloud()); 740 | pcl::PointCloud::Ptr surfaceMapCloudDS(new pcl::PointCloud()); 741 | 742 | for(int i = 0; i < cornerCloudKeyFrames.size(); i++) { 743 | *cornerMapCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); 744 | *surfaceMapCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); 745 | *surfaceMapCloud += *transformPointCloud(outlierCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); 746 | } 747 | 748 | downSizeFilterCorner.setInputCloud(cornerMapCloud); 749 | downSizeFilterCorner.filter(*cornerMapCloudDS); 750 | downSizeFilterSurf.setInputCloud(surfaceMapCloud); 751 | downSizeFilterSurf.filter(*surfaceMapCloudDS); 752 | 753 | pcl::io::savePCDFileASCII(fileDirectory+"cornerMap.pcd", *cornerMapCloudDS); 754 | pcl::io::savePCDFileASCII(fileDirectory+"surfaceMap.pcd", *surfaceMapCloudDS); 755 | pcl::io::savePCDFileASCII(fileDirectory+"trajectory.pcd", *cloudKeyPoses3D); 756 | } 757 | 758 | void publishGlobalMap(){ 759 | 760 | if (pubLaserCloudSurround.getNumSubscribers() == 0) 761 | return; 762 | 763 | if (cloudKeyPoses3D->points.empty() == true) 764 | return; 765 | // kd-tree to find near key frames to visualize 766 | std::vector pointSearchIndGlobalMap; 767 | std::vector pointSearchSqDisGlobalMap; 768 | // search near key frames to visualize 769 | mtx.lock(); 770 | kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D); 771 | kdtreeGlobalMap->radiusSearch(currentRobotPosPoint, globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0); 772 | mtx.unlock(); 773 | 774 | for (int i = 0; i < pointSearchIndGlobalMap.size(); ++i) 775 | globalMapKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]); 776 | // downsample near selected key frames 777 | downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses); 778 | downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS); 779 | // extract visualized and downsampled key frames 780 | for (int i = 0; i < globalMapKeyPosesDS->points.size(); ++i){ 781 | int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity; 782 | *globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]); 783 | *globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]); 784 | *globalMapKeyFrames += *transformPointCloud(outlierCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]); 785 | } 786 | // downsample visualized points 787 | downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames); 788 | downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS); 789 | 790 | sensor_msgs::PointCloud2 cloudMsgTemp; 791 | pcl::toROSMsg(*globalMapKeyFramesDS, cloudMsgTemp); 792 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 793 | cloudMsgTemp.header.frame_id = "/camera_init"; 794 | pubLaserCloudSurround.publish(cloudMsgTemp); 795 | 796 | globalMapKeyPoses->clear(); 797 | globalMapKeyPosesDS->clear(); 798 | globalMapKeyFrames->clear(); 799 | // globalMapKeyFramesDS->clear(); 800 | } 801 | 802 | void loopClosureThread(){ 803 | 804 | if (loopClosureEnableFlag == false) 805 | return; 806 | 807 | ros::Rate rate(1); 808 | while (ros::ok()){ 809 | rate.sleep(); 810 | performLoopClosure(); 811 | } 812 | } 813 | 814 | bool detectLoopClosure(){ 815 | 816 | latestSurfKeyFrameCloud->clear(); 817 | nearHistorySurfKeyFrameCloud->clear(); 818 | nearHistorySurfKeyFrameCloudDS->clear(); 819 | 820 | std::lock_guard lock(mtx); 821 | // find the closest history key frame 822 | std::vector pointSearchIndLoop; 823 | std::vector pointSearchSqDisLoop; 824 | kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D); 825 | kdtreeHistoryKeyPoses->radiusSearch(currentRobotPosPoint, historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0); 826 | 827 | closestHistoryFrameID = -1; 828 | for (int i = 0; i < pointSearchIndLoop.size(); ++i){ 829 | int id = pointSearchIndLoop[i]; 830 | if (abs(cloudKeyPoses6D->points[id].time - timeLaserOdometry) > 30.0){ 831 | closestHistoryFrameID = id; 832 | break; 833 | } 834 | } 835 | if (closestHistoryFrameID == -1){ 836 | return false; 837 | } 838 | // save latest key frames 839 | latestFrameIDLoopCloure = cloudKeyPoses3D->points.size() - 1; 840 | *latestSurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDLoopCloure], &cloudKeyPoses6D->points[latestFrameIDLoopCloure]); 841 | *latestSurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[latestFrameIDLoopCloure], &cloudKeyPoses6D->points[latestFrameIDLoopCloure]); 842 | 843 | pcl::PointCloud::Ptr hahaCloud(new pcl::PointCloud()); 844 | int cloudSize = latestSurfKeyFrameCloud->points.size(); 845 | for (int i = 0; i < cloudSize; ++i){ 846 | if ((int)latestSurfKeyFrameCloud->points[i].intensity >= 0){ 847 | hahaCloud->push_back(latestSurfKeyFrameCloud->points[i]); 848 | } 849 | } 850 | latestSurfKeyFrameCloud->clear(); 851 | *latestSurfKeyFrameCloud = *hahaCloud; 852 | // save history near key frames 853 | for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j){ 854 | if (closestHistoryFrameID + j < 0 || closestHistoryFrameID + j > latestFrameIDLoopCloure) 855 | continue; 856 | *nearHistorySurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[closestHistoryFrameID+j], &cloudKeyPoses6D->points[closestHistoryFrameID+j]); 857 | *nearHistorySurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[closestHistoryFrameID+j], &cloudKeyPoses6D->points[closestHistoryFrameID+j]); 858 | } 859 | 860 | downSizeFilterHistoryKeyFrames.setInputCloud(nearHistorySurfKeyFrameCloud); 861 | downSizeFilterHistoryKeyFrames.filter(*nearHistorySurfKeyFrameCloudDS); 862 | // publish history near key frames 863 | if (pubHistoryKeyFrames.getNumSubscribers() != 0){ 864 | sensor_msgs::PointCloud2 cloudMsgTemp; 865 | pcl::toROSMsg(*nearHistorySurfKeyFrameCloudDS, cloudMsgTemp); 866 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 867 | cloudMsgTemp.header.frame_id = "/camera_init"; 868 | pubHistoryKeyFrames.publish(cloudMsgTemp); 869 | } 870 | 871 | return true; 872 | } 873 | 874 | 875 | void performLoopClosure(){ 876 | 877 | if (cloudKeyPoses3D->points.empty() == true) 878 | return; 879 | // try to find close key frame if there are any 880 | if (potentialLoopFlag == false){ 881 | 882 | if (detectLoopClosure() == true){ 883 | potentialLoopFlag = true; // find some key frames that is old enough or close enough for loop closure 884 | timeSaveFirstCurrentScanForLoopClosure = timeLaserOdometry; 885 | } 886 | if (potentialLoopFlag == false) 887 | return; 888 | } 889 | // reset the flag first no matter icp successes or not 890 | potentialLoopFlag = false; 891 | // ICP Settings 892 | pcl::IterativeClosestPoint icp; 893 | icp.setMaxCorrespondenceDistance(100); 894 | icp.setMaximumIterations(100); 895 | icp.setTransformationEpsilon(1e-6); 896 | icp.setEuclideanFitnessEpsilon(1e-6); 897 | icp.setRANSACIterations(0); 898 | // Align clouds 899 | icp.setInputSource(latestSurfKeyFrameCloud); 900 | icp.setInputTarget(nearHistorySurfKeyFrameCloudDS); 901 | pcl::PointCloud::Ptr unused_result(new pcl::PointCloud()); 902 | icp.align(*unused_result); 903 | 904 | if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore) 905 | return; 906 | // publish corrected cloud 907 | if (pubIcpKeyFrames.getNumSubscribers() != 0){ 908 | pcl::PointCloud::Ptr closed_cloud(new pcl::PointCloud()); 909 | pcl::transformPointCloud (*latestSurfKeyFrameCloud, *closed_cloud, icp.getFinalTransformation()); 910 | sensor_msgs::PointCloud2 cloudMsgTemp; 911 | pcl::toROSMsg(*closed_cloud, cloudMsgTemp); 912 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 913 | cloudMsgTemp.header.frame_id = "/camera_init"; 914 | pubIcpKeyFrames.publish(cloudMsgTemp); 915 | } 916 | /* 917 | get pose constraint 918 | */ 919 | float x, y, z, roll, pitch, yaw; 920 | Eigen::Affine3f correctionCameraFrame; 921 | correctionCameraFrame = icp.getFinalTransformation(); // get transformation in camera frame (because points are in camera frame) 922 | pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw); 923 | Eigen::Affine3f correctionLidarFrame = pcl::getTransformation(z, x, y, yaw, roll, pitch); 924 | // transform from world origin to wrong pose 925 | Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar(cloudKeyPoses6D->points[latestFrameIDLoopCloure]); 926 | // transform from world origin to corrected pose 927 | Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong; // pre-multiplying -> successive rotation about a fixed frame 928 | pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw); 929 | gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); 930 | gtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[closestHistoryFrameID]); 931 | gtsam::Vector Vector6(6); 932 | float noiseScore = icp.getFitnessScore(); 933 | Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore; 934 | constraintNoise = noiseModel::Diagonal::Variances(Vector6); 935 | /* 936 | add constraints 937 | */ 938 | std::lock_guard lock(mtx); 939 | gtSAMgraph.add(BetweenFactor(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise)); 940 | isam->update(gtSAMgraph); 941 | isam->update(); 942 | gtSAMgraph.resize(0); 943 | 944 | aLoopIsClosed = true; 945 | } 946 | 947 | Pose3 pclPointTogtsamPose3(PointTypePose thisPoint){ // camera frame to lidar frame 948 | return Pose3(Rot3::RzRyRx(double(thisPoint.yaw), double(thisPoint.roll), double(thisPoint.pitch)), 949 | Point3(double(thisPoint.z), double(thisPoint.x), double(thisPoint.y))); 950 | } 951 | 952 | Eigen::Affine3f pclPointToAffine3fCameraToLidar(PointTypePose thisPoint){ // camera frame to lidar frame 953 | return pcl::getTransformation(thisPoint.z, thisPoint.x, thisPoint.y, thisPoint.yaw, thisPoint.roll, thisPoint.pitch); 954 | } 955 | 956 | void extractSurroundingKeyFrames(){ 957 | 958 | if (cloudKeyPoses3D->points.empty() == true) 959 | return; 960 | 961 | if (loopClosureEnableFlag == true){ 962 | // only use recent key poses for graph building 963 | if (recentCornerCloudKeyFrames.size() < surroundingKeyframeSearchNum){ // queue is not full (the beginning of mapping or a loop is just closed) 964 | // clear recent key frames queue 965 | recentCornerCloudKeyFrames. clear(); 966 | recentSurfCloudKeyFrames. clear(); 967 | recentOutlierCloudKeyFrames.clear(); 968 | int numPoses = cloudKeyPoses3D->points.size(); 969 | for (int i = numPoses-1; i >= 0; --i){ 970 | int thisKeyInd = (int)cloudKeyPoses3D->points[i].intensity; 971 | PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd]; 972 | updateTransformPointCloudSinCos(&thisTransformation); 973 | // extract surrounding map 974 | recentCornerCloudKeyFrames. push_front(transformPointCloud(cornerCloudKeyFrames[thisKeyInd])); 975 | recentSurfCloudKeyFrames. push_front(transformPointCloud(surfCloudKeyFrames[thisKeyInd])); 976 | recentOutlierCloudKeyFrames.push_front(transformPointCloud(outlierCloudKeyFrames[thisKeyInd])); 977 | if (recentCornerCloudKeyFrames.size() >= surroundingKeyframeSearchNum) 978 | break; 979 | } 980 | }else{ // queue is full, pop the oldest key frame and push the latest key frame 981 | if (latestFrameID != cloudKeyPoses3D->points.size() - 1){ // if the robot is not moving, no need to update recent frames 982 | 983 | recentCornerCloudKeyFrames. pop_front(); 984 | recentSurfCloudKeyFrames. pop_front(); 985 | recentOutlierCloudKeyFrames.pop_front(); 986 | // push latest scan to the end of queue 987 | latestFrameID = cloudKeyPoses3D->points.size() - 1; 988 | PointTypePose thisTransformation = cloudKeyPoses6D->points[latestFrameID]; 989 | updateTransformPointCloudSinCos(&thisTransformation); 990 | recentCornerCloudKeyFrames. push_back(transformPointCloud(cornerCloudKeyFrames[latestFrameID])); 991 | recentSurfCloudKeyFrames. push_back(transformPointCloud(surfCloudKeyFrames[latestFrameID])); 992 | recentOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[latestFrameID])); 993 | } 994 | } 995 | 996 | for (int i = 0; i < recentCornerCloudKeyFrames.size(); ++i){ 997 | *laserCloudCornerFromMap += *recentCornerCloudKeyFrames[i]; 998 | *laserCloudSurfFromMap += *recentSurfCloudKeyFrames[i]; 999 | *laserCloudSurfFromMap += *recentOutlierCloudKeyFrames[i]; 1000 | } 1001 | }else{ 1002 | surroundingKeyPoses->clear(); 1003 | surroundingKeyPosesDS->clear(); 1004 | // extract all the nearby key poses and downsample them 1005 | kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); 1006 | kdtreeSurroundingKeyPoses->radiusSearch(currentRobotPosPoint, (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis, 0); 1007 | for (int i = 0; i < pointSearchInd.size(); ++i) 1008 | surroundingKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchInd[i]]); 1009 | downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses); 1010 | downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS); 1011 | // delete key frames that are not in surrounding region 1012 | int numSurroundingPosesDS = surroundingKeyPosesDS->points.size(); 1013 | for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i){ 1014 | bool existingFlag = false; 1015 | for (int j = 0; j < numSurroundingPosesDS; ++j){ 1016 | if (surroundingExistingKeyPosesID[i] == (int)surroundingKeyPosesDS->points[j].intensity){ 1017 | existingFlag = true; 1018 | break; 1019 | } 1020 | } 1021 | if (existingFlag == false){ 1022 | surroundingExistingKeyPosesID. erase(surroundingExistingKeyPosesID. begin() + i); 1023 | surroundingCornerCloudKeyFrames. erase(surroundingCornerCloudKeyFrames. begin() + i); 1024 | surroundingSurfCloudKeyFrames. erase(surroundingSurfCloudKeyFrames. begin() + i); 1025 | surroundingOutlierCloudKeyFrames.erase(surroundingOutlierCloudKeyFrames.begin() + i); 1026 | --i; 1027 | } 1028 | } 1029 | // add new key frames that are not in calculated existing key frames 1030 | for (int i = 0; i < numSurroundingPosesDS; ++i) { 1031 | bool existingFlag = false; 1032 | for (auto iter = surroundingExistingKeyPosesID.begin(); iter != surroundingExistingKeyPosesID.end(); ++iter){ 1033 | if ((*iter) == (int)surroundingKeyPosesDS->points[i].intensity){ 1034 | existingFlag = true; 1035 | break; 1036 | } 1037 | } 1038 | if (existingFlag == true){ 1039 | continue; 1040 | }else{ 1041 | int thisKeyInd = (int)surroundingKeyPosesDS->points[i].intensity; 1042 | PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd]; 1043 | updateTransformPointCloudSinCos(&thisTransformation); 1044 | surroundingExistingKeyPosesID. push_back(thisKeyInd); 1045 | surroundingCornerCloudKeyFrames. push_back(transformPointCloud(cornerCloudKeyFrames[thisKeyInd])); 1046 | surroundingSurfCloudKeyFrames. push_back(transformPointCloud(surfCloudKeyFrames[thisKeyInd])); 1047 | surroundingOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[thisKeyInd])); 1048 | } 1049 | } 1050 | 1051 | for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i) { 1052 | *laserCloudCornerFromMap += *surroundingCornerCloudKeyFrames[i]; 1053 | *laserCloudSurfFromMap += *surroundingSurfCloudKeyFrames[i]; 1054 | *laserCloudSurfFromMap += *surroundingOutlierCloudKeyFrames[i]; 1055 | } 1056 | } 1057 | // Downsample the surrounding corner key frames (or map) 1058 | downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap); 1059 | downSizeFilterCorner.filter(*laserCloudCornerFromMapDS); 1060 | laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->points.size(); 1061 | // Downsample the surrounding surf key frames (or map) 1062 | downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap); 1063 | downSizeFilterSurf.filter(*laserCloudSurfFromMapDS); 1064 | laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->points.size(); 1065 | } 1066 | 1067 | void downsampleCurrentScan(){ 1068 | 1069 | laserCloudCornerLastDS->clear(); 1070 | downSizeFilterCorner.setInputCloud(laserCloudCornerLast); 1071 | downSizeFilterCorner.filter(*laserCloudCornerLastDS); 1072 | laserCloudCornerLastDSNum = laserCloudCornerLastDS->points.size(); 1073 | 1074 | laserCloudSurfLastDS->clear(); 1075 | downSizeFilterSurf.setInputCloud(laserCloudSurfLast); 1076 | downSizeFilterSurf.filter(*laserCloudSurfLastDS); 1077 | laserCloudSurfLastDSNum = laserCloudSurfLastDS->points.size(); 1078 | 1079 | laserCloudOutlierLastDS->clear(); 1080 | downSizeFilterOutlier.setInputCloud(laserCloudOutlierLast); 1081 | downSizeFilterOutlier.filter(*laserCloudOutlierLastDS); 1082 | laserCloudOutlierLastDSNum = laserCloudOutlierLastDS->points.size(); 1083 | 1084 | laserCloudSurfTotalLast->clear(); 1085 | laserCloudSurfTotalLastDS->clear(); 1086 | *laserCloudSurfTotalLast += *laserCloudSurfLastDS; 1087 | *laserCloudSurfTotalLast += *laserCloudOutlierLastDS; 1088 | downSizeFilterSurf.setInputCloud(laserCloudSurfTotalLast); 1089 | downSizeFilterSurf.filter(*laserCloudSurfTotalLastDS); 1090 | laserCloudSurfTotalLastDSNum = laserCloudSurfTotalLastDS->points.size(); 1091 | } 1092 | 1093 | void cornerOptimization(int iterCount){ 1094 | 1095 | updatePointAssociateToMapSinCos(); 1096 | for (int i = 0; i < laserCloudCornerLastDSNum; i++) { 1097 | pointOri = laserCloudCornerLastDS->points[i]; 1098 | pointAssociateToMap(&pointOri, &pointSel); 1099 | kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); 1100 | 1101 | if (pointSearchSqDis[4] < 1.0) { 1102 | float cx = 0, cy = 0, cz = 0; 1103 | for (int j = 0; j < 5; j++) { 1104 | cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x; 1105 | cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y; 1106 | cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z; 1107 | } 1108 | cx /= 5; cy /= 5; cz /= 5; 1109 | 1110 | float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0; 1111 | for (int j = 0; j < 5; j++) { 1112 | float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx; 1113 | float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy; 1114 | float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz; 1115 | 1116 | a11 += ax * ax; a12 += ax * ay; a13 += ax * az; 1117 | a22 += ay * ay; a23 += ay * az; 1118 | a33 += az * az; 1119 | } 1120 | a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5; 1121 | 1122 | matA1.at(0, 0) = a11; matA1.at(0, 1) = a12; matA1.at(0, 2) = a13; 1123 | matA1.at(1, 0) = a12; matA1.at(1, 1) = a22; matA1.at(1, 2) = a23; 1124 | matA1.at(2, 0) = a13; matA1.at(2, 1) = a23; matA1.at(2, 2) = a33; 1125 | 1126 | cv::eigen(matA1, matD1, matV1); 1127 | 1128 | if (matD1.at(0, 0) > 3 * matD1.at(0, 1)) { 1129 | 1130 | float x0 = pointSel.x; 1131 | float y0 = pointSel.y; 1132 | float z0 = pointSel.z; 1133 | float x1 = cx + 0.1 * matV1.at(0, 0); 1134 | float y1 = cy + 0.1 * matV1.at(0, 1); 1135 | float z1 = cz + 0.1 * matV1.at(0, 2); 1136 | float x2 = cx - 0.1 * matV1.at(0, 0); 1137 | float y2 = cy - 0.1 * matV1.at(0, 1); 1138 | float z2 = cz - 0.1 * matV1.at(0, 2); 1139 | 1140 | float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 1141 | * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 1142 | + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 1143 | * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 1144 | + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) 1145 | * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); 1146 | 1147 | float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); 1148 | 1149 | float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 1150 | + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12; 1151 | 1152 | float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 1153 | - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; 1154 | 1155 | float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 1156 | + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; 1157 | 1158 | float ld2 = a012 / l12; 1159 | 1160 | float s = 1 - 0.9 * fabs(ld2); 1161 | 1162 | coeff.x = s * la; 1163 | coeff.y = s * lb; 1164 | coeff.z = s * lc; 1165 | coeff.intensity = s * ld2; 1166 | 1167 | if (s > 0.1) { 1168 | laserCloudOri->push_back(pointOri); 1169 | coeffSel->push_back(coeff); 1170 | } 1171 | } 1172 | } 1173 | } 1174 | } 1175 | 1176 | void surfOptimization(int iterCount){ 1177 | updatePointAssociateToMapSinCos(); 1178 | for (int i = 0; i < laserCloudSurfTotalLastDSNum; i++) { 1179 | pointOri = laserCloudSurfTotalLastDS->points[i]; 1180 | pointAssociateToMap(&pointOri, &pointSel); 1181 | kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); 1182 | 1183 | if (pointSearchSqDis[4] < 1.0) { 1184 | for (int j = 0; j < 5; j++) { 1185 | matA0.at(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x; 1186 | matA0.at(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y; 1187 | matA0.at(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z; 1188 | } 1189 | cv::solve(matA0, matB0, matX0, cv::DECOMP_QR); 1190 | 1191 | float pa = matX0.at(0, 0); 1192 | float pb = matX0.at(1, 0); 1193 | float pc = matX0.at(2, 0); 1194 | float pd = 1; 1195 | 1196 | float ps = sqrt(pa * pa + pb * pb + pc * pc); 1197 | pa /= ps; pb /= ps; pc /= ps; pd /= ps; 1198 | 1199 | bool planeValid = true; 1200 | for (int j = 0; j < 5; j++) { 1201 | if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x + 1202 | pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y + 1203 | pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) { 1204 | planeValid = false; 1205 | break; 1206 | } 1207 | } 1208 | 1209 | if (planeValid) { 1210 | float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; 1211 | 1212 | float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x 1213 | + pointSel.y * pointSel.y + pointSel.z * pointSel.z)); 1214 | 1215 | coeff.x = s * pa; 1216 | coeff.y = s * pb; 1217 | coeff.z = s * pc; 1218 | coeff.intensity = s * pd2; 1219 | 1220 | if (s > 0.1) { 1221 | laserCloudOri->push_back(pointOri); 1222 | coeffSel->push_back(coeff); 1223 | } 1224 | } 1225 | } 1226 | } 1227 | } 1228 | 1229 | bool LMOptimization(int iterCount){ 1230 | float srx = sin(transformTobeMapped[0]); 1231 | float crx = cos(transformTobeMapped[0]); 1232 | float sry = sin(transformTobeMapped[1]); 1233 | float cry = cos(transformTobeMapped[1]); 1234 | float srz = sin(transformTobeMapped[2]); 1235 | float crz = cos(transformTobeMapped[2]); 1236 | 1237 | int laserCloudSelNum = laserCloudOri->points.size(); 1238 | if (laserCloudSelNum < 50) { 1239 | return false; 1240 | } 1241 | 1242 | cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0)); 1243 | cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0)); 1244 | cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0)); 1245 | cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0)); 1246 | cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0)); 1247 | cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0)); 1248 | for (int i = 0; i < laserCloudSelNum; i++) { 1249 | pointOri = laserCloudOri->points[i]; 1250 | coeff = coeffSel->points[i]; 1251 | 1252 | float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x 1253 | + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y 1254 | + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z; 1255 | 1256 | float ary = ((cry*srx*srz - crz*sry)*pointOri.x 1257 | + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x 1258 | + ((-cry*crz - srx*sry*srz)*pointOri.x 1259 | + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z; 1260 | 1261 | float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x 1262 | + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y 1263 | + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z; 1264 | 1265 | matA.at(i, 0) = arx; 1266 | matA.at(i, 1) = ary; 1267 | matA.at(i, 2) = arz; 1268 | matA.at(i, 3) = coeff.x; 1269 | matA.at(i, 4) = coeff.y; 1270 | matA.at(i, 5) = coeff.z; 1271 | matB.at(i, 0) = -coeff.intensity; 1272 | } 1273 | cv::transpose(matA, matAt); 1274 | matAtA = matAt * matA; 1275 | matAtB = matAt * matB; 1276 | cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR); 1277 | 1278 | if (iterCount == 0) { 1279 | cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0)); 1280 | cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0)); 1281 | cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0)); 1282 | 1283 | cv::eigen(matAtA, matE, matV); 1284 | matV.copyTo(matV2); 1285 | 1286 | isDegenerate = false; 1287 | float eignThre[6] = {100, 100, 100, 100, 100, 100}; 1288 | for (int i = 5; i >= 0; i--) { 1289 | if (matE.at(0, i) < eignThre[i]) { 1290 | for (int j = 0; j < 6; j++) { 1291 | matV2.at(i, j) = 0; 1292 | } 1293 | isDegenerate = true; 1294 | } else { 1295 | break; 1296 | } 1297 | } 1298 | matP = matV.inv() * matV2; 1299 | } 1300 | 1301 | if (isDegenerate) { 1302 | cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0)); 1303 | matX.copyTo(matX2); 1304 | matX = matP * matX2; 1305 | } 1306 | 1307 | transformTobeMapped[0] += matX.at(0, 0); 1308 | transformTobeMapped[1] += matX.at(1, 0); 1309 | transformTobeMapped[2] += matX.at(2, 0); 1310 | transformTobeMapped[3] += matX.at(3, 0); 1311 | transformTobeMapped[4] += matX.at(4, 0); 1312 | transformTobeMapped[5] += matX.at(5, 0); 1313 | 1314 | float deltaR = sqrt( 1315 | pow(pcl::rad2deg(matX.at(0, 0)), 2) + 1316 | pow(pcl::rad2deg(matX.at(1, 0)), 2) + 1317 | pow(pcl::rad2deg(matX.at(2, 0)), 2)); 1318 | float deltaT = sqrt( 1319 | pow(matX.at(3, 0) * 100, 2) + 1320 | pow(matX.at(4, 0) * 100, 2) + 1321 | pow(matX.at(5, 0) * 100, 2)); 1322 | 1323 | if (deltaR < 0.05 && deltaT < 0.05) { 1324 | return true; 1325 | } 1326 | return false; 1327 | } 1328 | 1329 | void scan2MapOptimization(){ 1330 | 1331 | if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) { 1332 | 1333 | kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS); 1334 | kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS); 1335 | 1336 | for (int iterCount = 0; iterCount < 10; iterCount++) { 1337 | 1338 | laserCloudOri->clear(); 1339 | coeffSel->clear(); 1340 | 1341 | cornerOptimization(iterCount); 1342 | surfOptimization(iterCount); 1343 | 1344 | if (LMOptimization(iterCount) == true) 1345 | break; 1346 | } 1347 | 1348 | transformUpdate(); 1349 | } 1350 | } 1351 | 1352 | 1353 | void saveKeyFramesAndFactor(){ 1354 | 1355 | currentRobotPosPoint.x = transformAftMapped[3]; 1356 | currentRobotPosPoint.y = transformAftMapped[4]; 1357 | currentRobotPosPoint.z = transformAftMapped[5]; 1358 | 1359 | bool saveThisKeyFrame = true; 1360 | if (sqrt((previousRobotPosPoint.x-currentRobotPosPoint.x)*(previousRobotPosPoint.x-currentRobotPosPoint.x) 1361 | +(previousRobotPosPoint.y-currentRobotPosPoint.y)*(previousRobotPosPoint.y-currentRobotPosPoint.y) 1362 | +(previousRobotPosPoint.z-currentRobotPosPoint.z)*(previousRobotPosPoint.z-currentRobotPosPoint.z)) < 0.3){ 1363 | saveThisKeyFrame = false; 1364 | } 1365 | 1366 | 1367 | 1368 | if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty()) 1369 | return; 1370 | 1371 | previousRobotPosPoint = currentRobotPosPoint; 1372 | /** 1373 | * update grsam graph 1374 | */ 1375 | if (cloudKeyPoses3D->points.empty()){ 1376 | gtSAMgraph.add(PriorFactor(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]), 1377 | Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise)); 1378 | initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]), 1379 | Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4]))); 1380 | for (int i = 0; i < 6; ++i) 1381 | transformLast[i] = transformTobeMapped[i]; 1382 | } 1383 | else{ 1384 | gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]), 1385 | Point3(transformLast[5], transformLast[3], transformLast[4])); 1386 | gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]), 1387 | Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4])); 1388 | gtSAMgraph.add(BetweenFactor(cloudKeyPoses3D->points.size()-1, cloudKeyPoses3D->points.size(), poseFrom.between(poseTo), odometryNoise)); 1389 | initialEstimate.insert(cloudKeyPoses3D->points.size(), Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]), 1390 | Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]))); 1391 | } 1392 | /** 1393 | * update iSAM 1394 | */ 1395 | isam->update(gtSAMgraph, initialEstimate); 1396 | isam->update(); 1397 | 1398 | gtSAMgraph.resize(0); 1399 | initialEstimate.clear(); 1400 | 1401 | /** 1402 | * save key poses 1403 | */ 1404 | PointType thisPose3D; 1405 | PointTypePose thisPose6D; 1406 | Pose3 latestEstimate; 1407 | 1408 | isamCurrentEstimate = isam->calculateEstimate(); 1409 | latestEstimate = isamCurrentEstimate.at(isamCurrentEstimate.size()-1); 1410 | 1411 | thisPose3D.x = latestEstimate.translation().y(); 1412 | thisPose3D.y = latestEstimate.translation().z(); 1413 | thisPose3D.z = latestEstimate.translation().x(); 1414 | thisPose3D.intensity = cloudKeyPoses3D->points.size(); // this can be used as index 1415 | cloudKeyPoses3D->push_back(thisPose3D); 1416 | 1417 | thisPose6D.x = thisPose3D.x; 1418 | thisPose6D.y = thisPose3D.y; 1419 | thisPose6D.z = thisPose3D.z; 1420 | thisPose6D.intensity = thisPose3D.intensity; // this can be used as index 1421 | thisPose6D.roll = latestEstimate.rotation().pitch(); 1422 | thisPose6D.pitch = latestEstimate.rotation().yaw(); 1423 | thisPose6D.yaw = latestEstimate.rotation().roll(); // in camera frame 1424 | thisPose6D.time = timeLaserOdometry; 1425 | cloudKeyPoses6D->push_back(thisPose6D); 1426 | /** 1427 | * save updated transform 1428 | */ 1429 | if (cloudKeyPoses3D->points.size() > 1){ 1430 | transformAftMapped[0] = latestEstimate.rotation().pitch(); 1431 | transformAftMapped[1] = latestEstimate.rotation().yaw(); 1432 | transformAftMapped[2] = latestEstimate.rotation().roll(); 1433 | transformAftMapped[3] = latestEstimate.translation().y(); 1434 | transformAftMapped[4] = latestEstimate.translation().z(); 1435 | transformAftMapped[5] = latestEstimate.translation().x(); 1436 | 1437 | for (int i = 0; i < 6; ++i){ 1438 | transformLast[i] = transformAftMapped[i]; 1439 | transformTobeMapped[i] = transformAftMapped[i]; 1440 | } 1441 | } 1442 | 1443 | pcl::PointCloud::Ptr thisCornerKeyFrame(new pcl::PointCloud()); 1444 | pcl::PointCloud::Ptr thisSurfKeyFrame(new pcl::PointCloud()); 1445 | pcl::PointCloud::Ptr thisOutlierKeyFrame(new pcl::PointCloud()); 1446 | 1447 | pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame); 1448 | pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame); 1449 | pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame); 1450 | 1451 | cornerCloudKeyFrames.push_back(thisCornerKeyFrame); 1452 | surfCloudKeyFrames.push_back(thisSurfKeyFrame); 1453 | outlierCloudKeyFrames.push_back(thisOutlierKeyFrame); 1454 | } 1455 | 1456 | void correctPoses(){ 1457 | if (aLoopIsClosed == true){ 1458 | recentCornerCloudKeyFrames. clear(); 1459 | recentSurfCloudKeyFrames. clear(); 1460 | recentOutlierCloudKeyFrames.clear(); 1461 | // update key poses 1462 | int numPoses = isamCurrentEstimate.size(); 1463 | for (int i = 0; i < numPoses; ++i){ 1464 | cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at(i).translation().y(); 1465 | cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at(i).translation().z(); 1466 | cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at(i).translation().x(); 1467 | 1468 | cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x; 1469 | cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y; 1470 | cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z; 1471 | cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at(i).rotation().pitch(); 1472 | cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at(i).rotation().yaw(); 1473 | cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at(i).rotation().roll(); 1474 | } 1475 | 1476 | aLoopIsClosed = false; 1477 | } 1478 | } 1479 | 1480 | void clearCloud(){ 1481 | laserCloudCornerFromMap->clear(); 1482 | laserCloudSurfFromMap->clear(); 1483 | laserCloudCornerFromMapDS->clear(); 1484 | laserCloudSurfFromMapDS->clear(); 1485 | } 1486 | 1487 | void run(){ 1488 | 1489 | if (newLaserCloudCornerLast && std::abs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 && 1490 | newLaserCloudSurfLast && std::abs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 && 1491 | newLaserCloudOutlierLast && std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 && 1492 | newLaserOdometry) 1493 | { 1494 | 1495 | newLaserCloudCornerLast = false; newLaserCloudSurfLast = false; newLaserCloudOutlierLast = false; newLaserOdometry = false; 1496 | 1497 | std::lock_guard lock(mtx); 1498 | 1499 | if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) { 1500 | 1501 | timeLastProcessing = timeLaserOdometry; 1502 | 1503 | transformAssociateToMap(); 1504 | 1505 | extractSurroundingKeyFrames(); 1506 | 1507 | downsampleCurrentScan(); 1508 | 1509 | scan2MapOptimization(); 1510 | 1511 | saveKeyFramesAndFactor(); 1512 | 1513 | correctPoses(); 1514 | 1515 | publishTF(); 1516 | 1517 | publishKeyPosesAndFrames(); 1518 | 1519 | clearCloud(); 1520 | } 1521 | } 1522 | } 1523 | }; 1524 | 1525 | 1526 | int main(int argc, char** argv) 1527 | { 1528 | ros::init(argc, argv, "lego_loam"); 1529 | 1530 | ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started."); 1531 | 1532 | mapOptimization MO; 1533 | 1534 | std::thread loopthread(&mapOptimization::loopClosureThread, &MO); 1535 | std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO); 1536 | 1537 | ros::Rate rate(200); 1538 | while (ros::ok()) 1539 | { 1540 | ros::spinOnce(); 1541 | 1542 | MO.run(); 1543 | 1544 | rate.sleep(); 1545 | } 1546 | 1547 | loopthread.join(); 1548 | visualizeMapThread.join(); 1549 | 1550 | return 0; 1551 | } 1552 | -------------------------------------------------------------------------------- /LeGO-LOAM/src/transformFusion.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 2 | // Further contributions copyright (c) 2016, Southwest Research Institute 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // 1. Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived from this 15 | // software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // This is an implementation of the algorithm described in the following paper: 30 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 31 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 32 | 33 | #include "utility.h" 34 | 35 | class TransformFusion{ 36 | 37 | private: 38 | 39 | ros::NodeHandle nh; 40 | 41 | ros::Publisher pubLaserOdometry2; 42 | ros::Subscriber subLaserOdometry; 43 | ros::Subscriber subOdomAftMapped; 44 | 45 | 46 | nav_msgs::Odometry laserOdometry2; 47 | tf::StampedTransform laserOdometryTrans2; 48 | tf::TransformBroadcaster tfBroadcaster2; 49 | 50 | tf::StampedTransform map_2_camera_init_Trans; 51 | tf::TransformBroadcaster tfBroadcasterMap2CameraInit; 52 | 53 | tf::StampedTransform camera_2_base_link_Trans; 54 | tf::TransformBroadcaster tfBroadcasterCamera2Baselink; 55 | 56 | float transformSum[6]; 57 | float transformIncre[6]; 58 | float transformMapped[6]; 59 | float transformBefMapped[6]; 60 | float transformAftMapped[6]; 61 | 62 | std_msgs::Header currentHeader; 63 | 64 | public: 65 | 66 | TransformFusion(){ 67 | 68 | pubLaserOdometry2 = nh.advertise ("/integrated_to_init", 5); 69 | subLaserOdometry = nh.subscribe("/laser_odom_to_init", 5, &TransformFusion::laserOdometryHandler, this); 70 | subOdomAftMapped = nh.subscribe("/aft_mapped_to_init", 5, &TransformFusion::odomAftMappedHandler, this); 71 | 72 | laserOdometry2.header.frame_id = "/camera_init"; 73 | laserOdometry2.child_frame_id = "/camera"; 74 | 75 | laserOdometryTrans2.frame_id_ = "/camera_init"; 76 | laserOdometryTrans2.child_frame_id_ = "/camera"; 77 | 78 | map_2_camera_init_Trans.frame_id_ = "/map"; 79 | map_2_camera_init_Trans.child_frame_id_ = "/camera_init"; 80 | 81 | camera_2_base_link_Trans.frame_id_ = "/camera"; 82 | camera_2_base_link_Trans.child_frame_id_ = "/base_link"; 83 | 84 | for (int i = 0; i < 6; ++i) 85 | { 86 | transformSum[i] = 0; 87 | transformIncre[i] = 0; 88 | transformMapped[i] = 0; 89 | transformBefMapped[i] = 0; 90 | transformAftMapped[i] = 0; 91 | } 92 | } 93 | 94 | void transformAssociateToMap() 95 | { 96 | float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 97 | - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 98 | float y1 = transformBefMapped[4] - transformSum[4]; 99 | float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 100 | + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 101 | 102 | float x2 = x1; 103 | float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1; 104 | float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1; 105 | 106 | transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2; 107 | transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2; 108 | transformIncre[5] = z2; 109 | 110 | float sbcx = sin(transformSum[0]); 111 | float cbcx = cos(transformSum[0]); 112 | float sbcy = sin(transformSum[1]); 113 | float cbcy = cos(transformSum[1]); 114 | float sbcz = sin(transformSum[2]); 115 | float cbcz = cos(transformSum[2]); 116 | 117 | float sblx = sin(transformBefMapped[0]); 118 | float cblx = cos(transformBefMapped[0]); 119 | float sbly = sin(transformBefMapped[1]); 120 | float cbly = cos(transformBefMapped[1]); 121 | float sblz = sin(transformBefMapped[2]); 122 | float cblz = cos(transformBefMapped[2]); 123 | 124 | float salx = sin(transformAftMapped[0]); 125 | float calx = cos(transformAftMapped[0]); 126 | float saly = sin(transformAftMapped[1]); 127 | float caly = cos(transformAftMapped[1]); 128 | float salz = sin(transformAftMapped[2]); 129 | float calz = cos(transformAftMapped[2]); 130 | 131 | float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz) 132 | - cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly) 133 | - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) 134 | - cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) 135 | - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx); 136 | transformMapped[0] = -asin(srx); 137 | 138 | float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly) 139 | - cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx) 140 | - cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz) 141 | + (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly) 142 | + cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) 143 | + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly); 144 | float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz) 145 | - cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx) 146 | + cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 147 | + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly) 148 | - cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly) 149 | + (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly); 150 | transformMapped[1] = atan2(srycrx / cos(transformMapped[0]), 151 | crycrx / cos(transformMapped[0])); 152 | 153 | float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) 154 | - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) 155 | - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) 156 | - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) 157 | + cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); 158 | float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly) 159 | - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly) 160 | - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz) 161 | - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx) 162 | + cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz); 163 | transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]), 164 | crzcrx / cos(transformMapped[0])); 165 | 166 | x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4]; 167 | y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4]; 168 | z1 = transformIncre[5]; 169 | 170 | x2 = x1; 171 | y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1; 172 | z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1; 173 | 174 | transformMapped[3] = transformAftMapped[3] 175 | - (cos(transformMapped[1]) * x2 + sin(transformMapped[1]) * z2); 176 | transformMapped[4] = transformAftMapped[4] - y2; 177 | transformMapped[5] = transformAftMapped[5] 178 | - (-sin(transformMapped[1]) * x2 + cos(transformMapped[1]) * z2); 179 | } 180 | 181 | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) 182 | { 183 | currentHeader = laserOdometry->header; 184 | 185 | double roll, pitch, yaw; 186 | geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; 187 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 188 | 189 | transformSum[0] = -pitch; 190 | transformSum[1] = -yaw; 191 | transformSum[2] = roll; 192 | 193 | transformSum[3] = laserOdometry->pose.pose.position.x; 194 | transformSum[4] = laserOdometry->pose.pose.position.y; 195 | transformSum[5] = laserOdometry->pose.pose.position.z; 196 | 197 | transformAssociateToMap(); 198 | 199 | geoQuat = tf::createQuaternionMsgFromRollPitchYaw 200 | (transformMapped[2], -transformMapped[0], -transformMapped[1]); 201 | 202 | laserOdometry2.header.stamp = laserOdometry->header.stamp; 203 | laserOdometry2.pose.pose.orientation.x = -geoQuat.y; 204 | laserOdometry2.pose.pose.orientation.y = -geoQuat.z; 205 | laserOdometry2.pose.pose.orientation.z = geoQuat.x; 206 | laserOdometry2.pose.pose.orientation.w = geoQuat.w; 207 | laserOdometry2.pose.pose.position.x = transformMapped[3]; 208 | laserOdometry2.pose.pose.position.y = transformMapped[4]; 209 | laserOdometry2.pose.pose.position.z = transformMapped[5]; 210 | pubLaserOdometry2.publish(laserOdometry2); 211 | 212 | laserOdometryTrans2.stamp_ = laserOdometry->header.stamp; 213 | laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 214 | laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5])); 215 | tfBroadcaster2.sendTransform(laserOdometryTrans2); 216 | } 217 | 218 | void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped) 219 | { 220 | double roll, pitch, yaw; 221 | geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation; 222 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 223 | 224 | transformAftMapped[0] = -pitch; 225 | transformAftMapped[1] = -yaw; 226 | transformAftMapped[2] = roll; 227 | 228 | transformAftMapped[3] = odomAftMapped->pose.pose.position.x; 229 | transformAftMapped[4] = odomAftMapped->pose.pose.position.y; 230 | transformAftMapped[5] = odomAftMapped->pose.pose.position.z; 231 | 232 | transformBefMapped[0] = odomAftMapped->twist.twist.angular.x; 233 | transformBefMapped[1] = odomAftMapped->twist.twist.angular.y; 234 | transformBefMapped[2] = odomAftMapped->twist.twist.angular.z; 235 | 236 | transformBefMapped[3] = odomAftMapped->twist.twist.linear.x; 237 | transformBefMapped[4] = odomAftMapped->twist.twist.linear.y; 238 | transformBefMapped[5] = odomAftMapped->twist.twist.linear.z; 239 | } 240 | }; 241 | 242 | 243 | int main(int argc, char** argv) 244 | { 245 | ros::init(argc, argv, "lego_loam"); 246 | 247 | TransformFusion TFusion; 248 | 249 | ROS_INFO("\033[1;32m---->\033[0m Transform Fusion Started."); 250 | 251 | ros::spin(); 252 | 253 | return 0; 254 | } 255 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LeGO-LOAM 2 | 3 | This repository contains code for a lightweight and ground optimized lidar odometry and mapping (LeGO-LOAM) system for ROS compatible UGVs. The system takes in point cloud from a Velodyne VLP-16 Lidar (palced horizontally) and optional IMU data as inputs. It outputs 6D pose estimation in real-time. A demonstration of the system can be found here -> https://www.youtube.com/watch?v=O3tz_ftHV48 4 | 7 |

8 | drawing 9 |

10 | 11 | ## Lidar-inertial Odometry 12 | 13 | An updated lidar-initial odometry package, [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM), has been open-sourced and available for testing. 14 | 15 | ## Dependency 16 | 17 | - [ROS](http://wiki.ros.org/ROS/Installation) (tested with indigo, kinetic, and melodic) 18 | - [gtsam](https://github.com/borglab/gtsam/releases) (Georgia Tech Smoothing and Mapping library, 4.0.0-alpha2) 19 | ``` 20 | wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.0-alpha2.zip 21 | cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/ 22 | cd ~/Downloads/gtsam-4.0.0-alpha2/ 23 | mkdir build && cd build 24 | cmake .. 25 | sudo make install 26 | ``` 27 | 28 | ## Compile 29 | 30 | You can use the following commands to download and compile the package. 31 | 32 | ``` 33 | cd ~/catkin_ws/src 34 | git clone https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.git 35 | cd .. 36 | catkin_make -j1 37 | ``` 38 | When you compile the code for the first time, you need to add "-j1" behind "catkin_make" for generating some message types. "-j1" is not needed for future compiling. 39 | 40 | ## The system 41 | 42 | LeGO-LOAM is speficifally optimized for a horizontally placed VLP-16 on a ground vehicle. It assumes there is always a ground plane in the scan. The UGV we are using is Clearpath Jackal. It has a built-in IMU. 43 | 44 |

45 | drawing 46 |

47 | 48 | The package performs segmentation before feature extraction. 49 | 50 |

51 | drawing 52 |

53 | 54 | Lidar odometry performs two-step Levenberg Marquardt optimization to get 6D transformation. 55 | 56 |

57 | drawing 58 |

59 | 60 | ## New Lidar 61 | 62 | The key thing to adapt the code to a new sensor is making sure the point cloud can be properly projected to an range image and ground can be correctly detected. For example, VLP-16 has a angular resolution of 0.2° and 2° along two directions. It has 16 beams. The angle of the bottom beam is -15°. Thus, the parameters in "utility.h" are listed as below. When you implement new sensor, make sure that the ground_cloud has enough points for matching. Before you post any issues, please read this. 63 | 64 | ``` 65 | extern const int N_SCAN = 16; 66 | extern const int Horizon_SCAN = 1800; 67 | extern const float ang_res_x = 0.2; 68 | extern const float ang_res_y = 2.0; 69 | extern const float ang_bottom = 15.0; 70 | extern const int groundScanInd = 7; 71 | ``` 72 | 73 | Another example for Velodyne HDL-32e range image projection: 74 | 75 | ``` 76 | extern const int N_SCAN = 32; 77 | extern const int Horizon_SCAN = 1800; 78 | extern const float ang_res_x = 360.0/Horizon_SCAN; 79 | extern const float ang_res_y = 41.333/float(N_Scan-1); 80 | extern const float ang_bottom = 30.666666; 81 | extern const int groundScanInd = 20; 82 | ``` 83 | 84 | **New**: a new **useCloudRing** flag has been added to help with point cloud projection (i.e., VLP-32C, VLS-128). Velodyne point cloud has "ring" channel that directly gives the point row id in a range image. Other lidars may have a same type of channel, i.e., "r" in Ouster. If you are using a non-Velodyne lidar but it has a similar "ring" channel, you can change the PointXYZIR definition in utility.h and the corresponding code in imageProjection.cpp. 85 | 86 | For **KITTI** users, if you want to use our algorithm with **HDL-64e**, you need to write your own implementation for such projection. If the point cloud is not projected properly, you will lose many points and performance. 87 | 88 | If you are using your lidar with an IMU, make sure your IMU is aligned properly with the lidar. The algorithm uses IMU data to correct the point cloud distortion that is cause by sensor motion. If the IMU is not aligned properly, the usage of IMU data will deteriorate the result. Ouster lidar IMU is not supported in the package as LeGO-LOAM needs a 9-DOF IMU. 89 | 90 | ## Run the package 91 | 92 | 1. Run the launch file: 93 | ``` 94 | roslaunch lego_loam run.launch 95 | ``` 96 | Notes: The parameter "/use_sim_time" is set to "true" for simulation, "false" to real robot usage. 97 | 98 | 2. Play existing bag files: 99 | ``` 100 | rosbag play *.bag --clock --topic /velodyne_points /imu/data 101 | ``` 102 | Notes: Though /imu/data is optinal, it can improve estimation accuracy greatly if provided. Some sample bags can be downloaded from [here](https://github.com/RobustFieldAutonomyLab/jackal_dataset_20170608). 103 | 104 | ## New data-set 105 | 106 | This dataset, [Stevens data-set](https://github.com/TixiaoShan/Stevens-VLP16-Dataset), is captured using a Velodyne VLP-16, which is mounted on an UGV - Clearpath Jackal, on Stevens Institute of Technology campus. The VLP-16 rotation rate is set to 10Hz. This data-set features over 20K scans and many loop-closures. 107 | 108 |

109 | drawing 110 |

111 |

112 | drawing 113 |

114 | 115 | ## Cite *LeGO-LOAM* 116 | 117 | Thank you for citing [our *LeGO-LOAM* paper](./Shan_Englot_IROS_2018_Preprint.pdf) if you use any of this code: 118 | ``` 119 | @inproceedings{legoloam2018, 120 | title={LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain}, 121 | author={Shan, Tixiao and Englot, Brendan}, 122 | booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 123 | pages={4758-4765}, 124 | year={2018}, 125 | organization={IEEE} 126 | } 127 | ``` 128 | 129 | ## Loop Closure 130 | 131 | The loop-closure method implemented in this package is a naive ICP-based method. It often fails when the odometry drift is too large. For more advanced loop-closure methods, there is a package called [SC-LeGO-LOAM](https://github.com/irapkaist/SC-LeGO-LOAM), which features utilizing point cloud descriptor. 132 | 133 | ## Speed Optimization 134 | 135 | An optimized version of LeGO-LOAM can be found [here](https://github.com/facontidavide/LeGO-LOAM/tree/speed_optimization). All credits go to @facontidavide. Improvements in this directory include but not limited to: 136 | 137 | + To improve the quality of the code, making it more readable, consistent and easier to understand and modify. 138 | + To remove hard-coded values and use proper configuration files to describe the hardware. 139 | + To improve performance, in terms of amount of CPU used to calculate the same result. 140 | + To convert a multi-process application into a single-process / multi-threading one; this makes the algorithm more deterministic and slightly faster. 141 | + To make it easier and faster to work with rosbags: processing a rosbag should be done at maximum speed allowed by the CPU and in a deterministic way. 142 | + As a consequence of the previous point, creating unit and regression tests will be easier. 143 | -------------------------------------------------------------------------------- /Shan_Englot_IROS_2018_Preprint.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/LeGO-LOAM/896a7a95a8bc510b76819d4cc48707e344bad621/Shan_Englot_IROS_2018_Preprint.pdf -------------------------------------------------------------------------------- /cloud_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(cloud_msgs) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | message_generation 7 | geometry_msgs 8 | std_msgs 9 | nav_msgs 10 | ) 11 | 12 | add_message_files( 13 | DIRECTORY msg 14 | FILES 15 | cloud_info.msg 16 | ) 17 | 18 | generate_messages( 19 | DEPENDENCIES 20 | geometry_msgs 21 | std_msgs 22 | nav_msgs 23 | ) 24 | 25 | 26 | catkin_package( 27 | CATKIN_DEPENDS 28 | message_runtime 29 | message_generation 30 | geometry_msgs 31 | std_msgs 32 | nav_msgs 33 | ) 34 | 35 | include_directories( 36 | ${catkin_INCLUDE_DIRS} 37 | ) 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /cloud_msgs/msg/cloud_info.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | int32[] startRingIndex 4 | int32[] endRingIndex 5 | 6 | float32 startOrientation 7 | float32 endOrientation 8 | float32 orientationDiff 9 | 10 | bool[] segmentedCloudGroundFlag # true - ground point, false - other points 11 | uint32[] segmentedCloudColInd # point column index in range image 12 | float32[] segmentedCloudRange # point range -------------------------------------------------------------------------------- /cloud_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cloud_msgs 4 | 1.0.0 5 | The cloud_msgs package contains messages. 6 | 7 | Tixiao Shan 8 | 9 | BSD 10 | 11 | catkin 12 | geometry_msgs 13 | std_msgs 14 | message_runtime 15 | message_generation 16 | nav_msgs 17 | 18 | nav_msgs 19 | message_generation 20 | message_runtime 21 | geometry_msgs 22 | std_msgs 23 | 24 | 25 | 26 | 27 | 28 | --------------------------------------------------------------------------------