Extracted some more constants.

* Added variable for the maximum number of iterations in LaserOdometry and LaserMapping (configurable via the "maxIterations" parameter).
* Added abort thresholds for deltaT and deltaR to LaserOdometry and LaserMapping (configurable via the "deltaTAbort" and "deltaRAbort" parameter).
* Added parsing of scanPeriod from parameters to LaserOdometry and LaserMapping (configurable via the "scanPeriod" parameter).
* Added parsing of ioRatio to LaserOdometry (configurable via the "ioRatio" parameter).
* Added parsing of filter sizes to LaserMapping (configurable via the "cornerFilterSize", "surfaceFilterSize" and "mapFilterSize" parameters).
* Added scanPeriod argument to launch files and hand it over to all relevant components.

include/loam_velodyne/LaserMapping.h

@@ -87,7 +87,8 @@ typedef struct IMUState2 {
  */
 class LaserMapping {
 public:
-  explicit LaserMapping(const float& scanPeriod);
+  explicit LaserMapping(const float& scanPeriod = 0.1,
+                        const size_t& maxIterations = 10);
 
   /** \brief Setup component in active mode.
    *
@@ -154,12 +155,16 @@ class LaserMapping {
 
 
 private:
-  const float _scanPeriod;
+  float _scanPeriod;          ///< time per scan
   const int _stackFrameNum;
   const int _mapFrameNum;
   long _frameCount;
   long _mapFrameCount;
 
+  size_t _maxIterations;  ///< maximum number of iterations
+  float _deltaTAbort;     ///< optimization abort threshold for deltaT
+  float _deltaRAbort;     ///< optimization abort threshold for deltaR
+
   int _laserCloudCenWidth;
   int _laserCloudCenHeight;
   int _laserCloudCenDepth;

include/loam_velodyne/LaserOdometry.h

@@ -53,8 +53,9 @@ namespace loam {
  */
 class LaserOdometry {
 public:
-  explicit LaserOdometry(const float& scanPeriod,
-                         const uint16_t& ioRatio = 2);
+  explicit LaserOdometry(const float& scanPeriod = 0.1,
+                         const uint16_t& ioRatio = 2,
+                         const size_t& maxIterations = 25);
 
   /** \brief Setup component.
    *
@@ -141,10 +142,14 @@ class LaserOdometry {
   void publishResult();
 
 private:
-  const float _scanPeriod;  ///< time per scan
-  const uint16_t _ioRatio;  ///< ratio of input to output frames
-  bool _systemInited;       ///< initialization flag
-  long _frameCount;         ///< number of processed frames
+  float _scanPeriod;       ///< time per scan
+  uint16_t _ioRatio;       ///< ratio of input to output frames
+  bool _systemInited;      ///< initialization flag
+  long _frameCount;        ///< number of processed frames
+
+  size_t _maxIterations;   ///< maximum number of iterations
+  float _deltaTAbort;     ///< optimization abort threshold for deltaT
+  float _deltaRAbort;     ///< optimization abort threshold for deltaR
 
   pcl::PointCloud<pcl::PointXYZI>::Ptr _cornerPointsSharp;      ///< sharp corner points cloud
   pcl::PointCloud<pcl::PointXYZI>::Ptr _cornerPointsLessSharp;  ///< less sharp corner points cloud

include/loam_velodyne/ScanRegistration.h

@@ -83,9 +83,6 @@ class RegistrationParams {
    */
   bool parseParams(const ros::NodeHandle& nh);
 
-  /** \brief Print parameters to ROS_INFO. */
-  void print();
-
   /** The time per scan. */
   float scanPeriod;
 

launch/hector_loam_velodyne.launch

@@ -1,21 +1,27 @@
 <launch>
 
   <arg name="rviz" default="true" />
+  <arg name="scanPeriod" default="0.1" />
 
   <node pkg="loam_velodyne" type="multiScanRegistration" name="multiScanRegistration" output="screen">
-      <param name="lidar" value="VLP-16" /> <!-- options: VLP-16  HDL-32  HDL-64E -->
-      <param name="scanPeriod" value="0.1" />
-      <remap from="/multi_scan_points" to="/velodyne_points" />
-      <remap from="/imu/data" to="/raw_imu" />  
-      <!-- <remap from="/imu/data" to="/djiros/imu" />   -->
-  </node>
+    <param name="lidar" value="VLP-16" /> <!-- options: VLP-16  HDL-32  HDL-64E -->
+    <param name="scanPeriod" value="$(arg scanPeriod)" />
 
-  <node pkg="loam_velodyne" type="laserOdometry" name="laserOdometry" output="screen" respawn="true"/>
+    <remap from="/multi_scan_points" to="/velodyne_points" />
+    <remap from="/imu/data" to="/raw_imu" />
+    <!-- <remap from="/imu/data" to="/djiros/imu" />   -->
+  </node>
 
+  <node pkg="loam_velodyne" type="laserOdometry" name="laserOdometry" output="screen" respawn="true">
+    <param name="scanPeriod" value="$(arg scanPeriod)" />
+  </node>
 
+  <node pkg="loam_velodyne" type="laserMapping" name="laserMapping" output="screen">
+    <param name="scanPeriod" value="$(arg scanPeriod)" />
+  </node>
 
-  <node pkg="loam_velodyne" type="laserMapping" name="laserMapping" output="screen"/>
-  <node pkg="loam_velodyne" type="transformMaintenance" name="transformMaintenance" output="screen"/>
+  <node pkg="loam_velodyne" type="transformMaintenance" name="transformMaintenance" output="screen">
+  </node>
 
   <group if="$(arg rviz)">
     <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find loam_velodyne)/rviz_cfg/loam_velodyne.rviz" />

launch/loam_velodyne.launch

@@ -1,18 +1,25 @@
 <launch>
 
   <arg name="rviz" default="true" />
+  <arg name="scanPeriod" default="0.1" />
 
   <node pkg="loam_velodyne" type="multiScanRegistration" name="multiScanRegistration" output="screen">
     <param name="lidar" value="VLP-16" /> <!-- options: VLP-16  HDL-32  HDL-64E -->
-    <param name="scanPeriod" value="0.1" />
+    <param name="scanPeriod" value="$(arg scanPeriod)" />
+
     <remap from="/multi_scan_points" to="/velodyne_points" />
   </node>
 
   <node pkg="loam_velodyne" type="laserOdometry" name="laserOdometry" output="screen" respawn="true">
+    <param name="scanPeriod" value="$(arg scanPeriod)" />
+  </node>
+
+  <node pkg="loam_velodyne" type="laserMapping" name="laserMapping" output="screen">
+    <param name="scanPeriod" value="$(arg scanPeriod)" />
   </node>
 
-  <node pkg="loam_velodyne" type="laserMapping" name="laserMapping" output="screen"/>
-  <node pkg="loam_velodyne" type="transformMaintenance" name="transformMaintenance" output="screen"/>
+  <node pkg="loam_velodyne" type="transformMaintenance" name="transformMaintenance" output="screen">
+  </node>
 
   <group if="$(arg rviz)">
     <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find loam_velodyne)/rviz_cfg/loam_velodyne.rviz" />

src/lib/LaserMapping.cpp

@@ -48,12 +48,16 @@ using std::atan2;
 using std::pow;
 
 
-LaserMapping::LaserMapping(const float& scanPeriod)
+LaserMapping::LaserMapping(const float& scanPeriod,
+                           const size_t& maxIterations)
       : _scanPeriod(scanPeriod),
         _stackFrameNum(1),
         _mapFrameNum(5),
         _frameCount(0),
         _mapFrameCount(0),
+        _maxIterations(maxIterations),
+        _deltaTAbort(0.05),
+        _deltaRAbort(0.05),
         _laserCloudCenWidth(10),
         _laserCloudCenHeight(5),
         _laserCloudCenDepth(10),
@@ -108,11 +112,87 @@ LaserMapping::LaserMapping(const float& scanPeriod)
 bool LaserMapping::setup(ros::NodeHandle& node,
                          ros::NodeHandle& privateNode)
 {
+  // fetch laser mapping params
+  float fParam;
+  int iParam;
+
+  if (privateNode.getParam("scanPeriod", fParam)) {
+    if (fParam <= 0) {
+      ROS_ERROR("Invalid scanPeriod parameter: %f (expected > 0)", fParam);
+      return false;
+    } else {
+      _scanPeriod = fParam;
+      ROS_INFO("Set scanPeriod: %g", fParam);
+    }
+  }
+
+  if (privateNode.getParam("maxIterations", iParam)) {
+    if (iParam < 1) {
+      ROS_ERROR("Invalid maxIterations parameter: %d (expected > 0)", iParam);
+      return false;
+    } else {
+      _maxIterations = iParam;
+      ROS_INFO("Set maxIterations: %d", iParam);
+    }
+  }
+
+  if (privateNode.getParam("deltaTAbort", fParam)) {
+    if (fParam <= 0) {
+      ROS_ERROR("Invalid deltaTAbort parameter: %f (expected > 0)", fParam);
+      return false;
+    } else {
+      _deltaTAbort = fParam;
+      ROS_INFO("Set deltaTAbort: %g", fParam);
+    }
+  }
+
+  if (privateNode.getParam("deltaRAbort", fParam)) {
+    if (fParam <= 0) {
+      ROS_ERROR("Invalid deltaRAbort parameter: %f (expected > 0)", fParam);
+      return false;
+    } else {
+      _deltaRAbort = fParam;
+      ROS_INFO("Set deltaRAbort: %g", fParam);
+    }
+  }
+
+  if (privateNode.getParam("cornerFilterSize", fParam)) {
+    if (fParam < 0.001) {
+      ROS_ERROR("Invalid cornerFilterSize parameter: %f (expected >= 0.001)", fParam);
+      return false;
+    } else {
+      _downSizeFilterCorner.setLeafSize(fParam, fParam, fParam);
+      ROS_INFO("Set corner down size filter leaf size: %g", fParam);
+    }
+  }
+
+  if (privateNode.getParam("surfaceFilterSize", fParam)) {
+    if (fParam < 0.001) {
+      ROS_ERROR("Invalid surfaceFilterSize parameter: %f (expected >= 0.001)", fParam);
+      return false;
+    } else {
+      _downSizeFilterSurf.setLeafSize(fParam, fParam, fParam);
+      ROS_INFO("Set surface down size filter leaf size: %g", fParam);
+    }
+  }
+
+  if (privateNode.getParam("mapFilterSize", fParam)) {
+    if (fParam < 0.001) {
+      ROS_ERROR("Invalid mapFilterSize parameter: %f (expected >= 0.001)", fParam);
+      return false;
+    } else {
+      _downSizeFilterMap.setLeafSize(fParam, fParam, fParam);
+      ROS_INFO("Set map down size filter leaf size: %g", fParam);
+    }
+  }
+
+
   // advertise laser mapping topics
   _pubLaserCloudSurround = node.advertise<sensor_msgs::PointCloud2> ("/laser_cloud_surround", 1);
   _pubLaserCloudFullRes = node.advertise<sensor_msgs::PointCloud2> ("/velodyne_cloud_registered", 2);
   _pubOdomAftMapped = node.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init", 5);
 
+
   // subscribe to laser odometry topics
   _subLaserCloudCornerLast = node.subscribe<sensor_msgs::PointCloud2>
       ("/laser_cloud_corner_last", 2, &LaserMapping::laserCloudCornerLastHandler, this);
@@ -779,7 +859,7 @@ void LaserMapping::optimizeTransformTobeMapped()
   pcl::PointCloud<pcl::PointXYZI> laserCloudOri;
   pcl::PointCloud<pcl::PointXYZI> coeffSel;
 
-  for (int iterCount = 0; iterCount < 10; iterCount++) {
+  for (size_t iterCount = 0; iterCount < _maxIterations; iterCount++) {
     laserCloudOri.clear();
     coeffSel.clear();
 
@@ -1025,7 +1105,7 @@ void LaserMapping::optimizeTransformTobeMapped()
                         pow(matX(4, 0) * 100, 2) +
                         pow(matX(5, 0) * 100, 2));
 
-    if (deltaR < 0.05 && deltaT < 0.05) {
+    if (deltaR < _deltaRAbort && deltaT < _deltaTAbort) {
       break;
     }
   }

src/lib/LaserOdometry.cpp

@@ -51,11 +51,15 @@ using std::pow;
 
 
 LaserOdometry::LaserOdometry(const float& scanPeriod,
-                             const uint16_t& ioRatio)
+                             const uint16_t& ioRatio,
+                             const size_t& maxIterations)
       : _scanPeriod(scanPeriod),
         _ioRatio(ioRatio),
         _systemInited(false),
         _frameCount(0),
+        _maxIterations(maxIterations),
+        _deltaTAbort(0.1),
+        _deltaRAbort(0.1),
         _cornerPointsSharp(new pcl::PointCloud<pcl::PointXYZI>()),
         _cornerPointsLessSharp(new pcl::PointCloud<pcl::PointXYZI>()),
         _surfPointsFlat(new pcl::PointCloud<pcl::PointXYZI>()),
@@ -79,12 +83,68 @@ LaserOdometry::LaserOdometry(const float& scanPeriod,
 bool LaserOdometry::setup(ros::NodeHandle &node,
                           ros::NodeHandle &privateNode)
 {
+  // fetch laser odometry params
+  float fParam;
+  int iParam;
+
+  if (privateNode.getParam("scanPeriod", fParam)) {
+    if (fParam <= 0) {
+      ROS_ERROR("Invalid scanPeriod parameter: %f (expected > 0)", fParam);
+      return false;
+    } else {
+      _scanPeriod = fParam;
+      ROS_INFO("Set scanPeriod: %g", fParam);
+    }
+  }
+
+  if (privateNode.getParam("ioRatio", iParam)) {
+    if (iParam < 1) {
+      ROS_ERROR("Invalid ioRatio parameter: %d (expected > 0)", iParam);
+      return false;
+    } else {
+      _ioRatio = iParam;
+      ROS_INFO("Set ioRatio: %d", iParam);
+    }
+  }
+
+  if (privateNode.getParam("maxIterations", iParam)) {
+    if (iParam < 1) {
+      ROS_ERROR("Invalid maxIterations parameter: %d (expected > 0)", iParam);
+      return false;
+    } else {
+      _maxIterations = iParam;
+      ROS_INFO("Set maxIterations: %d", iParam);
+    }
+  }
+
+  if (privateNode.getParam("deltaTAbort", fParam)) {
+    if (fParam <= 0) {
+      ROS_ERROR("Invalid deltaTAbort parameter: %f (expected > 0)", fParam);
+      return false;
+    } else {
+      _deltaTAbort = fParam;
+      ROS_INFO("Set deltaTAbort: %g", fParam);
+    }
+  }
+
+  if (privateNode.getParam("deltaRAbort", fParam)) {
+    if (fParam <= 0) {
+      ROS_ERROR("Invalid deltaRAbort parameter: %f (expected > 0)", fParam);
+      return false;
+    } else {
+      _deltaRAbort = fParam;
+      ROS_INFO("Set deltaRAbort: %g", fParam);
+    }
+  }
+
+
   // advertise laser odometry topics
   _pubLaserCloudCornerLast = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
   _pubLaserCloudSurfLast   = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
   _pubLaserCloudFullRes    = node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 2);
   _pubLaserOdometry        = node.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 5);
 
+
   // subscribe to scan registration topics
   _subCornerPointsSharp = node.subscribe<sensor_msgs::PointCloud2>
       ("/laser_cloud_sharp", 2, &LaserOdometry::laserCloudSharpHandler, this);
@@ -433,7 +493,7 @@ void LaserOdometry::process()
     _pointSearchSurfInd2.resize(surfPointsFlatNum);
     _pointSearchSurfInd3.resize(surfPointsFlatNum);
 
-    for (int iterCount = 0; iterCount < 25; iterCount++) {
+    for (size_t iterCount = 0; iterCount < _maxIterations; iterCount++) {
       pcl::PointXYZI pointSel, pointProj, tripod1, tripod2, tripod3;
       _laserCloudOri->clear();
       _coeffSel->clear();
@@ -771,7 +831,7 @@ void LaserOdometry::process()
                           pow(matX(4, 0) * 100, 2) +
                           pow(matX(5, 0) * 100, 2));
 
-      if (deltaR < 0.1 && deltaT < 0.1) {
+      if (deltaR < _deltaRAbort && deltaT < _deltaTAbort) {
         break;
       }
     }

src/lib/MultiScanRegistration.cpp

@@ -88,7 +88,7 @@ bool MultiScanRegistration::setup(ros::NodeHandle& node,
     return false;
   }
 
-  // parse scan mapping params
+  // fetch scan mapping params
   std::string lidarName;
 
   if (privateNode.getParam("lidar", lidarName)) {
@@ -103,8 +103,10 @@ bool MultiScanRegistration::setup(ros::NodeHandle& node,
       return false;
     }
 
+    ROS_INFO("Set  %s  scan mapper.", lidarName.c_str());
     if (!privateNode.hasParam("scanPeriod")) {
       _config.scanPeriod = 0.1;
+      ROS_INFO("Set scanPeriod: %f", _config.scanPeriod);
     }
   } else {
     float vAngleMin, vAngleMax;
@@ -122,6 +124,7 @@ bool MultiScanRegistration::setup(ros::NodeHandle& node,
       }
 
       _scanMapper.set(vAngleMin, vAngleMax, nScanRings);
+      ROS_INFO("Set linear scan mapper from %g to %g degrees with %d scan rings.", vAngleMin, vAngleMax, nScanRings);
     }
   }