Skip to content

Commit

Permalink
Supporting GTSAM bearing/range factors for landmarks (#1346)
Browse files Browse the repository at this point in the history 
* Supporting GTSAM bearing/range factors for landmarks

* refreshing initial covariance after being modified in same sessions
  • Loading branch information
@matlabbe
matlabbe authored Sep 22, 2024
1 parent 0e85bd8 commit 379e11b
Show file tree
Hide file tree
Showing 6 changed files with 528 additions and 324 deletions.
39 changes: 36 additions & 3 deletions corelib/src/optimizer/OptimizerGTSAM.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
Expand Down Expand Up @@ -551,7 +552,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
}
else
else if(1 / static_cast<double>(iter->second.infMatrix().at<double>(1,1)) < 9999)
{
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
Expand All @@ -566,6 +567,21 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
graph.add(gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), p.range(landmark), model));
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
else
{
Eigen::Matrix<double, 1, 1> information = Eigen::Matrix<double, 1, 1>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,1), cv::Range(0,1)).clone();;
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);

gtsam::Point2 landmark(t.x(), t.y());
gtsam::Pose2 p;
graph.add(gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), model));
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
}
else
{
Expand Down Expand Up @@ -599,21 +615,38 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
}
else
else if(1 / static_cast<double>(iter->second.infMatrix().at<double>(2,2)) < 9999)
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();;
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}

gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);

gtsam::Point3 landmark(t.x(), t.y(), t.z());
gtsam::Pose3 p;
graph.add(gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), p.range(landmark), model));
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
else
{
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,2), cv::Range(0,2)).clone();
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}

gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);

gtsam::Point3 landmark(t.x(), t.y(), t.z());
gtsam::Pose3 p;
graph.add(gtsam::BearingFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), model));
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
}
}
}
Expand Down
8 changes: 5 additions & 3 deletions guilib/include/rtabmap/gui/EditConstraintDialog.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,12 +42,14 @@ class RTABMAP_GUI_EXPORT EditConstraintDialog : public QDialog
Q_OBJECT

public:
EditConstraintDialog(const Transform & constraint, double linearSigma = 1, double angularSigma = 1, QWidget * parent = 0);
EditConstraintDialog(const Transform & constraint, const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1), QWidget * parent = 0);

void setPoseGroupVisible(bool visible);
void setCovarianceGroupVisible(bool visible);

virtual ~EditConstraintDialog();
Transform getTransform() const;
double getLinearVariance() const;
double getAngularVariance() const;
cv::Mat getCovariance() const;

private Q_SLOTS:
void switchUnits();
Expand Down
87 changes: 23 additions & 64 deletions guilib/src/DatabaseViewer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4357,13 +4357,19 @@ void DatabaseViewer::updateCovariances(const QList<Link> & links)
{
if(links.size())
{
bool ok = false;
double stddev = QInputDialog::getDouble(this, tr("Linear error"), tr("Std deviation (m) 0=inf"), 0.01, 0.0, 9, 4, &ok);
if(!ok) return;
double linearVar = stddev*stddev;
stddev = QInputDialog::getDouble(this, tr("Angular error"), tr("Std deviation (deg) 0=inf"), 1, 0.0, 90, 2, &ok)*M_PI/180.0;
if(!ok) return;
double angularVar = stddev*stddev;
cv::Mat infMatrix = links.first().infMatrix();
std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, links.first().from() ,links.first().to(), false, links.first().type());
if(findIter != linksRefined_.end())
{
infMatrix = findIter->second.infMatrix();
}

EditConstraintDialog dialog(Transform::getIdentity(), infMatrix.inv());
dialog.setPoseGroupVisible(false);
if(dialog.exec() != QDialog::Accepted)
{
return;
}

rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
progressDialog->setAttribute(Qt::WA_DeleteOnClose);
Expand All @@ -4372,23 +4378,7 @@ void DatabaseViewer::updateCovariances(const QList<Link> & links)
progressDialog->setMinimumWidth(800);
progressDialog->show();

cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(linearVar == 0.0)
{
infMatrix(cv::Range(0,3), cv::Range(0,3)) /= 9999.9;
}
else
{
infMatrix(cv::Range(0,3), cv::Range(0,3)) /= linearVar;
}
if(angularVar == 0.0)
{
infMatrix(cv::Range(3,6), cv::Range(3,6)) /= 9999.9;
}
else
{
infMatrix(cv::Range(3,6), cv::Range(3,6)) /= angularVar;
}
infMatrix = dialog.getCovariance().inv();

for(int i=0; i<links.size(); ++i)
{
Expand Down Expand Up @@ -6074,6 +6064,11 @@ void DatabaseViewer::editConstraint()
else if(ui_->label_type->text().toInt() == Link::kLandmark)
{
link = loopLinks_.at(ui_->horizontalSlider_loops->value());
std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, link.from() ,link.to(), false, link.type());
if(findIter != linksRefined_.end())
{
link = findIter->second;
}
}
else
{
Expand All @@ -6083,28 +6078,10 @@ void DatabaseViewer::editConstraint()
if(link.isValid())
{
cv::Mat covBefore = link.infMatrix().inv();
EditConstraintDialog dialog(link.transform(),
covBefore.at<double>(0,0)<9999.0?std::sqrt(covBefore.at<double>(0,0)):0.0,
covBefore.at<double>(5,5)<9999.0?std::sqrt(covBefore.at<double>(5,5)):0.0);
EditConstraintDialog dialog(link.transform(), covBefore);
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();
}
else
{
covariance(cv::Range(0,3), cv::Range(0,3)) *= 9999.9;
}
if(dialog.getAngularVariance()>0)
{
covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance();
}
else
{
covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9;
}
cv::Mat covariance = dialog.getCovariance();
Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv());
std::multimap<int, Link>::iterator iter = linksRefined_.find(link.from());
while(iter != linksRefined_.end() && iter->first == link.from())
Expand Down Expand Up @@ -6133,28 +6110,10 @@ void DatabaseViewer::editConstraint()
else
{
EditConstraintDialog dialog(
link.transform(),
priorId>0?0.001:1,
priorId>0?0.001:1);
link.transform(), cv::Mat::eye(6,6,CV_64FC1) * (priorId>0?0.00001:1));
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();
}
else
{
covariance(cv::Range(0,3), cv::Range(0,3)) *= 9999.9;
}
if(dialog.getAngularVariance()>0)
{
covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance();
}
else
{
covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9;
}
cv::Mat covariance = dialog.getCovariance();
int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value());
int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value());
Link newLink(
Expand Down
51 changes: 38 additions & 13 deletions guilib/src/EditConstraintDialog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,13 +28,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/gui/EditConstraintDialog.h"
#include "ui_editConstraintDialog.h"

#include <rtabmap/utilite/ULogger.h>
#include <iostream>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace rtabmap {

EditConstraintDialog::EditConstraintDialog(const Transform & constraint, double linearSigma, double angularSigma, QWidget * parent) :
EditConstraintDialog::EditConstraintDialog(const Transform & constraint, const cv::Mat & covariance, QWidget * parent) :
QDialog(parent)
{
_ui = new Ui_EditConstraintDialog();
Expand All @@ -49,9 +52,15 @@ EditConstraintDialog::EditConstraintDialog(const Transform & constraint, double
_ui->pitch->setValue(pitch);
_ui->yaw->setValue(yaw);

UASSERT(covariance.empty() || (covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1));

_ui->checkBox_radians->setChecked(true);
_ui->linear_sigma->setValue(linearSigma);
_ui->angular_sigma->setValue(angularSigma);
_ui->linear_sigma_x->setValue(covariance.empty() || covariance.at<double>(0,0)>=9999 || covariance.at<double>(0,0)<=0?0:sqrt(covariance.at<double>(0,0)));
_ui->linear_sigma_y->setValue(covariance.empty() || covariance.at<double>(1,1)>=9999 || covariance.at<double>(1,1)<=0?0:sqrt(covariance.at<double>(1,1)));
_ui->linear_sigma_z->setValue(covariance.empty() || covariance.at<double>(2,2)>=9999 || covariance.at<double>(2,2)<=0?0:sqrt(covariance.at<double>(2,2)));
_ui->angular_sigma_roll->setValue(covariance.empty() || covariance.at<double>(3,3)>=9999 || covariance.at<double>(3,3)<=0?0:sqrt(covariance.at<double>(3,3)));
_ui->angular_sigma_pitch->setValue(covariance.empty() || covariance.at<double>(4,4)>=9999 || covariance.at<double>(4,4)<=0?0:sqrt(covariance.at<double>(4,4)));
_ui->angular_sigma_yaw->setValue(covariance.empty() || covariance.at<double>(5,5)>=9999 || covariance.at<double>(5,5)<=0?0:sqrt(covariance.at<double>(5,5)));

connect(_ui->checkBox_radians, SIGNAL(stateChanged(int)), this, SLOT(switchUnits()));
}
Expand All @@ -61,6 +70,15 @@ EditConstraintDialog::~EditConstraintDialog()
delete _ui;
}

void EditConstraintDialog::setPoseGroupVisible(bool visible)
{
_ui->groupBox_pose->setVisible(visible);
}
void EditConstraintDialog::setCovarianceGroupVisible(bool visible)
{
_ui->groupBox_covariance->setVisible(visible);
}

void EditConstraintDialog::switchUnits()
{
double conversion = 180.0/M_PI;
Expand All @@ -72,13 +90,15 @@ void EditConstraintDialog::switchUnits()
boxes.push_back(_ui->roll);
boxes.push_back(_ui->pitch);
boxes.push_back(_ui->yaw);
boxes.push_back(_ui->angular_sigma);
boxes.push_back(_ui->angular_sigma_roll);
boxes.push_back(_ui->angular_sigma_pitch);
boxes.push_back(_ui->angular_sigma_yaw);
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)
if(boxes[i]!=_ui->angular_sigma_roll && boxes[i]!=_ui->angular_sigma_pitch && boxes[i]!=_ui->angular_sigma_yaw)
{
boxes[i]->setMinimum(-M_PI);
}
Expand All @@ -88,7 +108,7 @@ void EditConstraintDialog::switchUnits()
}
else
{
if(boxes[i]!=_ui->angular_sigma)
if(boxes[i]!=_ui->angular_sigma_roll && boxes[i]!=_ui->angular_sigma_pitch && boxes[i]!=_ui->angular_sigma_yaw)
{
boxes[i]->setMinimum(-180);
}
Expand All @@ -110,19 +130,24 @@ Transform EditConstraintDialog::getTransform() const
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()*_ui->linear_sigma->value();
}
double EditConstraintDialog::getAngularVariance() const
cv::Mat EditConstraintDialog::getCovariance() const
{
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
covariance.at<double>(0,0) = _ui->linear_sigma_x->value()==0?9999:_ui->linear_sigma_x->value()*_ui->linear_sigma_x->value();
covariance.at<double>(1,1) = _ui->linear_sigma_y->value()==0?9999:_ui->linear_sigma_y->value()*_ui->linear_sigma_y->value();
covariance.at<double>(2,2) = _ui->linear_sigma_z->value()==0?9999:_ui->linear_sigma_z->value()*_ui->linear_sigma_z->value();
double conversion = 1.0f;
if(!_ui->checkBox_radians->isChecked())
{
conversion = M_PI/180.0;
}
double value = _ui->angular_sigma->value()*conversion;
return value*value;
double sigma = _ui->angular_sigma_roll->value()*conversion;
covariance.at<double>(3,3) = sigma==0?9999:sigma*sigma;
sigma = _ui->angular_sigma_pitch->value()*conversion;
covariance.at<double>(4,4) = sigma==0?9999:sigma*sigma;
sigma = _ui->angular_sigma_yaw->value()*conversion;
covariance.at<double>(5,5) = sigma==0?9999:sigma*sigma;
return covariance;
}

}
Loading

0 comments on commit 379e11b

Please sign in to comment.