Added Monocular SLAM (experimental), Odometry classes refactoring

corelib/include/rtabmap/core/CameraRGBD.h

@@ -96,11 +96,13 @@ public:
 	float getImageRate() const {return _imageRate;}
 	const Transform & getLocalTransform() const {return _localTransform;}
 	bool isMirroringEnabled() const {return _mirroring;}
+	bool isColorOnly() const {return _colorOnly;}
 
 	//setters
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
 	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
 	void setMirroringEnabled(bool mirroring) {_mirroring = mirroring;}
+	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
 
 protected:
 	/**
@@ -120,6 +122,7 @@ private:
 	float _imageRate;
 	Transform _localTransform;
 	bool _mirroring;
+	bool _colorOnly;
 	UTimer * _frameRateTimer;
 };
 

corelib/include/rtabmap/core/Features2d.h

@@ -89,21 +89,26 @@ public:
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
 
+	int getMaxFeatures() const {return maxFeatures_;}
+
 public:
 	virtual ~Feature2D() {}
 
-	std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image, int maxKeypoints=0, const cv::Rect & roi = cv::Rect()) const;
+	std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image, const cv::Rect & roi = cv::Rect()) const;
 	cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
-	virtual void parseParameters(const ParametersMap & parameters) {}
+	virtual void parseParameters(const ParametersMap & parameters);
 	virtual Feature2D::Type getType() const = 0;
 
 protected:
-	Feature2D(const ParametersMap & parameters = ParametersMap()) {}
+	Feature2D(const ParametersMap & parameters = ParametersMap());
 
 private:
 	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const = 0;
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
+
+private:
+	int maxFeatures_;
 };
 
 //SURF

corelib/include/rtabmap/core/Memory.h

@@ -245,7 +245,6 @@ private:
 	bool _tfIdfLikelihoodUsed;
 	bool _parallelized;
 	float _wordsMaxDepth; // 0=inf
-	int _wordsPerImageTarget; // <0=none, 0=inf
 	std::vector<float> _roiRatios; // size 4
 
 	// RGBD-SLAM stuff
@@ -254,6 +253,8 @@ private:
 	int _bowIterations;
 	float _bowMaxDepth;
 	bool _bowForce2D;
+	bool _bowEpipolarGeometry;
+	float _bowEpipolarGeometryVar;
 	int _icpDecimation;
 	float _icpMaxDepth;
 	float _icpVoxelSize;

corelib/include/rtabmap/core/Odometry.h

@@ -30,20 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/RtabmapExp.h>
 
-#include <rtabmap/utilite/UThread.h>
-#include <rtabmap/utilite/UEventsHandler.h>
-#include <rtabmap/utilite/UEvent.h>
-#include <rtabmap/utilite/UMutex.h>
-#include <rtabmap/utilite/USemaphore.h>
-
-#include <rtabmap/core/Parameters.h>
-
+#include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
-#include <rtabmap/core/OdometryInfo.h>
-
-#include <opencv2/opencv.hpp>
-
-#include <pcl/common/eigen.h>
+#include <rtabmap/core/Parameters.h>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 
@@ -52,6 +41,7 @@ class UTimer;
 namespace rtabmap {
 
 class Feature2D;
+class OdometryInfo;
 
 class RTABMAP_EXP Odometry
 {
@@ -62,7 +52,6 @@ public:
 
 	//getters
 	const Transform & getPose() const {return _pose;}
-	int getMaxFeatures() const  {return _maxFeatures;}
 	const std::string & getRoiRatios() const {return _roiRatios;}
 	int getMinInliers() const {return _minInliers;}
 	float getInlierDistance() const {return _inlierDistance;}
@@ -71,14 +60,13 @@ public:
 	float getMaxDepth() const {return _maxDepth;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
 	bool isPnPEstimationUsed() const {return _pnpEstimation;}
-	double  getPnPReprojError() const {return _pnpReprojError;}
+	double getPnPReprojError() const {return _pnpReprojError;}
 	int  getPnPFlags() const {return _pnpFlags;}
 
 private:
 	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
 
 private:
-	int _maxFeatures;
 	std::string _roiRatios;
 	int _minInliers;
 	float _inlierDistance;
@@ -163,6 +151,38 @@ private:
 	pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
 };
 
+class OdometryMono : public Odometry
+{
+public:
+	OdometryMono(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryMono();
+	virtual void reset(const Transform & initialPose);
+
+private:
+	virtual Transform computeTransform(const SensorData & data, OdometryInfo * info = 0);
+private:
+	//Parameters:
+	int flowWinSize_;
+	int flowIterations_;
+	double flowEps_;
+	int flowMaxLevel_;
+
+	Memory * memory_;
+	int localHistoryMaxSize_;
+	float initMinFlow_;
+	float initMinTranslation_;
+	float minTranslation_;
+	float fundMatrixReprojError_;
+	float fundMatrixConfidence_;
+
+	cv::Mat refDepth_;
+	std::map<int, cv::Point2f> cornersMap_;
+	std::multimap<int, cv::Point3f> localMap_;
+	std::map<int, std::multimap<int, pcl::PointXYZ> > keyFrameWords3D_;
+	std::map<int, Transform> keyFramePoses_;
+	float maxVariance_;
+};
+
 class RTABMAP_EXP OdometryICP : public Odometry
 {
 public:
@@ -192,32 +212,5 @@ private:
 	pcl::PointCloud<pcl::PointXYZ>::Ptr _previousCloud; // for point to point
 };
 
-class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
-public:
-	// take ownership of Odometry
-	OdometryThread(Odometry * odometry);
-	virtual ~OdometryThread();
-
-protected:
-	virtual void handleEvent(UEvent * event);
-
-private:
-	void mainLoopKill();
-
-	//============================================================
-	// MAIN LOOP
-	//============================================================
-	void mainLoop();
-	void addData(const SensorData & data);
-	void getData(SensorData & data);
-
-private:
-	USemaphore _dataAdded;
-	UMutex _dataMutex;
-	SensorData _dataBuffer;
-	Odometry * _odometry;
-	bool _resetOdometry;
-};
-
 } /* namespace rtabmap */
 #endif /* ODOMETRY_H_ */

corelib/include/rtabmap/core/OdometryInfo.h

@@ -57,10 +57,11 @@ public:
 	std::multimap<int, cv::KeyPoint> words;
 	std::vector<int> wordMatches;
 	std::vector<int> wordInliers;
+	std::multimap<int, cv::Point3f> localMap;
 
 	// Optical Flow odometry
-	std::vector<cv::KeyPoint> refCorners;
-	std::vector<cv::KeyPoint> newCorners;
+	std::vector<cv::Point2f> refCorners;
+	std::vector<cv::Point2f> newCorners;
 	std::vector<int> cornerInliers;
 };
 

corelib/include/rtabmap/core/OdometryThread.h

@@ -0,0 +1,70 @@
+/*
+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 ODOMETRYTHREAD_H_
+#define ODOMETRYTHREAD_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsHandler.h>
+
+namespace rtabmap {
+
+class Odometry;
+
+class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
+public:
+	// take ownership of Odometry
+	OdometryThread(Odometry * odometry);
+	virtual ~OdometryThread();
+
+protected:
+	virtual void handleEvent(UEvent * event);
+
+private:
+	void mainLoopKill();
+
+	//============================================================
+	// MAIN LOOP
+	//============================================================
+	void mainLoop();
+	void addData(const SensorData & data);
+	void getData(SensorData & data);
+
+private:
+	USemaphore _dataAdded;
+	UMutex _dataMutex;
+	SensorData _dataBuffer;
+	Odometry * _odometry;
+	bool _resetOdometry;
+};
+
+} // namespace rtabmap
+
+
+#endif /* ODOMETRYTHREAD_H_ */

corelib/include/rtabmap/core/Parameters.h

@@ -245,14 +245,12 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(FAST, Gpu,                bool, false, 	"GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
 	RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, 	"Used with FAST GPU.");
 
-	RTABMAP_PARAM(GFTT, MaxCorners, int, 400, "");
 	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
 	RTABMAP_PARAM(GFTT, MinDistance, double, 5, "");
 	RTABMAP_PARAM(GFTT, BlockSize, int, 3, "");
 	RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false, "");
 	RTABMAP_PARAM(GFTT, K, double, 0.04, "");
 
-	RTABMAP_PARAM(ORB, NFeatures,            int, 400,     "The maximum number of features to retain.");
 	RTABMAP_PARAM(ORB, ScaleFactor,          float,  1.2, "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
 	RTABMAP_PARAM(ORB, NLevels,              int, 1,       "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
 	RTABMAP_PARAM(ORB, EdgeThreshold,        int, 31,      "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
@@ -309,7 +307,7 @@ class RTABMAP_EXP Parameters
 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Bag-of-words 1=Optical Flow");
 	RTABMAP_PARAM(Odom, FeatureType,            int, 6, 	    "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
-	RTABMAP_PARAM(Odom, MaxFeatures,            int, 0, 		"0 no limits.");
+	RTABMAP_PARAM(Odom, MaxFeatures,            int, 400, 		"0 no limits.");
 	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.02, 	"Maximum distance for visual word correspondences.");
 	RTABMAP_PARAM(Odom, MinInliers,             int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
 	RTABMAP_PARAM(Odom, Iterations,             int, 30, 		"Maximum iterations to compute the transform from visual words.");
@@ -328,6 +326,13 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(OdomBow, NNType,                 int, 3, 	    "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(OdomBow, NNDR,                   float, 0.8,  "NNDR: nearest neighbor distance ratio.");
 
+	// Odometry Mono
+	RTABMAP_PARAM(OdomMono, InitMinFlow,            float, 100,  "Minimum optical flow required for the initialization step.");
+	RTABMAP_PARAM(OdomMono, InitMinTranslation,     float, 0.1,  "Minimum translation required for the initialization step.");
+	RTABMAP_PARAM(OdomMono, MinTranslation,         float, 0.02,  "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
+	RTABMAP_PARAM(OdomMono, MaxVariance,            float, 0.01,  "Maximum variance to add new points to local map.");
+
+
 	// Odometry common stuff between BOW and Optical Flow approaches
 	RTABMAP_PARAM(OdomFlow, WinSize,               int, 16,       "Used for optical flow approach and for stereo matching. See cv::calcOpticalFlowPyrLK().");
 	RTABMAP_PARAM(OdomFlow, Iterations,            int, 30,       "Used for optical flow approach and for stereo matching. See cv::calcOpticalFlowPyrLK().");
@@ -346,7 +351,9 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.02, 	"Maximum distance for visual word correspondences.");
 	RTABMAP_PARAM(LccBow, Iterations,      int, 100, 		"Maximum iterations to compute the transform from visual words.");
 	RTABMAP_PARAM(LccBow, MaxDepth,        float, 4.0, 		"Max depth of the words (0 means no limit).");
-	RTABMAP_PARAM(LccBow, Force2D, 		   bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
+	RTABMAP_PARAM(LccBow, Force2D, 		   bool, false,    "Force 2D transform (3Dof: x,y and yaw).");
+	RTABMAP_PARAM(LccBow, EpipolarGeometry, bool, false,   "Use epipolar geometry to compute the loop closure transform.");
+	RTABMAP_PARAM(LccBow, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure.");
 	RTABMAP_PARAM_COND(LccReextract, Activated, bool, RTABMAP_NONFREE, false, true, "Activate re-extracting features on global loop closure.");
 	RTABMAP_PARAM(LccReextract, NNType, 	int, 3, 		"kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
 	RTABMAP_PARAM(LccReextract, NNDR, 		float, 0.8, 	"NNDR: nearest neighbor distance ratio.");

corelib/include/rtabmap/core/Signature.h

@@ -134,10 +134,14 @@ public:
 	const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
 	const cv::Mat & getDepthCompressed() const {return _depthCompressed;}
 	const cv::Mat & getLaserScanCompressed() const {return _laserScanCompressed;}
-	float getDepthFx() const {return _fx;}
-	float getDepthFy() const {return _fy;}
-	float getDepthCx() const {return _cx;}
-	float getDepthCy() const {return _cy;}
+	RTABMAP_DEPRECATED(float getDepthFx() const, "Use getFx() instead.");
+	RTABMAP_DEPRECATED(float getDepthFy() const, "Use getFy() instead.");
+	RTABMAP_DEPRECATED(float getDepthCx() const, "Use getCx() instead.");
+	RTABMAP_DEPRECATED(float getDepthCy() const, "Use getCy() instead.");
+	float getFx() const {return _fx;}
+	float getFy() const {return _fy;}
+	float getCx() const {return _cx;}
+	float getCy() const {return _cy;}
 	const Transform & getPose() const {return _pose;}
 	const Transform & getLocalTransform() const {return _localTransform;}
 	void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}

corelib/include/rtabmap/core/Statistics.h

@@ -99,9 +99,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Signature_creation, ms);
 	RTABMAP_STATS(TimingMem, Rehearsal, ms);
 	RTABMAP_STATS(TimingMem, Keypoints_detection, ms);
-	RTABMAP_STATS(TimingMem, Stereo_subpixel, ms);
+	RTABMAP_STATS(TimingMem, Subpixel, ms);
 	RTABMAP_STATS(TimingMem, Stereo_correspondences, ms);
-	RTABMAP_STATS(TimingMem, Keypoints_filtering, ms);
 	RTABMAP_STATS(TimingMem, Descriptors_extraction, ms);
 	RTABMAP_STATS(TimingMem, Keypoints_3D, ms);
 	RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);

corelib/include/rtabmap/core/util3d.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/RtabmapExp.h"
 #include <opencv2/core/core.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <list>
 #include <string>
@@ -99,6 +100,23 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
 		int flowIterations = 20,
 		double flowEps = 0.02);
 
+std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
+		const std::multimap<int, cv::KeyPoint> & kpts,
+		const std::multimap<int, cv::KeyPoint> & previousKpts,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
+		const Transform & localTransform,
+		Transform & cameraTransform,
+		int pnpIterations = 100,
+		float pnpReprojError = 8.0f,
+		int pnpFlags = cv::ITERATIVE,
+		float ransacParam1 = 3.0f,
+		float ransacParam2 = 0.99f,
+		const std::multimap<int, pcl::PointXYZ> & refGuess3D = std::multimap<int, pcl::PointXYZ>(),
+		double * variance = 0);
+
 std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,
 		const std::vector<cv::KeyPoint> & keypoints);
@@ -412,6 +430,11 @@ pcl::IndicesPtr RTABMAP_EXP concatenate(
 
 cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
 
+void RTABMAP_EXP savePCDWords(
+		const std::string & fileName,
+		const std::multimap<int, pcl::PointXYZ> & words,
+		const Transform & transform = Transform::getIdentity());
+
 ///////////////////
 // Templated PCL methods
 ///////////////////