Skip to content

Commit

Permalink
DBViewer: added buttons to edit/add priors
Browse files Browse the repository at this point in the history
  • Loading branch information
matlabbe committed Apr 3, 2024
1 parent 9cb0046 commit d794669
Show file tree
Hide file tree
Showing 3 changed files with 244 additions and 48 deletions.
3 changes: 3 additions & 0 deletions guilib/include/rtabmap/gui/DatabaseViewer.h
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ class Ui_DatabaseViewer;
class QGraphicsScene;
class QGraphicsView;
class QLabel;
class QToolButton;
class QDialog;

namespace rtabmap
Expand Down Expand Up @@ -176,6 +177,8 @@ private Q_SLOTS:
QLabel * labelScan,
QLabel * labelGravity,
QLabel * labelPrior,
QToolButton * editPriorButton,
QToolButton * removePriorButton,
QLabel * labelGps,
QLabel * labelGt,
QLabel * labelSensors,
Expand Down
171 changes: 145 additions & 26 deletions guilib/src/DatabaseViewer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()));
Expand Down Expand Up @@ -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());
}
}

Expand All @@ -962,15 +968,17 @@ 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());
}
}

// Rejected links
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();
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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();
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
{
// 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)
{
int position = ui_->horizontalSlider_loops->value();
link = loopLinks_.at(position);
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();
Expand All @@ -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)
{
Expand Down Expand Up @@ -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();
Expand All @@ -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);
Expand All @@ -6067,22 +6121,58 @@ 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())
{
newLink = newLink.inverse();
}
linksAdded_.insert(std::make_pair(newLink.from(), newLink));
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();
updateLoopClosuresSlider(from, to);
}
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);
}
}
}
Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand Down Expand Up @@ -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);
Expand All @@ -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;
Expand Down Expand Up @@ -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(
Expand Down
Loading

0 comments on commit d794669

Please sign in to comment.