Skip to content

Commit

Permalink
Extracted some more constants.
Browse files Browse the repository at this point in the history 
* 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.
  • Loading branch information
@StefanGlaser
StefanGlaser committed Dec 9, 2017
1 parent 4e1fb82 commit 40b38fb
Show file tree
Hide file tree
Showing 9 changed files with 202 additions and 47 deletions.
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
3 changes: 0 additions & 3 deletions include/loam_velodyne/ScanRegistration.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
24 changes: 15 additions & 9 deletions launch/hector_loam_velodyne.launch
View file
Original file line number Diff line number Diff line change
@@ -1,21 +1,27 @@
<launch>

<arg name="rviz" default="true" />
<arg name="scanPeriod" default="0.1" />

<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>
<param name="lidar" value="VLP-16" /> <!-- options: VLP-16 HDL-32 HDL-64E -->
<param name="scanPeriod" value="$(arg scanPeriod)" />

<node pkg="loam_velodyne" type="laserOdometry" name="laserOdometry" output="screen" respawn="true"/>
<remap from="/multi_scan_points" to="/velodyne_points" />
<remap from="/imu/data" to="/raw_imu" />
<!-- <remap from="/imu/data" to="/djiros/imu" /> -->
</node>

<node pkg="loam_velodyne" type="laserOdometry" name="laserOdometry" output="screen" respawn="true">
<param name="scanPeriod" value="$(arg scanPeriod)" />
</node>

<node pkg="loam_velodyne" type="laserMapping" name="laserMapping" output="screen">
<param name="scanPeriod" value="$(arg scanPeriod)" />
</node>

<node pkg="loam_velodyne" type="laserMapping" name="laserMapping" output="screen"/>
<node pkg="loam_velodyne" type="transformMaintenance" name="transformMaintenance" output="screen"/>
<node pkg="loam_velodyne" type="transformMaintenance" name="transformMaintenance" output="screen">
</node>

<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find loam_velodyne)/rviz_cfg/loam_velodyne.rviz" />
Expand Down
13 changes: 10 additions & 3 deletions launch/loam_velodyne.launch
View file
Original file line number Diff line number Diff line change
@@ -1,18 +1,25 @@
<launch>

<arg name="rviz" default="true" />
<arg name="scanPeriod" default="0.1" />

<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" />
<param name="scanPeriod" value="$(arg scanPeriod)" />

<remap from="/multi_scan_points" to="/velodyne_points" />
</node>

<node pkg="loam_velodyne" type="laserOdometry" name="laserOdometry" output="screen" respawn="true">
<param name="scanPeriod" value="$(arg scanPeriod)" />
</node>

<node pkg="loam_velodyne" type="laserMapping" name="laserMapping" output="screen">
<param name="scanPeriod" value="$(arg scanPeriod)" />
</node>

<node pkg="loam_velodyne" type="laserMapping" name="laserMapping" output="screen"/>
<node pkg="loam_velodyne" type="transformMaintenance" name="transformMaintenance" output="screen"/>
<node pkg="loam_velodyne" type="transformMaintenance" name="transformMaintenance" output="screen">
</node>

<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find loam_velodyne)/rviz_cfg/loam_velodyne.rviz" />
Expand Down
86 changes: 83 additions & 3 deletions src/lib/LaserMapping.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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<sensor_msgs::PointCloud2> ("/laser_cloud_surround", 1);
_pubLaserCloudFullRes = node.advertise<sensor_msgs::PointCloud2> ("/velodyne_cloud_registered", 2);
_pubOdomAftMapped = node.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init", 5);


// subscribe to laser odometry topics
_subLaserCloudCornerLast = node.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_corner_last", 2, &LaserMapping::laserCloudCornerLastHandler, this);
Expand Down Expand Up @@ -779,7 +859,7 @@ void LaserMapping::optimizeTransformTobeMapped()
pcl::PointCloud<pcl::PointXYZI> laserCloudOri;
pcl::PointCloud<pcl::PointXYZI> coeffSel;

for (int iterCount = 0; iterCount < 10; iterCount++) {
for (size_t iterCount = 0; iterCount < _maxIterations; iterCount++) {
laserCloudOri.clear();
coeffSel.clear();

Expand Down Expand Up @@ -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;
}
}
Expand Down
66 changes: 63 additions & 3 deletions src/lib/LaserOdometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<pcl::PointXYZI>()),
_cornerPointsLessSharp(new pcl::PointCloud<pcl::PointXYZI>()),
_surfPointsFlat(new pcl::PointCloud<pcl::PointXYZI>()),
Expand All @@ -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<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
_pubLaserCloudSurfLast = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
_pubLaserCloudFullRes = node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 2);
_pubLaserOdometry = node.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 5);


// subscribe to scan registration topics
_subCornerPointsSharp = node.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_sharp", 2, &LaserOdometry::laserCloudSharpHandler, this);
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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;
}
}
Expand Down
5 changes: 4 additions & 1 deletion src/lib/MultiScanRegistration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)) {
Expand All @@ -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;
Expand All @@ -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);
}
}

Expand Down
Loading

0 comments on commit 40b38fb

Please sign in to comment.