Skip to content

Commit

Permalink
Merge pull request laboshinl#35 from StefanGlaser/multi_scan
Browse files Browse the repository at this point in the history 
Multi scan
  • Loading branch information
@laboshinl
laboshinl authored Dec 10, 2017
2 parents 89af2a5 + 40b38fb commit b8784a8
Show file tree
Hide file tree
Showing 14 changed files with 542 additions and 184 deletions.
6 changes: 3 additions & 3 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 )
Expand All @@ -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)
Expand Down
21 changes: 21 additions & 0 deletions include/loam_velodyne/CircularBuffer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
9 changes: 7 additions & 2 deletions include/loam_velodyne/LaserMapping.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*
Expand Down Expand Up @@ -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;
Expand Down
17 changes: 11 additions & 6 deletions include/loam_velodyne/LaserOdometry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*
Expand Down Expand Up @@ -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
Expand Down
80 changes: 70 additions & 10 deletions .../loam_velodyne/VelodyneScanRegistration.h → ...ude/loam_velodyne/MultiScanRegistration.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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.
Expand All @@ -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
138 changes: 26 additions & 112 deletions include/loam_velodyne/ScanRegistration.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
};



Expand Down Expand Up @@ -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.
*
Expand Down Expand Up @@ -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
Expand Down
Loading

0 comments on commit b8784a8

Please sign in to comment.