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;}

corelib/src/CMakeLists.txt

@@ -26,7 +26,7 @@ SET(SRC_FILES
 	
 	util3d.cpp
 	Odometry.cpp
-	Image.cpp
+	SensorData.cpp
 	
 	toro3d/posegraph3.cpp
 	toro3d/treeoptimizer3_iteration.cpp

corelib/src/CameraThread.cpp

@@ -104,8 +104,6 @@ void CameraThread::mainLoop()
 {
 	UTimer timer;
 	UDEBUG("");
-	cv::Mat descriptors;
-	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat rgb, depth;
 	float fx = 0.0f;
 	float fy = 0.0f;

corelib/src/DBReader.cpp

@@ -145,7 +145,7 @@ void DBReader::mainLoop()
 		{
 			if(!_odometryIgnored)
 			{
-				Image data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform);
+				SensorData data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform);
 				this->post(new OdometryEvent(data));
 				if(pose.isNull())
 				{

corelib/src/Features2d.cpp

@@ -109,66 +109,6 @@ void filterKeypointsByDepth(
 	}
 }
 
-void filterKeypointsByDepth(
-		std::vector<cv::KeyPoint> & keypoints,
-		std::vector<cv::Point3f> & keypoints3,
-		float maxDepth)
-{
-	cv::Mat descriptors;
-	filterKeypointsByDepth(keypoints, keypoints3, descriptors, maxDepth);
-}
-
-void filterKeypointsByDepth(
-		std::vector<cv::KeyPoint> & keypoints,
-		std::vector<cv::Point3f> & keypoints3,
-		cv::Mat & descriptors,
-		float maxDepth)
-{
-	UASSERT(keypoints.size() == keypoints3.size());
-	UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
-	if(keypoints.size())
-	{
-		std::vector<cv::KeyPoint> output(keypoints.size());
-		std::vector<cv::Point3f> output3(keypoints3.size());
-		std::vector<int> indexes(keypoints.size(), 0);
-		int oi=0;
-		for(unsigned int i=0; i<keypoints.size(); ++i)
-		{
-			if(uIsFinite(keypoints3[i].z) && keypoints3[i].z < maxDepth)
-			{
-				output3[oi] = keypoints3[i];
-				output[oi++] = keypoints[i];
-				indexes[i] = 1;
-			}
-		}
-		output.resize(oi);
-		output3.resize(oi);
-		keypoints = output;
-		keypoints3 = output3;
-
-		if(!descriptors.empty() && (int)keypoints.size() != descriptors.rows)
-		{
-			if(keypoints.size() == 0)
-			{
-				descriptors = cv::Mat();
-			}
-			else
-			{
-				cv::Mat newDescriptors(keypoints.size(), descriptors.cols, descriptors.type());
-				int di = 0;
-				for(unsigned int i=0; i<indexes.size(); ++i)
-				{
-					if(indexes[i] == 1)
-					{
-						memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
-					}
-				}
-				descriptors = newDescriptors;
-			}
-		}
-	}
-}
-
 void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
 {
 	cv::Mat descriptors;
@@ -219,59 +159,6 @@ void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors
 	}
 }
 
-void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, int maxKeypoints)
-{
-	cv::Mat descriptors;
-	limitKeypoints(keypoints, keypoints3, descriptors, maxKeypoints);
-}
-
-void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, cv::Mat & descriptors, int maxKeypoints)
-{
-	UASSERT(((int)keypoints.size() == descriptors.rows && keypoints.size() == keypoints3.size()) || descriptors.rows == 0);
-	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
-	{
-		UTimer timer;
-		ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
-		// Remove words under the new hessian threshold
-
-		// Sort words by hessian
-		std::multimap<float, int> hessianMap; // <hessian,id>
-		for(unsigned int i = 0; i <keypoints.size(); ++i)
-		{
-			//Keep track of the data, to be easier to manage the data in the next step
-			hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
-		}
-
-		// Remove them from the signature
-		int removed = hessianMap.size()-maxKeypoints;
-		std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
-		std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
-		std::vector<cv::Point3f> kpts3Tmp(maxKeypoints);
-		cv::Mat descriptorsTmp;
-		if(descriptors.rows)
-		{
-			descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
-		}
-		for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
-		{
-			kptsTmp[k] = keypoints[iter->second];
-			kpts3Tmp[k] = keypoints3[iter->second];
-			if(descriptors.rows)
-			{
-				memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
-			}
-		}
-		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
-		ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
-		keypoints = kptsTmp;
-		keypoints3 = kpts3Tmp;
-		if(descriptors.rows)
-		{
-			descriptors = descriptorsTmp;
-		}
-	}
-}
-
 cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
 {
 	if(!image.empty() && roiRatios.size() == 4)

corelib/src/Memory.cpp

@@ -429,7 +429,7 @@ void Memory::preUpdate()
 	}
 }
 
-bool Memory::update(const Image & image, Statistics * stats)
+bool Memory::update(const SensorData & data, Statistics * stats)
 {
 	UDEBUG("");
 	UTimer timer;
@@ -449,7 +449,7 @@ bool Memory::update(const Image & image, Statistics * stats)
 	//============================================================
 	// Create a signature with the image received.
 	//============================================================
-	Signature * signature = this->createSignature(image, this->isRawDataKept());
+	Signature * signature = this->createSignature(data, this->isRawDataKept());
 	if (signature == 0)
 	{
 		UERROR("Failed to create a signature...");
@@ -1628,7 +1628,7 @@ void Memory::rejectLoopClosure(int oldId, int newId)
 }
 
 // compute transform newId -> oldId
-Transform Memory::computeVisualTransform(int oldId, int newId) const
+Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rejectedMsg) const
 {
 	const Signature * oldS = this->getSignature(oldId);
 	const Signature * newS = this->getSignature(newId);
@@ -1637,19 +1637,25 @@ Transform Memory::computeVisualTransform(int oldId, int newId) const
 
 	if(oldS && newId)
 	{
-		return computeVisualTransform(*oldS, *newS);
+		return computeVisualTransform(*oldS, *newS, rejectedMsg);
 	}
 	else
 	{
-		UWARN("Did not find nodes %d and/or %d", oldId, newId);
+		std::string msg = uFormat("Did not find nodes %d and/or %d", oldId, newId);
+		if(rejectedMsg)
+		{
+			*rejectedMsg = msg;
+		}
+		UWARN(msg.c_str());
 	}
 	return Transform();
 }
 
 // compute transform newId -> oldId
-Transform Memory::computeVisualTransform(const Signature & oldS, const Signature & newS) const
+Transform Memory::computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg) const
 {
 	Transform transform;
+	std::string msg;
 	// Guess transform from visual words
 	if(!oldS.getWords3().empty() && !newS.getWords3().empty())
 	{
@@ -1679,23 +1685,37 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
 			}
 			else if(inliersCount < _bowMinInliers)
 			{
-				UINFO("Not enough inliers (after RANSAC) %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
+				msg = uFormat("Not enough inliers (after RANSAC) %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
+				UINFO(msg.c_str());
+			}
+			else if(inliersCount == (int)inliersOld->size())
+			{
+				msg = uFormat("Rejected identity with full inliers.");
+				UINFO(msg.c_str());
 			}
 		}
 		else
 		{
-			UINFO("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
+			msg = uFormat("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
+			UINFO(msg.c_str());
 		}
 	}
 	else if(!oldS.isBadSignature() && !newS.isBadSignature())
 	{
-		UERROR("Words 3D empty?!?");
+		msg = "Words 3D empty?!?";
+		UERROR(msg.c_str());
 	}
+
+	if(rejectedMsg)
+	{
+		*rejectedMsg = msg;
+	}
+
 	return transform;
 }
 
 // compute transform newId -> oldId
-Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D)
+Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg)
 {
 	Signature * oldS = this->_getSignature(oldId);
 	Signature * newS = this->_getSignature(newId);
@@ -1738,13 +1758,22 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo
 	Transform t;
 	if(oldS && newS)
 	{
-		t = computeIcpTransform(*oldS, *newS, guess, icp3D);
+		t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg);
+	}
+	else
+	{
+		std::string msg = uFormat("Did not find nodes %d and/or %d", oldId, newId);
+		if(rejectedMsg)
+		{
+			*rejectedMsg = msg;
+		}
+		UWARN(msg.c_str());
 	}
 	return t;
 }
 
 // get transform from the new to old node
-Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D) const
+Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg) const
 {
 	if(guess.isNull())
 	{
@@ -1758,6 +1787,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 	}
 	UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
 
+	std::string msg;
 	Transform transform;
 
 	// ICP with guess transform
@@ -1816,33 +1846,36 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 					   _icpMaxIterations,
 					   hasConverged,
 					   fitness);
+
+				//pcl::io::savePCDFile("old.pcd", *oldCloudXYZ);
+				//pcl::io::savePCDFile("newguess.pcd", *newCloudXYZ);
+				//newCloudXYZ = util3d::transformPointCloud(newCloudXYZ, icpT);
+				//pcl::io::savePCDFile("newicp.pcd", *newCloudXYZ);
+
+				UDEBUG("fitness=%f", fitness);
+
+				if(!icpT.isNull() && hasConverged && (_icpMaxFitness == 0 || fitness < _icpMaxFitness))
+				{
+					transform = icpT * guess;
+					transform = transform.inverse();
+				}
+				else
+				{
+					msg = uFormat("Cannot compute transform (hasConverged=%s fitness=%f/%f)",
+							hasConverged?"true":"false", fitness, _icpMaxFitness);
+					UINFO(msg.c_str());
+				}
 			}
 			else
 			{
-				UWARN("Clouds empty ?!?");
-			}
-
-			//pcl::io::savePCDFile("old.pcd", *oldCloudXYZ);
-			//pcl::io::savePCDFile("newguess.pcd", *newCloudXYZ);
-			//newCloudXYZ = util3d::transformPointCloud(newCloudXYZ, icpT);
-			//pcl::io::savePCDFile("newicp.pcd", *newCloudXYZ);
-
-			UDEBUG("fitness=%f", fitness);
-
-			if(hasConverged && (_icpMaxFitness == 0 || fitness < _icpMaxFitness))
-			{
-				transform = icpT * guess;
-				transform = transform.inverse();
-			}
-			else
-			{
-				UWARN("Cannot compute transform (hasConverged=%s fitness=%f/%f)",
-						hasConverged?"true":"false", fitness, _icpMaxFitness);
+				msg = "Clouds empty ?!?";
+				UWARN(msg.c_str());
 			}
 		}
 		else
 		{
-			UERROR("Depths 3D empty?!?");
+			msg = "Depths 3D empty?!?";
+			UERROR(msg.c_str());
 		}
 	}
 	else // icp 2D
@@ -1907,31 +1940,39 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 						correspondences,
 						(int)oldCloud->size(),
 						correspondencesRatio*100.0f);
-			}
-			else
-			{
-				UWARN("Clouds empty ?!?");
-			}
 
-			if(hasConverged &&
-			   (_icp2MaxFitness == 0 || fitness < _icp2MaxFitness) &&
-			   correspondencesRatio >= _icp2CorrespondenceRatio)
-			{
-				transform = icpT * guess;
-				transform = transform.inverse();
+				if(!icpT.isNull() && hasConverged &&
+				   (_icp2MaxFitness == 0 || fitness < _icp2MaxFitness) &&
+				   correspondencesRatio >= _icp2CorrespondenceRatio)
+				{
+					transform = icpT * guess;
+					transform = transform.inverse();
+				}
+				else
+				{
+					msg = uFormat("Cannot compute transform (hasConverged=%s fitness=%f/%f correspondencesRatio=%f/%f)",
+							hasConverged?"true":"false", fitness, _icpMaxFitness, correspondencesRatio, _icp2CorrespondenceRatio);
+					UINFO(msg.c_str());
+				}
 			}
 			else
 			{
-				UWARN("Cannot compute transform (hasConverged=%s fitness=%f/%f correspondencesRatio=%f/%f)",
-						hasConverged?"true":"false", fitness, _icpMaxFitness, correspondencesRatio, _icp2CorrespondenceRatio);
+				msg = "Clouds 2D empty ?!?";
+				UWARN(msg.c_str());
 			}
 		}
 		else
 		{
-			UERROR("Depths 2D empty?!?");
+			msg = "Depths 2D empty?!?";
+			UERROR(msg.c_str());
 		}
 	}
 
+	if(rejectedMsg)
+	{
+		*rejectedMsg = msg;
+	}
+
 	UDEBUG("New transform = %s", transform.prettyPrint().c_str());
 	return transform;
 }
@@ -1940,7 +1981,8 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 Transform Memory::computeScanMatchingTransform(
 		int newId,
 		int oldId,
-		const std::map<int, Transform> & poses)
+		const std::map<int, Transform> & poses,
+		std::string * rejectedMsg)
 {
 	// make sure that all depth2D are loaded
 	std::list<Signature*> depthToLoad;
@@ -1958,6 +2000,7 @@ Transform Memory::computeScanMatchingTransform(
 		_dbDriver->loadNodeData(depthToLoad, true);
 	}
 
+	std::string msg;
 	pcl::PointCloud<pcl::PointXYZ>::Ptr assembledOldClouds(new pcl::PointCloud<pcl::PointXYZ>);
 	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
@@ -2023,7 +2066,7 @@ Transform Memory::computeScanMatchingTransform(
 				(int)newCloud->size(),
 				correspondencesRatio);
 
-		if(hasConverged &&
+		if(!icpT.isNull() && hasConverged &&
 		   (_icp2MaxFitness == 0 || fitness < _icp2MaxFitness) &&
 		   correspondencesRatio >= _icp2CorrespondenceRatio)
 		{
@@ -2034,14 +2077,25 @@ Transform Memory::computeScanMatchingTransform(
 		}
 		else
 		{
-			UWARN("Constraints failed... hasConverged=%s, fitness=%f, correspondences=%d/%d (%f%%)",
+			msg = uFormat("Constraints failed... hasConverged=%s, fitness=%f, correspondences=%d/%d (%f%%)",
 				hasConverged?"true":"false",
 				fitness,
 				correspondences,
 				(int)newCloud->size(),
 				correspondencesRatio);
+			UINFO(msg.c_str());
 		}
 	}
+	else
+	{
+		msg = "Empty data ?!?";
+		UWARN(msg.c_str());
+	}
+
+	if(rejectedMsg)
+	{
+		*rejectedMsg = msg;
+	}
 
 	return transform;
 }
@@ -2840,6 +2894,14 @@ void Memory::copyData(const Signature * from, Signature * to)
 	UDEBUG("Merging time = %fs", timer.ticks());
 }
 
+void Memory::extractKeypointsAndDescriptors(
+		const cv::Mat & image,
+		std::vector<cv::KeyPoint> & keypoints,
+		cv::Mat & descriptors) const
+{
+	extractKeypointsAndDescriptors(image, cv::Mat(), 0,0,0,0, keypoints, descriptors);
+}
+
 void Memory::extractKeypointsAndDescriptors(
 		const cv::Mat & image,
 		const cv::Mat & depth,
@@ -2848,7 +2910,7 @@ void Memory::extractKeypointsAndDescriptors(
 		float cx,
 		float cy,
 		std::vector<cv::KeyPoint> & keypoints,
-		cv::Mat & descriptors)
+		cv::Mat & descriptors) const
 {
 	if(_wordsPerImageTarget >= 0)
 	{
@@ -2895,20 +2957,19 @@ private:
 	VWDictionary * _vwp;
 };
 
-Signature * Memory::createSignature(const Image & image, bool keepRawData)
+Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 {
-	UASSERT(image.image().empty() || image.image().type() == CV_8UC1 || image.image().type() == CV_8UC3);
-	UASSERT(image.depth().empty() || image.depth().type() == CV_16UC1);
-	UASSERT(image.depth2d().empty() || image.depth2d().type() == CV_32FC2);
+	UASSERT(data.image().empty() || data.image().type() == CV_8UC1 || data.image().type() == CV_8UC3);
+	UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1);
+	UASSERT(data.depth2d().empty() || data.depth2d().type() == CV_32FC2);
 
 	PreUpdateThread preUpdateThread(_vwd);
 
 	UTimer timer;
 	timer.start();
 	std::vector<cv::KeyPoint> keypoints;
-	std::vector<cv::Point3f> keypoints3;
 	cv::Mat descriptors;
-	int id = image.id();
+	int id = data.id();
 	if(_generateIds)
 	{
 		id = this->getNextId();
@@ -2951,48 +3012,24 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 		preUpdateThread.start();
 	}
 
-	if(!image.descriptors().empty() && image.featureType() == _featureType)
+	// Extract features
+	cv::Mat imageMono;
+	// convert to grayscale
+	if(data.image().channels() > 1)
 	{
-		// DESCRIPTORS
-		if(image.descriptors().rows && image.descriptors().rows >= _badSignRatio * float(meanWordsPerLocation))
-		{
-			UASSERT(image.descriptors().type() == CV_32F || image.descriptors().type() == CV_8U);
-			descriptors = image.descriptors();
-			keypoints = image.keypoints();
-			keypoints3 = image.keypoints3();
-		}
-		if(keypoints3.size())
-		{
-			filterKeypointsByDepth(keypoints, keypoints3, descriptors, _wordsMaxDepth);
-			limitKeypoints(keypoints, keypoints3, descriptors, _wordsPerImageTarget);
-		}
-		else
-		{
-			filterKeypointsByDepth(keypoints, descriptors, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), _wordsMaxDepth);
-			limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
-		}
+		cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
 	}
 	else
 	{
-		// IMAGE RAW
-		cv::Mat imageMono;
-		// convert to grayscale
-		if(image.image().channels() > 1)
-		{
-			cv::cvtColor(image.image(), imageMono, cv::COLOR_BGR2GRAY);
-		}
-		else
-		{
-			imageMono = image.image();
-		}
+		imageMono = data.image();
+	}
 
-		this->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), keypoints, descriptors);
+	this->extractKeypointsAndDescriptors(imageMono, data.depth(), data.depthFx(), data.depthFy(), data.depthCx(), data.depthCy(), keypoints, descriptors);
 
-		UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
-		if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
-		{
-			descriptors = cv::Mat();
-		}
+	UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
+	if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
+	{
+		descriptors = cv::Mat();
 	}
 
 	if(_parallelized)
@@ -3021,7 +3058,6 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 	}
 
 	std::multimap<int, cv::KeyPoint> words;
-	std::multimap<int, pcl::PointXYZ> words3;
 	if(wordIds.size() > 0)
 	{
 		UASSERT(wordIds.size() == keypoints.size());
@@ -3029,18 +3065,14 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
 			words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
-			if(i < keypoints3.size())
-			{
-				words3.insert(std::pair<int, pcl::PointXYZ>(*iter, pcl::PointXYZ(keypoints3[i].x, keypoints3[i].y, keypoints3[i].z)));
-			}
 		}
 	}
-	UASSERT(keypoints3.size() == 0 || words3.size() == words.size());
 
 	//3d words
-	if(words3.size() == 0 && !image.depth().empty() && image.depthFx() && image.depthFy())
+	std::multimap<int, pcl::PointXYZ> words3;
+	if(!data.depth().empty() && data.depthFx() && data.depthFy())
 	{
-		words3 = util3d::generateWords3(words, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), image.localTransform());
+		words3 = util3d::generateWords3(words, data.depth(), data.depthFx(), data.depthFy(), data.depthCx(), data.depthCy(), data.localTransform());
 	}
 
 	Signature * s;
@@ -3048,8 +3080,8 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 	{
 		std::vector<unsigned char> imageBytes;
 		std::vector<unsigned char> depthBytes;
-		util3d::CompressionThread ctImage(image.image(), std::string(".jpg"));
-		util3d::CompressionThread ctDepth(image.depth(), std::string(".png"));
+		util3d::CompressionThread ctImage(data.image(), std::string(".jpg"));
+		util3d::CompressionThread ctDepth(data.depth(), std::string(".png"));
 		ctImage.start();
 		ctDepth.start();
 		ctImage.join();
@@ -3061,15 +3093,15 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 			_idMapCount,
 			words,
 			words3,
-			image.pose(),
-			util3d::compressData(image.depth2d()),
+			data.pose(),
+			util3d::compressData(data.depth2d()),
 			imageBytes,
 			depthBytes,
-			image.depthFx(),
-			image.depthFy(),
-			image.depthCx(),
-			image.depthCy(),
-			image.localTransform());
+			data.depthFx(),
+			data.depthFy(),
+			data.depthCx(),
+			data.depthCy(),
+			data.localTransform());
 	}
 	else
 	{
@@ -3077,8 +3109,8 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 			_idMapCount,
 			words,
 			words3,
-			image.pose(),
-			util3d::compressData(image.depth2d()));
+			data.pose(),
+			util3d::compressData(data.depth2d()));
 	}
 
 

corelib/src/Odometry.cpp

@@ -93,9 +93,9 @@ bool Odometry::isLargeEnoughTransform(const Transform & transform)
 	       fabs(transform.z()) > _linearUpdate;
 }
 
-Transform Odometry::process(Image & image, int * quality)
+Transform Odometry::process(SensorData & data, int * quality)
 {
-	Transform t = this->computeTransform(image, quality);
+	Transform t = this->computeTransform(data, quality);
 	if(!t.isNull())
 	{
 		_resetCurrentCount = _resetCountdown;
@@ -175,42 +175,35 @@ void OdometryBOW::reset()
 
 
 // return not null transform if odometry is correctly computed
-Transform OdometryBOW::computeTransform(Image & image, int * quality)
+Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
 {
 	UTimer timer;
 	Transform output;
 
 	cv::Mat imageMono;
 	// convert to grayscale
-	if(image.image().channels() > 1)
+	if(data.image().channels() > 1)
 	{
-		cv::cvtColor(image.image(), imageMono, cv::COLOR_BGR2GRAY);
+		cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
 	}
 	else
 	{
-		imageMono = image.image();
-	}
-
-	std::vector<cv::KeyPoint> keypoints;
-	cv::Mat descriptors;
-	_memory->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), keypoints, descriptors);
-
-	image.setDescriptors(descriptors, _memory->getFeatureType());
-	image.setKeypoints(keypoints);
-
-	if(this->getLocalHistory() && this->getLocalHistory() < descriptors.rows)
-	{
-		UWARN("Local history words size (%d) is smaller than extracted features from the current frame (%d).",
-				this->getLocalHistory(), descriptors.rows);
+		imageMono = data.image();
 	}
 
 	int inliers = 0;
 	int correspondences = 0;
+	int nFeatures = 0;
 
 	const Signature * previousSignature = _memory->getLastWorkingSignature();
-	if(_memory->update(image))
+	if(_memory->update(data))
 	{
 		const Signature * newSignature = _memory->getLastWorkingSignature();
+		if(newSignature)
+		{
+			nFeatures = newSignature->getWords().size();
+		}
+
 		if(previousSignature && newSignature)
 		{
 			Transform transform;
@@ -360,7 +353,7 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
 
 	UINFO("Odom update time = %fs features=%d inliers=%d/%d dict=%d nodes=%d",
 			timer.elapsed(),
-			descriptors.rows,
+			nFeatures,
 			inliers,
 			correspondences,
 			(int)_memory->getVWDictionary()->getVisualWords().size(),
@@ -398,7 +391,7 @@ void OdometryICP::reset()
 }
 
 // return not null transform if odometry is correctly computed
-Transform OdometryICP::computeTransform(Image & image, int * quality)
+Transform OdometryICP::computeTransform(const SensorData & data, int * quality)
 {
 	UTimer timer;
 	Transform output;
@@ -406,19 +399,19 @@ Transform OdometryICP::computeTransform(Image & image, int * quality)
 	bool hasConverged = false;
 	double fitness = 0;
 	unsigned int minPoints = 100;
-	if(!image.depth().empty())
+	if(!data.depth().empty())
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
-						image.depth(),
-						image.depthFx(),
-						image.depthFy(),
-						image.depthCx(),
-						image.depthCy(),
+						data.depth(),
+						data.depthFx(),
+						data.depthFy(),
+						data.depthCx(),
+						data.depthCy(),
 						_decimation,
 						this->getMaxDepth(),
 						_voxelSize,
 						_samples,
-						image.localTransform());
+						data.localTransform());
 
 		if(_pointToPlane)
 		{
@@ -538,7 +531,7 @@ void OdometryThread::handleEvent(UEvent * event)
 			CameraEvent * cameraEvent = (CameraEvent*)event;
 			if(cameraEvent->getCode() == CameraEvent::kCodeImageDepth)
 			{
-				this->addImage(cameraEvent->image());
+				this->addData(cameraEvent->data());
 			}
 			else if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages)
 			{
@@ -554,7 +547,7 @@ void OdometryThread::handleEvent(UEvent * event)
 
 void OdometryThread::mainLoopKill()
 {
-	_imageAdded.release();
+	_dataAdded.release();
 }
 
 //============================================================
@@ -568,51 +561,51 @@ void OdometryThread::mainLoop()
 		_resetOdometry = false;
 	}
 
-	Image image;
-	getImage(image);
-	if(!image.empty())
+	SensorData data;
+	getData(data);
+	if(data.isValid())
 	{
 		int quality = -1;
-		Transform pose = _odometry->process(image, &quality);
-		image.setPose(pose); // a null pose notify that odometry could not be computed
-		this->post(new OdometryEvent(image, quality));
+		Transform pose = _odometry->process(data, &quality);
+		data.setPose(pose); // a null pose notify that odometry could not be computed
+		this->post(new OdometryEvent(data, quality));
 	}
 }
 
-void OdometryThread::addImage(const Image & image)
+void OdometryThread::addData(const SensorData & data)
 {
-	if(image.empty() || image.depth().empty() || image.depthFx() == 0.0f || image.depthFy() == 0.0f)
+	if(data.image().empty() || data.depth().empty() || data.depthFx() == 0.0f || data.depthFy() == 0.0f)
 	{
 		ULOGGER_ERROR("image empty !?");
 		return;
 	}
 
 	bool notify = true;
-	_imageMutex.lock();
+	_dataMutex.lock();
 	{
-		notify = _imageBuffer.empty();
-		_imageBuffer = image;
+		notify = !_dataBuffer.isValid();
+		_dataBuffer = data;
 	}
-	_imageMutex.unlock();
+	_dataMutex.unlock();
 
 	if(notify)
 	{
-		_imageAdded.release();
+		_dataAdded.release();
 	}
 }
 
-void OdometryThread::getImage(Image & image)
+void OdometryThread::getData(SensorData & data)
 {
-	_imageAdded.acquire();
-	_imageMutex.lock();
+	_dataAdded.acquire();
+	_dataMutex.lock();
 	{
-		if(!_imageBuffer.empty())
+		if(_dataBuffer.isValid())
 		{
-			image = _imageBuffer;
-			_imageBuffer = Image();
+			data = _dataBuffer;
+			_dataBuffer = SensorData();
 		}
 	}
-	_imageMutex.unlock();
+	_dataMutex.unlock();
 }
 
 } /* namespace rtabmap */

corelib/src/Rtabmap.cpp

@@ -667,7 +667,7 @@ void Rtabmap::resetMemory(bool dbOverwritten)
 //============================================================
 // MAIN LOOP
 //============================================================
-bool Rtabmap::process(const Image & image)
+bool Rtabmap::process(const SensorData & data)
 {
 	UDEBUG("");
 
@@ -724,9 +724,9 @@ bool Rtabmap::process(const Image & image)
 	// Wait for an image...
 	//============================================================
 	ULOGGER_INFO("getting data...");
-	if(image.empty())
+	if(!data.isValid())
 	{
-		ULOGGER_INFO("image is null...");
+		ULOGGER_INFO("image is not valid...");
 		return false;
 	}
 
@@ -743,10 +743,10 @@ bool Rtabmap::process(const Image & image)
 	//============================================================
 	if(_rgbdSlamMode)
 	{
-		if(image.pose().isNull())
+		if(data.pose().isNull())
 		{
 			UERROR("RGB-D SLAM mode is enabled and no odometry is provided. "
-				   "Image %d is ignored!", image.id());
+				   "Image %d is ignored!", data.id());
 			return false;
 		}
 		else
@@ -755,7 +755,7 @@ bool Rtabmap::process(const Image & image)
 			if(_memory->getLastWorkingSignature())
 			{
 				const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
-				if(!lastPose.isIdentity() && image.pose().isIdentity())
+				if(!lastPose.isIdentity() && data.pose().isIdentity())
 				{
 					int mapId = _memory->incrementMapId();
 					UWARN("Odometry is reset (transform identity detected). A new map (%d) is created!", mapId);
@@ -764,7 +764,7 @@ bool Rtabmap::process(const Image & image)
 				}
 				else
 				{
-					Transform lastPoseToNewPose = lastPose.inverse() * image.pose();
+					Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
 					float x,y,z, roll,pitch,yaw;
 					lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
 					if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
@@ -774,7 +774,7 @@ bool Rtabmap::process(const Image & image)
 								_newMapOdomChangeDistance,
 								mapId,
 								lastPose.prettyPrint().c_str(),
-								image.pose().prettyPrint().c_str());
+								data.pose().prettyPrint().c_str());
 						_optimizedPoses.clear();
 						_constraints.clear();
 					}
@@ -787,7 +787,7 @@ bool Rtabmap::process(const Image & image)
 	// Memory Update : Location creation + Add to STM + Weight Update (Rehearsal)
 	//============================================================
 	ULOGGER_INFO("Updating memory...");
-	if(!_memory->update(image, &statistics_))
+	if(!_memory->update(data, &statistics_))
 	{
 		return false;
 	}
@@ -870,17 +870,22 @@ bool Rtabmap::process(const Image & image)
 			{
 				const Signature * oldS = _memory->getSignature(oldId);
 				UASSERT(oldS != 0);
-				Transform t = _memory->computeScanMatchingTransform(signature->id(), oldId, poses);
+				std::string rejectedMsg;
+				Transform t = _memory->computeScanMatchingTransform(signature->id(), oldId, poses, &rejectedMsg);
 				if(!t.isNull())
 				{
 					scanMatchingSuccess = true;
-					UDEBUG("Update neighbor link (%d->%d) from %s to %s",
+					UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
 							signature->id(),
 							oldId,
 							signature->getNeighbors().at(oldId).prettyPrint().c_str(),
 							t.prettyPrint().c_str());
 					_memory->updateNeighborLink(signature->id(), oldId, t);
 				}
+				else
+				{
+					UWARN("Scan matching rejected: %s", rejectedMsg.c_str());
+				}
 			}
 			else
 			{
@@ -909,11 +914,12 @@ bool Rtabmap::process(const Image & image)
 				   signature->getNeighbors().find(*iter) == signature->getNeighbors().end() &&
 				   _memory->getSignature(*iter)->mapId() == signature->mapId())
 				{
+					std::string rejectedMsg;
 					UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
-					Transform transform = _memory->computeVisualTransform(*iter, signature->id());
+					Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg);
 					if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 					{
-						Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1);
+						Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg);
 						float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
 						if(!icpTransform.isNull() &&
 							_globalLoopClosureIcpMaxDistance>0.0f &&
@@ -947,6 +953,11 @@ bool Rtabmap::process(const Image & image)
 									*iter, signature->id());
 						}
 					}
+					else
+					{
+						UINFO("Local loop closure (time) between %d and %d rejected: %s",
+								*iter, signature->id(), rejectedMsg.c_str());
+					}
 				}
 			}
 		}
@@ -1249,10 +1260,11 @@ bool Rtabmap::process(const Image & image)
 		Transform transform;
 		if(_rgbdSlamMode)
 		{
-			transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id());
+			std::string rejectedMsg;
+			transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg);
 			if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 			{
-				Transform icpTransform  = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1);
+				Transform icpTransform  = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg);
 				float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
 				if(!icpTransform.isNull() &&
 					_globalLoopClosureIcpMaxDistance>0.0f &&
@@ -1270,7 +1282,7 @@ bool Rtabmap::process(const Image & image)
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
 			{
-				UWARN("Cannot compute a loop closure transform between %d and %d", _lcHypothesisId, signature->id());
+				UWARN("Cannot compute a loop closure transform between %d and %d: %s", _lcHypothesisId, signature->id(), rejectedMsg.c_str());
 			}
 		}
 		if(!rejectedHypothesis)
@@ -1316,14 +1328,15 @@ bool Rtabmap::process(const Image & image)
 		localSpaceDetectionPosesCount = (int)poses.size()-1;
 		//The nearest will be the reference for a loop closure transform
 		if(poses.size() &&
-				localSpaceNearestId &&
+			localSpaceNearestId &&
 			signature->getChildLoopClosureIds().find(localSpaceNearestId) == signature->getChildLoopClosureIds().end())
 		{
-			Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses);
+			std::string rejectedMsg;
+			Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg);
 			if(!t.isNull())
 			{
 				localSpaceClosureId = localSpaceNearestId;
-				UDEBUG("Add local loop closure in SPACE (%d->%d) %s",
+				UINFO("Add local loop closure in SPACE (%d->%d) %s",
 						signature->id(),
 						localSpaceNearestId,
 						t.prettyPrint().c_str());
@@ -1334,6 +1347,10 @@ bool Rtabmap::process(const Image & image)
 				UASSERT(oldS != 0);
 				_mapTransform = oldS->getPose() * t.inverse() * signature->getPose().inverse();
 			}
+			else
+			{
+				UINFO("Local loop closure (space) rejected: %s", rejectedMsg.c_str());
+			}
 		}
 	}
 	timeLocalSpaceDetection = timer.ticks();
@@ -1720,9 +1737,9 @@ bool Rtabmap::process(const Image & image)
 	return true;
 }
 
-bool Rtabmap::process(const cv::Mat & image, int id)
+bool Rtabmap::process(const cv::Mat & sensorData, int id)
 {
-	return this->process(Image(image, id));
+	return this->process(SensorData(sensorData, id));
 }
 
 // SETTERS

corelib/src/RtabmapThread.cpp

@@ -42,7 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
-		_imageBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
+		_dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
 		_rate(Parameters::defaultRtabmapDetectionRate()),
 		_frameRateTimer(new UTimer()),
 		_rtabmap(rtabmap),
@@ -74,16 +74,16 @@ void RtabmapThread::pushNewState(State newState, const ParametersMap & parameter
 	}
 	_stateMutex.unlock();
 
-	_imageAdded.release();
+	_dataAdded.release();
 }
 
 void RtabmapThread::clearBufferedData()
 {
-	_imageMutex.lock();
+	_dataMutex.lock();
 	{
-		_imageBuffer.clear();
+		_dataBuffer.clear();
 	}
-	_imageMutex.unlock();
+	_dataMutex.unlock();
 }
 
 void RtabmapThread::setDetectorRate(float rate)
@@ -95,7 +95,7 @@ void RtabmapThread::setDetectorRate(float rate)
 void RtabmapThread::setBufferSize(int bufferSize)
 {
 	UASSERT(bufferSize >= 0);
-	_imageBufferMaxSize = bufferSize;
+	_dataBufferMaxSize = bufferSize;
 }
 
 void RtabmapThread::publishMap(bool optimized, bool full) const
@@ -177,7 +177,7 @@ void RtabmapThread::mainLoopKill()
 	this->clearBufferedData();
 
 	// this will post the newData semaphore
-	_imageAdded.release();
+	_dataAdded.release();
 }
 
 void RtabmapThread::mainLoop()
@@ -203,9 +203,9 @@ void RtabmapThread::mainLoop()
 		this->process();
 		break;
 	case kStateChangingParameters:
-		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _imageBufferMaxSize);
+		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
 		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
-		UASSERT(_imageBufferMaxSize >= 0);
+		UASSERT(_dataBufferMaxSize >= 0);
 		UASSERT(_rate >= 0.0f);
 		_rtabmap->parseParameters(parameters);
 		break;
@@ -270,11 +270,9 @@ void RtabmapThread::handleEvent(UEvent* event)
 	{
 		UDEBUG("CameraEvent");
 		CameraEvent * e = (CameraEvent*)event;
-		if(e->getCode() == CameraEvent::kCodeImage ||
-		   e->getCode() == CameraEvent::kCodeFeatures ||
-		   e->getCode() == CameraEvent::kCodeImageDepth)
+		if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth)
 		{
-			this->addImage(e->image());
+			this->addData(e->data());
 		}
 	}
 	else if(event->getClassName().compare("OdometryEvent") == 0)
@@ -283,7 +281,7 @@ void RtabmapThread::handleEvent(UEvent* event)
 		OdometryEvent * e = (OdometryEvent*)event;
 		if(e->isValid())
 		{
-			this->addImage(e->data());
+			this->addData(e->data());
 		}
 	}
 	else if(event->getClassName().compare("RtabmapEventCmd") == 0)
@@ -416,26 +414,26 @@ void RtabmapThread::handleEvent(UEvent* event)
 //============================================================
 void RtabmapThread::process()
 {
-	Image image;
-	getImage(image);
-	if(!image.empty())
+	SensorData data;
+	getData(data);
+	if(data.isValid())
 	{
-		_rtabmap->process(image);
+		_rtabmap->process(data);
 
 		Statistics stats = _rtabmap->getStatistics();
-		stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_imageBuffer.size());
+		stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
 		ULOGGER_DEBUG("posting statistics_ event...");
 		this->post(new RtabmapEvent(stats));
 	}
 }
 
-void RtabmapThread::addImage(const Image & image)
+void RtabmapThread::addData(const SensorData & sensorData)
 {
 	if(!_paused)
 	{
-		if(image.empty())
+		if(!sensorData.isValid())
 		{
-			ULOGGER_ERROR("image empty !?");
+			ULOGGER_ERROR("data not valid !?");
 			return;
 		}
 
@@ -449,42 +447,42 @@ void RtabmapThread::addImage(const Image & image)
 		_frameRateTimer->start();
 
 		bool notify = true;
-		_imageMutex.lock();
+		_dataMutex.lock();
 		{
-			_imageBuffer.push_back(image);
-			while(_imageBufferMaxSize > 0 && _imageBuffer.size() > (unsigned int)_imageBufferMaxSize)
+			_dataBuffer.push_back(sensorData);
+			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
 			{
 				ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
-				_imageBuffer.pop_front();
+				_dataBuffer.pop_front();
 				notify = false;
 			}
 		}
-		_imageMutex.unlock();
+		_dataMutex.unlock();
 
 		if(notify)
 		{
-			_imageAdded.release();
+			_dataAdded.release();
 		}
 	}
 }
 
-void RtabmapThread::getImage(Image & image)
+void RtabmapThread::getData(SensorData & image)
 {
 	ULOGGER_DEBUG("");
 
 	ULOGGER_INFO("waiting for data");
-	_imageAdded.acquire();
+	_dataAdded.acquire();
 	ULOGGER_INFO("wake-up");
 
-	_imageMutex.lock();
+	_dataMutex.lock();
 	{
-		if(!_imageBuffer.empty())
+		if(!_dataBuffer.empty())
 		{
-			image = _imageBuffer.front();
-			_imageBuffer.pop_front();
+			image = _dataBuffer.front();
+			_dataBuffer.pop_front();
 		}
 	}
-	_imageMutex.unlock();
+	_dataMutex.unlock();
 }
 
 void RtabmapThread::setDataBufferSize(int size)
@@ -492,11 +490,11 @@ void RtabmapThread::setDataBufferSize(int size)
 	if(size < 0)
 	{
 		ULOGGER_WARN("size < 0, then setting it to 0 (inf).");
-		_imageBufferMaxSize = 0;
+		_dataBufferMaxSize = 0;
 	}
 	else
 	{
-		_imageBufferMaxSize = size;
+		_dataBufferMaxSize = size;
 	}
 }
 

corelib/src/SensorData.cpp

@@ -26,9 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 
-#include "rtabmap/core/Image.h"
-
-#include <rtabmap/utilite/ULogger.h>
+#include "rtabmap/core/SensorData.h"
 
 namespace rtabmap
 {
@@ -36,16 +34,21 @@ namespace rtabmap
 /**
  * An id is automatically generated if id=0.
  */
-Image::Image(const cv::Mat & image,
-	  int id,
-	  const cv::Mat & descriptors,
-	  Feature2D::Type featureType,
-	  const std::vector<cv::KeyPoint> & keypoints) :
+SensorData::SensorData() :
+	_image(cv::Mat()),
+	_id(0),
+	_fx(0.0f),
+	_fy(0.0f),
+	_cx(0.0f),
+	_cy(0.0f),
+	_localTransform(Transform::getIdentity())
+{
+}
+
+SensorData::SensorData(const cv::Mat & image,
+	  int id) :
 	_image(image),
 	_id(id),
-	_descriptors(descriptors),
-	_featureType(featureType),
-	_keypoints(keypoints),
 	_fx(0.0f),
 	_fy(0.0f),
 	_cx(0.0f),
@@ -55,7 +58,7 @@ Image::Image(const cv::Mat & image,
 }
 
 	// Metric constructor
-Image::Image(const cv::Mat & image,
+SensorData::SensorData(const cv::Mat & image,
 		  const cv::Mat & depth,
 		  float fx,
 		  float fy,
@@ -66,7 +69,6 @@ Image::Image(const cv::Mat & image,
 		  int id) :
 	_image(image),
 	_id(id),
-	_featureType(Feature2D::kFeatureUndef),
 	_depth(depth),
 	_fx(fx),
 	_fy(fy),
@@ -78,7 +80,7 @@ Image::Image(const cv::Mat & image,
 }
 
 	// Metric constructor + 2d depth
-Image::Image(const cv::Mat & image,
+SensorData::SensorData(const cv::Mat & image,
 		  const cv::Mat & depth,
 		  const cv::Mat & depth2d,
 		  float fx,
@@ -90,7 +92,6 @@ Image::Image(const cv::Mat & image,
 		  int id) :
 	_image(image),
 	_id(id),
-	_featureType(Feature2D::kFeatureUndef),
 	_depth(depth),
 	_depth2d(depth2d),
 	_fx(fx),
@@ -102,16 +103,9 @@ Image::Image(const cv::Mat & image,
 {
 }
 
-void Image::setKeypoints(
-		const std::vector<cv::KeyPoint> & keypoints,
-		const std::vector<cv::Point3f> * keypoints3)
+bool SensorData::empty() const
 {
-	_keypoints = keypoints;
-	if(keypoints3 && keypoints3->size())
-	{
-		UASSERT(_keypoints.size() == keypoints3->size());
-		_keypoints3 = *keypoints3;
-	}
+	return _image.empty();
 }
 
 } // namespace rtabmap

corelib/src/util3d.cpp

@@ -1193,14 +1193,11 @@ Transform transformFromXYZCorrespondences(
 		if(correspondencesInliers.size() == correspondences->size() && transform.isIdentity())
 		{
 			//Wrong transform
-			UINFO("Wrong transform: identity");
+			UDEBUG("Wrong transform: identity with full inliers");
 			transform.setNull();
-			if(inliers)
-			{
-				*inliers = 0;
-			}
 		}
-		else if(inliers)
+
+		if(inliers)
 		{
 			*inliers = (int)correspondencesInliers.size();
 		}

examples/RGBDMapping/MapBuilder.h

@@ -60,7 +60,7 @@ public:
 		this->setLayout(layout);
 
 		qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
-		qRegisterMetaType<rtabmap::Image>("rtabmap::Image");
+		qRegisterMetaType<rtabmap::SensorData>("rtabmap::Image");
 	}
 
 	virtual ~MapBuilder()
@@ -69,7 +69,7 @@ public:
 	}
 
 private slots:
-	void processOdometry(const rtabmap::Image & data)
+	void processOdometry(const rtabmap::SensorData & data)
 	{
 		if(!this->isVisible())
 		{
@@ -214,7 +214,7 @@ protected:
 		{
 			OdometryEvent * odomEvent = (OdometryEvent *)event;
 			// Odometry must be processed in the Qt thread
-			QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::Image, odomEvent->data()));
+			QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data()));
 		}
 	}
 

guilib/include/rtabmap/gui/DataRecorder.h

@@ -32,7 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/utilite/UEventsHandler.h>
 #include <QtGui/QWidget>
-#include <rtabmap/core/Image.h>
+#include <rtabmap/core/SensorData.h>
 #include <rtabmap/utilite/UTimer.h>
 
 namespace rtabmap {
@@ -52,8 +52,8 @@ public:
 	virtual ~DataRecorder();
 
 public slots:
-	void addData(const rtabmap::Image & image);
-	void showImage(const rtabmap::Image & image);
+	void addData(const rtabmap::SensorData & data);
+	void showImage(const rtabmap::SensorData & data);
 protected:
 	void handleEvent(UEvent * event);
 

guilib/include/rtabmap/gui/MainWindow.h

@@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtGui/QMainWindow>
 #include <QtCore/QSet>
 #include "rtabmap/core/RtabmapEvent.h"
-#include "rtabmap/core/Image.h"
+#include "rtabmap/core/SensorData.h"
 #include "rtabmap/gui/PreferencesDialog.h"
 
 #include <pcl/point_cloud.h>
@@ -137,7 +137,7 @@ private slots:
 	void selectScreenCaptureFormat(bool checked);
 	void takeScreenshot();
 	void updateElapsedTime();
-	void processOdometry(const rtabmap::Image & data, int quality);
+	void processOdometry(const rtabmap::SensorData & data, int quality);
 	void applyAllPrefSettings();
 	void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
 	void applyPrefSettings(const rtabmap::ParametersMap & parameters);
@@ -169,7 +169,7 @@ private slots:
 
 signals:
 	void statsReceived(const rtabmap::Statistics &);
-	void odometryReceived(const rtabmap::Image &, int);
+	void odometryReceived(const rtabmap::SensorData &, int);
 	void thresholdsChanged(int, int);
 	void stateChanged(MainWindow::State);
 	void rtabmapEventInitReceived(int status, const QString & info);

guilib/include/rtabmap/gui/OdometryViewer.h

@@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
-#include "rtabmap/core/Image.h"
+#include "rtabmap/core/SensorData.h"
 #include "rtabmap/gui/CloudViewer.h"
 #include "rtabmap/utilite/UEventsHandler.h"
 #include "rtabmap/utilite/UTimer.h"
@@ -55,7 +55,7 @@ private slots:
 
 private:
 	UMutex dataMutex_;
-	std::list<rtabmap::Image> data_;
+	std::list<rtabmap::SensorData> data_;
 	int dataQuality_;
 	Transform lastOdomPose_;
 	UTimer timer_;

guilib/src/CalibrationDialog.cpp

@@ -119,7 +119,7 @@ void CalibrationDialog::handleEvent(UEvent * event)
 		if(e->getCode() == rtabmap::CameraEvent::kCodeImage ||
 		   e->getCode() == rtabmap::CameraEvent::kCodeImageDepth)
 		{
-			QMetaObject::invokeMethod(this, "processImage", Q_ARG(cv::Mat, e->image().image()));
+			QMetaObject::invokeMethod(this, "processImage", Q_ARG(cv::Mat, e->data().image()));
 		}
 	}
 }

guilib/src/DataRecorder.cpp

@@ -46,7 +46,7 @@ DataRecorder::DataRecorder(QWidget * parent) :
 	imageView_(new ImageView(this)),
 	dataQueue_(0)
 {
-	qRegisterMetaType<rtabmap::Image>("rtabmap::Image");
+	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
 
 	QHBoxLayout * layout = new QHBoxLayout(this);
 	layout->addWidget(imageView_);
@@ -95,16 +95,16 @@ DataRecorder::~DataRecorder()
 	this->close();
 }
 
-void DataRecorder::addData(const rtabmap::Image & image)
+void DataRecorder::addData(const rtabmap::SensorData & data)
 {
 	if(memory_)
 	{
 		//save to database
 		UTimer time;
-		memory_->update(image);
+		memory_->update(data);
 		memory_->cleanup();
 
-		if(image.id() % 30)
+		if(data.id() % 30)
 		{
 			memory_->emptyTrash();
 		}
@@ -118,12 +118,12 @@ void DataRecorder::addData(const rtabmap::Image & image)
 	--dataQueue_;
 }
 
-void DataRecorder::showImage(const rtabmap::Image & image)
+void DataRecorder::showImage(const rtabmap::SensorData & data)
 {
-	if(this->isVisible() && !image.empty())
+	if(this->isVisible() && data.isValid())
 	{
-		imageView_->setImage(uCvMat2QImage(image.image()));
-		imageView_->setImageDepth(uCvMat2QImage(image.depth()));
+		imageView_->setImage(uCvMat2QImage(data.image()));
+		imageView_->setImageDepth(uCvMat2QImage(data.depth()));
 		imageView_->fitInView(imageView_->sceneRect(), Qt::KeepAspectRatio);
 	}
 }
@@ -136,15 +136,15 @@ void DataRecorder::handleEvent(UEvent * event)
 		if(camEvent->getCode() == CameraEvent::kCodeImageDepth ||
 		   camEvent->getCode() == CameraEvent::kCodeImage)
 		{
-			if(!camEvent->image().empty())
+			if(camEvent->data().isValid())
 			{
 				UINFO("Receiving rate = %f Hz", 1.0f/timer_.ticks());
-				QMetaObject::invokeMethod(this, "addData", Q_ARG(rtabmap::Image, camEvent->image()));
+				QMetaObject::invokeMethod(this, "addData", Q_ARG(rtabmap::SensorData, camEvent->data()));
 				++dataQueue_;
 
 				if(dataQueue_ < 2 && this->isVisible())
 				{
-					QMetaObject::invokeMethod(this, "showImage", Q_ARG(rtabmap::Image, camEvent->image()));
+					QMetaObject::invokeMethod(this, "showImage", Q_ARG(rtabmap::SensorData, camEvent->data()));
 				}
 			}
 		}

guilib/src/DatabaseViewer.cpp

@@ -47,7 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/gui/DataRecorder.h"
-#include "rtabmap/core/Image.h"
+#include "rtabmap/core/SensorData.h"
 #include "ExportDialog.h"
 #include "DetailedProgressDialog.h"
 
@@ -273,7 +273,7 @@ void DatabaseViewer::exportDatabase()
 						depth2d = rtabmap::util3d::uncompressData(compressedDepth2d);
 					}
 
-					rtabmap::Image data(rgb, depth, depth2d, fx, fy, cx, cy, pose, localTransform, id);
+					rtabmap::SensorData data(rgb, depth, depth2d, fx, fy, cx, cy, pose, localTransform, id);
 					recorder.addData(data);
 
 					progressDialog.appendText(tr("Exported node %1").arg(id));
@@ -1645,6 +1645,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		UASSERT(!containsLink(linksRefined_, from, to));
 
 		Transform t;
+		std::string rejectedMsg;
 		if(ui_->checkBox_visual_recomputeFeatures->isChecked())
 		{
 			// create a fake memory to regenerate features
@@ -1669,18 +1670,18 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			memory_->getImageDepth(from, imageBytesA, depthBytesA, depth2dBytesA, fxA, fyA, cxA, cyA, localTransformA);
 			cv::Mat imageA = rtabmap::util3d::uncompressImage(imageBytesA);
 			cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
-			Image imageFrom(imageA, depthA, fxA, fyA, cxA, cyA, Transform::getIdentity(), localTransformA, 1);
+			SensorData dataFrom(imageA, depthA, fxA, fyA, cxA, cyA, Transform::getIdentity(), localTransformA, 1);
 
 			std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
 			memory_->getImageDepth(to, imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
 			cv::Mat imageB = rtabmap::util3d::uncompressImage(imageBytesB);
 			cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
-			Image imageTo(imageB, depthB, fxB, fyB, cxB, cyB, Transform::getIdentity(), localTransformB, 2);
+			SensorData dataTo(imageB, depthB, fxB, fyB, cxB, cyB, Transform::getIdentity(), localTransformB, 2);
 
-			tmpMemory.update(imageFrom);
-			tmpMemory.update(imageTo);
+			tmpMemory.update(dataFrom);
+			tmpMemory.update(dataTo);
 
-			t = tmpMemory.computeVisualTransform(2, 1);
+			t = tmpMemory.computeVisualTransform(2, 1, &rejectedMsg);
 		}
 		else
 		{
@@ -1690,7 +1691,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
 			parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
 			memory_->parseParameters(parameters);
-			t = memory_->computeVisualTransform(to, from);
+			t = memory_->computeVisualTransform(to, from, &rejectedMsg);
 		}
 
 		if(t.isNull())
@@ -1699,7 +1700,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			{
 				QMessageBox::warning(this,
 						tr("Add link"),
-						tr("Cannot find a transformation between nodes %1 and %2").arg(from).arg(to));
+						tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(rejectedMsg.c_str()));
 			}
 		}
 		else

guilib/src/LoopClosureViewer.cpp

@@ -48,6 +48,7 @@ LoopClosureViewer::LoopClosureViewer(QWidget * parent) :
 {
 	ui_ = new Ui_loopClosureViewer();
 	ui_->setupUi(this);
+	ui_->cloudViewerTransform->setCameraLockZ(false);
 
 	connect(ui_->checkBox_rawCloud, SIGNAL(clicked()), this, SLOT(updateView()));
 }

guilib/src/MainWindow.cpp

@@ -341,8 +341,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
 	connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
 
-	qRegisterMetaType<rtabmap::Image>("rtabmap::Image");
-	connect(this, SIGNAL(odometryReceived(rtabmap::Image, int)), this, SLOT(processOdometry(rtabmap::Image, int)));
+	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
+	connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, int)), this, SLOT(processOdometry(rtabmap::SensorData, int)));
 
 	connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(stopDetection()));
 
@@ -554,7 +554,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 	}
 }
 
-void MainWindow::processOdometry(const rtabmap::Image & data, int quality)
+void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality)
 {
 	Transform pose = data.pose();
 	if(pose.isNull())

guilib/src/OdometryViewer.cpp

@@ -70,7 +70,7 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, i
 
 void OdometryViewer::processData()
 {
-	rtabmap::Image data;
+	rtabmap::SensorData data;
 	int quality = -1;
 	dataMutex_.lock();
 	if(data_.size())

tools/ConsoleApp/main.cpp

@@ -416,8 +416,7 @@ int main(int argc, char * argv[])
 	std::list<std::vector<float> > teleopActions;
 	while(loopDataset <= repeat && g_forever)
 	{
-		cv::Mat cvImg = camera->takeImage();
-		Image img(cvImg, 0);
+		cv::Mat img = camera->takeImage();
 		int i=0;
 		double maxIterationTime = 0.0;
 		int maxIterationTimeId = 0;
@@ -433,8 +432,7 @@ int main(int argc, char * argv[])
 			{
 				++countLoopDetected;
 			}
-			cvImg = camera->takeImage();
-			img = Image(cvImg, 0);
+			img = camera->takeImage();
 			if(++count % 100 == 0)
 			{
 				printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n",