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

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), variance(0),
inliers(0), inliers(0),
matches(0), matches(0),
icpInliersRatio(0) icpInliersRatio(0),
icpTranslation(0.0f),
icpRotation(0.0f)
{ {
} }
@@ -33,6 +35,8 @@ public:
// RegistrationIcp // RegistrationIcp
float icpInliersRatio; 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; float ix,iy,iz, iroll,ipitch,iyaw;
Transform icpInTargetReferential = guess.inverse() * icpT.inverse() * guess; // actual local ICP refinement Transform icpInTargetReferential = guess.inverse() * icpT.inverse() * guess; // actual local ICP refinement
icpInTargetReferential.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw); 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 && if((_maxTranslation>0.0f &&
uMax3(fabs(ix), fabs(iy), fabs(iz)) > _maxTranslation) info.icpTranslation > _maxTranslation)
|| ||
(_maxRotation>0.0f && (_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)", 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)), info.icpTranslation,
uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw)), info.icpRotation,
_maxTranslation, _maxTranslation,
_maxRotation); _maxRotation);
UINFO(msg.c_str()); UINFO(msg.c_str());