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

Added g2o optimization option (along with TORO). Some refactoring: updated graph optimization parameter names, new graph::Optimizer class and new parameter RGBD/OptimizeSlam2d

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-12 17:00:56 -04:00
parent 05d4276ba0
commit aa8fe2e55c
22 changed files with 3254 additions and 820 deletions
Download Patch File Download Diff File Expand all files Collapse all files

149
guilib/src/DatabaseViewer.cpp
View File

@@ -89,6 +89,9 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->dockWidget_visual->setVisible(false);
ui_->dockWidget_stereoView->setVisible(false);
ui_->dockWidget_view3d->setVisible(false);
ui_->dockWidget_detectMoreLC->setVisible(false);
ui_->dockWidget_graphOptimizer->setVisible(false);
ui_->dockWidget_gridcloud->setVisible(false);
ui_->constraintsViewer->setCameraLockZ(false);
ui_->constraintsViewer->setCameraFree();
@@ -101,6 +104,9 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->menuView->addAction(ui_->dockWidget_visual->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_detectMoreLC->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_graphOptimizer->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_gridcloud->toggleViewAction());
connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
@@ -162,26 +168,33 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int)));
connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
connect(ui_->spinBox_iterations, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->spinBox_optimizationsFrom, SIGNAL(valueChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_initGuess, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_gridFromProjection, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(updateGrid()));
connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
connect(ui_->spinBox_projDecimation, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
// connect configuration changed
connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
//connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
//connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
// Graph view
connect(ui_->spinBox_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_initGuess, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_gridFromProjection, 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_->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()));
connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
// ICP parameters
connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -192,9 +205,10 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->spinBox_icp_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_icp_2d, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
// Visual parameters
connect(ui_->checkBox_visual_recomputeFeatures, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->groupBox_visual_recomputeFeatures, SIGNAL(clicked(bool)), this, SLOT(configModified()));
connect(ui_->comboBox_featureType, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
connect(ui_->comboBox_nnType, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_visual_2d, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_visual_hessian, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_visual_nndr, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->spinBox_visual_minCorrespondences, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_visual_maxCorrespDistance, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -216,6 +230,9 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->dockWidget_stereoView->installEventFilter(this);
ui_->dockWidget_visual->installEventFilter(this);
ui_->dockWidget_view3d->installEventFilter(this);
ui_->dockWidget_detectMoreLC->installEventFilter(this);
ui_->dockWidget_graphOptimizer->installEventFilter(this);
ui_->dockWidget_gridcloud->installEventFilter(this);
}
DatabaseViewer::~DatabaseViewer()
@@ -263,15 +280,23 @@ void DatabaseViewer::readSettings()
savedMaximized_ = settings.value("maximized", false).toBool();
// GraphViewer settings
settings.beginGroup("GraphView");
ui_->graphViewer->loadSettings(settings);
ui_->graphViewer->loadSettings(settings, "GraphView");
settings.beginGroup("optimization");
ui_->spinBox_iterations->setValue(settings.value("iterations", ui_->spinBox_iterations->value()).toInt());
ui_->checkBox_initGuess->setChecked(settings.value("initGuess", ui_->checkBox_initGuess->isChecked()).toBool());
ui_->checkBox_ignoreCovariance->setChecked(settings.value("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked()).toBool());
ui_->checkBox_gridFromProjection->setChecked(settings.value("gridFromProj", ui_->checkBox_gridFromProjection->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());
settings.endGroup();
settings.beginGroup("grid");
ui_->groupBox_gridFromProjection->setChecked(settings.value("gridFromProj", ui_->groupBox_gridFromProjection->isChecked()).toBool());
ui_->doubleSpinBox_gridCellSize->setValue(settings.value("gridCellSize", ui_->doubleSpinBox_gridCellSize->value()).toDouble());
ui_->spinBox_projDecimation->setValue(settings.value("projDecimation", ui_->spinBox_projDecimation->value()).toInt());
ui_->doubleSpinBox_projMaxDepth->setValue(settings.value("projMaxDepth", ui_->doubleSpinBox_projMaxDepth->value()).toDouble());
ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
settings.endGroup();
// ImageViews
@@ -292,9 +317,10 @@ void DatabaseViewer::readSettings()
// Visual parameters
settings.beginGroup("visual");
ui_->checkBox_visual_recomputeFeatures->setChecked(settings.value("reextract", ui_->checkBox_visual_recomputeFeatures->isChecked()).toBool());
ui_->groupBox_visual_recomputeFeatures->setChecked(settings.value("reextract", ui_->groupBox_visual_recomputeFeatures->isChecked()).toBool());
ui_->comboBox_featureType->setCurrentIndex(settings.value("featureType", ui_->comboBox_featureType->currentIndex()).toInt());
ui_->comboBox_nnType->setCurrentIndex(settings.value("nnType", ui_->comboBox_nnType->currentIndex()).toInt());
ui_->checkBox_visual_2d->setChecked(settings.value("force2d", ui_->checkBox_visual_2d->isChecked()).toBool());
ui_->doubleSpinBox_visual_hessian->setValue(settings.value("hessian", ui_->doubleSpinBox_visual_hessian->value()).toDouble());
ui_->doubleSpinBox_visual_nndr->setValue(settings.value("nndr", ui_->doubleSpinBox_visual_nndr->value()).toDouble());
ui_->spinBox_visual_minCorrespondences->setValue(settings.value("minCorr", ui_->spinBox_visual_minCorrespondences->value()).toInt());
ui_->doubleSpinBox_visual_maxCorrespDistance->setValue(settings.value("maxCorrDist", ui_->doubleSpinBox_visual_maxCorrespDistance->value()).toDouble());
@@ -324,15 +350,25 @@ void DatabaseViewer::writeSettings()
savedMaximized_ = this->isMaximized();
// save GraphViewer settings
settings.beginGroup("GraphView");
ui_->graphViewer->saveSettings(settings);
ui_->graphViewer->saveSettings(settings, "GraphView");
// save optimization settings
settings.beginGroup("optimization");
settings.setValue("iterations", ui_->spinBox_iterations->value());
settings.setValue("initGuess", ui_->checkBox_initGuess->isChecked());
settings.setValue("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked());
settings.setValue("gridFromProj", ui_->checkBox_gridFromProjection->isChecked());
settings.setValue("strategy", ui_->comboBox_graphOptimizer->currentIndex());
settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
settings.endGroup();
// save Grid settings
settings.beginGroup("grid");
settings.setValue("gridFromProj", ui_->groupBox_gridFromProjection->isChecked());
settings.setValue("gridCellSize", ui_->doubleSpinBox_gridCellSize->value());
settings.setValue("projDecimation", ui_->spinBox_projDecimation->value());
settings.setValue("projMaxDepth", ui_->doubleSpinBox_projMaxDepth->value());
settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
settings.endGroup();
// ImageViews
@@ -353,9 +389,10 @@ void DatabaseViewer::writeSettings()
// save Visual parameters
settings.beginGroup("visual");
settings.setValue("reextract", ui_->checkBox_visual_recomputeFeatures->isChecked());
settings.setValue("reextract", ui_->groupBox_visual_recomputeFeatures->isChecked());
settings.setValue("featureType", ui_->comboBox_featureType->currentIndex());
settings.setValue("nnType", ui_->comboBox_nnType->currentIndex());
settings.setValue("force2d", ui_->checkBox_visual_2d->isChecked());
settings.setValue("hessian", ui_->doubleSpinBox_visual_hessian->value());
settings.setValue("nndr", ui_->doubleSpinBox_visual_nndr->value());
settings.setValue("minCorr", ui_->spinBox_visual_minCorrespondences->value());
settings.setValue("maxCorrDist", ui_->doubleSpinBox_visual_maxCorrespDistance->value());
@@ -678,6 +715,7 @@ void DatabaseViewer::updateIds()
linksAdded_.clear();
linksRefined_.clear();
linksRemoved_.clear();
ui_->label_optimizeFrom->setText(tr("Optimize from"));
if(memory_->getLastWorkingSignature())
{
//get constraints only for parent links
@@ -710,7 +748,8 @@ void DatabaseViewer::updateIds()
int first = *ids.begin();
ui_->spinBox_optimizationsFrom->setRange(first, memory_->getLastWorkingSignature()->id());
ui_->spinBox_optimizationsFrom->setValue(first);
ui_->spinBox_optimizationsFrom->setValue(memory_->getLastWorkingSignature()->id());
ui_->label_optimizeFrom->setText(tr("Optimize from [%1, %2]").arg(first).arg(memory_->getLastWorkingSignature()->id()));
}
}
@@ -851,7 +890,7 @@ void DatabaseViewer::generateTOROGraph()
QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".graph", tr("TORO file (*.graph)"));
if(!path.isEmpty())
{
rtabmap::graph::saveTOROGraph(path.toStdString(), uValueAt(graphes_, id), links);
graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), links);
}
}
}
@@ -2080,7 +2119,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
{
UTimer time;
bool added = false;
if(ui_->checkBox_gridFromProjection->isChecked())
if(ui_->groupBox_gridFromProjection->isChecked())
{
Signature data = memory_->getSignatureData(ids_.at(i), true);
if(!data.getDepthRaw().empty())
@@ -2159,13 +2198,28 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value);
std::multimap<int, Link> links = updateLinksWithModifications(links_);
ui_->graphViewer->updateGraph(graph, links);
if(graph.size() && localMaps_.size())
if(graph.size() && localMaps_.size() && ui_->graphViewer->isGridMapVisible())
{
float xMin, yMin;
float cell = ui_->doubleSpinBox_gridCellSize->value();
cv::Mat map = rtabmap::util3d::convertMap2Image8U(rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graph, localMaps_, cell, xMin, yMin));
ui_->graphViewer->updateMap(map, cell, xMin, yMin);
cv::Mat map;
if(ui_->groupBox_posefiltering->isChecked())
{
std::map<int, rtabmap::Transform> graphFiltered = graph::radiusPosesFiltering(graph,
ui_->doubleSpinBox_posefilteringRadius->value(),
ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps_, cell, xMin, yMin);
}
else
{
map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graph, localMaps_, cell, xMin, yMin);
}
if(!map.empty())
{
ui_->graphViewer->updateMap(rtabmap::util3d::convertMap2Image8U(map), cell, xMin, yMin);
}
}
ui_->graphViewer->update();
ui_->label_iterations->setNum(value);
//compute total length (neighbor links)
@@ -2209,16 +2263,31 @@ void DatabaseViewer::updateGraphView()
graphes_.push_back(poses_);
ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
rtabmap::graph::optimizeTOROGraph(
depthGraph,
graph::Optimizer * optimizer = 0;
if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O)
{
optimizer = new graph::G2OOptimizer(ui_->spinBox_iterations->value(), ui_->checkBox_2dslam->isChecked(), ui_->checkBox_ignoreCovariance->isChecked());
}
else
{
optimizer = new graph::TOROOptimizer(ui_->spinBox_iterations->value(), ui_->checkBox_2dslam->isChecked(), ui_->checkBox_ignoreCovariance->isChecked());
}
int fromId = ui_->spinBox_optimizationsFrom->value();
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
optimizer->getConnectedGraph(
fromId,
poses_,
links, finalPoses,
ui_->spinBox_iterations->value(),
ui_->checkBox_initGuess->isChecked(),
ui_->checkBox_ignoreCovariance->isChecked(),
&graphes_);
links,
posesOut,
linksOut);
QTime time;
time.start();
finalPoses = optimizer->optimize(fromId, posesOut, linksOut, &graphes_);
ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
graphes_.push_back(finalPoses);
delete optimizer;
}
if(graphes_.size())
{
@@ -2238,8 +2307,8 @@ void DatabaseViewer::updateGraphView()
void DatabaseViewer::updateGrid()
{
if((sender() != ui_->spinBox_projDecimation && sender() != ui_->doubleSpinBox_projMaxDepth) ||
(sender() == ui_->spinBox_projDecimation && ui_->checkBox_gridFromProjection->isChecked()) ||
(sender() == ui_->doubleSpinBox_projMaxDepth && ui_->checkBox_gridFromProjection->isChecked()))
(sender() == ui_->spinBox_projDecimation && ui_->groupBox_gridFromProjection->isChecked()) ||
(sender() == ui_->doubleSpinBox_projMaxDepth && ui_->groupBox_gridFromProjection->isChecked()))
{
localMaps_.clear();
updateGraphView();
@@ -2535,11 +2604,12 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool updateGraph
std::string rejectedMsg;
double variance = -1.0;
int inliers = -1;
if(ui_->checkBox_visual_recomputeFeatures->isChecked())
if(ui_->groupBox_visual_recomputeFeatures->isChecked())
{
// create a fake memory to regenerate features
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), uNumber2Str(ui_->doubleSpinBox_visual_hessian->value())));
parameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(ui_->comboBox_featureType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(ui_->comboBox_nnType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
parameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
@@ -2645,11 +2715,12 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
std::string rejectedMsg;
double variance = -1.0;
int inliers = -1;
if(ui_->checkBox_visual_recomputeFeatures->isChecked())
if(ui_->groupBox_visual_recomputeFeatures->isChecked())
{
// create a fake memory to regenerate features
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), uNumber2Str(ui_->doubleSpinBox_visual_hessian->value())));
parameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(ui_->comboBox_featureType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(ui_->comboBox_nnType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
parameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));

36
guilib/src/MainWindow.cpp
View File

@@ -3145,12 +3145,9 @@ void MainWindow::postProcessing()
_initProgressDialog->show();
ParametersMap parameters = _preferencesDialog->getAllParameters();
int toroIterations = Parameters::defaultRGBDToroIterations();
bool ignoreVariance = Parameters::defaultRGBDToroIgnoreVariance();
bool toroOptimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations);
Parameters::parse(parameters, Parameters::kRGBDToroIgnoreVariance(), ignoreVariance);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), toroOptimizeFromGraphEnd);
graph::Optimizer * optimizer = graph::Optimizer::create(parameters);
bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
int loopClosuresAdded = 0;
if(detectMoreLoopClosures)
@@ -3282,9 +3279,17 @@ void MainWindow::postProcessing()
{
_initProgressDialog->appendText(tr("Optimizing graph with new links (%1 nodes, %2 constraints)...")
.arg(odomPoses.size()).arg(_currentLinksMap.size()));
int fromId = optimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first;
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
std::map<int, rtabmap::Transform> optimizedPoses;
std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
rtabmap::graph::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
optimizer->getConnectedGraph(
fromId,
odomPoses,
_currentLinksMap,
posesOut,
linksOut);
optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
_currentPosesMap = optimizedPoses;
_initProgressDialog->appendText(tr("Optimizing graph with new links... done!"));
}
@@ -3437,9 +3442,18 @@ void MainWindow::postProcessing()
_initProgressDialog->appendText(tr("Optimizing graph with updated links (%1 nodes, %2 constraints)...")
.arg(odomPoses.size()).arg(_currentLinksMap.size()));
int fromId = optimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first;
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
std::map<int, rtabmap::Transform> optimizedPoses;
std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
rtabmap::graph::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
optimizer->getConnectedGraph(
fromId,
odomPoses,
_currentLinksMap,
posesOut,
linksOut);
optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
_initProgressDialog->incrementStep();
@@ -3449,6 +3463,8 @@ void MainWindow::postProcessing()
_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
_initProgressDialog->appendText("Post-processing finished!");
delete optimizer;
}
void MainWindow::deleteMemory()

53
guilib/src/PreferencesDialog.cpp
View File

@@ -53,6 +53,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/gui/LoopClosureViewer.h"
#include "rtabmap/gui/DataRecorder.h"
@@ -119,11 +120,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
#endif
#ifdef _WIN32
_ui->radioButton_openni2->setChecked(true);
_ui->radioButton_opennipcl->setChecked(false);
_ui->groupBox_openni2->setVisible(true);
_ui->radioButton_openni2->setChecked(true);
_ui->radioButton_opennipcl->setChecked(false);
_ui->groupBox_openni2->setVisible(true);
#else
_ui->groupBox_openni2->setVisible(false);
_ui->groupBox_openni2->setVisible(false);
#endif
if(RTABMAP_NONFREE == 0)
@@ -139,6 +140,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->reextract_nn->setItemData(1, 0, Qt::UserRole - 1);
_ui->odom_bin_nn->setItemData(1, 0, Qt::UserRole - 1);
}
if(!graph::G2OOptimizer::available())
{
_ui->graphOptimization_type->setItemData(1, 0, Qt::UserRole - 1);
}
_ui->predictionPlot->showLegend(false);
@@ -455,11 +460,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
_ui->odomScanHistory->setObjectName(Parameters::kRGBDPoseScanMatching().c_str());
_ui->globalDetection_toroIterations->setObjectName(Parameters::kRGBDToroIterations().c_str());
_ui->globalDetection_toroIgnoreVariance->setObjectName(Parameters::kRGBDToroIgnoreVariance().c_str());
_ui->globalDetection_optimizeFromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
_ui->spinBox_maxLocalLocationsRetrieved->setObjectName(Parameters::kRGBDMaxLocalRetrieved().c_str());
_ui->graphOptimization_type->setObjectName(Parameters::kRGBDOptimizeStrategy().c_str());
_ui->graphOptimization_slam2d->setObjectName(Parameters::kRGBDOptimizeSlam2D().c_str());
_ui->graphOptimization_iterations->setObjectName(Parameters::kRGBDOptimizeIterations().c_str());
_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kRGBDOptimizeVarianceIgnored().c_str());
_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanWithNearNodesLinked().c_str());
@@ -1544,6 +1552,18 @@ bool PreferencesDialog::validateForm()
}
}
// optimization strategy
if(!graph::G2OOptimizer::available())
{
if(_ui->graphOptimization_type->currentIndex() > 0)
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("Selected graph optimization strategy (g2o) is not available. RTAB-Map is not built "
"with g2o. TORO is set instead for graph optimization strategy."));
_ui->graphOptimization_type->setCurrentIndex(graph::Optimizer::kTypeTORO);
}
}
//verify binary features and nearest neighbor
// BOW dictionary type
if(_ui->comboBox_dictionary_strategy->currentIndex() == VWDictionary::kNNFlannLSH && _ui->comboBox_detector_strategy->currentIndex() <= 1)
@@ -2120,6 +2140,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
"with the nonfree module from OpenCV. Keeping default combo value: %s.",
combo->objectName().toStdString().c_str(),
combo->currentText().toStdString().c_str());
ok = false;
}
else if(valueInt==1 &&
(combo->objectName().toStdString().compare(Parameters::kKpNNStrategy()) == 0 ||
@@ -2132,13 +2153,21 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
"with binary descriptors. Keeping default combo value: %s.",
combo->objectName().toStdString().c_str(),
combo->currentText().toStdString().c_str());
}
else
{
combo->setCurrentIndex(valueInt);
ok = false;
}
}
else
if(!graph::G2OOptimizer::available())
{
if(valueInt==1 && combo->objectName().toStdString().compare(Parameters::kRGBDOptimizeStrategy()) == 0)
{
UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built "
"with g2o. Keeping default combo value: %s.",
combo->objectName().toStdString().c_str(),
combo->currentText().toStdString().c_str());
ok = false;
}
}
if(ok)
{
combo->setCurrentIndex(valueInt);
}

926
guilib/src/ui/DatabaseViewer.ui
View File

File diff suppressed because it is too large Load Diff

136
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-171</y>
<width>744</width>
<height>1110</height>
<height>1389</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>7</number>
<number>19</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -5017,7 +5017,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="3" column="1">
<widget class="QLabel" name="label_scanMatching">
<property name="text">
<string>Odometry pose correction using laser scan matching. ICP 2D only is used here.</string>
<string>Pose scan matching: odometry pose correction using laser scan matching. ICP 2D only is used here.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5067,7 +5067,28 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
<layout class="QGridLayout" name="gridLayout_49" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="globalDetection_toroIterations">
<widget class="QComboBox" name="graphOptimization_type">
<item>
<property name="text">
<string>TORO</string>
</property>
</item>
<item>
<property name="text">
<string>g2o</string>
</property>
</item>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_31">
<property name="text">
<string>Iterations.</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="graphOptimization_iterations">
<property name="minimum">
<number>0</number>
</property>
@@ -5079,44 +5100,85 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_31">
<property name="text">
<string>TORO graph optimization iterations</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="globalDetection_toroIgnoreVariance">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_141">
<property name="text">
<string>Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="globalDetection_optimizeFromGraphEnd">
<widget class="QCheckBox" name="graphOptimization_covarianceIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_141">
<property name="text">
<string>Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links (transitional and rotational variances).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="graphOptimization_fromGraphEnd">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_151">
<property name="text">
<string>Optimize graph from the newest node.
-If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). An odometry correction from frames /map to /odom is computed.
-If true, there is no odometry correction computed since it is all previous poses in the map that are corrected, not the last one (which corresponds to latest odometry value).
Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
<string>Optimize graph from the newest node.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="graphOptimization_slam2d">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_164">
<property name="text">
<string>2d SLAM: use fast 3DoF (x, y, theta) optimization instead of 6DoF (x, y, z, roll, pitch, yaw) optimization.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_45">
<property name="text">
<string>Graph optimization algorithm.</string>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_183">
<property name="text">
<string>-If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_211">
<property name="text">
<string>-If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5138,7 +5200,7 @@ Warning when set to false: when some nodes are transferred, the first referentia
<item>
<widget class="QLabel" name="label_54">
<property name="text">
<string>Activate local detection over all locations in STM. The Bayes filter is not used here: If there are enough correspondences between the current image and others in STM, transformations are computed. The same 2-steps technique as for global loop closure constraints is used here. This generates more constraints in the map's graph, so more time is required to optimize the graph.</string>
<string>Activate local detection over all locations in STM. The Bayes filter is not used here: If there are enough correspondences between the current image and others in STM, transformations are computed. This generates more constraints in the map's graph, so more time is required to optimize the graph.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5160,7 +5222,7 @@ Warning when set to false: when some nodes are transferred, the first referentia
<item>
<widget class="QLabel" name="label_space1_2">
<property name="text">
<string>Activate local detection over locations (in Working Memory) near in space. The Bayes filter is not used here, so it may results in more false detections. ICP 2D only is used here. Only nodes in the local radius are used.</string>
<string>Activate local detection over locations (in Working Memory) near in space. The Bayes filter is not used here, so it may results in more false detections. ICP 2D only is used here. Only nodes in the local radius are used. The laser scans are compared to those inside a radius around the current estimated position of the robot. If the laser scans match, a constraint is added to the graph. Useful when the robot is visiting backward (camera not in the same direction) already visited locations.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5727,7 +5789,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<item>
<widget class="QGroupBox" name="groupBox_loopClosure_icp3">
<property name="title">
<string>ICP 3D - Point To Plane (e.g. Kinect depth)</string>
<string>ICP 3D (e.g. Kinect depth)</string>
</property>
<layout class="QGridLayout" name="gridLayout_53" columnstretch="0,1">
<item row="0" column="0">