From cad184e82bef25b90967ab88ebab5e99a7ce26bf Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 24 Feb 2021 09:43:22 -0500 Subject: [PATCH] Added Icp/PMForce4DoF parameter (works only with libpointmatcher > April 2020). Fixed some deprecated warnings. --- corelib/include/rtabmap/core/Parameters.h | 1 + .../include/rtabmap/core/RegistrationIcp.h | 1 + corelib/src/RegistrationIcp.cpp | 11 +++------ corelib/src/odometry/OdometryF2M.cpp | 6 ++--- corelib/src/util3d_surface.cpp | 14 +++++------ guilib/src/MainWindow.cpp | 20 ++++++++-------- guilib/src/PreferencesDialog.cpp | 1 + guilib/src/ui/preferencesDialog.ui | 24 +++++++++++++++++-- 8 files changed, 47 insertions(+), 31 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index f5d0c43899..d8a3ceb913 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -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."); diff --git a/corelib/include/rtabmap/core/RegistrationIcp.h b/corelib/include/rtabmap/core/RegistrationIcp.h index a665b0aa5b..d7576b72a6 100644 --- a/corelib/include/rtabmap/core/RegistrationIcp.h +++ b/corelib/include/rtabmap/core/RegistrationIcp.h @@ -78,6 +78,7 @@ class RTABMAP_EXP RegistrationIcp : public Registration float _libpointmatcherEpsilon; bool _libpointmatcherIntensity; float _libpointmatcherOutlierRatio; + bool _libpointmatcherForce4DoF; void * _libpointmatcherICP; }; diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index 3b8992aba9..e973b8a146 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -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); @@ -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) @@ -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 @@ -1029,7 +1032,6 @@ Transform RegistrationIcp::computeTransformationImpl( util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()), maxLaserScansFrom, fromScan.rangeMax(), - LaserScan::kXYINormal, fromScan.localTransform())); } else @@ -1039,7 +1041,6 @@ Transform RegistrationIcp::computeTransformationImpl( util3d::laserScanFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()), maxLaserScansFrom, fromScan.rangeMax(), - LaserScan::kXYZINormal, fromScan.localTransform())); } if(toScan.is2d()) @@ -1049,7 +1050,6 @@ Transform RegistrationIcp::computeTransformationImpl( util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()), maxLaserScansTo, toScan.rangeMax(), - LaserScan::kXYINormal, toScan.localTransform())); } else @@ -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()); @@ -1148,7 +1147,6 @@ Transform RegistrationIcp::computeTransformationImpl( util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()), maxLaserScansFrom, fromScan.rangeMax(), - LaserScan::kXYI, fromScan.localTransform())); } else @@ -1158,7 +1156,6 @@ Transform RegistrationIcp::computeTransformationImpl( util3d::laserScanFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()), maxLaserScansFrom, fromScan.rangeMax(), - LaserScan::kXYZI, fromScan.localTransform())); } if(toScan.is2d()) @@ -1168,7 +1165,6 @@ Transform RegistrationIcp::computeTransformationImpl( util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()), maxLaserScansTo, toScan.rangeMax(), - LaserScan::kXYI, toScan.localTransform())); } else @@ -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(); diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index 5c7759a54b..d8625e31d4 100644 --- a/corelib/src/odometry/OdometryF2M.cpp +++ b/corelib/src/odometry/OdometryF2M.cpp @@ -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; } @@ -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 @@ -1371,7 +1370,6 @@ Transform OdometryF2M::computeTransform( util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, - LaserScan::kXYZINormal, newFramePose.translation())); } diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index c03b2a1e8f..9288735418 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -2554,7 +2554,7 @@ LaserScan computeNormals( { UASSERT(!laserScan.is2d()); pcl::PointCloud::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()) @@ -2567,18 +2567,18 @@ LaserScan computeNormals( pcl::PointCloud::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::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()); } } } @@ -2592,17 +2592,17 @@ LaserScan computeNormals( pcl::PointCloud::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::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()); } } } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index d7c34ba5e6..09311f834b 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -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()) @@ -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()); } } } @@ -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()); } } } @@ -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()) @@ -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()); } } } @@ -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()); } } } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 1e65e45fca..2768650e9a 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -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()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index c63513f4ac..e18a2ab108 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - 0 + -976 686 3357 @@ -95,7 +95,7 @@ QFrame::Raised - 16 + 22 @@ -19504,6 +19504,26 @@ Lower the ratio -> higher the precision. + + + + Force 4 DoF: Limit ICP to x, y, z and yaw DoF. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + +