DbViewer: do the max optimization error test when adding a new loop closure (rejecting it if it fails)

app/android/jni/RTABMapApp.cpp

@@ -992,16 +992,44 @@ void RTABMapApp::setMeshTriangleSize(int value)
 
 int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value)
 {
-	if(rtabmap::Parameters::getDefaultParameters().find(key) != rtabmap::Parameters::getDefaultParameters().end())
+	std::string compatibleKey = key;
+
+	// Backward compatibility
+	std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().find(key);
+	if(iter != rtabmap::Parameters::getRemovedParameters().end())
 	{
-		LOGI(uFormat("Setting param \"%s\"  to \"%s\"", key.c_str(), value.c_str()).c_str());
+		if(iter->second.first)
+		{
+			// can be migrated
+			compatibleKey = iter->second.second;
+			LOGW("Parameter name changed: \"%s\" -> \"%s\". Please update the code accordingly. Value \"%s\" is still set to the new parameter name.",
+					iter->first.c_str(), iter->second.second.c_str(), value.c_str());
+		}
+		else
+		{
+			if(iter->second.second.empty())
+			{
+				LOGE("Parameter \"%s\" doesn't exist anymore!",
+						iter->first.c_str());
+			}
+			else
+			{
+				LOGE("Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
+						iter->first.c_str(), iter->second.second.c_str());
+			}
+		}
+	}
+
+	if(rtabmap::Parameters::getDefaultParameters().find(compatibleKey) != rtabmap::Parameters::getDefaultParameters().end())
+	{
+		LOGI(uFormat("Setting param \"%s\"  to \"%s\"", compatibleKey.c_str(), value.c_str()).c_str());
 		uInsert(mappingParameters_, rtabmap::ParametersPair(key, value));
 		UEventsManager::post(new rtabmap::ParamEvent(mappingParameters_));
 		return 0;
 	}
 	else
 	{
-		LOGE(uFormat("Key \"%s\" doesn't exist!", key.c_str()).c_str());
+		LOGE(uFormat("Key \"%s\" doesn't exist!", compatibleKey.c_str()).c_str());
 		return -1;
 	}
 }

guilib/src/DatabaseViewer.cpp

@@ -137,20 +137,25 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 
 	ui_->graphicsView_stereo->setAlpha(255);
 
-	QSet<QString> ignoredGroups;
+	ParametersMap parameters;
-	ignoredGroups.insert("Rtabmap");
+	uInsert(parameters, Parameters::getDefaultParameters("SURF"));
-	ignoredGroups.insert("Mem");
+	uInsert(parameters, Parameters::getDefaultParameters("SIFT"));
-	ignoredGroups.insert("Kp");
+	uInsert(parameters, Parameters::getDefaultParameters("BRIEF"));
-	ignoredGroups.insert("Odom");
+	uInsert(parameters, Parameters::getDefaultParameters("FAST"));
-	ignoredGroups.insert("OdomBow");
+	uInsert(parameters, Parameters::getDefaultParameters("GFTT"));
-	ignoredGroups.insert("OdomFlow");
+	uInsert(parameters, Parameters::getDefaultParameters("ORB"));
-	ignoredGroups.insert("OdomMono");
+	uInsert(parameters, Parameters::getDefaultParameters("FREAK"));
-	ignoredGroups.insert("VhEp");
+	uInsert(parameters, Parameters::getDefaultParameters("BRISK"));
-	ignoredGroups.insert("StereoBM");
+	uInsert(parameters, Parameters::getDefaultParameters("Optimizer"));
-	ignoredGroups.insert("RGBD");
+	uInsert(parameters, Parameters::getDefaultParameters("g2o"));
-	ignoredGroups.insert("DbSqlite3");
+	uInsert(parameters, Parameters::getDefaultParameters("Reg"));
-	ignoredGroups.insert("Bayes");
+	uInsert(parameters, Parameters::getDefaultParameters("Vis"));
-	ui_->parameters_toolbox->setupUi(ignoredGroups);
+	uInsert(parameters, Parameters::getDefaultParameters("Icp"));
+	uInsert(parameters, Parameters::getDefaultParameters("Stereo"));
+	uInsert(parameters, Parameters::getDefaultParameters("StereoBM"));
+	uInsert(parameters, Parameters::getDefaultParameters("Grid"));
+	parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDOptimizeMaxError()));
+	ui_->parameters_toolbox->setupUi(parameters);
 
 	this->readSettings();
 
@@ -4230,8 +4235,14 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 		UWARN("Cannot add link to same node");
 		return false;
 	}
+	else if(from < to)
+	{
+		int tmp = from;
+		from = to;
+		to = tmp;
+	}
 
-	bool updateSlider = false;
+	Link newLink;
 	if(!containsLink(linksAdded_, from, to) &&
 	   !containsLink(links_, from, to))
 	{
@@ -4269,16 +4280,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 		
 		if(!t.isNull())
 		{
-			// transform is valid, make a link
+			newLink = Link(from, to, Link::kUserClosure, t, info.variance, info.variance);
-			if(from>to)
-			{
-				linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.variance, info.variance)));
-			}
-			else
-			{
-				linksAdded_.insert(std::make_pair(to, Link(to, from, Link::kUserClosure, t.inverse(), info.variance, info.variance)));
-			}
-			updateSlider = true;
 		}
 		else if(!silent)
 		{
@@ -4289,20 +4291,120 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 	}
 	else if(containsLink(linksRemoved_, from, to))
 	{
-		//simply remove from linksRemoved
+		newLink = rtabmap::graph::findLink(linksRemoved_, from, to)->second;
-		linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
-		updateSlider = true;
 	}
 
-	if(updateSlider)
+	bool updateConstraints = true;
+	float maxOptimizationError = uStr2Float(ui_->parameters_toolbox->getParameters().at(Parameters::kRGBDOptimizeMaxError()));
+	if(newLink.isValid() &&
+	   maxOptimizationError > 0.0f &&
+	   uStr2Int(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerIterations())) > 0.0f)
 	{
+		int fromId = newLink.from();
+		int mapId = mapIds_.at(newLink.from());
+		// use first node of the map containing from
+		for(std::map<int, int>::iterator iter=mapIds_.begin(); iter!=mapIds_.end(); ++iter)
+		{
+			if(iter->second == mapId)
+			{
+				fromId = iter->first;
+				break;
+			}
+		}
+		std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
+		linksIn.insert(std::make_pair(newLink.from(), newLink));
+		const Link * maxLinearLink = 0;
+		const Link * maxAngularLink = 0;
+		float maxLinearError = 0.0f;
+		float maxAngularError = 0.0f;
+		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> links;
+		optimizer->getConnectedGraph(fromId, poses_, linksIn, poses, links);
+		poses = optimizer->optimize(fromId, poses, links);
+		for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+		{
+			// ignore links with high variance
+			if(iter->second.transVariance() <= 1.0)
+			{
+				Transform t1 = uValue(poses, iter->second.from(), Transform());
+				Transform t2 = uValue(poses, iter->second.to(), Transform());
+				Transform t = t1.inverse()*t2;
+				float linearError = uMax3(
+						fabs(iter->second.transform().x() - t.x()),
+						fabs(iter->second.transform().y() - t.y()),
+						fabs(iter->second.transform().z() - t.z()));
+				Eigen::Vector3f vA = t1.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+				Eigen::Vector3f vB = t2.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+				float angularError = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+				if(linearError > maxLinearError)
+				{
+					maxLinearError = linearError;
+					maxLinearLink = &iter->second;
+				}
+				if(angularError > maxAngularError)
+				{
+					maxAngularError = angularError;
+					maxAngularLink = &iter->second;
+				}
+			}
+		}
+		if(maxLinearLink)
+		{
+			UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
+		}
+		if(maxLinearLink)
+		{
+			UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
+		}
+
+		if(maxLinearError > maxOptimizationError)
+		{
+			std::string msg = uFormat("Rejecting edge %d->%d because "
+					  "graph error is too large after optimization (%f m for edge %d->%d, %f deg for edge %d->%d). "
+					  "\"%s\" is %f m.",
+					  newLink.from(),
+					  newLink.to(),
+					  maxLinearError,
+					  maxLinearLink->from(),
+					  maxLinearLink->to(),
+					  maxAngularError*180.0f/M_PI,
+					  maxAngularLink->from(),
+					  maxAngularLink->to(),
+					  Parameters::kRGBDOptimizeMaxError().c_str(),
+					  maxOptimizationError);
+			UWARN("%s", msg.c_str());
+
+			if(!silent)
+			{
+				QMessageBox::warning(this,
+						tr("Add link"),
+						tr("%1").arg(msg.c_str()));
+			}
+
+			updateConstraints = false;
+		}
+	}
+
+	if(updateConstraints)
+	{
+		if(containsLink(linksRemoved_, from, to))
+		{
+			//simply remove from linksRemoved
+			linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
+		}
+		else
+		{
+			linksAdded_.insert(std::make_pair(newLink.from(), newLink));
+		}
 		updateLoopClosuresSlider(from, to);
 		if(updateGraph)
 		{
 			this->updateGraphView();
 		}
 	}
-	return updateSlider;
+
+	return updateConstraints;
 }
 
 void DatabaseViewer::resetConstraint()

guilib/src/ParametersToolBox.cpp

@@ -54,8 +54,7 @@ namespace rtabmap {
 ParametersToolBox::ParametersToolBox(QWidget *parent) :
 	QWidget(parent),
 	comboBox_(new QComboBox(this)),
-	stackedWidget_(new QStackedWidget(this)),
+	stackedWidget_(new QStackedWidget(this))
-	parameters_(Parameters::getDefaultParameters())
 {
 	QVBoxLayout * layout = new QVBoxLayout(this);
 	this->setLayout(layout);
@@ -86,7 +85,7 @@ QStringList ParametersToolBox::resetPage(int index)
 		QString key = children.at(j)->objectName();
 		// ignore working memory
 		QString group = key.split("/").first();
-		if(!ignoredGroups_.contains(group) && parameters_.find(key.toStdString())!=parameters_.end())
+		if(parameters_.find(key.toStdString())!=parameters_.end())
 		{
 			UASSERT_MSG(parameters_.find(key.toStdString()) != parameters_.end(), uFormat("key=%s", key.toStdString().c_str()).c_str());
 			std::string value = Parameters::getDefaultParameters().at(key.toStdString());
@@ -202,11 +201,10 @@ void ParametersToolBox::updateParametersVisibility()
 	}*/
 }
 
-void ParametersToolBox::setupUi(const QSet<QString> & ignoredGroups)
+void ParametersToolBox::setupUi(const ParametersMap & parameters)
 {
-	ignoredGroups_ = ignoredGroups;
+	parameters_ = parameters;
 	QWidget * currentItem = 0;
-	const ParametersMap & parameters = Parameters::getDefaultParameters();
 	QStringList groups;
 	for(ParametersMap::const_iterator iter=parameters.begin();
 			iter!=parameters.end();
@@ -214,28 +212,26 @@ void ParametersToolBox::setupUi(const QSet<QString> & ignoredGroups)
 	{
 		QStringList splitted = QString::fromStdString(iter->first).split('/');
 		QString group = splitted.first();
-		if(!ignoredGroups_.contains(group))
-		{
-			QString name = splitted.last();
-			if(currentItem == 0 || currentItem->objectName().compare(group) != 0)
-			{
-				groups.push_back(group);
-				QScrollArea * area = new QScrollArea(this);
-				stackedWidget_->addWidget(area);
-				currentItem = new QWidget();
-				currentItem->setObjectName(group);
-				QVBoxLayout * layout = new QVBoxLayout(currentItem);
-				layout->setSizeConstraint(QLayout::SetMinimumSize);
-				layout->setContentsMargins(0,0,0,0);
-				layout->setSpacing(0);
-				area->setWidget(currentItem);
 
-				addParameter(layout, iter->first, iter->second);
+		QString name = splitted.last();
-			}
+		if(currentItem == 0 || currentItem->objectName().compare(group) != 0)
-			else
+		{
-			{
+			groups.push_back(group);
-				addParameter((QVBoxLayout*)currentItem->layout(), iter->first, iter->second);
+			QScrollArea * area = new QScrollArea(this);
-			}
+			stackedWidget_->addWidget(area);
+			currentItem = new QWidget();
+			currentItem->setObjectName(group);
+			QVBoxLayout * layout = new QVBoxLayout(currentItem);
+			layout->setSizeConstraint(QLayout::SetMinimumSize);
+			layout->setContentsMargins(0,0,0,0);
+			layout->setSpacing(0);
+			area->setWidget(currentItem);
+
+			addParameter(layout, iter->first, iter->second);
+		}
+		else
+		{
+			addParameter((QVBoxLayout*)currentItem->layout(), iter->first, iter->second);
 		}
 	}
 	comboBox_->addItems(groups);
@@ -247,51 +243,44 @@ void ParametersToolBox::setupUi(const QSet<QString> & ignoredGroups)
 void ParametersToolBox::updateParameter(const std::string & key, const std::string & value)
 {
 	QString group = QString::fromStdString(key).split("/").first();
-	if(!ignoredGroups_.contains(group))
+	if(parameters_.find(key) != parameters_.end())
 	{
-		if(parameters_.find(key) == parameters_.end())
+		parameters_.at(key) = value;
+		QWidget * widget = this->findChild<QWidget*>(key.c_str());
+		QString type = QString::fromStdString(Parameters::getType(key));
+		if(type.compare("string") == 0)
 		{
-			UWARN("key=\"%s\" doesn't exist", key.c_str());
+			QString valueQt = QString::fromStdString(value);
+			if(valueQt.contains(';'))
+			{
+				// It's a list, just change the index
+				QStringList splitted = valueQt.split(':');
+				((QComboBox*)widget)->setCurrentIndex(splitted.first().toInt());
+			}
+			else
+			{
+				((QLineEdit*)widget)->setText(valueQt);
+			}
 		}
-		else
+		else if(type.compare("int") == 0)
 		{
-			parameters_.at(key) = value;
+			((QSpinBox*)widget)->setValue(uStr2Int(value));
-			QWidget * widget = this->findChild<QWidget*>(key.c_str());
+		}
-			QString type = QString::fromStdString(Parameters::getType(key));
+		else if(type.compare("uint") == 0)
-			if(type.compare("string") == 0)
+		{
-			{
+			((QSpinBox*)widget)->setValue(uStr2Int(value));
-				QString valueQt = QString::fromStdString(value);
+		}
-				if(valueQt.contains(';'))
+		else if(type.compare("double") == 0)
-				{
+		{
-					// It's a list, just change the index
+			((QDoubleSpinBox*)widget)->setValue(uStr2Double(value));
-					QStringList splitted = valueQt.split(':');
+		}
-					((QComboBox*)widget)->setCurrentIndex(splitted.first().toInt());
+		else if(type.compare("float") == 0)
-				}
+		{
-				else
+			((QDoubleSpinBox*)widget)->setValue(uStr2Float(value));
-				{
+		}
-					((QLineEdit*)widget)->setText(valueQt);
+		else if(type.compare("bool") == 0)
-				}
+		{
-			}
+			((QCheckBox*)widget)->setChecked(uStr2Bool(value));
-			else if(type.compare("int") == 0)
-			{
-				((QSpinBox*)widget)->setValue(uStr2Int(value));
-			}
-			else if(type.compare("uint") == 0)
-			{
-				((QSpinBox*)widget)->setValue(uStr2Int(value));
-			}
-			else if(type.compare("double") == 0)
-			{
-				((QDoubleSpinBox*)widget)->setValue(uStr2Double(value));
-			}
-			else if(type.compare("float") == 0)
-			{
-				((QDoubleSpinBox*)widget)->setValue(uStr2Float(value));
-			}
-			else if(type.compare("bool") == 0)
-			{
-				((QCheckBox*)widget)->setChecked(uStr2Bool(value));
-			}
 		}
 	}
 }

guilib/src/ParametersToolBox.h

@@ -50,7 +50,7 @@ public:
 	ParametersToolBox(QWidget *parent = 0);
 	virtual ~ParametersToolBox();
 
-	void setupUi(const QSet<QString> & ignoredGroups);
+	void setupUi(const ParametersMap & parameters);
 	QWidget * getParameterWidget(const QString & key);
 	void updateParameter(const std::string & key, const std::string & value);
 	const ParametersMap & getParameters() const {return parameters_;}
@@ -81,7 +81,6 @@ private:
 	QComboBox * comboBox_;
 	QStackedWidget * stackedWidget_;
 	ParametersMap parameters_;
-	QSet<QString> ignoredGroups_;
 };
 
 } // namespace find_object