i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 04:20:20 +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
corelib/include/rtabmap/core/DBReader.h
+10 -2
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;
corelib/src/DBDriverSqlite3.cpp
+1
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());
}
}
corelib/src/DBReader.cpp
+46 -16
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);
}
corelib/src/Rtabmap.cpp
+1 -1
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())
guilib/include/rtabmap/gui/DatabaseViewer.h
+2
View File
@@ -165,10 +165,12 @@ private:
QLabel * labelId,
QLabel * labelMapId,
QLabel * labelPose,
QLabel * labelOptPose,
QLabel * labelVelocity,
QLabel * labelCalib,
QLabel * labelScan,
QLabel * labelGravity,
QLabel * labelPrior,
QLabel * labelGps,
QLabel * labelGt,
QLabel * labelSensors,
guilib/src/DatabaseViewer.cpp
+33 -2
View File
@@ -4373,10 +4373,12 @@ void DatabaseViewer::sliderAValueChanged(int value)
ui_->label_idA,
ui_->label_mapA,
ui_->label_poseA,
ui_->label_optposeA,
ui_->label_velA,
ui_->label_calibA,
ui_->label_scanA,
ui_->label_gravityA,
ui_->label_priorA,
ui_->label_gpsA,
ui_->label_gtA,
ui_->label_sensorsA,
@@ -4396,10 +4398,12 @@ void DatabaseViewer::sliderBValueChanged(int value)
ui_->label_idB,
ui_->label_mapB,
ui_->label_poseB,
ui_->label_optposeB,
ui_->label_velB,
ui_->label_calibB,
ui_->label_scanB,
ui_->label_gravityB,
ui_->label_priorB,
ui_->label_gpsB,
ui_->label_gtB,
ui_->label_sensorsB,
@@ -4417,10 +4421,12 @@ void DatabaseViewer::update(int value,
QLabel * labelId,
QLabel * labelMapId,
QLabel * labelPose,
QLabel * labelOptPose,
QLabel * labelVelocity,
QLabel * labelCalib,
QLabel * labelScan,
QLabel * labelGravity,
QLabel * labelPrior,
QLabel * labelGps,
QLabel * labelGt,
QLabel * labelSensors,
@@ -4436,11 +4442,13 @@ void DatabaseViewer::update(int value,
label->clear();
labelMapId->clear();
labelPose->clear();
labelOptPose->clear();
labelVelocity->clear();
stamp->clear();
labelCalib->clear();
labelScan ->clear();
labelGravity->clear();
labelPrior->clear();
labelGps->clear();
labelGt->clear();
labelSensors->clear();
@@ -4545,9 +4553,17 @@ void DatabaseViewer::update(int value,
float x,y,z,roll,pitch,yaw;
odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
labelPose->setText(QString("%1xyz=(%2,%3,%4)\nrpy=(%5,%6,%7)").arg(odomPose.isIdentity()?"* ":"").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
if(graphes_.size() && graphes_.back().find(id) == graphes_.back().end())
if(graphes_.size())
{
labelPose->setText(labelPose->text() + "\n<Not in optimized graph>");
if(graphes_.back().find(id) == graphes_.back().end())
{
labelOptPose->setText("<Not in optimized graph>");
}
else
{
graphes_.back().find(id)->second.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
labelOptPose->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
}
}
if(s!=0.0)
{
@@ -4570,6 +4586,17 @@ void DatabaseViewer::update(int value,
labelGravity->setToolTip(QString("roll=%1 pitch=%2 yaw=%3").arg(roll).arg(pitch).arg(yaw));
}
std::multimap<int, Link> priorLink;
dbDriver_->loadLinks(id, priorLink, Link::kPosePrior);
if(!priorLink.empty())
{
priorLink.begin()->second.transform().getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
labelPrior->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
std::stringstream out;
out << priorLink.begin()->second.infMatrix().inv();
labelPrior->setToolTip(QString("%1").arg(out.str().c_str()));
}
if(gps.stamp()>0.0)
{
labelGps->setText(QString("stamp=%1 longitude=%2 latitude=%3 altitude=%4m error=%5m bearing=%6deg").arg(QString::number(gps.stamp(), 'f')).arg(gps.longitude()).arg(gps.latitude()).arg(gps.altitude()).arg(gps.error()).arg(gps.bearing()));
@@ -5966,10 +5993,12 @@ void DatabaseViewer::updateConstraintView(
ui_->label_idA,
ui_->label_mapA,
ui_->label_poseA,
ui_->label_optposeA,
ui_->label_velA,
ui_->label_calibA,
ui_->label_scanA,
ui_->label_gravityA,
ui_->label_priorA,
ui_->label_gpsA,
ui_->label_gtA,
ui_->label_sensorsA,
@@ -5987,10 +6016,12 @@ void DatabaseViewer::updateConstraintView(
ui_->label_idB,
ui_->label_mapB,
ui_->label_poseB,
ui_->label_optposeB,
ui_->label_velB,
ui_->label_calibB,
ui_->label_scanB,
ui_->label_gravityB,
ui_->label_priorB,
ui_->label_gpsB,
ui_->label_gtB,
ui_->label_sensorsB,
guilib/src/ui/DatabaseViewer.ui
+349 -281
View File
@@ -60,21 +60,141 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-23</y>
<y>0</y>
<width>293</width>
<height>334</height>
<height>380</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="5" column="0">
<widget class="QLabel" name="label_childrenA_12">
<item row="2" column="0">
<widget class="QLabel" name="label_childrenA_8">
<property name="text">
<string>Pose</string>
<string>Weight</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_poseA">
<item row="11" column="0">
<widget class="QLabel" name="label_childrenA_24">
<property name="text">
<string>Gravity</string>
</property>
</widget>
</item>
<item row="12" column="1">
<widget class="QLabel" name="label_priorA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_childrenA_2">
<property name="text">
<string>Children</string>
</property>
</widget>
</item>
<item row="15" column="1">
<widget class="QLabel" name="label_sensorsA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QLabel" name="label_childrenA_6">
<property name="text">
<string>Stamp</string>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QLabel" name="label_childrenA_22">
<property name="text">
<string>Scan</string>
</property>
</widget>
</item>
<item row="15" column="0">
<widget class="QLabel" name="label_childrenA_20">
<property name="text">
<string>Sensors</string>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_scanA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_childrenA_12">
<property name="text">
<string>Odometry Pose</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_parentsA_2">
<property name="text">
<string>Parents</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_weightA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="13" column="1">
<widget class="QLabel" name="label_gpsA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QLabel" name="label_childrenA_14">
<property name="text">
<string>Calib</string>
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QLabel" name="label_childrenA_28">
<property name="text">
<string>Prior</string>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QLabel" name="label_childrenA_18">
<property name="text">
<string>Velocity</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_mapA">
<property name="text">
<string/>
</property>
@@ -93,14 +213,28 @@
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_childrenA_8">
<item row="4" column="0">
<widget class="QLabel" name="label_childrenA_10">
<property name="text">
<string>Weight</string>
<string>Map ID</string>
</property>
</widget>
</item>
<item row="6" column="1">
<item row="13" column="0">
<widget class="QLabel" name="label_childrenA_16">
<property name="text">
<string>GPS</string>
</property>
</widget>
</item>
<item row="14" column="0">
<widget class="QLabel" name="label_childrenA_27">
<property name="text">
<string>Ground Truth</string>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_velA">
<property name="text">
<string/>
@@ -110,6 +244,33 @@
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_childrenA_4">
<property name="text">
<string>Label</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_poseA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_calibA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_labelA">
<property name="text">
@@ -120,8 +281,8 @@
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_mapA">
<item row="11" column="1">
<widget class="QLabel" name="label_gravityA">
<property name="text">
<string/>
</property>
@@ -130,10 +291,13 @@
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QLabel" name="label_childrenA_14">
<item row="8" column="1">
<widget class="QLabel" name="label_stampA">
<property name="text">
<string>Calib</string>
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
@@ -147,8 +311,8 @@
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_stampA">
<item row="14" column="1">
<widget class="QLabel" name="label_gtA">
<property name="text">
<string/>
</property>
@@ -157,145 +321,15 @@
</property>
</widget>
</item>
<item row="13" column="1">
<widget class="QLabel" name="label_sensorsA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QLabel" name="label_childrenA_22">
<property name="text">
<string>Scan</string>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QLabel" name="label_childrenA_24">
<property name="text">
<string>Gravity</string>
</property>
</widget>
</item>
<item row="11" column="1">
<widget class="QLabel" name="label_gpsA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_weightA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="13" column="0">
<widget class="QLabel" name="label_childrenA_20">
<property name="text">
<string>Sensors</string>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QLabel" name="label_childrenA_16">
<property name="text">
<string>GPS</string>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QLabel" name="label_childrenA_6">
<property name="text">
<string>Stamp</string>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_childrenA_18">
<widget class="QLabel" name="label_childrenA_30">
<property name="text">
<string>Velocity</string>
<string>Optimized Pose</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_parentsA_2">
<property name="text">
<string>Parents</string>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_gravityA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_childrenA_10">
<property name="text">
<string>Map ID</string>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_scanA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_childrenA_4">
<property name="text">
<string>Label</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_childrenA_2">
<property name="text">
<string>Children</string>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_calibA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QLabel" name="label_childrenA_27">
<property name="text">
<string>Ground Truth</string>
</property>
</widget>
</item>
<item row="12" column="1">
<widget class="QLabel" name="label_gtA">
<item row="6" column="1">
<widget class="QLabel" name="label_optposeA">
<property name="text">
<string/>
</property>
@@ -320,16 +354,16 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-23</y>
<y>0</y>
<width>292</width>
<height>334</height>
<height>380</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
<item row="11" column="0">
<widget class="QLabel" name="label_childrenA_17">
<item row="4" column="0">
<widget class="QLabel" name="label_childrenA_11">
<property name="text">
<string>GPS</string>
<string>Map ID</string>
</property>
</widget>
</item>
@@ -343,8 +377,36 @@
</property>
</widget>
</item>
<item row="11" column="1">
<widget class="QLabel" name="label_gpsB">
<item row="14" column="0">
<widget class="QLabel" name="label_childrenA_26">
<property name="text">
<string>Ground Truth</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_childrenA_5">
<property name="text">
<string>Label</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_childrenA_3">
<property name="text">
<string>Children</string>
</property>
</widget>
</item>
<item row="15" column="0">
<widget class="QLabel" name="label_childrenA_21">
<property name="text">
<string>Sensors</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_childrenB">
<property name="text">
<string/>
</property>
@@ -353,27 +415,13 @@
</property>
</widget>
</item>
<item row="7" column="0">
<item row="8" column="0">
<widget class="QLabel" name="label_childrenA_7">
<property name="text">
<string>Stamp</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_childrenA_9">
<property name="text">
<string>Weight</string>
</property>
</widget>
</item>
<item row="13" column="0">
<widget class="QLabel" name="label_childrenA_21">
<property name="text">
<string>Sensors</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_mapB">
<property name="text">
@@ -384,56 +432,8 @@
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QLabel" name="label_childrenA_25">
<property name="text">
<string>Gravity</string>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QLabel" name="label_childrenA_15">
<property name="text">
<string>Calib</string>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_scanB">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_parentsB">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_childrenA_3">
<property name="text">
<string>Children</string>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_childrenA_13">
<property name="text">
<string>Pose</string>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_calibB">
<item row="11" column="1">
<widget class="QLabel" name="label_gravityB">
<property name="text">
<string/>
</property>
@@ -452,49 +452,8 @@
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_childrenA_19">
<property name="text">
<string>Velocity</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_childrenA_11">
<property name="text">
<string>Map ID</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_weightB">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_childrenA_5">
<property name="text">
<string>Label</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_childrenB">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_stampB">
<widget class="QLabel" name="label_velB">
<property name="text">
<string/>
</property>
@@ -503,8 +462,66 @@
</property>
</widget>
</item>
<item row="13" column="1">
<widget class="QLabel" name="label_gpsB">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_childrenA_13">
<property name="text">
<string>Odometry Pose</string>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QLabel" name="label_childrenA_23">
<property name="text">
<string>Scan</string>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_gravityB">
<widget class="QLabel" name="label_scanB">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QLabel" name="label_childrenA_15">
<property name="text">
<string>Calib</string>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QLabel" name="label_childrenA_25">
<property name="text">
<string>Gravity</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_parentsB">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="12" column="1">
<widget class="QLabel" name="label_priorB">
<property name="text">
<string/>
</property>
@@ -520,15 +537,8 @@
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QLabel" name="label_childrenA_23">
<property name="text">
<string>Scan</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_velB">
<item row="8" column="1">
<widget class="QLabel" name="label_stampB">
<property name="text">
<string/>
</property>
@@ -537,7 +547,14 @@
</property>
</widget>
</item>
<item row="13" column="1">
<item row="7" column="0">
<widget class="QLabel" name="label_childrenA_19">
<property name="text">
<string>Velocity</string>
</property>
</widget>
</item>
<item row="15" column="1">
<widget class="QLabel" name="label_sensorsB">
<property name="text">
<string/>
@@ -547,14 +564,24 @@
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QLabel" name="label_childrenA_26">
<item row="9" column="1">
<widget class="QLabel" name="label_calibB">
<property name="text">
<string>Ground Truth</string>
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="12" column="1">
<item row="2" column="0">
<widget class="QLabel" name="label_childrenA_9">
<property name="text">
<string>Weight</string>
</property>
</widget>
</item>
<item row="14" column="1">
<widget class="QLabel" name="label_gtB">
<property name="text">
<string/>
@@ -564,6 +591,47 @@
</property>
</widget>
</item>
<item row="13" column="0">
<widget class="QLabel" name="label_childrenA_17">
<property name="text">
<string>GPS</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_weightB">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QLabel" name="label_childrenA_29">
<property name="text">
<string>Prior</string>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_childrenA_31">
<property name="text">
<string>Optimized Pose</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_optposeB">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
tools/Reprocess/main.cpp
+185 -68
View File
@@ -69,8 +69,11 @@ void showUsage()
" arguments, they overwrite those in config file and the database.\n"
" -start # Start from this node ID.\n"
" -stop # Last node to process.\n"
" -start_s # Start from this map session ID.\n"
" -stop_s # Last map session to process.\n"
" -cam # Camera index to stream. Ignored if a database doesn't contain multi-camera data.\n"
" -nolandmark Don't republish landmarks contained in input database.\n"
" -pub_loops Republish loop closures contained in input database.\n"
" -loc_null On localization mode, reset localization pose to null and map correction to identity between sessions.\n"
" -gt When reprocessing a single database, load its original optimized graph, then \n"
" set it as ground truth for output database. If there was a ground truth in the input database, it will be ignored.\n"
@@ -226,9 +229,12 @@ int main(int argc, char * argv[])
bool useDatabaseRate = false;
int startId = 0;
int stopId = 0;
int startMapId = 0;
int stopMapId = -1;
int cameraIndex = -1;
int framesToSkip = 0;
bool ignoreLandmarks = false;
bool republishLoopClosures = false;
bool locNull = false;
bool originalGraphAsGT = false;
bool scanFromDepth = false;
@@ -293,6 +299,34 @@ int main(int argc, char * argv[])
showUsage();
}
}
else if (strcmp(argv[i], "-start_s") == 0 || strcmp(argv[i], "--start_s") == 0)
{
++i;
if(i < argc - 2)
{
startMapId = atoi(argv[i]);
printf("Start at map session ID = %d.\n", startMapId);
}
else
{
printf("-start_s option requires a value\n");
showUsage();
}
}
else if (strcmp(argv[i], "-stop_s") == 0 || strcmp(argv[i], "--stop_s") == 0)
{
++i;
if(i < argc - 2)
{
stopMapId = atoi(argv[i]);
printf("Stop at map session ID = %d.\n", stopMapId);
}
else
{
printf("-stop option requires a value\n");
showUsage();
}
}
else if (strcmp(argv[i], "-cam") == 0 || strcmp(argv[i], "--cam") == 0)
{
++i;
@@ -326,6 +360,11 @@ int main(int argc, char * argv[])
ignoreLandmarks = true;
printf("Ignoring landmarks from input database (-nolandmark option).\n");
}
else if(strcmp(argv[i], "-pub_loops") == 0 || strcmp(argv[i], "--pub_loops") == 0)
{
republishLoopClosures = true;
printf("Republish loop closures from input database (-pub_loops option).\n");
}
else if(strcmp(argv[i], "-loc_null") == 0 || strcmp(argv[i], "--loc_null") == 0)
{
locNull = true;
@@ -524,6 +563,8 @@ int main(int argc, char * argv[])
printf(" %s\t= %s\n", iter->first.c_str(), iter->second.c_str());
}
}
bool useOdomFeatures = Parameters::defaultMemUseOdomFeatures();
if((configParameters.find(Parameters::kKpDetectorStrategy())!=configParameters.end() ||
configParameters.find(Parameters::kVisFeatureType())!=configParameters.end() ||
customParameters.find(Parameters::kKpDetectorStrategy())!=customParameters.end() ||
@@ -531,7 +572,6 @@ int main(int argc, char * argv[])
configParameters.find(Parameters::kMemUseOdomFeatures())==configParameters.end() &&
customParameters.find(Parameters::kMemUseOdomFeatures())==customParameters.end())
{
bool useOdomFeatures = Parameters::defaultMemUseOdomFeatures();
Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), useOdomFeatures);
if(useOdomFeatures)
{
@@ -543,6 +583,43 @@ int main(int argc, char * argv[])
Parameters::kMemUseOdomFeatures().c_str(),
Parameters::kMemUseOdomFeatures().c_str());
uInsert(parameters, ParametersPair(Parameters::kMemUseOdomFeatures(), "false"));
useOdomFeatures = false;
}
}
if(republishLoopClosures)
{
if(databases.size() > 1)
{
printf("[Warning] \"pub_loops\" option cannot be used with multiple databases input. "
"Disabling \"pub_loops\" to avoid mismatched loop closue ids.\n");
republishLoopClosures = false;
}
else
{
bool generateIds = Parameters::defaultMemGenerateIds();
Parameters::parse(parameters, Parameters::kMemGenerateIds(), generateIds);
Parameters::parse(configParameters, Parameters::kMemGenerateIds(), generateIds);
Parameters::parse(customParameters, Parameters::kMemGenerateIds(), generateIds);
if(generateIds)
{
if(configParameters.find(Parameters::kMemGenerateIds())!=configParameters.end() ||
customParameters.find(Parameters::kMemGenerateIds())!=customParameters.end())
{
printf("[Warning] \"pub_loops\" option is used but parameter %s is set to true in custom arguments. "
"Disabling \"pub_loops\" to avoid mismatched loop closure ids.\n",
Parameters::kMemGenerateIds().c_str());
republishLoopClosures = false;
}
else
{
printf("[Warning] \"pub_loops\" option is used but parameter %s is true in the opened database. "
"Setting parameter %s to false for convenience to so that republished loop closure ids match.\n",
Parameters::kMemGenerateIds().c_str(),
Parameters::kMemGenerateIds().c_str());
uInsert(parameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
}
}
}
}
uInsert(parameters, configParameters);
@@ -627,7 +704,22 @@ int main(int argc, char * argv[])
bool rgbdEnabled = Parameters::defaultRGBDEnabled();
Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
bool odometryIgnored = !rgbdEnabled;
DBReader * dbReader = new DBReader(inputDatabasePath, useDatabaseRate?-1:0, odometryIgnored, false, false, startId, cameraIndex, stopId, !intermediateNodes, ignoreLandmarks);
DBReader * dbReader = new DBReader(
inputDatabasePath,
useDatabaseRate?-1:0,
odometryIgnored,
false,
false,
startId,
cameraIndex,
stopId,
!intermediateNodes,
ignoreLandmarks,
!useOdomFeatures,
startMapId,
stopMapId);
dbReader->init();
OccupancyGrid grid(parameters);
@@ -692,84 +784,109 @@ int main(int argc, char * argv[])
printf("Failed processing node %d.\n", data.id());
globalMapStats.clear();
}
else if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
else
{
globalMapStats.clear();
double timeRtabmap = t.ticks();
double timeUpdateInit = 0.0;
double timeUpdateGrid = 0.0;
#ifdef RTABMAP_OCTOMAP
double timeUpdateOctoMap = 0.0;
#endif
const rtabmap::Statistics & stats = rtabmap.getStatistics();
if(stats.poses().size() && stats.getLastSignatureData().id())
if(republishLoopClosures && dbReader->driver())
{
int id = stats.poses().rbegin()->first;
if(id == stats.getLastSignatureData().id() &&
stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f)
std::multimap<int, Link> links;
dbReader->driver()->loadLinks(data.id(), links);
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
bool updateGridMap = false;
bool updateOctoMap = false;
if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end())
if((iter->second.type() == Link::kGlobalClosure ||
iter->second.type() == Link::kLocalSpaceClosure ||
iter->second.type() == Link::kLocalTimeClosure ||
iter->second.type() == Link::kUserClosure) &&
iter->second.to() < data.id())
{
updateGridMap = true;
}
#ifdef RTABMAP_OCTOMAP
if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end())
{
updateOctoMap = true;
}
#endif
if(updateGridMap || updateOctoMap)
{
cv::Mat ground, obstacles, empty;
stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
timeUpdateInit = t.ticks();
if(updateGridMap)
if(!iter->second.transform().isNull() &&
rtabmap.getMemory()->getWorkingMem().find(iter->second.to()) != rtabmap.getMemory()->getWorkingMem().end() &&
rtabmap.addLink(iter->second))
{
grid.addToCache(id, ground, obstacles, empty);
grid.update(stats.poses());
timeUpdateGrid = t.ticks() + timeUpdateInit;
printf("Added link %d->%d from input database.\n", iter->second.from(), iter->second.to());
}
#ifdef RTABMAP_OCTOMAP
if(updateOctoMap)
{
const cv::Point3f & viewpoint = stats.getLastSignatureData().sensorData().gridViewPoint();
octomap.addToCache(id, ground, obstacles, empty, viewpoint);
octomap.update(stats.poses());
timeUpdateOctoMap = t.ticks() + timeUpdateInit;
}
#endif
}
}
}
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
{
globalMapStats.clear();
double timeRtabmap = t.ticks();
double timeUpdateInit = 0.0;
double timeUpdateGrid = 0.0;
#ifdef RTABMAP_OCTOMAP
//Simulate publishing
double timePub2dOctoMap = 0.0;
double timePub3dOctoMap = 0.0;
if(assemble2dOctoMap)
{
float xMin, yMin, size;
octomap.createProjectionMap(xMin, yMin, size);
timePub2dOctoMap = t.ticks();
}
if(assemble3dOctoMap)
{
octomap.createCloud();
timePub3dOctoMap = t.ticks();
}
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f));
#else
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeRtabmap)*1000.0f));
double timeUpdateOctoMap = 0.0;
#endif
const rtabmap::Statistics & stats = rtabmap.getStatistics();
if(stats.poses().size() && stats.getLastSignatureData().id())
{
int id = stats.poses().rbegin()->first;
if(id == stats.getLastSignatureData().id() &&
stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f)
{
bool updateGridMap = false;
bool updateOctoMap = false;
if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end())
{
updateGridMap = true;
}
#ifdef RTABMAP_OCTOMAP
if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end())
{
updateOctoMap = true;
}
#endif
if(updateGridMap || updateOctoMap)
{
cv::Mat ground, obstacles, empty;
stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
timeUpdateInit = t.ticks();
if(updateGridMap)
{
grid.addToCache(id, ground, obstacles, empty);
grid.update(stats.poses());
timeUpdateGrid = t.ticks() + timeUpdateInit;
}
#ifdef RTABMAP_OCTOMAP
if(updateOctoMap)
{
const cv::Point3f & viewpoint = stats.getLastSignatureData().sensorData().gridViewPoint();
octomap.addToCache(id, ground, obstacles, empty, viewpoint);
octomap.update(stats.poses());
timeUpdateOctoMap = t.ticks() + timeUpdateInit;
}
#endif
}
}
}
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
#ifdef RTABMAP_OCTOMAP
//Simulate publishing
double timePub2dOctoMap = 0.0;
double timePub3dOctoMap = 0.0;
if(assemble2dOctoMap)
{
float xMin, yMin, size;
octomap.createProjectionMap(xMin, yMin, size);
timePub2dOctoMap = t.ticks();
}
if(assemble3dOctoMap)
{
octomap.createCloud();
timePub3dOctoMap = t.ticks();
}
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f));
#else
globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeRtabmap)*1000.0f));
#endif
}
}
}