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

Memory::computeTransform(): removed setting guess to identity if input guess is null

Browse Source
This commit is contained in:
matlabbe
2016-05-19 17:01:46 -04:00
parent a8be08a19a
commit ff7406a755
3 changed files with 28 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
corelib/src/Memory.cpp
View File

@@ -2138,10 +2138,14 @@ Transform Memory::computeTransform(
// Remove descriptors, this will avoid recomputation of the correspondences in regVis
tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
Transform t = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
// set back descriptors
tmpFrom.setWordsDescriptors(fromS.getWordsDescriptors());
tmpTo.setWordsDescriptors(toS.getWordsDescriptors());
if(!t.isNull())
{
guess = t;
}
}
if(_reextractLoopClosureFeatures)
@@ -2157,44 +2161,32 @@ Transform Memory::computeTransform(
tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
}
if(guess.isNull())
{
if(!_registrationPipeline->isImageRequired())
{
UDEBUG("");
// no visual in the pipeline, make visual registration for guess
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
}
else
{
UDEBUG("");
guess.setIdentity();
}
}
if(!guess.isNull())
if(guess.isNull() && !_registrationPipeline->isImageRequired())
{
UDEBUG("");
transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
// no visual in the pipeline, make visual registration for guess
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
}
if(!transform.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)
{
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)
{
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;
}
info->rejectedMsg = msg;
}
}
}

2
corelib/src/Registration.cpp
View File

@@ -214,7 +214,7 @@ Transform Registration::computeTransformationMod(
}
else if(!guess.isNull())
{
// This registration approach failed, continue with the guess for the next registration
UDEBUG("This registration approach failed, continue with the guess for the next registration");
t = child_->computeTransformationMod(from, to, guess, &info);
}
}

2
corelib/src/Rtabmap.cpp
View File

@@ -1017,7 +1017,7 @@ bool Rtabmap::process(
//============================================================
if(!signature->sensorData().laserScanCompressed().empty())
{
UINFO("Odometry refining");
UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
RegistrationInfo info;
Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
if(!t.isNull())