Added OpenNI2 and Freenect calibration #115

corelib/include/rtabmap/core/CameraRGBD.h

@@ -155,9 +155,11 @@ class RTABMAP_EXP CameraOpenNI2 :
 public:
 	static bool available();
 	static bool exposureGainAvailable();
+	enum Type {kTypeColorDepth, kTypeIRDepth, kTypeIR};
 
 public:
 	CameraOpenNI2(const std::string & deviceId = "",
+					Type type = kTypeColorDepth,
 					float imageRate = 0,
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenNI2();
@@ -177,6 +179,7 @@ protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
+	Type _type;
 	openni::Device * _device;
 	openni::VideoStream * _color;
 	openni::VideoStream * _depth;
@@ -184,6 +187,7 @@ private:
 	float _depthFy;
 	std::string _deviceId;
 	bool _openNI2StampsAndIDsUsed;
+	StereoCameraModel _stereoModel;
 };
 
 
@@ -197,10 +201,12 @@ class RTABMAP_EXP CameraFreenect :
 {
 public:
 	static bool available();
+	enum Type {kTypeColorDepth, kTypeIRDepth};
 
 public:
 	// default local transform z in, x right, y down));
 	CameraFreenect(int deviceId= 0,
+					Type type = kTypeColorDepth,
 					float imageRate=0.0f,
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraFreenect();
@@ -214,8 +220,10 @@ protected:
 
 private:
 	int deviceId_;
+	Type type_;
 	freenect_context * ctx_;
 	FreenectDevice * freenectDevice_;
+	StereoCameraModel stereoModel_;
 };
 
 /////////////////////////

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -87,7 +87,7 @@ public:
 
 	void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();}
 
-	void setName(const std::string & name);
+	void setName(const std::string & name, const std::string & leftSuffix = "left", const std::string & rightSuffix = "right");
 	const std::string & name() const {return name_;}
 
 	// backward compatibility
@@ -95,6 +95,7 @@ public:
 
 	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
 	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
+	bool saveStereoTransform(const std::string & directory) const;
 
 	double baseline() const {return right_.fx()!=0.0?-right_.Tx()/right_.fx():0.0;}
 
@@ -117,7 +118,12 @@ public:
 	const CameraModel & left() const {return left_;}
 	const CameraModel & right() const {return right_;}
 
+	const std::string & getLeftSuffix() const {return leftSuffix_;}
+	const std::string & getRightSuffix() const {return rightSuffix_;}
+
 private:
+	std::string leftSuffix_;
+	std::string rightSuffix_;
 	CameraModel left_;
 	CameraModel right_;
 	std::string name_;

corelib/include/rtabmap/core/util2d.h

@@ -121,6 +121,7 @@ cv::Mat RTABMAP_EXP interpolate(const cv::Mat & image, int factor, float depthEr
 cv::Mat RTABMAP_EXP registerDepth(
 		const cv::Mat & depth,
 		const cv::Mat & depthK,
+		const cv::Size & colorSize,
 		const cv::Mat & colorK,
 		const rtabmap::Transform & transform);