Refactoring: Renamed rtabmap::Image to rtabmap::SensorData

removed keypoints and descriptors stuff from CameraEvent and Image/SensorData
removed keypoints3 from Image/SensorData

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1653 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/Camera.h

@@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
 #include <opencv2/highgui/highgui.hpp>
-#include "rtabmap/core/Image.h"
+#include "rtabmap/core/SensorData.h"
 #include <set>
 #include <stack>
 #include <list>

corelib/include/rtabmap/core/CameraEvent.h

@@ -27,10 +27,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/features2d/features2d.hpp>
 #include <rtabmap/utilite/UEvent.h>
-#include "rtabmap/core/Image.h"
+#include "rtabmap/core/SensorData.h"
 
 namespace rtabmap
 {
@@ -40,7 +38,6 @@ class CameraEvent :
 {
 public:
 	enum Code {
-		kCodeFeatures,
 		kCodeImage,
 		kCodeImageDepth,
 		kCodeNoMoreImages
@@ -49,12 +46,7 @@ public:
 public:
 	CameraEvent(const cv::Mat & image, int seq=0) :
 		UEvent(kCodeImage),
-		_image(image, seq)
-	{
-	}
-	CameraEvent(const cv::Mat & descriptors, Feature2D::Type featureType, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int seq=0) :
-		UEvent(kCodeFeatures),
-		_image(image, seq, descriptors, featureType, keypoints)
+		data_(image, seq)
 	{
 	}
 	CameraEvent() :
@@ -63,23 +55,23 @@ public:
 	}
 	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, fx, fy, cx, cy, Transform(), localTransform, seq)
+		data_(image, depth, fx, fy, cx, cy, Transform(), localTransform, seq)
 	{
 	}
 	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, fx, fy, cx, cy, Transform(), localTransform, seq)
+		data_(image, depth, depth2d, fx, fy, cx, cy, Transform(), localTransform, seq)
 	{
 	}
 
 	// Image or descriptors
-	const Image & image() const {return _image;}
+	const SensorData & data() const {return data_;}
 
 	virtual ~CameraEvent() {}
 	virtual std::string getClassName() const {return std::string("CameraEvent");}
 
 private:
-	Image _image;
+	SensorData data_;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraRGBD.h

@@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
 #include <opencv2/highgui/highgui.hpp>
-#include "rtabmap/core/Image.h"
+#include "rtabmap/core/SensorData.h"
 #include "rtabmap/utilite/UMutex.h"
 #include "rtabmap/utilite/USemaphore.h"
 #include <set>

corelib/include/rtabmap/core/Features2d.h

@@ -66,21 +66,8 @@ void RTABMAP_EXP filterKeypointsByDepth(
 		float cy,
 		float maxDepth);
 
-void RTABMAP_EXP filterKeypointsByDepth(
-		std::vector<cv::KeyPoint> & keypoints,
-		std::vector<cv::Point3f> & keypoints3,
-		float maxDepth);
-
-void RTABMAP_EXP filterKeypointsByDepth(
-		std::vector<cv::KeyPoint> & keypoints,
-		std::vector<cv::Point3f> & keypoints3,
-		cv::Mat & descriptors,
-		float maxDepth);
-
 void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
 void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
-void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, int maxKeypoints);
-void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, cv::Mat & descriptors, int maxKeypoints);
 
 cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
 
@@ -96,6 +83,7 @@ public:
 	cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 	virtual void parseParameters(const ParametersMap & parameters) {}
+	virtual Feature2D::Type getType() const = 0;
 
 protected:
 	Feature2D(const ParametersMap & parameters = ParametersMap()) {}
@@ -113,6 +101,7 @@ public:
 	virtual ~SURF();
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureSurf;}
 
 private:
 	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
@@ -139,6 +128,7 @@ public:
 	virtual ~SIFT();
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureSift;}
 
 private:
 	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
@@ -162,6 +152,7 @@ public:
 	virtual ~ORB();
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureOrb;}
 
 private:
 	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
@@ -215,6 +206,7 @@ public:
 	virtual ~FAST_BRIEF();
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureFastBrief;}
 
 private:
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
@@ -233,6 +225,7 @@ public:
 	virtual ~FAST_FREAK();
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureFastFreak;}
 
 private:
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;

corelib/include/rtabmap/core/Image.h

@@ -28,92 +28,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef IMAGE_H_
 #define IMAGE_H_
 
-#include <opencv2/core/core.hpp>
-#include <opencv2/features2d/features2d.hpp>
-
-#include <rtabmap/core/Transform.h>
-#include <rtabmap/core/Features2d.h>
-
-namespace rtabmap
-{
-
-/**
- * An id is automatically generated if id=0.
- */
-class Image
-{
-public:
-	Image(const cv::Mat & image = cv::Mat(),
-		  int id = 0,
-		  const cv::Mat & descriptors = cv::Mat(),
-		  Feature2D::Type featureType = Feature2D::kFeatureUndef,
-		  const std::vector<cv::KeyPoint> & keypoints = std::vector<cv::KeyPoint>());
-
-	// Metric constructor
-	Image(const cv::Mat & image,
-		  const cv::Mat & depth,
-		  float fx,
-		  float fy,
-		  float cx,
-		  float cy,
-		  const Transform & pose,
-		  const Transform & localTransform,
-		  int id = 0);
-
-	// Metric constructor + 2d depth
-	Image(const cv::Mat & image,
-		  const cv::Mat & depth,
-		  const cv::Mat & depth2d,
-		  float fx,
-		  float fy,
-		  float cx,
-		  float cy,
-		  const Transform & pose,
-		  const Transform & localTransform,
-		  int id = 0);
-
-	virtual ~Image() {}
-
-	bool empty() const {return _image.empty() && _descriptors.empty() && _keypoints.size() == 0;}
-	const cv::Mat & image() const {return _image;}
-	int id() const {return _id;};
-	const cv::Mat & descriptors() const {return _descriptors;}
-	Feature2D::Type featureType() const {return _featureType;}
-	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
-	const std::vector<cv::Point3f> & keypoints3() const {return _keypoints3;}
-	void setDescriptors(const cv::Mat & descriptors, Feature2D::Type featureType) {_descriptors = descriptors; _featureType=featureType;}
-	void setKeypoints(const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> * keypoints3 = 0);
-
-	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 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;}
-
-private:
-	cv::Mat _image;
-	int _id;
-	cv::Mat _descriptors;
-	Feature2D::Type _featureType;
-	std::vector<cv::KeyPoint> _keypoints;
-	std::vector<cv::Point3f> _keypoints3;
-
-	// Metric stuff
-	cv::Mat _depth;
-	cv::Mat _depth2d;
-	float _fx;
-	float _fy;
-	float _cx;
-	float _cy;
-	Transform _pose;
-	Transform _localTransform;
-};
+#include <rtabmap/core/SensorData.h>
 
+namespace rtabmap {
+RTABMAP_DEPRECATED(typedef SensorData Image, "rtabmap::Image class is renamed to rtabmap::SensorData, use the last one instead.");
 }
 
 

corelib/include/rtabmap/core/Memory.h

@@ -32,8 +32,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/utilite/UEventsHandler.h"
 #include "rtabmap/core/Parameters.h"
-#include "rtabmap/core/Image.h"
+#include "rtabmap/core/SensorData.h"
 #include "rtabmap/core/Link.h"
+#include "rtabmap/core/Features2d.h"
 #include <typeinfo>
 #include <list>
 #include <map>
@@ -64,7 +65,7 @@ public:
 	virtual ~Memory();
 
 	virtual void parseParameters(const ParametersMap & parameters);
-	bool update(const Image & image, Statistics * stats = 0);
+	bool update(const SensorData & data, Statistics * stats = 0);
 	bool init(const std::string & dbUrl,
 			bool dbOverwritten = false,
 			const ParametersMap & parameters = ParametersMap(),
@@ -132,6 +133,7 @@ public:
 	bool isIDsGenerated() const {return _generateIds;}
 	int getLastGlobalLoopClosureParentId() const {return _lastGlobalLoopClosureParentId;}
 	int getLastGlobalLoopClosureChildId() const {return _lastGlobalLoopClosureChildId;}
+	const Feature2D * getFeature2D() const {return _feature2D;}
 
 	void setRoi(const std::string & roi);
 
@@ -149,6 +151,10 @@ public:
 	const VWDictionary * getVWDictionary() const;
 	std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
 	Feature2D::Type getFeatureType() const {return _featureType;}
+	void extractKeypointsAndDescriptors(
+			const cv::Mat & image,
+			std::vector<cv::KeyPoint> & keypoints,
+			cv::Mat & descriptors) const;
 	void extractKeypointsAndDescriptors(
 			const cv::Mat & image,
 			const cv::Mat & depth,
@@ -157,7 +163,7 @@ public:
 			float cx,
 			float cy,
 			std::vector<cv::KeyPoint> & keypoints,
-			cv::Mat & descriptors);
+			cv::Mat & descriptors) const;
 
 	// RGB-D stuff
 	void getMetricConstraints(
@@ -165,14 +171,15 @@ public:
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
-	Transform computeVisualTransform(int oldId, int newId) const;
-	Transform computeVisualTransform(const Signature & oldS, const Signature & newS) const;
-	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D);
-	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D) const;
+	Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0) const;
+	Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0) const;
+	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0);
+	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0) const;
 	Transform computeScanMatchingTransform(
 			int newId,
 			int oldId,
-			const std::map<int, Transform> & poses);
+			const std::map<int, Transform> & poses,
+			std::string * rejectedMsg = 0);
 
 private:
 	void preUpdate();
@@ -193,7 +200,7 @@ private:
 
 	void copyData(const Signature * from, Signature * to);
 	Signature * createSignature(
-			const Image & image,
+			const SensorData & data,
 			bool keepRawData=false);
 
 	//keypoint stuff

corelib/include/rtabmap/core/Odometry.h

@@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/Parameters.h>
 
-#include <rtabmap/core/Image.h>
+#include <rtabmap/core/SensorData.h>
 
 #include <opencv2/opencv.hpp>
 
@@ -54,7 +54,7 @@ class RTABMAP_EXP Odometry
 {
 public:
 	virtual ~Odometry() {}
-	Transform process(Image & image, int * quality = 0);
+	Transform process(SensorData & data, int * quality = 0);
 	virtual void reset();
 
 	bool isLargeEnoughTransform(const Transform & transform);
@@ -72,7 +72,7 @@ public:
 	int getLocalHistory() const {return _localHistory;}
 
 private:
-	virtual Transform computeTransform(Image & image, int * quality = 0) = 0;
+	virtual Transform computeTransform(const SensorData & image, int * quality = 0) = 0;
 
 private:
 	int _maxFeatures;
@@ -103,7 +103,7 @@ public:
 	virtual void reset();
 
 private:
-	virtual Transform computeTransform(Image & image, int * quality = 0);
+	virtual Transform computeTransform(const SensorData & image, int * quality = 0);
 
 private:
 	Memory * _memory;
@@ -124,7 +124,7 @@ public:
 	void reset();
 
 private:
-	virtual Transform computeTransform(Image & image, int * quality = 0);
+	virtual Transform computeTransform(const SensorData & image, int * quality = 0);
 
 private:
 	int _decimation;
@@ -139,9 +139,6 @@ private:
 	pcl::PointCloud<pcl::PointXYZ>::Ptr _previousCloud; // for point to point
 };
 
-// return true if odometry is correctly computed
-Transform computeTransform(Image & image);
-
 class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
 public:
 	// take ownership of Odometry
@@ -158,13 +155,13 @@ private:
 	// MAIN LOOP
 	//============================================================
 	void mainLoop();
-	void addImage(const Image & image);
-	void getImage(Image & image);
+	void addData(const SensorData & data);
+	void getData(SensorData & data);
 
 private:
-	USemaphore _imageAdded;
-	UMutex _imageMutex;
-	Image _imageBuffer;
+	USemaphore _dataAdded;
+	UMutex _dataMutex;
+	SensorData _dataBuffer;
 	Odometry * _odometry;
 	bool _resetOdometry;
 };

corelib/include/rtabmap/core/OdometryEvent.h

@@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYEVENT_H_
 
 #include "rtabmap/utilite/UEvent.h"
-#include "rtabmap/core/Image.h"
+#include "rtabmap/core/SensorData.h"
 
 namespace rtabmap {
 
@@ -37,18 +37,18 @@ class OdometryEvent : public UEvent
 {
 public:
 	OdometryEvent(
-		const Image & data, int quality = -1) :
+		const SensorData & data, int quality = -1) :
 			_data(data),
 			_quality(quality) {}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
 
 	bool isValid() const {return !_data.pose().isNull();}
-	const Image & data() const {return _data;}
+	const SensorData & data() const {return _data;}
 	int quality() const {return _quality;}
 
 private:
-	Image _data;
+	SensorData _data;
 	int _quality;
 };
 

corelib/include/rtabmap/core/Rtabmap.h

@@ -31,7 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
 #include "rtabmap/core/Parameters.h"
-#include "rtabmap/core/Image.h"
+#include "rtabmap/core/SensorData.h"
 #include "rtabmap/core/Statistics.h"
 #include "rtabmap/core/Link.h"
 
@@ -63,7 +63,7 @@ public:
 	virtual ~Rtabmap();
 
 	bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
-	bool process(const Image & image); // for convenience
+	bool process(const SensorData & data); // for convenience
 
 	void init(const ParametersMap & param, bool deleteMemory = true);
 	void init(const std::string & configFile = "", bool deleteMemory = true);
@@ -90,6 +90,7 @@ public:
 	//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
 	Transform getPose(int locationId) const;
 	Transform getMapCorrection() const {return _mapCorrection;}
+	const Memory * getMemory() const {return _memory;}
 
 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms

corelib/include/rtabmap/core/RtabmapExp.h

@@ -38,4 +38,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   #define RTABMAP_EXP
 #endif
 
+#ifdef __GNUC__
+#define RTABMAP_DEPRECATED(func, msg) func __attribute__ ((deprecated(msg)))
+#elif defined(_MSC_VER)
+#define RTABMAP_DEPRECATED(func, msg) __declspec(deprecated(msg)) func
+#else
+#pragma message("WARNING: You need to implement DEPRECATED for this compiler")
+#define RTABMAP_DEPRECATED(func, msg) func
+#endif
+
 #endif // RTABMAPEXP_H

corelib/include/rtabmap/core/RtabmapThread.h

@@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMutex.h>
 
 #include "rtabmap/core/RtabmapEvent.h"
-#include "rtabmap/core/Image.h"
+#include "rtabmap/core/SensorData.h"
 #include "rtabmap/core/Parameters.h"
 
 #include <stack>
@@ -89,8 +89,8 @@ private:
 	virtual void mainLoop();
 	virtual void mainLoopKill();
 	void process();
-	void addImage(const Image & image);
-	void getImage(Image & image);
+	void addData(const SensorData & data);
+	void getData(SensorData & data);
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
 	void setDataBufferSize(int size);
 	void publishMap(bool optimized, bool full) const;
@@ -101,10 +101,10 @@ private:
 	std::stack<State> _state;
 	std::stack<ParametersMap> _stateParam;
 
-	std::list<Image> _imageBuffer;
-	UMutex _imageMutex;
-	USemaphore _imageAdded;
-	int _imageBufferMaxSize;
+	std::list<SensorData> _dataBuffer;
+	UMutex _dataMutex;
+	USemaphore _dataAdded;
+	int _dataBufferMaxSize;
 	float _rate;
 	UTimer * _frameRateTimer;
 

corelib/include/rtabmap/core/SensorData.h

@@ -0,0 +1,109 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef SENSORDATA_H_
+#define SENSORDATA_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/Transform.h>
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+/**
+ * An id is automatically generated if id=0.
+ */
+class SensorData
+{
+public:
+	SensorData(); // empty constructor
+	SensorData(const cv::Mat & image, int id = 0);
+
+	// Metric constructor
+	SensorData(const cv::Mat & image,
+		  const cv::Mat & depth,
+		  float fx,
+		  float fy,
+		  float cx,
+		  float cy,
+		  const Transform & pose,
+		  const Transform & localTransform,
+		  int id = 0);
+
+	// Metric constructor + 2d depth
+	SensorData(const cv::Mat & image,
+		  const cv::Mat & depth,
+		  const cv::Mat & depth2d,
+		  float fx,
+		  float fy,
+		  float cx,
+		  float cy,
+		  const Transform & pose,
+		  const Transform & localTransform,
+		  int id = 0);
+
+	virtual ~SensorData() {}
+
+	bool isValid() const {return !_image.empty();}
+
+	// use isValid() instead
+	RTABMAP_DEPRECATED(bool empty() const, "Use !isValid() instead.");
+
+	const cv::Mat & image() const {return _image;}
+	int id() const {return _id;};
+
+	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 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;}
+
+private:
+	cv::Mat _image;
+	int _id;
+
+	// Metric stuff
+	cv::Mat _depth;
+	cv::Mat _depth2d;
+	float _fx;
+	float _fy;
+	float _cx;
+	float _cy;
+	Transform _pose;
+	Transform _localTransform;
+};
+
+}
+
+
+#endif /* SENSORDATA_H_ */

corelib/include/rtabmap/core/VWDictionary.h

@@ -70,7 +70,7 @@ public:
 	VisualWord * getUnusedWord(int id) const;
 	void setLastWordId(int id) {_lastWordId = id;}
 	const std::map<int, VisualWord *> & getVisualWords() const {return _visualWords;}
-	float getNndrRatio() {return _nndrRatio;}
+	float getNndrRatio() const {return _nndrRatio;}
 	unsigned int getNotIndexedWordsCount() const {return (int)_notIndexedWords.size();}
 	int getLastIndexedWordId() const;
 	int getTotalActiveReferences() const {return _totalActiveReferences;}