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

OdomF2M: fixed complexity null for first scan is not having normals already

Browse Source
This commit is contained in:
matlabbe
2020-08-21 14:32:08 -04:00
parent 729f96f467
commit f263d560b4
1 changed files with 10 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1271,9 +1271,17 @@ Transform OdometryF2M::computeTransform(
Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity); Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
if(p2n && minComplexity>0.0f) if(p2n && minComplexity>0.0f)
{ {
complexity = util3d::computeNormalsComplexity(*mapCloudNormals, Transform::getIdentity(), lastFrame_->sensorData().laserScanRaw().is2d()); if(lastFrame_->sensorData().laserScanRaw().hasNormals())
if(complexity > minComplexity)
{ {
complexity = util3d::computeNormalsComplexity(*mapCloudNormals, Transform::getIdentity(), lastFrame_->sensorData().laserScanRaw().is2d());
if(complexity > minComplexity)
{
frameValid = true;
}
}
else
{
UWARN("Input raw scan doesn't have normals, complexity check on first frame is not done.");
frameValid = true; frameValid = true;
} }
} }