Extracted voxel size for less flat surface points filter.

* Added variable for the voxel size used in filtering the less flat surface points (configurable via the "lessFlatFilterSize" parameter).
hitbg-yjm · Nov 28, 2017 · 2a56189 · 2a56189
1 parent 49d5d2f
commit 2a56189
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git a/launch/loam_velodyne.launch b/launch/loam_velodyne.launch
@@ -2,7 +2,8 @@
 
   <arg name="rviz" default="true" />
 
-  <node pkg="loam_velodyne" type="scanRegistration" name="scanRegistration" output="screen"/>
+  <node pkg="loam_velodyne" type="scanRegistration" name="scanRegistration" output="screen">
+  </node>
 
   <node pkg="loam_velodyne" type="laserOdometry" name="laserOdometry" output="screen" respawn="true">
   </node>

diff --git a/src/scanRegistration.cpp b/src/scanRegistration.cpp
@@ -66,6 +66,9 @@ int MAX_CORNER_LESS_SHARP;
 /** The maximum number of flat surface points per feature region. */
 int MAX_SURFACE_FLAT = 4;
 
+/** The voxel size used for down sizing the remaining less flat surface points. */
+float LESS_FLAT_FILTER_SIZE = 0.2;
+
 /** The curvature threshold below / above which a point is considered a flat / corner point. */
 float SURFACE_CURVATURE_THRESHOLD = 0.1;
 
@@ -510,7 +513,7 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
     pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
     pcl::VoxelGrid<PointType> downSizeFilter;
     downSizeFilter.setInputCloud(surfPointsLessFlatScan);
-    downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
+    downSizeFilter.setLeafSize(LESS_FLAT_FILTER_SIZE, LESS_FLAT_FILTER_SIZE, LESS_FLAT_FILTER_SIZE);
     downSizeFilter.filter(surfPointsLessFlatScanDS);
 
     surfPointsLessFlat += surfPointsLessFlatScanDS;
@@ -635,13 +638,20 @@ int main(int argc, char** argv)
     ros::shutdown();
   }
 
+  LESS_FLAT_FILTER_SIZE = nh.param("/scanRegistration/lessFlatFilterSize", LESS_FLAT_FILTER_SIZE);
+  if (LESS_FLAT_FILTER_SIZE < 0.001) {
+    ROS_FATAL("Invalid lessFlatFilterSize parameter: %f (expected >= 0.001)", LESS_FLAT_FILTER_SIZE);
+    ros::shutdown();
+  }
+
 
   ROS_INFO("Using  %d  feature regions per scan.", N_FEATURE_REGIONS);
   ROS_INFO("Using  +/- %d  points for curvature calculation.", CURVATURE_REGION);
   ROS_INFO("Using at most  %d  sharp  and  %d  less sharp corner points per feature region.",
            MAX_CORNER_SHARP, MAX_CORNER_LESS_SHARP);
   ROS_INFO("Using at most  %d  flat surface points per feature region.", MAX_SURFACE_FLAT);
   ROS_INFO("Using  %g  as surface curvature threshold.", SURFACE_CURVATURE_THRESHOLD);
+  ROS_INFO("Using  %g  as less flat surface points voxel filter size.", LESS_FLAT_FILTER_SIZE);
 
 
   ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>