i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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

12
corelib/src/Rtabmap.cpp
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,
false, _optimizedPoses, cov, &_constraints);
}
if(!_optimizedPoses.empty())
if(_optimizedPoses.lower_bound(1) != _optimizedPoses.end())
{
if(_restartAtOrigin)
{
UWARN("last localization pose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDStartAtOrigin().c_str());
lastPose = _optimizedPoses.begin()->second;
UWARN("last localization pose is ignored (%s=true), assuming we start at the first node of the map.", Parameters::kRGBDStartAtOrigin().c_str());
lastPose = _optimizedPoses.lower_bound(1)->second;
}
_lastLocalizationPose = lastPose;
UINFO("Loaded optimizedPoses=%d firstPose %d=%s lastLocalizationPose=%s",
_optimizedPoses.size(),
_optimizedPoses.begin()->first,
_optimizedPoses.begin()->second.prettyPrint().c_str(),
_optimizedPoses.lower_bound(1)->first,
_optimizedPoses.lower_bound(1)->second.prettyPrint().c_str(),
_lastLocalizationPose.prettyPrint().c_str());
if(_constraints.empty())
@@ -411,7 +411,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
UTimer time;
std::map<int, float> likelihood;
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))
{