-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() &&
+	bool isValid() const {return (!K_.empty() || !P_.empty()) && fx()>0.0 && fy()>0.0;}
-									!D_.empty() &&
+	bool isValidForRectification() const
-									!R_.empty() &&
+	{
-									!P_.empty() &&
+		return imageSize_.width>0 &&
-									fx()>0.0 &&
+			   imageSize_.height>0 &&
-									fy()>0.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 fx() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(0,0):P_.at<double>(0,0);}
-	double fy() const {return P_.at<double>(1,1);}
+	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_.at<double>(0,2);}
+	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_.at<double>(1,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_.at<double>(0,3);}
+	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()) :
+			const Transform & localTransform = Transform::getIdentity());
-		left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
+
-		right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
+	// if R and T are not null, left and right camera models should be valid to be rectified.
-		name_(name),
-		R_(R),
-		T_(T),
-		E_(E),
-		F_(F)
-	{
-	}
 	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()) :
+			const cv::Mat & F = cv::Mat());
-		left_(leftCameraModel),
+	// if extrinsics transform is not null, left and right camera models should be valid to be rectified.
-		right_(rightCameraModel),
+	StereoCameraModel(
-		name_(name),
+			const std::string & name,
-		R_(R),
+			const CameraModel & leftCameraModel,
-		T_(T),
+			const CameraModel & rightCameraModel,
-		E_(E),
+			const Transform & extrinsics);
-		F_(F)
+
-	{
-		left_.setName(name+"_left");
-		right_.setName(name+"_right");
-	}
 	//minimal
 	StereoCameraModel(
 			double fx,
@@ -79,11 +68,7 @@ public:
 			double cx,
 			double cy,
 			double baseline,
-			const Transform & localTransform = Transform::getIdentity()) :
+			const Transform & localTransform = Transform::getIdentity());
-		left_(fx, fy, cx, cy, localTransform),
-		right_(fx, fy, cx, cy, localTransform, baseline*-fx)
-	{
-	}
 	//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()) :
+			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)
-	{
-	}
 	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_;

corelib/src/CMakeLists.txt

@@ -26,6 +26,7 @@ SET(SRC_FILES
     Signature.cpp
 	Features2d.cpp
 	Transform.cpp
+	GeodeticCoords.cpp
 	
 	util2d.cpp
 	

corelib/src/CameraModel.cpp

@@ -34,8 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-CameraModel::CameraModel() :
+CameraModel::CameraModel()
-		P_(cv::Mat::zeros(3, 4, CV_64FC1))
 {
 
 }
@@ -56,12 +55,10 @@ CameraModel::CameraModel(
 		P_(P),
 		localTransform_(localTransform)
 {
-	UASSERT(!name_.empty());
+	UASSERT(K_.empty() || (K_.rows == 3 && K_.cols == 3 && K_.type() == CV_64FC1));
-	UASSERT(imageSize_.width > 0 && imageSize_.height > 0);
+	UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8) && D_.type() == CV_64FC1));
-	UASSERT(K_.rows == 3 && K_.cols == 3);
+	UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
-	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
+	UASSERT(P_.empty() || (P_.rows == 3 && P_.cols == 4 && P_.type() == CV_64FC1));
-	UASSERT(R_.rows == 3 && R_.cols == 3);
-	UASSERT(P_.rows == 3 && P_.cols == 4);
 }
 
 CameraModel::CameraModel(
@@ -72,20 +69,22 @@ CameraModel::CameraModel(
 		const Transform & localTransform,
 		double Tx) :
 		K_(cv::Mat::eye(3, 3, CV_64FC1)),
-		D_(cv::Mat::zeros(1, 5, CV_64FC1)),
-		R_(cv::Mat::eye(3, 3, CV_64FC1)),
-		P_(cv::Mat::eye(3, 4, CV_64FC1)),
 		localTransform_(localTransform)
 {
-	UASSERT_MSG(fx >= 0.0, uFormat("fx=%f", fx).c_str());
+	UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
-	UASSERT_MSG(fy >= 0.0, uFormat("fy=%f", fy).c_str());
+	UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
 	UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
 	UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
-	P_.at<double>(0,0) = fx;
+	UASSERT(!localTransform.isNull());
-	P_.at<double>(1,1) = fy;
+	if(Tx != 0.0)
-	P_.at<double>(0,2) = cx;
+	{
-	P_.at<double>(1,2) = cy;
+		P_ = cv::Mat::eye(3, 4, CV_64FC1),
-	P_.at<double>(0,3) = Tx;
+		P_.at<double>(0,0) = fx;
+		P_.at<double>(1,1) = fy;
+		P_.at<double>(0,2) = cx;
+		P_.at<double>(1,2) = cy;
+		P_.at<double>(0,3) = Tx;
+	}
 
 	K_.at<double>(0,0) = fx;
 	K_.at<double>(1,1) = fy;
@@ -103,20 +102,22 @@ CameraModel::CameraModel(
 		double Tx) :
 		name_(name),
 		K_(cv::Mat::eye(3, 3, CV_64FC1)),
-		D_(cv::Mat::zeros(1, 5, CV_64FC1)),
-		R_(cv::Mat::eye(3, 3, CV_64FC1)),
-		P_(cv::Mat::eye(3, 4, CV_64FC1)),
 		localTransform_(localTransform)
 {
-	UASSERT_MSG(fx >= 0.0, uFormat("fx=%f", fx).c_str());
+	UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
-	UASSERT_MSG(fy >= 0.0, uFormat("fy=%f", fy).c_str());
+	UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
 	UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
 	UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
-	P_.at<double>(0,0) = fx;
+	UASSERT(!localTransform.isNull());
-	P_.at<double>(1,1) = fy;
+	if(Tx != 0.0)
-	P_.at<double>(0,2) = cx;
+	{
-	P_.at<double>(1,2) = cy;
+		P_ = cv::Mat::eye(3, 4, CV_64FC1),
-	P_.at<double>(0,3) = Tx;
+		P_.at<double>(0,0) = fx;
+		P_.at<double>(1,1) = fy;
+		P_.at<double>(0,2) = cx;
+		P_.at<double>(1,2) = cy;
+		P_.at<double>(0,3) = Tx;
+	}
 
 	K_.at<double>(0,0) = fx;
 	K_.at<double>(1,1) = fy;
@@ -127,6 +128,9 @@ CameraModel::CameraModel(
 void CameraModel::initRectificationMap()
 {
 	UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
+	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
+	UASSERT(R_.rows == 3 && R_.cols == 3);
+	UASSERT(P_.rows == 3 && P_.cols == 4);
 	// init rectification map
 	UINFO("Initialize rectify map");
 	cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
@@ -137,68 +141,122 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 	K_ = cv::Mat();
 	D_ = cv::Mat();
 	R_ = cv::Mat();
-	P_ = cv::Mat::zeros(3, 4, CV_64FC1);
+	P_ = cv::Mat();
 	mapX_ = cv::Mat();
 	mapY_ = cv::Mat();
+	name_.clear();
+	imageSize_ = cv::Size();
 
 	std::string filePath = directory+"/"+cameraName+".yaml";
 	if(UFile::exists(filePath))
 	{
-		UINFO("Reading calibration file \"%s\"", filePath.c_str());
+		try
-		cv::FileStorage fs(filePath, cv::FileStorage::READ);
-
-		name_ = (int)fs["camera_name"];
-		imageSize_.width = (int)fs["image_width"];
-		imageSize_.height = (int)fs["image_height"];
-		UASSERT(!name_.empty());
-		//UASSERT(imageSize_.width > 0);
-		//UASSERT(imageSize_.height > 0);
-
-		// import from ROS calibration format
-		cv::FileNode n = fs["camera_matrix"];
-		int rows = (int)n["rows"];
-		int cols = (int)n["cols"];
-		std::vector<double> data;
-		n["data"] >> data;
-		UASSERT(rows*cols == (int)data.size());
-		UASSERT(rows == 3 && cols == 3);
-		K_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-
-		n = fs["distortion_coefficients"];
-		rows = (int)n["rows"];
-		cols = (int)n["cols"];
-		data.clear();
-		n["data"] >> data;
-		UASSERT(rows*cols == (int)data.size());
-		UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
-		D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-
-		n = fs["rectification_matrix"];
-		rows = (int)n["rows"];
-		cols = (int)n["cols"];
-		data.clear();
-		n["data"] >> data;
-		UASSERT(rows*cols == (int)data.size());
-		UASSERT(rows == 3 && cols == 3);
-		R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-
-		n = fs["projection_matrix"];
-		rows = (int)n["rows"];
-		cols = (int)n["cols"];
-		data.clear();
-		n["data"] >> data;
-		UASSERT(rows*cols == (int)data.size());
-		UASSERT(rows == 3 && cols == 4);
-		P_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-
-		fs.release();
-
-		if(imageSize_.height > 0 && imageSize_.width > 0)
 		{
-			initRectificationMap();
+			UINFO("Reading calibration file \"%s\"", filePath.c_str());
-		}
+			cv::FileStorage fs(filePath, cv::FileStorage::READ);
 
-		return true;
+			cv::FileNode n,n2;
+
+			n = fs["camera_name"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				name_ = (int)n;
+			}
+			else
+			{
+				UWARN("Missing \"camera_name\" field in \"%s\"", filePath.c_str());
+			}
+
+			n = fs["image_width"];
+			n2 = fs["image_height"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				imageSize_.width = (int)fs["image_width"];
+				imageSize_.height = (int)fs["image_height"];
+			}
+			else
+			{
+				UWARN("Missing \"image_width\" and/or \"image_height\" fields in \"%s\"", filePath.c_str());
+			}
+
+			// import from ROS calibration format
+			n = fs["camera_matrix"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				int rows = (int)n["rows"];
+				int cols = (int)n["cols"];
+				std::vector<double> data;
+				n["data"] >> data;
+				UASSERT(rows*cols == (int)data.size());
+				UASSERT(rows == 3 && cols == 3);
+				K_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+			}
+			else
+			{
+				UWARN("Missing \"camera_matrix\" field in \"%s\"", filePath.c_str());
+			}
+
+			n = fs["distortion_coefficients"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				int rows = (int)n["rows"];
+				int cols = (int)n["cols"];
+				std::vector<double> data;
+				n["data"] >> data;
+				UASSERT(rows*cols == (int)data.size());
+				UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
+				D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+			}
+			else
+			{
+				UWARN("Missing \"distorsion_coefficients\" field in \"%s\"", filePath.c_str());
+			}
+
+			n = fs["rectification_matrix"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				int rows = (int)n["rows"];
+				int cols = (int)n["cols"];
+				std::vector<double> data;
+				n["data"] >> data;
+				UASSERT(rows*cols == (int)data.size());
+				UASSERT(rows == 3 && cols == 3);
+				R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+			}
+			else
+			{
+				UWARN("Missing \"rectification_matrix\" field in \"%s\"", filePath.c_str());
+			}
+
+			n = fs["projection_matrix"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				int rows = (int)n["rows"];
+				int cols = (int)n["cols"];
+				std::vector<double> data;
+				n["data"] >> data;
+				UASSERT(rows*cols == (int)data.size());
+				UASSERT(rows == 3 && cols == 4);
+				P_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+			}
+			else
+			{
+				UWARN("Missing \"projection_matrix\" field in \"%s\"", filePath.c_str());
+			}
+
+			fs.release();
+
+			if(isValidForRectification())
+			{
+				initRectificationMap();
+			}
+
+			return true;
+		}
+		catch(const cv::Exception & e)
+		{
+			UERROR("Error reading calibration file \"%s\": %s", filePath.c_str(), e.what());
+		}
 	}
 	else
 	{
@@ -210,56 +268,77 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 bool CameraModel::save(const std::string & directory) const
 {
 	std::string filePath = directory+"/"+name_+".yaml";
-	if(!filePath.empty() && !name_.empty() && !K_.empty() && !D_.empty() && !R_.empty() && !P_.empty())
+	if(!filePath.empty() && (!K_.empty() || !D_.empty() || !R_.empty() || !P_.empty()))
 	{
 		UINFO("Saving calibration to file \"%s\"", filePath.c_str());
 		cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
 
 		// export in ROS calibration format
 
-		fs << "camera_name" << name_;
+		if(!name_.empty())
-		fs << "image_width" << imageSize_.width;
-		fs << "image_height" << imageSize_.height;
-
-		fs << "camera_matrix" << "{";
-		fs << "rows" << K_.rows;
-		fs << "cols" << K_.cols;
-		fs << "data" << std::vector<double>((double*)K_.data, ((double*)K_.data)+(K_.rows*K_.cols));
-		fs << "}";
-
-		fs << "distortion_coefficients" << "{";
-		fs << "rows" << D_.rows;
-		fs << "cols" << D_.cols;
-		fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
-		fs << "}";
-
-		// compaibility with ROS
-
-		if(D_.cols > 5)
 		{
-			fs << "distortion_model" << "rational_polynomial";
+			fs << "camera_name" << name_;
 		}
-		else
+		if(imageSize_.width>0 && imageSize_.height>0)
 		{
-			fs << "distortion_model" << "plumb_bob";
+			fs << "image_width" << imageSize_.width;
+			fs << "image_height" << imageSize_.height;
 		}
 
-		fs << "rectification_matrix" << "{";
+		if(!K_.empty())
-		fs << "rows" << R_.rows;
+		{
-		fs << "cols" << R_.cols;
+			fs << "camera_matrix" << "{";
-		fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
+			fs << "rows" << K_.rows;
-		fs << "}";
+			fs << "cols" << K_.cols;
+			fs << "data" << std::vector<double>((double*)K_.data, ((double*)K_.data)+(K_.rows*K_.cols));
+			fs << "}";
+		}
 
-		fs << "projection_matrix" << "{";
+		if(!D_.empty())
-		fs << "rows" << P_.rows;
+		{
-		fs << "cols" << P_.cols;
+			fs << "distortion_coefficients" << "{";
-		fs << "data" << std::vector<double>((double*)P_.data, ((double*)P_.data)+(P_.rows*P_.cols));
+			fs << "rows" << D_.rows;
-		fs << "}";
+			fs << "cols" << D_.cols;
+			fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
+			fs << "}";
+
+			// compaibility with ROS
+			if(D_.cols > 5)
+			{
+				fs << "distortion_model" << "rational_polynomial";
+			}
+			else
+			{
+				fs << "distortion_model" << "plumb_bob";
+			}
+		}
+
+		if(!R_.empty())
+		{
+			fs << "rectification_matrix" << "{";
+			fs << "rows" << R_.rows;
+			fs << "cols" << R_.cols;
+			fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
+			fs << "}";
+		}
+
+		if(!P_.empty())
+		{
+			fs << "projection_matrix" << "{";
+			fs << "rows" << P_.rows;
+			fs << "cols" << P_.cols;
+			fs << "data" << std::vector<double>((double*)P_.data, ((double*)P_.data)+(P_.rows*P_.cols));
+			fs << "}";
+		}
 
 		fs.release();
 
 		return true;
 	}
+	else
+	{
+		UERROR("Cannot save calibration to \"%s\" because it is empty.", filePath.c_str());
+	}
 	return false;
 }
 
@@ -270,17 +349,25 @@ CameraModel CameraModel::scaled(double scale) const
 	if(this->isValid())
 	{
 		// has only effect on K and P
-		cv::Mat K = K_.clone();
+		cv::Mat K;
-		K.at<double>(0,0) *= scale;
+		if(!K_.empty())
-		K.at<double>(1,1) *= scale;
+		{
-		K.at<double>(0,2) *= scale;
+			K = K_.clone();
-		K.at<double>(1,2) *= scale;
+			K.at<double>(0,0) *= scale;
+			K.at<double>(1,1) *= scale;
+			K.at<double>(0,2) *= scale;
+			K.at<double>(1,2) *= scale;
+		}
 
-		cv::Mat P = P_.clone();
+		cv::Mat P;
-		P.at<double>(0,0) *= scale;
+		if(!P_.empty())
-		P.at<double>(1,1) *= scale;
+		{
-		P.at<double>(0,2) *= scale;
+			P = P_.clone();
-		P.at<double>(1,2) *= scale;
+			P.at<double>(0,0) *= scale;
+			P.at<double>(1,1) *= scale;
+			P.at<double>(0,2) *= scale;
+			P.at<double>(1,2) *= scale;
+		}
 		scaledModel = CameraModel(name_, cv::Size(double(imageSize_.width)*scale, double(imageSize_.height)*scale), K, D_, R_, P, localTransform_);
 	}
 	else
@@ -371,6 +458,7 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
 	}
 	else
 	{
+		UERROR("Cannot rectify image because the rectify map is not initialized.");
 		return raw.clone();
 	}
 }

corelib/src/CameraRGB.cpp

@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_surface.h>
 
 #include <iostream>
+#include <fstream>
 #include <cmath>
 
 namespace rtabmap
@@ -55,6 +56,7 @@ CameraImages::CameraImages() :
 		_startAt(0),
 		_refreshDir(false),
 		_rectifyImages(false),
+		_bayerMode(-1),
 		_isDepth(false),
 		_depthScaleFactor(1.0f),
 		_count(0),
@@ -81,6 +83,7 @@ CameraImages::CameraImages(const std::string & path,
 	_startAt(0),
 	_refreshDir(false),
 	_rectifyImages(false),
+	_bayerMode(-1),
 	_isDepth(false),
 	_depthScaleFactor(1.0f),
 	_count(0),
@@ -259,24 +262,39 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 		}
 		else if(timestampsPath_.size())
 		{
-			FILE * file = 0;
+			std::ifstream file;
-#ifdef _MSC_VER
+			file.open(timestampsPath_.c_str(), std::ifstream::in);
-			fopen_s(&file, timestampsPath_.c_str(), "r");
+			while(file.good())
-#else
-			file = fopen(timestampsPath_.c_str(), "r");
-#endif
-			if(file)
 			{
-				char line[16];
+				std::string str;
-				while ( fgets (line , 16 , file) != NULL )
+				std::getline(file, str);
+
+				if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
 				{
-					stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0)));
+					continue;
 				}
-				fclose(file);
+
+				std::list<std::string> strList = uSplit(str, ' ');
+				std::string stampStr = strList.front();
+				if(strList.size() == 2)
+				{
+					// format "seconds millisec"
+					// the millisec str needs 0-padding if size < 6
+					std::string millisecStr = strList.back();
+					while(millisecStr.size() < 6)
+					{
+						millisecStr = "0" + millisecStr;
+					}
+					stampStr = stampStr+'.'+millisecStr;
+				}
+				stamps_.push_back(uStr2Double(stampStr));
 			}
+
+			file.close();
+
 			if(stamps_.size() != this->imagesCount())
 			{
-				UERROR("The stamps count is not the same as the images (%d vs %d)! Please remove "
+				UERROR("The stamps count (%d) is not the same as the images (%d)! Please remove "
 						"the timestamps file path if you don't want to use them (current file path=%s).",
 						(int)stamps_.size(), this->imagesCount(), timestampsPath_.c_str());
 				stamps_.clear();
@@ -293,19 +311,19 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 				UERROR("Cannot read ground truth file \"%s\".", groundTruthPath_.c_str());
 				success = false;
 			}
-			else if((_groundTruthFormat != 1 && _groundTruthFormat != 5) && poses.size() != this->imagesCount())
+			else if((_groundTruthFormat != 1 && _groundTruthFormat != 5 && _groundTruthFormat != 6 && _groundTruthFormat != 7) && poses.size() != this->imagesCount())
 			{
 				UERROR("The ground truth count is not the same as the images (%d vs %d)! Please remove "
 						"the ground truth file path if you don't want to use it (current file path=%s).",
 						(int)poses.size(), this->imagesCount(), groundTruthPath_.c_str());
 				success = false;
 			}
-			else if((_groundTruthFormat == 1 || _groundTruthFormat == 5) && stamps_.size() == 0)
+			else if((_groundTruthFormat == 1 || _groundTruthFormat == 5 || _groundTruthFormat == 6 || _groundTruthFormat == 7) && stamps_.size() == 0)
 			{
-				UERROR("When using RGBD-SLAM and GPS formats for ground truth, images must have timestamps!");
+				UERROR("When using RGBD-SLAM, GPS, MALAGA and ST LUCIA formats for ground truth, images must have timestamps!");
 				success = false;
 			}
-			else if(_groundTruthFormat == 1 || _groundTruthFormat == 5)
+			else if(_groundTruthFormat == 1 || _groundTruthFormat == 5 || _groundTruthFormat == 6 || _groundTruthFormat == 7)
 			{
 				UDEBUG("");
 				//Match ground truth values with images
@@ -568,7 +586,6 @@ SensorData CameraImages::captureImage()
 					cvReleaseImage(&i);
 				}
 #endif
-
 				if(img.channels()>3)
 				{
 					UWARN("Conversion from 4 channels to 3 channels (file=%s)", imageFilePath.c_str());
@@ -576,6 +593,20 @@ SensorData CameraImages::captureImage()
 					cv::cvtColor(img, out, CV_BGRA2BGR);
 					img = out;
 				}
+				else if(_bayerMode >= 0 && _bayerMode <=3)
+				{
+					cv::Mat debayeredImg;
+					try
+					{
+						cv::cvtColor(img, debayeredImg, CV_BayerBG2BGR + _bayerMode);
+						img = debayeredImg;
+					}
+					catch(const cv::Exception & e)
+					{
+						UWARN("Error debayering images: \"%s\". Please set bayer mode to -1 if images are not bayered!", e.what());
+					}
+				}
+
 			}
 
 			if(!img.empty() && _model.isValid() && _rectifyImages)

corelib/src/CameraRGBD.cpp

@@ -1395,7 +1395,7 @@ SensorData CameraFreenect2::captureImage()
 			else
 			{
 				//rgb + ir or rgb + depth
-				if(stereoModel_.isValid())
+				if(stereoModel_.isValidForRectification())
 				{
 					cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
 					cv::Mat rgbMat; // rtabmap uses 3 channels RGB

corelib/src/CameraStereo.cpp

@@ -422,8 +422,14 @@ SensorData CameraStereoDC1394::captureImage()
 		if(!left.empty() && !right.empty())
 		{
 			// Rectification
-			left = stereoModel_.left().rectifyImage(left);
+			if(stereoModel_.left().isValidForRectification())
-			right = stereoModel_.right().rectifyImage(right);
+			{
+				left = stereoModel_.left().rectifyImage(left);
+			}
+			if(stereoModel_.right().isValidForRectification())
+			{
+				right = stereoModel_.right().rectifyImage(right);
+			}
 			StereoCameraModel model;
 			if(stereoModel_.isValid())
 			{
@@ -740,7 +746,6 @@ CameraStereoImages::CameraStereoImages(
 		camera2_(new CameraImages(pathRightImages))
 {
 	this->setImagesRectified(rectifyImages);
-	camera2_->setImagesRectified(rectifyImages);
 }
 
 CameraStereoImages::CameraStereoImages(
@@ -800,7 +805,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 
 	stereoModel_.setLocalTransform(this->getLocalTransform());
 	stereoModel_.setName(cameraName);
-	if(this->isImagesRectified() && !stereoModel_.isValid())
+	if(this->isImagesRectified() && !stereoModel_.isValidForRectification())
 	{
 		UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
 		return false;
@@ -815,6 +820,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 	{
 		if(camera2_)
 		{
+			camera2_->setBayerMode(this->getBayerMode());
 			if(camera2_->init())
 			{
 				if(this->imagesCount() == camera2_->imagesCount())
@@ -879,11 +885,12 @@ SensorData CameraStereoImages::captureImage()
 				cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
 				rightImage = tmp;
 			}
-			if(this->isImagesRectified() && stereoModel_.left().isValid() && stereoModel_.right().isValid())
+			if(this->isImagesRectified() && stereoModel_.isValidForRectification())
 			{
 				leftImage = stereoModel_.left().rectifyImage(leftImage);
 				rightImage = stereoModel_.right().rectifyImage(rightImage);
 			}
+
 			data = SensorData(left.laserScanRaw(), left.laserScanMaxPts(), 0, leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
 			data.setGroundTruth(left.groundTruth());
 		}
@@ -952,7 +959,7 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 		}
 
 		stereoModel_.setLocalTransform(this->getLocalTransform());
-		if(rectifyImages_ && !stereoModel_.isValid())
+		if(rectifyImages_ && !stereoModel_.isValidForRectification())
 		{
 			UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
 			return false;
@@ -991,6 +998,7 @@ SensorData CameraStereoVideo::captureImage()
 				rightImage = tmp;
 				rightCvt = true;
 			}
+
 			if(rectifyImages_ && stereoModel_.left().isValid() && stereoModel_.right().isValid())
 			{
 				leftImage = stereoModel_.left().rectifyImage(leftImage);

corelib/src/GeodeticCoords.cpp

@@ -0,0 +1,137 @@
+/*
+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.
+*/
+
+/*
+ * The methods in this file were modified from the originals of the MRPT toolkit (see notice below):
+ * https://github.com/MRPT/mrpt/blob/master/libs/topography/src/conversions.cpp
+ */
+
+/* +---------------------------------------------------------------------------+
+   |                     Mobile Robot Programming Toolkit (MRPT)               |
+   |                          http://www.mrpt.org/                             |
+   |                                                                           |
+   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
+   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
+   | Released under BSD License. See details in http://www.mrpt.org/License    |
+   +---------------------------------------------------------------------------+ */
+
+
+#include "rtabmap/core/GeodeticCoords.h"
+
+#include <math.h>
+
+namespace rtabmap {
+
+inline double DEG2RAD(const double x) { return x*M_PI/180.0;}
+inline double square(const double & value) {return value*value;}
+
+//*---------------------------------------------------------------
+//			geodeticToGeocentric_WGS84
+// ---------------------------------------------------------------*/
+cv::Point3d GeodeticCoords::toGeocentric_WGS84() const
+{
+	// --------------------------------------------------------------------
+	// See: http://en.wikipedia.org/wiki/Reference_ellipsoid
+	//  Constants are for WGS84
+	// --------------------------------------------------------------------
+
+	static const double a = 6378137;		// Semi-major axis of the Earth (meters)
+	static const double b = 6356752.3142;	// Semi-minor axis:
+
+	static const double ae = acos(b/a);  	// eccentricity:
+	static const double cos2_ae_earth =  square(cos(ae)); // The cos^2 of the angular eccentricity of the Earth: // 0.993305619995739L;
+	static const double sin2_ae_earth = square(sin(ae));  // The sin^2 of the angular eccentricity of the Earth: // 0.006694380004261L;
+
+	const double lon  = DEG2RAD( double(this->longitude()) );
+	const double lat  = DEG2RAD( double(this->latitude()) );
+
+	// The radius of curvature in the prime vertical:
+	const double N = a / std::sqrt( 1.0 - sin2_ae_earth*square( sin(lat) ) );
+
+	// Generate 3D point:
+	cv::Point3d out;
+	out.x = (N+this->altitude())*cos(lat)*cos(lon);
+	out.y = (N+this->altitude())*cos(lat)*sin(lon);
+	out.z = (cos2_ae_earth*N+this->altitude())*sin(lat);
+
+	return out;
+}
+
+
+/*---------------------------------------------------------------
+				geodeticToENU_WGS84
+ ---------------------------------------------------------------*/
+cv::Point3d GeodeticCoords::toENU_WGS84(const GeodeticCoords &origin) const
+{
+	// --------------------------------------------------------------------
+	//  Explanation: We compute the earth-centric coordinates first,
+	//    then make a system transformation to local XYZ coordinates
+	//    using a system of three orthogonal vectors as local reference.
+	//
+	// See: http://en.wikipedia.org/wiki/Reference_ellipsoid
+	// (JLBC 21/DEC/2006)  (Fixed: JLBC 9/JUL/2008)
+	// - Oct/2013, Emilio Sanjurjo: Fixed UP vector pointing exactly normal to ellipsoid surface.
+	// --------------------------------------------------------------------
+	// Generate 3D point:
+	cv::Point3d	P_geocentric = this->toGeocentric_WGS84();
+
+	// Generate reference 3D point:
+	cv::Point3d P_geocentric_ref = origin.toGeocentric_WGS84();
+
+	const double clat = cos(DEG2RAD(origin.latitude())), slat = sin(DEG2RAD(origin.latitude()));
+	const double clon = cos(DEG2RAD(origin.longitude())), slon = sin(DEG2RAD(origin.longitude()));
+
+	// Compute the resulting relative coordinates:
+	// For using smaller numbers:
+	P_geocentric -= P_geocentric_ref;
+
+	// Optimized calculation: Local transformed coordinates of P_geo(x,y,z)
+	//   after rotation given by the transposed rotation matrix from ENU -> ECEF.
+	cv::Point3d out;
+	out.x = -slon*P_geocentric.x + clon*P_geocentric.y;
+	out.y = -clon*slat*P_geocentric.x -slon*slat*P_geocentric.y + clat*P_geocentric.z;
+	out.z = clon*clat*P_geocentric.x + slon*clat*P_geocentric.y +slat*P_geocentric.z;
+
+	return out;
+}
+
+GeodeticCoords::GeodeticCoords() :
+		latitude_(0.0),
+		longitude_(0.0),
+		altitude_(0.0)
+{
+
+}
+GeodeticCoords::GeodeticCoords(double latitude, double longitude, double altitude) :
+		latitude_(latitude),
+		longitude_(longitude),
+		altitude_(altitude)
+{
+
+}
+
+}

corelib/src/Graph.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UFile.h>
+#include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/core/Memory.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/common/eigen.h>
@@ -153,7 +154,7 @@ bool exportPoses(
 
 bool importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=GPS (t,x,y)
+		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints, // optional for formats 3 and 4
 		std::map<int, double> * stamps) // optional for format 1
@@ -187,20 +188,106 @@ bool importPoses(
 			return false;
 		}
 		int id=1;
+		GeodeticCoords origin;
+		Transform originPose;
 		while(file.good())
 		{
 			std::string str;
 			std::getline(file, str);
 
-			if(str.empty() || str.at(0) == '#')
+			if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
 			{
 				continue;
 			}
 
-			if(format == 5) // GPS format
+			if(format == 7) // St Lucia format
 			{
 				std::vector<std::string> strList = uListToVector(uSplit(str));
-				if(strList.size() ==  3 || strList.size() ==  4)
+				if(strList.size() ==  12)
+				{
+					// Data Type
+					//0=Timestamp (seconds)
+					//1=Timestamp (millisec)
+					//2=Latitude (deg)
+					//3=Longitude (deg)
+					//4=Altitude (m)
+					//5=Height AMSL (m)
+					//6=vENU X
+					//7=vENU Y
+					//8=vENU Z
+					//9=Roll (rad)
+					//10=Pitch (rad)
+					//11=Yaw (rad)
+
+					// the millisec str needs 0-padding if size < 6
+					std::string millisecStr = strList[1];
+					while(millisecStr.size() < 6)
+					{
+						millisecStr = "0" + millisecStr;
+					}
+					double stamp = uStr2Double(strList[0] + "." + millisecStr);
+
+					// conversion GPS to local coordinate XYZ
+					double longitude = uStr2Double(strList[2]);
+					double latitude = uStr2Double(strList[3]);
+					double altitude = uStr2Double(strList[4]);
+					if(poses.empty())
+					{
+						origin = GeodeticCoords(longitude, latitude, altitude);
+					}
+					cv::Point3d coordENU = GeodeticCoords(longitude, latitude, altitude).toENU_WGS84(origin);
+					double roll = uStr2Double(strList[10]);
+					double pitch = uStr2Double(strList[9]);
+					double yaw = -(uStr2Double(strList[11])-M_PI_2);
+					if(stamps)
+					{
+						stamps->insert(std::make_pair(id, stamp));
+					}
+					poses.insert(std::make_pair(id, Transform(coordENU.x,coordENU.y,coordENU.z, roll,pitch,yaw)));
+				}
+				else
+				{
+					UERROR("Error parsing \"%s\" with St Lucia format (should have 12 values, e.g., 101215_153851_Ins0.log)", str.c_str());
+				}
+			}
+			else if(format == 6) // MALAGA URBAN format
+			{
+				std::vector<std::string> strList = uListToVector(uSplit(str));
+				if(strList.size() ==  25)
+				{
+					// 0=Time
+					// Lat Lon Alt fix #sats speed dir
+					// 8=Local_X
+					// 9=Local_Y
+					// 10=Local_Z
+					// rawlog_ID Geocen_X Geocen_Y Geocen_Z GPS_X GPS_Y GPS_Z GPS_VX GPS_VY GPS_VZ Local_VX Local_VY Local_VZ SAT_Time
+					double stamp = uStr2Double(strList[0]);
+					double x = uStr2Double(strList[8]);
+					double y = uStr2Double(strList[9]);
+					double z = uStr2Double(strList[10]);
+					if(stamps)
+					{
+						stamps->insert(std::make_pair(id, stamp));
+					}
+					float yaw = 0.0f;
+					if(uContains(poses, id-1))
+					{
+						// set yaw depending on successive poses
+						Transform & previousPose = poses.at(id-1);
+						yaw = atan2(y-previousPose.y(),x-previousPose.x());
+						previousPose = Transform(previousPose.x(), previousPose.y(), yaw);
+					}
+					poses.insert(std::make_pair(id, Transform(x,y,z,0,0,yaw)));
+				}
+				else
+				{
+					UERROR("Error parsing \"%s\" with Malaga Urban format (should have 25 values, *_GPS.txt)", str.c_str());
+				}
+			}
+			else if(format == 5) // NewCollege format
+			{
+				std::vector<std::string> strList = uListToVector(uSplit(str));
+				if(strList.size() ==  3)
 				{
 					if( uIsNumber(uReplaceChar(strList[0], ' ', "")) && 
 						uIsNumber(uReplaceChar(strList[1], ' ', "")) && 
@@ -216,18 +303,20 @@ bool importPoses(
 							stamps->insert(std::make_pair(id, stamp));
 						}
 						float yaw = 0.0f; 
-						if(strList.size()==4)
+						if(uContains(poses, id-1))
-						{
-							yaw = uStr2Double(uReplaceChar(strList[3], ' ', ""));
-						}
-						else if(uContains(poses, id-1))
 						{
 							// set yaw depending on successive poses
 							Transform & previousPose = poses.at(id-1);
 							yaw = atan2(y-previousPose.y(),x-previousPose.x());
 							previousPose = Transform(previousPose.x(), previousPose.y(), yaw);
 						}
-						poses.insert(std::make_pair(id, Transform(x,y,0,0,0,yaw)));
+						Transform pose = Transform(x,y,0,0,0,yaw);
+						if(poses.size() == 0)
+						{
+							originPose = pose.inverse();
+						}
+						pose = originPose * pose; // transform in local coordinate where first value is the origin
+						poses.insert(std::make_pair(id, pose));
 					}
 					else
 					{
@@ -236,7 +325,7 @@ bool importPoses(
 				}
 				else
 				{
-					UERROR("Error parsing \"%s\" with GPS format (should have 3 values: stamp x y)", str.c_str());
+					UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y)", str.c_str());
 				}
 			}
 			else if(format == 1) // rgbd-slam format

corelib/src/RegistrationVis.cpp

@@ -698,8 +698,11 @@ Transform RegistrationVis::computeTransformationImpl(
 			transforms[1] = transforms[1].inverse();
 		}
 
-		UDEBUG("t1=%s", transforms[0].prettyPrint().c_str());
+		if(!_forwardEstimateOnly)
-		UDEBUG("t2=%s", transforms[1].prettyPrint().c_str());
+		{
+			UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
+			UDEBUG("from->from=%s", transforms[1].prettyPrint().c_str());
+		}
 		if(!transforms[1].isNull())
 		{
 			if(transforms[0].isNull())

corelib/src/StereoCameraModel.cpp

@@ -34,6 +34,123 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+StereoCameraModel::StereoCameraModel(
+		const std::string & name,
+		const cv::Size & imageSize1,
+		const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
+		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) :
+	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)
+{
+	UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
+	UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
+	UASSERT(E_.empty() || (E_.rows == 3 && E_.cols == 3 && E_.type() == CV_64FC1));
+	UASSERT(F_.empty() || (F_.rows == 3 && F_.cols == 3 && F_.type() == CV_64FC1));
+}
+
+StereoCameraModel::StereoCameraModel(
+		const std::string & name,
+		const CameraModel & leftCameraModel,
+		const CameraModel & rightCameraModel,
+		const cv::Mat & R,
+		const cv::Mat & T,
+		const cv::Mat & E,
+		const cv::Mat & F) :
+				left_(leftCameraModel),
+				right_(rightCameraModel),
+				name_(name),
+				R_(R),
+				T_(T),
+				E_(E),
+				F_(F)
+{
+	left_.setName(name+"_left");
+	right_.setName(name+"_right");
+	UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
+	UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
+	UASSERT(E_.empty() || (E_.rows == 3 && E_.cols == 3 && E_.type() == CV_64FC1));
+	UASSERT(F_.empty() || (F_.rows == 3 && F_.cols == 3 && F_.type() == CV_64FC1));
+
+	if(!R_.empty() && !T_.empty())
+	{
+		UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification());
+
+		cv::Mat R1,R2,P1,P2,Q;
+		cv::stereoRectify(left_.K(), left_.D(),
+				right_.K(), right_.D(),
+				left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
+				cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
+
+		left_ = CameraModel(left_.name(), left_.imageSize(), left_.K(), left_.D(), R1, P1, left_.localTransform());
+		right_ = CameraModel(right_.name(), right_.imageSize(), right_.K(), right_.D(), R2, P2, right_.localTransform());
+	}
+}
+
+StereoCameraModel::StereoCameraModel(
+		const std::string & name,
+		const CameraModel & leftCameraModel,
+		const CameraModel & rightCameraModel,
+		const Transform & extrinsics) :
+				left_(leftCameraModel),
+				right_(rightCameraModel),
+				name_(name)
+{
+	left_.setName(name+"_left");
+	right_.setName(name+"_right");
+
+	if(!extrinsics.isNull())
+	{
+		UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification());
+
+		extrinsics.rotationMatrix().convertTo(R_, CV_64FC1);
+		extrinsics.translationMatrix().convertTo(T_, CV_64FC1);
+
+		cv::Mat R1,R2,P1,P2,Q;
+		cv::stereoRectify(left_.K(), left_.D(),
+				right_.K(), right_.D(),
+				left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
+				cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
+
+		left_ = CameraModel(left_.name(), left_.imageSize(), left_.K(), left_.D(), R1, P1, left_.localTransform());
+		right_ = CameraModel(right_.name(), right_.imageSize(), right_.K(), right_.D(), R2, P2, right_.localTransform());
+	}
+}
+
+StereoCameraModel::StereoCameraModel(
+		double fx,
+		double fy,
+		double cx,
+		double cy,
+		double baseline,
+		const Transform & localTransform) :
+	left_(fx, fy, cx, cy, localTransform),
+	right_(fx, fy, cx, cy, localTransform, baseline*-fx)
+{
+}
+
+//minimal to be saved
+StereoCameraModel::StereoCameraModel(
+		const std::string & name,
+		double fx,
+		double fy,
+		double cx,
+		double cy,
+		double baseline,
+		const Transform & localTransform) :
+	left_(name+"_left", fx, fy, cx, cy, localTransform),
+	right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
+	name_(name)
+{
+}
+
 void StereoCameraModel::setName(const std::string & name)
 {
 	name_=name;
@@ -53,6 +170,8 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
 		//load rotation, translation
 		R_ = cv::Mat();
 		T_ = cv::Mat();
+		E_ = cv::Mat();
+		F_ = cv::Mat();
 
 		std::string filePath = directory+"/"+cameraName+"_pose.yaml";
 		if(UFile::exists(filePath))
@@ -60,44 +179,82 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
 			UINFO("Reading stereo calibration file \"%s\"", filePath.c_str());
 			cv::FileStorage fs(filePath, cv::FileStorage::READ);
 
-			name_ = (int)fs["camera_name"];
+			cv::FileNode n;
+
+			n = fs["camera_name"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				name_ = (int)n;
+			}
+			else
+			{
+				UWARN("Missing \"camera_name\" field in \"%s\"", filePath.c_str());
+			}
 
 			// import from ROS calibration format
-			cv::FileNode n = fs["rotation_matrix"];
+			n = fs["rotation_matrix"];
-			int rows = (int)n["rows"];
+			if(n.type() != cv::FileNode::NONE)
-			int cols = (int)n["cols"];
+			{
-			std::vector<double> data;
+				int rows = (int)n["rows"];
-			n["data"] >> data;
+				int cols = (int)n["cols"];
-			UASSERT(rows*cols == (int)data.size());
+				std::vector<double> data;
-			UASSERT(rows == 3 && cols == 3);
+				n["data"] >> data;
-			R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+				UASSERT(rows*cols == (int)data.size());
+				UASSERT(rows == 3 && cols == 3);
+				R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+			}
+			else
+			{
+				UWARN("Missing \"rotation_matrix\" field in \"%s\"", filePath.c_str());
+			}
 
-			n = fs["translation_matrix"];
+			if(n.type() != cv::FileNode::NONE)
-			rows = (int)n["rows"];
+			{
-			cols = (int)n["cols"];
+				n = fs["translation_matrix"];
-			data.clear();
+				int rows = (int)n["rows"];
-			n["data"] >> data;
+				int cols = (int)n["cols"];
-			UASSERT(rows*cols == (int)data.size());
+				std::vector<double> data;
-			UASSERT(rows == 3 && cols == 1);
+				n["data"] >> data;
-			T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+				UASSERT(rows*cols == (int)data.size());
+				UASSERT(rows == 3 && cols == 1);
+				T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+			}
+			else
+			{
+				UWARN("Missing \"translation_matrix\" field in \"%s\"", filePath.c_str());
+			}
 
-			n = fs["essential_matrix"];
+			if(n.type() != cv::FileNode::NONE)
-			rows = (int)n["rows"];
+			{
-			cols = (int)n["cols"];
+				n = fs["essential_matrix"];
-			data.clear();
+				int rows = (int)n["rows"];
-			n["data"] >> data;
+				int cols = (int)n["cols"];
-			UASSERT(rows*cols == (int)data.size());
+				std::vector<double> data;
-			UASSERT(rows == 3 && cols == 3);
+				n["data"] >> data;
-			E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+				UASSERT(rows*cols == (int)data.size());
+				UASSERT(rows == 3 && cols == 3);
+				E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+			}
+			else
+			{
+				UWARN("Missing \"essential_matrix\" field in \"%s\"", filePath.c_str());
+			}
 
 			n = fs["fundamental_matrix"];
-			rows = (int)n["rows"];
+			if(n.type() != cv::FileNode::NONE)
-			cols = (int)n["cols"];
+			{
-			data.clear();
+				int rows = (int)n["rows"];
-			n["data"] >> data;
+				int cols = (int)n["cols"];
-			UASSERT(rows*cols == (int)data.size());
+				std::vector<double> data;
-			UASSERT(rows == 3 && cols == 3);
+				n["data"] >> data;
-			F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+				UASSERT(rows*cols == (int)data.size());
+				UASSERT(rows == 3 && cols == 3);
+				F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+			}
+			else
+			{
+				UWARN("Missing \"fundamental_matrix\" field in \"%s\"", filePath.c_str());
+			}
 
 			fs.release();
 
@@ -119,43 +276,62 @@ bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTra
 			return true;
 		}
 		std::string filePath = directory+"/"+name_+"_pose.yaml";
-		if(!filePath.empty() && !name_.empty() && !R_.empty() && !T_.empty())
+		if(!filePath.empty() && (!R_.empty() && !T_.empty()))
 		{
 			UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
 			cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
 
 			// export in ROS calibration format
 
-			fs << "camera_name" << name_;
+			if(!name_.empty())
+			{
+				fs << "camera_name" << name_;
+			}
 
-			fs << "rotation_matrix" << "{";
+			if(!R_.empty())
-			fs << "rows" << R_.rows;
+			{
-			fs << "cols" << R_.cols;
+				fs << "rotation_matrix" << "{";
-			fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
+				fs << "rows" << R_.rows;
-			fs << "}";
+				fs << "cols" << R_.cols;
+				fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
+				fs << "}";
+			}
 
-			fs << "translation_matrix" << "{";
+			if(!T_.empty())
-			fs << "rows" << T_.rows;
+			{
-			fs << "cols" << T_.cols;
+				fs << "translation_matrix" << "{";
-			fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
+				fs << "rows" << T_.rows;
-			fs << "}";
+				fs << "cols" << T_.cols;
+				fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
+				fs << "}";
+			}
 
-			fs << "essential_matrix" << "{";
+			if(!E_.empty())
-			fs << "rows" << E_.rows;
+			{
-			fs << "cols" << E_.cols;
+				fs << "essential_matrix" << "{";
-			fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
+				fs << "rows" << E_.rows;
-			fs << "}";
+				fs << "cols" << E_.cols;
+				fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
+				fs << "}";
+			}
 
-			fs << "fundamental_matrix" << "{";
+			if(!F_.empty())
-			fs << "rows" << F_.rows;
+			{
-			fs << "cols" << F_.cols;
+				fs << "fundamental_matrix" << "{";
-			fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
+				fs << "rows" << F_.rows;
-			fs << "}";
+				fs << "cols" << F_.cols;
+				fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
+				fs << "}";
+			}
 
 			fs.release();
 
 			return true;
 		}
+		else
+		{
+			UERROR("Failed saving stereo extrinsics (they are null).");
+		}
 	}
 	return false;
 }

corelib/src/Transform.cpp

@@ -180,6 +180,16 @@ Transform Transform::to3DoF() const
 	return Transform(x,y,0, 0,0,yaw);
 }
 
+cv::Mat Transform::rotationMatrix() const
+{
+	return data_.colRange(0, 3).clone();
+}
+
+cv::Mat Transform::translationMatrix() const
+{
+	return data_.col(3).clone();
+}
+
 void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
 {
 	pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
@@ -404,4 +414,17 @@ Transform Transform::fromString(const std::string & string)
 	return t;
 }
 
+/**
+ * Format (3 values): x y z
+ * Format (6 values): x y z roll pitch yaw
+ * Format (7 values): x y z qx qy qz qw
+ * Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33
+ * Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
+ */
+bool Transform::canParseString(const std::string & string)
+{
+	std::list<std::string> list = uSplit(string, ' ');
+	return list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12;
+}
+
 }

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -73,6 +73,7 @@ private slots:
 	void restart();
 	void calibrate();
 	bool save();
+	void unlock();
 
 protected:
 	virtual void closeEvent(QCloseEvent* event);

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -61,6 +61,7 @@ class Signature;
 class LoopClosureViewer;
 class Camera;
 class CalibrationDialog;
+class CreateSimpleCalibrationDialog;
 
 class RTABMAPGUI_EXP PreferencesDialog : public QDialog
 {
@@ -125,7 +126,7 @@ public:
 	void saveCustomConfig(const QString & section, const QString & key, const QString & value);
 	QString loadCustomConfig(const QString & section, const QString & key);
 
-	const rtabmap::ParametersMap & getAllParameters() const;
+	rtabmap::ParametersMap getAllParameters() const;
 
 	//General panel
 	int getGeneralLoggerLevel() const;
@@ -330,6 +331,7 @@ private:
 
 	//calibration
 	CalibrationDialog * _calibrationDialog;
+	CreateSimpleCalibrationDialog * _createCalibrationDialog;
 
 	QVector<QCheckBox*> _3dRenderingShowClouds;
 	QVector<QDoubleSpinBox*> _3dRenderingVoxelSize;

guilib/src/CalibrationDialog.cpp

@@ -75,6 +75,7 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector
 	connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
 	connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save()));
 	connect(ui_->checkBox_switchImages, SIGNAL(stateChanged(int)), this, SLOT(restart()));
+	connect(ui_->checkBox_unlock, SIGNAL(stateChanged(int)), SLOT(unlock()));
 
 	connect(ui_->spinBox_boardWidth, SIGNAL(valueChanged(int)), this, SLOT(setBoardWidth(int)));
 	connect(ui_->spinBox_boardHeight, SIGNAL(valueChanged(int)), this, SLOT(setBoardHeight(int)));
@@ -506,27 +507,27 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 
 	if(!stereo_ && readyToCalibrate[0])
 	{
-		ui_->pushButton_calibrate->setEnabled(true);
+		unlock();
 	}
 	else if(stereo_ && readyToCalibrate[0] && readyToCalibrate[1] && stereoImagePoints_[0].size())
 	{
-		ui_->pushButton_calibrate->setEnabled(true);
+		unlock();
 	}
 
 	if(ui_->radioButton_rectified->isChecked())
 	{
-		if(models_[0].isValid())
+		if(models_[0].isValidForRectification())
 		{
 			images[0] = models_[0].rectifyImage(images[0]);
 		}
-		if(models_[1].isValid())
+		if(models_[1].isValidForRectification())
 		{
 			images[1] = models_[1].rectifyImage(images[1]);
 		}
 	}
 	else if(ui_->radioButton_stereoRectified->isChecked() &&
-			(stereoModel_.left().isValid() &&
+			(stereoModel_.left().isValidForRectification() &&
-			stereoModel_.right().isValid()&&
+			stereoModel_.right().isValidForRectification()&&
 			(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)))
 	{
 		images[0] = stereoModel_.left().rectifyImage(images[0]);
@@ -576,7 +577,7 @@ void CalibrationDialog::restart()
 	minIrs_[1] = 0x0000;
 	maxIrs_[1] = 0x7fff;
 
-	ui_->pushButton_calibrate->setEnabled(false);
+	ui_->pushButton_calibrate->setEnabled(ui_->checkBox_unlock->isChecked());
 	ui_->pushButton_save->setEnabled(false);
 	ui_->radioButton_raw->setChecked(true);
 	ui_->radioButton_rectified->setEnabled(false);
@@ -617,6 +618,11 @@ void CalibrationDialog::restart()
 	ui_->lineEdit_P_2->clear();
 }
 
+void CalibrationDialog::unlock()
+{
+	ui_->pushButton_calibrate->setEnabled(true);
+}
+
 void CalibrationDialog::calibrate()
 {
 	processingData_ = true;
@@ -845,17 +851,17 @@ void CalibrationDialog::calibrate()
 	}
 
 	if(stereo_ &&
-		stereoModel_.left().isValid() &&
+		stereoModel_.isValidForRectification())
-		stereoModel_.right().isValid()&&
-		(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0))
 	{
+		stereoModel_.initRectificationMap();
 		ui_->radioButton_rectified->setEnabled(true);
 		ui_->radioButton_stereoRectified->setEnabled(true);
 		ui_->radioButton_stereoRectified->setChecked(true);
 		ui_->pushButton_save->setEnabled(true);
 	}
-	else if(models_[0].isValid())
+	else if(models_[0].isValidForRectification())
 	{
+		models_[0].initRectificationMap();
 		ui_->radioButton_rectified->setEnabled(true);
 		ui_->radioButton_rectified->setChecked(true);
 		ui_->pushButton_save->setEnabled(!stereo_);

guilib/src/CameraViewer.cpp

@@ -165,8 +165,9 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 		}
 	}
 
-	cloudView_->setVisible(showCloudCheckbox_->isEnabled() || showScanCheckbox_->isEnabled());
+	cloudView_->setVisible((showCloudCheckbox_->isEnabled() && showCloudCheckbox_->isChecked()) ||
-	if(showCloudCheckbox_->isEnabled() || showScanCheckbox_->isEnabled())
+						   (showScanCheckbox_->isEnabled() && showScanCheckbox_->isChecked()));
+	if(cloudView_->isVisible())
 	{
 		cloudView_->update();
 	}

guilib/src/CreateSimpleCalibrationDialog.cpp

@@ -46,16 +46,49 @@ CreateSimpleCalibrationDialog::CreateSimpleCalibrationDialog(
 	savingFolder_(savingFolder),
 	cameraName_(cameraName)
 {
+	if(cameraName_.isEmpty())
+	{
+		cameraName_ = "calib";
+	}
+
 	ui_ = new Ui_createSimpleCalibrationDialog();
 	ui_->setupUi(this);
 
 	connect(ui_->buttonBox->button(QDialogButtonBox::Save), SIGNAL(clicked()), this, SLOT(saveCalibration()));
 	connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
 
+	connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), ui_->stackedWidget, SLOT(setCurrentIndex(int)));
+
+	connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateStereoView()));
+	connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateSaveStatus()));
+	connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSaveStatus()));
+
 	connect(ui_->doubleSpinBox_fx, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
 	connect(ui_->doubleSpinBox_fy, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
 
+	connect(ui_->doubleSpinBox_fx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_fy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_cx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_cy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->lineEdit_D_l, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->doubleSpinBox_fx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_fy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_cx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_cy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->lineEdit_D_r, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->spinBox_width, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
+	connect(ui_->spinBox_height, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->doubleSpinBox_baseline, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->lineEdit_RT, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
+
+	ui_->stackedWidget->setCurrentIndex(ui_->comboBox_advanced->currentIndex());
+
 	ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(false);
+
+	updateStereoView();
 }
 
 CreateSimpleCalibrationDialog::~CreateSimpleCalibrationDialog()
@@ -63,29 +96,189 @@ CreateSimpleCalibrationDialog::~CreateSimpleCalibrationDialog()
 	delete ui_;
 }
 
+void CreateSimpleCalibrationDialog::updateStereoView()
+{
+	bool checked = ui_->checkBox_stereo->isChecked();
+	ui_->doubleSpinBox_baseline->setVisible(checked);
+	ui_->label_baseline->setVisible(checked);
+	ui_->label_right->setVisible(checked);
+	ui_->doubleSpinBox_fx_r->setVisible(checked);
+	ui_->doubleSpinBox_fy_r->setVisible(checked);
+	ui_->doubleSpinBox_cx_r->setVisible(checked);
+	ui_->doubleSpinBox_cy_r->setVisible(checked);
+	ui_->lineEdit_D_r->setVisible(checked);
+	ui_->groupBox_stereo_extrinsics->setVisible(checked);
+	ui_->label_left->setVisible(checked);
+}
+
 void CreateSimpleCalibrationDialog::updateSaveStatus()
 {
-	ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(ui_->doubleSpinBox_fx->value() > 0.0 && ui_->doubleSpinBox_fy->value() > 0.0);
+	bool valid = false;
+	if(ui_->comboBox_advanced->currentIndex() == 0 &&
+	   ui_->doubleSpinBox_fx->value() > 0.0 &&
+	   ui_->doubleSpinBox_fy->value() > 0.0 &&
+	   (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_baseline->value() != 0.0))
+	{
+		// basic
+		valid = true;
+	}
+	else if(ui_->comboBox_advanced->currentIndex() == 1 &&
+			 ui_->doubleSpinBox_fx_l->value() > 0.0 &&
+			 ui_->doubleSpinBox_fy_l->value() > 0.0 &&
+			 ui_->doubleSpinBox_cx_l->value() > 0.0 &&
+			 ui_->doubleSpinBox_cy_l->value() > 0.0 &&
+			 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fx_r->value() > 0.0) &&
+			 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fy_r->value() > 0.0) &&
+			 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cx_r->value() > 0.0) &&
+			 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cy_r->value() > 0.0) &&
+			 ui_->spinBox_width->value() > 0 &&
+			 ui_->spinBox_height->value() > 0 &&
+			 !ui_->lineEdit_D_l->text().isEmpty() &&
+			 (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_D_r->text().isEmpty()) &&
+			 (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_RT->text().isEmpty()))
+	{
+		//advanced
+		QStringList distorsionsStrListL = ui_->lineEdit_D_l->text().trimmed().split(' ');
+		QStringList distorsionsStrListR = ui_->lineEdit_D_r->text().trimmed().split(' ');
+
+		if((distorsionsStrListL.size() == 4 || distorsionsStrListL.size() == 5 || distorsionsStrListL.size() == 8) &&
+				(!ui_->checkBox_stereo->isChecked() || distorsionsStrListL.size() == 4 || distorsionsStrListL.size() == 5 || distorsionsStrListL.size() == 8) &&
+				(!ui_->checkBox_stereo->isChecked() || Transform::canParseString(ui_->lineEdit_RT->text().trimmed().toStdString())))
+		{
+			valid = true;
+		}
+	}
+	ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(valid);
 }
 
 void CreateSimpleCalibrationDialog::saveCalibration()
 {
-	if(ui_->doubleSpinBox_baseline->value()==0)
+	QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_+"/"+cameraName_+".yaml", "*.yaml");
+	QString name = QFileInfo(filePath).baseName();
+	QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
+	if(!filePath.isEmpty())
 	{
-		QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_+"/"+cameraName_+".yaml", "*.yaml");
+		cameraName_ = name;
-		QString name = QFileInfo(filePath).baseName();
+		CameraModel modelLeft;
-		QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
+		float width = ui_->spinBox_width->value();
-		if(!filePath.isEmpty())
+		float height = ui_->spinBox_height->value();
+		if(ui_->comboBox_advanced->currentIndex() == 0)
 		{
-			cameraName_ = name;
+			//basic
-			CameraModel model(
+			modelLeft = CameraModel(
 					name.toStdString(),
 					ui_->doubleSpinBox_fx->value(),
 					ui_->doubleSpinBox_fy->value(),
 					ui_->doubleSpinBox_cx->value(),
 					ui_->doubleSpinBox_cy->value());
-			UASSERT(model.isValid());
+			UASSERT(modelLeft.isValid());
-			if(model.save(dir.toStdString()))
+		}
+		else
+		{
+			//advanced
+			cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+			K.at<double>(0,0) = ui_->doubleSpinBox_fx_l->value();
+			K.at<double>(1,1) = ui_->doubleSpinBox_fy_l->value();
+			K.at<double>(0,2) = ui_->doubleSpinBox_cx_l->value();
+			K.at<double>(1,2) = ui_->doubleSpinBox_cy_l->value();
+			cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+			cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
+			K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
+
+			QStringList distorsionCoeffs = ui_->lineEdit_D_l->text().trimmed().split(' ');
+			UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
+			cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
+			bool ok;
+			for(int i=0; i<distorsionCoeffs.size(); ++i)
+			{
+				D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
+				if(!ok)
+				{
+					QMessageBox::warning(this, tr("Save"), tr("Error parsing left distorsion coefficients \"%1\".").arg(ui_->lineEdit_D_l->text()));
+					return;
+				}
+			}
+
+			modelLeft = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
+			UASSERT(modelLeft.isValidForRectification());
+		}
+
+		if(ui_->checkBox_stereo->isChecked())
+		{
+			StereoCameraModel stereoModel;
+			if(ui_->comboBox_advanced->currentIndex() == 0)
+			{
+				CameraModel modelRight(
+						name.toStdString(),
+						ui_->doubleSpinBox_fx->value(),
+						ui_->doubleSpinBox_fy->value(),
+						ui_->doubleSpinBox_cx->value(),
+						ui_->doubleSpinBox_cy->value(),
+						Transform::getIdentity(),
+						ui_->doubleSpinBox_baseline->value()*-ui_->doubleSpinBox_fx->value());
+				UASSERT(modelRight.isValid());
+				stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform());
+			}
+			else if(ui_->comboBox_advanced->currentIndex() == 1)
+			{
+				CameraModel modelRight;
+				cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+				K.at<double>(0,0) = ui_->doubleSpinBox_fx_r->value();
+				K.at<double>(1,1) = ui_->doubleSpinBox_fy_r->value();
+				K.at<double>(0,2) = ui_->doubleSpinBox_cx_r->value();
+				K.at<double>(1,2) = ui_->doubleSpinBox_cy_r->value();
+				cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+				cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
+				K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
+
+				QStringList distorsionCoeffs = ui_->lineEdit_D_r->text().trimmed().split(' ');
+				UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
+				cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
+				bool ok;
+				for(int i=0; i<distorsionCoeffs.size(); ++i)
+				{
+					D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
+					if(!ok)
+					{
+						QMessageBox::warning(this, tr("Save"), tr("Error parsing right distorsion coefficients \"%1\".").arg(ui_->lineEdit_D_l->text()));
+						return;
+					}
+				}
+
+				modelRight = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
+				UASSERT(modelRight.isValidForRectification());
+
+				UASSERT(Transform::canParseString(ui_->lineEdit_RT->text().trimmed().toStdString()));
+				stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform::fromString(ui_->lineEdit_RT->text().toStdString()));
+			}
+
+			std::string base = (dir+QDir::separator()+name).toStdString();
+			std::string leftPath = base+"_left.yaml";
+			std::string rightPath = base+"_right.yaml";
+
+			if(stereoModel.save(dir.toStdString(), ui_->comboBox_advanced->currentIndex() != 1))
+			{
+				if(ui_->comboBox_advanced->currentIndex() == 0)
+				{
+					QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\".").
+							arg(leftPath.c_str()).arg(rightPath.c_str()));
+				}
+				else
+				{
+					std::string posePath = base+"_pose.yaml";
+					QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\"\n  \"%3\".").
+							arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
+				}
+				this->accept();
+			}
+			else
+			{
+				QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\" and \"%2\"").arg(leftPath.c_str()).arg(rightPath.c_str()));
+			}
+		}
+		else
+		{
+			if(modelLeft.save(dir.toStdString()))
 			{
 				QMessageBox::information(this, tr("Save"), tr("Calibration file saved to \"%1\".").arg(filePath));
 				this->accept();
@@ -96,42 +289,6 @@ void CreateSimpleCalibrationDialog::saveCalibration()
 			}
 		}
 	}
-	else
-	{
-		QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_ + "/" + cameraName_, "*.yaml");
-		QString name = QFileInfo(filePath).baseName();
-		QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
-		if(!name.isEmpty())
-		{
-			cameraName_ = name;
-			std::string base = (dir+QDir::separator()+name).toStdString();
-			std::string leftPath = base+"_left.yaml";
-			std::string rightPath = base+"_right.yaml";
-			std::string posePath = base+"_pose.yaml";
-
-			StereoCameraModel model(
-					name.toStdString(),
-					ui_->doubleSpinBox_fx->value(),
-					ui_->doubleSpinBox_fy->value(),
-					ui_->doubleSpinBox_cx->value(),
-					ui_->doubleSpinBox_cy->value(),
-					ui_->doubleSpinBox_baseline->value());
-			UASSERT(model.left().isValid() &&
-					model.right().isValid()&&
-					model.baseline() > 0.0);
-
-			if(model.save(dir.toStdString(), true))
-			{
-				QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\"\n  \"%3\".").
-						arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
-				this->accept();
-			}
-			else
-			{
-				QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\" and \"%2\"").arg(leftPath.c_str()).arg(rightPath.c_str()));
-			}
-		}
-	}
 }
 
 }

guilib/src/CreateSimpleCalibrationDialog.h

@@ -47,7 +47,11 @@ public:
 
 	virtual ~CreateSimpleCalibrationDialog();
 
+	void setCameraInfoDir(const QString & folder) {savingFolder_ = folder;}
+	const QString & cameraName() const {return cameraName_;}
+
 private slots:
+	void updateStereoView();
 	void updateSaveStatus();
 	void saveCalibration();
 

guilib/src/PreferencesDialog.cpp

@@ -92,7 +92,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui(0),
 	_indexModel(0),
 	_initialized(false),
-	_calibrationDialog(new CalibrationDialog(false, ".", this))
+	_calibrationDialog(new CalibrationDialog(false, ".", this)),
+	_createCalibrationDialog(new CreateSimpleCalibrationDialog(".", "", this))
 {
 	ULOGGER_DEBUG("");
 	_calibrationDialog->setWindowFlags(Qt::Window);
@@ -354,6 +355,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_images_spinBox_startPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_images_refreshDir, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_rgbImages_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_cameraImages_bayerMode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	//video group
 	connect(_ui->source_video_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceVideoPath()));
 	connect(_ui->source_video_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1142,6 +1144,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->source_images_spinBox_startPos->setValue(0);
 		_ui->source_images_refreshDir->setChecked(false);
 		_ui->checkBox_rgbImages_rectify->setChecked(false);
+		_ui->comboBox_cameraImages_bayerMode->setCurrentIndex(0);
 		_ui->checkBox_rgbVideo_rectify->setChecked(false);
 
 		_ui->source_checkBox_ignoreOdometry->setChecked(false);
@@ -1523,6 +1526,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_images_spinBox_startPos->setValue(settings.value("startPos",_ui->source_images_spinBox_startPos->value()).toInt());
 	_ui->source_images_refreshDir->setChecked(settings.value("refreshDir",_ui->source_images_refreshDir->isChecked()).toBool());
 	_ui->checkBox_rgbImages_rectify->setChecked(settings.value("rectify",_ui->checkBox_rgbImages_rectify->isChecked()).toBool());
+	_ui->comboBox_cameraImages_bayerMode->setCurrentIndex(settings.value("bayerMode",_ui->comboBox_cameraImages_bayerMode->currentIndex()).toInt());
 
 	_ui->checkBox_cameraImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_cameraImages_timestamps->isChecked()).toBool());
 	_ui->checkBox_cameraImages_syncTimeStamps->setChecked(settings.value("sync_stamps",_ui->checkBox_cameraImages_syncTimeStamps->isChecked()).toBool());
@@ -1895,6 +1899,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("startPos", 		_ui->source_images_spinBox_startPos->value());
 	settings.setValue("refreshDir", 	_ui->source_images_refreshDir->isChecked());
 	settings.setValue("rectify", 	    _ui->checkBox_rgbImages_rectify->isChecked());
+	settings.setValue("bayerMode", 	    _ui->comboBox_cameraImages_bayerMode->currentIndex());
 	settings.setValue("filenames_as_stamps", _ui->checkBox_cameraImages_timestamps->isChecked());
 	settings.setValue("sync_stamps",    _ui->checkBox_cameraImages_syncTimeStamps->isChecked());
 	settings.setValue("stamps",              _ui->lineEdit_cameraImages_timestamps->text());
@@ -2404,11 +2409,15 @@ QString PreferencesDialog::loadCustomConfig(const QString & section, const QStri
 	return value;
 }
 
-const rtabmap::ParametersMap & PreferencesDialog::getAllParameters() const
+rtabmap::ParametersMap PreferencesDialog::getAllParameters() const
 {
 	UASSERT_MSG(_parameters.size() == Parameters::getDefaultParameters().size(),
 			uFormat("%d vs %d (Is PreferencesDialog::init() called?)", (int)_parameters.size(), (int)Parameters::getDefaultParameters().size()).c_str());
-	return _parameters;
+
+	ParametersMap parameters = _parameters;
+	uInsert(parameters, _modifiedParameters);
+
+	return parameters;
 }
 
 void PreferencesDialog::selectSourceDriver(Src src)
@@ -2497,6 +2506,10 @@ void PreferencesDialog::selectCalibrationPath()
 	{
 		dir = getWorkingDirectory()+"/camera_info";
 	}
+	else if(!dir.contains('.'))
+	{
+		dir = getWorkingDirectory()+"/camera_info/"+dir;
+	}
 	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Calibration file (*.yaml)"));
 	if(path.size())
 	{
@@ -2568,7 +2581,7 @@ void PreferencesDialog::selectSourceImagesPathGt()
 	{
 		dir = getWorkingDirectory();
 	}
-	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Ground Truth (*.txt)"));
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Ground Truth (*.txt *.log *.toro *.g2o)"));
 	if(path.size())
 	{
 		QStringList list;
@@ -2691,7 +2704,6 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 	QWidget * obj = _ui->stackedWidget->findChild<QWidget*>(key.c_str());
 	if(obj)
 	{
-		bool oldState = obj->blockSignals(true);
 		QSpinBox * spin = qobject_cast<QSpinBox *>(obj);
 		QDoubleSpinBox * doubleSpin = qobject_cast<QDoubleSpinBox *>(obj);
 		QComboBox * combo = qobject_cast<QComboBox *>(obj);
@@ -2799,7 +2811,6 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 		{
 			ULOGGER_WARN("QObject called %s can't be cast to a supported widget", key.c_str());
 		}
-		obj->blockSignals(oldState);
 	}
 	else
 	{
@@ -2866,79 +2877,6 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 			// Add parameter
 			UDEBUG("modify param %s=%s", object->objectName().toStdString().c_str(), uNumber2Str(value).c_str());
 			uInsert(_modifiedParameters, rtabmap::ParametersPair(object->objectName().toStdString(), QString::number(value).toStdString()));
-
-			if(comboBox)
-			{
-				// Add related panels to parameters
-				if(comboBox == _ui->comboBox_vh_strategy)
-				{
-					if(value == 0) // 0 none
-					{
-						// No panel related...
-					}
-					else if(value == 1) // 2 epipolar
-					{
-						this->addParameters(_ui->groupBox_vh_epipolar2);
-					}
-				}
-				else if(comboBox == _ui->comboBox_detector_strategy ||
-						comboBox == _ui->reextract_type)
-				{
-					if(value == 0) //  surf
-					{
-						this->addParameters(_ui->groupBox_detector_surf2);
-					}
-					else if(value == 1) //  sift
-					{
-						this->addParameters(_ui->groupBox_detector_sift2);
-					}
-					else if(value == 2) //  orb
-					{
-						this->addParameters(_ui->groupBox_detector_orb2);
-					}
-					else if(value == 3) //  fast+freak
-					{
-						this->addParameters(_ui->groupBox_detector_fast2);
-						this->addParameters(_ui->groupBox_detector_freak2);
-					}
-					else if(value == 4) //  fast+brief
-					{
-						this->addParameters(_ui->groupBox_detector_fast2);
-						this->addParameters(_ui->groupBox_detector_brief2);
-					}
-					else if(value == 5) //  gftt+freak
-					{
-						this->addParameters(_ui->groupBox_detector_gftt2);
-						this->addParameters(_ui->groupBox_detector_freak2);
-					}
-					else if(value == 6) //  gftt+brief
-					{
-						this->addParameters(_ui->groupBox_detector_gftt2);
-						this->addParameters(_ui->groupBox_detector_brief2);
-					}
-					else if(value == 7) //  brisk
-					{
-						this->addParameters(_ui->groupBox_detector_brisk2);
-					}
-				}
-				else if(comboBox == _ui->loopClosure_estimationType)
-				{
-					this->addParameters(_ui->stackedWidget_loopClosureEstimation, _ui->loopClosure_estimationType->currentIndex());
-				}
-				else if(comboBox == _ui->graphOptimization_type)
-				{
-					this->addParameter(_ui->graphOptimization_iterations,        _ui->graphOptimization_iterations->value());
-					this->addParameter(_ui->graphOptimization_covarianceIgnored, _ui->graphOptimization_covarianceIgnored->isChecked());
-					this->addParameter(_ui->graphOptimization_slam2d,            _ui->graphOptimization_slam2d->isChecked());
-					this->addParameter(_ui->graphOptimization_stopEpsilon,       _ui->graphOptimization_stopEpsilon->value());
-					this->addParameter(_ui->graphOptimization_robust,            _ui->graphOptimization_robust->isChecked());
-				}
-				else if(comboBox == _ui->comboBox_registrationStrategy)
-				{
-					this->addParameters(_ui->groupBox_visualTransform2);
-					this->addParameters(_ui->groupBox_icp2);
-				}
-			}
 		}
 		else
 		{
@@ -2964,23 +2902,6 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
 			// Add parameter
 			UDEBUG("modify param %s=%s", object->objectName().toStdString().c_str(), uBool2Str(value).c_str());
 			uInsert(_modifiedParameters, rtabmap::ParametersPair(object->objectName().toStdString(), uBool2Str(value)));
-
-			// RGBD panel
-			if(value && checkbox == _ui->general_checkBox_activateRGBD)
-			{
-				// add all RGBD parameters!
-				this->addParameters(_ui->groupBox_slam_update);
-				this->addParameters(_ui->groupBox_graphOptimization2);
-				this->addParameters(_ui->groupBox_proximityDetection2);
-				this->addParameters(_ui->groupBox_visualTransform2);
-				this->addParameters(_ui->groupBox_icp2);
-				this->addParameters(_ui->groupBox_pathPlanning2);
-			}
-
-			if(groupBox)
-			{
-				this->addParameters(groupBox);
-			}
 		}
 		else
 		{
@@ -3825,6 +3746,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 			_ui->doubleSpinBox_cameraRGBDImages_scale->value(),
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
+		((CameraRGBDImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
 		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
 		((CameraRGBDImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -3865,9 +3787,10 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 		camera = new CameraStereoImages(
 			_ui->lineEdit_cameraStereoImages_path_left->text().append(QDir::separator()).toStdString(),
 			_ui->lineEdit_cameraStereoImages_path_right->text().append(QDir::separator()).toStdString(),
-			_ui->checkBox_stereoImages_rectify->isChecked(),
+			_ui->checkBox_stereoImages_rectify->isChecked() && !useRawImages,
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
+		((CameraStereoImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
 		((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
 		((CameraStereoImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -3885,7 +3808,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 	{
 		camera = new CameraStereoVideo(
 			_ui->lineEdit_cameraStereoVideo_path->text().toStdString(),
-			_ui->checkBox_stereoVideo_rectify->isChecked(),
+			_ui->checkBox_stereoVideo_rectify->isChecked() && !useRawImages,
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
 	}
@@ -3900,7 +3823,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 	{
 		camera = new CameraVideo(
 			_ui->source_video_lineEdit_path->text().toStdString(),
-			_ui->checkBox_rgbVideo_rectify->isChecked(),
+			_ui->checkBox_rgbVideo_rectify->isChecked() && !useRawImages,
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
 	}
@@ -3913,23 +3836,24 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 
 		((CameraImages*)camera)->setStartIndex(_ui->source_images_spinBox_startPos->value());
 		((CameraImages*)camera)->setDirRefreshed(_ui->source_images_refreshDir->isChecked());
-		((CameraImages*)camera)->setImagesRectified(_ui->checkBox_rgbImages_rectify->isChecked());
+		((CameraImages*)camera)->setImagesRectified(_ui->checkBox_rgbImages_rectify->isChecked() && !useRawImages);
 
-		((CameraRGBDImages*)camera)->setGroundTruthPath(
+		((CameraImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
+		((CameraImages*)camera)->setGroundTruthPath(
 				_ui->lineEdit_cameraImages_gt->text().toStdString(),
 				_ui->comboBox_cameraImages_gtFormat->currentIndex());
-		((CameraRGBDImages*)camera)->setScanPath(
+		((CameraImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
 						_ui->spinBox_cameraImages_max_scan_pts->value(),
 						_ui->spinBox_cameraImages_scanDownsampleStep->value(),
 						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 						_ui->spinBox_cameraImages_scanNormalsK->value(),
 						this->getLaserLocalTransform());
-		((CameraRGBDImages*)camera)->setDepthFromScan(
+		((CameraImages*)camera)->setDepthFromScan(
 				_ui->groupBox_depthFromScan->isChecked(),
 				_ui->checkBox_depthFromScan_vertical->isChecked(),
 				_ui->checkBox_depthFromScan_fillBorders->isChecked());
-		((CameraRGBDImages*)camera)->setTimestamps(
+		((CameraImages*)camera)->setTimestamps(
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
 				_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
 				_ui->checkBox_cameraImages_syncTimeStamps->isChecked());
@@ -4303,8 +4227,11 @@ void PreferencesDialog::calibrate()
 
 void PreferencesDialog::calibrateSimple()
 {
-	CreateSimpleCalibrationDialog dialog(this->getCameraInfoDir(), "", this);
+	_createCalibrationDialog->setCameraInfoDir(this->getCameraInfoDir());
-	dialog.exec();
+	if(_createCalibrationDialog->exec() == QMessageBox::Accepted)
+	{
+		_ui->lineEdit_calibrationFile->setText(_createCalibrationDialog->cameraName());
+	}
 }
 
 }

guilib/src/ui/calibrationDialog.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1314</width>
-    <height>795</height>
+    <height>860</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -404,11 +404,22 @@
         </widget>
        </item>
        <item>
-        <widget class="QPushButton" name="pushButton_calibrate">
+        <layout class="QHBoxLayout" name="horizontalLayout_5" stretch="1,0">
-         <property name="text">
+         <item>
-          <string>Calibrate</string>
+          <widget class="QPushButton" name="pushButton_calibrate">
-         </property>
+           <property name="text">
-        </widget>
+            <string>Calibrate</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QCheckBox" name="checkBox_unlock">
+           <property name="text">
+            <string>Unlock</string>
+           </property>
+          </widget>
+         </item>
+        </layout>
        </item>
        <item>
         <widget class="QGroupBox" name="groupBox_3">
@@ -707,6 +718,9 @@
     </widget>
    </item>
   </layout>
+  <zorder>buttonBox</zorder>
+  <zorder>groupBox</zorder>
+  <zorder></zorder>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/createSimpleCalibrationDialog.ui

@@ -6,106 +6,399 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>306</width>
+    <width>546</width>
-    <height>212</height>
+    <height>568</height>
    </rect>
   </property>
   <property name="windowTitle">
    <string>Create simple calibration</string>
   </property>
-  <layout class="QVBoxLayout" name="verticalLayout">
+  <layout class="QVBoxLayout" name="verticalLayout_3">
    <item>
-    <layout class="QFormLayout" name="formLayout">
+    <layout class="QHBoxLayout" name="horizontalLayout">
-     <item row="0" column="0">
+     <item>
-      <widget class="QDoubleSpinBox" name="doubleSpinBox_fx">
+      <widget class="QComboBox" name="comboBox_advanced">
-       <property name="decimals">
+       <item>
-        <number>6</number>
+        <property name="text">
-       </property>
+         <string>Basic (images are already rectified)</string>
-       <property name="maximum">
+        </property>
-        <double>99999.000000000000000</double>
+       </item>
-       </property>
+       <item>
+        <property name="text">
+         <string>Advanced (images should be rectified)</string>
+        </property>
+       </item>
       </widget>
      </item>
-     <item row="0" column="1">
+     <item>
-      <widget class="QLabel" name="label">
+      <widget class="QCheckBox" name="checkBox_stereo">
        <property name="text">
-        <string>fx</string>
+        <string>Stereo</string>
-       </property>
-      </widget>
-     </item>
-     <item row="1" column="0">
-      <widget class="QDoubleSpinBox" name="doubleSpinBox_fy">
-       <property name="decimals">
-        <number>6</number>
-       </property>
-       <property name="maximum">
-        <double>99999.000000000000000</double>
-       </property>
-      </widget>
-     </item>
-     <item row="1" column="1">
-      <widget class="QLabel" name="label_2">
-       <property name="text">
-        <string>fy</string>
-       </property>
-      </widget>
-     </item>
-     <item row="2" column="0">
-      <widget class="QDoubleSpinBox" name="doubleSpinBox_cx">
-       <property name="decimals">
-        <number>6</number>
-       </property>
-       <property name="maximum">
-        <double>99999.000000000000000</double>
-       </property>
-      </widget>
-     </item>
-     <item row="2" column="1">
-      <widget class="QLabel" name="label_3">
-       <property name="text">
-        <string>cx</string>
-       </property>
-      </widget>
-     </item>
-     <item row="3" column="0">
-      <widget class="QDoubleSpinBox" name="doubleSpinBox_cy">
-       <property name="decimals">
-        <number>6</number>
-       </property>
-       <property name="maximum">
-        <double>99999.000000000000000</double>
-       </property>
-      </widget>
-     </item>
-     <item row="3" column="1">
-      <widget class="QLabel" name="label_4">
-       <property name="text">
-        <string>cy</string>
-       </property>
-      </widget>
-     </item>
-     <item row="4" column="0">
-      <widget class="QDoubleSpinBox" name="doubleSpinBox_baseline">
-       <property name="decimals">
-        <number>6</number>
-       </property>
-       <property name="minimum">
-        <double>0.000000000000000</double>
-       </property>
-       <property name="maximum">
-        <double>99999.000000000000000</double>
-       </property>
-      </widget>
-     </item>
-     <item row="4" column="1">
-      <widget class="QLabel" name="label_5">
-       <property name="text">
-        <string>baseline (only for stereo)</string>
        </property>
       </widget>
      </item>
     </layout>
    </item>
+   <item>
+    <widget class="QStackedWidget" name="stackedWidget">
+     <property name="currentIndex">
+      <number>1</number>
+     </property>
+     <widget class="QWidget" name="page_2">
+      <layout class="QVBoxLayout" name="verticalLayout">
+       <item>
+        <widget class="QGroupBox" name="groupBox">
+         <property name="title">
+          <string>Camera Intrinsics</string>
+         </property>
+         <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
+          <item row="1" column="1">
+           <widget class="QLabel" name="label_2">
+            <property name="text">
+             <string>fy</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_fx">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="1">
+           <widget class="QLabel" name="label_baseline">
+            <property name="text">
+             <string>baseline (m)</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QLabel" name="label_3">
+            <property name="text">
+             <string>cx</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_fy">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="1">
+           <widget class="QLabel" name="label_4">
+            <property name="text">
+             <string>cy</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="1">
+           <widget class="QLabel" name="label">
+            <property name="text">
+             <string>fx</string>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_baseline">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="minimum">
+             <double>0.000000000000000</double>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_cy">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_cx">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item>
+        <spacer name="verticalSpacer">
+         <property name="orientation">
+          <enum>Qt::Vertical</enum>
+         </property>
+         <property name="sizeHint" stdset="0">
+          <size>
+           <width>20</width>
+           <height>40</height>
+          </size>
+         </property>
+        </spacer>
+       </item>
+      </layout>
+     </widget>
+     <widget class="QWidget" name="page">
+      <layout class="QVBoxLayout" name="verticalLayout_2">
+       <item>
+        <widget class="QGroupBox" name="groupBox_3">
+         <property name="title">
+          <string>Camera Intrinsics</string>
+         </property>
+         <layout class="QVBoxLayout" name="verticalLayout_4">
+          <item>
+           <layout class="QGridLayout" name="gridLayout_3" columnstretch="0,0,1">
+            <item row="0" column="0">
+             <widget class="QLabel" name="label_left">
+              <property name="text">
+               <string>Left</string>
+              </property>
+             </widget>
+            </item>
+            <item row="0" column="1">
+             <widget class="QLabel" name="label_right">
+              <property name="text">
+               <string>Right</string>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="0">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_fx_l">
+              <property name="decimals">
+               <number>6</number>
+              </property>
+              <property name="maximum">
+               <double>99999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="1">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_fx_r">
+              <property name="decimals">
+               <number>6</number>
+              </property>
+              <property name="maximum">
+               <double>99999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="2">
+             <widget class="QLabel" name="label_10">
+              <property name="text">
+               <string>fx</string>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="0">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_fy_l">
+              <property name="decimals">
+               <number>6</number>
+              </property>
+              <property name="maximum">
+               <double>99999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="1">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_fy_r">
+              <property name="decimals">
+               <number>6</number>
+              </property>
+              <property name="maximum">
+               <double>99999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="2">
+             <widget class="QLabel" name="label_9">
+              <property name="text">
+               <string>fy</string>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="0">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_cx_l">
+              <property name="decimals">
+               <number>6</number>
+              </property>
+              <property name="maximum">
+               <double>99999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="1">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_cx_r">
+              <property name="decimals">
+               <number>6</number>
+              </property>
+              <property name="maximum">
+               <double>99999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="2">
+             <widget class="QLabel" name="label_11">
+              <property name="text">
+               <string>cx</string>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="0">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_cy_l">
+              <property name="decimals">
+               <number>6</number>
+              </property>
+              <property name="maximum">
+               <double>99999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="1">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_cy_r">
+              <property name="decimals">
+               <number>6</number>
+              </property>
+              <property name="maximum">
+               <double>99999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="2">
+             <widget class="QLabel" name="label_8">
+              <property name="text">
+               <string>cy</string>
+              </property>
+             </widget>
+            </item>
+            <item row="5" column="0">
+             <widget class="QLineEdit" name="lineEdit_D_l"/>
+            </item>
+            <item row="5" column="1">
+             <widget class="QLineEdit" name="lineEdit_D_r"/>
+            </item>
+            <item row="5" column="2">
+             <widget class="QLabel" name="label_12">
+              <property name="toolTip">
+               <string>k1, k2, p1, p2[, k3[, k4, k5, k6 ]]</string>
+              </property>
+              <property name="text">
+               <string>Distorsion coefficients (4, 5 or 8 values)</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+           </layout>
+          </item>
+          <item>
+           <widget class="Line" name="line">
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <layout class="QGridLayout" name="gridLayout_4" columnstretch="0,1">
+            <item row="0" column="0">
+             <widget class="QSpinBox" name="spinBox_width">
+              <property name="maximum">
+               <number>99999</number>
+              </property>
+             </widget>
+            </item>
+            <item row="0" column="1">
+             <widget class="QLabel" name="label_5">
+              <property name="text">
+               <string>Image width</string>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="0">
+             <widget class="QSpinBox" name="spinBox_height">
+              <property name="maximum">
+               <number>99999</number>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="1">
+             <widget class="QLabel" name="label_13">
+              <property name="text">
+               <string>Image height</string>
+              </property>
+             </widget>
+            </item>
+           </layout>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item>
+        <widget class="QGroupBox" name="groupBox_stereo_extrinsics">
+         <property name="title">
+          <string>Stereo Camera Extrinsics</string>
+         </property>
+         <layout class="QGridLayout" name="gridLayout_2">
+          <item row="0" column="0" rowspan="2" colspan="2">
+           <widget class="QLineEdit" name="lineEdit_RT">
+            <property name="toolTip">
+             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /gray_camera = 0 0 1 -1 0 0 0 -1 0&lt;br/&gt;KITTI: /base_link to /color_camera = 0 0 1 0 -1 0 0 -0.06 0 -1 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="2">
+           <widget class="QLabel" name="label_6">
+            <property name="text">
+             <string>Transform between the cameras</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item>
+        <spacer name="verticalSpacer_2">
+         <property name="orientation">
+          <enum>Qt::Vertical</enum>
+         </property>
+         <property name="sizeHint" stdset="0">
+          <size>
+           <width>20</width>
+           <height>40</height>
+          </size>
+         </property>
+        </spacer>
+       </item>
+      </layout>
+     </widget>
+    </widget>
+   </item>
    <item>
     <widget class="QDialogButtonBox" name="buttonBox">
      <property name="orientation">
@@ -119,38 +412,5 @@
   </layout>
  </widget>
  <resources/>
- <connections>
+ <connections/>
-  <connection>
-   <sender>buttonBox</sender>
-   <signal>accepted()</signal>
-   <receiver>createSimpleCalibrationDialog</receiver>
-   <slot>accept()</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>248</x>
-     <y>254</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>157</x>
-     <y>274</y>
-    </hint>
-   </hints>
-  </connection>
-  <connection>
-   <sender>buttonBox</sender>
-   <signal>rejected()</signal>
-   <receiver>createSimpleCalibrationDialog</receiver>
-   <slot>reject()</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>316</x>
-     <y>260</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>286</x>
-     <y>274</y>
-    </hint>
-   </hints>
-  </connection>
- </connections>
 </ui>

guilib/src/ui/preferencesDialog.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>979</width>
+    <width>984</width>
-    <height>655</height>
+    <height>662</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-499</y>
+            <y>-855</y>
-            <width>680</width>
+            <width>681</width>
-            <height>1171</height>
+            <height>1919</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -1703,7 +1703,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                       <item row="9" column="1">
                        <widget class="QLabel" name="label_244">
                         <property name="text">
-                         <string>Create a simple calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of a source of rectified or registered images.</string>
+                         <string>Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -1868,7 +1868,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          </sizepolicy>
                         </property>
                         <property name="text">
-                         <string>Simple calibration</string>
+                         <string>Create calibration</string>
                         </property>
                        </widget>
                       </item>
@@ -3331,21 +3331,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        </property>
                        <item>
                         <layout class="QGridLayout" name="gridLayout_68" columnstretch="0,0,1">
-                         <item row="5" column="1">
+                         <item row="1" column="2">
-                          <widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="0" column="1">
-                          <widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="0" column="2">
                           <widget class="QLabel" name="label_255">
                            <property name="text">
                             <string>Use file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;</string>
@@ -3358,48 +3344,28 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="0">
+                         <item row="3" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_timestamps">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
+                         <item row="3" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_timestamps">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="2">
+                         <item row="4" column="1">
-                          <widget class="QLabel" name="label_251">
-                           <property name="text">
-                            <string>Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use file names as timestamps&quot; above is checked. </string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="3" column="0">
-                          <widget class="QToolButton" name="toolButton_cameraImages_gt">
-                           <property name="text">
-                            <string>...</string>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="3" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_gt">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="2">
+                         <item row="4" column="2">
                           <widget class="QLabel" name="label_288">
                            <property name="text">
                             <string>Ground truth file. Select the correct format below.</string>
@@ -3412,8 +3378,94 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="1">
+                         <item row="5" column="2">
+                          <widget class="QLabel" name="label_289">
+                           <property name="text">
+                            <string>Ground truth format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="8" column="2">
+                          <widget class="QLabel" name="label_292">
+                           <property name="text">
+                            <string>Maximum laser scan points.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="2">
+                          <widget class="QLabel" name="label_256">
+                           <property name="text">
+                            <string>Synchronize capture rate with timestamps.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="1">
+                          <widget class="QCheckBox" name="checkBox_cameraImages_syncTimeStamps">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="6" column="1">
+                          <widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="1" column="1">
+                          <widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="3" column="2">
+                          <widget class="QLabel" name="label_251">
+                           <property name="text">
+                            <string>Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use file names as timestamps&quot; above is checked. </string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="4" column="0">
+                          <widget class="QToolButton" name="toolButton_cameraImages_gt">
+                           <property name="text">
+                            <string>...</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="5" column="1">
                           <widget class="QComboBox" name="comboBox_cameraImages_gtFormat">
+                           <property name="toolTip">
+                            <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Raw Format (3 values): x y z&lt;br/&gt;Raw Format (6 values): x y z roll pitch yaw&lt;br/&gt;Raw Format (7 values): x y z qx qy qz qw&lt;br/&gt;Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;RGBD-SLAM (stamp tx ty tz qx qy qz qw)&lt;br/&gt;KITTI (stamp + 12 values transform)&lt;br/&gt;TORO&lt;br/&gt;g2o&lt;br/&gt;NewCollege (stamp x y)&lt;br/&gt;Malaga Urban (GPS)&lt;br/&gt;St Lucia Stereo (INS)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                           </property>
+                           <property name="sizeAdjustPolicy">
+                            <enum>QComboBox::AdjustToContents</enum>
+                           </property>
                            <item>
                             <property name="text">
                              <string>Raw</string>
@@ -3441,32 +3493,29 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </item>
                            <item>
                             <property name="text">
-                             <string>GPS</string>
+                             <string>NewCollege</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>Malaga Urban</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>St Lucia Stereo</string>
                             </property>
                            </item>
                           </widget>
                          </item>
-                         <item row="4" column="2">
+                         <item row="6" column="0">
-                          <widget class="QLabel" name="label_289">
-                           <property name="text">
-                            <string>Ground truth format.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="5" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_path_scans">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="2">
+                         <item row="6" column="2">
                           <widget class="QLabel" name="label_293">
                            <property name="text">
                             <string>Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. </string>
@@ -3479,7 +3528,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="1">
+                         <item row="7" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_laser_transform">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -3489,7 +3538,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="2">
+                         <item row="7" column="2">
                           <widget class="QLabel" name="label_294">
                            <property name="text">
                             <string>Local transform from /base_link to /scan_link. Mouse over the box to show formats.</string>
@@ -3502,7 +3551,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="7" column="1">
+                         <item row="8" column="1">
                           <widget class="QSpinBox" name="spinBox_cameraImages_max_scan_pts">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -3512,10 +3561,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="7" column="2">
+                         <item row="0" column="2">
-                          <widget class="QLabel" name="label_292">
+                          <widget class="QLabel" name="label_265">
                            <property name="text">
-                            <string>Maximum laser scan points.</string>
+                            <string>Bayer mode. For convenience, if the images are bayered.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -3525,24 +3574,36 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="2">
+                         <item row="0" column="1">
-                          <widget class="QLabel" name="label_256">
+                          <widget class="QComboBox" name="comboBox_cameraImages_bayerMode">
-                           <property name="text">
+                           <property name="sizeAdjustPolicy">
-                            <string>Synchronize capture rate with timestamps.</string>
+                            <enum>QComboBox::AdjustToContents</enum>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="1" column="1">
-                          <widget class="QCheckBox" name="checkBox_cameraImages_syncTimeStamps">
-                           <property name="text">
-                            <string/>
                            </property>
+                           <item>
+                            <property name="text">
+                             <string>Disabled</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>BG</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>GB</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>RG</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>GR</string>
+                            </property>
+                           </item>
                           </widget>
                          </item>
                         </layout>

tools/ConsoleApp/main.cpp

@@ -63,8 +63,6 @@ void showUsage()
 			"  -debug                          Set Log level to Debug (Default Error)\n"
 			"  -info                           Set Log level to Info (Default Error)\n"
 			"  -warn                           Set Log level to Warning (Default Error)\n"
-			"  -exit_warn                      Set exit level to Warning (Default Fatal)\n"
-			"  -exit_error                     Set exit level to Error (Default Fatal)\n"
 			"  -log_console                    Log to console\n"
 			"  -v                              Get version of RTAB-Map\n"
 			"  -input \"path\"                 Load previous database if it exists.\n");
@@ -118,7 +116,6 @@ int main(int argc, char * argv[])
 	int startAt = 1;
 	ParametersMap pm;
 	ULogger::Level logLevel = ULogger::kError;
-	ULogger::Level exitLevel = ULogger::kFatal;
 	bool logConsole = false;
 
 	for(int i=1; i<argc; ++i)
@@ -240,16 +237,6 @@ int main(int argc, char * argv[])
 			logLevel = ULogger::kWarning;
 			continue;
 		}
-		if(strcmp(argv[i], "-exit_warn") == 0)
-		{
-			exitLevel = ULogger::kWarning;
-			continue;
-		}
-		if(strcmp(argv[i], "-exit_error") == 0)
-		{
-			exitLevel = ULogger::kError;
-			continue;
-		}
 		if(strcmp(argv[i], "-log_console") == 0)
 		{
 			logConsole = true;
@@ -351,7 +338,6 @@ int main(int argc, char * argv[])
 	//ULogger::setType(ULogger::kTypeFile, rtabmap.getWorkingDir()+"/LogConsole.txt", false);
 	//ULogger::setBuffered(true);
 	ULogger::setLevel(logLevel);
-	ULogger::setExitLevel(exitLevel);
 
 	UTimer timer;
 	timer.start();

utilite/include/rtabmap/utilite/UException.h

@@ -0,0 +1,33 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite is distributed in the hope that it will be useful,
+*  but WITHOUT ANY WARRANTY; without even the implied warranty of
+*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+*  GNU Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UEXCEPTION_H_
+#define UEXCEPTION_H_
+
+#include <stdexcept>
+
+class UException: public std::runtime_error {
+public:
+
+  UException(const std::string & what) :
+	  std::runtime_error(what)
+  {}
+};
+
+#endif /* UEXCEPTION_H_ */

utilite/include/rtabmap/utilite/ULogger.h

@@ -25,6 +25,7 @@
 #include "rtabmap/utilite/UMutex.h"
 #include "rtabmap/utilite/UDestroyer.h"
 #include "rtabmap/utilite/UEvent.h"
+#include "rtabmap/utilite/UException.h"
 
 #include <stdio.h>
 #include <time.h>
@@ -51,14 +52,15 @@
 #define ULOGGER_INFO(...)    ULOGGER_LOG(ULogger::kInfo,    __VA_ARGS__)
 #define ULOGGER_WARN(...) 	 ULOGGER_LOG(ULogger::kWarning, __VA_ARGS__)
 #define ULOGGER_ERROR(...)   ULOGGER_LOG(ULogger::kError,   __VA_ARGS__)
-#define ULOGGER_FATAL(...)   ULOGGER_LOG(ULogger::kFatal,   __VA_ARGS__)
+#define ULOGGER_FATAL(...)   ULOGGER_LOG(ULogger::kFatal,   __VA_ARGS__) // Throw UException
 
 #define UDEBUG(...)   ULOGGER_DEBUG(__VA_ARGS__)
 #define UINFO(...)    ULOGGER_INFO(__VA_ARGS__)
 #define UWARN(...) 	  ULOGGER_WARN(__VA_ARGS__)
 #define UERROR(...)   ULOGGER_ERROR(__VA_ARGS__)
-#define UFATAL(...)   ULOGGER_FATAL(__VA_ARGS__)
+#define UFATAL(...)   ULOGGER_FATAL(__VA_ARGS__) // Throw UException
 
+// Throw UException
 #define UASSERT(condition) if(!(condition)) ULogger::write(ULogger::kFatal, __FILE__, __LINE__, __FUNCTION__, "Condition (%s) not met!", #condition)
 #define UASSERT_MSG(condition, msg_str) if(!(condition)) ULogger::write(ULogger::kFatal, __FILE__, __LINE__, __FUNCTION__, "Condition (%s) not met! [%s]", #condition, msg_str)
 
@@ -335,15 +337,6 @@ public:
     static void setLevel(ULogger::Level level) {level_ = level;}
     static ULogger::Level level() {return level_;}
 
-    /**
-	 * Make application to exit when a log with level is written (useful for debugging). The message is printed to
-	 * console (whatever the logger type) and an ULogEvent is sent (synchronously... see UEventsManager::post()) before exiting.
-	 *
-	 * Note : A kFatal level will always exit whatever the level specified here.
-	 */
-	static void setExitLevel(ULogger::Level exitLevel) {exitLevel_ = exitLevel;}
-	static ULogger::Level exitLevel() {return exitLevel_;}
-
 	/**
 	 * An ULogEvent is sent on each message logged at the specified level.
 	 * Note : On message with level >= exitLevel, the event is sent synchronously (see UEventsManager::post()).
@@ -386,7 +379,7 @@ public:
     		int line,
     		const char *function,
     		const char* msg,
-    		...);
+    		...) throw(UException);
 
     /**
      * Get the time in the format "2008-7-13 12:23:44".
@@ -538,12 +531,6 @@ private:
 	 */
     static Level level_;
 
-    /*
-     * The severity at which the application exits.
-     * Note : A FATAL level will always exit whatever the level specified here.
-     */
-    static Level exitLevel_;
-
     /*
 	 * The severity at which the message is also sent in a ULogEvent.
 	 */
@@ -563,14 +550,6 @@ private:
 	static bool buffered_;
 
 	static std::string bufferedMsgs_;
-
-	/*
-	 * State attribute. This state happens when an exit level
-	 * message is received.
-	 * Messages received during this state are not logged.
-	 * @see exitLevel_
-	 */
-	static bool exitingState_;
 };
 
 #endif // ULOGGER_H

utilite/src/ULogger.cpp

@@ -53,9 +53,7 @@ bool ULogger::printWhereFullPath_ = false;
 bool ULogger::printThreadID_ = false;
 bool ULogger::limitWhereLength_ = false;
 bool ULogger::buffered_ = false;
-bool ULogger::exitingState_ = false;
 ULogger::Level ULogger::level_ = kInfo; // By default, we show all info msgs + upper level (Warning, Error)
-ULogger::Level ULogger::exitLevel_ = kFatal;
 ULogger::Level ULogger::eventLevel_ = kFatal;
 const char * ULogger::levelName_[5] = {"DEBUG", " INFO", " WARN", "ERROR", "FATAL"};
 ULogger* ULogger::instance_ = 0;
@@ -313,18 +311,13 @@ void ULogger::write(ULogger::Level level,
 		const char* msg,
 		...)
 {
-	if(exitingState_)
-	{
-		// Ignore messages after a fatal exit...
-		return;
-	}
 	loggerMutex_.lock();
 	if(type_ == kTypeNoLog && level < kFatal)
 	{
 		loggerMutex_.unlock();
 		return;
 	}
-	if(strlen(msg) == 0 && !printWhere_ && level < exitLevel_)
+	if(strlen(msg) == 0 && !printWhere_ && level < kFatal)
 	{
 		loggerMutex_.unlock();
 		// No need to show an empty message if we don't print where.
@@ -502,11 +495,10 @@ void ULogger::write(ULogger::Level level,
 			va_start(args, msg);
 			fullMsg.append(uFormatv(msg, args));
 			va_end(args);
-			if(level >= exitLevel_)
+			if(level >= kFatal)
 			{
 				// Send it synchronously, then receivers
 				// can do something before the code (exiting) below is executed.
-				exitingState_ = true;
 				UEventsManager::post(new ULogEvent(fullMsg, kFatal), false);
 			}
 			else
@@ -515,23 +507,23 @@ void ULogger::write(ULogger::Level level,
 			}
 		}
 
-		if(level >= exitLevel_)
+		if(level >= kFatal)
 		{
-			printf("\n*******\n%s message occurred! Application will now exit.\n", levelName_[level]);
+			std::string fullMsg = uFormat("%s%s%s%s", levelStr.c_str(), pidStr.c_str(), time.c_str(), whereStr.c_str());
-			if(type_ != kTypeConsole)
+			va_start(args, msg);
+			fullMsg.append(uFormatv(msg, args));
+			va_end(args);
+
+			if(instance_)
 			{
-				printf("  %s%s%s%s", levelStr.c_str(), pidStr.c_str(), time.c_str(), whereStr.c_str());
+				destroyer_.setDoomed(0);
-				va_start(args, msg);
+				delete instance_; // If a FileLogger is used, this will close the file.
-				vprintf(msg, args);
+				instance_ = 0;
-				va_end(args);
 			}
-			printf("\n*******\n");
-			destroyer_.setDoomed(0);
-			delete instance_; // If a FileLogger is used, this will close the file.
-			instance_ = 0;
 			//========================================================================
-			//                          EXIT APPLICATION
+			//                          Throw exception
-			exit(1);
+			loggerMutex_.unlock();
+			 throw UException(fullMsg);
 			//========================================================================
 		}
     }