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

DbViewer: manual constraints can now be added (http://official-rtab-map-forum.67519.x6.nabble.com/Manually-adding-modifying-node-constraints-td6983.html)

Browse Source
This commit is contained in:
matlabbe
2020-10-07 15:41:13 -04:00
parent 168c87b5ba
commit 2cb509c825
6 changed files with 229 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
guilib/src/DatabaseViewer.cpp
View File

@@ -1068,6 +1068,9 @@ bool DatabaseViewer::closeDatabase()
ui_->label_constraint_opt->clear();
ui_->label_variance->clear();
ui_->lineEdit_covariance->clear();
ui_->label_type->clear();
ui_->label_type_name->clear();
ui_->checkBox_showOptimized->setEnabled(false);
ui_->horizontalSlider_A->setEnabled(false);
ui_->horizontalSlider_A->setMaximum(0);
@@ -4880,9 +4883,11 @@ void DatabaseViewer::update(int value,
ui_->horizontalSlider_neighbors->blockSignals(false);
constraintsViewer_->removeAllClouds();
Link link = this->findActiveLink(from, to);
bool constraintViewUpdated = false;
if(link.isValid() && link.type() != Link::kGravity)
{
this->updateConstraintView(link, false);
constraintViewUpdated = true;
}
@@ -4897,9 +4902,20 @@ void DatabaseViewer::update(int value,
if(fromIter != optimizedPoses.end() &&
toIter != optimizedPoses.end())
{
Link link(from, to, Link::kUndef, fromIter->second.inverse() * toIter->second);
this->updateConstraintView(link, false);
constraintViewUpdated = true;
}
}
}
if(!constraintViewUpdated)
{
ui_->label_constraint->clear();
ui_->label_constraint_opt->clear();
ui_->label_variance->clear();
ui_->lineEdit_covariance->clear();
ui_->label_type->clear();
ui_->label_type_name->clear();
ui_->checkBox_showOptimized->setEnabled(false);
}
constraintsViewer_->update();
@@ -5259,12 +5275,23 @@ void DatabaseViewer::editConstraint()
if(ids_.size())
{
Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
if(link.isValid())
if(link.isValid())
{
cv::Mat covBefore = link.infMatrix().inv();
EditConstraintDialog dialog(link.transform(),
covBefore.at<double>(0,0)!=1.0?std::sqrt(covBefore.at<double>(0,0)):0,
covBefore.at<double>(5,5)!=1.0?std::sqrt(covBefore.at<double>(5,5)):0);
if(dialog.exec() == QDialog::Accepted)
{
bool updated = false;
{
bool updated = false;
cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
if(dialog.getLinearVariance()>0)
{
covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance()*dialog.getLinearVariance();
}
if(dialog.getAngularVariance()>0)
{
covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance()*dialog.getAngularVariance();
}
Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv());
std::multimap<int, Link>::iterator iter = linksRefined_.find(link.from());
@@ -5285,9 +5312,41 @@ void DatabaseViewer::editConstraint()
updated = true;
}
if(updated)
{
updateConstraintView();
this->updateGraphView();
}
}
}
else
{
EditConstraintDialog dialog(Transform::getIdentity());
if(dialog.exec() == QDialog::Accepted)
{
cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
if(dialog.getLinearVariance()>0)
{
covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance()*dialog.getLinearVariance();
}
if(dialog.getAngularVariance()>0)
{
covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance()*dialog.getAngularVariance();
}
int from = ids_.at(ui_->horizontalSlider_A->value());
int to = ids_.at(ui_->horizontalSlider_B->value());
Link newLink(
from,
to,
Link::kUserClosure,
dialog.getTransform(),
covariance.inv());
if(newLink.from() < newLink.to())
{
newLink = newLink.inverse();
}
linksAdded_.insert(std::make_pair(newLink.from(), newLink));
updateLoopClosuresSlider(from, to);
this->updateGraphView();
}
}
}
@@ -5407,7 +5466,7 @@ void DatabaseViewer::updateConstraintView(
float diff = topt.getDistance(t);
Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
a = (a *180.0f) / CV_PI;
ui_->label_constraint_opt->setText(QString("%1\n(error=%2% a=%3)").arg(QString(topt.prettyPrint().c_str()).replace(" ", "\n")).arg((t.getNorm()>0?diff/t.getNorm():0)*100.0f).arg(a));
@@ -5885,7 +5944,11 @@ void DatabaseViewer::updateConstraintButtons()
return;
}
int from = ids_.at(ui_->horizontalSlider_A->value());
int from = ids_.at(ui_->horizontalSlider_A->value());
int to = ids_.at(ui_->horizontalSlider_B->value());
if(from!=to && from && to &&
odomPoses_.find(from) != odomPoses_.end() &&
odomPoses_.find(to) != odomPoses_.end() &&
weights_.find(from) != weights_.end() && weights_.at(from)>=0,
weights_.find(to) != weights_.end() && weights_.at(to)>=0)
{
@@ -7450,9 +7513,16 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
newLink = Link(from, to, Link::kUserClosure, t, information);
}
else if(!silent)
else if(!silent)
{
QMessageBox::warning(this,
QMessageBox::StandardButton button = QMessageBox::warning(this,
tr("Add link"),
tr("Cannot find a transformation between nodes %1 and %2: %3\n\nDo you want to add it manually?").arg(from).arg(to).arg(info.rejectedMsg.c_str()),
QMessageBox::Yes | QMessageBox::No,
QMessageBox::No);
if(button == QMessageBox::Yes)
{
editConstraint();
silent = true;
}
}

67
guilib/src/EditConstraintDialog.cpp
View File

@@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
EditConstraintDialog::EditConstraintDialog(const Transform & constraint, QWidget * parent) :
EditConstraintDialog::EditConstraintDialog(const Transform & constraint, double linearSigma, double angularSigma, QWidget * parent) :
QDialog(parent)
{
_ui = new Ui_EditConstraintDialog();
@@ -44,6 +44,11 @@ EditConstraintDialog::EditConstraintDialog(const Transform & constraint, QWidget
_ui->roll->setValue(roll);
_ui->pitch->setValue(pitch);
_ui->yaw->setValue(yaw);
_ui->linear_sigma->setValue(linearSigma);
_ui->angular_sigma->setValue(angularSigma);
connect(_ui->checkBox_radians, SIGNAL(stateChanged(int)), this, SLOT(switchUnits()));
_ui->checkBox_radians->setChecked(false);
}
EditConstraintDialog::~EditConstraintDialog()
@@ -51,9 +56,67 @@ EditConstraintDialog::~EditConstraintDialog()
delete _ui;
}
void EditConstraintDialog::switchUnits()
{
double conversion = 180.0/M_PI;
if(_ui->checkBox_radians->isChecked())
{
conversion = M_PI/180.0;
}
QVector<QDoubleSpinBox*> boxes;
boxes.push_back(_ui->roll);
boxes.push_back(_ui->pitch);
boxes.push_back(_ui->yaw);
boxes.push_back(_ui->angular_sigma);
for(int i=0; i<boxes.size(); ++i)
{
double value = boxes[i]->value()*conversion;
if(_ui->checkBox_radians->isChecked())
{
if(boxes[i]!=_ui->angular_sigma)
{
boxes[i]->setMinimum(-M_PI);
}
boxes[i]->setMaximum(M_PI);
boxes[i]->setSuffix(" rad");
boxes[i]->setSingleStep(0.01);
}
else
{
if(boxes[i]!=_ui->angular_sigma)
{
boxes[i]->setMinimum(-180);
}
boxes[i]->setMaximum(180);
boxes[i]->setSuffix(" deg");
boxes[i]->setSingleStep(1);
}
boxes[i]->setValue(value);
}
}
Transform EditConstraintDialog::getTransform() const
{
return Transform(_ui->x->value(), _ui->y->value(), _ui->z->value(), _ui->roll->value(), _ui->pitch->value(), _ui->yaw->value());
double conversion = 1.0f;
if(!_ui->checkBox_radians->isChecked())
{
conversion = M_PI/180.0;
}
return Transform(_ui->x->value(), _ui->y->value(), _ui->z->value(), _ui->roll->value()*conversion, _ui->pitch->value()*conversion, _ui->yaw->value()*conversion);
}
double EditConstraintDialog::getLinearVariance() const
{
return _ui->linear_sigma->value();
}
double EditConstraintDialog::getAngularVariance() const
{
double conversion = 1.0f;
if(!_ui->checkBox_radians->isChecked())
{
conversion = M_PI/180.0;
}
return _ui->angular_sigma->value()*conversion;
}
}

81
guilib/src/ui/editConstraintDialog.ui
View File

@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>347</width>
<height>147</height>
<width>430</width>
<height>231</height>
</rect>
</property>
<property name="windowTitle">
@@ -15,7 +15,7 @@
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1,0,1">
<layout class="QGridLayout" name="gridLayout" columnstretch="0,0,0,0">
<item row="0" column="0">
<widget class="QLabel" name="label">
<property name="text">
@@ -172,6 +172,74 @@
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QDoubleSpinBox" name="linear_sigma">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>6</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>9999.000000000000000</double>
</property>
<property name="singleStep">
<double>0.001000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="3">
<widget class="QDoubleSpinBox" name="angular_sigma">
<property name="suffix">
<string> rad</string>
</property>
<property name="decimals">
<number>6</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>3.150000000000000</double>
</property>
<property name="singleStep">
<double>0.001000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_7">
<property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Linear &amp;sigma; &lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QLabel" name="label_8">
<property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Angular σ&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
</widget>
</item>
<item row="4" column="3">
<widget class="QCheckBox" name="checkBox_radians">
<property name="text">
<string>Radians</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -187,6 +255,13 @@
</property>
</spacer>
</item>
<item>
<widget class="QLabel" name="label_9">
<property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Setting &amp;sigma; to 0 will set identity covariance.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
</widget>
</item>
<item>
<widget class="QDialogButtonBox" name="buttonBox">
<property name="orientation">