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

Refactoring Odometry

Browse Source
This commit is contained in:
matlabbe
2016-01-07 13:59:21 -05:00
parent 923ba78bfa
commit 6d399b6fb9
18 changed files with 255 additions and 268 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+23 -2
View File
@@ -2076,6 +2076,27 @@ Transform Memory::computeTransform(
if(!guess.isNull())
{
transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
if(!transform.isNull())
{
UDEBUG("");
// verify if it is a 180 degree transform, well verify > 90
float x,y,z, roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(fabs(roll) > CV_PI/2 ||
fabs(pitch) > CV_PI/2 ||
fabs(yaw) > CV_PI/2)
{
transform.setNull();
std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
roll, pitch, yaw);
UINFO(msg.c_str());
if(info)
{
info->rejectedMsg = msg;
}
}
}
}
}
}
@@ -2084,7 +2105,7 @@ Transform Memory::computeTransform(
std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
if(info)
{
info->rejectedMsg_ = msg;
info->rejectedMsg = msg;
}
UWARN(msg.c_str());
}
@@ -2139,7 +2160,7 @@ Transform Memory::computeIcpTransform(
std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
if(info)
{
info->rejectedMsg_ = msg;
info->rejectedMsg = msg;
}
UWARN(msg.c_str());
}