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

@@ -140,14 +140,15 @@ public:
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures,
 			std::map<int, cv::Point3f> & points3DMap,
-			std::map<int, std::map<int, FeatureBA> > & wordReferences); // <ID words, IDs frames + keypoint/depth/descriptor>
-
+			std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
+			bool rematchFeatures = false);
 
 	std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, Signature> & signatures);
+			const std::map<int, Signature> & signatures,
+			bool rematchFeatures = false);
 
 	Transform optimizeBA(
 			const Link & link,
@@ -161,7 +162,8 @@ public:
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures,
 			std::map<int, cv::Point3f> & points3DMap,
-			std::map<int, std::map<int, FeatureBA > > & wordReferences); // <ID words, IDs frames + keypoint/depth/descriptor>
+			std::map<int, std::map<int, FeatureBA > > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
+			bool rematchFeatures = false);
 
 protected:
 	Optimizer(

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.");

corelib/src/Optimizer.cpp

@@ -372,7 +372,8 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		const std::multimap<int, Link> & links,
 		const std::map<int, Signature> & signatures,
 		std::map<int, cv::Point3f> & points3DMap,
-		std::map<int, std::map<int, FeatureBA> > & wordReferences)
+		std::map<int, std::map<int, FeatureBA> > & wordReferences,
+		bool rematchFeatures)
 {
 	UDEBUG("");
 	std::map<int, CameraModel> models;
@@ -417,7 +418,7 @@ std::map<int, Transform> 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<int, Transform> Optimizer::optimizeBA(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		const std::map<int, Signature> & signatures)
+		const std::map<int, Signature> & signatures,
+		bool rematchFeatures)
 {
 	std::map<int, cv::Point3f> points3DMap;
 	std::map<int, std::map<int, FeatureBA> > 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<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
 		const std::map<int, Signature> & signatures,
 		std::map<int, cv::Point3f> & points3DMap,
-		std::map<int, std::map<int, FeatureBA> > & wordReferences)
+		std::map<int, std::map<int, FeatureBA> > & wordReferences,
+		bool rematchFeatures)
 {
 	UDEBUG("");
 	int wordCount = 0;
 	int edgeWithWordsAdded = 0;
+	std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> > frameToWordMap; // <FrameId, <Keypoint, wordId> >
 	for(std::multimap<int, Link>::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<int, cv::Mat>());
-					//sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+					if(!rematchFeatures)
+					{
+						sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+						sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+					}
 
 					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; i<info.inliersIDs.size(); ++i)
@@ -523,27 +555,75 @@ void Optimizer::computeBACorrespondences(
 							cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
 							if(p.x > 0.0f) // make sure the point is valid
 							{
-								int wordId = ++wordCount;
-
-								wordReferences.insert(std::make_pair(wordId, std::map<int, FeatureBA>()));
-
 								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<int, cv::Point3f>::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<int, std::map<cv::KeyPoint, int, KeyPointCompare> >::iterator fromIter = frameToWordMap.find(sFrom.id());
+								std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> >::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<int, FeatureBA>()));
+
+									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<cv::KeyPoint, int, KeyPointCompare>()));
+									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<int, cv::Point3f>::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<cv::KeyPoint, int, KeyPointCompare>()));
+									frameToWordMap.at(sTo.id()).insert(std::make_pair(ptTo, wordId));
+								}
 							}
 						}
 						++edgeWithWordsAdded;

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
 	{

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 <rtabmap/core/Signature.h>
+#include <rtabmap/core/Parameters.h>
 #include <QDialog>
 #include <QSettings>
 
@@ -55,7 +56,8 @@ public:
 	void 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);
 
 Q_SIGNALS:
 	void configChanged();
@@ -63,6 +65,7 @@ Q_SIGNALS:
 private Q_SLOTS:
 	void getPath();
 	void restoreDefaults();
+	void updateVisibility();
 
 private:
 	Ui_ExportBundlerDialog * _ui;

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();

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

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

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);
+}
 
 
 }

guilib/src/ui/exportBundlerDialog.ui

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

guilib/src/ui/postProcessingDialog.ui

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

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())
 	{