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

RGBD/SavedLocalizationIgnored: if true, it is now starting at the origin (0,0,0) instead of null (not linked to graph).

Browse Source
This commit is contained in:
matlabbe
2020-04-23 11:57:01 -04:00
parent 046e0b7888
commit 00d8375a2e
5 changed files with 41 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+12 -4
View File
@@ -318,17 +318,25 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
Transform lastPose;
_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), lastPose.prettyPrint().c_str());
if(!_optimizedPoses.empty())
{
if(!_savedLocalizationIgnored)
if(_savedLocalizationIgnored)
{
_lastLocalizationPose = lastPose;
UDEBUG("lastPose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDSavedLocalizationIgnored().c_str());
lastPose.setIdentity();
}
_lastLocalizationPose = lastPose;
UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), _lastLocalizationPose.prettyPrint().c_str());
std::map<int, Transform> tmp;
// Get just the links
_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
}
else
{
UINFO("Loaded optimizedPoses=0, last localization pose is ignored!");
}
if(_databasePath.empty())
{
@@ -5015,7 +5023,7 @@ bool Rtabmap::computePath(int targetNode, bool global)
{
if(_lastLocalizationPose.isNull() || _optimizedPoses.empty())
{
UWARN("Last localization pose is null... cannot compute a path");
UWARN("Last localization pose is null or optimized graph is empty... cannot compute a path");
return false;
}
if(_optimizedPoses.begin()->first < 0)