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 buttons to edit/add priors

Browse Source
This commit is contained in:
matlabbe
2024-04-02 17:37:52 -07:00
parent 9cb0046734
commit d794669ef9
3 changed files with 245 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

173
guilib/src/DatabaseViewer.cpp
View File

@@ -351,6 +351,10 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
connect(ui_->horizontalSlider_B, SIGNAL(valueChanged(int)), this, SLOT(sliderBValueChanged(int)));
connect(ui_->horizontalSlider_A, SIGNAL(sliderMoved(int)), this, SLOT(sliderAMoved(int)));
connect(ui_->horizontalSlider_B, SIGNAL(sliderMoved(int)), this, SLOT(sliderBMoved(int)));
connect(ui_->toolButton_edit_priorA, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
connect(ui_->toolButton_edit_priorB, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
connect(ui_->toolButton_remove_priorA, SIGNAL(clicked(bool)), this, SLOT(rejectConstraint()));
connect(ui_->toolButton_remove_priorB, SIGNAL(clicked(bool)), this, SLOT(rejectConstraint()));
connect(ui_->spinBox_mesh_angleTolerance, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
connect(ui_->spinBox_mesh_minClusterSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
@@ -947,12 +951,14 @@ bool DatabaseViewer::closeDatabase()
if(refinedIter != linksRefined_.end())
{
dbDriver_->addLink(refinedIter->second);
dbDriver_->addLink(refinedIter->second.inverse());
if(refinedIter->second.from() != refinedIter->second.to())
dbDriver_->addLink(refinedIter->second.inverse());
}
else
{
dbDriver_->addLink(iter->second);
dbDriver_->addLink(iter->second.inverse());
if(iter->second.from() != iter->second.to())
dbDriver_->addLink(iter->second.inverse());
}
}
@@ -962,7 +968,8 @@ bool DatabaseViewer::closeDatabase()
if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
{
dbDriver_->updateLink(iter->second);
dbDriver_->updateLink(iter->second.inverse());
if(iter->second.from() != iter->second.to())
dbDriver_->updateLink(iter->second.inverse());
}
}
@@ -970,7 +977,8 @@ bool DatabaseViewer::closeDatabase()
for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
{
dbDriver_->removeLink(iter->second.to(), iter->second.from());
dbDriver_->removeLink(iter->second.from(), iter->second.to());
if(iter->second.from() != iter->second.to())
dbDriver_->removeLink(iter->second.from(), iter->second.to());
}
linksAdded_.clear();
linksRefined_.clear();
@@ -1753,6 +1761,10 @@ void DatabaseViewer::updateIds()
ui_->label_alignPosesWithGPS->setVisible(false);
ui_->label_alignPosesWithGroundTruth->setVisible(false);
ui_->label_alignScansCloudsWithGroundTruth->setVisible(false);
ui_->toolButton_edit_priorA->setVisible(false);
ui_->toolButton_edit_priorB->setVisible(false);
ui_->toolButton_remove_priorA->setVisible(false);
ui_->toolButton_remove_priorB->setVisible(false);
ui_->menuEdit->setEnabled(true);
ui_->actionGenerate_3D_map_pcd->setEnabled(true);
ui_->actionExport->setEnabled(true);
@@ -4585,6 +4597,8 @@ void DatabaseViewer::sliderAValueChanged(int value)
ui_->label_scanA,
ui_->label_gravityA,
ui_->label_priorA,
ui_->toolButton_edit_priorA,
ui_->toolButton_remove_priorA,
ui_->label_gpsA,
ui_->label_gtA,
ui_->label_sensorsA,
@@ -4610,6 +4624,8 @@ void DatabaseViewer::sliderBValueChanged(int value)
ui_->label_scanB,
ui_->label_gravityB,
ui_->label_priorB,
ui_->toolButton_edit_priorB,
ui_->toolButton_remove_priorB,
ui_->label_gpsB,
ui_->label_gtB,
ui_->label_sensorsB,
@@ -4633,6 +4649,8 @@ void DatabaseViewer::update(int value,
QLabel * labelScan,
QLabel * labelGravity,
QLabel * labelPrior,
QToolButton * editPriorButton,
QToolButton * removePriorButton,
QLabel * labelGps,
QLabel * labelGt,
QLabel * labelSensors,
@@ -4655,6 +4673,8 @@ void DatabaseViewer::update(int value,
labelScan ->clear();
labelGravity->clear();
labelPrior->clear();
editPriorButton->setVisible(false);
removePriorButton->setVisible(false);
labelGps->clear();
labelGt->clear();
labelSensors->clear();
@@ -4797,15 +4817,25 @@ 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())
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
if(graph::findLink(links, id, id, false, Link::kPosePrior)!=links.end())
{
priorLink.begin()->second.transform().getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
Link & priorLink = graph::findLink(links, id, id, false, Link::kPosePrior)->second;
priorLink.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();
out << priorLink.infMatrix().inv();
labelPrior->setToolTip(QString("%1").arg(out.str().c_str()));
removePriorButton->setVisible(true);
editPriorButton->setToolTip(tr("Edit Prior"));
editPriorButton->setText("...");
editPriorButton->setVisible(true);
}
else if(!odomPose.isNull())
{
editPriorButton->setToolTip(tr("Add Prior"));
editPriorButton->setText("+");
editPriorButton->setVisible(true);
}
if(gps.stamp()>0.0)
@@ -5984,16 +6014,38 @@ void DatabaseViewer::editConstraint()
{
if(ids_.size())
{
Link link;
if(ui_->label_type->text().toInt() == Link::kLandmark)
Link link(0,0,Link::kUndef, Transform::getIdentity());
int priorId = sender() == ui_->toolButton_edit_priorA?ids_.at(ui_->horizontalSlider_A->value()):
sender() == ui_->toolButton_edit_priorB?ids_.at(ui_->horizontalSlider_B->value()):0;
if(priorId>0)
{
int position = ui_->horizontalSlider_loops->value();
link = loopLinks_.at(position);
// Prior
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
if(graph::findLink(links, priorId, priorId, false, Link::kPosePrior) != links.end())
{
link = graph::findLink(links, priorId, priorId, false, Link::kPosePrior)->second;
}
else if(odomPoses_.find(priorId) != odomPoses_.end())
{
// fallback to odom pose
// set undef to go in "add" branch below
link = Link(priorId, priorId, Link::kUndef, odomPoses_.at(priorId));
}
else
{
QMessageBox::warning(this, tr(""), tr("Node %1 doesn't have odometry pose, cannot add a prior for it!").arg(priorId));
return;
}
}
else if(ui_->label_type->text().toInt() == Link::kLandmark)
{
link = loopLinks_.at(ui_->horizontalSlider_loops->value());
}
else
{
link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
}
bool updated = false;
if(link.isValid())
{
cv::Mat covBefore = link.infMatrix().inv();
@@ -6002,7 +6054,6 @@ void DatabaseViewer::editConstraint()
covBefore.at<double>(5,5)<9999.0?std::sqrt(covBefore.at<double>(5,5)):0.0);
if(dialog.exec() == QDialog::Accepted)
{
bool updated = false;
cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
if(dialog.getLinearVariance()>0)
{
@@ -6038,7 +6089,7 @@ void DatabaseViewer::editConstraint()
linksRefined_.insert(std::make_pair(newLink.from(), newLink));
updated = true;
}
if(updated)
if(priorId==0)
{
this->updateGraphView();
updateConstraintView();
@@ -6047,7 +6098,10 @@ void DatabaseViewer::editConstraint()
}
else
{
EditConstraintDialog dialog(Transform::getIdentity());
EditConstraintDialog dialog(
link.transform(),
priorId>0?0.001:1,
priorId>0?0.001:1);
if(dialog.exec() == QDialog::Accepted)
{
cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
@@ -6067,12 +6121,12 @@ void DatabaseViewer::editConstraint()
{
covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9;
}
int from = ids_.at(ui_->horizontalSlider_A->value());
int to = ids_.at(ui_->horizontalSlider_B->value());
int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value());
int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value());
Link newLink(
from,
to,
Link::kUserClosure,
priorId>0?Link::kPosePrior:Link::kUserClosure,
dialog.getTransform(),
covariance.inv());
if(newLink.from() < newLink.to())
@@ -6080,10 +6134,46 @@ void DatabaseViewer::editConstraint()
newLink = newLink.inverse();
}
linksAdded_.insert(std::make_pair(newLink.from(), newLink));
this->updateGraphView();
updateLoopClosuresSlider(from, to);
updated = true;
if(priorId==0)
{
this->updateGraphView();
updateLoopClosuresSlider(from, to);
}
}
}
if(updated && priorId>0)
{
bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored();
Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored);
if(priorsIgnored)
{
if(QMessageBox::question(this,
tr("Updating Prior"),
tr("Parameter %1 is true, do you want to turn it off to update the graph with the updated prior?").arg(Parameters::kOptimizerPriorsIgnored().c_str()),
QMessageBox::Yes | QMessageBox::No,
QMessageBox::Yes) == QMessageBox::Yes)
{
priorsIgnored = false;
ui_->parameters_toolbox->updateParameter(Parameters::kOptimizerPriorsIgnored(), "false");
}
}
int indexA = ui_->horizontalSlider_A->value();
int indexB = ui_->horizontalSlider_B->value();
if(!priorsIgnored)
{
this->updateGraphView();
}
if(ui_->horizontalSlider_A->value() != indexA)
ui_->horizontalSlider_A->setValue(indexA);
else
sliderAValueChanged(indexA);
if(ui_->horizontalSlider_B->value() != indexB)
ui_->horizontalSlider_B->setValue(indexB);
else
sliderBValueChanged(indexB);
}
}
}
@@ -6241,6 +6331,8 @@ void DatabaseViewer::updateConstraintView(
ui_->label_scanA,
ui_->label_gravityA,
ui_->label_priorA,
ui_->toolButton_edit_priorA,
ui_->toolButton_remove_priorA,
ui_->label_gpsA,
ui_->label_gtA,
ui_->label_sensorsA,
@@ -6264,6 +6356,8 @@ void DatabaseViewer::updateConstraintView(
ui_->label_scanB,
ui_->label_gravityB,
ui_->label_priorB,
ui_->toolButton_edit_priorB,
ui_->toolButton_remove_priorB,
ui_->label_gpsB,
ui_->label_gtB,
ui_->label_sensorsB,
@@ -8934,9 +9028,12 @@ void DatabaseViewer::resetConstraint()
void DatabaseViewer::rejectConstraint()
{
int from = ids_.at(ui_->horizontalSlider_A->value());
int to = ids_.at(ui_->horizontalSlider_B->value());
if(ui_->label_type->text().toInt() == Link::kLandmark)
int priorId = sender() == ui_->toolButton_remove_priorA?ids_.at(ui_->horizontalSlider_A->value()):
sender() == ui_->toolButton_remove_priorB?ids_.at(ui_->horizontalSlider_B->value()):0;
int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value());
int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value());
if(priorId==0 && ui_->label_type->text().toInt() == Link::kLandmark)
{
int position = ui_->horizontalSlider_loops->value();
const rtabmap::Link & link = loopLinks_.at(position);
@@ -8951,7 +9048,7 @@ void DatabaseViewer::rejectConstraint()
from = tmp;
}
if(from == to)
if(priorId==0 && from == to)
{
UWARN("Cannot reject link to same node");
return;
@@ -8993,9 +9090,31 @@ void DatabaseViewer::rejectConstraint()
}
if(removed)
{
this->updateGraphView();
if(priorId==0)
{
this->updateGraphView();
updateLoopClosuresSlider();
}
else
{
bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored();
Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored);
int indexA = ui_->horizontalSlider_A->value();
int indexB = ui_->horizontalSlider_B->value();
if(!priorsIgnored)
{
this->updateGraphView();
}
if(ui_->horizontalSlider_A->value() != indexA)
ui_->horizontalSlider_A->setValue(indexA);
else
sliderAValueChanged(indexA);
if(ui_->horizontalSlider_B->value() != indexB)
ui_->horizontalSlider_B->setValue(indexB);
else
sliderBValueChanged(indexB);
}
}
updateLoopClosuresSlider();
}
std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(

118
guilib/src/ui/DatabaseViewer.ui
View File

@@ -62,7 +62,7 @@
<x>0</x>
<y>0</y>
<width>307</width>
<height>380</height>
<height>389</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -80,16 +80,6 @@
</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">
@@ -338,6 +328,53 @@
</property>
</widget>
</item>
<item row="12" column="1">
<layout class="QHBoxLayout" name="horizontalLayout_18">
<item>
<widget class="QLabel" name="label_priorA">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<widget class="QToolButton" name="toolButton_edit_priorA">
<property name="toolTip">
<string>Edit Constraint</string>
</property>
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item>
<widget class="QToolButton" name="toolButton_remove_priorA">
<property name="toolTip">
<string>Remove Prior</string>
</property>
<property name="text">
<string>X</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_7">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</widget>
</widget>
@@ -356,7 +393,7 @@
<x>0</x>
<y>0</y>
<width>306</width>
<height>380</height>
<height>389</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -520,16 +557,6 @@
</property>
</widget>
</item>
<item row="12" column="1">
<widget class="QLabel" name="label_priorB">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_parentsA_4">
<property name="text">
@@ -632,6 +659,53 @@
</property>
</widget>
</item>
<item row="12" column="1">
<layout class="QHBoxLayout" name="horizontalLayout_19">
<item>
<widget class="QLabel" name="label_priorB">
<property name="text">
<string/>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<widget class="QToolButton" name="toolButton_edit_priorB">
<property name="toolTip">
<string>Edit Constraint</string>
</property>
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item>
<widget class="QToolButton" name="toolButton_remove_priorB">
<property name="toolTip">
<string>Remove Prior</string>
</property>
<property name="text">
<string>X</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_8">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>0</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</widget>
</widget>