Added parameter Mem/RotateImagesUpsideUp

corelib/include/rtabmap/core/Memory.h

@@ -330,6 +330,7 @@ class RTABMAP_CORE_EXPORT Memory
 	bool _rehearsalWeightIgnoredWhileMoving;
 	bool _useOdometryFeatures;
 	bool _useOdometryGravity;
+	bool _rotateImagesUpsideUp;
 	bool _createOccupancyGrid;
 	int _visMaxFeatures;
 	bool _imagesAlreadyRectified;

corelib/include/rtabmap/core/Parameters.h

@@ -231,6 +231,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, true,     "Use odometry features instead of regenerating them.");
     RTABMAP_PARAM(Mem, UseOdomGravity,              bool, false,    uFormat("Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if \"%s\" is not zero.", kOptimizerGravitySigma().c_str()));
     RTABMAP_PARAM(Mem, CovOffDiagIgnored,           bool, true,     "Ignore off diagonal values of the covariance matrix.");
+    RTABMAP_PARAM(Mem, RotateImagesUpsideUp,        bool, false,    "Rotate images so that upside is up if they are not already. This can be useful in case the robots don't have all same camera orientation but are using the same map, so that not rotation-invariant visual features can still be used across the fleet.");
 
     // KeypointMemory (Keypoint-based)
     RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");

corelib/include/rtabmap/core/util2d.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/core.hpp>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/CameraModel.h>
 #include <vector>
 
 namespace rtabmap
@@ -163,6 +164,25 @@ void RTABMAP_CORE_EXPORT NMS(
 		cv::Mat & descriptorsOut,
 		int border, int dist_thresh, int img_width, int img_height);
 
+/**
+ * @brief Rotate images and camera model so that the top of the image is up.
+ *
+ * The roll value of local transform of the camera model is used to estimate
+ * if the images have to be rotated. If there is a pitch value higher than
+ * 45 deg, the original images and camera model will be returned (no rotation will happen).
+ * The return local transform of the camera model is updated accordingly. The distortion
+ * model is ignored and won't be transfered to modified camera model, so this function
+ * expects already rectified images.
+ *
+ * @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)
+ */
+void RTABMAP_CORE_EXPORT rotateImagesUpsideUpIfNecessary(
+	CameraModel & model,
+	cv::Mat & rgb,
+	cv::Mat & depth);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/src/Memory.cpp

@@ -107,6 +107,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
 	_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
 	_useOdometryGravity(Parameters::defaultMemUseOdomGravity()),
+	_rotateImagesUpsideUp(Parameters::defaultMemRotateImagesUpsideUp()),
 	_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
 	_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
 	_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
@@ -597,6 +598,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
 	Parameters::parse(params, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
 	Parameters::parse(params, Parameters::kMemUseOdomGravity(), _useOdometryGravity);
+	Parameters::parse(params, Parameters::kMemRotateImagesUpsideUp(), _rotateImagesUpsideUp);
 	Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
 	Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
 	Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
@@ -4667,6 +4669,96 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		preUpdateThread.start();
 	}
 
+	if(_rotateImagesUpsideUp && !data.imageRaw().empty() && !data.cameraModels().empty())
+	{
+		// Currently stereo is not supported
+		UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
+		int subInputImageWidth = data.imageRaw().cols/data.cameraModels().size();
+		int subInputDepthWidth = data.depthRaw().cols/data.cameraModels().size();
+		int subOutputImageWidth = 0;
+		int subOutputDepthWidth = 0;
+		cv::Mat rotatedColorImages;
+		cv::Mat rotatedDepthImages;
+		std::vector<CameraModel> rotatedCameraModels;
+		bool allOutputSizesAreOkay = true;
+		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);
+			if(rotatedColorImages.empty())
+			{
+				rotatedColorImages = cv::Mat(cv::Size(rgb.cols * data.cameraModels().size(), rgb.rows), rgb.type());
+				subOutputImageWidth = rgb.cols;;
+				if(!depth.empty())
+				{
+					rotatedDepthImages = cv::Mat(cv::Size(depth.cols * data.cameraModels().size(), depth.rows), depth.type());
+					subOutputDepthWidth = depth.cols;
+				}
+			}
+			else if(rgb.cols != subOutputImageWidth || depth.cols != subOutputDepthWidth ||
+					rgb.rows != rotatedColorImages.rows || depth.rows != rotatedDepthImages.rows)
+			{
+				UWARN("Rotated image for camera index %d (rgb=%dx%d depth=%dx%d) doesn't tally "
+				      "with the first camera (rgb=%dx%d, depth=%dx%d). Aborting upside up rotation, "
+					  "will use original image orientation. Set parameter %s to false to avoid "
+					  "this warning.",
+						i,
+						rgb.cols, rgb.rows,
+						depth.cols, depth.rows,
+						subOutputImageWidth, rotatedColorImages.rows,
+						subOutputDepthWidth, rotatedDepthImages.rows,
+						Parameters::kMemRotateImagesUpsideUp().c_str());
+				allOutputSizesAreOkay = false;
+				break;
+			}
+			rgb.copyTo(cv::Mat(rotatedColorImages, cv::Rect(subOutputImageWidth*i, 0, subOutputImageWidth, rgb.rows)));
+			if(!depth.empty())
+			{
+				depth.copyTo(cv::Mat(rotatedDepthImages, cv::Rect(subOutputDepthWidth*i, 0, subOutputDepthWidth, depth.rows)));
+			}
+			rotatedCameraModels.push_back(model);
+		}
+		if(allOutputSizesAreOkay)
+		{
+			data.setRGBDImage(rotatedColorImages, rotatedDepthImages, rotatedCameraModels);
+
+			// Clear any features to avoid confusion with the rotated cameras.
+			if(!data.keypoints().empty() || !data.keypoints3D().empty() || !data.descriptors().empty())
+			{
+				if(_useOdometryFeatures)
+				{
+					static bool warned = false;
+					if(!warned)
+					{
+						UWARN("Because parameter %s is enabled, parameter %s is inhibited as "
+							"features have to be regenerated. To avoid this warning, set "
+							"explicitly %s to false. This message is only "
+							"printed once.",
+							Parameters::kMemRotateImagesUpsideUp().c_str(),
+							Parameters::kMemUseOdomFeatures().c_str(),
+							Parameters::kMemUseOdomFeatures().c_str());
+						warned = true;
+					}
+				}
+				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
+			}
+		}
+	}
+	else if(_rotateImagesUpsideUp)
+	{
+		static bool warned = false;
+		if(!warned)
+		{
+			UWARN("Parameter %s can only be used with RGB-only or RGB-D cameras. "
+			      "Ignoring upside up rotation. This message is only printed once.",
+				  Parameters::kMemRotateImagesUpsideUp().c_str());
+			warned = true;
+		}
+	}
+
 	unsigned int preDecimation = 1;
 	std::vector<cv::Point3f> keypoints3D;
 	SensorData decimatedData;

corelib/src/util2d.cpp

@@ -2202,6 +2202,98 @@ void NMS(
     }
 }
 
+void rotateImagesUpsideUpIfNecessary(
+	CameraModel & model,
+	cv::Mat & rgb,
+	cv::Mat & depth)
+{
+	float roll,pitch,yaw;
+	// remove optical rotation
+	Transform localTransform = model.localTransform()*CameraModel::opticalRotation().inverse();
+	localTransform.getEulerAngles(roll, pitch, yaw);
+	UDEBUG("roll=%f pitch=%f yaw=%f", roll, pitch, yaw);
+	if(fabs(pitch > M_PI/4))
+	{
+		// Return original because of ambiguity for what would be considered up...
+		UDEBUG("Ignoring image rotation as pitch(%f)>Pi/4", pitch);
+		return;
+	}
+	if(roll<0)
+	{
+		roll+=2*M_PI;
+	}
+	if(roll >= M_PI/4 && roll < 3*M_PI/4)
+	{
+		UDEBUG("ROTATION_90 (roll=%f)", roll);
+		if(!rgb.empty())
+		{
+			cv::flip(rgb,rgb,1);
+			cv::transpose(rgb,rgb);
+		}
+		if(!depth.empty())
+		{
+			cv::flip(depth,depth,1);
+			cv::transpose(depth,depth);
+		}
+		cv::Size sizet(model.imageHeight(), model.imageWidth());
+		model = CameraModel(
+				model.fy(),
+				model.fx(),
+				model.cy(),
+				model.cx()>0?model.imageWidth()-model.cx():0,
+				model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
+		model.setImageSize(sizet);
+	}
+	else if(roll >= 3*M_PI/4 && roll < 5*M_PI/4)
+	{
+		UDEBUG("ROTATION_180 (roll=%f)", roll);
+		if(!rgb.empty())
+		{
+			cv::flip(rgb,rgb,1);
+			cv::flip(rgb,rgb,0);
+		}
+		if(!depth.empty())
+		{
+			cv::flip(depth,depth,1);
+			cv::flip(depth,depth,0);
+		}
+		cv::Size sizet(model.imageWidth(), model.imageHeight());
+		model = CameraModel(
+				model.fx(),
+				model.fy(),
+				model.cx()>0?model.imageWidth()-model.cx():0,
+				model.cy()>0?model.imageHeight()-model.cy():0,
+				model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
+		model.setImageSize(sizet);
+	}
+	else if(roll >= 5*M_PI/4 && roll < 7*M_PI/4)
+	{
+		UDEBUG("ROTATION_270 (roll=%f)", roll);
+		if(!rgb.empty())
+		{
+			cv::transpose(rgb,rgb);
+			cv::flip(rgb,rgb,1);
+		}
+		if(!depth.empty())
+		{
+			cv::transpose(depth,depth);
+			cv::flip(depth,depth,1);
+		}
+		cv::Size sizet(model.imageHeight(), model.imageWidth());
+		model = CameraModel(
+				model.fy(),
+				model.fx(),
+				model.cy()>0?model.imageHeight()-model.cy():0,
+				model.cx(),
+				model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
+		model.setImageSize(sizet);
+	}
+	else
+	{
+		UDEBUG("ROTATION_0 (roll=%f)", roll);
+	}
+}
+
 }
 
 }

guilib/src/PreferencesDialog.cpp

@@ -954,6 +954,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_doubleSpinBox_laserScanNormalRadius->setObjectName(Parameters::kMemLaserScanNormalRadius().c_str());
 	_ui->checkBox_useOdomFeatures->setObjectName(Parameters::kMemUseOdomFeatures().c_str());
 	_ui->memCovOffDiagIgnored->setObjectName(Parameters::kMemCovOffDiagIgnored().c_str());
+	_ui->general_checkBox_rotateImagesUpsideUp->setObjectName(Parameters::kMemRotateImagesUpsideUp().c_str());
 
 	// Database
 	_ui->checkBox_dbInMemory->setObjectName(Parameters::kDbSqlite3InMemory().c_str());