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

Fixed ICP-only registration with already provided guess (no need to do visual guess, as the guess can be already good)

Browse Source
This commit is contained in:
matlabbe
2016-06-23 11:08:20 -04:00
parent 22766e958f
commit f2d48cb894
2 changed files with 7 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
corelib/src/Memory.cpp
View File

@@ -2124,32 +2124,13 @@ Transform Memory::computeTransform(
// compute transform fromId -> toId
std::vector<int> inliersV;
if(_reextractLoopClosureFeatures || (fromS.getWords().size() && toS.getWords().size()))
if(_reextractLoopClosureFeatures ||
(fromS.getWords().size() && toS.getWords().size()) ||
(!guess.isNull() && !_registrationPipeline->isImageRequired()))
{
Signature tmpFrom = fromS;
Signature tmpTo = toS;
// make a guess fast with known correspondences (if there are)
RegistrationVis regVis(parameters_);
if(tmpFrom.getWords().size() &&
tmpTo.getWords().size() &&
tmpFrom.getWords3().size() &&
tmpTo.getWords3().size())
{
UDEBUG("");
// 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>());
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)
{
UDEBUG("");
@@ -2167,6 +2148,7 @@ Transform Memory::computeTransform(
{
UDEBUG("");
// no visual in the pipeline, make visual registration for guess
RegistrationVis regVis(parameters_);
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
}

3
corelib/src/Rtabmap.cpp
View File

@@ -1834,7 +1834,8 @@ bool Rtabmap::process(
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
{
RegistrationInfo info;
Transform transform = _memory->computeTransform(signature->id(), nearestId, Transform(), &info);
Transform guess = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(nearestId);
Transform transform = _memory->computeTransform(signature->id(), nearestId, guess, &info);
if(!transform.isNull())
{
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)