Supporting GTSAM bearing/range factors for landmarks (#1346)

* Supporting GTSAM bearing/range factors for landmarks * refreshing initial covariance after being modified in same sessions
2026-09-02 17:40:23 +08:00 · 2024-09-21 18:14:52 -07:00
parent 0e85bd849c
commit 379e11bd66
6 changed files with 528 additions and 324 deletions
--- a/guilib/include/rtabmap/gui/EditConstraintDialog.h
+++ b/guilib/include/rtabmap/gui/EditConstraintDialog.h
@@ -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();
--- a/guilib/src/DatabaseViewer.cpp
+++ b/guilib/src/DatabaseViewer.cpp
@@ -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);
@@ -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)
 		{
@@ -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
 		{
@@ -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())
@@ -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(
--- a/guilib/src/EditConstraintDialog.cpp
+++ b/guilib/src/EditConstraintDialog.cpp
@@ -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();
@@ -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()));
 }
@@ -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;
@@ -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);
 			}
@@ -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);
 			}
@@ -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;
 }

 }
--- a/guilib/src/ui/editConstraintDialog.ui
+++ b/guilib/src/ui/editConstraintDialog.ui
@@ -6,8 +6,8 @@
   <rect>
    <x>0</x>
    <y>0</y>
-    <width>430</width>
-    <height>231</height>
+    <width>393</width>
+    <height>360</height>
   </rect>
  </property>
  <property name="windowTitle">
@@ -15,232 +15,352 @@
  </property>
  <layout class="QVBoxLayout" name="verticalLayout">
   <item>
-    <layout class="QGridLayout" name="gridLayout" columnstretch="0,0,0,0">
-     <item row="0" column="0">
-      <widget class="QLabel" name="label">
-       <property name="text">
-        <string>x</string>
-       </property>
-      </widget>
-     </item>
-     <item row="0" column="1">
-      <widget class="QDoubleSpinBox" name="x">
-       <property name="suffix">
-        <string> m</string>
-       </property>
-       <property name="decimals">
-        <number>6</number>
-       </property>
-       <property name="minimum">
-        <double>-9999.000000000000000</double>
-       </property>
-       <property name="maximum">
-        <double>9999.000000000000000</double>
-       </property>
-       <property name="singleStep">
-        <double>0.001000000000000</double>
-       </property>
-      </widget>
-     </item>
-     <item row="0" column="2">
-      <widget class="QLabel" name="label_4">
-       <property name="text">
-        <string>roll</string>
-       </property>
-      </widget>
-     </item>
-     <item row="0" column="3">
-      <widget class="QDoubleSpinBox" name="roll">
-       <property name="suffix">
-        <string> rad</string>
-       </property>
-       <property name="decimals">
-        <number>6</number>
-       </property>
-       <property name="minimum">
-        <double>-3.150000000000000</double>
-       </property>
-       <property name="maximum">
-        <double>3.150000000000000</double>
-       </property>
-       <property name="singleStep">
-        <double>0.001000000000000</double>
-       </property>
-      </widget>
-     </item>
-     <item row="1" column="0">
-      <widget class="QLabel" name="label_2">
-       <property name="text">
-        <string>y</string>
-       </property>
-      </widget>
-     </item>
-     <item row="1" column="1">
-      <widget class="QDoubleSpinBox" name="y">
-       <property name="suffix">
-        <string> m</string>
-       </property>
-       <property name="decimals">
-        <number>6</number>
-       </property>
-       <property name="minimum">
-        <double>-9999.000000000000000</double>
-       </property>
-       <property name="maximum">
-        <double>9999.000000000000000</double>
-       </property>
-       <property name="singleStep">
-        <double>0.001000000000000</double>
-       </property>
-      </widget>
-     </item>
-     <item row="1" column="2">
-      <widget class="QLabel" name="label_5">
-       <property name="text">
-        <string>pitch</string>
-       </property>
-      </widget>
-     </item>
-     <item row="1" column="3">
-      <widget class="QDoubleSpinBox" name="pitch">
-       <property name="suffix">
-        <string> rad</string>
-       </property>
-       <property name="decimals">
-        <number>6</number>
-       </property>
-       <property name="minimum">
-        <double>-3.150000000000000</double>
-       </property>
-       <property name="maximum">
-        <double>3.150000000000000</double>
-       </property>
-       <property name="singleStep">
-        <double>0.001000000000000</double>
-       </property>
-      </widget>
-     </item>
-     <item row="2" column="0">
-      <widget class="QLabel" name="label_3">
-       <property name="text">
-        <string>z</string>
-       </property>
-      </widget>
-     </item>
-     <item row="2" column="1">
-      <widget class="QDoubleSpinBox" name="z">
-       <property name="suffix">
-        <string> m</string>
-       </property>
-       <property name="decimals">
-        <number>6</number>
-       </property>
-       <property name="minimum">
-        <double>-9999.000000000000000</double>
-       </property>
-       <property name="maximum">
-        <double>9999.000000000000000</double>
-       </property>
-       <property name="singleStep">
-        <double>0.001000000000000</double>
-       </property>
-      </widget>
-     </item>
-     <item row="2" column="2">
-      <widget class="QLabel" name="label_6">
-       <property name="text">
-        <string>yaw</string>
-       </property>
-      </widget>
-     </item>
-     <item row="2" column="3">
-      <widget class="QDoubleSpinBox" name="yaw">
-       <property name="suffix">
-        <string> rad</string>
-       </property>
-       <property name="decimals">
-        <number>6</number>
-       </property>
-       <property name="minimum">
-        <double>-3.150000000000000</double>
-       </property>
-       <property name="maximum">
-        <double>3.150000000000000</double>
-       </property>
-       <property name="singleStep">
-        <double>0.001000000000000</double>
-       </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>
+    <widget class="QGroupBox" name="groupBox_pose">
+     <property name="title">
+      <string>Pose</string>
+     </property>
+     <layout class="QGridLayout" name="gridLayout" columnstretch="0,1,0,1">
+      <item row="0" column="0">
+       <widget class="QLabel" name="label">
+        <property name="text">
+         <string>x</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QDoubleSpinBox" name="x">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>-9999.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>9999.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="2">
+       <widget class="QLabel" name="label_4">
+        <property name="text">
+         <string>roll</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="3">
+       <widget class="QDoubleSpinBox" name="roll">
+        <property name="suffix">
+         <string> rad</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>-3.150000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>3.150000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_2">
+        <property name="text">
+         <string>y</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="1">
+       <widget class="QDoubleSpinBox" name="y">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>-9999.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>9999.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="2">
+       <widget class="QLabel" name="label_5">
+        <property name="text">
+         <string>pitch</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="3">
+       <widget class="QDoubleSpinBox" name="pitch">
+        <property name="suffix">
+         <string> rad</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>-3.150000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>3.150000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="0">
+       <widget class="QLabel" name="label_3">
+        <property name="text">
+         <string>z</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="1">
+       <widget class="QDoubleSpinBox" name="z">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>-9999.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>9999.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="2">
+       <widget class="QLabel" name="label_6">
+        <property name="text">
+         <string>yaw</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="3">
+       <widget class="QDoubleSpinBox" name="yaw">
+        <property name="suffix">
+         <string> rad</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>-3.150000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>3.150000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <widget class="QGroupBox" name="groupBox_covariance">
+     <property name="title">
+      <string>Covariance</string>
+     </property>
+     <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1,0,1">
+      <item row="0" column="3">
+       <widget class="QDoubleSpinBox" name="angular_sigma_roll">
+        <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="1" column="2">
+       <widget class="QLabel" name="label_10">
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;σ pitch&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="0">
+       <widget class="QLabel" name="label_7">
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;σ  x&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="2">
+       <widget class="QLabel" name="label_8">
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;σ roll&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="3">
+       <widget class="QDoubleSpinBox" name="angular_sigma_pitch">
+        <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="0" column="1">
+       <widget class="QDoubleSpinBox" name="linear_sigma_x">
+        <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="2" column="3">
+       <widget class="QDoubleSpinBox" name="angular_sigma_yaw">
+        <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="1" column="1">
+       <widget class="QDoubleSpinBox" name="linear_sigma_y">
+        <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="2" column="1">
+       <widget class="QDoubleSpinBox" name="linear_sigma_z">
+        <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="2" column="2">
+       <widget class="QLabel" name="label_11">
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;σ yaw&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_12">
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;σ y&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="0">
+       <widget class="QLabel" name="label_13">
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;σ z&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
   </item>
   <item>
    <spacer name="verticalSpacer">
@@ -256,11 +376,38 @@
    </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 σ to 0 will set 9999 covariance.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-     </property>
-    </widget>
+    <layout class="QHBoxLayout" name="horizontalLayout">
+     <item>
+      <widget class="QLabel" name="label_9">
+       <property name="text">
+        <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Setting σ to 0 will set 9999 covariance.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+     <item>
+      <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>
    <widget class="QDialogButtonBox" name="buttonBox">