From 4e1fb82e1ecbc1eb49c49b1e09421d2e49a37df7 Mon Sep 17 00:00:00 2001 From: Stefan Glaser Date: Sat, 9 Dec 2017 17:51:49 +0100 Subject: [PATCH 1/2] Replaced VelodyneScanRegistration with more generic MultiScanRegistration. * 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. --- CMakeLists.txt | 6 +- include/loam_velodyne/CircularBuffer.h | 21 +++ ...Registration.h => MultiScanRegistration.h} | 80 ++++++++-- include/loam_velodyne/ScanRegistration.h | 141 ++++------------- launch/hector_loam_velodyne.launch | 6 +- launch/loam_velodyne.launch | 5 +- src/lib/CMakeLists.txt | 2 +- ...stration.cpp => MultiScanRegistration.cpp} | 111 ++++++++++--- src/lib/ScanRegistration.cpp | 147 +++++++++++++++++- ...e.cpp => multi_scan_registration_node.cpp} | 6 +- 10 files changed, 364 insertions(+), 161 deletions(-) rename include/loam_velodyne/{VelodyneScanRegistration.h => MultiScanRegistration.h} (52%) rename src/lib/{VelodyneScanRegistration.cpp => MultiScanRegistration.cpp} (60%) rename src/{velodyne_scan_registration_node.cpp => multi_scan_registration_node.cpp} (64%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2c1c5ca..997d4b1 100644 --- 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 index 60e1f4a..90ad93b 100644 --- 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/include/loam_velodyne/VelodyneScanRegistration.h b/include/loam_velodyne/MultiScanRegistration.h similarity index 52% rename from include/loam_velodyne/VelodyneScanRegistration.h rename to include/loam_velodyne/MultiScanRegistration.h index f09eab6..c431702 100644 --- a/include/loam_velodyne/VelodyneScanRegistration.h +++ b/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& 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 index c5d9053..89d2f9f 100644 --- 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 index 4cfd8ba..2abb407 100644 --- a/launch/hector_loam_velodyne.launch +++ b/launch/hector_loam_velodyne.launch @@ -2,8 +2,10 @@ - - + + + + diff --git a/launch/loam_velodyne.launch b/launch/loam_velodyne.launch index e6776bf..dc1e67a 100644 --- a/launch/loam_velodyne.launch +++ b/launch/loam_velodyne.launch @@ -2,7 +2,10 @@ - + + + + diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index da2f148..0a662ec 100644 --- 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) diff --git a/src/lib/VelodyneScanRegistration.cpp b/src/lib/MultiScanRegistration.cpp similarity index 60% rename from src/lib/VelodyneScanRegistration.cpp rename to src/lib/MultiScanRegistration.cpp index 4d2dede..b817b03 100644 --- a/src/lib/VelodyneScanRegistration.cpp +++ b/src/lib/MultiScanRegistration.cpp @@ -30,7 +30,7 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include "loam_velodyne/VelodyneScanRegistration.h" +#include "loam_velodyne/MultiScanRegistration.h" #include "math_utils.h" #include @@ -38,35 +38,104 @@ namespace loam { -VelodyneScanRegistration::VelodyneScanRegistration(const float& scanPeriod, - const uint16_t& nScanRings, - const RegistrationParams& config, - const size_t& imuHistorySize) - : ScanRegistration(scanPeriod, config, imuHistorySize), - _systemDelay(20), - _nScanRings(nScanRings) +MultiScanMapper::MultiScanMapper(const float& lowerBound, + const float& upperBound, + const uint16_t& nScanRings) + : _lowerBound(lowerBound), + _upperBound(upperBound), + _nScanRings(nScanRings), + _factor((nScanRings - 1) / (upperBound - lowerBound)) +{ + +} + +void MultiScanMapper::set(const float &lowerBound, + const float &upperBound, + const uint16_t &nScanRings) +{ + _lowerBound = lowerBound; + _upperBound = upperBound; + _nScanRings = nScanRings; + _factor = (nScanRings - 1) / (upperBound - lowerBound); +} + + + +int MultiScanMapper::getRingForAngle(const float& angle) { + return int(((angle * 180 / M_PI) - _lowerBound) * _factor + 0.5); +} + + + + + + +MultiScanRegistration::MultiScanRegistration(const MultiScanMapper& scanMapper, + const RegistrationParams& config) + : ScanRegistration(config), + _systemDelay(SYSTEM_DELAY), + _scanMapper(scanMapper) { }; -bool VelodyneScanRegistration::setup(ros::NodeHandle& node, - ros::NodeHandle& privateNode) +bool MultiScanRegistration::setup(ros::NodeHandle& node, + ros::NodeHandle& privateNode) { if (!ScanRegistration::setup(node, privateNode)) { return false; } + // parse scan mapping params + std::string lidarName; + + if (privateNode.getParam("lidar", lidarName)) { + if (lidarName == "VLP-16") { + _scanMapper = MultiScanMapper::Velodyne_VLP_16(); + } else if (lidarName == "HDL-32") { + _scanMapper = MultiScanMapper::Velodyne_HDL_32(); + } else if (lidarName == "HDL-64E") { + _scanMapper = MultiScanMapper::Velodyne_HDL_64E(); + } else { + ROS_ERROR("Invalid lidar parameter: %s (only \"VLP-16\", \"HDL-32\" and \"HDL-64E\" are supported)", lidarName.c_str()); + return false; + } + + if (!privateNode.hasParam("scanPeriod")) { + _config.scanPeriod = 0.1; + } + } else { + float vAngleMin, vAngleMax; + int nScanRings; + + if (privateNode.getParam("minVerticalAngle", vAngleMin) && + privateNode.getParam("maxVerticalAngle", vAngleMax) && + privateNode.getParam("nScanRings", nScanRings)) { + if (vAngleMin >= vAngleMax) { + ROS_ERROR("Invalid vertical range (min >= max)"); + return false; + } else if (nScanRings < 2) { + ROS_ERROR("Invalid number of scan rings (n < 2)"); + return false; + } + + _scanMapper.set(vAngleMin, vAngleMax, nScanRings); + } + } + + + // subscribe to input cloud topic _subLaserCloud = node.subscribe - ("/velodyne_points", 2, &VelodyneScanRegistration::handleCloudMessage, this); + ("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this); return true; } -void VelodyneScanRegistration::handleCloudMessage(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) +void MultiScanRegistration::handleCloudMessage(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) { if (_systemDelay > 0) { _systemDelay--; @@ -82,8 +151,8 @@ void VelodyneScanRegistration::handleCloudMessage(const sensor_msgs::PointCloud2 -void VelodyneScanRegistration::process(const pcl::PointCloud& laserCloudIn, - const ros::Time& scanTime) +void MultiScanRegistration::process(const pcl::PointCloud& laserCloudIn, + const ros::Time& scanTime) { size_t cloudSize = laserCloudIn.size(); @@ -102,7 +171,7 @@ void VelodyneScanRegistration::process(const pcl::PointCloud& las bool halfPassed = false; pcl::PointXYZI point; - std::vector > laserCloudScans(_nScanRings); + std::vector > laserCloudScans(_scanMapper.getNumberOfScanRings()); // extract valid points from input cloud for (int i = 0; i < cloudSize; i++) { @@ -123,11 +192,9 @@ void VelodyneScanRegistration::process(const pcl::PointCloud& las } // calculate vertical point angle and scan ID - float angle = rad2deg(std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z))); - int roundedAngle = int(angle + (angle < 0.0 ? -0.5 : 0.5)); - int scanID = roundedAngle > 0 ? roundedAngle : roundedAngle + (_nScanRings - 1); - - if (scanID >= _nScanRings || scanID < 0 ){ + float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z)); + int scanID = _scanMapper.getRingForAngle(angle); + if (scanID >= _scanMapper.getNumberOfScanRings() || scanID < 0 ){ continue; } @@ -154,7 +221,7 @@ void VelodyneScanRegistration::process(const pcl::PointCloud& las } // calculate relative scan time based on point orientation - float relTime = _scanPeriod * (ori - startOri) / (endOri - startOri); + float relTime = _config.scanPeriod * (ori - startOri) / (endOri - startOri); point.intensity = scanID + relTime; // project point to the start of the sweep using corresponding IMU data @@ -168,7 +235,7 @@ void VelodyneScanRegistration::process(const pcl::PointCloud& las // construct sorted full resolution cloud cloudSize = 0; - for (int i = 0; i < _nScanRings; i++) { + for (int i = 0; i < _scanMapper.getNumberOfScanRings(); i++) { _laserCloud += laserCloudScans[i]; IndexRange range(cloudSize, 0); diff --git a/src/lib/ScanRegistration.cpp b/src/lib/ScanRegistration.cpp index 0b23ce0..b5f5e8a 100644 --- a/src/lib/ScanRegistration.cpp +++ b/src/lib/ScanRegistration.cpp @@ -39,17 +39,149 @@ namespace loam { -ScanRegistration::ScanRegistration(const float& scanPeriod, - const RegistrationParams& config, - const size_t& imuHistorySize) - : _scanPeriod(scanPeriod), - _config(config), + +RegistrationParams::RegistrationParams(const float& scanPeriod_, + const int& imuHistorySize_, + const int& nFeatureRegions_, + const int& curvatureRegion_, + const int& maxCornerSharp_, + const int& maxSurfaceFlat_, + const float& lessFlatFilterSize_, + const float& surfaceCurvatureThreshold_) + : scanPeriod(scanPeriod_), + imuHistorySize(imuHistorySize_), + nFeatureRegions(nFeatureRegions_), + curvatureRegion(curvatureRegion_), + maxCornerSharp(maxCornerSharp_), + maxCornerLessSharp(10 * maxCornerSharp_), + maxSurfaceFlat(maxSurfaceFlat_), + lessFlatFilterSize(lessFlatFilterSize_), + surfaceCurvatureThreshold(surfaceCurvatureThreshold_) +{ + +}; + + + +bool RegistrationParams::parseParams(const ros::NodeHandle& nh) { + bool success = true; + int iParam = 0; + float fParam = 0; + + if (nh.getParam("scanPeriod", fParam)) { + if (fParam <= 0) { + ROS_ERROR("Invalid scanPeriod parameter: %f (expected > 0)", fParam); + success = false; + } else { + scanPeriod = fParam; + } + } + + if (nh.getParam("imuHistorySize", iParam)) { + if (iParam < 1) { + ROS_ERROR("Invalid imuHistorySize parameter: %d (expected >= 1)", iParam); + success = false; + } else { + imuHistorySize = iParam; + } + } + + 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; +} + + + +void RegistrationParams::print() +{ + ROS_INFO_STREAM(" ===== scan registration parameters =====" << std::endl + << " - Using " << scanPeriod << " as scan period." << std::endl + << " - Using " << imuHistorySize << " as IMU state history size." << 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."); +} + + + + + + +ScanRegistration::ScanRegistration(const RegistrationParams& config) + : _config(config), _sweepStart(), _scanTime(), _imuStart(), _imuCur(), _imuIdx(0), - _imuHistory(imuHistorySize), + _imuHistory(_config.imuHistorySize), _laserCloud(), _cornerPointsSharp(), _cornerPointsLessSharp(), @@ -62,7 +194,7 @@ ScanRegistration::ScanRegistration(const float& scanPeriod, _scanNeighborPicked() { -}; +} @@ -73,6 +205,7 @@ bool ScanRegistration::setup(ros::NodeHandle& node, return false; } _config.print(); + _imuHistory.ensureCapacity(_config.imuHistorySize); // subscribe to IMU topic _subImu = node.subscribe("/imu/data", 50, &ScanRegistration::handleIMUMessage, this); diff --git a/src/velodyne_scan_registration_node.cpp b/src/multi_scan_registration_node.cpp similarity index 64% rename from src/velodyne_scan_registration_node.cpp rename to src/multi_scan_registration_node.cpp index 678804c..e171c66 100644 --- a/src/velodyne_scan_registration_node.cpp +++ b/src/multi_scan_registration_node.cpp @@ -1,5 +1,5 @@ #include -#include "loam_velodyne/VelodyneScanRegistration.h" +#include "loam_velodyne/MultiScanRegistration.h" /** Main node entry point. */ @@ -9,9 +9,9 @@ int main(int argc, char **argv) ros::NodeHandle node; ros::NodeHandle privateNode("~"); - loam::VelodyneScanRegistration veloScan(0.1, 16); + loam::MultiScanRegistration multiScan; - if (veloScan.setup(node, privateNode)) { + if (multiScan.setup(node, privateNode)) { // initialization successful ros::spin(); } From 40b38fb9f2888116d804443cbb6c52fab49a184c Mon Sep 17 00:00:00 2001 From: Stefan Glaser Date: Sat, 9 Dec 2017 19:24:00 +0100 Subject: [PATCH 2/2] 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 | 9 ++- include/loam_velodyne/LaserOdometry.h | 17 +++-- include/loam_velodyne/ScanRegistration.h | 3 - launch/hector_loam_velodyne.launch | 24 ++++--- launch/loam_velodyne.launch | 13 +++- src/lib/LaserMapping.cpp | 86 +++++++++++++++++++++++- src/lib/LaserOdometry.cpp | 66 +++++++++++++++++- src/lib/MultiScanRegistration.cpp | 5 +- src/lib/ScanRegistration.cpp | 26 +++---- 9 files changed, 202 insertions(+), 47 deletions(-) diff --git a/include/loam_velodyne/LaserMapping.h b/include/loam_velodyne/LaserMapping.h index e3e7354..0102e37 100644 --- a/include/loam_velodyne/LaserMapping.h +++ b/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; diff --git a/include/loam_velodyne/LaserOdometry.h b/include/loam_velodyne/LaserOdometry.h index 2405152..e0d138f 100644 --- a/include/loam_velodyne/LaserOdometry.h +++ b/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::Ptr _cornerPointsSharp; ///< sharp corner points cloud pcl::PointCloud::Ptr _cornerPointsLessSharp; ///< less sharp corner points cloud diff --git a/include/loam_velodyne/ScanRegistration.h b/include/loam_velodyne/ScanRegistration.h index 89d2f9f..084e424 100644 --- a/include/loam_velodyne/ScanRegistration.h +++ b/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; diff --git a/launch/hector_loam_velodyne.launch b/launch/hector_loam_velodyne.launch index 2abb407..84b0dcd 100644 --- a/launch/hector_loam_velodyne.launch +++ b/launch/hector_loam_velodyne.launch @@ -1,21 +1,27 @@ + - - - - - - + + - + + + + + + + + + + - - + + diff --git a/launch/loam_velodyne.launch b/launch/loam_velodyne.launch index dc1e67a..642c4bb 100644 --- a/launch/loam_velodyne.launch +++ b/launch/loam_velodyne.launch @@ -1,18 +1,25 @@ + - + + + + + + + - - + + diff --git a/src/lib/LaserMapping.cpp b/src/lib/LaserMapping.cpp index 88221e1..755b4d2 100644 --- a/src/lib/LaserMapping.cpp +++ b/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 ("/laser_cloud_surround", 1); _pubLaserCloudFullRes = node.advertise ("/velodyne_cloud_registered", 2); _pubOdomAftMapped = node.advertise ("/aft_mapped_to_init", 5); + // subscribe to laser odometry topics _subLaserCloudCornerLast = node.subscribe ("/laser_cloud_corner_last", 2, &LaserMapping::laserCloudCornerLastHandler, this); @@ -779,7 +859,7 @@ void LaserMapping::optimizeTransformTobeMapped() pcl::PointCloud laserCloudOri; pcl::PointCloud 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; } } diff --git a/src/lib/LaserOdometry.cpp b/src/lib/LaserOdometry.cpp index 32eab9d..8a8ed02 100644 --- a/src/lib/LaserOdometry.cpp +++ b/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()), _cornerPointsLessSharp(new pcl::PointCloud()), _surfPointsFlat(new pcl::PointCloud()), @@ -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("/laser_cloud_corner_last", 2); _pubLaserCloudSurfLast = node.advertise("/laser_cloud_surf_last", 2); _pubLaserCloudFullRes = node.advertise("/velodyne_cloud_3", 2); _pubLaserOdometry = node.advertise("/laser_odom_to_init", 5); + // subscribe to scan registration topics _subCornerPointsSharp = node.subscribe ("/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; } } diff --git a/src/lib/MultiScanRegistration.cpp b/src/lib/MultiScanRegistration.cpp index b817b03..4659c8e 100644 --- a/src/lib/MultiScanRegistration.cpp +++ b/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); } } diff --git a/src/lib/ScanRegistration.cpp b/src/lib/ScanRegistration.cpp index b5f5e8a..519cba1 100644 --- a/src/lib/ScanRegistration.cpp +++ b/src/lib/ScanRegistration.cpp @@ -74,6 +74,7 @@ bool RegistrationParams::parseParams(const ros::NodeHandle& nh) { success = false; } else { scanPeriod = fParam; + ROS_INFO("Set scanPeriod: %g", fParam); } } @@ -83,6 +84,7 @@ bool RegistrationParams::parseParams(const ros::NodeHandle& nh) { success = false; } else { imuHistorySize = iParam; + ROS_INFO("Set imuHistorySize: %d", iParam); } } @@ -92,6 +94,7 @@ bool RegistrationParams::parseParams(const ros::NodeHandle& nh) { success = false; } else { nFeatureRegions = iParam; + ROS_INFO("Set nFeatureRegions: %d", iParam); } } @@ -101,6 +104,7 @@ bool RegistrationParams::parseParams(const ros::NodeHandle& nh) { success = false; } else { curvatureRegion = iParam; + ROS_INFO("Set curvatureRegion: +/- %d", iParam); } } @@ -111,6 +115,7 @@ bool RegistrationParams::parseParams(const ros::NodeHandle& nh) { } else { maxCornerSharp = iParam; maxCornerLessSharp = 10 * iParam; + ROS_INFO("Set maxCornerSharp / less sharp: %d / %d", iParam, maxCornerLessSharp); } } @@ -120,6 +125,7 @@ bool RegistrationParams::parseParams(const ros::NodeHandle& nh) { success = false; } else { maxCornerLessSharp = iParam; + ROS_INFO("Set maxCornerLessSharp: %d", iParam); } } @@ -129,6 +135,7 @@ bool RegistrationParams::parseParams(const ros::NodeHandle& nh) { success = false; } else { maxSurfaceFlat = iParam; + ROS_INFO("Set maxSurfaceFlat: %d", iParam); } } @@ -138,6 +145,7 @@ bool RegistrationParams::parseParams(const ros::NodeHandle& nh) { success = false; } else { surfaceCurvatureThreshold = fParam; + ROS_INFO("Set surfaceCurvatureThreshold: %g", fParam); } } @@ -147,6 +155,7 @@ bool RegistrationParams::parseParams(const ros::NodeHandle& nh) { success = false; } else { lessFlatFilterSize = fParam; + ROS_INFO("Set lessFlatFilterSize: %g", fParam); } } @@ -155,22 +164,6 @@ bool RegistrationParams::parseParams(const ros::NodeHandle& nh) { -void RegistrationParams::print() -{ - ROS_INFO_STREAM(" ===== scan registration parameters =====" << std::endl - << " - Using " << scanPeriod << " as scan period." << std::endl - << " - Using " << imuHistorySize << " as IMU state history size." << 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."); -} - - - @@ -204,7 +197,6 @@ bool ScanRegistration::setup(ros::NodeHandle& node, if (!_config.parseParams(privateNode)) { return false; } - _config.print(); _imuHistory.ensureCapacity(_config.imuHistorySize); // subscribe to IMU topic