i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
matlabbe
2016-11-24 15:09:16 -05:00
parent 2a43a95ddd
commit 5b183d9105
4 changed files with 221 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
app/android/jni/RTABMapApp.cpp
View File

@@ -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;
}
}

162
guilib/src/DatabaseViewer.cpp
View File

@@ -137,20 +137,25 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
ui_->graphicsView_stereo->setAlpha(255);
QSet<QString> 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<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()

125
guilib/src/ParametersToolBox.cpp
View File

@@ -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<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);
}
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<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;
QWidget * widget = this->findChild<QWidget*>(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));
}
}
}

3
guilib/src/ParametersToolBox.h
View File

@@ -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