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

Odom ICP: added min ratio gate to init first scan (#1768)

Browse Source
This commit is contained in:
matlabbe
2026-09-27 22:45:24 -07:00
committed by GitHub
parent b7079e050b
commit 8a06d53c83
2 changed files with 60 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/odometry/OdometryF2M.cpp
+19
View File
@@ -1469,6 +1469,25 @@ Transform OdometryF2M::computeTransform(
}
}
const int scanMaxPoints = lastFrame_->sensorData().laserScanRaw().maxPoints();
if(frameValid && scanMaxPoints > 0)
{
float correspondenceRatio = Parameters::defaultIcpCorrespondenceRatio();
Parameters::parse(parameters_, Parameters::kIcpCorrespondenceRatio(), correspondenceRatio);
if(float(lastFrame_->sensorData().laserScanRaw().size()) <
float(scanMaxPoints) * correspondenceRatio)
{
UWARN("Scan has %d points of the %d of a full sweep, under the %s=%f "
"that a registration against it would have to reach, so no "
"later scan could be matched to it. Not initializing on it.",
(int)lastFrame_->sensorData().laserScanRaw().size(),
scanMaxPoints,
Parameters::kIcpCorrespondenceRatio().c_str(),
correspondenceRatio);
frameValid = false;
}
}
if(frameValid)
{
if (scanMapMaxRange_ > 0 ){