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

2
corelib/src/Memory.cpp
View File

@@ -3710,6 +3710,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
{ {
_lastGlobalLoopClosureId = newS->id(); _lastGlobalLoopClosureId = newS->id();
} }
oldS->setWeight(-9);
} }
else else
{ {
@@ -3722,6 +3723,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
{ {
_lastSignature = oldS; _lastSignature = oldS;
} }
newS->setWeight(-9);
} }
// remove location // remove location

7
guilib/include/rtabmap/gui/EditConstraintDialog.h
View File

@@ -42,10 +42,15 @@ class RTABMAPGUI_EXP EditConstraintDialog : public QDialog
Q_OBJECT Q_OBJECT
public: public:
EditConstraintDialog(const Transform & constraint, QWidget * parent = 0); EditConstraintDialog(const Transform & constraint, double linearSigma = 0, double angularSigma = 0, QWidget * parent = 0);
virtual ~EditConstraintDialog(); virtual ~EditConstraintDialog();
Transform getTransform() const; Transform getTransform() const;
double getLinearVariance() const;
double getAngularVariance() const;
private Q_SLOTS:
void switchUnits();
private: private:
Ui_EditConstraintDialog * _ui; Ui_EditConstraintDialog * _ui;

84
guilib/src/DatabaseViewer.cpp
View File

@@ -1068,6 +1068,9 @@ bool DatabaseViewer::closeDatabase()
ui_->label_constraint_opt->clear(); ui_->label_constraint_opt->clear();
ui_->label_variance->clear(); ui_->label_variance->clear();
ui_->lineEdit_covariance->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->setEnabled(false);
ui_->horizontalSlider_A->setMaximum(0); ui_->horizontalSlider_A->setMaximum(0);
@@ -4880,9 +4883,11 @@ void DatabaseViewer::update(int value,
ui_->horizontalSlider_neighbors->blockSignals(false); ui_->horizontalSlider_neighbors->blockSignals(false);
constraintsViewer_->removeAllClouds(); constraintsViewer_->removeAllClouds();
Link link = this->findActiveLink(from, to); Link link = this->findActiveLink(from, to);
bool constraintViewUpdated = false; bool constraintViewUpdated = false;
if(link.isValid() && link.type() != Link::kGravity) if(link.isValid() && link.type() != Link::kGravity)
{
this->updateConstraintView(link, false); this->updateConstraintView(link, false);
constraintViewUpdated = true; constraintViewUpdated = true;
} }
@@ -4897,9 +4902,20 @@ void DatabaseViewer::update(int value,
if(fromIter != optimizedPoses.end() && if(fromIter != optimizedPoses.end() &&
toIter != optimizedPoses.end()) toIter != optimizedPoses.end())
{ {
Link link(from, to, Link::kUndef, fromIter->second.inverse() * toIter->second);
this->updateConstraintView(link, false); this->updateConstraintView(link, false);
constraintViewUpdated = true; 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); ui_->checkBox_showOptimized->setEnabled(false);
} }
constraintsViewer_->update(); constraintsViewer_->update();
@@ -5259,12 +5275,23 @@ void DatabaseViewer::editConstraint()
if(ids_.size()) if(ids_.size())
{ {
Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value())); 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>(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); covBefore.at<double>(5,5)!=1.0?std::sqrt(covBefore.at<double>(5,5)):0);
if(dialog.exec() == QDialog::Accepted) 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()); Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv());
std::multimap<int, Link>::iterator iter = linksRefined_.find(link.from()); std::multimap<int, Link>::iterator iter = linksRefined_.find(link.from());
@@ -5285,9 +5312,41 @@ void DatabaseViewer::editConstraint()
updated = true; updated = true;
} }
if(updated) if(updated)
{
updateConstraintView(); updateConstraintView();
this->updateGraphView(); 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); float diff = topt.getDistance(t);
Transform v1 = t.rotation()*Transform(1,0,0,0,0,0); Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
Transform v2 = topt.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; 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)); 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; 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(from) != weights_.end() && weights_.at(from)>=0,
weights_.find(to) != weights_.end() && weights_.at(to)>=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); 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; 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 { namespace rtabmap {
EditConstraintDialog::EditConstraintDialog(const Transform & constraint, QWidget * parent) : EditConstraintDialog::EditConstraintDialog(const Transform & constraint, double linearSigma, double angularSigma, QWidget * parent) :
QDialog(parent) QDialog(parent)
{ {
_ui = new Ui_EditConstraintDialog(); _ui = new Ui_EditConstraintDialog();
@@ -44,6 +44,11 @@ EditConstraintDialog::EditConstraintDialog(const Transform & constraint, QWidget
_ui->roll->setValue(roll); _ui->roll->setValue(roll);
_ui->pitch->setValue(pitch); _ui->pitch->setValue(pitch);
_ui->yaw->setValue(yaw); _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() EditConstraintDialog::~EditConstraintDialog()
@@ -51,9 +56,67 @@ EditConstraintDialog::~EditConstraintDialog()
delete _ui; 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 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> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>347</width> <width>430</width>
<height>147</height> <height>231</height>
</rect> </rect>
</property> </property>
<property name="windowTitle"> <property name="windowTitle">
@@ -15,7 +15,7 @@
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout"> <layout class="QVBoxLayout" name="verticalLayout">
<item> <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"> <item row="0" column="0">
<widget class="QLabel" name="label"> <widget class="QLabel" name="label">
<property name="text"> <property name="text">
@@ -172,6 +172,74 @@
</property> </property>
</widget> </widget>
</item> </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> </layout>
</item> </item>
<item> <item>
@@ -187,6 +255,13 @@
</property> </property>
</spacer> </spacer>
</item> </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> <item>
<widget class="QDialogButtonBox" name="buttonBox"> <widget class="QDialogButtonBox" name="buttonBox">
<property name="orientation"> <property name="orientation">

2
tools/Reprocess/main.cpp
View File

@@ -575,7 +575,7 @@ int main(int argc, char * argv[])
int processed = 0; int processed = 0;
CameraInfo info; CameraInfo info;
SensorData data = dbReader->takeImage(&info); SensorData data = dbReader->takeImage(&info);
CameraThread camThread(dbReader, parameters); CameraThread camThread(dbReader, parameters); // take ownership of dbReader
camThread.setScanParameters(scanFromDepth, scanDecimation, scanRangeMin, scanRangeMax, scanVoxelSize, scanNormalK, scanNormalRadius); camThread.setScanParameters(scanFromDepth, scanDecimation, scanRangeMin, scanRangeMax, scanVoxelSize, scanNormalK, scanNormalRadius);
if(scanFromDepth) if(scanFromDepth)
{ {