diff --git a/CMakeLists.txt b/CMakeLists.txt
index 42688593..f02de05f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 19)
-SET(RTABMAP_PATCH_VERSION 4)
+SET(RTABMAP_PATCH_VERSION 5)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h
index bfaf178a..51f1bda9 100644
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -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
diff --git a/corelib/include/rtabmap/core/RegistrationIcp.h b/corelib/include/rtabmap/core/RegistrationIcp.h
index 25a44138..a1c23bec 100644
--- a/corelib/include/rtabmap/core/RegistrationIcp.h
+++ b/corelib/include/rtabmap/core/RegistrationIcp.h
@@ -60,6 +60,8 @@ private:
float _maxRotation;
float _voxelSize;
int _downsamplingStep;
+ float _rangeMin;
+ float _rangeMax;
float _maxCorrespondenceDistance;
int _maxIterations;
float _epsilon;
diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp
index dd9b7132..2e2f5301 100644
--- a/corelib/src/RegistrationIcp.cpp
+++ b/corelib/src/RegistrationIcp.cpp
@@ -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())
diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp
index caae8f40..186d58c4 100644
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -1014,6 +1014,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kIcpMaxRotation().c_str());
_ui->loopClosure_icpVoxelSize->setObjectName(Parameters::kIcpVoxelSize().c_str());
_ui->loopClosure_icpDownsamplingStep->setObjectName(Parameters::kIcpDownsamplingStep().c_str());
+ _ui->loopClosure_icpRangeMin->setObjectName(Parameters::kIcpRangeMin.c_str());
+ _ui->loopClosure_icpRangeMax->setObjectName(Parameters::kIcpRangeMax.c_str());
_ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kIcpMaxCorrespondenceDistance().c_str());
_ui->loopClosure_icpIterations->setObjectName(Parameters::kIcpIterations().c_str());
_ui->loopClosure_icpEpsilon->setObjectName(Parameters::kIcpEpsilon().c_str());
diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui
index 1922c562..6353b491 100644
--- a/guilib/src/ui/preferencesDialog.ui
+++ b/guilib/src/ui/preferencesDialog.ui
@@ -64,8 +64,8 @@
0
0
- 677
- 2974
+ 680
+ 3082
@@ -95,7 +95,7 @@
QFrame::Raised
- 5
+ 22
@@ -17539,7 +17539,7 @@ Lower the ratio -> higher the precision.
-
-
-
+
-
3
@@ -17587,14 +17587,14 @@ Lower the ratio -> higher the precision.
- -
+
-
0.010000000000000
- -
+
-
@@ -17614,7 +17614,7 @@ Lower the ratio -> higher the precision.
- -
+
-
Max distance for point correspondences.
@@ -17640,7 +17640,7 @@ Lower the ratio -> higher the precision.
- -
+
-
0.000000000000000
@@ -17656,7 +17656,7 @@ Lower the ratio -> higher the precision.
- -
+
-
Ratio of matching correspondences to accept the transform. If the maximum laser scans is set, this is the minimum ratio of correspondences on laser scan maximum size to accept the transform.
@@ -17669,7 +17669,7 @@ Lower the ratio -> higher the precision.
- -
+
-
Number of neighbors to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans.
@@ -17682,7 +17682,7 @@ Lower the ratio -> higher the precision.
- -
+
-
1
@@ -17714,7 +17714,7 @@ Lower the ratio -> higher the precision.
- -
+
-
Max iterations.
@@ -17727,7 +17727,7 @@ Lower the ratio -> higher the precision.
- -
+
-
1
@@ -17791,7 +17791,7 @@ Lower the ratio -> higher the precision.
- -
+
-
Point to plane ICP.
@@ -17804,7 +17804,7 @@ Lower the ratio -> higher the precision.
- -
+
-
Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.
@@ -17817,7 +17817,7 @@ Lower the ratio -> higher the precision.
- -
+
-
m
@@ -17839,7 +17839,7 @@ Lower the ratio -> higher the precision.
- -
+
-
Search radius to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans.
@@ -17852,7 +17852,7 @@ Lower the ratio -> higher the precision.
- -
+
-
Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.
@@ -17865,7 +17865,7 @@ Lower the ratio -> higher the precision.
- -
+
-
3
@@ -17878,6 +17878,73 @@ Lower the ratio -> higher the precision.
+ -
+
+
+ m
+
+
+ 2
+
+
+ 0.000000000000000
+
+
+ 1.000000000000000
+
+
+ 0.000000000000000
+
+
+
+ -
+
+
+ Minimum range filtering. Set to 0 to disable.
+
+
+ true
+
+
+ Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse
+
+
+
+ -
+
+
+ Maximum range filtering. Set to 0 to disable.
+
+
+ true
+
+
+ Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse
+
+
+
+ -
+
+
+ m
+
+
+ 2
+
+
+ 0.000000000000000
+
+
+ 999.000000000000000
+
+
+ 1.000000000000000
+
+
+ 0.000000000000000
+
+
+
-