Parameters: Added "Reg/VarianceNormalized". Registration: Transforms are now only normalized in Registration. Added rgbd_dataset tool. MainWindow: odom inliers-only shown option. ExportCloudsDialog: ignoring intermediate nodes for texturing (if they don't have data). DatabaseViewer: "optimized" checkbox enabled in Constraints view for non-neighbor links too.

corelib/include/rtabmap/core/Parameters.h

@@ -399,6 +399,7 @@ class RTABMAP_EXP Parameters
 
 	// Common registration parameters
 	RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
+	RTABMAP_PARAM(Reg, VarianceNormalized,       bool, false,   "Normalize covariance values. Position variances are multiplied by norm of the transform and orientation variances are multiplied by angle of the transform.");
 	RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
 	RTABMAP_PARAM(Reg, Force3DoF,                bool, false,   "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
 	

corelib/include/rtabmap/core/Registration.h

@@ -63,6 +63,7 @@ public:
 
 	bool varianceFromInliersCount() const {return varianceFromInliersCount_;}
 	bool force3DoF() const {return force3DoF_;}
+	bool covarianceNormalized() const {return covarianceNormalized_;}
 
 	// take ownership!
 	void setChildRegistration(Registration * child);
@@ -104,6 +105,7 @@ protected:
 
 private:
 	bool varianceFromInliersCount_;
+	bool covarianceNormalized_;
 	bool force3DoF_;
 	Registration * child_;
 

corelib/src/Link.cpp

@@ -117,7 +117,7 @@ Link Link::merge(const Link & link, Type outputType) const
 			link.to(),
 			outputType,
 			transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
-			transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_ + link.infMatrix());
+			transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.inv() + link.infMatrix().inv()).inv());
 }
 
 Link Link::inverse() const

corelib/src/Memory.cpp

@@ -2269,15 +2269,6 @@ Transform Memory::computeTransform(
 					info->rejectedMsg = msg;
 				}
 			}
-			else if(info && !transform.isIdentity())
-			{
-				//normalize variance
-				info->covariance *= transform.getNorm();
-				if(info->covariance.at<double>(0,0) < 0.0001)
-				{
-					info->covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
-				}
-			}
 		}
 	}
 	return transform;
@@ -2325,16 +2316,6 @@ Transform Memory::computeIcpTransform(
 		// compute transform fromId -> toId
 		std::vector<int> inliersV;
 		t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, info);
-
-		if(!t.isNull() && !t.isIdentity() && info)
-		{
-			// normalize variance
-			info->covariance *= t.getNorm();
-			if(info->covariance.at<double>(0,0)<=0.0)
-			{
-				info->covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
-			}
-		}
 	}
 	else
 	{

corelib/src/Odometry.cpp

@@ -529,12 +529,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			info->distanceTravelled = distanceTravelled_;
 		}
 
-		info->covariance *= t.getNorm();
-		if(info->covariance.at<double>(0,0)<=0.0)
-		{
-			info->covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
-		}
-
 		return _pose *= t; // update
 	}
 	else if(_resetCurrentCount > 0)

corelib/src/OdometryF2M.cpp

@@ -250,7 +250,6 @@ Transform OdometryF2M::computeTransform(
 						UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
 								uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
 
-						regInfo.covariance *= transform.getNorm();
 						bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.covariance.inv())));
 						bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
 

corelib/src/Registration.cpp

@@ -62,6 +62,7 @@ Registration * Registration::create(Registration::Type & type, const ParametersM
 
 Registration::Registration(const ParametersMap & parameters, Registration * child) :
 	varianceFromInliersCount_(Parameters::defaultRegVarianceFromInliersCount()),
+	covarianceNormalized_(Parameters::defaultRegVarianceNormalized()),
 	force3DoF_(Parameters::defaultRegForce3DoF()),
 	child_(child)
 {
@@ -78,6 +79,7 @@ Registration::~Registration()
 void Registration::parseParameters(const ParametersMap & parameters)
 {
 	Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), varianceFromInliersCount_);
+	Parameters::parse(parameters, Parameters::kRegVarianceNormalized(), covarianceNormalized_);
 	Parameters::parse(parameters, Parameters::kRegForce3DoF(), force3DoF_);
 	if(child_)
 	{
@@ -193,6 +195,11 @@ Transform Registration::computeTransformationMod(
 
 	Transform t = computeTransformationImpl(from, to, guess, info);
 
+	if(info.covariance.empty())
+	{
+		info.covariance = cv::Mat::eye(6,6,CV_64FC1);
+	}
+
 	if(varianceFromInliersCount_)
 	{
 		if(info.icpInliersRatio)
@@ -203,12 +210,38 @@ Transform Registration::computeTransformationMod(
 		{
 			info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(info.inliers > 0?1.0/double(info.inliers):1.0);
 		}
-		if(info.covariance.at<double>(0,0)<0.0001)
+	}
+
+	if(covarianceNormalized_)
+	{
+		// normalize variance
+		UASSERT(info.covariance.cols == 6 && info.covariance.rows == 6);
+		float norm = t.getNorm();
+		if(norm > 0.0f)
 		{
-			info.covariance =cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
+			cv::Mat(info.covariance, cv::Range(0,3), cv::Range(0,3)) *= norm;
+		}
+		float angle = t.getAngle();
+		if(angle > 0.0f)
+		{
+			cv::Mat(info.covariance, cv::Range(3,6), cv::Range(3,6)) *= angle;
 		}
 	}
 
+	double epsilon = 0.000001;
+	if(info.covariance.at<double>(0,0)<=0.0)
+		info.covariance.at<double>(0,0) = epsilon; // epsilon if exact transform
+	if(info.covariance.at<double>(1,1)<=0.0)
+		info.covariance.at<double>(1,1) = epsilon; // epsilon if exact transform
+	if(info.covariance.at<double>(2,2)<=0.0)
+		info.covariance.at<double>(2,2) = epsilon; // epsilon if exact transform
+	if(info.covariance.at<double>(3,3)<=0.0)
+		info.covariance.at<double>(3,3) = epsilon; // epsilon if exact transform
+	if(info.covariance.at<double>(4,4)<=0.0)
+		info.covariance.at<double>(4,4) = epsilon; // epsilon if exact transform
+	if(info.covariance.at<double>(5,5)<=0.0)
+		info.covariance.at<double>(5,5) = epsilon; // epsilon if exact transform
+
 	if(child_)
 	{
 		if(!t.isNull())

corelib/src/RegistrationVis.cpp

@@ -1291,10 +1291,30 @@ Transform RegistrationVis::computeTransformationImpl(
 			poses.insert(std::make_pair(1, Transform::getIdentity()));
 			poses.insert(std::make_pair(2, transforms[0]));
 
-			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], (covariances[0]*transforms[0].getNorm()).inv())));
+			cv::Mat cov = covariances[0].clone();
+			if(covarianceNormalized())
+			{
+				cv::Mat(cov, cv::Range(0,3), cv::Range(0,3)) *= transform.getNorm();
+				cv::Mat(cov, cv::Range(3,6), cv::Range(3,6)) *= transform.getAngle();
+			}
+			if(cov.at<double>(0,0)<=0.0)
+			{
+				cov = cv::Mat::eye(6,6,CV_64FC1)*0.000001; // epsilon if exact transform
+			}
+			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
 			if(!transforms[1].isNull() && inliers[1].size())
 			{
-				links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], (covariances[1]*transforms[1].getNorm()).inv())));
+				cov = covariances[1].clone();
+				if(covarianceNormalized())
+				{
+					cv::Mat(cov, cv::Range(0,3), cv::Range(0,3)) *= transform.getNorm();
+					cv::Mat(cov, cv::Range(3,6), cv::Range(3,6)) *= transform.getAngle();
+				}
+				if(cov.at<double>(0,0)<=0.0)
+				{
+					cov = cv::Mat::eye(6,6,CV_64FC1)*0.000001; // epsilon if exact transform
+				}
+				links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
 			}
 
 			std::map<int, Transform> optimizedPoses;
@@ -1450,7 +1470,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	info.inliers = inliersCount;
 	info.matches = matchesCount;
 	info.rejectedMsg = msg;
-	info.covariance = covariance.at<double>(0,0)>0.0001?covariance:cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
+	info.covariance = covariance;
 
 	UDEBUG("transform=%s", transform.prettyPrint().c_str());
 	return transform;

corelib/src/RtabmapThread.cpp

@@ -589,7 +589,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		double maxTransVar = odomEvent.transVariance();
 		if(maxRotVar != 1.0f && maxTransVar != 1.0f && !covariance_.empty())
 		{
-			covariance_ = (covariance_.inv() + odomEvent.covariance().inv()).inv();
+			covariance_ += odomEvent.covariance();
 		}
 		else
 		{

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -149,6 +149,7 @@ public:
 	bool isCacheSavedInFigures() const;
 	bool notifyWhenNewGlobalPathIsReceived() const;
 	int getOdomQualityWarnThr() const;
+	bool isOdomOnlyInliersShown() const;
 	bool isPosteriorGraphView() const;
 	int getOdomRegistrationApproach() const;
 	bool isOdomDisabled() const;

guilib/src/DatabaseViewer.cpp

@@ -2206,6 +2206,10 @@ void DatabaseViewer::detectMoreLoopClosures()
 				}
 			}
 			progressDialog->incrementStep();
+			if(i%100)
+			{
+				QApplication::processEvents();
+			}
 		}
 		UINFO("Iteration %d/%d: added %d loop closures.", n+1, iterations, (int)addedLinks.size()/2);
 		progressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!").arg(n+1).arg(iterations).arg(addedLinks.size()/2));
@@ -3321,30 +3325,27 @@ void DatabaseViewer::updateConstraintView(
 			.arg(sqrt(link.rotVariance()))
 			.arg(sqrt(link.transVariance())));
 	ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
-	if((link.type() == Link::kNeighbor || link.type() == Link::kNeighborMerged) &&
-	   graphes_.size() &&
+	if(graphes_.size() &&
 	   (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
 	{
 		std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
-		if(link.type() == Link::kNeighbor || link.type() == Link::kNeighborMerged)
-		{
-			std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
-			std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
-			if(iterFrom != graph.end() && iterTo != graph.end())
-			{
-				ui_->checkBox_showOptimized->setEnabled(true);
-				Transform topt = iterFrom->second.inverse()*iterTo->second;
-				float diff = topt.getDistance(t);
-				Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
-				Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
-				float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
-				a = (a *180.0f) / CV_PI;
-				ui_->label_constraint_opt->setText(QString("%1\n(error=%2% a=%3)").arg(QString(topt.prettyPrint().c_str()).replace(" ", "\n")).arg((diff/t.getNorm())*100.0f).arg(a));
 
-				if(ui_->checkBox_showOptimized->isChecked())
-				{
-					t = topt;
-				}
+		std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
+		std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
+		if(iterFrom != graph.end() && iterTo != graph.end())
+		{
+			ui_->checkBox_showOptimized->setEnabled(true);
+			Transform topt = iterFrom->second.inverse()*iterTo->second;
+			float diff = topt.getDistance(t);
+			Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
+			Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
+			float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
+			a = (a *180.0f) / CV_PI;
+			ui_->label_constraint_opt->setText(QString("%1\n(error=%2% a=%3)").arg(QString(topt.prettyPrint().c_str()).replace(" ", "\n")).arg((diff/t.getNorm())*100.0f).arg(a));
+
+			if(ui_->checkBox_showOptimized->isChecked())
+			{
+				t = topt;
 			}
 		}
 	}

guilib/src/ExportCloudsDialog.cpp

@@ -1071,6 +1071,8 @@ bool ExportCloudsDialog::getExportedClouds(
 				parameters,
 				has2dScans);
 
+		std::set<int> validCameras = uKeysSet(clouds);
+
 		UDEBUG("");
 		if(_canceled)
 		{
@@ -1915,60 +1917,63 @@ bool ExportCloudsDialog::getExportedClouds(
 				std::map<int, std::vector<CameraModel> > cameraModels;
 				for(std::map<int, Transform>::iterator jter=cameras.begin(); jter!=cameras.end(); ++jter)
 				{
-					std::vector<CameraModel> models;
-					StereoCameraModel stereoModel;
-					bool cacheHasCompressedImage = false;
-					if(cachedSignatures.contains(jter->first))
+					if(validCameras.find(jter->first) != validCameras.end())
 					{
-						const SensorData & data = cachedSignatures.find(jter->first)->sensorData();
-						models = data.cameraModels();
-						stereoModel = data.stereoCameraModel();
-						cacheHasCompressedImage = !data.imageCompressed().empty();
-					}
-					else if(_dbDriver)
-					{
-						_dbDriver->getCalibration(jter->first, models, stereoModel);
-					}
-
-					if(stereoModel.isValidForProjection())
-					{
-						models.clear();
-						models.push_back(stereoModel.left());
-					}
-					else if(models.size() == 0 || !models[0].isValidForProjection())
-					{
-						models.clear();
-					}
-					if(!jter->second.isNull() && models.size())
-					{
-						if(models[0].imageWidth() == 0 || models[0].imageHeight() == 0)
+						std::vector<CameraModel> models;
+						StereoCameraModel stereoModel;
+						bool cacheHasCompressedImage = false;
+						if(cachedSignatures.contains(jter->first))
 						{
-							// we are using an old database format (image size not saved in calibrations), we should
-							// uncompress images to get their size
-							cv::Mat img;
-							if(cacheHasCompressedImage)
-							{
-								cachedSignatures.find(jter->first)->sensorData().uncompressDataConst(&img, 0);
-							}
-							else if(_dbDriver)
-							{
-								SensorData data;
-								_dbDriver->getNodeData(jter->first, data, true, false, false, false);
-								data.uncompressDataConst(&img, 0);
-							}
-							cv::Size imageSize = img.size();
-							imageSize.width /= models.size();
-							for(unsigned int i=0; i<models.size(); ++i)
-							{
-								models[i].setImageSize(imageSize);
-							}
-
+							const SensorData & data = cachedSignatures.find(jter->first)->sensorData();
+							models = data.cameraModels();
+							stereoModel = data.stereoCameraModel();
+							cacheHasCompressedImage = !data.imageCompressed().empty();
+						}
+						else if(_dbDriver)
+						{
+							_dbDriver->getCalibration(jter->first, models, stereoModel);
 						}
 
-						if(models[0].imageWidth() != 0 && models[0].imageHeight() != 0)
+						if(stereoModel.isValidForProjection())
 						{
-							cameraPoses.insert(std::make_pair(jter->first, jter->second));
-							cameraModels.insert(std::make_pair(jter->first, models));
+							models.clear();
+							models.push_back(stereoModel.left());
+						}
+						else if(models.size() == 0 || !models[0].isValidForProjection())
+						{
+							models.clear();
+						}
+						if(!jter->second.isNull() && models.size())
+						{
+							if(models[0].imageWidth() == 0 || models[0].imageHeight() == 0)
+							{
+								// we are using an old database format (image size not saved in calibrations), we should
+								// uncompress images to get their size
+								cv::Mat img;
+								if(cacheHasCompressedImage)
+								{
+									cachedSignatures.find(jter->first)->sensorData().uncompressDataConst(&img, 0);
+								}
+								else if(_dbDriver)
+								{
+									SensorData data;
+									_dbDriver->getNodeData(jter->first, data, true, false, false, false);
+									data.uncompressDataConst(&img, 0);
+								}
+								cv::Size imageSize = img.size();
+								imageSize.width /= models.size();
+								for(unsigned int i=0; i<models.size(); ++i)
+								{
+									models[i].setImageSize(imageSize);
+								}
+
+							}
+
+							if(models[0].imageWidth() != 0 && models[0].imageHeight() != 0)
+							{
+								cameraPoses.insert(std::make_pair(jter->first, jter->second));
+								cameraModels.insert(std::make_pair(jter->first, models));
+							}
 						}
 					}
 				}

guilib/src/MainWindow.cpp

@@ -1120,8 +1120,13 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 						bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
 						(*cloud)[i].r = inlier?0:255;
 						(*cloud)[i].g = 255;
-						(*cloud)[i++].b = 0;
+						(*cloud)[i].b = 0;
+						if(!_preferencesDialog->isOdomOnlyInliersShown() || inlier)
+						{
+							++i;
+						}
 					}
+					cloud->resize(i);
 
 					_cloudViewer->addCloud("featuresOdom", cloud, _odometryCorrection);
 					_cloudViewer->setCloudVisibility("featuresOdom", true);
@@ -1185,10 +1190,25 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 		{
 			if(odom.info().type == 0)
 			{
-				_ui->imageView_odometry->setFeatures(
-						odom.info().words,
-						odom.data().depthRaw(),
-						Qt::yellow);
+				if(_preferencesDialog->isOdomOnlyInliersShown())
+				{
+					std::multimap<int, cv::KeyPoint> kpInliers;
+					for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
+					{
+						kpInliers.insert(*odom.info().words.find(odom.info().wordInliers[i]));
+					}
+					_ui->imageView_odometry->setFeatures(
+							kpInliers,
+							odom.data().depthRaw(),
+							Qt::green);
+				}
+				else
+				{
+					_ui->imageView_odometry->setFeatures(
+							odom.info().words,
+							odom.data().depthRaw(),
+							Qt::yellow);
+				}
 			}
 			else if(odom.info().type == 1)
 			{
@@ -1238,7 +1258,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 
 			if(odom.info().type == 0)
 			{
-				if(_ui->imageView_odometry->isFeaturesShown())
+				if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
 				{
 					for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
 					{
@@ -5179,7 +5199,7 @@ void MainWindow::postProcessing()
 										if(!msg.empty())
 										{
 											UWARN("%s", msg.c_str());
-											_initProgressDialog->appendText(tr("%s").arg(msg.c_str()));
+											_initProgressDialog->appendText(tr("%1").arg(msg.c_str()));
 											QApplication::processEvents();
 											updateConstraint = false;
 										}
@@ -6678,7 +6698,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		}
 	}
 	actions = _ui->menuFile->actions();
-	if(actions.size()==17)
+	if(actions.size()==16)
 	{
 		if(actions.at(2)->isSeparator())
 		{
@@ -6688,9 +6708,9 @@ void MainWindow::changeState(MainWindow::State newState)
 		{
 			UWARN("Menu File separators have not the same order.");
 		}
-		if(actions.at(13)->isSeparator())
+		if(actions.at(12)->isSeparator())
 		{
-			actions.at(13)->setVisible(!monitoring);
+			actions.at(12)->setVisible(!monitoring);
 		}
 		else
 		{

guilib/src/PreferencesDialog.cpp

@@ -306,6 +306,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_cacheStatistics, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_notifyWhenNewGlobalPathIsReceived, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->spinBox_odomQualityWarnThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
+	connect(_ui->checkBox_odom_onlyInliersShown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_posteriorGraphView, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkbox_odomDisabled, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->odom_registration, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
@@ -785,6 +786,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	// Registration
 	_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
+	_ui->reg_varianceNormalized->setObjectName(Parameters::kRegVarianceNormalized().c_str());
 	_ui->comboBox_registrationStrategy->setObjectName(Parameters::kRegStrategy().c_str());
 	_ui->loopClosure_bowForce2D->setObjectName(Parameters::kRegForce3DoF().c_str());
 
@@ -1255,6 +1257,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_imageRejectedShown->setChecked(true);
 		_ui->checkBox_imageHighestHypShown->setChecked(false);
 		_ui->spinBox_odomQualityWarnThr->setValue(50);
+		_ui->checkBox_odom_onlyInliersShown->setChecked(false);
 		_ui->checkBox_posteriorGraphView->setChecked(true);
 		_ui->checkbox_odomDisabled->setChecked(false);
 		_ui->checkbox_groundTruthAlign->setChecked(true);
@@ -1651,6 +1654,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->checkBox_cacheStatistics->setChecked(settings.value("figure_cache", _ui->checkBox_cacheStatistics->isChecked()).toBool());
 	_ui->checkBox_notifyWhenNewGlobalPathIsReceived->setChecked(settings.value("notifyNewGlobalPath", _ui->checkBox_notifyWhenNewGlobalPathIsReceived->isChecked()).toBool());
 	_ui->spinBox_odomQualityWarnThr->setValue(settings.value("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value()).toInt());
+	_ui->checkBox_odom_onlyInliersShown->setChecked(settings.value("odomOnlyInliersShown", _ui->checkBox_odom_onlyInliersShown->isChecked()).toBool());
 	_ui->checkBox_posteriorGraphView->setChecked(settings.value("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked()).toBool());
 	_ui->checkbox_odomDisabled->setChecked(settings.value("odomDisabled", _ui->checkbox_odomDisabled->isChecked()).toBool());
 	_ui->odom_registration->setCurrentIndex(settings.value("odomRegistration", _ui->odom_registration->currentIndex()).toInt());
@@ -2037,6 +2041,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("figure_cache",         _ui->checkBox_cacheStatistics->isChecked());
 	settings.setValue("notifyNewGlobalPath",  _ui->checkBox_notifyWhenNewGlobalPathIsReceived->isChecked());
 	settings.setValue("odomQualityThr",       _ui->spinBox_odomQualityWarnThr->value());
+	settings.setValue("odomOnlyInliersShown", _ui->checkBox_odom_onlyInliersShown->isChecked());
 	settings.setValue("posteriorGraphView",   _ui->checkBox_posteriorGraphView->isChecked());
 	settings.setValue("odomDisabled",         _ui->checkbox_odomDisabled->isChecked());
 	settings.setValue("odomRegistration",     _ui->odom_registration->currentIndex());
@@ -4026,6 +4031,10 @@ int PreferencesDialog::getOdomQualityWarnThr() const
 {
 	return _ui->spinBox_odomQualityWarnThr->value();
 }
+bool PreferencesDialog::isOdomOnlyInliersShown() const
+{
+	return _ui->checkBox_odom_onlyInliersShown->isChecked();
+}
 bool PreferencesDialog::isPosteriorGraphView() const
 {
 	return _ui->checkBox_posteriorGraphView->isChecked();

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-667</y>
+            <y>0</y>
             <width>673</width>
             <height>2747</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>18</number>
+                <number>19</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -272,7 +272,7 @@
                     <item>
                      <widget class="QGroupBox" name="groupBox_5">
                       <property name="title">
-                       <string>Loop Closure Detection View</string>
+                       <string>Loop Closure Detection</string>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_31" columnstretch="0,1">
                        <item row="1" column="0">
@@ -371,16 +371,15 @@
                      </widget>
                     </item>
                     <item>
-                     <widget class="QGroupBox" name="groupBox">
+                     <widget class="QGroupBox" name="groupBox_21">
                       <property name="title">
-                       <string>3D Map View</string>
+                       <string>Odometry</string>
                       </property>
-                      <layout class="QGridLayout" name="gridLayout_38" columnstretch="0,1">
+                      <layout class="QGridLayout" name="gridLayout_85" columnstretch="0,1">
                        <item row="0" column="1">
-                        <widget class="QLabel" name="label_86">
+                        <widget class="QLabel" name="label_369">
                          <property name="text">
-                          <string>Odometry warning theshold:
-Show a yellow background when the number of odometry inliers goes under this threshold. If 0, it is ignored. You can see the current odometry inliers count under Statistics view -&gt; General -&gt; Odom inliers.</string>
+                          <string>Show only inliers.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -391,6 +390,30 @@ Show a yellow background when the number of odometry inliers goes under this thr
                         </widget>
                        </item>
                        <item row="0" column="0">
+                        <widget class="QCheckBox" name="checkBox_odom_onlyInliersShown">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>false</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_159">
+                         <property name="text">
+                          <string>Warning theshold:
+Show a yellow background when the number of odometry inliers goes under this threshold. If 0, it is ignored. You can see the current odometry inliers count under Statistics view -&gt; General -&gt; Odom inliers.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="0">
                         <widget class="QSpinBox" name="spinBox_odomQualityWarnThr">
                          <property name="maximum">
                           <number>9999</number>
@@ -2678,7 +2701,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>2</number>
+                       <number>0</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -2788,7 +2811,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_rgbd">
                              <property name="currentIndex">
-                              <number>6</number>
+                              <number>7</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -3480,7 +3503,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                      <string>For RGBD-SLAM datasets, it is 5.</string>
                                     </property>
                                     <property name="decimals">
-                                     <number>1</number>
+                                     <number>3</number>
                                     </property>
                                     <property name="minimum">
                                      <double>1.000000000000000</double>
@@ -10763,6 +10786,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       </property>
                      </widget>
                     </item>
+                    <item row="2" column="1">
+                     <widget class="QLabel" name="label_370">
+                      <property name="text">
+                       <string>Normalize covariance values. Position variances are multiplied by norm of the transform and orientation variances are multiplied by angle of the transform.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="2" column="0">
+                     <widget class="QCheckBox" name="reg_varianceNormalized">
+                      <property name="text">
+                       <string/>
+                      </property>
+                     </widget>
+                    </item>
                    </layout>
                   </widget>
                  </item>

tools/CMakeLists.txt

@@ -6,6 +6,7 @@ ADD_SUBDIRECTORY( Camera )
 ADD_SUBDIRECTORY( CameraRGBD )
 ADD_SUBDIRECTORY( StereoEval )
 ADD_SUBDIRECTORY( KittiDataset )
+ADD_SUBDIRECTORY( RgbdDataset )
 
 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )

tools/KittiDataset/CMakeLists.txt

@@ -1,28 +1,15 @@
 cmake_minimum_required(VERSION 2.8)
 
-IF(DEFINED PROJECT_NAME)
-   set(internal TRUE)
-ENDIF(DEFINED PROJECT_NAME)
-
-if(internal)
-	# inside rtabmap project (see below for external build)
-	SET(RTABMap_INCLUDE_DIRS 
-	    ${PROJECT_SOURCE_DIR}/utilite/include
-		${PROJECT_SOURCE_DIR}/corelib/include
-	)
-	SET(RTABMap_LIBRARIES 
-	    rtabmap_core
-		rtabmap_utilite
-	)  
-else()
-	# external build
-	PROJECT( MyProject )
+# inside rtabmap project (see below for external build)
+SET(RTABMap_INCLUDE_DIRS 
+    ${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+)
+SET(RTABMap_LIBRARIES 
+    rtabmap_core
+	rtabmap_utilite
+)  
 	
-	FIND_PACKAGE(RTABMap REQUIRED)
-	FIND_PACKAGE(OpenCV REQUIRED)
-	FIND_PACKAGE(PCL 1.7 REQUIRED)
-endif()
-
 if(POLICY CMP0020)
 	cmake_policy(SET CMP0020 OLD)
 endif()
@@ -45,7 +32,5 @@ ADD_EXECUTABLE(kitti_dataset main.cpp)
   
 TARGET_LINK_LIBRARIES(kitti_dataset ${LIBRARIES})
 
-if(internal)
-  SET_TARGET_PROPERTIES( kitti_dataset 
+SET_TARGET_PROPERTIES( kitti_dataset 
     PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-kitti_dataset)
-endif(internal)

tools/KittiDataset/main.cpp

@@ -216,11 +216,14 @@ int main(int argc, char * argv[])
 	{
 		gtPath = uReplaceChar(gtPath, '~', UDirectory::homeDir());
 		gtPath = uReplaceChar(gtPath, '\\', '/');
-		printf("   Ground Truth:      %s\n", gtPath.c_str());
 		if(!UFile::exists(gtPath))
 		{
-			UERROR("Ground truth file path is not valid: \"%s\"", gtPath.c_str());
-			return -1;
+			UWARN("Ground truth file path doesn't exist: \"%s\", benchmark values won't be computed.", gtPath.c_str());
+			gtPath.clear();
+		}
+		else
+		{
+			printf("   Ground Truth:      %s\n", gtPath.c_str());
 		}
 	}
 	if(disp)
@@ -323,7 +326,7 @@ int main(int argc, char * argv[])
 						Transform(-0.27f, 0.0f, 0.08, 0.0f, 0.0f, 0.0f));
 	}
 
-	bool intermediateNodes = false;
+	bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
 	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
 	std::string databasePath = output+"/rtabmap" + seq + ".db";
 	UFile::erase(databasePath);
@@ -386,7 +389,7 @@ int main(int argc, char * argv[])
 			}
 			else
 			{
-				covariance = (covariance.inv() + odomInfo.covariance.inv()).inv();
+				covariance += odomInfo.covariance;
 			}
 
 			timer.restart();
@@ -399,8 +402,13 @@ int main(int argc, char * argv[])
 			double slamTime = timer.ticks();
 
 			++iteration;
-			printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms\n",
+			printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms",
 					iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+			if(processData && rtabmap.getLoopClosureId()>0)
+			{
+				printf(" *");
+			}
+			printf("\n");
 
 			cameraInfo = CameraInfo();
 			timer.restart();

tools/RgbdDataset/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.8)
+
+# inside rtabmap project (see below for external build)
+SET(RTABMap_INCLUDE_DIRS 
+    ${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+)
+SET(RTABMap_LIBRARIES 
+    rtabmap_core
+	rtabmap_utilite
+)  
+
+if(POLICY CMP0020)
+	cmake_policy(SET CMP0020 OLD)
+endif()
+
+SET(INCLUDE_DIRS
+	${RTABMap_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${RTABMap_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${PCL_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(rgbd_dataset main.cpp)
+  
+TARGET_LINK_LIBRARIES(rgbd_dataset ${LIBRARIES})
+
+
+SET_TARGET_PROPERTIES( rgbd_dataset 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_dataset)

tools/RgbdDataset/main.cpp

@@ -0,0 +1,463 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/OdometryF2M.h>
+#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraThread.h"
+#include "rtabmap/core/Graph.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/UStl.h"
+#include <pcl/common/common.h>
+#include <stdio.h>
+#include <signal.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-rgbd_dataset [options] path\n"
+			"  path               Folder of the sequence (e.g., \"~/rgbd_dataset_freiburg3_long_office_household\")\n"
+			"                        containing least rgb_sync and depth_sync folders. These folders contain\n"
+			"                        synchronized images using associate.py tool (use tool version from\n"
+			"                        https://gist.github.com/matlabbe/484134a2d9da8ad425362c6669824798). If \n"
+			"                        \"groundtruth.txt\" is found in the sequence folder, they will be saved in the database.\n"
+			"  --output           Output directory. By default, results are saved in \"path\".\n\n"
+			"%s\n"
+			"Example:\n\n"
+			"   $ rtabmap-kitti_dataset \\\n"
+			"       --Vis/EstimationType 1\\\n"
+			"       --Vis/BundleAdjustment 1\\\n"
+			"       --Vis/PnPReprojError 1.5\\\n"
+			"       --Odom/GuessMotion true\\\n"
+			"       --OdomF2M/BundleAdjustment 1\\\n"
+			"       --Rtabmap/CreateIntermediateNodes true\\\n"
+			"       --Rtabmap/DetectionRate 1\\\n"
+			"       ~/rgbd_dataset_freiburg3_long_office_household\n\n", rtabmap::Parameters::showUsage());
+	exit(1);
+}
+
+// catch ctrl-c
+bool g_forever = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_forever = false;
+}
+
+int main(int argc, char * argv[])
+{
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ParametersMap parameters;
+	std::string path;
+	std::string output;
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else
+	{
+		for(int i=1; i<argc; ++i)
+		{
+			if(std::strcmp(argv[i], "--output") == 0)
+			{
+				output = argv[++i];
+			}
+		}
+		parameters = Parameters::parseArguments(argc, argv);
+		path = argv[argc-1];
+		path = uReplaceChar(path, '~', UDirectory::homeDir());
+		path = uReplaceChar(path, '\\', '/');
+		if(output.empty())
+		{
+			output = path;
+		}
+		else
+		{
+			output = uReplaceChar(output, '~', UDirectory::homeDir());
+			UDirectory::makeDir(output);
+		}
+	}
+
+	std::string pathRgbImages  = path+"/rgb_sync";
+	std::string pathDepthImages = path+"/depth_sync";
+	std::string pathGt = path+"/groundtruth.txt";
+	if(!UFile::exists(pathGt))
+	{
+		UWARN("Ground truth file path doesn't exist: \"%s\", benchmark values won't be computed.", pathGt.c_str());
+		pathGt.clear();
+	}
+
+	printf("Paths:\n"
+			"   Dataset path:    %s\n"
+			"   RGB path:        %s\n"
+			"   Depth path:      %s\n"
+			"   Output:          %s\n",
+			path.c_str(),
+			pathRgbImages.c_str(),
+			pathDepthImages.c_str(),
+			output.c_str());
+	if(!pathGt.empty())
+	{
+		printf("   groundtruth.txt: %s\n", pathGt.c_str());
+	}
+	if(!parameters.empty())
+	{
+		printf("Parameters:\n");
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			printf("   %s=%s\n", iter->first.c_str(), iter->second.c_str());
+		}
+	}
+
+	// setup calibraiton file
+	CameraModel model;
+	std::string sequenceName = UFile(path).getName();
+	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+	float depthFactor = 5.0f;
+	if(sequenceName.find("freiburg1") != std::string::npos)
+	{
+		model = CameraModel("rtabmap_calib", 517.3, 516.5, 318.6, 255.3, opticalRotation, 0, cv::Size(640,480));
+	}
+	else if(sequenceName.find("freiburg2") != std::string::npos)
+	{
+		model = CameraModel("rtabmap_calib", 520.9, 521.0, 325.1, 249.7, opticalRotation, 0, cv::Size(640,480));
+		depthFactor = 5.208f;
+	}
+	else //if(sequenceName.find("freiburg3") != std::string::npos)
+	{
+		model = CameraModel("rtabmap_calib", 535.4, 539.2, 320.1, 247.6, opticalRotation, 0, cv::Size(640,480));
+	}
+	model.save(output);
+
+	CameraThread cameraThread(new
+		CameraRGBDImages(
+				pathRgbImages,
+				pathDepthImages,
+				depthFactor,
+				0.0f,
+				opticalRotation), parameters);
+	((CameraRGBDImages*)cameraThread.camera())->setTimestamps(true, "", false);
+	if(!pathGt.empty())
+	{
+		((CameraRGBDImages*)cameraThread.camera())->setGroundTruthPath(pathGt, 1);
+	}
+
+	bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
+	float detectionRate = Parameters::defaultRtabmapDetectionRate();
+	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
+	Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
+	std::string databasePath = output+"/rtabmap.db";
+	UFile::erase(databasePath);
+	if(cameraThread.camera()->init(output, "rtabmap_calib"))
+	{
+		int totalImages = (int)((CameraRGBDImages*)cameraThread.camera())->filenames().size();
+
+		OdometryF2M odom(parameters);
+		Rtabmap rtabmap;
+		rtabmap.init(parameters, databasePath);
+
+		UTimer totalTime;
+		UTimer timer;
+		CameraInfo cameraInfo;
+		SensorData data = cameraThread.camera()->takeImage(&cameraInfo);
+		int iteration = 0;
+
+		/////////////////////////////
+		// Processing dataset begin
+		/////////////////////////////
+		cv::Mat covariance;
+		double previousStamp = 0.0;
+		while(data.isValid() && g_forever)
+		{
+			std::map<std::string, float> externalStats;
+			cameraThread.postUpdate(&data, &cameraInfo);
+			cameraInfo.timeTotal = timer.ticks();
+
+			// save camera statistics to database
+			externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
+			externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
+			externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
+			externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
+			externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
+			externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
+			externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
+			externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
+
+			OdometryInfo odomInfo;
+			Transform pose = odom.process(data, &odomInfo);
+			externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
+			externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
+			externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers));
+			externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features));
+
+			bool processData = true;
+			if(detectionRate>0.0f &&
+				previousStamp>0.0 &&
+				data.stamp()>previousStamp && data.stamp() - previousStamp < 1.0/detectionRate)
+			{
+				processData = false;
+			}
+
+			if(processData)
+			{
+				previousStamp = data.stamp();
+			}
+
+			if(!processData)
+			{
+				// set negative id so rtabmap will detect it as an intermediate node
+				data.setId(-1);
+				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
+				processData = intermediateNodes;
+			}
+			if(covariance.empty())
+			{
+				covariance = odomInfo.covariance;
+			}
+			else
+			{
+				covariance += odomInfo.covariance;
+			}
+
+			timer.restart();
+			if(processData)
+			{
+				OdometryEvent e(SensorData(), Transform(), odomInfo);
+				rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
+				covariance = cv::Mat();
+			}
+			double slamTime = timer.ticks();
+
+			++iteration;
+			printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms",
+					iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+			if(processData && rtabmap.getLoopClosureId()>0)
+			{
+				printf(" *");
+			}
+			printf("\n");
+
+			cameraInfo = CameraInfo();
+			timer.restart();
+			data = cameraThread.camera()->takeImage(&cameraInfo);
+		}
+		printf("Total time=%fs\n", totalTime.ticks());
+		/////////////////////////////
+		// Processing dataset end
+		/////////////////////////////
+
+		// Save trajectory
+		printf("Saving rtabmap_trajectory.txt ...\n");
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> links;
+		rtabmap.getGraph(poses, links, true, true);
+		std::string pathTrajectory = output+"/rtabmap_poses.txt";
+		if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
+		{
+			printf("Saving %s... done!\n", pathTrajectory.c_str());
+		}
+		else
+		{
+			printf("Saving %s... failed!\n", pathTrajectory.c_str());
+		}
+
+		if(!pathGt.empty())
+		{
+			// Log ground truth statistics (in TUM's RGBD-SLAM format)
+			std::map<int, Transform> groundTruth;
+
+			//align with ground truth for more meaningful results
+			pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
+			cloud1.resize(poses.size());
+			cloud2.resize(poses.size());
+			int oi = 0;
+			int idFirst = 0;
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				Transform o, gtPose;
+				int m,w;
+				std::string l;
+				double s;
+				std::vector<float> v;
+				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, true);
+				if(!gtPose.isNull())
+				{
+					groundTruth.insert(std::make_pair(iter->first, gtPose));
+					if(oi==0)
+					{
+						idFirst = iter->first;
+					}
+					cloud1[oi] = pcl::PointXYZ(gtPose.x(), gtPose.y(), gtPose.z());
+					cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+				}
+			}
+
+			Transform t = Transform::getIdentity();
+			if(oi>5)
+			{
+				cloud1.resize(oi);
+				cloud2.resize(oi);
+
+				t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
+			}
+			else if(idFirst)
+			{
+				t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
+			}
+			if(!t.isIdentity())
+			{
+				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					iter->second = t * iter->second;
+				}
+			}
+
+			std::vector<float> translationalErrors(poses.size());
+			std::vector<float> rotationalErrors(poses.size());
+			float sumTranslationalErrors = 0.0f;
+			float sumRotationalErrors = 0.0f;
+			float sumSqrdTranslationalErrors = 0.0f;
+			float sumSqrdRotationalErrors = 0.0f;
+			float radToDegree = 180.0f / M_PI;
+			float translational_min = 0.0f;
+			float translational_max = 0.0f;
+			float rotational_min = 0.0f;
+			float rotational_max = 0.0f;
+			oi=0;
+			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				std::map<int, Transform>::const_iterator jter = groundTruth.find(iter->first);
+				if(jter!=groundTruth.end())
+				{
+					Eigen::Vector3f vA = iter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+					Eigen::Vector3f vB = jter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+					double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+					rotationalErrors[oi] = a*radToDegree;
+					translationalErrors[oi] = iter->second.getDistance(jter->second);
+
+					sumTranslationalErrors+=translationalErrors[oi];
+					sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
+					sumRotationalErrors+=rotationalErrors[oi];
+					sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
+
+					if(oi == 0)
+					{
+						translational_min = translational_max = translationalErrors[oi];
+						rotational_min = rotational_max = rotationalErrors[oi];
+					}
+					else
+					{
+						if(translationalErrors[oi] < translational_min)
+						{
+							translational_min = translationalErrors[oi];
+						}
+						else if(translationalErrors[oi] > translational_max)
+						{
+							translational_max = translationalErrors[oi];
+						}
+
+						if(rotationalErrors[oi] < rotational_min)
+						{
+							rotational_min = rotationalErrors[oi];
+						}
+						else if(rotationalErrors[oi] > rotational_max)
+						{
+							rotational_max = rotationalErrors[oi];
+						}
+					}
+
+					++oi;
+				}
+			}
+			translationalErrors.resize(oi);
+			rotationalErrors.resize(oi);
+			if(oi)
+			{
+				float total = float(oi);
+				float translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
+				float translational_mean = sumTranslationalErrors/total;
+				float translational_median = translationalErrors[oi/2];
+				float translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
+
+				float rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
+				float rotational_mean = sumRotationalErrors/total;
+				float rotational_median = rotationalErrors[oi/2];
+				float rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
+
+				printf("  translational_rmse=   %f\n", translational_rmse);
+				printf("  rotational_rmse=      %f\n", rotational_rmse);
+
+				FILE * pFile = 0;
+				std::string pathErrors = output+"/rtabmap_rmse.txt";
+				pFile = fopen(pathErrors.c_str(),"w");
+				if(!pFile)
+				{
+					UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
+				}
+				fprintf(pFile, "Ground truth comparison:\n");
+				fprintf(pFile, "  translational_rmse=   %f\n", translational_rmse);
+				fprintf(pFile, "  translational_mean=   %f\n", translational_mean);
+				fprintf(pFile, "  translational_median= %f\n", translational_median);
+				fprintf(pFile, "  translational_std=    %f\n", translational_std);
+				fprintf(pFile, "  translational_min=    %f\n", translational_min);
+				fprintf(pFile, "  translational_max=    %f\n", translational_max);
+				fprintf(pFile, "  rotational_rmse=      %f\n", rotational_rmse);
+				fprintf(pFile, "  rotational_mean=      %f\n", rotational_mean);
+				fprintf(pFile, "  rotational_median=    %f\n", rotational_median);
+				fprintf(pFile, "  rotational_std=       %f\n", rotational_std);
+				fprintf(pFile, "  rotational_min=       %f\n", rotational_min);
+				fprintf(pFile, "  rotational_max=       %f\n", rotational_max);
+				fclose(pFile);
+			}
+		}
+	}
+	else
+	{
+		UERROR("Camera init failed!");
+	}
+
+	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/rtabmap.db").c_str());
+	printf("Do:\n"
+			" $ rtabmap-databaseViewer %s\n\n", (output+"/rtabmap.db").c_str());
+
+	return 0;
+}