i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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

2
guilib/include/rtabmap/gui/DatabaseViewer.h
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,

35
guilib/src/DatabaseViewer.cpp
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,

630
guilib/src/ui/DatabaseViewer.ui
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>