From 4911dbe9bbfcdaebc1a60bc2a72c5cdc5f614bd6 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Mon, 12 Aug 2024 14:27:08 -0700
Subject: [PATCH] With Mem/RotateImagesUpsideUp=true, do not clear features if
 none of the cameras are rotated. DBViewer: show local transform of all
 cameras in tooltip

---
 corelib/include/rtabmap/core/util2d.h | 3 ++-
 corelib/src/Memory.cpp                | 5 +++--
 corelib/src/util2d.cpp                | 6 ++++--
 guilib/src/DatabaseViewer.cpp         | 2 ++
 4 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/corelib/include/rtabmap/core/util2d.h b/corelib/include/rtabmap/core/util2d.h
index 602768e20c..edec1ed9b3 100644
--- a/corelib/include/rtabmap/core/util2d.h
+++ b/corelib/include/rtabmap/core/util2d.h
@@ -180,8 +180,9 @@ std::vector<int> RTABMAP_CORE_EXPORT SSC(
  * @param model a valid camera model
  * @param rgb a rgb/grayscale image (set cv::Mat() if not used)
  * @param depth a depth image (set cv::Mat() if not used)
+ * @return true if the model/images have been rotated, false otherwise
  */
-void RTABMAP_CORE_EXPORT rotateImagesUpsideUpIfNecessary(
+bool RTABMAP_CORE_EXPORT rotateImagesUpsideUpIfNecessary(
 	CameraModel & model,
 	cv::Mat & rgb,
 	cv::Mat & depth);
diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp
index 92a014c97f..ef3a555525 100644
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -4717,13 +4717,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		cv::Mat rotatedDepthImages;
 		std::vector<CameraModel> rotatedCameraModels;
 		bool allOutputSizesAreOkay = true;
+		bool atLeastOneCameraRotated = false;
 		for(size_t i=0; i<data.cameraModels().size(); ++i)
 		{
 			UDEBUG("Rotating camera %ld", i);
 			cv::Mat rgb = cv::Mat(data.imageRaw(), cv::Rect(subInputImageWidth*i, 0, subInputImageWidth, data.imageRaw().rows));
 			cv::Mat depth = !data.depthRaw().empty()?cv::Mat(data.depthRaw(), cv::Rect(subInputDepthWidth*i, 0, subInputDepthWidth, data.depthRaw().rows)):cv::Mat();
 			CameraModel model = data.cameraModels()[i];
-			util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
+			atLeastOneCameraRotated |= util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
 			if(rotatedColorImages.empty())
 			{
 				rotatedColorImages = cv::Mat(cv::Size(rgb.cols * data.cameraModels().size(), rgb.rows), rgb.type());
@@ -4757,7 +4758,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}
 			rotatedCameraModels.push_back(model);
 		}
-		if(allOutputSizesAreOkay)
+		if(allOutputSizesAreOkay && atLeastOneCameraRotated)
 		{
 			data.setRGBDImage(rotatedColorImages, rotatedDepthImages, rotatedCameraModels);
 
diff --git a/corelib/src/util2d.cpp b/corelib/src/util2d.cpp
index b0ce436252..8e91b8fbac 100644
--- a/corelib/src/util2d.cpp
+++ b/corelib/src/util2d.cpp
@@ -2279,7 +2279,7 @@ std::vector<int> SSC(
 	return ResultVec;
 }
 
-void rotateImagesUpsideUpIfNecessary(
+bool rotateImagesUpsideUpIfNecessary(
 	CameraModel & model,
 	cv::Mat & rgb,
 	cv::Mat & depth)
@@ -2293,7 +2293,7 @@ void rotateImagesUpsideUpIfNecessary(
 	{
 		// Return original because of ambiguity for what would be considered up...
 		UDEBUG("Ignoring image rotation as pitch(%f)>Pi/4", pitch);
-		return;
+		return false;
 	}
 	if(roll<0)
 	{
@@ -2368,7 +2368,9 @@ void rotateImagesUpsideUpIfNecessary(
 	else
 	{
 		UDEBUG("ROTATION_0 (roll=%f)", roll);
+		return false;
 	}
+	return true;
 }
 
 }
diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp
index 12129a748f..cb182c26b5 100644
--- a/guilib/src/DatabaseViewer.cpp
+++ b/guilib/src/DatabaseViewer.cpp
@@ -4952,6 +4952,7 @@ void DatabaseViewer::update(int value,
 							if( data.cameraModels()[i].D_raw().total()) calibrationDetails << "D=" << data.cameraModels()[i].D_raw() << std::endl;
 							if( data.cameraModels()[i].R().total()) calibrationDetails << "R=" << data.cameraModels()[i].R() << std::endl;
 							if( data.cameraModels()[i].P().total()) calibrationDetails << "P=" << data.cameraModels()[i].P() << std::endl;
+							calibrationDetails << "BaseToCam(without opt rot)=" << (data.cameraModels()[i].localTransform()*CameraModel::opticalRotation().inverse()).prettyPrint() << std::endl;
 						}
 
 					}
@@ -4988,6 +4989,7 @@ void DatabaseViewer::update(int value,
 							if( data.stereoCameraModels()[i].T().total()) calibrationDetails << " T=" << data.stereoCameraModels()[i].T() << std::endl;
 							if( data.stereoCameraModels()[i].F().total()) calibrationDetails << " F=" << data.stereoCameraModels()[i].F() << std::endl;
 							if( data.stereoCameraModels()[i].E().total()) calibrationDetails << " E=" << data.stereoCameraModels()[i].E() << std::endl;
+							calibrationDetails << "BaseToLeftCam(without opt rot)=" << (data.stereoCameraModels()[i].left().localTransform()*CameraModel::opticalRotation().inverse()).prettyPrint() << std::endl;
 						}
 					}
 					labelCalib->setToolTip(calibrationDetails.str().c_str());