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

Fixed loop closure rejected if the camera on the robot changes orientation accordingly to base frame (http://official-rtab-map-forum.67519.x6.nabble.com/Mapping-with-multiple-stereocameras-localization-with-multiple-monochrome-monocular-cameras-tp6647p6953.html). The reason why that check was there was because ICP flipping 180 deg in some cases, but should be already detected with that commit 7a1cf84b08 (diff-b65a61c23197f7ff77a877f62b630b2cR283).

Browse Source
This commit is contained in:
matlabbe
2020-09-27 14:38:36 -04:00
parent eef0a23b1b
commit c5158cade5
1 changed files with 0 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
corelib/src/Memory.cpp
View File

@@ -2980,36 +2980,6 @@ Transform Memory::computeTransform(
{ {
transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info); transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
} }
if(!transform.isNull() &&
fromS.sensorData().cameraModels().size()<=1 &&
toS.sensorData().cameraModels().size()<=1)
{
UDEBUG("");
// verify if it is a 180 degree transform, well verify > 90
float x,y,z, roll,pitch,yaw;
if(guess.isNull())
{
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
}
else
{
Transform guessError = guess.inverse() * transform;
guessError.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
}
if(fabs(pitch) > CV_PI/2 ||
fabs(yaw) > CV_PI/2)
{
transform.setNull();
std::string msg = uFormat("Too large rotation detected! (pitch=%f, yaw=%f) max is %f",
pitch, yaw, CV_PI/2);
UINFO(msg.c_str());
if(info)
{
info->rejectedMsg = msg;
}
}
}
} }
return transform; return transform;
} }