From 5b183d9105509c6c6d3a25375d2b51de5a539c5a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 24 Nov 2016 15:09:16 -0500 Subject: [PATCH] DbViewer: do the max optimization error test when adding a new loop closure (rejecting it if it fails) --- app/android/jni/RTABMapApp.cpp | 34 ++++++- guilib/src/DatabaseViewer.cpp | 162 +++++++++++++++++++++++++------ guilib/src/ParametersToolBox.cpp | 125 +++++++++++------------- guilib/src/ParametersToolBox.h | 3 +- 4 files changed, 221 insertions(+), 103 deletions(-) diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 5401055a..1f8751ea 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/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 >::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; } } diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 804756e5..b6d1c3c9 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -137,20 +137,25 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : ui_->graphicsView_stereo->setAlpha(255); - QSet ignoredGroups; - ignoredGroups.insert("Rtabmap"); - ignoredGroups.insert("Mem"); - ignoredGroups.insert("Kp"); - ignoredGroups.insert("Odom"); - ignoredGroups.insert("OdomBow"); - ignoredGroups.insert("OdomFlow"); - ignoredGroups.insert("OdomMono"); - ignoredGroups.insert("VhEp"); - ignoredGroups.insert("StereoBM"); - ignoredGroups.insert("RGBD"); - ignoredGroups.insert("DbSqlite3"); - ignoredGroups.insert("Bayes"); - ui_->parameters_toolbox->setupUi(ignoredGroups); + ParametersMap parameters; + uInsert(parameters, Parameters::getDefaultParameters("SURF")); + uInsert(parameters, Parameters::getDefaultParameters("SIFT")); + uInsert(parameters, Parameters::getDefaultParameters("BRIEF")); + uInsert(parameters, Parameters::getDefaultParameters("FAST")); + uInsert(parameters, Parameters::getDefaultParameters("GFTT")); + uInsert(parameters, Parameters::getDefaultParameters("ORB")); + uInsert(parameters, Parameters::getDefaultParameters("FREAK")); + uInsert(parameters, Parameters::getDefaultParameters("BRISK")); + uInsert(parameters, Parameters::getDefaultParameters("Optimizer")); + uInsert(parameters, Parameters::getDefaultParameters("g2o")); + uInsert(parameters, Parameters::getDefaultParameters("Reg")); + uInsert(parameters, Parameters::getDefaultParameters("Vis")); + 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 - 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; + newLink = Link(from, to, Link::kUserClosure, t, info.variance, info.variance); } 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 - linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to)); - updateSlider = true; + newLink = rtabmap::graph::findLink(linksRemoved_, from, to)->second; } - 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::iterator iter=mapIds_.begin(); iter!=mapIds_.end(); ++iter) + { + if(iter->second == mapId) + { + fromId = iter->first; + break; + } + } + std::multimap 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 poses; + std::multimap links; + optimizer->getConnectedGraph(fromId, poses_, linksIn, poses, links); + poses = optimizer->optimize(fromId, poses, links); + for(std::multimap::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() diff --git a/guilib/src/ParametersToolBox.cpp b/guilib/src/ParametersToolBox.cpp index faf8ad6a..1a3d3d76 100644 --- a/guilib/src/ParametersToolBox.cpp +++ b/guilib/src/ParametersToolBox.cpp @@ -54,8 +54,7 @@ namespace rtabmap { ParametersToolBox::ParametersToolBox(QWidget *parent) : QWidget(parent), comboBox_(new QComboBox(this)), - stackedWidget_(new QStackedWidget(this)), - parameters_(Parameters::getDefaultParameters()) + stackedWidget_(new QStackedWidget(this)) { 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 & 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 & 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); - } - else - { - addParameter((QVBoxLayout*)currentItem->layout(), iter->first, iter->second); - } + 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); + } + else + { + addParameter((QVBoxLayout*)currentItem->layout(), iter->first, iter->second); } } comboBox_->addItems(groups); @@ -247,51 +243,44 @@ void ParametersToolBox::setupUi(const QSet & 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(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; - QWidget * widget = this->findChild(key.c_str()); - QString type = QString::fromStdString(Parameters::getType(key)); - if(type.compare("string") == 0) - { - 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 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)); - } + ((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)); } } } diff --git a/guilib/src/ParametersToolBox.h b/guilib/src/ParametersToolBox.h index 2634ee14..986fb6c6 100644 --- a/guilib/src/ParametersToolBox.h +++ b/guilib/src/ParametersToolBox.h @@ -50,7 +50,7 @@ public: ParametersToolBox(QWidget *parent = 0); virtual ~ParametersToolBox(); - void setupUi(const QSet & 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 ignoredGroups_; }; } // namespace find_object