Multi scan

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)

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

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/VelodyneScanRegistration.h → include/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

include/loam_velodyne/ScanRegistration.h

@@ -64,22 +64,30 @@ 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);
+
+  /** 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 +109,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 +175,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 +264,6 @@ class ScanRegistration {
 
 
 protected:
-  const float _scanPeriod;      ///< time per scan
   RegistrationParams _config;   ///< registration parameter
 
   ros::Time _sweepStart;                  ///< time stamp of beginning of current sweep