Replaced VelodyneScanRegistration with more generic MultiScanRegistra…

…tion. * The VedyneScanRegistration was using a mapping from vertical point angles to scan rings. This is a rather generic approach of mapping points to scan rings and thus applicapbe on arbitrary multi-laser lidars. The new MultiScanRegistration component reflects this generic approach in its name and allows a flexible configuration of the mapping interval via a MultiScanMapper instance. * Renamed input cloud topic for MultiScanRegistration component to "\multi_scan_points" and added default mapping to launch files. * Added MultiScanMapper class providing a linear mapping from vertical point angles to scan rings. * Moved scanPeriod from ScanRegistration to RegistrationParams (configurable via the "scanPeriod" parameter). * Added imuHistorySize to RegistrationParams (configurable via the "imuHistorySize" parameter). * Added "lidar", respectively "minVerticalAngle", "maxVerticalAngle" and "nScanRings" parameters to MultiScanRegistration component. Possible options for "lidar" parameter: "VLP-16", "HDL-32" and "HDL-64E". Alternatively, the linear mapping can be specified via the vertical range ("min/maxVerticalAngle") and the number of scan rings ("nScanRings"). The min/max vertical angles together with the number of scan rings is only evaluated if no "lidar" parameter is present. * Renamed the "scanRegistration" node to "multiScanRegistration" and adapted launch files. Changes to the logic: * The mapping of the scan rings for the VLP-16 lidar is now linear from -15 to 15. The old mapping was alternating scan rings based on positive and negative vertical angles. However, the algorithm IMHO requires scans to be next to each other, as it searches the neighbouring scans for corresponding points. New features: * Simple support for arbitrary multi laser lidar systems. Some calculations like the reference system transformation need some further work to be truely generic, but it should at least work for all Velodyne lidars.
hitbg-yjm · Dec 9, 2017 · 4e1fb82 · 4e1fb82
1 parent 89af2a5
commit 4e1fb82
Show file tree

Hide file tree

Showing 10 changed files with 364 additions and 161 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -31,8 +31,8 @@ add_definitions( -march=native )
 
 add_subdirectory(src/lib)
 
-add_executable(scanRegistration src/velodyne_scan_registration_node.cpp)
-target_link_libraries(scanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam )
+add_executable(multiScanRegistration src/multi_scan_registration_node.cpp)
+target_link_libraries(multiScanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam )
 
 add_executable(laserOdometry src/laser_odometry_node.cpp)
 target_link_libraries(laserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam )
@@ -57,7 +57,7 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest(${PROJECT_BINARY_DIR}/test/loam.test
     DEPENDENCIES
       ${PROJECT_NAME}_test_data
-      scanRegistration
+      multiScanRegistration
       laserOdometry
       laserMapping
       transformMaintenance)

diff --git a/include/loam_velodyne/CircularBuffer.h b/include/loam_velodyne/CircularBuffer.h
@@ -46,6 +46,27 @@ class CircularBuffer {
     return _capacity;
   }
 
+  /** \brief Ensure that this buffer has at least the required capacity.
+   *
+   * @param reqCapacity the minimum required capacity
+   */
+  void ensureCapacity(const int& reqCapacity) {
+    if (reqCapacity > 0 && _capacity < reqCapacity) {
+      // create new buffer and copy (valid) entries
+      T* newBuffer = new T[reqCapacity];
+      for (size_t i = 0; i < _size; i++) {
+        newBuffer[i] = (*this)[i];
+      }
+
+      // switch buffer pointers and delete old buffer
+      T* oldBuffer = _buffer;
+      _buffer = newBuffer;
+      _startIdx = 0;
+
+      delete[] oldBuffer;
+    }
+  }
+
   /** \brief Check if the buffer is empty.
    *
    * @return true if the buffer is empty, false otherwise

diff --git a/.../loam_velodyne/VelodyneScanRegistration.h → ...ude/loam_velodyne/MultiScanRegistration.h b/.../loam_velodyne/VelodyneScanRegistration.h → ...ude/loam_velodyne/MultiScanRegistration.h
@@ -30,8 +30,8 @@
 //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
 //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
 
-#ifndef LOAM_VELODYNESCANREGISTRATION_H
-#define LOAM_VELODYNESCANREGISTRATION_H
+#ifndef LOAM_MULTISCANREGISTRATION_H
+#define LOAM_MULTISCANREGISTRATION_H
 
 
 #include "loam_velodyne/ScanRegistration.h"
@@ -41,15 +41,70 @@
 
 namespace loam {
 
-/** \brief Class for registering Velodyne VLP-16 scans.
+
+
+/** \brief Class realizing a linear mapping from vertical point angle to the corresponding scan ring.
+ *
+ */
+class MultiScanMapper {
+public:
+  /** \brief Construct a new multi scan mapper instance.
+   *
+   * @param lowerBound - the lower vertical bound (degrees)
+   * @param upperBound - the upper vertical bound (degrees)
+   * @param nScanRings - the number of scan rings
+   */
+  MultiScanMapper(const float& lowerBound = -15,
+                  const float& upperBound = 15,
+                  const uint16_t& nScanRings = 16);
+
+  const float& getLowerBound() { return _lowerBound; }
+  const float& getUpperBound() { return _upperBound; }
+  const uint16_t& getNumberOfScanRings() { return _nScanRings; }
+
+  /** \brief Set mapping parameters.
+   *
+   * @param lowerBound - the lower vertical bound (degrees)
+   * @param upperBound - the upper vertical bound (degrees)
+   * @param nScanRings - the number of scan rings
+   */
+  void set(const float& lowerBound,
+           const float& upperBound,
+           const uint16_t& nScanRings);
+
+  /** \brief Map the specified vertical point angle to its ring ID.
+   *
+   * @param angle the vertical point angle (in rad)
+   * @return the ring ID
+   */
+  inline int getRingForAngle(const float& angle);
+
+  /** Multi scan mapper for Velodyne VLP-16 according to data sheet. */
+  static inline MultiScanMapper Velodyne_VLP_16() { return MultiScanMapper(-15, 15, 16); };
+
+  /** Multi scan mapper for Velodyne HDL-32 according to data sheet. */
+  static inline MultiScanMapper Velodyne_HDL_32() { return MultiScanMapper(-30.67f, 10.67f, 32); };
+
+  /** Multi scan mapper for Velodyne HDL-64E according to data sheet. */
+  static inline MultiScanMapper Velodyne_HDL_64E() { return MultiScanMapper(-24.9f, 2, 64); };
+
+
+private:
+  float _lowerBound;      ///< the vertical angle of the first scan ring
+  float _upperBound;      ///< the vertical angle of the last scan ring
+  uint16_t _nScanRings;   ///< number of scan rings
+  float _factor;          ///< linear interpolation factor
+};
+
+
+
+/** \brief Class for registering point clouds received from multi-laser lidars.
  *
  */
-class VelodyneScanRegistration : virtual public ScanRegistration {
+class MultiScanRegistration : virtual public ScanRegistration {
 public:
-  VelodyneScanRegistration(const float& scanPeriod,
-                           const uint16_t& nScanRings,
-                           const RegistrationParams& config = RegistrationParams(),
-                           const size_t& imuHistorySize = 200);
+  MultiScanRegistration(const MultiScanMapper& scanMapper = MultiScanMapper(),
+                        const RegistrationParams& config = RegistrationParams());
 
 
   /** \brief Setup component in active mode.
@@ -74,14 +129,19 @@ class VelodyneScanRegistration : virtual public ScanRegistration {
   void process(const pcl::PointCloud<pcl::PointXYZ>& laserCloudIn,
                const ros::Time& scanTime);
 
+
 protected:
   int _systemDelay;             ///< system startup delay counter
-  const uint16_t _nScanRings;   ///< number of scan rings
+  MultiScanMapper _scanMapper;  ///< mapper for mapping vertical point angles to scan ring IDs
 
   ros::Subscriber _subLaserCloud;   ///< input cloud message subscriber
+
+
+private:
+  static const int SYSTEM_DELAY = 20;
 };
 
 } // end namespace loam
 
 
-#endif //LOAM_VELODYNESCANREGISTRATION_H
+#endif //LOAM_MULTISCANREGISTRATION_H
diff --git a/include/loam_velodyne/ScanRegistration.h b/include/loam_velodyne/ScanRegistration.h
@@ -64,22 +64,33 @@ enum PointLabel {
 
 
 /** Scan Registration configuration parameters. */
-typedef struct RegistrationParams {
-  RegistrationParams(int nFeatureRegions_ = 6,
-                     int curvatureRegion_ = 5,
-                     int maxCornerSharp_ = 2,
-                     int maxSurfaceFlat_ = 4,
-                     float lessFlatFilterSize_ = 0.2,
-                     float surfaceCurvatureThreshold_ = 0.1)
-        : nFeatureRegions(nFeatureRegions_),
-          curvatureRegion(curvatureRegion_),
-          maxCornerSharp(maxCornerSharp_),
-          maxCornerLessSharp(10 * maxCornerSharp_),
-          maxSurfaceFlat(maxSurfaceFlat_),
-          lessFlatFilterSize(lessFlatFilterSize_),
-          surfaceCurvatureThreshold(surfaceCurvatureThreshold_) {};
-
-  ~RegistrationParams() {};
+class RegistrationParams {
+public:
+  RegistrationParams(const float& scanPeriod_ = 0.1,
+                     const int& imuHistorySize_ = 200,
+                     const int& nFeatureRegions_ = 6,
+                     const int& curvatureRegion_ = 5,
+                     const int& maxCornerSharp_ = 2,
+                     const int& maxSurfaceFlat_ = 4,
+                     const float& lessFlatFilterSize_ = 0.2,
+                     const float& surfaceCurvatureThreshold_ = 0.1);
+
+
+  /** \brief Parse node parameter.
+   *
+   * @param nh the ROS node handle
+   * @return true, if all specified parameters are valid, false if at least one specified parameter is invalid
+   */
+  bool parseParams(const ros::NodeHandle& nh);
+
+  /** \brief Print parameters to ROS_INFO. */
+  void print();
+
+  /** The time per scan. */
+  float scanPeriod;
+
+  /** The size of the IMU history state buffer. */
+  int imuHistorySize;
 
   /** The number of (equally sized) regions used to distribute the feature extraction within a scan. */
   int nFeatureRegions;
@@ -101,98 +112,7 @@ typedef struct RegistrationParams {
 
   /** The curvature threshold below / above a point is considered a flat / corner point. */
   float surfaceCurvatureThreshold;
-
-
-  /** \brief Parse node parameter.
-   *
-   * @param nh the ROS node handle
-   * @return true, if all specified parameters are valid, false if at least one specified parameter is invalid
-   */
-  bool parseParams(const ros::NodeHandle& nh) {
-    bool success = true;
-    int iParam = 0;
-    float fParam = 0;
-
-    if (nh.getParam("featureRegions", iParam)) {
-      if (iParam < 1) {
-        ROS_ERROR("Invalid featureRegions parameter: %d (expected >= 1)", iParam);
-        success = false;
-      } else {
-        nFeatureRegions = iParam;
-      }
-    }
-
-    if (nh.getParam("curvatureRegion", iParam)) {
-      if (iParam < 1) {
-        ROS_ERROR("Invalid curvatureRegion parameter: %d (expected >= 1)", iParam);
-        success = false;
-      } else {
-        curvatureRegion = iParam;
-      }
-    }
-
-    if (nh.getParam("maxCornerSharp", iParam)) {
-      if (iParam < 1) {
-        ROS_ERROR("Invalid maxCornerSharp parameter: %d (expected >= 1)", iParam);
-        success = false;
-      } else {
-        maxCornerSharp = iParam;
-        maxCornerLessSharp = 10 * iParam;
-      }
-    }
-
-    if (nh.getParam("maxCornerLessSharp", iParam)) {
-      if (iParam < maxCornerSharp) {
-        ROS_ERROR("Invalid maxCornerLessSharp parameter: %d (expected >= %d)", iParam, maxCornerSharp);
-        success = false;
-      } else {
-        maxCornerLessSharp = iParam;
-      }
-    }
-
-    if (nh.getParam("maxSurfaceFlat", iParam)) {
-      if (iParam < 1) {
-        ROS_ERROR("Invalid maxSurfaceFlat parameter: %d (expected >= 1)", iParam);
-        success = false;
-      } else {
-        maxSurfaceFlat = iParam;
-      }
-    }
-
-    if (nh.getParam("surfaceCurvatureThreshold", fParam)) {
-      if (fParam < 0.001) {
-        ROS_ERROR("Invalid surfaceCurvatureThreshold parameter: %f (expected >= 0.001)", fParam);
-        success = false;
-      } else {
-        surfaceCurvatureThreshold = fParam;
-      }
-    }
-
-    if (nh.getParam("lessFlatFilterSize", fParam)) {
-      if (fParam < 0.001) {
-        ROS_ERROR("Invalid lessFlatFilterSize parameter: %f (expected >= 0.001)", fParam);
-        success = false;
-      } else {
-        lessFlatFilterSize = fParam;
-      }
-    }
-
-    return success;
-  };
-
-  /** \brief Print parameters to ROS_INFO. */
-  void print()
-  {
-    ROS_INFO_STREAM(" ===== scan registration parameters =====" << std::endl
-        << "  - Using  " << nFeatureRegions << "  feature regions per scan." << std::endl
-        << "  - Using  +/- " << curvatureRegion << "  points for curvature calculation." << std::endl
-        << "  - Using at most  " << maxCornerSharp << "  sharp" << std::endl
-        << "              and  " << maxCornerLessSharp << "  less sharp corner points per feature region." << std::endl
-        << "  - Using at most  " << maxSurfaceFlat << "  flat surface points per feature region." << std::endl
-        << "  - Using  " << surfaceCurvatureThreshold << "  as surface curvature threshold." << std::endl
-        << "  - Using  " << lessFlatFilterSize << "  as less flat surface points voxel filter size.");
-  };
-} RegistrationParams;
+};
 
 
 
@@ -258,9 +178,7 @@ typedef struct IMUState {
  */
 class ScanRegistration {
 public:
-  explicit ScanRegistration(const float& scanPeriod,
-                            const RegistrationParams& config = RegistrationParams(),
-                            const size_t& imuHistorySize = 200);
+  explicit ScanRegistration(const RegistrationParams& config = RegistrationParams());
 
   /** \brief Setup component.
    *
@@ -349,7 +267,6 @@ class ScanRegistration {
 
 
 protected:
-  const float _scanPeriod;      ///< time per scan
   RegistrationParams _config;   ///< registration parameter
 
   ros::Time _sweepStart;                  ///< time stamp of beginning of current sweep

diff --git a/launch/hector_loam_velodyne.launch b/launch/hector_loam_velodyne.launch
@@ -2,8 +2,10 @@
 
   <arg name="rviz" default="true" />
 
-  <node pkg="loam_velodyne" type="scanRegistration" name="scanRegistration" output="screen">
-      <remap from="/velodyne_points" to="/velodyne_points" />
+  <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>

diff --git a/launch/loam_velodyne.launch b/launch/loam_velodyne.launch
@@ -2,7 +2,10 @@
 
   <arg name="rviz" default="true" />
 
-  <node pkg="loam_velodyne" type="scanRegistration" name="scanRegistration" output="screen">
+  <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" />
   </node>
 
   <node pkg="loam_velodyne" type="laserOdometry" name="laserOdometry" output="screen" respawn="true">

diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt
@@ -1,7 +1,7 @@
 add_library(loam
             math_utils.h
             ScanRegistration.cpp
-            VelodyneScanRegistration.cpp
+            MultiScanRegistration.cpp
             LaserOdometry.cpp
             LaserMapping.cpp
             TransformMaintenance.cpp)