DbViewer: when align with ground truth is enabled, exported poses are those aligned with ground truth

corelib/src/OdometryOkvis.cpp

@@ -69,6 +69,7 @@ public:
 
 	Transform getLastTransform()
 	{
+		UDEBUG("");
 		Transform tf;
 		mutex_.lock();
 		tf = transform_;
@@ -79,6 +80,7 @@ public:
 
 	std::map<int, cv::Point3f> getLastLandmarks()
 	{
+		UDEBUG("");
 		std::map<int, cv::Point3f> landmarks;
 		mutexLandmarks_.lock();
 		landmarks = landmarks_;
@@ -104,22 +106,14 @@ public:
 			const okvis::MapPointVector & landmarksVector,
 			const okvis::MapPointVector & /*transferredLandmarks*/)
 	{
-		bool notify = true;
+		UDEBUG("");
 		mutexLandmarks_.lock();
-		if(landmarks_.size())
-		{
-			notify = false;
-		}
 		landmarks_.clear();
 		for(unsigned int i=0; i<landmarksVector.size(); ++i)
 		{
 			landmarks_.insert(std::make_pair((int)landmarksVector[i].id, cv::Point3f(landmarksVector[i].point[0], landmarksVector[i].point[1], landmarksVector[i].point[2])));
 		}
 		mutexLandmarks_.unlock();
-		if(notify)
-		{
-			semLandmarks_.release();
-		}
 	}
 
 
@@ -129,8 +123,6 @@ private:
 	std::map<int, cv::Point3f> landmarks_;
 	UMutex mutex_;
 	UMutex mutexLandmarks_;
-	USemaphore semTf_;
-	USemaphore semLandmarks_;
 };
 #endif
 
@@ -156,6 +148,7 @@ OdometryOkvis::OdometryOkvis(const ParametersMap & parameters) :
 
 OdometryOkvis::~OdometryOkvis()
 {
+	UDEBUG("");
 #ifdef RTABMAP_OKVIS
 	if(okvisEstimator_)
 	{

guilib/src/DatabaseViewer.cpp

@@ -2051,6 +2051,56 @@ void DatabaseViewer::exportPoses(int format)
 	else
 	{
 		optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+
+		if(ui_->checkBox_alignPosesWithGroundTruth->isChecked())
+		{
+			std::map<int, Transform> refPoses = groundTruthPoses_;
+			if(refPoses.empty())
+			{
+				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(ui_->checkBox_alignPosesWithGroundTruth->isChecked() && !gtToMap.isIdentity())
+				{
+					for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+					{
+						iter->second = gtToMap * iter->second;
+					}
+				}
+			}
+		}
 	}
 
 	if(optimizedPoses.size())