DatabaseViewer: added graph depth param. Rtabmap: when the robot is not moving, now deleting location "after" retrieval

corelib/src/Graph.cpp

@@ -148,16 +148,16 @@ void Optimizer::getConnectedGraph(
 	linksOut.clear();
 
 	std::set<int> ids;
-	std::list<int> curentDepthList;
+	std::set<int> curentDepth;
 	std::set<int> nextDepth;
 	nextDepth.insert(fromId);
 	int d = 0;
 	while((depth == 0 || d < depth) && nextDepth.size())
 	{
-		curentDepthList = std::list<int>(nextDepth.begin(), nextDepth.end());
+		curentDepth = nextDepth;
 		nextDepth.clear();
 
-		for(std::list<int>::iterator jter = curentDepthList.begin(); jter!=curentDepthList.end(); ++jter)
+		for(std::set<int>::iterator jter = curentDepth.begin(); jter!=curentDepth.end(); ++jter)
 		{
 			if(ids.find(*jter) == ids.end())
 			{
@@ -170,16 +170,33 @@ void Optimizer::getConnectedGraph(
 					{
 						if(ids.find(iter->second.to()) == ids.end() && uContains(posesIn, iter->second.to()))
 						{
-							linksOut.insert(*iter);
 							nextDepth.insert(iter->second.to());
+							if(depth == 0 || d < depth-1)
+							{
+								linksOut.insert(*iter);
+							}
+							else if(curentDepth.find(iter->second.to()) != curentDepth.end() ||
+									ids.find(iter->second.to()) != ids.end())
+							{
+								linksOut.insert(*iter);
+							}
 						}
 					}
 					else if(iter->second.to() == *jter)
 					{
 						if(ids.find(iter->second.from()) == ids.end() && uContains(posesIn, iter->second.from()))
 						{
-							linksOut.insert(*iter);
 							nextDepth.insert(iter->second.from());
+
+							if(depth == 0 || d < depth-1)
+							{
+								linksOut.insert(*iter);
+							}
+							else if(curentDepth.find(iter->second.from()) != curentDepth.end() ||
+									ids.find(iter->second.from()) != ids.end())
+							{
+								linksOut.insert(*iter);
+							}
 						}
 					}
 				}
@@ -199,7 +216,7 @@ std::map<int, Transform> TOROOptimizer::optimize(
 		std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
 {
 	std::map<int, Transform> optimizedPoses;
-	UDEBUG("Optimizing graph...");
+	UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
 	if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
 	{
 		// Apply TORO optimization
@@ -263,6 +280,8 @@ std::map<int, Transform> TOROOptimizer::optimize(
 				int id2 = iter->second.to();
 				AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
 				AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
+				UASSERT(v1 != 0);
+				UASSERT(v2 != 0);
 				AISNavigation::TreePoseGraph2::Transformation t(p);
 				if (!pg2.addEdge(v1, v2, t, inf))
 				{
@@ -300,6 +319,8 @@ std::map<int, Transform> TOROOptimizer::optimize(
 				int id2 = iter->second.to();
 				AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
 				AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
+				UASSERT(v1 != 0);
+				UASSERT(v2 != 0);
 				AISNavigation::TreePoseGraph3::Transformation t(p);
 				if (!pg3.addEdge(v1, v2, t, inf))
 				{
@@ -652,6 +673,8 @@ std::map<int, Transform> G2OOptimizer::optimize(
 				g2o::EdgeSE2 * e = new g2o::EdgeSE2();
 				g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
 				g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
+				UASSERT(v1 != 0);
+				UASSERT(v2 != 0);
 				e->setVertex(0, v1);
 				e->setVertex(1, v2);
 				e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
@@ -685,6 +708,8 @@ std::map<int, Transform> G2OOptimizer::optimize(
 				g2o::EdgeSE3 * e = new g2o::EdgeSE3();
 				g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
 				g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
+				UASSERT(v1 != 0);
+				UASSERT(v2 != 0);
 				e->setVertex(0, v1);
 				e->setVertex(1, v2);
 				e->setMeasurement(constraint);

corelib/src/Rtabmap.cpp

@@ -855,6 +855,7 @@ bool Rtabmap::process(const SensorData & data)
 	//============================================================
 	// Metric
 	//============================================================
+	bool smallDisplacement = false;
 	if(_rgbdSlamMode)
 	{
 		//Verify if there was a rehearsal
@@ -882,10 +883,9 @@ bool Rtabmap::process(const SensorData & data)
 					 fabs(pitch) < _rgbdAngularUpdate &&
 					 fabs(yaw) < _rgbdAngularUpdate)))
 				{
-					UWARN("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
+					// This will disable global loop closure detection, only retrieval will be done.
-							signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
+					// The location will also be deleted at the end.
-					_memory->deleteLocation(signature->id());
+					smallDisplacement = true;
-					return false;
 				}
 			}
 		}
@@ -1003,7 +1003,7 @@ bool Rtabmap::process(const SensorData & data)
 	//============================================================
 	// Bayes filter update
 	//============================================================
-	if(!signature->isBadSignature())
+	if(!signature->isBadSignature() && !smallDisplacement)
 	{
 		// If the working memory is empty, don't do the detection. It happens when it
 		// is the first time the detector is started (there needs some images to
@@ -1037,6 +1037,11 @@ bool Rtabmap::process(const SensorData & data)
 			timePosteriorCalculation = timer.ticks();
 			ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation);
 
+			// For statistics, copy weights
+			if(_publishStats && (_publishLikelihood || _publishPdf))
+			{
+				weights = _memory->getWeights();
+			}
 
 			timer.start();
 			//============================================================
@@ -1106,6 +1111,10 @@ bool Rtabmap::process(const SensorData & data)
 			}
 		} // if(_memory->getWorkingMemSize())
 	}// !isBadSignature
+	else if(!signature->isBadSignature() && smallDisplacement)
+	{
+		_highestHypothesis = lastHighestHypothesis;
+	}
 
 	//============================================================
 	// Before retrieval, make sure the trash has finished
@@ -1115,12 +1124,6 @@ bool Rtabmap::process(const SensorData & data)
 	timeJoiningTrash = timer.ticks();
 	ULOGGER_INFO("Time emptying memory trash = %fs,  joining (actual overhead) = %fs", timeEmptyingTrash, timeJoiningTrash);
 
-	// also copy weights before the memory is changed
-	if(_publishStats && (_publishLikelihood || _publishPdf))
-	{
-		weights = _memory->getWeights();
-	}
-
 	//============================================================
 	// RETRIEVAL 1/3 : Loop closure neighbors reactivation
 	//============================================================
@@ -1771,6 +1774,15 @@ bool Rtabmap::process(const SensorData & data)
 		signature->getLinks().size() == 0 &&     // alone in the current map
 		_memory->getWorkingMem().size()>1)       // The working memory should not be empty
 	{
+		UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
+				signature->id());
+		_memory->deleteLocation(signature->id());
+	}
+	else if(smallDisplacement)
+	{
+		UWARN("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
+			  signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
+		// If there is a too small displacement, remove the node
 		_memory->deleteLocation(signature->id());
 	}
 

guilib/src/DatabaseViewer.cpp

@@ -194,6 +194,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
+	connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
+
 
 	connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(updateGrid()));
 	connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
@@ -210,6 +212,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+	connect(ui_->spinBox_optimizationDepth, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->spinBox_projDecimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
@@ -309,6 +312,7 @@ void DatabaseViewer::readSettings()
 	ui_->checkBox_ignoreCovariance->setChecked(settings.value("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked()).toBool());
 	ui_->comboBox_graphOptimizer->setCurrentIndex(settings.value("strategy", ui_->comboBox_graphOptimizer->currentIndex()).toInt());
 	ui_->checkBox_2dslam->setChecked(settings.value("slam2d", ui_->checkBox_2dslam->isChecked()).toBool());
+	ui_->spinBox_optimizationDepth->setValue(settings.value("depth", ui_->spinBox_optimizationDepth->value()).toInt());
 	settings.endGroup();
 
 	settings.beginGroup("grid");
@@ -380,6 +384,7 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked());
 	settings.setValue("strategy", ui_->comboBox_graphOptimizer->currentIndex());
 	settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
+	settings.setValue("depth", ui_->spinBox_optimizationDepth->value());
 	settings.endGroup();
 
 	// save Grid settings
@@ -947,7 +952,13 @@ void DatabaseViewer::view3DMap()
 		double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
 		if(ok)
 		{
-			const std::map<int, Transform> & optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+			std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+			if(ui_->groupBox_posefiltering->isChecked())
+			{
+				optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
+						ui_->doubleSpinBox_posefilteringRadius->value(),
+						ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
+			}
 			if(optimizedPoses.size() > 0)
 			{
 				rtabmap::DetailedProgressDialog progressDialog(this);
@@ -1067,7 +1078,13 @@ void DatabaseViewer::generate3DMap()
 			QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
 			if(!path.isEmpty())
 			{
-				const std::map<int, Transform> & optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+				std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+				if(ui_->groupBox_posefiltering->isChecked())
+				{
+					optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
+							ui_->doubleSpinBox_posefilteringRadius->value(),
+							ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
+				}
 				if(optimizedPoses.size() > 0)
 				{
 					rtabmap::DetailedProgressDialog progressDialog;
@@ -2302,7 +2319,8 @@ void DatabaseViewer::updateGraphView()
 				poses_,
 				links,
 				posesOut,
-				linksOut);
+				linksOut,
+				ui_->spinBox_optimizationDepth->value());
 
 		QTime time;
 		time.start();

guilib/src/ui/DatabaseViewer.ui

@@ -1186,7 +1186,7 @@
          </property>
         </widget>
        </item>
-       <item row="3" column="1">
+       <item row="4" column="1">
         <widget class="QLabel" name="label_41">
          <property name="text">
           <string>2d SLAM</string>
@@ -1200,14 +1200,14 @@
          </property>
         </widget>
        </item>
-       <item row="2" column="1">
+       <item row="3" column="1">
         <widget class="QLabel" name="label_34">
          <property name="text">
           <string>Ignore covariance</string>
          </property>
         </widget>
        </item>
-       <item row="2" column="0">
+       <item row="3" column="0">
         <widget class="QCheckBox" name="checkBox_ignoreCovariance">
          <property name="text">
           <string/>
@@ -1217,7 +1217,7 @@
          </property>
         </widget>
        </item>
-       <item row="3" column="0">
+       <item row="4" column="0">
         <widget class="QCheckBox" name="checkBox_2dslam">
          <property name="text">
           <string/>
@@ -1227,6 +1227,29 @@
          </property>
         </widget>
        </item>
+       <item row="2" column="1">
+        <widget class="QLabel" name="label_8">
+         <property name="text">
+          <string>Depth (0=inf)</string>
+         </property>
+        </widget>
+       </item>
+       <item row="2" column="0">
+        <widget class="QSpinBox" name="spinBox_optimizationDepth">
+         <property name="suffix">
+          <string/>
+         </property>
+         <property name="minimum">
+          <number>0</number>
+         </property>
+         <property name="maximum">
+          <number>1000</number>
+         </property>
+         <property name="value">
+          <number>0</number>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
      <item>