Skip to content

Commit

Permalink
Added Icp/PMForce4DoF parameter (works only with libpointmatcher > Ap…
Browse files Browse the repository at this point in the history 
…ril 2020). Fixed some deprecated warnings.
  • Loading branch information
@matlabbe
matlabbe committed Feb 24, 2021
1 parent d61e463 commit cad184e
Show file tree
Hide file tree
Showing 8 changed files with 47 additions and 31 deletions.
1 change: 1 addition & 0 deletions corelib/include/rtabmap/core/Parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -672,6 +672,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Icp, PMMatcherEpsilon, float, 0.0, "KDTreeMatcher/epsilon: approximation to use for the nearest-neighbor search. For convenience when configuration file is not set.");
RTABMAP_PARAM(Icp, PMMatcherIntensity, bool, false, uFormat("KDTreeMatcher: among nearest neighbors, keep only the one with the most similar intensity. This only work with %s>1.", kIcpPMMatcherKnn().c_str()));
RTABMAP_PARAM(Icp, PMOutlierRatio, float, 0.85, "TrimmedDistOutlierFilter/ratio: For convenience when configuration file is not set. For kinect-like point cloud, use 0.65.");
RTABMAP_PARAM(Icp, PMForce4DoF, bool, false, "Limit ICP to x, y, z and yaw DoF.");

// Stereo disparity
RTABMAP_PARAM(Stereo, WinWidth, int, 15, "Window width.");
Expand Down
1 change: 1 addition & 0 deletions corelib/include/rtabmap/core/RegistrationIcp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,7 @@ class RTABMAP_EXP RegistrationIcp : public Registration
float _libpointmatcherEpsilon;
bool _libpointmatcherIntensity;
float _libpointmatcherOutlierRatio;
bool _libpointmatcherForce4DoF;
void * _libpointmatcherICP;
};

Expand Down
11 changes: 3 additions & 8 deletions corelib/src/RegistrationIcp.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -494,6 +494,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
_libpointmatcherEpsilon(Parameters::defaultIcpPMMatcherEpsilon()),
_libpointmatcherIntensity(Parameters::defaultIcpPMMatcherIntensity()),
_libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
_libpointmatcherForce4DoF(Parameters::defaultIcpPMForce4DoF()),
_libpointmatcherICP(0)
{
this->parseParameters(parameters);
Expand Down Expand Up @@ -535,6 +536,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpPMMatcherKnn(), _libpointmatcherKnn);
Parameters::parse(parameters, Parameters::kIcpPMMatcherEpsilon(), _libpointmatcherEpsilon);
Parameters::parse(parameters, Parameters::kIcpPMMatcherIntensity(), _libpointmatcherIntensity);
Parameters::parse(parameters, Parameters::kIcpPMForce4DoF(), _libpointmatcherForce4DoF);

#ifndef RTABMAP_POINTMATCHER
if(_libpointmatcher)
Expand Down Expand Up @@ -613,6 +615,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
params.clear();

params["force2D"] = force3DoF()?"1":"0";
params["force4DOF"] = !force3DoF()&&_libpointmatcherForce4DoF?"1":"0";
#if POINTMATCHER_VERSION_INT >= 10300
icp->errorMinimizer = PM::get().ErrorMinimizerRegistrar.create("PointToPlaneErrorMinimizer", params);
#else
Expand Down Expand Up @@ -1029,7 +1032,6 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.rangeMax(),
LaserScan::kXYINormal,
fromScan.localTransform()));
}
else
Expand All @@ -1039,7 +1041,6 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScanFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.rangeMax(),
LaserScan::kXYZINormal,
fromScan.localTransform()));
}
if(toScan.is2d())
Expand All @@ -1049,7 +1050,6 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.rangeMax(),
LaserScan::kXYINormal,
toScan.localTransform()));
}
else
Expand All @@ -1059,7 +1059,6 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.rangeMax(),
LaserScan::kXYZINormal,
toScan.localTransform()));
}
UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
Expand Down Expand Up @@ -1148,7 +1147,6 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.rangeMax(),
LaserScan::kXYI,
fromScan.localTransform()));
}
else
Expand All @@ -1158,7 +1156,6 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScanFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.rangeMax(),
LaserScan::kXYZI,
fromScan.localTransform()));
}
if(toScan.is2d())
Expand All @@ -1168,7 +1165,6 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.rangeMax(),
LaserScan::kXYI,
toScan.localTransform()));
}
else
Expand All @@ -1178,7 +1174,6 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.rangeMax(),
LaserScan::kXYZI,
toScan.localTransform()));
}
fromScan = fromSignature.sensorData().laserScanRaw();
Expand Down
6 changes: 2 additions & 4 deletions corelib/src/odometry/OdometryF2M.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1093,12 +1093,12 @@ Transform OdometryF2M::computeTransform(
if(mapScan.is2d())
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYINormal);
mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f);
}
else
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYZINormal);
mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f);
}
modified=true;
}
Expand Down Expand Up @@ -1360,7 +1360,6 @@ Transform OdometryF2M::computeTransform(
util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
0,
0.0f,
LaserScan::kXYINormal,
Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
}
else
Expand All @@ -1371,7 +1370,6 @@ Transform OdometryF2M::computeTransform(
util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
0,
0.0f,
LaserScan::kXYZINormal,
newFramePose.translation()));
}

Expand Down
14 changes: 7 additions & 7 deletions corelib/src/util3d_surface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2554,7 +2554,7 @@ LaserScan computeNormals(
{
UASSERT(!laserScan.is2d());
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
}
}
else if(laserScan.hasIntensity())
Expand All @@ -2567,18 +2567,18 @@ LaserScan computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
if(laserScan.angleIncrement() > 0.0f)
{
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYINormal, laserScan.rangeMin(), laserScan.rangeMax(), laserScan.angleMin(), laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform());
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.rangeMin(), laserScan.rangeMax(), laserScan.angleMin(), laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform());
}
else
{
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYINormal, laserScan.localTransform());
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
}

}
else
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYZINormal, laserScan.localTransform());
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
}
}
}
Expand All @@ -2592,17 +2592,17 @@ LaserScan computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
if(laserScan.angleIncrement() > 0.0f)
{
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYNormal, laserScan.rangeMin(), laserScan.rangeMax(), laserScan.angleMin(), laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform());
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.rangeMin(), laserScan.rangeMax(), laserScan.angleMin(), laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform());
}
else
{
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYNormal, laserScan.localTransform());
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
}
}
else
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYZNormal, laserScan.localTransform());
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
}
}
}
Expand Down
20 changes: 10 additions & 10 deletions guilib/src/MainWindow.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3742,7 +3742,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
added = _cloudViewer->addCloud(scanName, cloudRGBWithNormals, pose, color);
if(added && nodeId > 0)
{
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGBWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZRGBNormal, scan.localTransform());
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGBWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
}
}
else if(cloudIWithNormals.get())
Expand All @@ -3752,11 +3752,11 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
{
if(scan.is2d())
{
scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYINormal, scan.localTransform());
scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
}
else
{
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZINormal, scan.localTransform());
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
}
}
}
Expand All @@ -3767,11 +3767,11 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
{
if(scan.is2d())
{
scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYNormal, scan.localTransform());
scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
}
else
{
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZNormal, scan.localTransform());
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
}
}
}
Expand All @@ -3780,7 +3780,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
added = _cloudViewer->addCloud(scanName, cloudRGB, pose, color);
if(added && nodeId > 0)
{
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGB, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZRGB, scan.localTransform());
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGB, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
}
}
else if(cloudI.get())
Expand All @@ -3790,11 +3790,11 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
{
if(scan.is2d())
{
scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYI, scan.localTransform());
scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
}
else
{
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZI, scan.localTransform());
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
}
}
}
Expand All @@ -3806,11 +3806,11 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
{
if(scan.is2d())
{
scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXY, scan.localTransform());
scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
}
else
{
scan = LaserScan(util3d::laserScanFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZ, scan.localTransform());
scan = LaserScan(util3d::laserScanFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), scan.localTransform());
}
}
}
Expand Down
1 change: 1 addition & 0 deletions guilib/src/PreferencesDialog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1153,6 +1153,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->spinBox_icpPMMatcherKnn->setObjectName(Parameters::kIcpPMMatcherKnn().c_str());
_ui->doubleSpinBox_icpPMMatcherEpsilon->setObjectName(Parameters::kIcpPMMatcherEpsilon().c_str());
_ui->loopClosure_icpPMMatcherIntensity->setObjectName(Parameters::kIcpPMMatcherIntensity().c_str());
_ui->loopClosure_icpPMForce4DoF->setObjectName(Parameters::kIcpPMForce4DoF().c_str());

// Occupancy grid
_ui->groupBox_grid_3d->setObjectName(Parameters::kGrid3D().c_str());
Expand Down
24 changes: 22 additions & 2 deletions guilib/src/ui/preferencesDialog.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-976</y>
<width>686</width>
<height>3357</height>
</rect>
Expand Down Expand Up @@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>16</number>
<number>22</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
Expand Down Expand Up @@ -19504,6 +19504,26 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_617">
<property name="text">
<string>Force 4 DoF: Limit ICP to x, y, z and yaw DoF.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="loopClosure_icpPMForce4DoF">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
Expand Down

1 comment on commit cad184e

@dagata-mining
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nice!

Please sign in to comment.