-Updated CameraModel and StereoCameraModel so that a stereo camera model can be initialization from know intrinsics and extrinsics

-CreateSimpleCalibrationDialog updated to create mono/stereo calibration files with advanced option (setting intrinsics of each camera + extrinsics between them). Rectification R and new camera P matrices are then automatically computed.
-Added GeodeticCoords class for convience conversion between GPS values (latitude/longitude/altitude) to local coordinate (ENU).
-Added support of Malaga Urban and St Lucia ground truths.
-CameraImages: added option to debayer images. Timestamps file: added support of "sec millisec" format.
-Added UException class: exceptions are sent instead of exiting the application on UASSERT or UFATAL.

corelib/include/rtabmap/core/CameraModel.h

@@ -74,21 +74,25 @@ public:
 
 	void initRectificationMap();
 
-	bool isValid() const {return !K_.empty() &&
-									!D_.empty() &&
-									!R_.empty() &&
-									!P_.empty() &&
-									fx()>0.0 &&
-									fy()>0.0;}
+	bool isValid() const {return (!K_.empty() || !P_.empty()) && fx()>0.0 && fy()>0.0;}
+	bool isValidForRectification() const
+	{
+		return imageSize_.width>0 &&
+			   imageSize_.height>0 &&
+			   !K_.empty() &&
+			   !D_.empty() &&
+			   !R_.empty() &&
+			   !P_.empty();
+	}
 
 	void setName(const std::string & name) {name_=name;}
 	const std::string & name() const {return name_;}
 
-	double fx() const {return P_.at<double>(0,0);}
-	double fy() const {return P_.at<double>(1,1);}
-	double cx() const {return P_.at<double>(0,2);}
-	double cy() const {return P_.at<double>(1,2);}
-	double Tx() const {return P_.at<double>(0,3);}
+	double fx() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(0,0):P_.at<double>(0,0);}
+	double fy() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(1,1):P_.at<double>(1,1);}
+	double cx() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(0,2):P_.at<double>(0,2);}
+	double cy() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(1,2):P_.at<double>(1,2);}
+	double Tx() const {return P_.empty()?0.0:P_.at<double>(0,3);}
 
 	const cv::Mat & K() const {return K_;} //intrinsic camera matrix
 	const cv::Mat & D() const {return D_;} //intrinsic distorsion matrix

corelib/include/rtabmap/core/CameraRGB.h

@@ -63,12 +63,14 @@ public:
 	unsigned int imagesCount() const;
 	std::vector<std::string> filenames() const;
 	bool isImagesRectified() const {return _rectifyImages;}
+	int getBayerMode() const {return _bayerMode;}
 	const CameraModel & cameraModel() const {return _model;}
 
 	void setPath(const std::string & dir) {_path=dir;}
 	void setStartIndex(int index) {_startAt = index;} // negative means last
 	void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
 	void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
+	void setBayerMode(int mode) {_bayerMode = mode;} // -1=disabled (default) 0=BayerBG, 1=BayerGB, 2=BayerRG, 3=BayerGR
 
 	void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "", bool syncImageRateWithStamps=true)
 	{
@@ -126,6 +128,7 @@ private:
 	// on each call of takeImage()
 	bool _refreshDir;
 	bool _rectifyImages;
+	int _bayerMode;
 	bool _isDepth;
 	float _depthScaleFactor;
 	int _count;

corelib/include/rtabmap/core/GeodeticCoords.h

@@ -0,0 +1,63 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#ifndef GEODETICCOORDS_H_
+#define GEODETICCOORDS_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+
+#include <opencv2/core/types.hpp>
+
+namespace RTABMAP_EXP rtabmap {
+
+class GeodeticCoords
+{
+public:
+	GeodeticCoords();
+	GeodeticCoords(double latitude, double longitude, double altitude);
+
+	const double & latitude() const {return latitude_;}
+	const double & longitude() const {return longitude_;}
+	const double & altitude() const {return altitude_;}
+
+	void setLatitude(const double & value) {latitude_ = value;}
+	void setLongitude(const double & value) {longitude_ = value;}
+	void setAltitude(const double & value) {altitude_ = value;}
+
+	cv::Point3d toGeocentric_WGS84() const;
+	cv::Point3d toENU_WGS84(const GeodeticCoords & origin) const;
+
+private:
+	double latitude_;  // deg
+	double longitude_; // deg
+	double altitude_;  // m
+};
+
+}
+
+#endif /* GEODETICCOORDS_H_ */

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -43,16 +43,9 @@ public:
 			const cv::Size & imageSize2,
 			const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
 			const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
-			const Transform & localTransform = Transform::getIdentity()) :
-		left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
-		right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
-		name_(name),
-		R_(R),
-		T_(T),
-		E_(E),
-		F_(F)
-	{
-	}
+			const Transform & localTransform = Transform::getIdentity());
+
+	// if R and T are not null, left and right camera models should be valid to be rectified.
 	StereoCameraModel(
 			const std::string & name,
 			const CameraModel & leftCameraModel,
@@ -60,18 +53,14 @@ public:
 			const cv::Mat & R = cv::Mat(),
 			const cv::Mat & T = cv::Mat(),
 			const cv::Mat & E = cv::Mat(),
-			const cv::Mat & F = cv::Mat()) :
-		left_(leftCameraModel),
-		right_(rightCameraModel),
-		name_(name),
-		R_(R),
-		T_(T),
-		E_(E),
-		F_(F)
-	{
-		left_.setName(name+"_left");
-		right_.setName(name+"_right");
-	}
+			const cv::Mat & F = cv::Mat());
+	// if extrinsics transform is not null, left and right camera models should be valid to be rectified.
+	StereoCameraModel(
+			const std::string & name,
+			const CameraModel & leftCameraModel,
+			const CameraModel & rightCameraModel,
+			const Transform & extrinsics);
+
 	//minimal
 	StereoCameraModel(
 			double fx,
@@ -79,11 +68,7 @@ public:
 			double cx,
 			double cy,
 			double baseline,
-			const Transform & localTransform = Transform::getIdentity()) :
-		left_(fx, fy, cx, cy, localTransform),
-		right_(fx, fy, cx, cy, localTransform, baseline*-fx)
-	{
-	}
+			const Transform & localTransform = Transform::getIdentity());
 	//minimal to be saved
 	StereoCameraModel(
 			const std::string & name,
@@ -92,15 +77,13 @@ public:
 			double cx,
 			double cy,
 			double baseline,
-			const Transform & localTransform = Transform::getIdentity()) :
-		left_(name+"_left", fx, fy, cx, cy, localTransform),
-		right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
-		name_(name)
-	{
-	}
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~StereoCameraModel() {}
 
 	bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;}
+	bool isValidForRectification() const {return left_.isValidForRectification() && right_.isValidForRectification();}
+
+	void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();}
 
 	void setName(const std::string & name);
 	const std::string & name() const {return name_;}
@@ -108,7 +91,7 @@ public:
 	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
 	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
 
-	double baseline() const {return -right_.Tx()/right_.fx();}
+	double baseline() const {return right_.fx()!=0.0?-right_.Tx()/right_.fx():0.0;}
 
 	float computeDepth(float disparity) const;
 	float computeDisparity(float depth) const; // m

corelib/include/rtabmap/core/Transform.h

@@ -81,6 +81,7 @@ public:
 	void setNull();
 	void setIdentity();
 
+	const cv::Mat & dataMatrix() const {return data_;}
 	const float * data() const {return (const float *)data_.data;}
 	float * data() {return (float *)data_.data;}
 	int size() const {return 12;}
@@ -99,6 +100,9 @@ public:
 	Transform translation() const;
 	Transform to3DoF() const;
 
+	cv::Mat rotationMatrix() const;
+	cv::Mat translationMatrix() const;
+
 	void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
 	void getEulerAngles(float & roll, float & pitch, float & yaw) const;
 	void getTranslation(float & x, float & y, float & z) const;
@@ -139,6 +143,7 @@ public:
 	 * Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
 	 */
 	static Transform fromString(const std::string & string);
+	static bool canParseString(const std::string & string);
 
 private:
 	cv::Mat data_;