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

RegistrationInfo: added IcpTranslation and IcpRotation members

Browse Source
This commit is contained in:
matlabbe
2016-07-05 11:23:59 -04:00
parent 1365eaca3a
commit 20862f07bc
2 changed files with 11 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/include/rtabmap/core/RegistrationInfo.h
View File

@@ -18,7 +18,9 @@ public:
variance(0),
inliers(0),
matches(0),
icpInliersRatio(0)
icpInliersRatio(0),
icpTranslation(0.0f),
icpRotation(0.0f)
{
}
@@ -33,6 +35,8 @@ public:
// RegistrationIcp
float icpInliersRatio;
float icpTranslation;
float icpRotation;
};
}

10
corelib/src/RegistrationIcp.cpp
View File

@@ -313,15 +313,17 @@ Transform RegistrationIcp::computeTransformationImpl(
float ix,iy,iz, iroll,ipitch,iyaw;
Transform icpInTargetReferential = guess.inverse() * icpT.inverse() * guess; // actual local ICP refinement
icpInTargetReferential.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
info.icpTranslation = uMax3(fabs(ix), fabs(iy), fabs(iz));
info.icpRotation = uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw));
if((_maxTranslation>0.0f &&
uMax3(fabs(ix), fabs(iy), fabs(iz)) > _maxTranslation)
info.icpTranslation > _maxTranslation)
||
(_maxRotation>0.0f &&
uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw)) > _maxRotation))
info.icpRotation > _maxRotation))
{
msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
uMax3(fabs(ix), fabs(iy), fabs(iz)),
uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw)),
info.icpTranslation,
info.icpRotation,
_maxTranslation,
_maxRotation);
UINFO(msg.c_str());