From 12a7349165f18e5c10279ad762202511ea8657b5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 25 Oct 2018 16:31:13 -0400 Subject: [PATCH] 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. --- corelib/include/rtabmap/core/Optimizer.h | 10 +- corelib/include/rtabmap/core/Parameters.h | 2 +- corelib/src/Optimizer.cpp | 130 ++++++++++++++---- corelib/src/RegistrationVis.cpp | 2 +- .../include/rtabmap/gui/ExportBundlerDialog.h | 5 +- .../rtabmap/gui/PostProcessingDialog.h | 2 + guilib/src/ExportBundlerDialog.cpp | 102 +++++++++++--- guilib/src/MainWindow.cpp | 10 +- guilib/src/PostProcessingDialog.cpp | 15 +- guilib/src/ui/exportBundlerDialog.ui | 111 +++++++++++++-- guilib/src/ui/postProcessingDialog.ui | 27 ++++ tools/Reprocess/main.cpp | 6 +- 12 files changed, 355 insertions(+), 67 deletions(-) diff --git a/corelib/include/rtabmap/core/Optimizer.h b/corelib/include/rtabmap/core/Optimizer.h index c91ef26b..184c68ef 100644 --- a/corelib/include/rtabmap/core/Optimizer.h +++ b/corelib/include/rtabmap/core/Optimizer.h @@ -140,14 +140,15 @@ public: const std::multimap & links, const std::map & signatures, std::map & points3DMap, - std::map > & wordReferences); // - + std::map > & wordReferences, // + bool rematchFeatures = false); std::map optimizeBA( int rootId, const std::map & poses, const std::multimap & links, - const std::map & signatures); + const std::map & signatures, + bool rematchFeatures = false); Transform optimizeBA( const Link & link, @@ -161,7 +162,8 @@ public: const std::multimap & links, const std::map & signatures, std::map & points3DMap, - std::map > & wordReferences); // + std::map > & wordReferences, // + bool rematchFeatures = false); protected: Optimizer( diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index a515772d..4cb2d445 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -339,7 +339,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(RGBD, AngularSpeedUpdate, float, 0.0, "Maximum angular speed (rad/s) to update the map (0 means not limit)."); RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled)."); RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "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 node of the current graph, but it can be useful to preserve the map referential from the oldest node). 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)."); - RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 1.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str())); + RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 3.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str())); RTABMAP_PARAM(RGBD, SavedLocalizationIgnored, bool, false, "Ignore last saved localization pose from previous session. If true, RTAB-Map won't assume it is restarting from the same place than where it shut down previously."); RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m)."); RTABMAP_PARAM(RGBD, PlanStuckIterations, int, 0, "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails."); diff --git a/corelib/src/Optimizer.cpp b/corelib/src/Optimizer.cpp index 508e1cef..26b55626 100644 --- a/corelib/src/Optimizer.cpp +++ b/corelib/src/Optimizer.cpp @@ -372,7 +372,8 @@ std::map Optimizer::optimizeBA( const std::multimap & links, const std::map & signatures, std::map & points3DMap, - std::map > & wordReferences) + std::map > & wordReferences, + bool rematchFeatures) { UDEBUG(""); std::map models; @@ -417,7 +418,7 @@ std::map Optimizer::optimizeBA( } // compute correspondences - this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences); + this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures); return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences); } @@ -426,11 +427,12 @@ std::map Optimizer::optimizeBA( int rootId, const std::map & poses, const std::multimap & links, - const std::map & signatures) + const std::map & signatures, + bool rematchFeatures) { std::map points3DMap; std::map > wordReferences; - return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences); + return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures); } Transform Optimizer::optimizeBA( @@ -459,16 +461,26 @@ Transform Optimizer::optimizeBA( } } +struct KeyPointCompare +{ + bool operator() (const cv::KeyPoint& lhs, const cv::KeyPoint& rhs) const + { + return lhs.pt.x < rhs.pt.x || (lhs.pt.x == rhs.pt.x && lhs.pt.y < rhs.pt.y); + } +}; + void Optimizer::computeBACorrespondences( const std::map & poses, const std::multimap & links, const std::map & signatures, std::map & points3DMap, - std::map > & wordReferences) + std::map > & wordReferences, + bool rematchFeatures) { UDEBUG(""); int wordCount = 0; int edgeWithWordsAdded = 0; + std::map > frameToWordMap; // > for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { Link link = iter->second; @@ -506,8 +518,11 @@ void Optimizer::computeBACorrespondences( regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6")); RegistrationVis reg(regParam); - //sFrom.setWordsDescriptors(std::multimap()); - //sTo.setWordsDescriptors(std::multimap()); + if(!rematchFeatures) + { + sFrom.setWordsDescriptors(std::multimap()); + sTo.setWordsDescriptors(std::multimap()); + } RegistrationInfo info; Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info); @@ -516,6 +531,23 @@ void Optimizer::computeBACorrespondences( if(!t.isNull()) { + if(!rematchFeatures) + { + // set descriptors for the output + if(sFrom.getWords().size() && + sFrom.getWordsDescriptors().empty() && + sFrom.getWords().size() == signatures.at(link.from()).getWordsDescriptors().size()) + { + sFrom.setWordsDescriptors(signatures.at(link.from()).getWordsDescriptors()); + } + if(sTo.getWords().size() && + sTo.getWordsDescriptors().empty() && + sTo.getWords().size() == signatures.at(link.to()).getWordsDescriptors().size()) + { + sTo.setWordsDescriptors(signatures.at(link.to()).getWordsDescriptors()); + } + } + Transform pose = poses.at(sFrom.id()); UASSERT(!pose.isNull()); for(unsigned int i=0; isecond; if(p.x > 0.0f) // make sure the point is valid { - int wordId = ++wordCount; - - wordReferences.insert(std::make_pair(wordId, std::map())); - cv::KeyPoint ptFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second; - cv::Mat descriptorFrom = sFrom.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second; - wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, p.x, descriptorFrom))); - cv::KeyPoint ptTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second; - cv::Mat descriptorTo = sTo.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second; - float depth = 0.0f; - std::multimap::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]); - if( iterTo!=sTo.getWords3().end() && - iterTo->second.x > 0) - { - depth = iterTo->second.x; - } - wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo))); - p = util3d::transformPoint(p, pose); - points3DMap.insert(std::make_pair(wordId, p)); + int wordId = -1; + + // find if the word is already added + std::map >::iterator fromIter = frameToWordMap.find(sFrom.id()); + std::map >::iterator toIter = frameToWordMap.find(sTo.id()); + bool fromAlreadyAdded = false; + bool toAlreadyAdded = false; + if( fromIter != frameToWordMap.end() && + fromIter->second.find(ptFrom) != fromIter->second.end()) + { + wordId = fromIter->second.at(ptFrom); + fromAlreadyAdded = true; + } + if( toIter != frameToWordMap.end() && + toIter->second.find(ptTo) != toIter->second.end()) + { + wordId = toIter->second.at(ptTo); + toAlreadyAdded = true; + } + + if(wordId == -1) + { + wordId = ++wordCount; + wordReferences.insert(std::make_pair(wordId, std::map())); + + p = util3d::transformPoint(p, pose); + points3DMap.insert(std::make_pair(wordId, p)); + } + else + { + UASSERT(wordReferences.find(wordId) != wordReferences.end()); + UASSERT(points3DMap.find(wordId) != points3DMap.end()); + } + + if(!fromAlreadyAdded) + { + cv::Mat descriptorFrom; + if(sFrom.getWordsDescriptors().size()) + { + UASSERT(sFrom.getWordsDescriptors().find(info.inliersIDs[i]) != sFrom.getWordsDescriptors().end()); + descriptorFrom = sFrom.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second; + } + wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, p.x, descriptorFrom))); + frameToWordMap.insert(std::make_pair(sFrom.id(), std::map())); + frameToWordMap.at(sFrom.id()).insert(std::make_pair(ptFrom, wordId)); + } + + if(!toAlreadyAdded) + { + cv::Mat descriptorTo; + if(sTo.getWordsDescriptors().size()) + { + UASSERT(sTo.getWordsDescriptors().find(info.inliersIDs[i]) != sTo.getWordsDescriptors().end()); + descriptorTo = sTo.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second; + } + float depth = 0.0f; + std::multimap::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]); + if( iterTo!=sTo.getWords3().end() && + iterTo->second.x > 0) + { + depth = iterTo->second.x; + } + wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo))); + frameToWordMap.insert(std::make_pair(sTo.id(), std::map())); + frameToWordMap.at(sTo.id()).insert(std::make_pair(ptTo, wordId)); + } } } ++edgeWithWordsAdded; diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp index bb70609e..47e0af1d 100644 --- a/corelib/src/RegistrationVis.cpp +++ b/corelib/src/RegistrationVis.cpp @@ -244,7 +244,7 @@ Transform RegistrationVis::computeTransformationImpl( (_estimationType==1 || toSignature.getWords3().size())) // required only for 3D->3D and 2D->2D { // no need to extract new features, we have all the data we need - UDEBUG(""); + UDEBUG("Bypassing feature matching as descriptors and images are empty. We assume features are already matched."); } else { diff --git a/guilib/include/rtabmap/gui/ExportBundlerDialog.h b/guilib/include/rtabmap/gui/ExportBundlerDialog.h index b75bc9e2..96f1e62a 100644 --- a/guilib/include/rtabmap/gui/ExportBundlerDialog.h +++ b/guilib/include/rtabmap/gui/ExportBundlerDialog.h @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines #include +#include #include #include @@ -55,7 +56,8 @@ public: void exportBundler( const std::map & poses, const std::multimap & links, - const QMap & signatures); + const QMap & signatures, + const ParametersMap & parameters); Q_SIGNALS: void configChanged(); @@ -63,6 +65,7 @@ Q_SIGNALS: private Q_SLOTS: void getPath(); void restoreDefaults(); + void updateVisibility(); private: Ui_ExportBundlerDialog * _ui; diff --git a/guilib/include/rtabmap/gui/PostProcessingDialog.h b/guilib/include/rtabmap/gui/PostProcessingDialog.h index e7cc2fec..41c31fa8 100644 --- a/guilib/include/rtabmap/gui/PostProcessingDialog.h +++ b/guilib/include/rtabmap/gui/PostProcessingDialog.h @@ -63,6 +63,7 @@ public: int sbaIterations() const; double sbaVariance() const; Optimizer::Type sbaType() const; + bool sbaRematchFeatures() const; //setters void setDetectMoreLoopClosures(bool on); @@ -75,6 +76,7 @@ public: void setSBAIterations(int iterations); void setSBAVariance(double variance); void setSBAType(Optimizer::Type type); + void setSBARematchFeatures(bool value); Q_SIGNALS: void configChanged(); diff --git a/guilib/src/ExportBundlerDialog.cpp b/guilib/src/ExportBundlerDialog.cpp index dfccdfa9..28c65521 100644 --- a/guilib/src/ExportBundlerDialog.cpp +++ b/guilib/src/ExportBundlerDialog.cpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/gui/ExportBundlerDialog.h" #include "ui_exportBundlerDialog.h" #include +#include #include #include #include @@ -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 & poses, const std::multimap & links, - const QMap & signatures) + const QMap & signatures, + const ParametersMap & parameters) { if(this->exec() != QDialog::Accepted) { @@ -138,11 +186,16 @@ void ExportBundlerDialog::exportBundler( std::map points3DMap; std::map > wordReferences; std::map 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 posesOut; std::multimap 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((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); } } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 6cc4a9ef..1d6e3384 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -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 newPoses = sba->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap()); - delete sba; + Optimizer * sbaOptimizer = Optimizer::create(sbaType, parametersSBA); + std::map 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 { diff --git a/guilib/src/PostProcessingDialog.cpp b/guilib/src/PostProcessingDialog.cpp index 27fa30f8..694ebb9f 100644 --- a/guilib/src/PostProcessingDialog.cpp +++ b/guilib/src/PostProcessingDialog.cpp @@ -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); +} } diff --git a/guilib/src/ui/exportBundlerDialog.ui b/guilib/src/ui/exportBundlerDialog.ui index d8a78d70..469955f6 100644 --- a/guilib/src/ui/exportBundlerDialog.ui +++ b/guilib/src/ui/exportBundlerDialog.ui @@ -6,8 +6,8 @@ 0 0 - 524 - 363 + 557 + 466 @@ -119,21 +119,116 @@ - - + + + + + + + + + Export 3D Points + + + true + + + false + + + + + + + + 1 + + + 999999 + + + 100 + + + + + - Export 3D points. RTAB-Map must be built with g2o. + SBA Iterations true - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + SBA Pixel variance used by g2o. + + + true + + + + + + + + + + 2 + + + 0.010000000000000 + + + 999.000000000000000 + + + 0.010000000000000 + + + 1.000000000000000 + + + + + + + SBA Type + + + true + + + + + + + + g2o + + + + + cvsba + + + + + + + + Rematch features. + + + true - + diff --git a/guilib/src/ui/postProcessingDialog.ui b/guilib/src/ui/postProcessingDialog.ui index 9d0ee1b8..73cb568f 100644 --- a/guilib/src/ui/postProcessingDialog.ui +++ b/guilib/src/ui/postProcessingDialog.ui @@ -260,6 +260,23 @@ + + + + Rematch features. + + + true + + + + + + + + + + @@ -278,6 +295,16 @@ + + + + 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->Edit database...). + + + true + + + diff --git a/tools/Reprocess/main.cpp b/tools/Reprocess/main.cpp index 66056a84..84202db8 100644 --- a/tools/Reprocess/main.cpp +++ b/tools/Reprocess/main.cpp @@ -50,6 +50,7 @@ void showUsage() " rtabmap-reprocess [options] \"input.db\" \"output.db\"\n" " rtabmap-reprocess [options] \"input1.db;input2.db;input3.db\" \"output.db\"\n" " For the second example, only parameters from the first database are used.\n" + " To see warnings when loop closures are rejected, add \"--uwarn\" argument.\n" " Options:\n" " -r Use database stamps as input rate.\n" " -c \"path.ini\" Configuration file, overwritting parameters read \n" @@ -191,10 +192,7 @@ int main(int argc, char * argv[]) ParametersMap parameters = dbDriver->getLastParameters(); if(parameters.empty()) { - printf("Failed getting parameters from database, reprocessing cannot be done. Database version may be too old.\n"); - dbDriver->closeConnection(false); - delete dbDriver; - return -1; + printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", dbDriver->getDatabaseVersion().c_str()); } if(customParameters.size()) {