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

Refactored RegistrationIcp: libpointmatcher yaml config usage / integrated CCCoreLib (#704)

Browse Source 
* Refactored RegistrationIcp so that libpointmatcher yaml can work with icp odometry (we can then avoid refiltering data with local map of F2M). All data filtering (including libpointmatcher DataFilters) are done at the beginning of the function.

* ICP: Restored ref and data scans order for libpointmatcher (seems more stable this way).

* Fixed compilation error without libpointmatcher

* CCCoreLib integration (Icp/Strategy=2). Icp/PMForce4DoF is now Icp/Force4DoF. Icp/PM is now Icp/Strategy. Icp/PMOutlierRatio is now Icp/OutlierRatio.

* Fixed build without CCCoreLib

* Cleanup RegistrationIcp from third party functions.

* Preferences: disable libpointmatcher and cccorlib options if not available
This commit is contained in:
matlabbe
2021-03-27 15:11:20 -04:00
committed by GitHub
parent c4d127cae4
commit 06e85e140c
18 changed files with 3153 additions and 2686 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/odometry/OdometryF2F.cpp
+11 -6
View File
@@ -99,7 +99,7 @@ Transform OdometryF2F::computeTransform(
if(refFrame_.sensorData().isValid())
{
float maxCorrespondenceDistance = 0.0f;
float pmOutlierRatio = 0.0f;
float outlierRatio = 0.0f;
if(guess.isNull() &&
!registrationPipeline_->isImageRequired() &&
registrationPipeline_->isScanRequired() &&
@@ -107,12 +107,12 @@ Transform OdometryF2F::computeTransform(
{
// only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
pmOutlierRatio = Parameters::defaultIcpPMOutlierRatio();
outlierRatio = Parameters::defaultIcpOutlierRatio();
Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
Parameters::parse(parameters_, Parameters::kIcpPMOutlierRatio(), pmOutlierRatio);
Parameters::parse(parameters_, Parameters::kIcpOutlierRatio(), outlierRatio);
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(0.95f)));
params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), uNumber2Str(0.95f)));
registrationPipeline_->parseParameters(params);
}
@@ -129,7 +129,7 @@ Transform OdometryF2F::computeTransform(
// set it back
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(pmOutlierRatio)));
params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), uNumber2Str(outlierRatio)));
registrationPipeline_->parseParameters(params);
}
@@ -240,15 +240,19 @@ Transform OdometryF2F::computeTransform(
{
UDEBUG("Update key frame");
int features = newFrame.getWordsDescriptors().rows;
if(registrationPipeline_->isImageRequired() && features == 0)
if(!refFrame_.sensorData().isValid())
{
newFrame = Signature(data);
// this will generate features only for the first frame or if optical flow was used (no 3d words)
// For scan, we want to use reading filters, so set dummy's scan and set back to reference afterwards
Signature dummy;
dummy.sensorData().setLaserScan(newFrame.sensorData().laserScanRaw());
newFrame.sensorData().setLaserScan(LaserScan());
registrationPipeline_->computeTransformationMod(
newFrame,
dummy);
features = (int)newFrame.sensorData().keypoints().size();
newFrame.sensorData().setLaserScan(dummy.sensorData().laserScanRaw(), true);
}
if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&
@@ -295,6 +299,7 @@ Transform OdometryF2F::computeTransform(
}
data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().keypoints3D(), newFrame.sensorData().descriptors());
data.setLaserScan(newFrame.sensorData().laserScanRaw());
if(info)
{
corelib/src/odometry/OdometryF2M.cpp
+17 -14
View File
@@ -268,7 +268,7 @@ Transform OdometryF2M::computeTransform(
bundleModels.clear();
float maxCorrespondenceDistance = 0.0f;
float pmOutlierRatio = 0.0f;
float outlierRatio = 0.0f;
if(guess.isNull() &&
!regPipeline_->isImageRequired() &&
regPipeline_->isScanRequired() &&
@@ -276,12 +276,12 @@ Transform OdometryF2M::computeTransform(
{
// only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
pmOutlierRatio = Parameters::defaultIcpPMOutlierRatio();
outlierRatio = Parameters::defaultIcpOutlierRatio();
Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
Parameters::parse(parameters_, Parameters::kIcpPMOutlierRatio(), pmOutlierRatio);
Parameters::parse(parameters_, Parameters::kIcpOutlierRatio(), outlierRatio);
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(0.95f)));
params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), uNumber2Str(0.95f)));
regPipeline_->parseParameters(params);
}
@@ -302,11 +302,12 @@ Transform OdometryF2M::computeTransform(
// set it back
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(pmOutlierRatio)));
params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), uNumber2Str(outlierRatio)));
regPipeline_->parseParameters(params);
}
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
data.setLaserScan(lastFrame_->sensorData().laserScanRaw());
UDEBUG("Registration time = %fs", regInfo.totalTime);
if(!transform.isNull())
@@ -950,7 +951,7 @@ Transform OdometryF2M::computeTransform(
}
else
{
newPoints = mapCloudNormals->size();
newPoints = frameCloudNormals->size();
}
if(newPoints)
@@ -1124,16 +1125,18 @@ Transform OdometryF2M::computeTransform(
}
else
{
// just generate keypoints for the new signature
if(regPipeline_->isImageRequired())
{
Signature dummy;
regPipeline_->computeTransformationMod(
*lastFrame_,
dummy);
}
// Just generate keypoints for the new signature
// For scan, we want to use reading filters, so set dummy's scan and set back to reference afterwards
Signature dummy;
dummy.sensorData().setLaserScan(lastFrame_->sensorData().laserScanRaw());
lastFrame_->sensorData().setLaserScan(LaserScan());
regPipeline_->computeTransformationMod(
*lastFrame_,
dummy);
lastFrame_->sensorData().setLaserScan(dummy.sensorData().laserScanRaw());
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
data.setLaserScan(lastFrame_->sensorData().laserScanRaw());
// a very high variance tells that the new pose is not linked with the previous one
regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;