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

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;
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);
}
if(!_optimizedPoses.empty())
{
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();
}
_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())
{