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

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

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

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