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

Added Icp/PMForce4DoF parameter (works only with libpointmatcher > April 2020). Fixed some deprecated warnings.

Browse Source
This commit is contained in:
matlabbe
2021-02-24 09:43:22 -05:00
parent d61e463595
commit cad184e82b
8 changed files with 47 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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.");

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

@@ -78,6 +78,7 @@ private:
float _libpointmatcherEpsilon;
bool _libpointmatcherIntensity;
float _libpointmatcherOutlierRatio;
bool _libpointmatcherForce4DoF;
void * _libpointmatcherICP;
};

11
corelib/src/RegistrationIcp.cpp
View File

@@ -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();

6
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -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()));
}

14
corelib/src/util3d_surface.cpp
View File

@@ -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())
@@ -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());
}
}
}
@@ -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());
}
}
}