i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

0.19.5: added Icp/RangeMin and Icp/RangeMax parameters

Browse Source
This commit is contained in:
matlabbe
2019-08-01 16:21:01 -04:00
parent 487caab349
commit 6767c2ade2
6 changed files with 101 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/Parameters.h
View File

@@ -608,6 +608,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Icp, MaxRotation, float, 0.78, "Maximum ICP rotation correction accepted (rad).");
RTABMAP_PARAM(Icp, VoxelSize, float, 0.0, "Uniform sampling voxel size (0=disabled).");
RTABMAP_PARAM(Icp, DownsamplingStep, int, 1, "Downsampling step size (1=no sampling). This is done before uniform sampling.");
RTABMAP_PARAM(Icp, RangeMin, float, 0, "Minimum range filtering (0=disabled).");
RTABMAP_PARAM(Icp, RangeMax, float, 0, "Maximum range filtering (0=disabled).");
#ifdef RTABMAP_POINTMATCHER
RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.1, "Max distance for point correspondences.");
#else

2
corelib/include/rtabmap/core/RegistrationIcp.h
View File

@@ -60,6 +60,8 @@ private:
float _maxRotation;
float _voxelSize;
int _downsamplingStep;
float _rangeMin;
float _rangeMax;
float _maxCorrespondenceDistance;
int _maxIterations;
float _epsilon;

12
corelib/src/RegistrationIcp.cpp
View File

@@ -368,6 +368,8 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
_maxRotation(Parameters::defaultIcpMaxRotation()),
_voxelSize(Parameters::defaultIcpVoxelSize()),
_downsamplingStep(Parameters::defaultIcpDownsamplingStep()),
_rangeMin(Parameters::defaultIcpRangeMin()),
_rangeMax(Parameters::defaultIcpRangeMax()),
_maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
_maxIterations(Parameters::defaultIcpIterations()),
_epsilon(Parameters::defaultIcpEpsilon()),
@@ -401,6 +403,8 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _maxRotation);
Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _voxelSize);
Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _downsamplingStep);
Parameters::parse(parameters, Parameters::kIcpRangeMin(), _rangeMin);
Parameters::parse(parameters, Parameters::kIcpRangeMax(), _rangeMax);
Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance);
Parameters::parse(parameters, Parameters::kIcpIterations(), _maxIterations);
Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
@@ -569,11 +573,11 @@ Transform RegistrationIcp::computeTransformationImpl(
// ICP with guess transform
LaserScan fromScan = dataFrom.laserScanRaw();
LaserScan toScan = dataTo.laserScanRaw();
if(_downsamplingStep>1)
if(_downsamplingStep>1 || _rangeMin >0.0f || _rangeMax > 0.0f)
{
fromScan = util3d::downsample(fromScan, _downsamplingStep);
toScan = util3d::downsample(toScan, _downsamplingStep);
UDEBUG("Downsampling time (step=%d) = %f s", _downsamplingStep, timer.ticks());
fromScan = util3d::commonFiltering(fromScan, _downsamplingStep, _rangeMin, _rangeMax);
toScan = util3d::commonFiltering(toScan, _downsamplingStep, _rangeMin, _rangeMax);
UDEBUG("Downsampling and/or range filtering time (step=%d, min=%fm, max=%fm) = %f s", _downsamplingStep, _rangeMin, _rangeMax, timer.ticks());
}
if(fromScan.size() && toScan.size())