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

Localization: fixed initial graph optimization root id (based on RGBD/OptimizeFromGraphEnd) when optimized graph is not saved in db.

Browse Source
This commit is contained in:
matlabbe
2021-07-09 14:57:15 -04:00
parent 456560b9e4
commit 7338cb8a85
1 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/Rtabmap.cpp
View File

@@ -357,19 +357,23 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
{ {
cv::Mat cov; cv::Mat cov;
this->optimizeCurrentMap( this->optimizeCurrentMap(
_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.empty())
{ {
if(_restartAtOrigin) if(_restartAtOrigin)
{ {
UINFO("lastPose 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 origin of the map.", Parameters::kRGBDStartAtOrigin().c_str());
lastPose.setIdentity(); lastPose.setIdentity();
} }
_lastLocalizationPose = lastPose; _lastLocalizationPose = lastPose;
UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), _lastLocalizationPose.prettyPrint().c_str()); UINFO("Loaded optimizedPoses=%d firstPose %d=%s lastLocalizationPose=%s",
_optimizedPoses.size(),
_optimizedPoses.begin()->first,
_optimizedPoses.begin()->second.prettyPrint().c_str(),
_lastLocalizationPose.prettyPrint().c_str());
if(_constraints.empty()) if(_constraints.empty())
{ {