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

DBViewer: added optimzed and prior poses in node details. Reprocess: add start_s, stop_s and pub_loops options.

Browse Source
This commit is contained in:
matlabbe
2022-10-12 13:05:01 -07:00
parent 2cbd43dfb6
commit fdb78d33b0
8 changed files with 627 additions and 370 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/include/rtabmap/core/DBReader.h
View File

@@ -55,7 +55,9 @@ public:
int stopId = 0,
bool intermediateNodesIgnored = false,
bool landmarksIgnored = false,
bool featuresIgnored = false);
bool featuresIgnored = false,
int startMapId = 0,
int stopMapId = -1);
DBReader(const std::list<std::string> & databasePaths,
float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
bool odometryIgnored = false,
@@ -66,7 +68,9 @@ public:
int stopId = 0,
bool intermediateNodesIgnored = false,
bool landmarksIgnored = false,
bool featuresIgnored = false);
bool featuresIgnored = false,
int startMapId = 0,
int stopMapId = -1);
virtual ~DBReader();
virtual bool init(
@@ -77,6 +81,8 @@ public:
virtual std::string getSerial() const;
virtual bool odomProvided() const {return !_odometryIgnored;}
const DBDriver * driver() const {return _dbDriver;}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
@@ -94,6 +100,8 @@ private:
bool _intermediateNodesIgnored;
bool _landmarksIgnored;
bool _featuresIgnored;
int _startMapId;
int _stopMapId;
DBDriver * _dbDriver;
UTimer _timer;

1
corelib/src/DBDriverSqlite3.cpp
View File

@@ -500,6 +500,7 @@ void DBDriverSqlite3::disconnectDatabaseQuery(bool save, const std::string & out
UERROR("Failed to rename just closed db %s to %s", this->getUrl().c_str(), outputUrl.c_str());
}
}
UINFO("Disconnected database %s!", this->getUrl().c_str());
}
}

62
corelib/src/DBReader.cpp
View File

@@ -52,7 +52,9 @@ DBReader::DBReader(const std::string & databasePath,
int stopId,
bool intermediateNodesIgnored,
bool landmarksIgnored,
bool featuresIgnored) :
bool featuresIgnored,
int startMapId,
int stopMapId) :
Camera(frameRate),
_paths(uSplit(databasePath, ';')),
_odometryIgnored(odometryIgnored),
@@ -64,6 +66,8 @@ DBReader::DBReader(const std::string & databasePath,
_intermediateNodesIgnored(intermediateNodesIgnored),
_landmarksIgnored(landmarksIgnored),
_featuresIgnored(featuresIgnored),
_startMapId(startMapId),
_stopMapId(stopMapId),
_dbDriver(0),
_currentId(_ids.end()),
_previousMapId(-1),
@@ -75,6 +79,11 @@ DBReader::DBReader(const std::string & databasePath,
{
_stopId = _startId;
}
if(_stopMapId>-1 && _stopMapId<_startMapId)
{
_stopMapId = _startMapId;
}
}
DBReader::DBReader(const std::list<std::string> & databasePaths,
@@ -87,7 +96,9 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
int stopId,
bool intermediateNodesIgnored,
bool landmarksIgnored,
bool featuresIgnored) :
bool featuresIgnored,
int startMapId,
int stopMapId) :
Camera(frameRate),
_paths(databasePaths),
_odometryIgnored(odometryIgnored),
@@ -99,6 +110,8 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
_intermediateNodesIgnored(intermediateNodesIgnored),
_landmarksIgnored(landmarksIgnored),
_featuresIgnored(featuresIgnored),
_startMapId(startMapId),
_stopMapId(stopMapId),
_dbDriver(0),
_currentId(_ids.end()),
_previousMapId(-1),
@@ -110,6 +123,11 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
{
_stopId = _startId;
}
if(_stopMapId>-1 && _stopMapId<_startMapId)
{
_stopMapId = _startMapId;
}
}
DBReader::~DBReader()
@@ -368,6 +386,15 @@ SensorData DBReader::getNextData(CameraInfo * info)
if(_intermediateNodesIgnored && s->getWeight() == -1)
{
UDEBUG("Ignoring node %d (intermediate nodes ignored)", s->id());
++_currentId;
delete s;
continue;
}
if(s->mapId() < _startMapId || (_stopMapId>=0 && s->mapId() > _stopMapId))
{
UDEBUG("Ignoring node %d (map id=%d, min=%d max=%d)", s->id(), s->mapId(), _startMapId, _stopMapId);
++_currentId;
delete s;
continue;
@@ -439,15 +466,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
}
else
{
// if localization data saved in database, covariance will be set in a prior link
_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
if(links.size())
{
// assume the first is the backward neighbor, take its variance
infMatrix = links.begin()->second.infMatrix();
_previousInfMatrix = infMatrix;
}
else if(_previousMapId != s->mapId())
if(_previousMapId != s->mapId())
{
// first node, set high variance to make rtabmap trigger a new map
infMatrix /= 9999.0;
@@ -455,12 +474,23 @@ SensorData DBReader::getNextData(CameraInfo * info)
}
else
{
if(_previousInfMatrix.empty())
// if localization data saved in database, covariance will be set in a prior link
_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
if(links.size())
{
_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
// assume the first is the backward neighbor, take its variance
infMatrix = links.begin()->second.infMatrix();
_previousInfMatrix = infMatrix;
}
else
{
if(_previousInfMatrix.empty())
{
_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
}
// we have a node not linked to map, use last variance
infMatrix = _previousInfMatrix;
}
// we have a node not linked to map, use last variance
infMatrix = _previousInfMatrix;
}
}
_previousMapId = s->mapId();
@@ -545,7 +575,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
data.setId(seq);
data.setStamp(s->getStamp());
data.setGroundTruth(s->getGroundTruthPose());
if(globalPose.isNull())
if(!globalPose.isNull())
{
data.setGlobalPose(globalPose, globalPoseCov);
}

2
corelib/src/Rtabmap.cpp
View File

@@ -5680,7 +5680,7 @@ bool Rtabmap::addLink(const Link & link)
}
if(t.isNull())
{
UERROR("Link's transform is null!");
UERROR("Link's transform is null! (%d->%d type=%s)", link.from(), link.to(), link.typeName().c_str());
return false;
}
if(_memory->isIncremental())