i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +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
corelib/src/Memory.cpp
+6 -14
View File
@@ -2138,10 +2138,14 @@ Transform Memory::computeTransform(
// Remove descriptors, this will avoid recomputation of the correspondences in regVis // Remove descriptors, this will avoid recomputation of the correspondences in regVis
tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>()); tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
tmpTo.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 // set back descriptors
tmpFrom.setWordsDescriptors(fromS.getWordsDescriptors()); tmpFrom.setWordsDescriptors(fromS.getWordsDescriptors());
tmpTo.setWordsDescriptors(toS.getWordsDescriptors()); tmpTo.setWordsDescriptors(toS.getWordsDescriptors());
if(!t.isNull())
{
guess = t;
}
} }
if(_reextractLoopClosureFeatures) if(_reextractLoopClosureFeatures)
@@ -2157,24 +2161,13 @@ Transform Memory::computeTransform(
tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat()); tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
} }
if(guess.isNull()) if(guess.isNull() && !_registrationPipeline->isImageRequired())
{
if(!_registrationPipeline->isImageRequired())
{ {
UDEBUG(""); UDEBUG("");
// no visual in the pipeline, make visual registration for guess // no visual in the pipeline, make visual registration for guess
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info); guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
} }
else
{
UDEBUG("");
guess.setIdentity();
}
}
if(!guess.isNull())
{
UDEBUG("");
transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info); transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
if(!transform.isNull()) if(!transform.isNull())
@@ -2198,7 +2191,6 @@ Transform Memory::computeTransform(
} }
} }
} }
}
return transform; return transform;
} }
corelib/src/Registration.cpp
+1 -1
View File
@@ -214,7 +214,7 @@ Transform Registration::computeTransformationMod(
} }
else if(!guess.isNull()) 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); t = child_->computeTransformationMod(from, to, guess, &info);
} }
} }
corelib/src/Rtabmap.cpp
+1 -1
View File
@@ -1017,7 +1017,7 @@ bool Rtabmap::process(
//============================================================ //============================================================
if(!signature->sensorData().laserScanCompressed().empty()) if(!signature->sensorData().laserScanCompressed().empty())
{ {
UINFO("Odometry refining"); UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
RegistrationInfo info; RegistrationInfo info;
Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info); Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
if(!t.isNull()) if(!t.isNull())