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

Refactoring: Split all graph optimization approaches in multiple files

Browse Source
This commit is contained in:
matlabbe
2015-11-27 11:09:39 -05:00
parent 93326ab1fe
commit 7d35906398
23 changed files with 2373 additions and 1958 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
guilib/src/AboutDialog.cpp
View File

@@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/CameraStereo.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/Optimizer.h"
#include "ui_aboutDialog.h"
#include <opencv2/core/version.hpp>
#include <pcl/pcl_config.h>
@@ -60,9 +60,9 @@ AboutDialog::AboutDialog(QWidget * parent) :
_ui->label_dc1394->setText(CameraStereoDC1394::available()?"Yes":"No");
_ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No");
_ui->label_g2o->setText(graph::G2OOptimizer::available()?"Yes":"No");
_ui->label_gtsam->setText(graph::GTSAMOptimizer::available()?"Yes":"No");
_ui->label_cvsba->setText(graph::CVSBAOptimizer::available()?"Yes":"No");
_ui->label_g2o->setText(Optimizer::isAvailable(Optimizer::kTypeG2O)?"Yes":"No");
_ui->label_gtsam->setText(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?"Yes":"No");
_ui->label_cvsba->setText(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?"Yes":"No");
}

89
guilib/src/DatabaseViewer.cpp
View File

@@ -60,6 +60,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/RegistrationVis.h"
#include "rtabmap/core/RegistrationIcp.h"
#include "rtabmap/gui/DataRecorder.h"
@@ -126,7 +127,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->comboBox_nnType->setCurrentIndex(3);
}
}
if(!graph::G2OOptimizer::available())
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
ui_->comboBox_graphOptimizer->setItemData(1, 0, Qt::UserRole - 1);
if(ui_->comboBox_graphOptimizer->currentIndex() == 1)
@@ -135,7 +136,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->comboBox_graphOptimizer->setCurrentIndex(0);
}
}
if(!graph::GTSAMOptimizer::available())
if(!Optimizer::isAvailable(Optimizer::kTypeGTSAM))
{
ui_->comboBox_graphOptimizer->setItemData(1, 0, Qt::UserRole - 1);
if(ui_->comboBox_graphOptimizer->currentIndex() == 2)
@@ -144,7 +145,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->comboBox_graphOptimizer->setCurrentIndex(0);
}
}
if(!graph::G2OOptimizer::available() && !graph::GTSAMOptimizer::available())
if(!Optimizer::isAvailable(Optimizer::kTypeG2O) && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
{
ui_->checkBox_robust->setEnabled(false);
}
@@ -169,7 +170,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
connect(ui_->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this, SLOT(generateTOROGraph()));
connect(ui_->actionGenerate_g2o_graph_g2o, SIGNAL(triggered()), this, SLOT(generateG2OGraph()));
ui_->actionGenerate_g2o_graph_g2o->setEnabled(graph::G2OOptimizer::available());
ui_->actionGenerate_g2o_graph_g2o->setEnabled(Optimizer::isAvailable(Optimizer::kTypeG2O));
connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
connect(ui_->actionView_3D_laser_scans, SIGNAL(triggered()), this, SLOT(view3DLaserScans()));
connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
@@ -1410,11 +1411,11 @@ void DatabaseViewer::generateTOROGraph()
{
iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1));
}
graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), links);
graph::exportPoses(path.toStdString(), 3, uValueAt(graphes_, id), links);
}
else
{
graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), graphLinks_);
graph::exportPoses(path.toStdString(), 3, uValueAt(graphes_, id), graphLinks_);
}
}
}
@@ -1442,11 +1443,11 @@ void DatabaseViewer::generateG2OGraph()
{
iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1));
}
graph::G2OOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), links, ui_->checkBox_robust->isChecked());
graph::exportPoses(path.toStdString(), 4, uValueAt(graphes_, id), links, std::map<int, double>(), ui_->checkBox_robust->isChecked());
}
else
{
graph::G2OOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), graphLinks_, ui_->checkBox_robust->isChecked());
graph::exportPoses(path.toStdString(), 4, uValueAt(graphes_, id), graphLinks_, std::map<int, double>(), ui_->checkBox_robust->isChecked());
}
}
}
@@ -2985,34 +2986,6 @@ void DatabaseViewer::updateConstraintView(
//add other scans matching
//optimize the path's poses locally
graph::Optimizer * optimizer = 0;
if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeGTSAM)
{
optimizer = new graph::GTSAMOptimizer(
ui_->spinBox_iterations->value(),
ui_->checkBox_2dslam->isChecked(),
ui_->checkBox_ignoreCovariance->isChecked(),
0.0,
ui_->checkBox_robust->isChecked());
}
else if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O)
{
UINFO("ui_->checkBox_robust->isChecked()=%d", ui_->checkBox_robust->isChecked()?1:0);
optimizer = new graph::G2OOptimizer(
ui_->spinBox_iterations->value(),
ui_->checkBox_2dslam->isChecked(),
ui_->checkBox_ignoreCovariance->isChecked(),
0.0,
ui_->checkBox_robust->isChecked());
}
else
{
optimizer = new graph::TOROOptimizer(
ui_->spinBox_iterations->value(),
ui_->checkBox_2dslam->isChecked(),
ui_->checkBox_ignoreCovariance->isChecked(),
0.0);
}
std::map<int, rtabmap::Transform> poses;
for(unsigned int i=0; i<ids.size(); ++i)
{
@@ -3027,6 +3000,14 @@ void DatabaseViewer::updateConstraintView(
}
if(poses.size())
{
ParametersMap optimizerParameters;
uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerStrategy(), uNumber2Str(ui_->comboBox_graphOptimizer->currentIndex())));
uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(ui_->spinBox_iterations->value())));
uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerSlam2D(), uBool2Str(ui_->checkBox_2dslam->isChecked())));
uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ui_->checkBox_ignoreCovariance->isChecked())));
uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerRobust(), uBool2Str(ui_->checkBox_robust->isChecked())));
Optimizer * optimizer = Optimizer::create(optimizerParameters);
UASSERT(uContains(poses, link.to()));
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
@@ -3500,34 +3481,14 @@ void DatabaseViewer::updateGraphView()
.arg(totalLocalTime)
.arg(totalUser));
graph::Optimizer * optimizer = 0;
if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeGTSAM)
{
optimizer = new graph::GTSAMOptimizer(
ui_->spinBox_iterations->value(),
ui_->checkBox_2dslam->isChecked(),
ui_->checkBox_ignoreCovariance->isChecked(),
0.0,
ui_->checkBox_robust->isChecked());
}
else if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O)
{
UINFO("ui_->checkBox_robust->isChecked()=%d", ui_->checkBox_robust->isChecked()?1:0);
optimizer = new graph::G2OOptimizer(
ui_->spinBox_iterations->value(),
ui_->checkBox_2dslam->isChecked(),
ui_->checkBox_ignoreCovariance->isChecked(),
0.0,
ui_->checkBox_robust->isChecked());
}
else
{
optimizer = new graph::TOROOptimizer(
ui_->spinBox_iterations->value(),
ui_->checkBox_2dslam->isChecked(),
ui_->checkBox_ignoreCovariance->isChecked(),
0.0);
}
ParametersMap optimizerParameters;
uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerStrategy(), uNumber2Str(ui_->comboBox_graphOptimizer->currentIndex())));
uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(ui_->spinBox_iterations->value())));
uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerSlam2D(), uBool2Str(ui_->checkBox_2dslam->isChecked())));
uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ui_->checkBox_ignoreCovariance->isChecked())));
uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerRobust(), uBool2Str(ui_->checkBox_robust->isChecked())));
Optimizer * optimizer = Optimizer::create(optimizerParameters);
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());

15
guilib/src/MainWindow.cpp
View File

@@ -94,6 +94,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_mapping.h"
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/OptimizerCVSBA.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/RegistrationIcp.h"
#include <pcl/visualization/cloud_viewer.h>
@@ -310,7 +312,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
connect(_ui->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
_ui->actionG2o_g2o->setVisible(graph::G2OOptimizer::available());
_ui->actionG2o_g2o->setVisible(Optimizer::isAvailable(Optimizer::kTypeG2O));
connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
@@ -3420,7 +3422,7 @@ void MainWindow::postProcessing()
_initProgressDialog->show();
ParametersMap parameters = _preferencesDialog->getAllParameters();
graph::Optimizer * optimizer = graph::Optimizer::create(parameters);
Optimizer * optimizer = Optimizer::create(parameters);
bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
@@ -3436,7 +3438,7 @@ void MainWindow::postProcessing()
_initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... (iteration=%1/%2, radius=%3 m angle=%4 degrees)")
.arg(n+1).arg(detectLoopClosureIterations).arg(clusterRadius).arg(clusterAngle));
std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
std::multimap<int, int> clusters = graph::radiusPosesClustering(
_currentPosesMap,
clusterRadius,
clusterAngle*CV_PI/180.0);
@@ -3634,6 +3636,7 @@ void MainWindow::postProcessing()
if(sba)
{
UASSERT(OptimizerCVSBA::available());
_initProgressDialog->appendText(tr("SBA (%1 nodes, %2 constraints, %3 iterations)...")
.arg(optimizedPoses.size()).arg(linksOut.size()).arg(sbaIterations));
QApplication::processEvents();
@@ -3641,9 +3644,9 @@ void MainWindow::postProcessing()
QApplication::processEvents();
ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
uInsert(parametersSBA, std::make_pair(Parameters::kRGBDOptimizeIterations(), uNumber2Str(sbaIterations)));
uInsert(parametersSBA, std::make_pair(Parameters::kRGBDOptimizeEpsilon(), uNumber2Str(sbaEpsilon)));
graph::CVSBAOptimizer cvsba = graph::CVSBAOptimizer(parametersSBA);
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerEpsilon(), uNumber2Str(sbaEpsilon)));
OptimizerCVSBA cvsba = OptimizerCVSBA(parametersSBA);
cvsba.setInlierDistance(sbaInlierDistance);
cvsba.setMinInliers(sbaMinInliers);
std::map<int, Transform> newPoses = cvsba.optimizeBA(0, optimizedPoses, linksOut, _cachedSignatures.toStdMap());

6
guilib/src/PostProcessingDialog.cpp
View File

@@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "ui_postProcessingDialog.h"
#include <QPushButton>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Optimizer.h>
namespace rtabmap {
@@ -39,7 +39,7 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
_ui = new Ui_PostProcessingDialog();
_ui->setupUi(this);
if(!graph::CVSBAOptimizer::available())
if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
{
_ui->sba->setEnabled(false);
_ui->sba->setChecked(false);
@@ -231,7 +231,7 @@ void PostProcessingDialog::setRefineLoopClosureLinks(bool on)
}
void PostProcessingDialog::setSBA(bool on)
{
_ui->sba->setChecked(graph::CVSBAOptimizer::available() && on);
_ui->sba->setChecked(Optimizer::isAvailable(Optimizer::kTypeCVSBA) && on);
}
void PostProcessingDialog::setSBAIterations(int iterations)
{

36
guilib/src/PreferencesDialog.cpp
View File

@@ -56,7 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/CameraStereo.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/DBReader.h"
#include "rtabmap/gui/LoopClosureViewer.h"
@@ -163,15 +163,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->reextract_type->setItemData(6, 0, Qt::UserRole - 1);
#endif
}
if(!graph::G2OOptimizer::available())
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
_ui->graphOptimization_type->setItemData(1, 0, Qt::UserRole - 1);
}
if(!graph::GTSAMOptimizer::available())
if(!Optimizer::isAvailable(Optimizer::kTypeGTSAM))
{
_ui->graphOptimization_type->setItemData(2, 0, Qt::UserRole - 1);
}
if(!graph::G2OOptimizer::available() && !graph::GTSAMOptimizer::available())
if(!Optimizer::isAvailable(Optimizer::kTypeG2O) && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
{
_ui->graphOptimization_robust->setEnabled(false);
}
@@ -596,14 +596,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odomScanHistory->setObjectName(Parameters::kRGBDNeighborLinkRefining().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_type->setObjectName(Parameters::kOptimizerStrategy().c_str());
_ui->graphOptimization_slam2d->setObjectName(Parameters::kOptimizerSlam2D().c_str());
_ui->graphOptimization_iterations->setObjectName(Parameters::kOptimizerIterations().c_str());
_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kOptimizerVarianceIgnored().c_str());
_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
_ui->graphOptimization_maxError->setObjectName(Parameters::kRGBDOptimizeMaxError().c_str());
_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kRGBDOptimizeEpsilon().c_str());
_ui->graphOptimization_robust->setObjectName(Parameters::kRGBDOptimizeRobust().c_str());
_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kOptimizerEpsilon().c_str());
_ui->graphOptimization_robust->setObjectName(Parameters::kOptimizerRobust().c_str());
_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
@@ -1915,24 +1915,24 @@ bool PreferencesDialog::validateForm()
}
// optimization strategy
if(!graph::G2OOptimizer::available())
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
if(_ui->graphOptimization_type->currentIndex() == 1)
{
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);
_ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeTORO);
}
}
if(!graph::GTSAMOptimizer::available())
if(!Optimizer::isAvailable(Optimizer::kTypeGTSAM))
{
if(_ui->graphOptimization_type->currentIndex() == 2)
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("Selected graph optimization strategy (GTSAM) is not available. RTAB-Map is not built "
"with GTSAM. TORO is set instead for graph optimization strategy."));
_ui->graphOptimization_type->setCurrentIndex(graph::Optimizer::kTypeTORO);
_ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeTORO);
}
}
@@ -2613,9 +2613,9 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
ok = false;
}
}
if(!graph::G2OOptimizer::available())
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
if(valueInt==1 && combo->objectName().toStdString().compare(Parameters::kRGBDOptimizeStrategy()) == 0)
if(valueInt==1 && combo->objectName().toStdString().compare(Parameters::kOptimizerStrategy()) == 0)
{
UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built "
"with g2o. Keeping default combo value: %s.",
@@ -2624,9 +2624,9 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
ok = false;
}
}
if(!graph::GTSAMOptimizer::available())
if(!Optimizer::isAvailable(Optimizer::kTypeGTSAM))
{
if(valueInt==2 && combo->objectName().toStdString().compare(Parameters::kRGBDOptimizeStrategy()) == 0)
if(valueInt==2 && combo->objectName().toStdString().compare(Parameters::kOptimizerStrategy()) == 0)
{
UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built "
"with GTSAM. Keeping default combo value: %s.",