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

Fixed RGBD/StartAtOrigin using landmark position

Browse Source
This commit is contained in:
matlabbe
2026-01-22 17:42:04 -08:00
parent da55168f60
commit e6bc39028a
1 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+6 -6
View File
@@ -385,19 +385,19 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
!_optimizeFromGraphEnd?_memory->getWorkingMem().lower_bound(1)->first:_memory->getWorkingMem().rbegin()->first, !_optimizeFromGraphEnd?_memory->getWorkingMem().lower_bound(1)->first:_memory->getWorkingMem().rbegin()->first,
false, _optimizedPoses, cov, &_constraints); false, _optimizedPoses, cov, &_constraints);
} }
if(!_optimizedPoses.empty()) if(_optimizedPoses.lower_bound(1) != _optimizedPoses.end())
{ {
if(_restartAtOrigin) if(_restartAtOrigin)
{ {
UWARN("last localization pose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDStartAtOrigin().c_str()); UWARN("last localization pose is ignored (%s=true), assuming we start at the first node of the map.", Parameters::kRGBDStartAtOrigin().c_str());
lastPose = _optimizedPoses.begin()->second; lastPose = _optimizedPoses.lower_bound(1)->second;
} }
_lastLocalizationPose = lastPose; _lastLocalizationPose = lastPose;
UINFO("Loaded optimizedPoses=%d firstPose %d=%s lastLocalizationPose=%s", UINFO("Loaded optimizedPoses=%d firstPose %d=%s lastLocalizationPose=%s",
_optimizedPoses.size(), _optimizedPoses.size(),
_optimizedPoses.begin()->first, _optimizedPoses.lower_bound(1)->first,
_optimizedPoses.begin()->second.prettyPrint().c_str(), _optimizedPoses.lower_bound(1)->second.prettyPrint().c_str(),
_lastLocalizationPose.prettyPrint().c_str()); _lastLocalizationPose.prettyPrint().c_str());
if(_constraints.empty()) if(_constraints.empty())
@@ -411,7 +411,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
UTimer time; UTimer time;
std::map<int, float> likelihood; std::map<int, float> likelihood;
likelihood.insert(std::make_pair(Memory::kIdVirtual, 1)); likelihood.insert(std::make_pair(Memory::kIdVirtual, 1));
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) for(std::map<int, Transform>::iterator iter=_optimizedPoses.lower_bound(1); iter!=_optimizedPoses.end(); ++iter)
{ {
if(_memory->getSignature(iter->first)) if(_memory->getSignature(iter->first))
{ {