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

Removed Optimizer:computeBACorrespondences() duplicated 3d points (more than 2 frames can reference a 3D point), added option to rematch features. Bundler: added more options to export dialog, fixed inverted colors.

Browse Source
This commit is contained in:
matlabbe
2018-10-25 16:31:13 -04:00
parent 1ef61db73b
commit 12a7349165
12 changed files with 355 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
guilib/src/ExportBundlerDialog.cpp
View File

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/gui/ExportBundlerDialog.h"
#include "ui_exportBundlerDialog.h"
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/util3d_transforms.h>
#include <QFileDialog>
@@ -51,11 +52,31 @@ ExportBundlerDialog::ExportBundlerDialog(QWidget * parent) :
connect(_ui->doubleSpinBox_laplacianVariance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_linearSpeed, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_angularSpeed, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_export_points, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->groupBox_export_points, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateVisibility()));
connect(_ui->sba_rematchFeatures, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
_ui->checkBox_export_points->setEnabled(Optimizer::isAvailable(Optimizer::kTypeG2O));
if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA) && !Optimizer::isAvailable(Optimizer::kTypeG2O))
{
_ui->groupBox_export_points->setEnabled(false);
_ui->groupBox_export_points->setChecked(false);
}
else if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
{
_ui->comboBox_sbaType->setItemData(1, 0, Qt::UserRole - 1);
_ui->comboBox_sbaType->setCurrentIndex(0);
}
else if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
_ui->comboBox_sbaType->setItemData(0, 0, Qt::UserRole - 1);
_ui->comboBox_sbaType->setCurrentIndex(1);
}
_ui->lineEdit_path->setText(QDir::currentPath());
updateVisibility();
}
ExportBundlerDialog::~ExportBundlerDialog()
@@ -72,7 +93,11 @@ void ExportBundlerDialog::saveSettings(QSettings & settings, const QString & gro
settings.setValue("maxLinearSpeed", _ui->doubleSpinBox_linearSpeed->value());
settings.setValue("maxAngularSpeed", _ui->doubleSpinBox_angularSpeed->value());
settings.setValue("laplacianThr", _ui->doubleSpinBox_laplacianVariance->value());
settings.setValue("exportPoints", _ui->checkBox_export_points->isChecked());
settings.setValue("exportPoints", _ui->groupBox_export_points->isChecked());
settings.setValue("sba_iterations", _ui->sba_iterations->value());
settings.setValue("sba_type", _ui->comboBox_sbaType->currentIndex());
settings.setValue("sba_variance", _ui->sba_variance->value());
settings.setValue("sba_rematch_features", _ui->sba_rematchFeatures->isChecked());
if(!group.isEmpty())
{
settings.endGroup();
@@ -88,7 +113,11 @@ void ExportBundlerDialog::loadSettings(QSettings & settings, const QString & gro
_ui->doubleSpinBox_linearSpeed->setValue(settings.value("maxLinearSpeed", _ui->doubleSpinBox_linearSpeed->value()).toDouble());
_ui->doubleSpinBox_angularSpeed->setValue(settings.value("maxAngularSpeed", _ui->doubleSpinBox_angularSpeed->value()).toDouble());
_ui->doubleSpinBox_laplacianVariance->setValue(settings.value("laplacianThr", _ui->doubleSpinBox_laplacianVariance->value()).toDouble());
_ui->checkBox_export_points->setChecked(settings.value("exportPoints", _ui->checkBox_export_points->isChecked()).toBool());
_ui->groupBox_export_points->setChecked(settings.value("exportPoints", _ui->groupBox_export_points->isChecked()).toBool());
_ui->sba_iterations->setValue(settings.value("sba_iterations", _ui->sba_iterations->value()).toInt());
_ui->comboBox_sbaType->setCurrentIndex((Optimizer::Type)settings.value("sba_type", _ui->comboBox_sbaType->currentIndex()).toInt());
_ui->sba_variance->setValue(settings.value("sba_variance", _ui->sba_variance->value()).toDouble());
_ui->sba_rematchFeatures->setChecked(settings.value("sba_rematch_features", _ui->sba_rematchFeatures->isChecked()).toBool());
if(!group.isEmpty())
{
settings.endGroup();
@@ -105,7 +134,25 @@ void ExportBundlerDialog::restoreDefaults()
_ui->doubleSpinBox_linearSpeed->setValue(0);
_ui->doubleSpinBox_angularSpeed->setValue(0);
_ui->doubleSpinBox_laplacianVariance->setValue(0);
_ui->checkBox_export_points->setChecked(false);
_ui->groupBox_export_points->setChecked(false);
_ui->sba_iterations->setValue(20);
if(Optimizer::isAvailable(Optimizer::kTypeG2O) || !Optimizer::isAvailable(Optimizer::kTypeCVSBA))
{
_ui->comboBox_sbaType->setCurrentIndex(0);
}
else
{
_ui->comboBox_sbaType->setCurrentIndex(1);
}
_ui->sba_variance->setValue(1.0);
_ui->sba_rematchFeatures->setChecked(true);
}
void ExportBundlerDialog::updateVisibility()
{
_ui->sba_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
_ui->label_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
}
void ExportBundlerDialog::getPath()
@@ -120,7 +167,8 @@ void ExportBundlerDialog::getPath()
void ExportBundlerDialog::exportBundler(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const QMap<int, Signature> & signatures)
const QMap<int, Signature> & signatures,
const ParametersMap & parameters)
{
if(this->exec() != QDialog::Accepted)
{
@@ -138,11 +186,16 @@ void ExportBundlerDialog::exportBundler(
std::map<int, cv::Point3f> points3DMap;
std::map<int, std::map<int, FeatureBA> > wordReferences;
std::map<int, Transform> newPoses = poses;
if(_ui->checkBox_export_points->isEnabled() && _ui->checkBox_export_points->isChecked())
if(_ui->groupBox_export_points->isEnabled() && _ui->groupBox_export_points->isChecked())
{
std::map<int, Transform> posesOut;
std::multimap<int, Link> linksOut;
Optimizer * sba = Optimizer::create(Optimizer::kTypeG2O);
Optimizer::Type sbaType = _ui->comboBox_sbaType->currentIndex()==0?Optimizer::kTypeG2O:Optimizer::kTypeCVSBA;
UASSERT(Optimizer::isAvailable(sbaType));
ParametersMap parametersSBA = parameters;
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(_ui->sba_iterations->value())));
uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(_ui->sba_variance->value())));
Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
sba->getConnectedGraph(poses.begin()->first, poses, links, posesOut, linksOut);
newPoses = sba->optimizeBA(
posesOut.begin()->first,
@@ -150,7 +203,8 @@ void ExportBundlerDialog::exportBundler(
linksOut,
signatures.toStdMap(),
points3DMap,
wordReferences);
wordReferences,
_ui->sba_rematchFeatures->isChecked());
delete sba;
if(newPoses.empty())
@@ -269,7 +323,10 @@ void ExportBundlerDialog::exportBundler(
{
if(kter->first == iter->first)
{
descriptors.push_back(kter->second);
if(!kter->second.descriptor.empty())
{
descriptors.push_back(kter->second);
}
if(colors.find(jter->first) == colors.end())
{
@@ -282,7 +339,7 @@ void ExportBundlerDialog::exportBundler(
if(image.channels() == 3)
{
cv::Vec3b & pixel = image.at<cv::Vec3b>((int)kter->second.kpt.pt.y, (int)kter->second.kpt.pt.x);
c.setRgb(pixel[0], pixel[1], pixel[2]);
c.setRgb(pixel[2], pixel[1], pixel[0]);
}
else // grayscale
{
@@ -295,12 +352,13 @@ void ExportBundlerDialog::exportBundler(
}
}
}
if(descriptors.size())
QString p = QString("keys")+QDir::separator()+tr("%1.key").arg(iter->first);
p = path+QDir::separator()+p;
QFile fileKey(p);
if(fileKey.open(QIODevice::WriteOnly | QIODevice::Text))
{
QString p = QString("keys")+QDir::separator()+tr("%1.key").arg(iter->first);
p = path+QDir::separator()+p;
QFile fileKey(p);
if(fileKey.open(QIODevice::WriteOnly | QIODevice::Text))
if(descriptors.size())
{
QTextStream key(&fileKey);
key << descriptors.size() << " " << descriptors.front().descriptor.cols << "\n";
@@ -329,15 +387,23 @@ void ExportBundlerDialog::exportBundler(
}
key << "\n";
}
fileKey.close();
}
else
{
UWARN("No descriptors saved for frame %d in file %s. "
"Descriptors may not have been saved in the nodes. "
"Verify that parameter %s was true during mapping.",
iter->first, p.toStdString().c_str(),
Parameters::kMemRawDescriptorsKept().c_str());
}
fileKey.close();
}
}
}
}
else
{
UWARN("Could not find signature data for pose %d", iter->first);
UWARN("Could not find node data for pose %d", iter->first);
}
}

10
guilib/src/MainWindow.cpp
View File

@@ -5390,6 +5390,7 @@ void MainWindow::postProcessing()
int sbaIterations = _postProcessingDialog->sbaIterations();
double sbaVariance = _postProcessingDialog->sbaVariance();
Optimizer::Type sbaType = _postProcessingDialog->sbaType();
double sbaRematchFeatures = _postProcessingDialog->sbaRematchFeatures();
if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks && !sba)
{
@@ -5864,9 +5865,9 @@ void MainWindow::postProcessing()
ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(sbaVariance)));
Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
std::map<int, Transform> newPoses = sba->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
delete sba;
Optimizer * sbaOptimizer = Optimizer::create(sbaType, parametersSBA);
std::map<int, Transform> newPoses = sbaOptimizer->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap(), sbaRematchFeatures);
delete sbaOptimizer;
if(newPoses.size())
{
optimizedPoses = newPoses;
@@ -7041,7 +7042,8 @@ void MainWindow::exportBundlerFormat()
_exportBundlerDialog->exportBundler(
poses,
_currentLinksMap,
_cachedSignatures);
_cachedSignatures,
_preferencesDialog->getAllParameters());
}
else
{

15
guilib/src/PostProcessingDialog.cpp
View File

@@ -74,6 +74,7 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateVisibility()));
connect(_ui->sba_rematchFeatures, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
updateVisibility();
}
@@ -105,6 +106,7 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr
settings.setValue("sba_iterations", this->sbaIterations());
settings.setValue("sba_type", this->sbaType());
settings.setValue("sba_variance", this->sbaVariance());
settings.setValue("sba_rematch_features", this->sbaRematchFeatures());
if(!group.isEmpty())
{
settings.endGroup();
@@ -127,6 +129,8 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
this->setSBAIterations(settings.value("sba_iterations", this->sbaIterations()).toInt());
this->setSBAType((Optimizer::Type)settings.value("sba_type", this->sbaType()).toInt());
this->setSBAVariance(settings.value("sba_variance", this->sbaVariance()).toDouble());
this->setSBARematchFeatures(settings.value("sba_rematch_features", this->sbaRematchFeatures()).toBool());
if(!group.isEmpty())
{
settings.endGroup();
@@ -145,6 +149,7 @@ void PostProcessingDialog::restoreDefaults()
setSBAIterations(20);
setSBAType(!Optimizer::isAvailable(Optimizer::kTypeG2O)&&Optimizer::isAvailable(Optimizer::kTypeCVSBA)?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O);
setSBAVariance(1.0);
setSBARematchFeatures(true);
}
void PostProcessingDialog::updateButtonBox()
@@ -200,6 +205,10 @@ Optimizer::Type PostProcessingDialog::sbaType() const
{
return _ui->comboBox_sbaType->currentIndex()==0?Optimizer::kTypeG2O:Optimizer::kTypeCVSBA;
}
bool PostProcessingDialog::sbaRematchFeatures() const
{
return _ui->sba_rematchFeatures->isChecked();
}
//setters
void PostProcessingDialog::setDetectMoreLoopClosures(bool on)
@@ -228,7 +237,7 @@ void PostProcessingDialog::setRefineLoopClosureLinks(bool on)
}
void PostProcessingDialog::setSBA(bool on)
{
_ui->sba->setChecked(Optimizer::isAvailable(Optimizer::kTypeCVSBA) && on);
_ui->sba->setChecked((Optimizer::isAvailable(Optimizer::kTypeCVSBA) || Optimizer::isAvailable(Optimizer::kTypeG2O)) && on);
}
void PostProcessingDialog::setSBAIterations(int iterations)
{
@@ -249,6 +258,10 @@ void PostProcessingDialog::setSBAType(Optimizer::Type type)
_ui->comboBox_sbaType->setCurrentIndex(0);
}
}
void PostProcessingDialog::setSBARematchFeatures(bool value)
{
_ui->sba_rematchFeatures->setChecked(value);
}
}

111
guilib/src/ui/exportBundlerDialog.ui
View File

@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>524</width>
<height>363</height>
<width>557</width>
<height>466</height>
</rect>
</property>
<property name="windowTitle">
@@ -119,21 +119,116 @@
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_43">
</layout>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_export_points">
<property name="title">
<string>Export 3D Points</string>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
<item>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
<item row="1" column="0">
<widget class="QSpinBox" name="sba_iterations">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_8">
<property name="text">
<string>Export 3D points. RTAB-Map must be built with g2o.</string>
<string>SBA Iterations</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_variance">
<property name="text">
<string>SBA Pixel variance used by g2o.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="sba_variance">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="maximum">
<double>999.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_10">
<property name="text">
<string>SBA Type</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QComboBox" name="comboBox_sbaType">
<item>
<property name="text">
<string>g2o</string>
</property>
</item>
<item>
<property name="text">
<string>cvsba</string>
</property>
</item>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_variance_2">
<property name="text">
<string>Rematch features.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_export_points">
<widget class="QCheckBox" name="sba_rematchFeatures">
<property name="text">
<string/>
</property>

27
guilib/src/ui/postProcessingDialog.ui
View File

@@ -260,6 +260,23 @@
</item>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_variance_2">
<property name="text">
<string>Rematch features.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="sba_rematchFeatures">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
@@ -278,6 +295,16 @@
</property>
</spacer>
</item>
<item>
<widget class="QLabel" name="label_4">
<property name="text">
<string>Note: Post-processing only affects visualization in the GUI and the export actions. To actually push those optimizations to the database, use Database Viewer tool (Tools-&gt;Edit database...).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QDialogButtonBox" name="buttonBox">
<property name="orientation">