Skip to content

Commit

Permalink
Further code cleanup in scan registration components.
Browse files Browse the repository at this point in the history 
* Replaced scan start and end indices vectors with one std::pair vector.
* Removed _nScans variable from ScanRegistration class and replaced its usage with the size of the scan indices vector. This way feature extration will automatically run for all available scans.
* Extracted neighbor marking on feature selection into own function.
* Removed some duplicate code for IMU interpolation.
* Split interpolation of IMU state and point transformation into two functions and store all relevant information needed for the transformation. This way, multi-laser lidars can use the same IMU interpolation for registering all laser measurements to a given point in time.
* Removed point cloud getters.
* Automatically generate IMU transformation information cloud when publishing the result.
* Removed unnecessary shared pointer declaration of internal cloud buffers.
* Some minor simplifications.
  • Loading branch information
@StefanGlaser
StefanGlaser committed Dec 4, 2017
1 parent 55d90c4 commit 44fb3e4
Show file tree
Hide file tree
Showing 4 changed files with 221 additions and 254 deletions.
111 changes: 52 additions & 59 deletions include/loam_velodyne/ScanRegistration.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#define LOAM_SCANREGISTRATION_H


#include "common.h"
#include "loam_types.h"
#include "CircularBuffer.h"

Expand All @@ -46,6 +47,11 @@

namespace loam {

/** \brief A pair describing the start end end index of a range. */
typedef std::pair<size_t, size_t> IndexRange;



/** Point label options. */
enum PointLabel {
CORNER_SHARP = 2, ///< sharp corner point
Expand Down Expand Up @@ -252,9 +258,8 @@ typedef struct IMUState {
class ScanRegistration {
public:
explicit ScanRegistration(const float& scanPeriod,
const uint16_t& nScans = 0,
const size_t& imuHistorySize = 200,
const RegistrationParams& config = RegistrationParams());
const RegistrationParams& config = RegistrationParams(),
const size_t& imuHistorySize = 200);

/** \brief Setup component.
*
Expand All @@ -270,57 +275,30 @@ class ScanRegistration {
*/
virtual void handleIMUMessage(const sensor_msgs::Imu::ConstPtr& imuIn);

/** \brief Retrieve the current full resolution input cloud.
*
* @return the current full resolution input cloud
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr getFullResCloud() { return _laserCloud; };

/** \brief Retrieve the current sharp corner input sub cloud.
*
* @return the current sharp corner input sub cloud
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr getCornerPointsSharp() { return _cornerPointsSharp; };

/** \brief Retrieve the current less sharp corner input sub cloud.
*
* @return the current less sharp corner input sub cloud
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr getCornerPointsLessSharp() { return _cornerPointsLessSharp; };

/** \brief Retrieve the current flat surface input sub cloud.
*
* @return the current flat surface input sub cloud
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr getSurfacePointsFlat() { return _surfacePointsFlat; };

/** \brief Retrieve the current less flat surface input sub cloud.
*
* @return the current less flat surface input sub cloud
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr getSurfacePointsLessFlat() { return _surfacePointsLessFlat; };

/** \brief Retrieve the current IMU transformation information.
protected:
/** \brief Prepare for next scan / sweep.
*
* @return the current IMU transformation information
* @param scanTime the current scan time
* @param newSweep indicator if a new sweep has started
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr getIMUTrans() { return _imuTrans; };
void reset(const ros::Time& scanTime,
const bool& newSweep = true);

/** \breif Check is IMU data is available. */
inline bool hasIMUData() { return _imuHistory.size() > 0; };

protected:
/** \brief Prepare for next sweep: Reset internal cloud buffers and re-initialize start IMU state.
/** \brief Set up the current IMU transformation for the specified relative time.
*
* @param scanTime the current scan time
* @param relTime the time relative to the scan time
*/
void reset(const ros::Time& scanTime);
void setIMUTransformFor(const float& relTime);

/** \brief Project the given point to the start of the sweep, using the current IMU state and relative time.
/** \brief Project the given point to the start of the sweep, using the current IMU state and position shift.
*
* @param point the point to project
* @param relTime the relative point measurement time
*/
void transformToStartIMU(pcl::PointXYZI& point,
const float& relTime);
void transformToStartIMU(pcl::PointXYZI& point);

/** \brief Extract features from current laser cloud.
*
Expand All @@ -344,36 +322,51 @@ class ScanRegistration {
void setScanBuffersFor(const size_t& startIdx,
const size_t& endIdx);

/** \brief Set sweep end IMU transformation information.
/** \brief Mark a point and its neighbors as picked.
*
* This method will mark neighboring points within the curvature region as picked,
* as long as they remain within close distance to each other.
*
* @param sweepDuration the total duration of the current sweep
* @param cloudIdx the index of the picked point in the full resolution cloud
* @param scanIdx the index of the picked point relative to the current scan
*/
void setIMUTrans(const double& sweepDuration);
void markAsPicked(const size_t& cloudIdx,
const size_t& scanIdx);

/** \brief Publish the current result via the respective topics. */
void publishResult();


private:
/** \brief Try to interpolate the IMU state for the given time.
*
* @param relTime the time relative to the scan time
* @param outputState the output state instance
*/
void interpolateIMUStateFor(const float& relTime,
IMUState& outputState);


protected:
const uint16_t _nScans; ///< number of scans per sweep
const float _scanPeriod; ///< time per scan
ros::Time _sweepStamp; ///< time stamp of the beginning of current sweep
RegistrationParams _config; ///< registration parameter
const float _scanPeriod; ///< time per scan
RegistrationParams _config; ///< registration parameter

ros::Time _sweepStart; ///< time stamp of beginning of current sweep
ros::Time _scanTime; ///< time stamp of most recent scan
IMUState _imuStart; ///< the interpolated IMU state corresponding to the start time of the currently processed laser scan
IMUState _imuCur; ///< the interpolated IMU state corresponding to the time of the currently processed laser scan point
size_t _imuStartIdx; ///< the index in the IMU history of the first IMU state received after the current scan time
Vector3 _imuPositionShift; ///< position shift between accumulated IMU position and interpolated IMU position
size_t _imuIdx; ///< the current index in the IMU history
CircularBuffer<IMUState> _imuHistory; ///< history of IMU states for cloud registration

pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloud; ///< full resolution input cloud
std::vector<size_t> _scanStartIndices; ///< start indices of the individual scans
std::vector<size_t> _scanEndIndices; ///< end indices of the individual scans
pcl::PointCloud<pcl::PointXYZI> _laserCloud; ///< full resolution input cloud
std::vector<IndexRange> _scanIndices; ///< start and end indices of the individual scans withing the full resolution cloud

pcl::PointCloud<pcl::PointXYZI>::Ptr _cornerPointsSharp; ///< sharp corner points cloud
pcl::PointCloud<pcl::PointXYZI>::Ptr _cornerPointsLessSharp; ///< less sharp corner points cloud
pcl::PointCloud<pcl::PointXYZI>::Ptr _surfacePointsFlat; ///< flat surface points cloud
pcl::PointCloud<pcl::PointXYZI>::Ptr _surfacePointsLessFlat; ///< less flat surface points cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr _imuTrans; ///< IMU transformation information
pcl::PointCloud<pcl::PointXYZI> _cornerPointsSharp; ///< sharp corner points cloud
pcl::PointCloud<pcl::PointXYZI> _cornerPointsLessSharp; ///< less sharp corner points cloud
pcl::PointCloud<pcl::PointXYZI> _surfacePointsFlat; ///< flat surface points cloud
pcl::PointCloud<pcl::PointXYZI> _surfacePointsLessFlat; ///< less flat surface points cloud
pcl::PointCloud<pcl::PointXYZ> _imuTrans; ///< IMU transformation information

std::vector<float> _regionCurvature; ///< point curvature buffer
std::vector<PointLabel> _regionLabel; ///< point label buffer
Expand Down
14 changes: 8 additions & 6 deletions include/loam_velodyne/VelodyneScanRegistration.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,9 +47,9 @@ namespace loam {
class VelodyneScanRegistration : virtual public ScanRegistration {
public:
VelodyneScanRegistration(const float& scanPeriod,
const uint16_t& nScans,
const size_t& imuHistorySize = 200,
const RegistrationParams& config = RegistrationParams());
const uint16_t& nScanRings,
const RegistrationParams& config = RegistrationParams(),
const size_t& imuHistorySize = 200);


/** \brief Setup component in active mode.
Expand All @@ -60,11 +60,11 @@ class VelodyneScanRegistration : virtual public ScanRegistration {
bool setup(ros::NodeHandle& node,
ros::NodeHandle& privateNode);

/** \brief Process a new input cloud message.
/** \brief Handler method for input cloud messages.
*
* @param laserCloudMsg the new input cloud message to process
*/
void processCloudMessage(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg);
void handleCloudMessage(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg);

/** \brief Process a new input cloud.
*
Expand All @@ -75,7 +75,9 @@ class VelodyneScanRegistration : virtual public ScanRegistration {
const ros::Time& scanTime);

protected:
int _systemDelay; ///< system startup delay counter
int _systemDelay; ///< system startup delay counter
const uint16_t _nScanRings; ///< number of scan rings

ros::Subscriber _subLaserCloud; ///< input cloud message subscriber
};

Expand Down
Loading

0 comments on commit 44fb3e4

Please sign in to comment.