DbViewer: added update all landmark covariances menu action, also enabled edit constraint on landmark links. Covariance can be set to 9999.

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -122,6 +122,7 @@ private Q_SLOTS:
 	void detectMoreLoopClosures();
 	void updateAllNeighborCovariances();
 	void updateAllLoopClosureCovariances();
+	void updateAllLandmarkCovariances();
 	void refineAllNeighborLinks();
 	void refineAllLoopClosureLinks();
 	void resetAllChanges();

guilib/include/rtabmap/gui/EditConstraintDialog.h

@@ -42,7 +42,7 @@ class RTABMAPGUI_EXP EditConstraintDialog : public QDialog
 	Q_OBJECT
 
 public:
-	EditConstraintDialog(const Transform & constraint, double linearSigma = 0, double angularSigma = 0, QWidget * parent = 0);
+	EditConstraintDialog(const Transform & constraint, double linearSigma = 1, double angularSigma = 1, QWidget * parent = 0);
 
 	virtual ~EditConstraintDialog();
 	Transform getTransform() const;

guilib/src/DatabaseViewer.cpp

@@ -292,6 +292,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
 	connect(ui_->actionUpdate_all_neighbor_covariances, SIGNAL(triggered()), this, SLOT(updateAllNeighborCovariances()));
 	connect(ui_->actionUpdate_all_loop_closure_covariances, SIGNAL(triggered()), this, SLOT(updateAllLoopClosureCovariances()));
+	connect(ui_->actionUpdate_all_landmark_covariances, SIGNAL(triggered()), this, SLOT(updateAllLandmarkCovariances()));
 	connect(ui_->actionRefine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineAllNeighborLinks()));
 	connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
 	connect(ui_->actionRegenerate_local_grid_maps, SIGNAL(triggered()), this, SLOT(regenerateLocalMaps()));
@@ -4128,7 +4129,27 @@ void DatabaseViewer::updateAllNeighborCovariances()
 {
 	updateAllCovariances(neighborLinks_);
 }
-void DatabaseViewer::updateAllLoopClosureCovariances()
+void DatabaseViewer::updateAllLoopClosureCovariances()
+{
+	QList<rtabmap::Link> links;
+	for(int i=0; i<loopLinks_.size(); ++i)
+	{
+		if(loopLinks_.at(i).type() != Link::kLandmark)
+		{
+			links.push_back(loopLinks_.at(i));
+		}
+	}
+	updateAllCovariances(links);
+}
+void DatabaseViewer::updateAllLandmarkCovariances()
+{
+	QList<rtabmap::Link> links;
+	for(int i=0; i<loopLinks_.size(); ++i)
+	{
+		if(loopLinks_.at(i).type() == Link::kLandmark)
+		{
+			links.push_back(loopLinks_.at(i));
+		}
 	}
 	updateAllCovariances(links);
 }
@@ -4136,10 +4157,10 @@ void DatabaseViewer::updateAllCovariances(const QList<Link> & links)
 void DatabaseViewer::updateAllCovariances(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;
+		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;
@@ -4151,8 +4172,22 @@ void DatabaseViewer::updateAllCovariances(const QList<Link> & links)
 		progressDialog->setCancelButtonVisible(true);
 		progressDialog->setMinimumWidth(800);
 		progressDialog->show();
-
+
-		cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
+		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;
 		}
 
@@ -5602,24 +5637,41 @@ void DatabaseViewer::editConstraint()
 
 void DatabaseViewer::editConstraint()
 {
-	if(ids_.size())
+	if(ids_.size())
+	{
+		Link link;
+		if(ui_->label_type->text().toInt() == Link::kLandmark)
+		{
+			int position = ui_->horizontalSlider_loops->value();
+			link = loopLinks_.at(position);
+		}
+		else
+		{
 			link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
 		}
 		if(link.isValid())
 		{
-			cv::Mat covBefore = link.infMatrix().inv();
+			cv::Mat covBefore = link.infMatrix().inv();
-			EditConstraintDialog dialog(link.transform(),
+			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);
 			if(dialog.exec() == QDialog::Accepted)
 			{
 				bool updated = false;
 				cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
-				if(dialog.getLinearVariance()>0)
+				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)
+				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;
 				}
@@ -5654,11 +5706,19 @@ void DatabaseViewer::editConstraint()
 			if(dialog.exec() == QDialog::Accepted)
 			{
 				cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
-				if(dialog.getLinearVariance()>0)
+				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)
+				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;
 				}
@@ -6315,29 +6375,36 @@ void DatabaseViewer::updateConstraintButtons()
 	ui_->pushButton_reset->setEnabled(false);
 	ui_->pushButton_add->setEnabled(false);
 	ui_->pushButton_reject->setEnabled(false);
+	ui_->toolButton_constraint->setEnabled(false);
+
+	Link currentLink;
 	int from;
 	int to;
-	if(ui_->label_type->text().toInt() == Link::kLandmark)
+	if(ui_->label_type->text().toInt() == Link::kLandmark)
-	{
+	{
+		//check for modified link
+		currentLink = loopLinks_.at(ui_->horizontalSlider_loops->value());
 		from = currentLink.from();
-		return;
+		to = currentLink.to();
-	}
-
-	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() &&
-	   (ui_->checkBox_enableForAll->isChecked() ||
 	}
-	   weights_.find(to) != weights_.end() && weights_.at(to)>=0)))
+	else
-	{
+	{
+		from = ids_.at(ui_->horizontalSlider_A->value());
+		to = ids_.at(ui_->horizontalSlider_B->value());
+		if(from!=to && from && to &&
+		   odomPoses_.find(from) != odomPoses_.end() &&
+		   odomPoses_.find(to) != odomPoses_.end() &&
+		   (ui_->checkBox_enableForAll->isChecked() ||
 		   (weights_.find(from) != weights_.end() && weights_.at(from)>=0 &&
-			containsLink(linksRemoved_, from ,to))
+		   weights_.find(to) != weights_.end() && weights_.at(to)>=0)))
+		{
+			if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) ||
+				containsLink(linksRemoved_, from ,to))
+			{
 				ui_->pushButton_add->setEnabled(true);
-			ui_->pushButton_add->setEnabled(true);
 			}
-	}
+		}
+
 		currentLink = findActiveLink(from ,to);
 	}
 
@@ -6348,19 +6415,13 @@ void DatabaseViewer::updateConstraintButtons()
 		{
 			ui_->pushButton_reject->setEnabled(true);
 		}
-
 
 		//check for modified link
 		std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
 		if(iter != linksRefined_.end())
 		{
-			currentLink = iter->second;
 			currentLink = iter->second;
-			modified = true;
+			ui_->pushButton_reset->setEnabled(true);
-		}
-		if(!modified)
-		{
-			ui_->pushButton_reset->setEnabled(false);
 		}
 		ui_->pushButton_refine->setEnabled(currentLink.from()!=currentLink.to() && currentLink.type() != Link::kLandmark);
 		ui_->toolButton_constraint->setEnabled(true);
@@ -8194,6 +8255,14 @@ void DatabaseViewer::resetConstraint()
 
 void DatabaseViewer::resetConstraint()
 {
+	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 position = ui_->horizontalSlider_loops->value();
+		const rtabmap::Link & link = loopLinks_.at(position);
+		from = link.from();
+		to = link.to();
 	}
 
 	if(from < to)

guilib/src/EditConstraintDialog.cpp

@@ -48,11 +48,12 @@ EditConstraintDialog::EditConstraintDialog(const Transform & constraint, double
 	_ui->roll->setValue(roll);
 	_ui->pitch->setValue(pitch);
 	_ui->yaw->setValue(yaw);
+
+	_ui->checkBox_radians->setChecked(true);
 	_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()
@@ -111,7 +112,7 @@ Transform EditConstraintDialog::getTransform() const
 
 double EditConstraintDialog::getLinearVariance() const
 {
-	return _ui->linear_sigma->value();
+	return _ui->linear_sigma->value()*_ui->linear_sigma->value();
 }
 double EditConstraintDialog::getAngularVariance() const
 {
@@ -120,7 +121,8 @@ double EditConstraintDialog::getAngularVariance() const
 	{
 		conversion = M_PI/180.0;
 	}
-	return _ui->angular_sigma->value()*conversion;
+	double value = _ui->angular_sigma->value()*conversion;
+	return value*value;
 }
 
 }

guilib/src/ui/DatabaseViewer.ui

@@ -745,6 +745,7 @@
     <addaction name="actionRefine_all_loop_closure_links"/>
     <addaction name="actionUpdate_all_neighbor_covariances"/>
     <addaction name="actionUpdate_all_loop_closure_covariances"/>
+    <addaction name="actionUpdate_all_landmark_covariances"/>
     <addaction name="separator"/>
     <addaction name="actionRegenerate_local_grid_maps"/>
     <addaction name="actionRegenerate_local_grid_maps_selected"/>
@@ -1542,7 +1543,7 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>-322</y>
+          <y>0</y>
           <width>518</width>
           <height>911</height>
          </rect>
@@ -2137,7 +2138,7 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>298</width>
+          <width>226</width>
           <height>192</height>
          </rect>
         </property>
@@ -3015,6 +3016,11 @@
     <string>RGBD-SLAM ID format (*.txt)</string>
    </property>
   </action>
+  <action name="actionUpdate_all_landmark_covariances">
+   <property name="text">
+    <string>Update all landmark covariances...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/editConstraintDialog.ui

@@ -258,7 +258,7 @@
    <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>
+      <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>