Refactoring: set default local transform for camera drivers to opticalRotation. Added CameraModel::opticalRtotation() static function for convenience. Added CameraModel::load() from file directly for convenience. Added Camera::initFromFile() for convenience. Transform: Added opengl_T_rtabmap() and opengl_T_rtabmap() functions for convenience (convert back and forth between rtabmap world and opengl world coordinate frames)

corelib/include/rtabmap/core/Camera.h

@@ -53,6 +53,7 @@ public:
 	virtual ~Camera();
 	SensorData takeImage(CameraInfo * info = 0);
 
+	bool initFromFile(const std::string & calibrationPath);
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
@@ -73,7 +74,7 @@ protected:
 	 *
 	 * @param imageRate : image/second , 0 for fast as the camera can
 	 */
-	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
+	Camera(float imageRate = 0, const Transform & localTransform = CameraModel::opticalRotation());
 
 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded

corelib/include/rtabmap/core/CameraModel.h

@@ -37,6 +37,13 @@ namespace rtabmap {
 
 class RTABMAP_EXP CameraModel
 {
+public:
+	/**
+	 * Optical rotation used to transform image coordinate frame (x->right, y->down, z->forward)
+	 * to robot coordinate frame (x->forward, y->left, z->up).
+	 */
+	static Transform opticalRotation() {return Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);}
+
 public:
 	CameraModel();
 	// K is the camera intrinsic 3x3 CV_64FC1
@@ -50,7 +57,7 @@ public:
 			const cv::Mat & D,
 			const cv::Mat & R,
 			const cv::Mat & P,
-			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+			const Transform & localTransform = opticalRotation());
 
 	// minimal
 	CameraModel(
@@ -58,7 +65,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+			const Transform & localTransform = opticalRotation(),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 	// minimal to be saved
@@ -68,7 +75,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+			const Transform & localTransform = opticalRotation(),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 
@@ -113,9 +120,12 @@ public:
 	int imageWidth() const {return imageSize_.width;}
 	int imageHeight() const {return imageSize_.height;}
 
-	double fovX() const {return imageSize_.width>0 && fx()>0?2.0*atan(imageSize_.width/(fx()*2.0)):0.0;}
-	double fovY() const {return imageSize_.height>0 && fy()>0?2.0*atan(imageSize_.height/(fy()*2.0)):0.0;}
+	double fovX() const; // in radians
+	double fovY() const; // in radians
+	double horizontalFOV() const; // in degrees
+	double verticalFOV() const;   // in degrees
 
+	bool load(const std::string & filePath);
 	bool load(const std::string & directory, const std::string & cameraName);
 	bool save(const std::string & directory) const;
 	std::vector<unsigned char> serialize() const;
@@ -125,9 +135,6 @@ public:
 	CameraModel scaled(double scale) const;
 	CameraModel roi(const cv::Rect & roi) const;
 
-	double horizontalFOV() const; // in degrees
-	double verticalFOV() const;   // in degrees
-
 	// For depth images, your should use cv::INTER_NEAREST
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
 	cv::Mat rectifyDepth(const cv::Mat & raw) const;

corelib/include/rtabmap/core/Transform.h

@@ -140,6 +140,15 @@ public:
 	static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
 	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
 
+	static Transform opengl_T_rtabmap() {return Transform(
+			 0.0f, -1.0f, 0.0f, 0.0f,
+			 0.0f,  0.0f, 1.0f, 0.0f,
+			-1.0f,  0.0f, 0.0f, 0.0f);}
+	static Transform rtabmap_T_opengl() {return Transform(
+			 0.0f,  0.0f,-1.0f, 0.0f,
+			-1.0f,  0.0f, 0.0f, 0.0f,
+			 0.0f,  1.0f, 0.0f, 0.0f);}
+
 	/**
 	 * Format (3 values): x y z
 	 * Format (6 values): x y z roll pitch yaw

corelib/include/rtabmap/core/camera/CameraFreenect.h

@@ -53,7 +53,7 @@ public:
 	CameraFreenect(int deviceId= 0,
 					Type type = kTypeColorDepth,
 					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity());
+					const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraFreenect();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraFreenect2.h

@@ -65,7 +65,7 @@ public:
 	CameraFreenect2(int deviceId= 0,
 					Type type = kTypeDepth2ColorSD,
 					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity(),
+					const Transform & localTransform = CameraModel::opticalRotation(),
 					float minDepth = 0.3f,
 					float maxDepth = 12.0f,
 					bool bilateralFiltering = true,

corelib/include/rtabmap/core/camera/CameraImages.h

@@ -46,7 +46,7 @@ public:
 	CameraImages(
 			const std::string & path,
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraImages();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraK4A.h

@@ -50,10 +50,10 @@ public:
 public:
 	CameraK4A(int deviceId = 0,
 		float imageRate = 0.0f,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	CameraK4A(const std::string & fileName,
 		float imageRate = 0.0f,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraK4A();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraK4W2.h

@@ -66,7 +66,7 @@ public:
 	CameraK4W2(int deviceId = 0, // not used
 		Type type = kTypeDepth2ColorSD,
 		float imageRate = 0.0f,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraK4W2();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraMyntEye.h

@@ -52,7 +52,7 @@ public:
 	static bool available();
 
 public:
-	CameraMyntEye(const std::string & device = "", bool apiRectification = false, bool apiDepth = false, float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
+	CameraMyntEye(const std::string & device = "", bool apiRectification = false, bool apiDepth = false, float imageRate = 0, const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraMyntEye();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraOpenNI2.h

@@ -54,7 +54,7 @@ public:
 	CameraOpenNI2(const std::string & deviceId = "",
 					Type type = kTypeColorDepth,
 					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity());
+					const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraOpenNI2();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraOpenNICV.h

@@ -45,7 +45,7 @@ public:
 public:
 	CameraOpenNICV(bool asus = false,
 					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity());
+					const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraOpenNICV();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraOpenni.h

@@ -66,7 +66,7 @@ public:
 	// default local transform z in, x right, y down));
 	CameraOpenni(const std::string & deviceId="",
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraOpenni();
 #ifdef RTABMAP_OPENNI
 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)

corelib/include/rtabmap/core/camera/CameraRGBDImages.h

@@ -44,7 +44,7 @@ public:
 			const std::string & pathDepthImages,
 			float depthScaleFactor = 1.0f,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraRGBDImages();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraRealSense.h

@@ -64,7 +64,7 @@ public:
 		int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate
 		bool computeOdometry = false,
 		float imageRate = 0,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraRealSense();
 
 	void setDepthScaledToRGBSize(bool enabled);

corelib/include/rtabmap/core/camera/CameraRealSense2.h

@@ -62,7 +62,7 @@ public:
 	CameraRealSense2(
 		const std::string & deviceId = "",
 		float imageRate = 0,
-		const Transform & localTransform = Transform::getIdentity());
+		const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraRealSense2();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraStereoDC1394.h

@@ -45,7 +45,7 @@ public:
 	static bool available();
 
 public:
-	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraStereoDC1394();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h

@@ -47,7 +47,7 @@ public:
 	static bool available();
 
 public:
-	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraStereoFlyCapture2();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraStereoImages.h

@@ -49,12 +49,12 @@ public:
 			const std::string & pathRightImages,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	CameraStereoImages(
 			const std::string & pathLeftRightImages,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraStereoImages();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraStereoTara.h

@@ -53,7 +53,7 @@ public:
 			int device,
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 
 	virtual ~CameraStereoTara();
 

corelib/include/rtabmap/core/camera/CameraStereoVideo.h

@@ -46,24 +46,24 @@ public:
 			const std::string & pathSideBySide,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	CameraStereoVideo(
 			const std::string & pathLeft,
 			const std::string & pathRight,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	CameraStereoVideo(
 			int device,
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	CameraStereoVideo(
 			int deviceLeft,
 			int deviceRight,
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraStereoVideo();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/camera/CameraStereoZed.h

@@ -57,7 +57,7 @@ public:
 			int confidenceThr = 100,
 			bool computeOdometry = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = CameraModel::opticalRotation(),
 			bool selfCalibration = true,
 			bool odomForce3DoF = false,
 			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
@@ -68,7 +68,7 @@ public:
 			int confidenceThr = 100,
 			bool computeOdometry = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = CameraModel::opticalRotation(),
 			bool selfCalibration = true,
 			bool odomForce3DoF = false,
 			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3

corelib/include/rtabmap/core/camera/CameraVideo.h

@@ -45,11 +45,11 @@ public:
 	CameraVideo(int usbDevice = 0,
 			bool rectifyImages = false,
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	CameraVideo(const std::string & filePath,
 			bool rectifyImages = false,
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = CameraModel::opticalRotation());
 	virtual ~CameraVideo();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/src/Camera.cpp

@@ -64,6 +64,11 @@ void Camera::resetTimer()
 	_frameRateTimer->start();
 }
 
+bool Camera::initFromFile(const std::string & calibrationPath)
+{
+	return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front());
+}
+
 SensorData Camera::takeImage(CameraInfo * info)
 {
 	bool warnFrameRateTooHigh = false;

corelib/src/CameraModel.cpp

@@ -211,7 +211,7 @@ void CameraModel::setImageSize(const cv::Size & size)
 	}
 }
 
-bool CameraModel::load(const std::string & directory, const std::string & cameraName)
+bool CameraModel::load(const std::string & filePath)
 {
 	K_ = cv::Mat();
 	D_ = cv::Mat();
@@ -222,7 +222,6 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 	name_.clear();
 	imageSize_ = cv::Size();
 
-	std::string filePath = directory+"/"+cameraName+".yaml";
 	if(UFile::exists(filePath))
 	{
 		try
@@ -361,6 +360,11 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 	return false;
 }
 
+bool CameraModel::load(const std::string & directory, const std::string & cameraName)
+{
+	return load(directory+"/"+cameraName+".yaml");
+}
+
 bool CameraModel::save(const std::string & directory) const
 {
 	std::string filePath = directory+"/"+name_+".yaml";
@@ -636,22 +640,23 @@ CameraModel CameraModel::roi(const cv::Rect & roi) const
 	return roiModel;
 }
 
+double CameraModel::fovX() const
+{
+	return imageSize_.width>0 && fx()>0?2.0*atan(imageSize_.width/(fx()*2.0)):0.0;
+}
+double CameraModel::fovY() const
+{
+	return imageSize_.height>0 && fy()>0?2.0*atan(imageSize_.height/(fy()*2.0)):0.0;
+}
+
 double CameraModel::horizontalFOV() const
 {
-	if(imageWidth() > 0 && fx() > 0.0)
-	{
-		return atan((double(imageWidth())/2.0)/fx())*2.0*180.0/CV_PI;
-	}
-	return 0.0;
+	return fovX()*180.0/CV_PI;
 }
 
 double CameraModel::verticalFOV() const
 {
-	if(imageHeight() > 0 && fy() > 0.0)
-	{
-		return atan((double(imageHeight())/2.0)/fy())*2.0*180.0/CV_PI;
-	}
-	return 0.0;
+	return fovY()*180.0/CV_PI;
 }
 
 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const