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

RGBD/StartAtOrigin: set first node of the graph, not Identity

Browse Source
This commit is contained in:
matlabbe
2021-12-20 17:01:08 -05:00
parent 8662eb0dd7
commit 7ca881453e
1 changed files with 9 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/Rtabmap.cpp
View File

@@ -343,6 +343,8 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
_constraints.clear();
_globalScanMap.clear();
_globalScanMapPoses.clear();
_odomCachePoses.clear();
_odomCacheConstraints.clear();
// Parse all parameters
this->parseParameters(allParameters);
@@ -365,7 +367,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
if(_restartAtOrigin)
{
UWARN("last localization pose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDStartAtOrigin().c_str());
lastPose.setIdentity();
lastPose = _optimizedPoses.begin()->second;
}
_lastLocalizationPose = lastPose;
@@ -654,6 +656,12 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
{
this->createGlobalScanMap();
}
if(_memory->isIncremental())
{
_odomCachePoses.clear();
_odomCacheConstraints.clear();
}
}
if(!_epipolarGeometry)