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-02 01:20:25 +08:00 · 2014-07-26 04:36:00 +00:00
parent 1557b3d44d
commit f090c81aaf
44 changed files with 1389 additions and 715 deletions
--- a/corelib/include/rtabmap/core/CameraEvent.h
+++ b/corelib/include/rtabmap/core/CameraEvent.h
@@ -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)
 	{
 	}

--- a/corelib/include/rtabmap/core/CameraRGBD.h
+++ b/corelib/include/rtabmap/core/CameraRGBD.h
@@ -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_;
--- a/corelib/include/rtabmap/core/DBDriver.h
+++ b/corelib/include/rtabmap/core/DBDriver.h
@@ -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;
--- a/corelib/include/rtabmap/core/DBReader.h
+++ b/corelib/include/rtabmap/core/DBReader.h
@@ -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();
--- a/corelib/include/rtabmap/core/Features2d.h
+++ b/corelib/include/rtabmap/core/Features2d.h
@@ -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);
--- a/corelib/include/rtabmap/core/Image.h
+++ b/corelib/include/rtabmap/core/Image.h
@@ -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;
 };
--- a/corelib/include/rtabmap/core/Memory.h
+++ b/corelib/include/rtabmap/core/Memory.h
@@ -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);

--- a/corelib/include/rtabmap/core/Rtabmap.h
+++ b/corelib/include/rtabmap/core/Rtabmap.h
@@ -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,
--- a/corelib/include/rtabmap/core/RtabmapEvent.h
+++ b/corelib/include/rtabmap/core/RtabmapEvent.h
@@ -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;
--- a/corelib/include/rtabmap/core/Signature.h
+++ b/corelib/include/rtabmap/core/Signature.h
@@ -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>
--- a/corelib/include/rtabmap/core/Statistics.h
+++ b/corelib/include/rtabmap/core/Statistics.h
@@ -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;
--- a/corelib/include/rtabmap/core/util3d.h
+++ b/corelib/include/rtabmap/core/util3d.h
@@ -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,
--- a/corelib/src/CMakeLists.txt
+++ b/corelib/src/CMakeLists.txt
@@ -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
 )
--- a/corelib/src/CameraRGBD.cpp
+++ b/corelib/src/CameraRGBD.cpp
@@ -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
--- a/corelib/src/CameraThread.cpp
+++ b/corelib/src/CameraThread.cpp
@@ -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
 		{
--- a/corelib/src/DBDriver.cpp
+++ b/corelib/src/DBDriver.cpp
@@ -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();
 }

--- a/corelib/src/DBDriverSqlite3.cpp
+++ b/corelib/src/DBDriverSqlite3.cpp
@@ -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());
--- a/corelib/src/DBDriverSqlite3.h
+++ b/corelib/src/DBDriverSqlite3.h
@@ -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;
--- a/corelib/src/DBReader.cpp
+++ b/corelib/src/DBReader.cpp
@@ -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())
--- a/corelib/src/Features2d.cpp
+++ b/corelib/src/Features2d.cpp
@@ -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];
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -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
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -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;
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -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())
 			{
--- a/corelib/src/RtabmapThread.cpp
+++ b/corelib/src/RtabmapThread.cpp
@@ -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));
--- a/corelib/src/Signature.cpp
+++ b/corelib/src/Signature.cpp
@@ -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
--- a/corelib/src/resources/DatabaseSchema.sql.in
+++ b/corelib/src/resources/DatabaseSchema.sql.in
@@ -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@');
+
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -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())