Added CalibrationDialog in Preferences->source for convenience. The database is also changed to handle new FX, FY, CX and CY intrinsic parameters instead of only depthConstant parameter. Added version of the database in a new table Admin. Updated to version 0.7.0.

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1613 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-09-11 22:10:21 +08:00 · 2014-07-26 04:36:00 +00:00
parent 1557b3d44d
commit f090c81aaf
44 changed files with 1389 additions and 715 deletions
@@ -15,8 +15,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 6)
-SET(RTABMAP_PATCH_VERSION 5)
+SET(RTABMAP_MINOR_VERSION 7)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -53,14 +53,14 @@ public:
 		UEvent(kCodeNoMoreImages)
 	{
 	}
-	CameraEvent(const cv::Mat & image, const cv::Mat & depth, float depthConstant, const Transform & localTransform, int seq=0) :
+	CameraEvent(const cv::Mat & image, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int seq=0) :
 		UEvent(kCodeImageDepth),
-		_image(image, depth, depthConstant, Transform(), localTransform, seq)
+		_image(image, depth, fx, fy, cx, cy, Transform(), localTransform, seq)
 	{
 	}
-	CameraEvent(const cv::Mat & image, const cv::Mat & depth, const cv::Mat & depth2d, float depthConstant, const Transform & localTransform, int seq=0) :
+	CameraEvent(const cv::Mat & image, const cv::Mat & depth, const cv::Mat & depth2d, float fx, float fy, float cx, float cy, const Transform & localTransform, int seq=0) :
 		UEvent(kCodeImageDepth),
-		_image(image, depth, depth2d, depthConstant, Transform(), localTransform, seq)
+		_image(image, depth, depth2d, fx, fy, cx, cy, Transform(), localTransform, seq)
 	{
 	}

@@ -63,18 +63,24 @@ class RTABMAP_EXP CameraRGBD
 {
 public:
 	virtual ~CameraRGBD();
-	void takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+	void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
 	virtual bool init() = 0;

 	//getters
 	float getImageRate() const {return _imageRate;}
 	const Transform & getLocalTransform() const {return _localTransform;}
-	float getFocalLength() const {return _focalLength;}
+	float getFx() const {return _fx;}
+	float getFy() const {return _fy;}
+	float getCx() const {return _cx;}
+	float getCy() const {return _cy;}

 	//setters
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
 	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
-	void setFocalLength(float focalLength) {_focalLength = focalLength;}
+	void setFx(float fx) {_fx = fx;}
+	void setFy(float fy) {_fy = fy;}
+	void setCx(float cx) {_cx = cx;}
+	void setCy(float cy) {_cy = cy;}

 protected:
 	/**
@@ -84,15 +90,21 @@ protected:
 	 */
 	CameraRGBD(float imageRate = 0,
 				const Transform & localTransform = Transform::getIdentity(),
-				float focalLength = 0.0f);
+				float fx = 0.0f,
+				float fy = 0.0f,
+				float cx = 0.0f,
+				float cy = 0.0f);

-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant) = 0;
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) = 0;

 private:
 	float _imageRate;
 	Transform _localTransform;
 	UTimer * _frameRateTimer;
-	float _focalLength;
+	float _fx;
+	float _fy;
+	float _cx;
+	float _cy;
 };

 /////////////////////////
@@ -109,7 +121,10 @@ public:
 	CameraOpenni(const std::string & deviceId="",
 			float imageRate = 0,
 			const Transform & localTransform = Transform::getIdentity(),
-			float focalLength = 0.0f);
+			float fx = 0.0f,
+			float fy = 0.0f,
+			float cx = 0.0f,
+			float cy = 0.0f);
 	virtual ~CameraOpenni();

    void image_cb (
@@ -120,7 +135,7 @@ public:
    bool init();

 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);

 private:
    pcl::Grabber* interface_;
@@ -147,13 +162,16 @@ public:
 	CameraOpenNICV(bool asus = false,
 					float imageRate = 0,
 					const Transform & localTransform = Transform::getIdentity(),
-					float focalLength = 0.0f);
+					float fx = 0.0f,
+					float fy = 0.0f,
+					float cx = 0.0f,
+					float cy = 0.0f);
 	virtual ~CameraOpenNICV();

 	virtual bool init();

 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);

 private:
 	bool _asus;
@@ -174,19 +192,23 @@ public:
 public:
 	CameraOpenNI2(float imageRate = 0,
 					const Transform & localTransform = Transform::getIdentity(),
-					float focalLength = 0.0f);
+					float fx = 0.0f,
+					float fy = 0.0f,
+					float cx = 0.0f,
+					float cy = 0.0f);
 	virtual ~CameraOpenNI2();

 	virtual bool init();

 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);

 private:
 	openni::Device * _device;
 	openni::VideoStream * _color;
 	openni::VideoStream * _depth;
-	float _depthFocal;
+	float _depthFx;
+	float _depthFy;
 };


@@ -206,13 +228,16 @@ public:
 	CameraFreenect(int deviceId= 0,
 					float imageRate=0.0f,
 					const Transform & localTransform = Transform::getIdentity(),
-					float focalLength = 0.0f);
+					float fx = 0.0f,
+					float fy = 0.0f,
+					float cx = 0.0f,
+					float cy = 0.0f);
 	virtual ~CameraFreenect();

 	bool init();

 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);

 private:
 	int deviceId_;
@@ -88,7 +88,7 @@ public:

 	// Specific queries...
 	void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
-	void getNodeData(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & depthConstant, Transform & localTransform) const;
+	void getNodeData(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
 	void getNodeData(int signatureId, std::vector<unsigned char> & image) const;
 	void getPose(int signatureId, Transform & pose, int & mapId) const;
 	void loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const;
@@ -127,7 +127,7 @@ private:
 	virtual void loadLoopClosuresQuery(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const = 0;

 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
-	virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & depthConstant, Transform & localTransform) const = 0;
+	virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0;
 	virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const = 0;
 	virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids) const = 0;
@@ -33,7 +33,7 @@ public:

 	bool init(int startIndex=0);
 	void setFrameRate(float frameRate);
-	void getNextImage(cv::Mat & image, cv::Mat & depth, cv::Mat & depth2d, float & depthConstant, Transform & localTransform, Transform & pose);
+	void getNextImage(cv::Mat & image, cv::Mat & depth, cv::Mat & depth2d, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, Transform & pose);

 protected:
 	virtual void mainLoopBegin();
@@ -43,13 +43,19 @@ namespace rtabmap {
 void RTABMAP_EXP filterKeypointsByDepth(
 			std::vector<cv::KeyPoint> & keypoints,
 			const cv::Mat & depth,
-			float depthConstant,
+			float fx,
+			float fy,
+			float cx,
+			float cy,
 			float maxDepth);
 void RTABMAP_EXP filterKeypointsByDepth(
 		std::vector<cv::KeyPoint> & keypoints,
 		cv::Mat & descriptors,
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		float maxDepth);

 void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
@@ -33,7 +33,10 @@ public:
 		_descriptors(descriptors),
 		_featureType(featureType),
 		_keypoints(keypoints),
-		_depthConstant(0.0f),
+		_fx(0.0f),
+		_fy(0.0f),
+		_cx(0.0f),
+		_cy(0.0f),
 		_localTransform(Transform::getIdentity())
 	{
 	}
@@ -41,7 +44,10 @@ public:
 	// Metric constructor
 	Image(const cv::Mat & image,
 		  const cv::Mat & depth,
-		  float depthConstant,
+		  float fx,
+		  float fy,
+		  float cx,
+		  float cy,
 		  const Transform & pose,
 		  const Transform & localTransform,
 		  int id = 0) :
@@ -49,7 +55,10 @@ public:
 		_id(id),
 		_featureType(Feature2D::kFeatureUndef),
 		_depth(depth),
-		_depthConstant(depthConstant),
+		_fx(fx),
+		_fy(fy),
+		_cx(cx),
+		_cy(cy),
 		_pose(pose),
 		_localTransform(localTransform)
 	{
@@ -59,7 +68,10 @@ public:
 	Image(const cv::Mat & image,
 		  const cv::Mat & depth,
 		  const cv::Mat & depth2d,
-		  float depthConstant,
+		  float fx,
+		  float fy,
+		  float cx,
+		  float cy,
 		  const Transform & pose,
 		  const Transform & localTransform,
 		  int id = 0) :
@@ -68,7 +80,10 @@ public:
 		_featureType(Feature2D::kFeatureUndef),
 		_depth(depth),
 		_depth2d(depth2d),
-		_depthConstant(depthConstant),
+		_fx(fx),
+		_fy(fy),
+		_cx(cx),
+		_cy(cy),
 		_pose(pose),
 		_localTransform(localTransform)
 	{
@@ -85,11 +100,14 @@ public:
 	void setDescriptors(const cv::Mat & descriptors, Feature2D::Type featureType) {_descriptors = descriptors; _featureType=featureType;}
 	void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}

-	bool isMetric() const {return !_depth.empty() || _depthConstant != 0.0f || !_pose.isNull();}
+	bool isMetric() const {return !_depth.empty() || _fx != 0.0f || _fy != 0.0f || !_pose.isNull();}
 	void setPose(const Transform & pose) {_pose = pose;}
 	const cv::Mat & depth() const {return _depth;}
 	const cv::Mat & depth2d() const {return _depth2d;}
-	float depthConstant() const {return _depthConstant;}
+	float depthFx() const {return _fx;}
+	float depthFy() const {return _fy;}
+	float depthCx() const {return _cx;}
+	float depthCy() const {return _cy;}
 	const Transform & pose() const {return _pose;}
 	const Transform & localTransform() const {return _localTransform;}

@@ -103,7 +121,10 @@ private:
 	// Metric stuff
 	cv::Mat _depth;
 	cv::Mat _depth2d;
-	float _depthConstant;
+	float _fx;
+	float _fy;
+	float _cx;
+	float _cy;
 	Transform _pose;
 	Transform _localTransform;
 };
@@ -109,7 +109,10 @@ public:
 			int locationId, std::vector<unsigned char> & rgb,
 			std::vector<unsigned char> & depth,
 			std::vector<unsigned char> & depth2d,
-			float & depthConstant,
+			float & fx,
+			float & fy,
+			float & cx,
+			float & cy,
 			Transform & localTransform) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
@@ -141,7 +144,10 @@ public:
 	void extractKeypointsAndDescriptors(
 			const cv::Mat & image,
 			const cv::Mat & depth,
-			float depthConstant,
+			float fx,
+			float fy,
+			float cx,
+			float cy,
 			std::vector<cv::KeyPoint> & keypoints,
 			cv::Mat & descriptors);

@@ -100,7 +100,10 @@ public:
 	void get3DMap(std::map<int, std::vector<unsigned char> > & images,
 			std::map<int, std::vector<unsigned char> > & depths,
 			std::map<int, std::vector<unsigned char> > & depths2d,
-			std::map<int, float> & depthConstants,
+			std::map<int, float> & depthFxs,
+			std::map<int, float> & depthFys,
+			std::map<int, float> & depthCxs,
+			std::map<int, float> & depthCys,
 			std::map<int, Transform> & localTransforms,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
@@ -147,7 +147,10 @@ public:
 			const std::map<int, std::vector<unsigned char> > & images,
 			const std::map<int, std::vector<unsigned char> > & depths,
 			const std::map<int, std::vector<unsigned char> > & depths2d,
-			const std::map<int, float> & depthConstants,
+			const std::map<int, float> & depthFxs,
+			const std::map<int, float> & depthFys,
+			const std::map<int, float> & depthCxs,
+			const std::map<int, float> & depthCys,
 			const std::map<int, Transform> & localTransforms,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & constraints) :
@@ -155,7 +158,10 @@ public:
 		_images(images),
 		_depths(depths),
 		_depths2d(depths2d),
-		_depthConstants(depthConstants),
+		_depthFxs(depthFxs),
+		_depthFys(depthFys),
+		_depthCxs(depthCxs),
+		_depthCys(depthCys),
 		_localTransforms(localTransforms),
 		_poses(poses),
 		_constraints(constraints)
@@ -166,7 +172,10 @@ public:
 	const std::map<int, std::vector<unsigned char> > & getImages() const {return _images;}
 	const std::map<int, std::vector<unsigned char> > & getDepths() const {return _depths;}
 	const std::map<int, std::vector<unsigned char> > & getDepths2d() const {return _depths2d;}
-	const std::map<int, float> & getDepthConstants() const {return _depthConstants;}
+	const std::map<int, float> & getDepthFxs() const {return _depthFxs;}
+	const std::map<int, float> & getDepthFys() const {return _depthFys;}
+	const std::map<int, float> & getDepthCxs() const {return _depthCxs;}
+	const std::map<int, float> & getDepthCys() const {return _depthCys;}
 	const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
 	const std::map<int, Transform> & getPoses() const {return _poses;}
 	const std::multimap<int, Link> & getConstraints() const {return _constraints;}
@@ -177,7 +186,10 @@ private:
 	std::map<int, std::vector<unsigned char> > _images;
 	std::map<int, std::vector<unsigned char> > _depths;
 	std::map<int, std::vector<unsigned char> > _depths2d;
-	std::map<int, float> _depthConstants;
+	std::map<int, float> _depthFxs;
+	std::map<int, float> _depthFys;
+	std::map<int, float> _depthCxs;
+	std::map<int, float> _depthCys;
 	std::map<int, Transform> _localTransforms;
 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;
@@ -50,7 +50,10 @@ public:
 			const std::vector<unsigned char> & depth2D = std::vector<unsigned char>(),
 			const std::vector<unsigned char> & image = std::vector<unsigned char>(),
 			const std::vector<unsigned char> & depth = std::vector<unsigned char>(),
-			float depthConstant = 0.0f,
+			float fx = 0.0f,
+			float fy = 0.0f,
+			float cx = 0.0f,
+			float cy = 0.0f,
 			const Transform & localTransform =Transform::getIdentity());
 	virtual ~Signature();

@@ -106,14 +109,17 @@ public:

 	//metric stuff
 	void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
-	void setDepth(const std::vector<unsigned char> & depth, float depthConstant);
+	void setDepth(const std::vector<unsigned char> & depth, float fx, float fy, float cx, float cy);
 	void setDepth2D(const std::vector<unsigned char> & depth2D) {_depth2D = depth2D;}
 	void setLocalTransform(const Transform & t) {_localTransform = t;}
 	void setPose(const Transform & pose) {_pose = pose;}
 	const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
 	const std::vector<unsigned char> & getDepth() const {return _depth;}
 	const std::vector<unsigned char> & getDepth2D() const {return _depth2D;}
-	float getDepthConstant() const {return _depthConstant;}
+	float getDepthFx() const {return _fx;}
+	float getDepthFy() const {return _fy;}
+	float getDepthCx() const {return _cx;}
+	float getDepthCy() const {return _cy;}
 	const Transform & getPose() const {return _pose;}
 	const Transform & getLocalTransform() const {return _localTransform;}

@@ -138,7 +144,10 @@ private:

 	std::vector<unsigned char> _depth; // compressed image CV_16UC1
 	std::vector<unsigned char> _depth2D; // compressed data CV_32FC2
-	float _depthConstant;
+	float _fx;
+	float _fy;
+	float _cx;
+	float _cy;
 	Transform _pose;
 	Transform _localTransform; // camera_link -> base_link
 	std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
@@ -114,7 +114,10 @@ public:
 	void setImages(const std::map<int, std::vector<unsigned char> > & images) {_images = images;}
 	void setDepths(const std::map<int, std::vector<unsigned char> > & depths) {_depths = depths;}
 	void setDepth2ds(const std::map<int, std::vector<unsigned char> > & depth2ds) {_depth2ds = depth2ds;}
-	void setDepthConstants(const std::map<int, float> & depthConstants) {_depthConstants = depthConstants;}
+	void setDepthFxs(const std::map<int, float> & fxs) {_depthFxs = fxs;}
+	void setDepthFys(const std::map<int, float> & fys) {_depthFys = fys;}
+	void setDepthCxs(const std::map<int, float> & cxs) {_depthCxs = cxs;}
+	void setDepthCys(const std::map<int, float> & cys) {_depthCys = cys;}
 	void setLocalTransforms(const std::map<int, Transform> & localTransforms) {_localTransforms = localTransforms;}

 	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
@@ -139,7 +142,10 @@ public:
 	const std::map<int, std::vector<unsigned char> > & getImages() const {return _images;}
 	const std::map<int, std::vector<unsigned char> > & getDepths() const {return _depths;}
 	const std::map<int, std::vector<unsigned char> > & getDepth2ds() const {return _depth2ds;}
-	const std::map<int, float> & getDepthConstants() const {return _depthConstants;}
+	const std::map<int, float> & getDepthFxs() const {return _depthFxs;}
+	const std::map<int, float> & getDepthFys() const {return _depthFys;}
+	const std::map<int, float> & getDepthCxs() const {return _depthCxs;}
+	const std::map<int, float> & getDepthCys() const {return _depthCys;}
 	const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}

 	const std::map<int, Transform> & poses() const {return _poses;}
@@ -170,7 +176,10 @@ private:
 	// Metric data
 	std::map<int, std::vector<unsigned char> > _depths;
 	std::map<int, std::vector<unsigned char> > _depth2ds;
-	std::map<int, float> _depthConstants;
+	std::map<int, float> _depthFxs;
+	std::map<int, float> _depthFys;
+	std::map<int, float> _depthCxs;
+	std::map<int, float> _depthCys;
 	std::map<int, Transform> _localTransforms;

 	std::map<int, Transform> _poses;
@@ -84,18 +84,16 @@ cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
 std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3(
 		const std::multimap<int, cv::KeyPoint> & words,
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		const Transform & transform);

 std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,
 		const std::vector<cv::KeyPoint> & keypoints);

-pcl::PointXYZ RTABMAP_EXP getDepth(
-		const cv::Mat & depthImage,
-		int x, int y,
-		float depthConstant);
-
 pcl::PointXYZ RTABMAP_EXP getDepth(const cv::Mat & depthImage,
 					   int x, int y,
 					   float cx, float cy,
@@ -150,21 +148,12 @@ pcl::PointXYZRGB RTABMAP_EXP transformPoint(
 		const pcl::PointXYZRGB & pt,
 		const Transform & transform);

-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
-		const cv::Mat & imageDepth,
-		float depthConstant,
-		int decimation);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
 		const cv::Mat & imageDepth,
 		float cx, float cy,
 		float fx, float fy,
 		int decimation = 1);

-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
-		const cv::Mat & imageRgb,
-		const cv::Mat & imageDepth,
-		float depthConstant,
-		int decimation = 1);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDepth,
@@ -290,7 +279,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(

 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		int decimation,
 		double maxDepth,
 		float voxel,
@@ -80,6 +80,8 @@ ENDIF(OpenNI2_FOUND)
 ####################################
 # Generate resources files
 ####################################
+CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql)
+
 SET(R
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
 )
@@ -50,11 +50,14 @@
 namespace rtabmap
 {

-CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform, float focalLength) :
+CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
 	_imageRate(imageRate),
 	_localTransform(localTransform),
 	_frameRateTimer(new UTimer()),
-	_focalLength(focalLength)
+	_fx(fx),
+	_fy(fy),
+	_cx(cx),
+	_cy(cy)
 {
 }

@@ -66,7 +69,7 @@ CameraRGBD::~CameraRGBD()
 	}
 }

-void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
 {
 	float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
 	if(imageRate>0)
@@ -89,10 +92,22 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant
 	}

 	UTimer timer;
-	this->captureImage(rgb, depth, depthConstant);
-	if(_focalLength)
+	this->captureImage(rgb, depth, fx, fy, cx, cy);
+	if(_fx)
 	{
-		depthConstant = 1.0f/_focalLength; // override if set
+		fx = _fx; // override if set
+	}
+	if(_fy)
+	{
+		fy = _fy; // override if set
+	}
+	if(_cx)
+	{
+		cx = _cx; // override if set
+	}
+	if(_cy)
+	{
+		cy = _cy; // override if set
 	}
 	UDEBUG("Time capturing image = %fs", timer.ticks());
 }
@@ -100,8 +115,8 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant
 /////////////////////////
 // CameraOpenNIPCL
 /////////////////////////
-CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform, float focalLength) :
-		CameraRGBD(imageRate, localTransform, focalLength),
+CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
+		CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
 		interface_(0),
 		deviceId_(deviceId),
 		depthConstant_(0.0f)
@@ -184,15 +199,21 @@ bool CameraOpenni::init()
 	return true;
 }

-void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
 {
 	if(interface_ && interface_->isRunning())
 	{
 		dataReady_.acquire();
 		UScopeMutex s(dataMutex_);
-		depth = depth_;
-		rgb = rgb_;
-		depthConstant = depthConstant_;
+		if(depthConstant_)
+		{
+			depth = depth_;
+			rgb = rgb_;
+			fx = 1.0f/depthConstant_;
+			fy = 1.0f/depthConstant_;
+			cx = float(depth_.cols/2) - 0.5f;
+			cy = float(depth_.rows/2) - 0.5f;
+		}

 		depth_ = cv::Mat();
 		rgb_ = cv::Mat();
@@ -210,8 +231,8 @@ bool CameraOpenNICV::available()
 	return cv::getBuildInformation().find("OpenNI:                      YES") != std::string::npos;
 }

-CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform, float focalLength) :
-	CameraRGBD(imageRate, localTransform, focalLength),
+CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform, float fx, float fy, float cx, float cy) :
+	CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
 	_asus(asus),
 	_depthFocal(0.0f)
 {
@@ -272,7 +293,7 @@ bool CameraOpenNICV::init()
 	return true;
 }

-void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
 {
 	if(_capture.isOpened())
 	{
@@ -283,7 +304,10 @@ void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthC
 		depth = depth.clone();
 		rgb = rgb.clone();
 		UASSERT(_depthFocal > 0.0f);
-		depthConstant = 1.0f/_depthFocal;
+		fx = _depthFocal;
+		fy = _depthFocal;
+		cx = float(depth.cols/2) - 0.5f;
+		cy = float(depth.rows/2) - 0.5f;
 	}
 	else
 	{
@@ -304,8 +328,8 @@ bool CameraOpenNI2::available()
 #endif
 }

-CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTransform, float focalLength) :
-	CameraRGBD(imageRate, localTransform, focalLength),
+CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTransform, float fx, float fy, float cx, float cy) :
+	CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
 #ifdef WITH_OPENNI2
 	_device(new openni::Device()),
 	_color(new openni::VideoStream()),
@@ -315,7 +339,8 @@ CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTr
 	_color(0),
 	_depth(0),
 #endif
-	_depthFocal(0.0f)
+	_depthFx(0.0f),
+	_depthFy(0.0f)
 {
 }

@@ -435,13 +460,15 @@ bool CameraOpenNI2::init()
 	bool registered = true;
 	if(registered)
 	{
-		_depthFocal = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
+		_depthFx = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
+		_depthFy = float(_color->getVideoMode().getResolutionY()/2) / std::tan(_color->getVerticalFieldOfView()/2.0f);
 	}
 	else
 	{
-		_depthFocal = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
+		_depthFx = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
+		_depthFy = float(_depth->getVideoMode().getResolutionY()/2) / std::tan(_depth->getVerticalFieldOfView()/2.0f);
 	}
-	UINFO("depth focal = %f", _depthFocal);
+	UINFO("depth fx=%f fy=%f", _depthFx, _depthFy);

 	UINFO("CameraOpenNI2: Using color video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
 			_color->getVideoMode().getFps(),
@@ -473,7 +500,7 @@ bool CameraOpenNI2::init()
 #endif
 }

-void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
 {
 #ifdef WITH_OPENNI2
 	if(_device->isValid() &&
@@ -498,8 +525,11 @@ void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCo
 			cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
 			cv::cvtColor(tmp, rgb, CV_RGB2BGR);
 		}
-		UASSERT(_depthFocal != 0.0f);
-		depthConstant = 1.0f/_depthFocal;
+		UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
+		fx = _depthFx;
+		fy = _depthFy;
+		cx = float(depth.cols/2) - 0.5f;
+		cy = float(depth.rows/2) - 0.5f;
 	}
 	else
 	{
@@ -690,8 +720,8 @@ bool CameraFreenect::available()
 #endif
 }

-CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform, float focalLength) :
-		CameraRGBD(imageRate, localTransform, focalLength),
+CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
+		CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
 		deviceId_(deviceId),
 		ctx_(0),
 		freenectDevice_(0)
@@ -756,7 +786,7 @@ bool CameraFreenect::init()
 	return false;
 }

-void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
 {
 #ifdef WITH_FREENECT
 	if(ctx_ && freenectDevice_)
@@ -765,7 +795,10 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthC
 		{
 			freenectDevice_->getData(rgb, depth);
 			UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
-			depthConstant = 1.0f/freenectDevice_->getDepthFocal();
+			fx = freenectDevice_->getDepthFocal();
+			fy = freenectDevice_->getDepthFocal();
+			cx = float(depth.cols/2) - 0.5f;
+			cy = float(depth.rows/2) - 0.5f;

 			if(depth.empty())
 			{
@@ -783,7 +816,10 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthC
 		{
 			rgb = cv::Mat();
 			depth = cv::Mat();
-			depthConstant = 0.0f;
+			fx = 0.0f;
+			fy = 0.0f;
+			cx = 0.0f;
+			cy = 0.0f;
 		}
 	}
 #else
@@ -99,10 +99,13 @@ void CameraThread::mainLoop()
 	cv::Mat descriptors;
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat rgb, depth;
-	float depthConstant = 0.0f;
+	float fx = 0.0f;
+	float fy = 0.0f;
+	float cx = 0.0f;
+	float cy = 0.0f;
 	if(_cameraRGBD)
 	{
-		_cameraRGBD->takeImage(rgb, depth, depthConstant);
+		_cameraRGBD->takeImage(rgb, depth, fx, fy, cx, cy);
 	}
 	else
 	{
@@ -113,7 +116,7 @@ void CameraThread::mainLoop()
 	{
 		if(_cameraRGBD)
 		{
-			this->post(new CameraEvent(rgb, depth, depthConstant, _cameraRGBD->getLocalTransform(), ++_seq));
+			this->post(new CameraEvent(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), ++_seq));
 		}
 		else
 		{
@@ -393,11 +393,14 @@ void DBDriver::getNodeData(
 		std::vector<unsigned char> & image,
 		std::vector<unsigned char> & depth,
 		std::vector<unsigned char> & depth2d,
-		float & depthConstant,
+		float & fx,
+		float & fy,
+		float & cx,
+		float & cy,
 		Transform & localTransform) const
 {
 	_dbSafeAccessMutex.lock();
-	this->getNodeDataQuery(signatureId, image, depth, depth2d, depthConstant, localTransform);
+	this->getNodeDataQuery(signatureId, image, depth, depth2d, fx, fy, cx, cy, localTransform);
 	_dbSafeAccessMutex.unlock();
 }

@@ -33,6 +33,7 @@ namespace rtabmap {
 DBDriverSqlite3::DBDriverSqlite3(const ParametersMap & parameters) :
 	DBDriver(parameters),
 	_ppDb(0),
+	_version("0.0.0"),
 	_dbInMemory(Parameters::defaultDbSqlite3InMemory()),
 	_cacheSize(Parameters::defaultDbSqlite3CacheSize()),
 	_journalMode(Parameters::defaultDbSqlite3JournalMode()),
@@ -259,6 +260,44 @@ int DBDriverSqlite3::loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileNa
  return rc;
 }

+bool DBDriverSqlite3::getVersion(std::string & version) const
+{
+	version = "0.0.0";
+	if(_ppDb)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		query << "SELECT version FROM Admin;";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		if(rc == SQLITE_OK)
+		{
+			// Process the result if one
+			rc = sqlite3_step(ppStmt);
+			if(rc == SQLITE_ROW)
+			{
+				version = reinterpret_cast<const char*>(sqlite3_column_text(ppStmt, 0));
+				rc = sqlite3_step(ppStmt);
+			}
+			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		}
+		//else
+		//{
+			// old version detected
+		//}
+		return true;
+	}
+	return false;
+}
+

 bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten)
 {
@@ -322,6 +361,8 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 		schema = uHex2Str(schema);
 		this->executeNoResultQuery(schema.c_str());
 	}
+	UASSERT(this->getVersion(_version)); // must be true!
+	UINFO("Database version = %s", _version.c_str());

 	//Set database optimizations
 	this->setCacheSize(_cacheSize); // this will call the SQL
@@ -409,13 +450,26 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo

 		if(loadMetricData)
 		{
-			query << "SELECT Image.data, "
-					 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
-				  << "FROM Image "
-				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
-				  << "ON Image.id = Depth.id "
-				  << "WHERE Image.id = ?"
-				  <<";";
+			if(uStrNumCmp(_version, "0.7.0") < 0)
+			{
+				query << "SELECT Image.data, "
+						 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
+					  << "FROM Image "
+					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
+					  << "ON Image.id = Depth.id "
+					  << "WHERE Image.id = ?"
+					  <<";";
+			}
+			else
+			{
+				query << "SELECT Image.data, "
+						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
+					  << "FROM Image "
+					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
+					  << "ON Image.id = Depth.id "
+					  << "WHERE Image.id = ?"
+					  <<";";
+			}
 		}
 		else
 		{
@@ -471,8 +525,19 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 						memcpy(depth.data(), data, dataSize);
 					}

-					float depthConstant = sqlite3_column_double(ppStmt, index++);
-					(*iter)->setDepth(depth, depthConstant); // depth constant
+					if(uStrNumCmp(_version, "0.7.0") < 0)
+					{
+						float depthConstant = sqlite3_column_double(ppStmt, index++);
+						(*iter)->setDepth(depth, 1.0f/depthConstant, 1.0f/depthConstant, 0, 0);
+					}
+					else
+					{
+						float fx = sqlite3_column_double(ppStmt, index++);
+						float fy = sqlite3_column_double(ppStmt, index++);
+						float cx = sqlite3_column_double(ppStmt, index++);
+						float cy = sqlite3_column_double(ppStmt, index++);
+						(*iter)->setDepth(depth, fx, fy, cx, cy);
+					}

 					data = sqlite3_column_blob(ppStmt, index); // local transform
 					dataSize = sqlite3_column_bytes(ppStmt, index++);
@@ -516,7 +581,10 @@ void DBDriverSqlite3::getNodeDataQuery(
 		std::vector<unsigned char> & image,
 		std::vector<unsigned char> & depth,
 		std::vector<unsigned char> & depth2d,
-		float & depthConstant,
+		float & fx,
+		float & fy,
+		float & cx,
+		float & cy,
 		Transform & localTransform) const
 {
 	if(_ppDb)
@@ -527,13 +595,26 @@ void DBDriverSqlite3::getNodeDataQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;

-		query << "SELECT Image.data, "
-				 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
-			  << "FROM Image "
-			  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
-			  << "ON Image.id = Depth.id "
-			  << "WHERE Image.id = " << signatureId
-			  <<";";
+		if(uStrNumCmp(_version, "0.7.0") < 0)
+		{
+			query << "SELECT Image.data, "
+					 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
+				  << "FROM Image "
+				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
+				  << "ON Image.id = Depth.id "
+				  << "WHERE Image.id = " << signatureId
+				  <<";";
+		}
+		else
+		{
+			query << "SELECT Image.data, "
+					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
+				  << "FROM Image "
+				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
+				  << "ON Image.id = Depth.id "
+				  << "WHERE Image.id = " << signatureId
+				  <<";";
+		}

 		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -570,7 +651,21 @@ void DBDriverSqlite3::getNodeDataQuery(
 				memcpy(depth.data(), data, dataSize);
 			}

-			depthConstant = sqlite3_column_double(ppStmt, index++);
+			if(uStrNumCmp(_version, "0.7.0") < 0)
+			{
+				float depthConstant = sqlite3_column_double(ppStmt, index++);
+				fx = 1.0f/depthConstant;
+				fy = 1.0f/depthConstant;
+				cx = 0.0f;
+				cy = 0.0f;
+			}
+			else
+			{
+				fx = sqlite3_column_double(ppStmt, index++);
+				fy = sqlite3_column_double(ppStmt, index++);
+				cx = sqlite3_column_double(ppStmt, index++);
+				cy = sqlite3_column_double(ppStmt, index++);
+			}

 			data = sqlite3_column_blob(ppStmt, index); // local transform
 			dataSize = sqlite3_column_bytes(ppStmt, index++);
@@ -588,7 +683,7 @@ void DBDriverSqlite3::getNodeDataQuery(
 				memcpy(depth2d.data(), data, dataSize);
 			}

-			if(depth.empty() || depthConstant <= 0)
+			if(depth.empty() || fx <= 0 || fy <= 0 || cx < 0 || cy < 0)
 			{
 				UWARN("No metric data loaded!? Consider using getNodeDataQuery() with image only.");
 			}
@@ -1707,7 +1802,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 			//metric
 			if((*i)->getDepth().size() || (*i)->getDepth2D().size())
 			{
-				stepDepth(ppStmt, (*i)->id(), (*i)->getDepth(), (*i)->getDepth2D(), (*i)->getDepthConstant(), (*i)->getLocalTransform());
+				stepDepth(ppStmt, (*i)->id(), (*i)->getDepth(), (*i)->getDepth2D(), (*i)->getDepthFx(), (*i)->getDepthFy(), (*i)->getDepthCx(), (*i)->getDepthCy(), (*i)->getLocalTransform());
 			}
 		}
 		// Finalize (delete) the statement
@@ -1836,13 +1931,23 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,

 std::string DBDriverSqlite3::queryStepDepth() const
 {
-	return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
+	if(uStrNumCmp(_version, "0.7.0") < 0)
+	{
+		return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
+	}
+	else
+	{
+		return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
+	}
 }
 void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 		int id,
 		const std::vector<unsigned char> & depth,
 		const std::vector<unsigned char> & depth2d,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		const Transform & localTransform) const
 {
 	UDEBUG("Save depth %d (size=%d) depth2d = %d", id, (int)depth.size(), (int)depth2d.size());
@@ -1867,8 +1972,22 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	}
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

-	rc = sqlite3_bind_double(ppStmt, index++, depthConstant);
-	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	if(uStrNumCmp(_version, "0.7.0") < 0)
+	{
+		rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fy);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
+	else
+	{
+		rc = sqlite3_bind_double(ppStmt, index++, fx);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		rc = sqlite3_bind_double(ppStmt, index++, fy);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		rc = sqlite3_bind_double(ppStmt, index++, cx);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		rc = sqlite3_bind_double(ppStmt, index++, cy);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}

 	rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -72,7 +72,10 @@ private:
 			std::vector<unsigned char> & image,
 			std::vector<unsigned char> & depth,
 			std::vector<unsigned char> & depth2d,
-			float & depthConstant,
+			float & fx,
+			float & fy,
+			float & cx,
+			float & cy,
 			Transform & localTransform) const;
 	virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const;
 	virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const;
@@ -97,7 +100,10 @@ private:
 			int id,
 			const std::vector<unsigned char> & depth,
 			const std::vector<unsigned char> & depth2d,
-			float depthConstant,
+			float fx,
+			float fy,
+			float cx,
+			float cy,
 			const Transform & localTransform) const;
 	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, const Transform & transform) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
@@ -106,9 +112,11 @@ private:
 private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
+	bool getVersion(std::string &) const;

 private:
 	sqlite3 * _ppDb;
+	std::string _version;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;
@@ -112,9 +112,9 @@ void DBReader::mainLoopBegin()
 void DBReader::mainLoop()
 {
 	cv::Mat image, depth, depth2d;
-	float depthConstant;
+	float fx,fy,cx,cy;
 	Transform localTransform, pose;
-	this->getNextImage(image, depth, depth2d, depthConstant, localTransform, pose);
+	this->getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose);
 	if(!image.empty())
 	{
 		if(depth.empty())
@@ -125,7 +125,7 @@ void DBReader::mainLoop()
 		{
 			if(!_odometryIgnored)
 			{
-				Image data(image, depth, depth2d, depthConstant, pose, localTransform);
+				Image data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform);
 				this->post(new OdometryEvent(data));
 				if(pose.isNull())
 				{
@@ -137,7 +137,7 @@ void DBReader::mainLoop()
 			else
 			{
 				// without odometry
-				this->post(new CameraEvent(image, depth, depth2d, depthConstant, localTransform));
+				this->post(new CameraEvent(image, depth, depth2d, fx, fy, cx, cy, localTransform));
 			}
 		}
 	}
@@ -154,7 +154,10 @@ void DBReader::getNextImage(
 		cv::Mat & image,
 		cv::Mat & depth,
 		cv::Mat & depth2d,
-		float & depthConstant,
+		float & fx,
+		float & fy,
+		float & cx,
+		float & cy,
 		Transform & localTransform,
 		Transform & pose)
 {
@@ -186,7 +189,7 @@ void DBReader::getNextImage(
 			std::vector<unsigned char> depthBytes;
 			std::vector<unsigned char> depth2dBytes;
 			int mapId;
-			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, depthConstant, localTransform);
+			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
 			_dbDriver->getPose(*_currentId, pose, mapId);
 			++_currentId;
 			if(imageBytes.empty())
@@ -41,28 +41,34 @@ namespace rtabmap {
 void filterKeypointsByDepth(
 		std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		float maxDepth)
 {
 	cv::Mat descriptors;
-	filterKeypointsByDepth(keypoints, descriptors, depth, depthConstant, maxDepth);
+	filterKeypointsByDepth(keypoints, descriptors, depth, fx, fy, cx, cy, maxDepth);
 }

 void filterKeypointsByDepth(
 		std::vector<cv::KeyPoint> & keypoints,
 		cv::Mat & descriptors,
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		float maxDepth)
 {
-	if(!depth.empty() && depthConstant > 0.0f && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
+	if(!depth.empty() && fx > 0.0f && fy > 0.0f && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
 	{
 		std::vector<cv::KeyPoint> output(keypoints.size());
 		std::vector<int> indexes(keypoints.size(), 0);
 		int oi=0;
 		for(unsigned int i=0; i<keypoints.size(); ++i)
 		{
-			pcl::PointXYZ pt = util3d::getDepth(depth, keypoints[i].pt.x, keypoints[i].pt.y, depthConstant);
+			pcl::PointXYZ pt = util3d::getDepth(depth, keypoints[i].pt.x, keypoints[i].pt.y, cx, cy, fx, fy);
 			if(uIsFinite(pt.z) && pt.z < maxDepth)
 			{
 				output[oi++] = keypoints[i];
@@ -1759,7 +1759,10 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloudXYZ = util3d::getICPReadyCloud(
 					oldDepth,
-					oldS.getDepthConstant(),
+					oldS.getDepthFx(),
+					oldS.getDepthFy(),
+					oldS.getDepthCx(),
+					oldS.getDepthCy(),
 					_icpDecimation,
 					_icpMaxDepth,
 					_icpVoxelSize,
@@ -1767,7 +1770,10 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 					oldS.getLocalTransform());
 			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
 					newDepth,
-					newS.getDepthConstant(),
+					newS.getDepthFx(),
+					newS.getDepthFy(),
+					newS.getDepthCx(),
+					newS.getDepthCy(),
 					_icpDecimation,
 					_icpMaxDepth,
 					_icpVoxelSize,
@@ -2400,7 +2406,10 @@ void Memory::getImageDepth(
 		std::vector<unsigned char> & rgb,
 		std::vector<unsigned char> & depth,
 		std::vector<unsigned char> & depth2d,
-		float & depthConstant,
+		float & fx,
+		float & fy,
+		float & cx,
+		float & cy,
 		Transform & localTransform) const
 {
 	const Signature * s = this->getSignature(locationId);
@@ -2409,12 +2418,15 @@ void Memory::getImageDepth(
 		rgb = s->getImage();
 		depth = s->getDepth();
 		depth2d = s->getDepth2D();
-		depthConstant = s->getDepthConstant();
+		fx = s->getDepthFx();
+		fy = s->getDepthFy();
+		cx = s->getDepthCx();
+		cy = s->getDepthCy();
 		localTransform = s->getLocalTransform();
 	}
 	if(rgb.empty() && this->isRawDataKept() && _dbDriver)
 	{
-		_dbDriver->getNodeData(locationId, rgb, depth, depth2d, depthConstant, localTransform);
+		_dbDriver->getNodeData(locationId, rgb, depth, depth2d, fx, fy, cx, cy, localTransform);
 	}
 }

@@ -2782,12 +2794,12 @@ void Memory::copyData(const Signature * from, Signature * to)
 			std::vector<unsigned char> image;
 			std::vector<unsigned char> depth;
 			std::vector<unsigned char> depth2d;
-			float depthConstant;
+			float fx, fy, cx, cy;
 			Transform localTransform;
-			_dbDriver->getNodeData(from->id(), image, depth, depth2d, depthConstant, localTransform);
+			_dbDriver->getNodeData(from->id(), image, depth, depth2d, fx, fy, cx, cy, localTransform);

 			to->setImage(image);
-			to->setDepth(depth, depthConstant);
+			to->setDepth(depth, fx, fy, cx, cy);
 			to->setDepth2D(depth2d);
 			to->setLocalTransform(localTransform);

@@ -2796,7 +2808,7 @@ void Memory::copyData(const Signature * from, Signature * to)
 		else
 		{
 			to->setImage(from->getImage());
-			to->setDepth(from->getDepth(), from->getDepthConstant());
+			to->setDepth(from->getDepth(), from->getDepthFx(), from->getDepthFy(), from->getDepthCx(), from->getDepthCy());
 			to->setDepth2D(from->getDepth2D());
 			to->setLocalTransform(from->getLocalTransform());
 		}
@@ -2814,7 +2826,10 @@ void Memory::copyData(const Signature * from, Signature * to)
 void Memory::extractKeypointsAndDescriptors(
 		const cv::Mat & image,
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		std::vector<cv::KeyPoint> & keypoints,
 		cv::Mat & descriptors)
 {
@@ -2827,7 +2842,7 @@ void Memory::extractKeypointsAndDescriptors(
 			keypoints = _feature2D->generateKeypoints(image, 0, roi);
 			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());

-			filterKeypointsByDepth(keypoints, depth, depthConstant, _wordsMaxDepth);
+			filterKeypointsByDepth(keypoints, depth, fx, fy, cx, cy, _wordsMaxDepth);
 			limitKeypoints(keypoints, _wordsPerImageTarget);
 		}

@@ -2923,7 +2938,7 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 			descriptors = image.descriptors();
 			keypoints = image.keypoints();
 		}
-		filterKeypointsByDepth(keypoints, descriptors, image.depth(), image.depthConstant(), _wordsMaxDepth);
+		filterKeypointsByDepth(keypoints, descriptors, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), _wordsMaxDepth);
 		limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
 	}
 	else
@@ -2940,7 +2955,7 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 			imageMono = image.image();
 		}

-		this->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthConstant(), keypoints, descriptors);
+		this->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), keypoints, descriptors);

 		UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
 		if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
@@ -2998,9 +3013,9 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)

 	//3d words
 	std::multimap<int, pcl::PointXYZ> words3;
-	if(!image.depth().empty() && image.depthConstant())
+	if(!image.depth().empty() && image.depthFx() && image.depthFy())
 	{
-		words3 = util3d::generateWords3(words, image.depth(), image.depthConstant(), image.localTransform());
+		words3 = util3d::generateWords3(words, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), image.localTransform());
 	}

 	Signature * s;
@@ -3025,7 +3040,10 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 			util3d::compressData(image.depth2d()),
 			imageBytes,
 			depthBytes,
-			image.depthConstant(),
+			image.depthFx(),
+			image.depthFy(),
+			image.depthCx(),
+			image.depthCy(),
 			image.localTransform());
 	}
 	else
@@ -173,7 +173,7 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)

 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat descriptors;
-	_memory->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthConstant(), keypoints, descriptors);
+	_memory->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), keypoints, descriptors);

 	image.setDescriptors(descriptors, _memory->getFeatureType());
 	image.setKeypoints(keypoints);
@@ -389,7 +389,10 @@ Transform OdometryICP::computeTransform(Image & image, int * quality)
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
 						image.depth(),
-						image.depthConstant(),
+						image.depthFx(),
+						image.depthFy(),
+						image.depthCx(),
+						image.depthCy(),
 						_decimation,
 						this->getMaxDepth(),
 						_voxelSize,
@@ -520,7 +523,7 @@ void OdometryThread::mainLoop()

 void OdometryThread::addImage(const Image & image)
 {
-	if(image.empty() || image.depth().empty() || image.depthConstant() == 0.0f)
+	if(image.empty() || image.depth().empty() || image.depthFx() == 0.0f || image.depthFy() == 0.0f)
 	{
 		ULOGGER_ERROR("image empty !?");
 		return;
@@ -1482,7 +1482,10 @@ bool Rtabmap::process(const Image & image)
 				std::map<int, std::vector<unsigned char> > images;
 				std::map<int, std::vector<unsigned char> > depths;
 				std::map<int, std::vector<unsigned char> > depth2ds;
-				std::map<int, float> depthConstants;
+				std::map<int, float> depthFxs;
+				std::map<int, float> depthFys;
+				std::map<int, float> depthCxs;
+				std::map<int, float> depthCys;
 				std::map<int, Transform> localTransforms;

 				std::vector<int> ids(signaturesRetrieved.begin(), signaturesRetrieved.end());
@@ -1500,14 +1503,17 @@ bool Rtabmap::process(const Image & image)
 					if(_rgbdSlamMode)
 					{
 						std::vector<unsigned char> depth, depth2d;
-						float depthConstant;
+						float fx, fy, cx, cy;
 						Transform localTransform;
-						_memory->getImageDepth(ids[i], im, depth, depth2d, depthConstant, localTransform);
+						_memory->getImageDepth(ids[i], im, depth, depth2d, fx, fy, cx, cy, localTransform);

 						if(!depth.empty())
 						{
 							depths.insert(std::make_pair(ids[i], depth));
-							depthConstants.insert(std::make_pair(ids[i], depthConstant));
+							depthFxs.insert(std::make_pair(ids[i], fx));
+							depthFys.insert(std::make_pair(ids[i], fy));
+							depthCxs.insert(std::make_pair(ids[i], cx));
+							depthCys.insert(std::make_pair(ids[i], cy));
 							localTransforms.insert(std::make_pair(ids[i], localTransform));
 						}
 						if(!depth2d.empty())
@@ -1536,7 +1542,10 @@ bool Rtabmap::process(const Image & image)
 				statistics_.setImages(images);
 				statistics_.setDepths(depths);
 				statistics_.setDepth2ds(depth2ds);
-				statistics_.setDepthConstants(depthConstants);
+				statistics_.setDepthFxs(depthFxs);
+				statistics_.setDepthFys(depthFys);
+				statistics_.setDepthCxs(depthCxs);
+				statistics_.setDepthCys(depthCys);
 				statistics_.setLocalTransforms(localTransforms);
 			}

@@ -2153,7 +2162,10 @@ void Rtabmap::dumpPrediction() const
 void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
 		std::map<int, std::vector<unsigned char> > & depths,
 		std::map<int, std::vector<unsigned char> > & depths2d,
-		std::map<int, float> & depthConstants,
+		std::map<int, float> & depthFxs,
+		std::map<int, float> & depthFys,
+		std::map<int, float> & depthCxs,
+		std::map<int, float> & depthCys,
 		std::map<int, Transform> & localTransforms,
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
@@ -2182,9 +2194,9 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
 		for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
 		{
 			std::vector<unsigned char> image, depth, depth2d;
-			float depthConstant;
+			float fx, fy, cx, cy;
 			Transform localTransform;
-			_memory->getImageDepth(*iter, image, depth, depth2d, depthConstant, localTransform);
+			_memory->getImageDepth(*iter, image, depth, depth2d, fx, fy, cx, cy, localTransform);

 			if(image.size())
 			{
@@ -2198,9 +2210,12 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
 			{
 				depths2d.insert(std::make_pair(*iter, depth2d));
 			}
-			if(depthConstant > 0)
+			if(fx > 0 && fy > 0)
 			{
-				depthConstants.insert(std::make_pair(*iter, depthConstant));
+				depthFxs.insert(std::make_pair(*iter, fx));
+				depthFys.insert(std::make_pair(*iter, fy));
+				depthCxs.insert(std::make_pair(*iter, cx));
+				depthCys.insert(std::make_pair(*iter, cy));
 			}
 			if(!localTransform.isNull())
 			{
@@ -95,7 +95,10 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	std::map<int, std::vector<unsigned char> > images;
 	std::map<int, std::vector<unsigned char> > depths;
 	std::map<int, std::vector<unsigned char> > depths2d;
-	std::map<int, float> depthConstants;
+	std::map<int, float> depthFxs;
+	std::map<int, float> depthFys;
+	std::map<int, float> depthCxs;
+	std::map<int, float> depthCys;
 	std::map<int, Transform> localTransforms;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
@@ -103,7 +106,10 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	_rtabmap->get3DMap(images,
 			depths,
 			depths2d,
-			depthConstants,
+			depthFxs,
+			depthFys,
+			depthCxs,
+			depthCys,
 			localTransforms,
 			poses,
 			constraints,
@@ -113,7 +119,10 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	this->post(new RtabmapEvent3DMap(images,
 			depths,
 			depths2d,
-			depthConstants,
+			depthFxs,
+			depthFys,
+			depthCxs,
+			depthCys,
 			localTransforms,
 			poses,
 			constraints));
@@ -124,7 +133,10 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
 	std::map<int, std::vector<unsigned char> > images;
 	std::map<int, std::vector<unsigned char> > depths;
 	std::map<int, std::vector<unsigned char> > depths2d;
-	std::map<int, float> depthConstants;
+	std::map<int, float> depthFxs;
+	std::map<int, float> depthFys;
+	std::map<int, float> depthCxs;
+	std::map<int, float> depthCys;
 	std::map<int, Transform> localTransforms;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
@@ -137,7 +149,10 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
 	this->post(new RtabmapEvent3DMap(images,
 			depths,
 			depths2d,
-			depthConstants,
+			depthFxs,
+			depthFys,
+			depthCxs,
+			depthCys,
 			localTransforms,
 			poses,
 			constraints));
@@ -42,7 +42,10 @@ Signature::Signature(
 		const std::vector<unsigned char> & depth2D, // in base_link frame
 		const std::vector<unsigned char> & image, // in camera_link frame
 		const std::vector<unsigned char> & depth, // in camera_link frame
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		const Transform & localTransform) :
 	_id(id),
 	_mapId(mapId),
@@ -55,7 +58,10 @@ Signature::Signature(
 	_image(image),
 	_depth(depth),
 	_depth2D(depth2D),
-	_depthConstant(depthConstant),
+	_fx(fx),
+	_fy(fy),
+	_cx(cx),
+	_cy(cy),
 	_pose(pose),
 	_localTransform(localTransform),
 	_words3(words3)
@@ -188,11 +194,14 @@ void Signature::removeWord(int wordId)
 	_words3.erase(wordId);
 }

-void Signature::setDepth(const std::vector<unsigned char> & depth, float depthConstant)
+void Signature::setDepth(const std::vector<unsigned char> & depth, float fx, float fy, float cx, float cy)
 {
-	UASSERT_MSG(depth.empty() || (!depth.empty() && depthConstant > 0.0f), uFormat("depthConstant=%f",depthConstant).c_str());
+	UASSERT_MSG(depth.empty() || (!depth.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f), uFormat("fx=%f fy=%f cx=%f cy=%f",fx,fy,cx,cy).c_str());
 	_depth = depth;
-	_depthConstant=depthConstant;
+	_fx=fx;
+	_fy=fy;
+	_cx=cx;
+	_cy=cy;
 }

 } //namespace rtabmap
@@ -1,115 +1,128 @@
-- *******************************************************************
--  DatabaseSchema: Script for creating the database
--   Usage:
--       $ sqlite3 LTM.db < DatabaseSchema.sql
--
-- *******************************************************************
-
-- *******************************************************************
-- CLEAN
-- *******************************************************************
-/*DROP TABLE Node;*/
-
-- *******************************************************************
-- CREATE
-- *******************************************************************
-CREATE TABLE Node (
-	id INTEGER NOT NULL,
-	map_id INTEGER NOT NULL,
-	weight INTEGER,
-	pose BLOB,
-	time_enter DATE,
-	PRIMARY KEY (id)
-);
-
-CREATE TABLE Image (
-	id INTEGER NOT NULL,
-	data BLOB,
-	time_enter DATE,
-	PRIMARY KEY (id)
-);
-
-CREATE TABLE Depth (
-	id INTEGER NOT NULL,
-	data BLOB,   -- CV_32FC1, width = Image/raw_width, height=Image/raw_height
-	constant FLOAT,
-	local_transform BLOB,
-	data2d BLOB,   -- CV_32FC2, Example: Laser scan
-	time_enter DATE,
-	PRIMARY KEY (id)
-);
-
-CREATE TABLE Link (
-	from_id INTEGER NOT NULL,
-	to_id INTEGER NOT NULL,
-	type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
-	transform BLOB,
-	FOREIGN KEY (from_id) REFERENCES Node(id),
-	FOREIGN KEY (to_id) REFERENCES Node(id)
-);
-
-- 
-CREATE TABLE Word (
-	id INTEGER NOT NULL,
-	descriptor_size INTEGER NOT NULL,
-	descriptor BLOB NOT NULL,
-	time_enter DATE,
-	PRIMARY KEY (id)
-);
-
-CREATE TABLE Map_Node_Word (
-	node_id INTEGER NOT NULL,
-	word_id INTEGER NOT NULL,
-	pos_x FLOAT NOT NULL,
-	pos_y FLOAT NOT NULL,
-	size INTEGER NOT NULL,
-	dir FLOAT NOT NULL,
-	response FLOAT NOT NULL,
-	depth_x FLOAT,
-	depth_y FLOAT,
-	depth_z FLOAT,
-	FOREIGN KEY (node_id) REFERENCES Node(id),
-	FOREIGN KEY (word_id) REFERENCES Word(id)
-);
-
-CREATE TABLE Statistics (
-	STM_size INTEGER,
-	last_sign_added INTEGER,
-	process_mem_used INTEGER,
-	database_mem_used INTEGER,
-	dictionary_size INTEGER,
-	time_enter DATE
-);
-
-- *******************************************************************
-- TRIGGERS
-- *******************************************************************
-CREATE TRIGGER insert_Map_Node_Word BEFORE INSERT ON Map_Node_Word 
-WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
-BEGIN
- SELECT RAISE(ABORT, 'Foreign key constraint failed in Map_Node_Word table');
-END;
-
- --   Creating a trigger for time_enter
-CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
-BEGIN
- UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
-END;
-
-CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
-BEGIN
- UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
-END;
-
-CREATE TRIGGER insert_Statistics_timeEnter AFTER INSERT ON Statistics
-BEGIN
- UPDATE Statistics SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
-END;
-
-
-- *******************************************************************
-- INDEXES
-- *******************************************************************
-CREATE INDEX IDX_Map_Node_Word_node_id on Map_Node_Word (node_id);
-CREATE INDEX IDX_Link_from_id on Link (from_id);
-
+-- *******************************************************************
+--  DatabaseSchema: Script for creating the database
+--   Usage:
+--       $ sqlite3 LTM.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Node;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Node (
+	id INTEGER NOT NULL,
+	map_id INTEGER NOT NULL,
+	weight INTEGER,
+	pose BLOB,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Image (
+	id INTEGER NOT NULL,
+	data BLOB,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Depth (
+	id INTEGER NOT NULL,
+	data BLOB,   -- CV_32FC1, width = Image/raw_width, height=Image/raw_height
+	fx FLOAT,
+	fy FLOAT,
+	cx FLOAT,
+	cy FLOAT,
+	local_transform BLOB,
+	data2d BLOB,   -- CV_32FC2, Example: Laser scan
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Link (
+	from_id INTEGER NOT NULL,
+	to_id INTEGER NOT NULL,
+	type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
+	transform BLOB,
+	FOREIGN KEY (from_id) REFERENCES Node(id),
+	FOREIGN KEY (to_id) REFERENCES Node(id)
+);
+
+-- 
+CREATE TABLE Word (
+	id INTEGER NOT NULL,
+	descriptor_size INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Map_Node_Word (
+	node_id INTEGER NOT NULL,
+	word_id INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	response FLOAT NOT NULL,
+	depth_x FLOAT,
+	depth_y FLOAT,
+	depth_z FLOAT,
+	FOREIGN KEY (node_id) REFERENCES Node(id),
+	FOREIGN KEY (word_id) REFERENCES Word(id)
+);
+
+CREATE TABLE Statistics (
+	STM_size INTEGER,
+	last_sign_added INTEGER,
+	process_mem_used INTEGER,
+	database_mem_used INTEGER,
+	dictionary_size INTEGER,
+	time_enter DATE
+);
+
+CREATE TABLE Admin (
+	version INTEGER,
+	time_enter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Map_Node_Word BEFORE INSERT ON Map_Node_Word 
+WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed in Map_Node_Word table');
+END;
+
+ --   Creating a trigger for time_enter
+CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
+BEGIN
+ UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Statistics_timeEnter AFTER INSERT ON Statistics
+BEGIN
+ UPDATE Statistics SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE INDEX IDX_Map_Node_Word_node_id on Map_Node_Word (node_id);
+CREATE INDEX IDX_Link_from_id on Link (from_id);
+
+-- *******************************************************************
+-- VERSION
+-- *******************************************************************
+INSERT INTO Admin(version) VALUES('@PROJECT_VERSION@');
+
@@ -301,7 +301,10 @@ cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
 std::multimap<int, pcl::PointXYZ> generateWords3(
 		const std::multimap<int, cv::KeyPoint> & words,
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		const Transform & transform)
 {
 	std::multimap<int, pcl::PointXYZ> words3;
@@ -311,10 +314,10 @@ std::multimap<int, pcl::PointXYZ> generateWords3(
 				depth,
 				iter->second.pt.x+0.5f,
 				iter->second.pt.y+0.5f,
-				depth.cols/2,
-				depth.rows/2,
-				1.0f/depthConstant,
-				1.0f/depthConstant);
+				cx,
+				cy,
+				fx,
+				fy);

 		if(!transform.isNull() && !transform.isIdentity())
 		{
@@ -411,18 +414,6 @@ void findCorrespondences(
 	inliers2.resize(oi);
 }

-pcl::PointXYZ getDepth(
-		const cv::Mat & depthImage,
-		int x, int y,
-		float depthConstant)
-{
-	return getDepth(depthImage, x, y,
-			(float)depthImage.cols/2,
-			(float)depthImage.rows/2,
-			1.0f/depthConstant,
-			1.0f/depthConstant);
-}
-
 pcl::PointXYZ getDepth(const cv::Mat & depthImage,
 					   int x, int y,
 			       float cx, float cy,
@@ -433,8 +424,8 @@ pcl::PointXYZ getDepth(const cv::Mat & depthImage,
 	pcl::PointXYZ pt;

 	// Use correct principal point from calibration
-	float center_x = cx; //cameraInfo.K.at(2)
-	float center_y = cy; //cameraInfo.K.at(5)
+	float center_x = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
+	float center_y = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)

 	bool isInMM = depthImage.type() == CV_16UC1; // is in mm?

@@ -624,19 +615,6 @@ pcl::PointXYZRGB RTABMAP_EXP transformPoint(
 	return pcl::transformPoint(pt, transformToEigen3f(transform));
 }

-pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
-		const cv::Mat & imageDepth,
-		float depthConstant,
-		int decimation)
-{
-	return cloudFromDepth(
-			imageDepth,
-			float(imageDepth.cols/2),
-			float(imageDepth.rows/2),
-			1.0f/depthConstant,
-			1.0f/depthConstant,
-			decimation);
-}
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		const cv::Mat & imageDepth,
 		float cx, float cy,
@@ -675,21 +653,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 	return cloud;
 }

-pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
-		const cv::Mat & imageRgb,
-		const cv::Mat & imageDepth,
-		float depthConstant,
-		int decimation)
-{
-	return cloudFromDepthRGB(
-			imageRgb,
-			imageDepth,
-			float(imageDepth.cols/2),
-			float(imageDepth.rows/2),
-			1.0f/depthConstant,
-			1.0f/depthConstant,
-			decimation);
-}
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDepth,
@@ -1421,7 +1384,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
 // If "voxel" > 0, "samples" is ignored
 pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		int decimation,
 		double maxDepth,
 		float voxel,
@@ -1431,10 +1397,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 	cloud = cloudFromDepth(
 			depth,
-			depth.cols/2,
-			depth.rows/2,
-			1.0f/depthConstant,
-			1.0f/depthConstant,
+			cx,
+			cy,
+			fx,
+			fy,
 			decimation);

 	if(cloud->size())
@@ -76,15 +76,16 @@ private slots:
 			if(data.depth().cols == data.image().cols &&
 			   data.depth().rows == data.image().rows &&
 			   !data.depth().empty() &&
-			   data.depthConstant() > 0.0f)
+			   data.depthFx() > 0.0f &&
+			   data.depthFy() > 0.0f)
 			{
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
 					data.image(),
 					data.depth(),
-					float(data.depth().cols/2),
-					float(data.depth().rows/2),
-					1.0f/data.depthConstant(),
-					1.0f/data.depthConstant(),
+					data.depthCx(),
+					data.depthCy(),
+					data.depthFx(),
+					data.depthFy(),
 					2); // decimation // high definition
 				if(cloud->size())
 				{
@@ -143,21 +144,21 @@ private slots:
 				else if(iter->first == stats.refImageId() &&
 						uContains(stats.getImages(), iter->first) &&
 						uContains(stats.getDepths(), iter->first) &&
-						uContains(stats.getDepthConstants(), iter->first) &&
+						uContains(stats.getDepthFxs(), iter->first) &&
+						uContains(stats.getDepthFys(), iter->first) &&
 						uContains(stats.getLocalTransforms(), iter->first))
 				{
 					// Add the new cloud
 					cv::Mat rgb = util3d::uncompressImage(stats.getImages().at(iter->first));
 					cv::Mat depth = util3d::uncompressImage(stats.getDepths().at(iter->first));
-					float depthConstant = stats.getDepthConstants().at(iter->first);
 					Transform localTransform = stats.getLocalTransforms().at(iter->first);
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
 							rgb,
 							depth,
-						   float(depth.cols/2),
-						   float(depth.rows/2),
-						   1.0f/depthConstant,
-						   1.0f/depthConstant,
+							stats.getDepthCxs().at(iter->first),
+							stats.getDepthCys().at(iter->first),
+							stats.getDepthFxs().at(iter->first),
+							stats.getDepthFys().at(iter->first),
 						   8); // decimation

 					if(cloud->size())
@@ -27,11 +27,23 @@ public:
 	CalibrationDialog(QWidget * parent = 0);
 	virtual ~CalibrationDialog();

+	bool isCalibrated() const {return calibrated_;}
+	const cv::Mat & cameraMatrix() const {return cameraMatrix_;} // Matrix K
+	const cv::Mat & distCoeffs() const {return distCoeffs_;} // Matrix D
+	float fx() const {return cameraMatrix_.at<double>(0,0);} // K(0)
+	float fy() const {return cameraMatrix_.at<double>(1,1);} // K(4)
+	float cx() const {return cameraMatrix_.at<double>(0,2);} // K(2)
+	float cy() const {return cameraMatrix_.at<double>(1,2);} // K(5)
+
+public slots:
+	void setBoardWidth(int width);
+	void setBoardHeight(int height);
+	void setSquareSize(double size);
+
 private slots:
 	void processImage(const cv::Mat & image);
 	void restart();
 	void calibrate();
-	void exit();

 protected:
 	virtual void closeEvent(QCloseEvent* event);
@@ -189,7 +189,10 @@ private:
 			int id,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
-			float depthConstant,
+			float fx,
+			float fy,
+			float cx,
+			float cy,
 			const Transform & localTransform,
 			const Transform & pose,
 			float voxelSize,
@@ -224,7 +227,10 @@ private:
 	QMap<int, std::vector<unsigned char> > _imagesMap;
 	QMap<int, std::vector<unsigned char> > _depthsMap;
 	QMap<int, std::vector<unsigned char> > _depths2DMap;
-	QMap<int, float> _depthConstantsMap;
+	QMap<int, float> _depthFxsMap;
+	QMap<int, float> _depthFysMap;
+	QMap<int, float> _depthCxsMap;
+	QMap<int, float> _depthCysMap;
 	QMap<int, Transform> _localTransformsMap;
 	std::map<int, Transform> _currentPosesMap;
 	Transform _odometryCorrection;
@@ -159,7 +159,10 @@ public:
 	Src getSourceRGBD() const; 			// Openni group
 	QString getSourceOpenniDevice() const;            //Openni group
 	Transform getSourceOpenniLocalTransform() const;    //Openni group
-	float getSourceOpenniFocalLength() const; // Openni group
+	float getSourceOpenniFx() const; // Openni group
+	float getSourceOpenniFy() const; // Openni group
+	float getSourceOpenniCx() const; // Openni group
+	float getSourceOpenniCy() const; // Openni group

 	int getIgnoredDCComponents() const;

@@ -219,6 +222,7 @@ private slots:
 	void openDatabaseViewer();
 	void cleanOdometryTest();
 	void testOdometry();
+	void calibrate();

 protected:
 	virtual void showEvent ( QShowEvent * event );
@@ -93,8 +93,6 @@ SET(LIBRARIES
    ${QT_LIBRARIES} 
 	${OpenCV_LIBS} 
 	${PCL_LIBRARIES}
-	QVTK 
-	vtkHybrid
 )

 #include files
@@ -37,7 +37,15 @@ CalibrationDialog::CalibrationDialog(QWidget * parent) :

 	connect(ui_->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
 	connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
-	connect(ui_->pushButton_exit, SIGNAL(clicked()), this, SLOT(exit()));
+
+	connect(ui_->spinBox_boardWidth, SIGNAL(valueChanged(int)), this, SLOT(setBoardWidth(int)));
+	connect(ui_->spinBox_boardHeight, SIGNAL(valueChanged(int)), this, SLOT(setBoardHeight(int)));
+	connect(ui_->doubleSpinBox_squareSize, SIGNAL(valueChanged(double)), this, SLOT(setSquareSize(double)));
+
+	connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
+	connect(ui_->buttonBox, SIGNAL(accepted()), this, SLOT(accept()));
+
+	ui_->image_view->setFocus();

 	ui_->progressBar_count->setMaximum(COUNT_MIN);
 	ui_->progressBar_count->setFormat("%v");
@@ -51,6 +59,33 @@ CalibrationDialog::~CalibrationDialog()
 	delete ui_;
 }

+void CalibrationDialog::setBoardWidth(int width)
+{
+	if(width != boardSize_.width)
+	{
+		boardSize_.width = width;
+		this->restart();
+	}
+}
+
+void CalibrationDialog::setBoardHeight(int height)
+{
+	if(height != boardSize_.height)
+	{
+		boardSize_.height = height;
+		this->restart();
+	}
+}
+
+void CalibrationDialog::setSquareSize(double size)
+{
+	if(size != squareSize_)
+	{
+		squareSize_ = size;
+		this->restart();
+	}
+}
+
 void CalibrationDialog::closeEvent(QCloseEvent* event)
 {
 	this->unregisterFromEventsManager();
@@ -197,7 +232,7 @@ void CalibrationDialog::restart()
 	imageParams_.clear();

 	ui_->pushButton_calibrate->setEnabled(false);
-	ui_->pushButton_save->setEnabled(false);
+	ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(false);
 	ui_->checkBox_rectified->setEnabled(false);

 	ui_->progressBar_count->reset();
@@ -286,12 +321,9 @@ void CalibrationDialog::calibrate()

 		ui_->checkBox_rectified->setEnabled(true);
 		ui_->checkBox_rectified->setChecked(true);
-	}
-}

-void CalibrationDialog::exit()
-{
-	this->close();
+		ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(true);
+	}
 }

 float CalibrationDialog::getArea(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)
@@ -448,13 +448,13 @@ void CloudViewer::updateCameraPosition(const Transform & pose)
 				cameras.front().view[2] = Fp[10];
 			}

+			_visualizer->removeCoordinateSystem(0);
+			_visualizer->addCoordinateSystem(0.2, m, 0);
 			_visualizer->setCameraPosition(
 					cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
 					cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
 					cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
 		}
-		_visualizer->removeCoordinateSystem(0);
-		_visualizer->addCoordinateSystem(0.2, m, 0);
 	}

 	_lastPose = pose;
@@ -200,17 +200,17 @@ void DatabaseViewer::exportDatabase()
 				{
 					int id = ids_.at(i);
 					std::vector<unsigned char> compressedRgb, compressedDepth, compressedDepth2d;
-					float tmpDepthConstant;
+					float tmpFx, tmpFy, tmpCx, tmpCy;
 					rtabmap::Transform tmpLocalTransform, pose;

-					memory_->getImageDepth(id, compressedRgb, compressedDepth, compressedDepth2d, tmpDepthConstant, tmpLocalTransform);
+					memory_->getImageDepth(id, compressedRgb, compressedDepth, compressedDepth2d, tmpFx, tmpFy, tmpCx, tmpCy, tmpLocalTransform);
 					if(dialog.isOdomExported())
 					{
 						memory_->getPose(id, pose, true);
 					}

 					cv::Mat rgb, depth, depth2d;
-					float depthConstant = 0;
+					float fx = 0, fy = 0, cx = 0, cy = 0;
 					rtabmap::Transform localTransform;

 					if(dialog.isRgbExported())
@@ -220,7 +220,10 @@ void DatabaseViewer::exportDatabase()
 					if(dialog.isDepthExported())
 					{
 						depth = rtabmap::util3d::uncompressImage(compressedDepth);
-						depthConstant = tmpDepthConstant;
+						fx = tmpFx;
+						fy = tmpFy;
+						cx = tmpCx;
+						cy = tmpCy;
 						localTransform = tmpLocalTransform;
 					}
 					if(dialog.isDepth2dExported())
@@ -228,7 +231,7 @@ void DatabaseViewer::exportDatabase()
 						depth2d = rtabmap::util3d::uncompressData(compressedDepth2d);
 					}

-					rtabmap::Image data(rgb, depth, depth2d, depthConstant, pose, localTransform, id);
+					rtabmap::Image data(rgb, depth, depth2d, fx, fy, cx, cy, pose, localTransform, id);
 					recorder.addData(data);

 					progressDialog.appendText(tr("Exported node %1").arg(id));
@@ -518,17 +521,17 @@ void DatabaseViewer::view3DMap()
 							if(!pose.isNull())
 							{
 								std::vector<unsigned char> image, depth, depth2d;
-								float depthConstant;
+								float fx, fy, cx, cy;
 								rtabmap::Transform localTransform;
-								memory_->getImageDepth(iter->first, image, depth, depth2d, depthConstant, localTransform);
+								memory_->getImageDepth(iter->first, image, depth, depth2d, fx, fy, cx, cy, localTransform);
 								pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 								cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
 								cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
 								cloud = rtabmap::util3d::cloudFromDepthRGB(
 										imageMat,
 										depthMat,
-										depthMat.cols/2, depthMat.rows/2,
-										1.0f/depthConstant, 1.0f/depthConstant,
+										cx, cy,
+										fx, fy,
 										decimation);

 								if(maxDepth)
@@ -616,17 +619,17 @@ void DatabaseViewer::generate3DMap()
 								if(!pose.isNull())
 								{
 									std::vector<unsigned char> image, depth, depth2d;
-									float depthConstant;
+									float fx, fy, cx, cy;
 									rtabmap::Transform localTransform;
-									memory_->getImageDepth(iter->first, image, depth, depth2d, depthConstant, localTransform);
+									memory_->getImageDepth(iter->first, image, depth, depth2d, fx, fy, cx, cy, localTransform);
 									pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 									cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
 									cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
 									cloud = rtabmap::util3d::cloudFromDepthRGB(
 											imageMat,
 											depthMat,
-											depthMat.cols/2, depthMat.rows/2,
-											1.0f/depthConstant, 1.0f/depthConstant,
+											cx, cy,
+											fx, fy,
 											decimation);

 									if(maxDepth)
@@ -703,9 +706,9 @@ void DatabaseViewer::update(int value,
 			if(memory_)
 			{
 				std::vector<unsigned char> image, depth, depth2d;
-				float depthConstant;
+				float fx, fy, cx, cy;
 				rtabmap::Transform localTransform;
-				memory_->getImageDepth(id, image, depth, depth2d, depthConstant, localTransform);
+				memory_->getImageDepth(id, image, depth, depth2d, fx, fy, cx, cy, localTransform);
 				cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
 				cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
 				if(!image.empty())
@@ -830,17 +833,18 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link)
 		ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
 		ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));

-		float depthConstantA, depthConstantB;
+		float fxA, fyA, cxA, cyA;
+		float fxB, fyB, cxB, cyB;
 		rtabmap::Transform localTransformA, localTransformB;

 		std::vector<unsigned char> imageBytesA, depthBytesA, depth2dBytesA;
-		memory_->getImageDepth(link.from(), imageBytesA, depthBytesA, depth2dBytesA, depthConstantA, localTransformA);
+		memory_->getImageDepth(link.from(), imageBytesA, depthBytesA, depth2dBytesA, fxA, fyA, cxA, cyA, localTransformA);
 		cv::Mat imageA = rtabmap::util3d::uncompressImage(imageBytesA);
 		cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
 		cv::Mat depth2dA = rtabmap::util3d::uncompressData(depth2dBytesA);

 		std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
-		memory_->getImageDepth(link.to(), imageBytesB, depthBytesB, depth2dBytesB, depthConstantB, localTransformB);
+		memory_->getImageDepth(link.to(), imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
 		cv::Mat imageB = rtabmap::util3d::uncompressImage(imageBytesB);
 		cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
 		cv::Mat depth2dB = rtabmap::util3d::uncompressData(depth2dBytesB);
@@ -850,10 +854,8 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link)
 		cloudA = rtabmap::util3d::cloudFromDepthRGB(
 				imageA,
 				depthA,
-				depthA.cols/2,
-				depthA.rows/2,
-				1.0f/depthConstantA,
-				1.0f/depthConstantA,
+				cxA, cyA,
+				fxA, fyA,
 				1);

 		cloudA = rtabmap::util3d::removeNaNFromPointCloud(cloudA);
@@ -863,10 +865,8 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link)
 		cloudB = rtabmap::util3d::cloudFromDepthRGB(
 				imageB,
 				depthB,
-				depthB.cols/2,
-				depthB.rows/2,
-				1.0f/depthConstantB,
-				1.0f/depthConstantB,
+				cxB, cyB,
+				fxB, fyB,
 				1);

 		cloudB = rtabmap::util3d::removeNaNFromPointCloud(cloudB);
@@ -908,13 +908,13 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			UINFO("Update scans list...");
 			for(int i=0; i<ids_.size(); ++i)
 			{
-				std::vector<unsigned char> imageBytesA, depthBytesA, depth2dBytesA;
-				float depthConstantA;
-				rtabmap::Transform localTransformA;
-				memory_->getImageDepth(ids_.at(i), imageBytesA, depthBytesA, depth2dBytesA, depthConstantA, localTransformA);
-				if(depth2dBytesA.size())
+				std::vector<unsigned char> imageBytes, depthBytes, depth2dBytes;
+				float fx, fy, cx, cy;
+				rtabmap::Transform localTransform;
+				memory_->getImageDepth(ids_.at(i), imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
+				if(depth2dBytes.size())
 				{
-					scans_.insert(ids_.at(i), depth2dBytesA);
+					scans_.insert(ids_.at(i), depth2dBytes);
 				}
 			}
 			UINFO("Update scans list... done");
@@ -129,10 +129,8 @@ void LoopClosureViewer::updateView(const Transform & transform)
 			cloudA = util3d::cloudFromDepthRGB(
 					imageA,
 					depthA,
-					depthA.cols/2,
-					depthA.rows/2,
-					1.0f/sA_->getDepthConstant(),
-					1.0f/sA_->getDepthConstant(),
+					sA_->getDepthCx(), sA_->getDepthCy(),
+					sA_->getDepthFx(), sA_->getDepthFy(),
 					decimation);

 			cloudA = util3d::removeNaNFromPointCloud(cloudA);
@@ -151,10 +149,8 @@ void LoopClosureViewer::updateView(const Transform & transform)
 			cloudB = util3d::cloudFromDepthRGB(
 					imageB,
 					depthB,
-					depthB.cols/2,
-					depthB.rows/2,
-					1.0f/sB_->getDepthConstant(),
-					1.0f/sB_->getDepthConstant(),
+					sB_->getDepthCx(), sB_->getDepthCy(),
+					sB_->getDepthFx(), sB_->getDepthFy(),
 					decimation);

 			cloudB = util3d::removeNaNFromPointCloud(cloudB);
@@ -575,14 +575,18 @@ void MainWindow::processOdometry(const rtabmap::Image & data, int quality)
 		if(data.depth().cols == data.image().cols &&
 		   data.depth().rows == data.image().rows &&
 		   !data.depth().empty() &&
-		   data.depthConstant() > 0.0f &&
+		   data.depthFx() > 0.0f &&
+		   data.depthFy() > 0.0f &&
 		   _preferencesDialog->isCloudsShown(1))
 		{
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 			cloud = createCloud(0,
 					data.image(),
 					data.depth(),
-					data.depthConstant(),
+					data.depthFx(),
+					data.depthFy(),
+					data.depthCx(),
+					data.depthCy(),
 					data.localTransform(),
 					pose,
 					_preferencesDialog->getCloudVoxelSize(1),
@@ -685,12 +689,18 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			{
 				if(!iter->second.empty() && !_depthsMap.contains(iter->first))
 				{
-					float constant = uValue(stat.getDepthConstants(), iter->first, 0.0f);
+					float fx = uValue(stat.getDepthFxs(), iter->first, 0.0f);
+					float fy = uValue(stat.getDepthFys(), iter->first, 0.0f);
+					float cx = uValue(stat.getDepthCxs(), iter->first, 0.0f);
+					float cy = uValue(stat.getDepthCys(), iter->first, 0.0f);
 					Transform transform = uValue(stat.getLocalTransforms(), iter->first, Transform());
-					if(constant != 0.0f && !transform.isNull())
+					if(fx > 0.0f && fy > 0.0f && !transform.isNull())
 					{
 						_depthsMap.insert(iter->first, iter->second);
-						_depthConstantsMap.insert(iter->first, constant);
+						_depthFxsMap.insert(iter->first, fx);
+						_depthFysMap.insert(iter->first, fy);
+						_depthCxsMap.insert(iter->first, cx);
+						_depthCysMap.insert(iter->first, cy);
 						_localTransformsMap.insert(iter->first, transform);
 					}
 					else
@@ -992,7 +1002,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 						_depths2DMap.value(loopOldId, std::vector<unsigned char>()),
 						_imagesMap.value(loopOldId, std::vector<unsigned char>()),
 						_depthsMap.value(loopOldId, std::vector<unsigned char>()),
-						_depthConstantsMap.value(loopOldId, 0.0f),
+						_depthFxsMap.value(loopOldId, 0.0f),
+						_depthFysMap.value(loopOldId, 0.0f),
+						_depthCxsMap.value(loopOldId, 0.0f),
+						_depthCysMap.value(loopOldId, 0.0f),
 						_localTransformsMap.value(loopOldId, Transform()));

 				Signature * loopNew = new Signature(
@@ -1004,7 +1017,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 						_depths2DMap.value(loopNewId, std::vector<unsigned char>()),
 						_imagesMap.value(loopNewId, std::vector<unsigned char>()),
 						_depthsMap.value(loopNewId, std::vector<unsigned char>()),
-						_depthConstantsMap.value(loopNewId, 0.0f),
+						_depthFxsMap.value(loopNewId, 0.0f),
+						_depthFysMap.value(loopNewId, 0.0f),
+						_depthCxsMap.value(loopNewId, 0.0f),
+						_depthCysMap.value(loopNewId, 0.0f),
 						_localTransformsMap.value(loopNewId, Transform()));

 				_ui->widget_loopClosureViewer->setData(loopOld, loopNew);
@@ -1212,7 +1228,10 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
 	cloud = createCloud(nodeId,
 			util3d::uncompressImage(_imagesMap.value(nodeId)),
 			util3d::uncompressImage(_depthsMap.value(nodeId)),
-			_depthConstantsMap.value(nodeId),
+			_depthFxsMap.value(nodeId),
+			_depthFysMap.value(nodeId),
+			_depthCxsMap.value(nodeId),
+			_depthCysMap.value(nodeId),
 			_localTransformsMap.value(nodeId),
 			Transform::getIdentity(),
 			_preferencesDialog->getCloudVoxelSize(0),
@@ -1376,7 +1395,10 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		UINFO(" images = %d", event.getImages().size());
 		UINFO(" depths = %d", event.getDepths().size());
 		UINFO(" depths2d = %d", event.getDepths2d().size());
-		UINFO(" depthConstants = %d", event.getDepthConstants().size());
+		UINFO(" depthFxs = %d", event.getDepthFxs().size());
+		UINFO(" depthFys = %d", event.getDepthFys().size());
+		UINFO(" depthCxs = %d", event.getDepthCxs().size());
+		UINFO(" depthCys = %d", event.getDepthCys().size());
 		UINFO(" localTransforms = %d", event.getLocalTransforms().size());
 		UINFO(" poses = %d", event.getPoses().size());
 		UINFO(" constraints = %d", event.getConstraints().size());
@@ -1401,13 +1423,40 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		_initProgressDialog->appendText(tr("Inserted %1 depth images.").arg(_depthsMap.size()));
 		_initProgressDialog->incrementStep();

-		for(std::map<int, float>::const_iterator iter = event.getDepthConstants().begin();
-			iter!=event.getDepthConstants().end();
+		for(std::map<int, float>::const_iterator iter = event.getDepthFxs().begin();
+			iter!=event.getDepthFxs().end();
 			++iter)
 		{
-			_depthConstantsMap.insert(iter->first, iter->second);
+			_depthFxsMap.insert(iter->first, iter->second);
 		}
-		_initProgressDialog->appendText(tr("Inserted %1 depth constants.").arg(_depthConstantsMap.size()));
+		_initProgressDialog->appendText(tr("Inserted %1 depth fx parameters.").arg(_depthFxsMap.size()));
+		_initProgressDialog->incrementStep();
+
+		for(std::map<int, float>::const_iterator iter = event.getDepthFys().begin();
+			iter!=event.getDepthFys().end();
+			++iter)
+		{
+			_depthFysMap.insert(iter->first, iter->second);
+		}
+		_initProgressDialog->appendText(tr("Inserted %1 depth fy parameters.").arg(_depthFysMap.size()));
+		_initProgressDialog->incrementStep();
+
+		for(std::map<int, float>::const_iterator iter = event.getDepthCxs().begin();
+			iter!=event.getDepthCxs().end();
+			++iter)
+		{
+			_depthCxsMap.insert(iter->first, iter->second);
+		}
+		_initProgressDialog->appendText(tr("Inserted %1 depth cx parameters.").arg(_depthCxsMap.size()));
+		_initProgressDialog->incrementStep();
+
+		for(std::map<int, float>::const_iterator iter = event.getDepthCys().begin();
+			iter!=event.getDepthCys().end();
+			++iter)
+		{
+			_depthCysMap.insert(iter->first, iter->second);
+		}
+		_initProgressDialog->appendText(tr("Inserted %1 depth cy parameters.").arg(_depthCysMap.size()));
 		_initProgressDialog->incrementStep();

 		for(std::map<int, std::vector<unsigned char> >::const_iterator iter = event.getDepths2d().begin();
@@ -1966,14 +2015,20 @@ void MainWindow::startDetection()
 					_preferencesDialog->getSourceOpenniDevice().toStdString(),
 					_preferencesDialog->getGeneralInputRate(),
 					_preferencesDialog->getSourceOpenniLocalTransform(),
-					_preferencesDialog->getSourceOpenniFocalLength());
+					_preferencesDialog->getSourceOpenniFx(),
+					_preferencesDialog->getSourceOpenniFy(),
+					_preferencesDialog->getSourceOpenniCx(),
+					_preferencesDialog->getSourceOpenniCy());
 		}
 		else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2)
 		{
 			camera = new CameraOpenNI2(
 					_preferencesDialog->getGeneralInputRate(),
 					_preferencesDialog->getSourceOpenniLocalTransform(),
-					_preferencesDialog->getSourceOpenniFocalLength());
+					_preferencesDialog->getSourceOpenniFx(),
+					_preferencesDialog->getSourceOpenniFy(),
+					_preferencesDialog->getSourceOpenniCx(),
+					_preferencesDialog->getSourceOpenniCy());
 		}
 		else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect)
 		{
@@ -1981,7 +2036,10 @@ void MainWindow::startDetection()
 					_preferencesDialog->getSourceOpenniDevice().isEmpty()?0:atoi(_preferencesDialog->getSourceOpenniDevice().toStdString().c_str()),
 					_preferencesDialog->getGeneralInputRate(),
 					_preferencesDialog->getSourceOpenniLocalTransform(),
-					_preferencesDialog->getSourceOpenniFocalLength());
+					_preferencesDialog->getSourceOpenniFx(),
+					_preferencesDialog->getSourceOpenniFy(),
+					_preferencesDialog->getSourceOpenniCx(),
+					_preferencesDialog->getSourceOpenniCy());
 		}
 		else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV ||
 				_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS)
@@ -1990,7 +2048,10 @@ void MainWindow::startDetection()
 					_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS,
 					_preferencesDialog->getGeneralInputRate(),
 					_preferencesDialog->getSourceOpenniLocalTransform(),
-					_preferencesDialog->getSourceOpenniFocalLength());
+					_preferencesDialog->getSourceOpenniFx(),
+					_preferencesDialog->getSourceOpenniFy(),
+					_preferencesDialog->getSourceOpenniCx(),
+					_preferencesDialog->getSourceOpenniCy());
 		}
 		else
 		{
@@ -2611,7 +2672,10 @@ void MainWindow::clearTheCache()
 	_imagesMap.clear();
 	_depthsMap.clear();
 	_depths2DMap.clear();
-	_depthConstantsMap.clear();
+	_depthFxsMap.clear();
+	_depthFysMap.clear();
+	_depthCxsMap.clear();
+	_depthCysMap.clear();
 	_localTransformsMap.clear();
 	_ui->widget_cloudViewer->removeAllClouds();
 	_ui->widget_cloudViewer->render();
@@ -3481,7 +3545,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
 		int id,
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
-		float depthConstant,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		const Transform & localTransform,
 		const Transform & pose,
 		float voxelSize,
@@ -3492,11 +3559,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
 			rgb,
 			depth,
-		   float(depth.cols/2),
-		   float(depth.rows/2),
-		   1.0f/depthConstant,
-		   1.0f/depthConstant,
-		   decimation);
+		    cx, cy,
+		    fx, fy,
+		    decimation);

 	if(cloud->size())
 	{
@@ -3543,7 +3608,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createAssembledCloud(const st
 				cloud = createCloud(iter->first,
 						util3d::uncompressImage(_imagesMap.value(iter->first)),
 						util3d::uncompressImage(_depthsMap.value(iter->first)),
-						_depthConstantsMap.value(iter->first),
+						_depthFxsMap.value(iter->first),
+						_depthFysMap.value(iter->first),
+						_depthCxsMap.value(iter->first),
+						_depthCysMap.value(iter->first),
 						_localTransformsMap.value(iter->first),
 						iter->second,
 						_preferencesDialog->getCloudVoxelSize(2),
@@ -3619,7 +3687,10 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::createPointCl
 				cloud = createCloud(iter->first,
 						util3d::uncompressImage(_imagesMap.value(iter->first)),
 						util3d::uncompressImage(_depthsMap.value(iter->first)),
-						_depthConstantsMap.value(iter->first),
+						_depthFxsMap.value(iter->first),
+						_depthFysMap.value(iter->first),
+						_depthCxsMap.value(iter->first),
+						_depthCysMap.value(iter->first),
 						_localTransformsMap.value(iter->first),
 						Transform::getIdentity(),
 						_preferencesDialog->getCloudVoxelSize(2),
@@ -3686,7 +3757,10 @@ std::map<int, pcl::PolygonMesh::Ptr> MainWindow::createMeshes(const std::map<int
 				cloud = createCloud(iter->first,
 						util3d::uncompressImage(_imagesMap.value(iter->first)),
 						util3d::uncompressImage(_depthsMap.value(iter->first)),
-						_depthConstantsMap.value(iter->first),
+						_depthFxsMap.value(iter->first),
+						_depthFysMap.value(iter->first),
+						_depthCxsMap.value(iter->first),
+						_depthCysMap.value(iter->first),
 						_localTransformsMap.value(iter->first),
 						Transform::getIdentity(),
 						_preferencesDialog->getCloudVoxelSize(2),
@@ -62,7 +62,7 @@ void OdometryViewer::processData()
 	}
 	dataMutex_.unlock();

-	if(!data.image().empty() && !data.depth().empty() && data.depthConstant()>0.0f && this->isVisible())
+	if(!data.image().empty() && !data.depth().empty() && data.depthFx()>0.0f && data.depthFy()>0.0f && this->isVisible())
 	{
 		UDEBUG("New pose = %s, quality=%d", data.pose().prettyPrint().c_str(), quality);

@@ -72,11 +72,9 @@ void OdometryViewer::processData()
 		cloud = util3d::cloudFromDepthRGB(
 				data.image(),
 				data.depth(),
-			   float(data.depth().cols/2),
-			   float(data.depth().rows/2),
-			   1.0f/data.depthConstant(),
-			   1.0f/data.depthConstant(),
-			   decimation_);
+				data.depthCx(), data.depthCy(),
+			    data.depthFx(), data.depthFy(),
+			    decimation_);

 		if(voxelSize_ > 0.0f)
 		{
@@ -20,6 +20,7 @@
 #include "rtabmap/gui/PreferencesDialog.h"
 #include "rtabmap/gui/DatabaseViewer.h"
 #include "rtabmap/gui/OdometryViewer.h"
+#include "rtabmap/gui/CalibrationDialog.h"

 #include <QtCore/QSettings>
 #include <QtCore/QDir>
@@ -256,7 +257,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->radioButton_openni2->setEnabled(CameraOpenNI2::available());
 	connect(_ui->lineEdit_openniDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_openniLocalTransform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->doubleSpinBox_openniFocal, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_openniFx, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_openniFy, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_openniCx, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_openniCy, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));


 	//Rtabmap basic
@@ -807,7 +812,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->radioButton_opennicvasus->setChecked(false);
 		_ui->lineEdit_openniDevice->setText("");
 		_ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2");
-		_ui->doubleSpinBox_openniFocal->setValue(0.0);
+		_ui->doubleSpinBox_openniFx->setValue(0.0);
+		_ui->doubleSpinBox_openniFy->setValue(0.0);
+		_ui->doubleSpinBox_openniCx->setValue(0.0);
+		_ui->doubleSpinBox_openniCy->setValue(0.0);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_rtabmap_basic0->objectName())
 	{
@@ -1063,7 +1071,10 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->radioButton_opennicvasus->setChecked(settings.value("openniCvAsusType", _ui->radioButton_opennicvasus->isChecked()).toBool());
 	_ui->lineEdit_openniDevice->setText(settings.value("device",_ui->lineEdit_openniDevice->text()).toString());
 	_ui->lineEdit_openniLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_openniLocalTransform->text()).toString());
-	_ui->doubleSpinBox_openniFocal->setValue(settings.value("focalLength", _ui->doubleSpinBox_openniFocal->value()).toDouble());
+	_ui->doubleSpinBox_openniFx->setValue(settings.value("fx", _ui->doubleSpinBox_openniFx->value()).toDouble());
+	_ui->doubleSpinBox_openniFy->setValue(settings.value("fy", _ui->doubleSpinBox_openniFy->value()).toDouble());
+	_ui->doubleSpinBox_openniCx->setValue(settings.value("cx", _ui->doubleSpinBox_openniCx->value()).toDouble());
+	_ui->doubleSpinBox_openniCy->setValue(settings.value("cy", _ui->doubleSpinBox_openniCy->value()).toDouble());
 	settings.endGroup(); // Openni
 }

@@ -1273,7 +1284,10 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath)
 	settings.setValue("openniCvAsusType", 	_ui->radioButton_opennicvasus->isChecked());
 	settings.setValue("device", 		_ui->lineEdit_openniDevice->text());
 	settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text());
-	settings.setValue("focalLength", _ui->doubleSpinBox_openniFocal->value());
+	settings.setValue("fx", _ui->doubleSpinBox_openniFx->value());
+	settings.setValue("fy", _ui->doubleSpinBox_openniFy->value());
+	settings.setValue("cx", _ui->doubleSpinBox_openniCx->value());
+	settings.setValue("cy", _ui->doubleSpinBox_openniCy->value());
 	settings.endGroup();
 }

@@ -2640,9 +2654,21 @@ Transform PreferencesDialog::getSourceOpenniLocalTransform() const
 	return t;
 }

-float PreferencesDialog::getSourceOpenniFocalLength() const
+float PreferencesDialog::getSourceOpenniFx() const
 {
-	return _ui->doubleSpinBox_openniFocal->value();
+	return _ui->doubleSpinBox_openniFx->value();
+}
+float PreferencesDialog::getSourceOpenniFy() const
+{
+	return _ui->doubleSpinBox_openniFy->value();
+}
+float PreferencesDialog::getSourceOpenniCx() const
+{
+	return _ui->doubleSpinBox_openniCx->value();
+}
+float PreferencesDialog::getSourceOpenniCy() const
+{
+	return _ui->doubleSpinBox_openniCy->value();
 }

 bool PreferencesDialog::isStatisticsPublished() const
@@ -2778,14 +2804,20 @@ void PreferencesDialog::testOdometry(int type)
 			this->getSourceOpenniDevice().toStdString(),
 			this->getGeneralInputRate(),
 			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFocalLength());
+			this->getSourceOpenniFx(),
+			this->getSourceOpenniFy(),
+			this->getSourceOpenniCx(),
+			this->getSourceOpenniCy());
 	}
 	else if(this->getSourceRGBD() == kSrcOpenNI2)
 	{
 		camera = new CameraOpenNI2(
 			this->getGeneralInputRate(),
 			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFocalLength());
+			this->getSourceOpenniFx(),
+			this->getSourceOpenniFy(),
+			this->getSourceOpenniCx(),
+			this->getSourceOpenniCy());

 	}
 	else if(this->getSourceRGBD() == kSrcFreenect)
@@ -2794,7 +2826,10 @@ void PreferencesDialog::testOdometry(int type)
 			this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
 			this->getGeneralInputRate(),
 			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFocalLength());
+			this->getSourceOpenniFx(),
+			this->getSourceOpenniFy(),
+			this->getSourceOpenniCx(),
+			this->getSourceOpenniCy());
 	}
 	else if(this->getSourceRGBD() == kSrcOpenNI_CV || this->getSourceRGBD() == kSrcOpenNI_CV_ASUS)
 	{
@@ -2802,7 +2837,10 @@ void PreferencesDialog::testOdometry(int type)
 			this->getSourceRGBD() == kSrcOpenNI_CV_ASUS,
 			this->getGeneralInputRate(),
 			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFocalLength());
+			this->getSourceOpenniFx(),
+			this->getSourceOpenniFy(),
+			this->getSourceOpenniCx(),
+			this->getSourceOpenniCy());
 	}
 	else
 	{
@@ -2881,4 +2919,90 @@ void PreferencesDialog::cleanOdometryTest()
 	_ui->pushButton_testOdometry->setEnabled(true);
 }

+
+void PreferencesDialog::calibrate()
+{
+	CameraRGBD * camera = 0;
+	if(this->getSourceRGBD() == kSrcOpenNI_PCL)
+	{
+		camera = new CameraOpenni(
+			this->getSourceOpenniDevice().toStdString(),
+			this->getGeneralInputRate(),
+			this->getSourceOpenniLocalTransform(),
+			this->getSourceOpenniFx(),
+			this->getSourceOpenniFy(),
+			this->getSourceOpenniCx(),
+			this->getSourceOpenniCy());
+	}
+	else if(this->getSourceRGBD() == kSrcOpenNI2)
+	{
+		camera = new CameraOpenNI2(
+			this->getGeneralInputRate(),
+			this->getSourceOpenniLocalTransform(),
+			this->getSourceOpenniFx(),
+			this->getSourceOpenniFy(),
+			this->getSourceOpenniCx(),
+			this->getSourceOpenniCy());
+
+	}
+	else if(this->getSourceRGBD() == kSrcFreenect)
+	{
+		camera = new CameraFreenect(
+			this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
+			this->getGeneralInputRate(),
+			this->getSourceOpenniLocalTransform(),
+			this->getSourceOpenniFx(),
+			this->getSourceOpenniFy(),
+			this->getSourceOpenniCx(),
+			this->getSourceOpenniCy());
+	}
+	else if(this->getSourceRGBD() == kSrcOpenNI_CV || this->getSourceRGBD() == kSrcOpenNI_CV_ASUS)
+	{
+		camera = new CameraOpenNICV(
+			this->getSourceRGBD() == kSrcOpenNI_CV_ASUS,
+			this->getGeneralInputRate(),
+			this->getSourceOpenniLocalTransform(),
+			this->getSourceOpenniFx(),
+			this->getSourceOpenniFy(),
+			this->getSourceOpenniCx(),
+			this->getSourceOpenniCy());
+	}
+	else
+	{
+		UFATAL("RGBD Source type undefined!");
+	}
+
+	if(!camera->init())
+	{
+		QMessageBox::warning(this,
+			   tr("RTAB-Map"),
+			   tr("RGBD camera initialization failed!"));
+		delete camera;
+		camera = 0;
+	}
+
+
+	if(camera)
+	{
+		CalibrationDialog * dialog = new CalibrationDialog(this);
+		dialog->registerToEventsManager();
+
+		CameraThread cameraThread(camera);
+		UEventsManager::createPipe(&cameraThread, dialog, "CameraEvent");
+
+		cameraThread.start();
+
+		if(dialog->exec() == QDialog::Accepted)
+		{
+			_ui->doubleSpinBox_openniFx->setValue(dialog->fx());
+			_ui->doubleSpinBox_openniFy->setValue(dialog->fy());
+			_ui->doubleSpinBox_openniCx->setValue(dialog->cx());
+			_ui->doubleSpinBox_openniCy->setValue(dialog->cy());
+		}
+
+		cameraThread.join(true);
+		delete dialog;
+	}
+}
+
 }
@@ -222,7 +222,7 @@ p, li { white-space: pre-wrap; }
        <item>
         <widget class="QLabel" name="label_5">
          <property name="text">
-           <string>Copyright (C) 2011 IntRoLab - Université de Sherbrooke</string>
+           <string>Copyright (C) 2010-2014 IntRoLab - Université de Sherbrooke</string>
          </property>
          <property name="alignment">
           <set>Qt::AlignCenter</set>
@@ -6,8 +6,8 @@
   <rect>
    <x>0</x>
    <y>0</y>
-    <width>629</width>
-    <height>452</height>
+    <width>1040</width>
+    <height>745</height>
   </rect>
  </property>
  <property name="windowTitle">
@@ -33,84 +33,158 @@
     <item>
      <layout class="QVBoxLayout" name="verticalLayout">
       <item>
-        <layout class="QFormLayout" name="formLayout">
-         <item row="0" column="0">
-          <widget class="QLabel" name="label_5">
-           <property name="text">
-            <string>Count</string>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="1">
-          <widget class="QProgressBar" name="progressBar_count">
-           <property name="value">
-            <number>24</number>
-           </property>
-           <property name="format">
-            <string>%p</string>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="0">
-          <widget class="QLabel" name="label">
-           <property name="text">
-            <string>X</string>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="1">
-          <widget class="QProgressBar" name="progressBar_x">
-           <property name="value">
-            <number>24</number>
-           </property>
-           <property name="format">
-            <string>%p%</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="0">
-          <widget class="QLabel" name="label_2">
-           <property name="text">
-            <string>Y</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="1">
-          <widget class="QProgressBar" name="progressBar_y">
-           <property name="value">
-            <number>24</number>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="0">
-          <widget class="QLabel" name="label_3">
-           <property name="text">
-            <string>Size</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="1">
-          <widget class="QProgressBar" name="progressBar_size">
-           <property name="value">
-            <number>24</number>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="0">
-          <widget class="QLabel" name="label_4">
-           <property name="text">
-            <string>Skew</string>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="1">
-          <widget class="QProgressBar" name="progressBar_skew">
-           <property name="value">
-            <number>24</number>
-           </property>
-          </widget>
-         </item>
-        </layout>
+        <widget class="QGroupBox" name="groupBox">
+         <property name="title">
+          <string>Chessboard</string>
+         </property>
+         <layout class="QFormLayout" name="formLayout_3">
+          <item row="0" column="0">
+           <widget class="QLabel" name="label_12">
+            <property name="toolTip">
+             <string>Number of inner squares on the board</string>
+            </property>
+            <property name="text">
+             <string>Board width</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="1">
+           <widget class="QSpinBox" name="spinBox_boardWidth">
+            <property name="minimum">
+             <number>2</number>
+            </property>
+            <property name="value">
+             <number>8</number>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QLabel" name="label_15">
+            <property name="toolTip">
+             <string>Number of inner squares on the board</string>
+            </property>
+            <property name="text">
+             <string>Board height</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="1">
+           <widget class="QSpinBox" name="spinBox_boardHeight">
+            <property name="minimum">
+             <number>2</number>
+            </property>
+            <property name="value">
+             <number>6</number>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="0">
+           <widget class="QLabel" name="label_16">
+            <property name="text">
+             <string>Square size (m)</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_squareSize">
+            <property name="decimals">
+             <number>3</number>
+            </property>
+            <property name="maximum">
+             <double>1.000000000000000</double>
+            </property>
+            <property name="value">
+             <double>0.033000000000000</double>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item>
+        <widget class="QGroupBox" name="groupBox_2">
+         <property name="title">
+          <string>Progress</string>
+         </property>
+         <layout class="QFormLayout" name="formLayout">
+          <item row="0" column="0">
+           <widget class="QLabel" name="label_5">
+            <property name="text">
+             <string>Count</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="1">
+           <widget class="QProgressBar" name="progressBar_count">
+            <property name="value">
+             <number>24</number>
+            </property>
+            <property name="format">
+             <string>%p</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QLabel" name="label">
+            <property name="text">
+             <string>X</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="1">
+           <widget class="QProgressBar" name="progressBar_x">
+            <property name="value">
+             <number>24</number>
+            </property>
+            <property name="format">
+             <string>%p%</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="0">
+           <widget class="QLabel" name="label_2">
+            <property name="text">
+             <string>Y</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QProgressBar" name="progressBar_y">
+            <property name="value">
+             <number>24</number>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="0">
+           <widget class="QLabel" name="label_3">
+            <property name="text">
+             <string>Size</string>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="1">
+           <widget class="QProgressBar" name="progressBar_size">
+            <property name="value">
+             <number>24</number>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="0">
+           <widget class="QLabel" name="label_4">
+            <property name="text">
+             <string>Skew</string>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="1">
+           <widget class="QProgressBar" name="progressBar_skew">
+            <property name="value">
+             <number>24</number>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
       </item>
       <item>
        <widget class="QPushButton" name="pushButton_calibrate">
@@ -120,134 +194,139 @@
        </widget>
       </item>
       <item>
-        <layout class="QFormLayout" name="formLayout_2">
-         <item row="0" column="0">
-          <widget class="QLabel" name="label_8">
-           <property name="text">
-            <string>fx</string>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="1">
-          <widget class="QLabel" name="label_fx">
-           <property name="text">
-            <string>0</string>
-           </property>
-           <property name="textInteractionFlags">
-            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="0">
-          <widget class="QLabel" name="label_9">
-           <property name="text">
-            <string>fy</string>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="1">
-          <widget class="QLabel" name="label_fy">
-           <property name="text">
-            <string>0</string>
-           </property>
-           <property name="textInteractionFlags">
-            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="0">
-          <widget class="QLabel" name="label_10">
-           <property name="text">
-            <string>cx</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="1">
-          <widget class="QLabel" name="label_cx">
-           <property name="text">
-            <string>0</string>
-           </property>
-           <property name="textInteractionFlags">
-            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="0">
-          <widget class="QLabel" name="label_11">
-           <property name="text">
-            <string>cy</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="1">
-          <widget class="QLabel" name="label_cy">
-           <property name="text">
-            <string>0</string>
-           </property>
-           <property name="textInteractionFlags">
-            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="0">
-          <widget class="QLabel" name="label_6">
-           <property name="toolTip">
-            <string>Camera matrix</string>
-           </property>
-           <property name="text">
-            <string>K</string>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="1">
-          <widget class="QLineEdit" name="lineEdit_K">
-           <property name="readOnly">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="5" column="0">
-          <widget class="QLabel" name="label_7">
-           <property name="toolTip">
-            <string>Distorsion coefficients</string>
-           </property>
-           <property name="text">
-            <string>D</string>
-           </property>
-          </widget>
-         </item>
-         <item row="5" column="1">
-          <widget class="QLineEdit" name="lineEdit_D">
-           <property name="readOnly">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="8" column="0">
-          <widget class="QLabel" name="label_13">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
-         <item row="6" column="0">
-          <widget class="QLabel" name="label_14">
-           <property name="toolTip">
-            <string>Avg. reproduction error</string>
-           </property>
-           <property name="text">
-            <string>Error</string>
-           </property>
-          </widget>
-         </item>
-         <item row="6" column="1">
-          <widget class="QLabel" name="label_error">
-           <property name="text">
-            <string>0</string>
-           </property>
-          </widget>
-         </item>
-        </layout>
+        <widget class="QGroupBox" name="groupBox_3">
+         <property name="title">
+          <string>Camera intrinsic parameters</string>
+         </property>
+         <layout class="QFormLayout" name="formLayout_2">
+          <item row="0" column="0">
+           <widget class="QLabel" name="label_8">
+            <property name="text">
+             <string>fx</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="1">
+           <widget class="QLabel" name="label_fx">
+            <property name="text">
+             <string>0</string>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QLabel" name="label_9">
+            <property name="text">
+             <string>fy</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="1">
+           <widget class="QLabel" name="label_fy">
+            <property name="text">
+             <string>0</string>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="0">
+           <widget class="QLabel" name="label_10">
+            <property name="text">
+             <string>cx</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QLabel" name="label_cx">
+            <property name="text">
+             <string>0</string>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="0">
+           <widget class="QLabel" name="label_11">
+            <property name="text">
+             <string>cy</string>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="1">
+           <widget class="QLabel" name="label_cy">
+            <property name="text">
+             <string>0</string>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="0">
+           <widget class="QLabel" name="label_6">
+            <property name="toolTip">
+             <string>Camera matrix</string>
+            </property>
+            <property name="text">
+             <string>K</string>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="1">
+           <widget class="QLineEdit" name="lineEdit_K">
+            <property name="readOnly">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="0">
+           <widget class="QLabel" name="label_7">
+            <property name="toolTip">
+             <string>Distorsion coefficients</string>
+            </property>
+            <property name="text">
+             <string>D</string>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="1">
+           <widget class="QLineEdit" name="lineEdit_D">
+            <property name="readOnly">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="0">
+           <widget class="QLabel" name="label_14">
+            <property name="toolTip">
+             <string>Avg. reproduction error</string>
+            </property>
+            <property name="text">
+             <string>Error</string>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="1">
+           <widget class="QLabel" name="label_error">
+            <property name="text">
+             <string>0</string>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="0" colspan="2">
+           <widget class="QLabel" name="label_13">
+            <property name="text">
+             <string/>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
       </item>
       <item>
        <layout class="QHBoxLayout" name="horizontalLayout">
@@ -258,13 +337,6 @@
           </property>
          </widget>
         </item>
-         <item>
-          <widget class="QPushButton" name="pushButton_save">
-           <property name="text">
-            <string>Save</string>
-           </property>
-          </widget>
-         </item>
        </layout>
       </item>
       <item>
@@ -285,28 +357,11 @@
    </layout>
   </item>
   <item>
-    <layout class="QHBoxLayout" name="horizontalLayout_3">
-     <item>
-      <spacer name="horizontalSpacer">
-       <property name="orientation">
-        <enum>Qt::Horizontal</enum>
-       </property>
-       <property name="sizeHint" stdset="0">
-        <size>
-         <width>40</width>
-         <height>20</height>
-        </size>
-       </property>
-      </spacer>
-     </item>
-     <item>
-      <widget class="QPushButton" name="pushButton_exit">
-       <property name="text">
-        <string>Exit</string>
-       </property>
-      </widget>
-     </item>
-    </layout>
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+     </property>
+    </widget>
   </item>
  </layout>
 </widget>
@@ -6,7 +6,7 @@
   <rect>
    <x>0</x>
    <y>0</y>
-    <width>1007</width>
+    <width>1035</width>
    <height>628</height>
   </rect>
  </property>
@@ -63,9 +63,9 @@
          <property name="geometry">
           <rect>
            <x>0</x>
-            <y>0</y>
-            <width>709</width>
-            <height>853</height>
+            <y>-617</y>
+            <width>732</width>
+            <height>1198</height>
           </rect>
          </property>
          <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                <enum>QFrame::Raised</enum>
               </property>
               <property name="currentIndex">
-                <number>1</number>
+                <number>3</number>
               </property>
               <widget class="QWidget" name="page_22">
                <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1763,25 +1763,97 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                           </property>
                          </widget>
                         </item>
-                         <item row="2" column="0">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_openniFocal">
-                           <property name="maximum">
-                            <double>1000.000000000000000</double>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="2" column="1">
-                          <widget class="QLabel" name="label_160">
-                           <property name="text">
-                            <string>Intrinsic focal length (fx=fy, K[4]). Set 0 to use the default from the camera. Examples:  ~525 for Kinect, ~545 for Xtion Pro Live.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
                        </layout>
                       </item>
+                       <item>
+                        <widget class="QGroupBox" name="groupBox_3">
+                         <property name="title">
+                          <string>Calibration</string>
+                         </property>
+                         <layout class="QVBoxLayout" name="verticalLayout_41">
+                          <item>
+                           <widget class="QLabel" name="label_160">
+                            <property name="text">
+                             <string>Camera intrinsic parameters. While the default values are good, a calibration is recommended to increase the precision of the 3D clouds. Set values to 0 to use the default parameters from the camera. For example, the focal length is ~525 for Kinect and ~545 for Xtion Pro Live. The button &quot;Calibrate&quot; below can be used to calibrate the camera with a chessboard pattern.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item>
+                           <layout class="QFormLayout" name="formLayout_21">
+                            <property name="fieldGrowthPolicy">
+                             <enum>QFormLayout::ExpandingFieldsGrow</enum>
+                            </property>
+                            <item row="0" column="0">
+                             <widget class="QDoubleSpinBox" name="doubleSpinBox_openniFx">
+                              <property name="maximum">
+                               <double>1000.000000000000000</double>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="0" column="1">
+                             <widget class="QLabel" name="label_161">
+                              <property name="text">
+                               <string>fx=K[0]</string>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="1" column="0">
+                             <widget class="QDoubleSpinBox" name="doubleSpinBox_openniFy">
+                              <property name="maximum">
+                               <double>1000.000000000000000</double>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="1" column="1">
+                             <widget class="QLabel" name="label_162">
+                              <property name="text">
+                               <string>fy=K[4]</string>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="2" column="0">
+                             <widget class="QDoubleSpinBox" name="doubleSpinBox_openniCx">
+                              <property name="maximum">
+                               <double>1000.000000000000000</double>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="2" column="1">
+                             <widget class="QLabel" name="label_166">
+                              <property name="text">
+                               <string>cx=K[2]</string>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="3" column="0">
+                             <widget class="QDoubleSpinBox" name="doubleSpinBox_openniCy">
+                              <property name="maximum">
+                               <double>1000.000000000000000</double>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="3" column="1">
+                             <widget class="QLabel" name="label_165">
+                              <property name="text">
+                               <string>cy=K[5]</string>
+                              </property>
+                             </widget>
+                            </item>
+                           </layout>
+                          </item>
+                          <item>
+                           <widget class="QPushButton" name="pushButton_calibrate">
+                            <property name="text">
+                             <string>Calibrate</string>
+                            </property>
+                           </widget>
+                          </item>
+                         </layout>
+                        </widget>
+                       </item>
                      </layout>
                     </widget>
                    </item>
@@ -107,8 +107,8 @@ int main(int argc, char * argv[])
 	}

 	cv::Mat rgb, depth;
-	float constant;
-	camera->takeImage(rgb, depth, constant);
+	float fx, fy, cx, cy;
+	camera->takeImage(rgb, depth, fx, fy, cx, cy);
 	cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
 	cv::namedWindow("Depth", CV_WINDOW_AUTOSIZE); // create window
 	pcl::visualization::CloudViewer viewer("cloud");
@@ -120,7 +120,7 @@ int main(int argc, char * argv[])
 		cv::imshow("Video", rgb); // show frame
 		cv::imshow("Depth",tmp);

-		viewer.showCloud(rtabmap::util3d::cloudFromDepthRGB(rgb, depth, constant), "cloud");
+		viewer.showCloud(rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy), "cloud");

 		int c = cv::waitKey(10); // wait 10 ms or for key stroke
 		if(c == 27)
@@ -128,7 +128,7 @@ int main(int argc, char * argv[])

 		rgb = cv::Mat();
 		depth = cv::Mat();
-		camera->takeImage(rgb, depth, constant);
+		camera->takeImage(rgb, depth, fx, fy, cx, cy);
 	}
 	cv::destroyWindow("Video");
 	cv::destroyWindow("Depth");