Merge branch 'master' of github.com:introlab/rtabmap into noetic-devel

.github/workflows/cmake-ros.yml

@@ -3,7 +3,7 @@ name: CMake-ROS
 on:
   push:
     branches: 
-      - master
+      - noetic-devel
   pull_request:
     branches: 
       - '**'
@@ -23,12 +23,10 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ros_distribution: [ noetic, humble ]
+        ros_distribution: [ noetic ]
         include:
         - ros_distribution: 'noetic'
           os: ubuntu-20.04
-        - ros_distribution: 'humble'
-          os: ubuntu-22.04
 
     steps:   
     - uses: ros-tooling/setup-ros@v0.7

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 21)
-SET(RTABMAP_PATCH_VERSION 9)
+SET(RTABMAP_PATCH_VERSION 10)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/Graph.h

@@ -115,7 +115,8 @@ Transform RTABMAP_CORE_EXPORT calcRMSE(
 		float & rotational_median,
 		float & rotational_std,
 		float & rotational_min,
-		float & rotational_max);
+		float & rotational_max,
+		bool align2D = false);
 
 void RTABMAP_CORE_EXPORT computeMaxGraphErrors(
 		const std::map<int, Transform> & poses,

corelib/src/DBDriver.cpp

@@ -1179,7 +1179,7 @@ void DBDriver::addInfoAfterRun(
 					  << processMemUsed << ","
 					  << databaseMemUsed << ","
 					  << dictionarySize << ","
-					  "\"" << param.c_str() << "\");";
+					  "'" << param.c_str() << "');";
 			}
 			else
 			{
@@ -1189,7 +1189,7 @@ void DBDriver::addInfoAfterRun(
 					  << processMemUsed << ","
 					  << databaseMemUsed << ","
 					  << dictionarySize << ","
-					  "\"" << param.c_str() << "\");";
+					  "'" << param.c_str() << "');";
 			}
 		}
 		else

corelib/src/DBDriverSqlite3.cpp

@@ -2427,9 +2427,13 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 			  << "FROM Node ";
 		if(ignoreChildren)
 		{
-			query << "INNER JOIN Link ";
-			query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
-			query << "WHERE from_id != to_id "; // ignore self referring links
+			// use to_id to ignore all children (which don't have link pointing on them)
+			// ignore self referring links
+			// keep nodes without link to other nodes (map has only a single node)
+			query << "WHERE ";
+			query << "(EXISTS (select 1 from Link where Node.id=to_id and from_id != to_id) OR ";
+			query << " NOT EXISTS (select 1 from Link where id=to_id and from_id != to_id)) ";
+			
 			query << "AND weight>-9 "; //ignore invalid nodes
 			if(ignoreIntermediateNodes)
 			{

corelib/src/Graph.cpp

@@ -783,7 +783,8 @@ Transform calcRMSE (
 		float & rotational_median,
 		float & rotational_std,
 		float & rotational_min,
-		float & rotational_max)
+		float & rotational_max,
+		bool align2D)
 {
 
 	translational_rmse = 0.0f;
@@ -815,8 +816,8 @@ Transform calcRMSE (
 			{
 				idFirst = iter->first;
 			}
-			cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), jter->second.z());
-			cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), align2D?0:jter->second.z());
+			cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), align2D?0:iter->second.z());
 		}
 	}
 

corelib/src/Rtabmap.cpp

@@ -716,6 +716,36 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 
 	if(_memory)
 	{
+		bool isMemIncremental = _memory->isIncremental();
+		if(Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), isMemIncremental) &&
+			isMemIncremental != _memory->isIncremental())
+		{
+			// Mode has changed from Mapping to Localization, cleanup the local graph
+			if(_memory->isGraphReduced() && _memory->isIncremental())
+			{
+				// Force reducing graph, then remove filtered nodes from the optimized poses
+				std::map<int, int> reducedIds;
+				_memory->incrementMapId(&reducedIds);
+				for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
+				{
+					_optimizedPoses.erase(iter->first);
+				}
+			}
+
+			// In both cases, we save the latest optimized graph and latest localization pose
+			_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
+
+			// Mode changed from Localization to Mapping, clear local graph
+			if(!_memory->isIncremental()) {
+				_optimizedPoses.clear();
+				_lastLocalizationPose.setNull();
+				_mapCorrection.setIdentity();
+				_mapCorrectionBackup.setNull();
+				_localizationCovariance = cv::Mat();
+				_lastLocalizationNodeId = 0;
+			}
+		}
+
 		_memory->parseParameters(parameters);
 		if(_memory->isIncremental() && !_globalScanMap.empty())
 		{
@@ -1720,6 +1750,7 @@ bool Rtabmap::process(
 					_constraints.erase(--_constraints.end());
 				}
 			}
+
 			_constraints.insert(std::make_pair(tmp.from(), tmp));
 		}
 		// Localization mode stuff

corelib/src/optimizer/OptimizerG2O.cpp

@@ -1037,6 +1037,12 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		int it = 0;
 		UTimer timer;
 		double lastError = 0.0;
+
+		if (!optimizer.solver()->init()) {
+			UERROR("g2o: Error while initializing solver");
+			return optimizedPoses;
+		}
+
 		if(intermediateGraphes || this->epsilon() > 0.0)
 		{
 			for(int i=0; i<iterations(); ++i)
@@ -1150,7 +1156,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					}
 				}
 
-				it += optimizer.optimize(1);
+				g2o::OptimizationAlgorithm::SolverResult result = optimizer.solver()->solve(i);
+				++it;
 
 				// early stop condition
 				optimizer.computeActiveErrors();
@@ -1163,6 +1170,12 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					return optimizedPoses;
 				}
 
+				if(result == g2o::OptimizationAlgorithm::Fail)
+				{
+					UERROR("g2o: Solver failed, aborting optimization!");
+					return optimizedPoses;
+				}
+
 				double errorDelta = lastError - chi2;
 				if(i>0 && errorDelta < this->epsilon())
 				{

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -209,7 +209,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			UDEBUG("hasGPSPrior=%s", hasGPSPrior?"true":"false");
 			if(isSlam2d())
 			{
-				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:std::numeric_limits<double>::min()));
+				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:1e-9));
 				graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
 				addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1));
 			}
@@ -217,7 +217,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			{
 				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
 						(gtsam::Vector(6) <<
-								(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:std::numeric_limits<double>::min()), // roll, pitch, fixed yaw if there are no priors
+								(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:1e-9), // roll, pitch, fixed yaw if there are no priors
 								(hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
 								).finished());
 				graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
@@ -924,22 +924,42 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			// early stop condition
 			UDEBUG("iteration %d error =%f", i+1, error);
 			double errorDelta = lastError - error;
-			if((isam2_ || i>0) && errorDelta < this->epsilon())
+			if(this->epsilon() > 0.0 && fabs(error) > 1000000000000.0)
 			{
-				if(errorDelta < 0)
-				{
-					UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
-				}
-				else
-				{
-					UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
-					break;
-				}
+				UWARN("Error computed (%e) is very huge and/or diverging! Aborting! "
+					   "Set %s to 0 to ignore that check and keep iterating up to %s (%d).",
+					   error,
+					   Parameters::kOptimizerEpsilon().c_str(),
+					   Parameters::kOptimizerIterations().c_str(),
+					   this->iterations());
+				return optimizedPoses;
 			}
-			else if(i==0 && error < this->epsilon())
+			else
 			{
-				UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
-				break;
+				if((isam2_ || i>0) && errorDelta < this->epsilon())
+				{
+					if(errorDelta < 0)
+					{
+						UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
+					}
+					else
+					{
+						UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+						break;
+					}
+				}
+				else if(i==0)
+				{ 
+					if(error < 0)
+					{
+						UDEBUG("Negative error?! Ignore and continue optimizing... (%f)", error);
+					}
+					else if(error < this->epsilon())
+					{
+						UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
+						break;
+					}
+				}
 			}
 			lastError = error;
 		}

corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h

@@ -31,8 +31,13 @@
 #ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_
 #define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_
 
-#include "ceres/autodiff_manifold.h"
-#include "ceres/manifold.h"
+#if CERES_VERSION_MAJOR >= 3 || \
+    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
+#include <ceres/autodiff_manifold.h>
+#include <ceres/manifold.h>
+#else
+#include <ceres/local_parameterization.h>
+#endif
 #include "normalize_angle.h"
 
 namespace ceres {

corelib/src/util3d_filtering.cpp

@@ -694,7 +694,7 @@ typename pcl::PointCloud<PointT>::Ptr voxelizeImpl(
 
 		if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
 		{
-			UWARN("Leaf size is too small for the input dataset. Integer indices would overflow. "
+			UDEBUG("Leaf size is too small for the input dataset. Integer indices would overflow. "
 				  "We will split space to be able to voxelize (lvl=%d cloud=%d min=[%f %f %f] max=[%f %f %f] voxel=%f).",
 				  level,
 				  (int)(indices->empty()?cloud->size():indices->size()),

guilib/src/DatabaseViewer.cpp

@@ -2502,115 +2502,9 @@ void DatabaseViewer::exportPoses(int format)
 		return;
 	}
 
-	if(format == 5)
+	if(format == 5 && (gpsValues_.empty() || gpsPoses_.empty()))
 	{
-		if(gpsValues_.empty() || gpsPoses_.empty())
-		{
-			QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
-		}
-		else
-		{
-			std::map<int, rtabmap::Transform> graph;
-			if(groundTruth)
-			{
-				graph = groundTruthPoses_;
-			}
-			else if(odometry)
-			{
-				graph = odomPoses_;
-			}
-			else
-			{
-				graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
-			}
-
-
-			//align with ground truth for more meaningful results
-			pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
-			cloud1.resize(graph.size());
-			cloud2.resize(graph.size());
-			int oi = 0;
-			int idFirst = 0;
-			for(std::map<int, Transform>::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter)
-			{
-				std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
-				if(iter2!=graph.end())
-				{
-					if(oi==0)
-					{
-						idFirst = iter->first;
-					}
-					cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-					cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
-				}
-			}
-
-			Transform t = Transform::getIdentity();
-			if(oi>5)
-			{
-				cloud1.resize(oi);
-				cloud2.resize(oi);
-
-				t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
-			}
-			else if(idFirst)
-			{
-				t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse();
-			}
-
-			std::map<int, GPS> values;
-			GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
-			for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
-			{
-				iter->second = t * iter->second;
-
-				GeodeticCoords coord;
-				coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
-				double bearing = -(iter->second.theta()*180.0/M_PI-90.0);
-				if(bearing < 0)
-				{
-					bearing += 360;
-				}
-
-				Transform p, g;
-				int w;
-				std::string l;
-				double stamp=0.0;
-				int mapId;
-				std::vector<float> v;
-				GPS gps;
-				EnvSensors sensors;
-				dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
-				values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
-			}
-
-			QString output = pathDatabase_ + QDir::separator() + "poses.kml";
-			QString path = QFileDialog::getSaveFileName(
-					this,
-					tr("Save File"),
-					output,
-					tr("Google Earth file (*.kml)"));
-
-			if(!path.isEmpty())
-			{
-				bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
-
-				if(saved)
-				{
-					QMessageBox::information(this,
-							tr("Export poses..."),
-							tr("GPS coordinates saved to \"%1\".")
-							.arg(path));
-				}
-				else
-				{
-					QMessageBox::information(this,
-							tr("Export poses..."),
-							tr("Failed to save GPS coordinates to \"%1\"!")
-							.arg(path));
-				}
-			}
-		}
+		QMessageBox::warning(this, tr("Cannot export poses in KML format"), tr("No GPS in database?!"));
 		return;
 	}
 
@@ -2619,70 +2513,132 @@ void DatabaseViewer::exportPoses(int format)
 	{
 		optimizedPoses = groundTruthPoses_;
 	}
+	else if(odometry)
+	{
+		optimizedPoses = odomPoses_;
+	}
 	else
 	{
-		if(odometry)
+		optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+	}
+
+	bool alignToGPS = 
+			(ui_->checkBox_alignPosesWithGPS->isEnabled() && 
+		     ui_->checkBox_alignPosesWithGPS->isChecked()) ||
+		    format == 5;
+
+	if(alignToGPS ||
+	   (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
+	{
+		std::map<int, Transform> refPoses = groundTruthPoses_;
+		if(alignToGPS)
 		{
-			optimizedPoses = odomPoses_;
-		}
-		else
-		{
-			optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+			refPoses = gpsPoses_;
 		}
 
-		if((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) ||
-		   (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
+		// Log ground truth statistics (in TUM's RGBD-SLAM format)
+		if(refPoses.size())
 		{
-			std::map<int, Transform> refPoses = groundTruthPoses_;
-			if(ui_->checkBox_alignPosesWithGPS->isEnabled() && 
-			   ui_->checkBox_alignPosesWithGPS->isChecked())
+			float translational_rmse = 0.0f;
+			float translational_mean = 0.0f;
+			float translational_median = 0.0f;
+			float translational_std = 0.0f;
+			float translational_min = 0.0f;
+			float translational_max = 0.0f;
+			float rotational_rmse = 0.0f;
+			float rotational_mean = 0.0f;
+			float rotational_median = 0.0f;
+			float rotational_std = 0.0f;
+			float rotational_min = 0.0f;
+			float rotational_max = 0.0f;
+
+			Transform gtToMap = graph::calcRMSE(
+					refPoses,
+					optimizedPoses,
+					translational_rmse,
+					translational_mean,
+					translational_median,
+					translational_std,
+					translational_min,
+					translational_max,
+					rotational_rmse,
+					rotational_mean,
+					rotational_median,
+					rotational_std,
+					rotational_min,
+					rotational_max,
+					alignToGPS);
+
+			if(!gtToMap.isIdentity())
 			{
-				refPoses = gpsPoses_;
-			}
-
-			// Log ground truth statistics (in TUM's RGBD-SLAM format)
-			if(refPoses.size())
-			{
-				float translational_rmse = 0.0f;
-				float translational_mean = 0.0f;
-				float translational_median = 0.0f;
-				float translational_std = 0.0f;
-				float translational_min = 0.0f;
-				float translational_max = 0.0f;
-				float rotational_rmse = 0.0f;
-				float rotational_mean = 0.0f;
-				float rotational_median = 0.0f;
-				float rotational_std = 0.0f;
-				float rotational_min = 0.0f;
-				float rotational_max = 0.0f;
-
-				Transform gtToMap = graph::calcRMSE(
-						refPoses,
-						optimizedPoses,
-						translational_rmse,
-						translational_mean,
-						translational_median,
-						translational_std,
-						translational_min,
-						translational_max,
-						rotational_rmse,
-						rotational_mean,
-						rotational_median,
-						rotational_std,
-						rotational_min,
-						rotational_max);
-
-				if(!gtToMap.isIdentity())
+				for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
 				{
+					iter->second = gtToMap * iter->second;
+				}
+				if(alignToGPS && format != 5 && optimizedPoses.find(gpsValues_.begin()->first)!=optimizedPoses.end())
+				{
+					// This will make the exported first pose the GPS origin. Don't do it for KML format as is it done implicitly below.
+					int originId = gpsValues_.begin()->first;
+					Transform offset = optimizedPoses.at(originId).translation().inverse();
 					for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
 					{
-						iter->second = gtToMap * iter->second;
+						iter->second = offset * iter->second;
 					}
 				}
 			}
 		}
 	}
 
+	if(format == 5)
+	{
+		std::map<int, GPS> values;
+		GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
+		for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+		{
+			GeodeticCoords coord;
+			coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
+
+			Transform p, g;
+			int w;
+			std::string l;
+			double stamp=0.0;
+			int mapId;
+			std::vector<float> v;
+			GPS gps;
+			EnvSensors sensors;
+			dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
+			values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
+		}
+
+		QString output = pathDatabase_ + QDir::separator() + "poses.kml";
+		QString path = QFileDialog::getSaveFileName(
+				this,
+				tr("Save File"),
+				output,
+				tr("Google Earth file (*.kml)"));
+
+		if(!path.isEmpty())
+		{
+			bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
+
+			if(saved)
+			{
+				QMessageBox::information(this,
+						tr("Export poses..."),
+						tr("GPS coordinates saved to \"%1\".")
+						.arg(path));
+			}
+			else
+			{
+				QMessageBox::information(this,
+						tr("Export poses..."),
+						tr("Failed to save GPS coordinates to \"%1\"!")
+						.arg(path));
+			}
+		}
+		return;
+	}
+
 	if(optimizedPoses.size())
 	{
 		std::map<int, Transform> localTransforms;
@@ -4174,6 +4130,72 @@ void DatabaseViewer::generate3DMap()
 	else
 	{
 		optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+
+		bool alignToGPS = 
+			ui_->checkBox_alignPosesWithGPS->isEnabled() && 
+		    ui_->checkBox_alignPosesWithGPS->isChecked();
+
+		if(alignToGPS ||
+		   (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
+		{
+			std::map<int, Transform> refPoses = groundTruthPoses_;
+			if(alignToGPS)
+			{
+				refPoses = gpsPoses_;
+			}
+
+			// Log ground truth statistics (in TUM's RGBD-SLAM format)
+			if(refPoses.size())
+			{
+				float translational_rmse = 0.0f;
+				float translational_mean = 0.0f;
+				float translational_median = 0.0f;
+				float translational_std = 0.0f;
+				float translational_min = 0.0f;
+				float translational_max = 0.0f;
+				float rotational_rmse = 0.0f;
+				float rotational_mean = 0.0f;
+				float rotational_median = 0.0f;
+				float rotational_std = 0.0f;
+				float rotational_min = 0.0f;
+				float rotational_max = 0.0f;
+
+				Transform gtToMap = graph::calcRMSE(
+						refPoses,
+						optimizedPoses,
+						translational_rmse,
+						translational_mean,
+						translational_median,
+						translational_std,
+						translational_min,
+						translational_max,
+						rotational_rmse,
+						rotational_mean,
+						rotational_median,
+						rotational_std,
+						rotational_min,
+						rotational_max,
+						alignToGPS);
+
+				if(!gtToMap.isIdentity())
+				{
+					for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+					{
+						iter->second = gtToMap * iter->second;
+					}
+					if(alignToGPS && optimizedPoses.find(gpsValues_.begin()->first)!=optimizedPoses.end())
+					{
+						// This will make the exported first pose the GPS origin.
+						int originId = gpsValues_.begin()->first;
+						Transform offset = optimizedPoses.at(originId).translation().inverse();
+						for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+						{
+							iter->second = offset * iter->second;
+						}
+					}
+				}
+			}
+		}
 	}
 	if(ui_->groupBox_posefiltering->isChecked())
 	{
@@ -6954,7 +6976,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 		std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, value);
 
 		std::map<int, Transform> refPoses = groundTruthPoses_;
-		if(ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked())
+		bool alignToGPS = ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked();
+		if(alignToGPS)
 		{
 			refPoses = gpsPoses_;
 		}
@@ -7000,7 +7023,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					rotational_median,
 					rotational_std,
 					rotational_min,
-					rotational_max);
+					rotational_max,
+					alignToGPS);
 
 			// ground truth live statistics
 			ui_->label_rmse->setNum(translational_rmse);
@@ -7018,7 +7042,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			UINFO("rotational_min=%f", rotational_min);
 			UINFO("rotational_max=%f", rotational_max);
 
-			if(((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) ||
+			if((alignToGPS ||
 				(ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) &&
 			   !gtToMap.isIdentity())
 			{

guilib/src/PreferencesDialog.cpp

@@ -4892,10 +4892,13 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 				{
 					if(valueInt==2 && combo->objectName().toStdString().compare(Parameters::kOptimizerStrategy()) == 0)
 					{
+					if(
 #ifndef RTABMAP_ORB_SLAM
-						if(Optimizer::isAvailable(Optimizer::kTypeG2O))
+						Optimizer::isAvailable(Optimizer::kTypeG2O)
+#else
+						true
 #endif
-						{
+						){
 							UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built "
 								"with GTSAM. Falling back to g2o.",
 								combo->objectName().toStdString().c_str());

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>rtabmap</name>
-  <version>0.21.9</version>
+  <version>0.21.10</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>