Refactoring: removed some code duplication about transformation estimation and features (2D-3D) extraction.

Added Feature2D::generateKeypoints3D() for convenience.
Added parameter "Vis/PnPOpenCV2".
Added parameter "Vis/ForwardEstOnly".
Removed OdometryOpticalFlow class, replaced by OdometryF2F (frame-to-frame). To get the same previous OpticalFLow approach, parameter "Vis/CorType" should be set to 1.
Some parameters under group "OdomFlow/..." are now under "Vis/CorFlow...".
In Registration class, add computeTransformationMod() method to modify input signatures.
Added constructor Signature(SensorData) for convenience.
Modified words multimap used with cv::Point3f instead of pcl::PointXYZ to limit the use of PCL headers where they are not really required.
Added Stereo::create() for convenience.
Transform: fixed quaternion constructor where data_ was not initialized. Added parentheses operator for convenience.
DatabaseViewer: loading .rtabmap/rtabmap.ini instead of .rtabmap/dbViewer.ini when used from rtabmap application. Added vertical layout option for convenience.
MainWindow: fixed wrong Odometry speed values
ParametersToolBox: using QStackedWidget instead of a QToolBox for space, added "Restore Defaults" button.

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -86,6 +86,16 @@ public:
 
 	static cv::Mat findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty);
 
+
+	/**
+	 * if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
+	 * realPairsCount = 4
+	 */
+	static int findPairs(
+			const std::map<int, cv::KeyPoint> & wordsA,
+			const std::map<int, cv::KeyPoint> & wordsB,
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
+
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
 	 * realPairsCount = 5

corelib/include/rtabmap/core/Features2d.h

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/features2d/features2d.hpp>
 #include <list>
 #include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/SensorData.h"
 
 #if CV_MAJOR_VERSION < 3
 namespace cv{
@@ -87,6 +88,8 @@ typedef cv::cuda::FastFeatureDetector CV_FAST_GPU;
 
 namespace rtabmap {
 
+class Stereo;
+
 // Feature2D
 class RTABMAP_EXP Feature2D {
 public:
@@ -104,7 +107,6 @@ public:
 
 	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
 	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
-	static Feature2D * create(Feature2D::Type type, int wordsPerImage, const ParametersMap & parameters = ParametersMap()); // for convenience
 
 	static void filterKeypointsByDepth(
 				std::vector<cv::KeyPoint> & keypoints,
@@ -137,10 +139,13 @@ public:
 	int getMaxFeatures() const {return maxFeatures_;}
 
 public:
-	virtual ~Feature2D() {}
+	virtual ~Feature2D();
 
-	std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image, const cv::Rect & roi = cv::Rect()) const;
+	std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image) const;
 	cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+	std::vector<cv::Point3f> generateKeypoints3D(
+			const SensorData & data,
+			const std::vector<cv::KeyPoint> & keypoints) const;
 
 	virtual void parseParameters(const ParametersMap & parameters);
 	virtual Feature2D::Type getType() const = 0;
@@ -154,6 +159,14 @@ private:
 
 private:
 	int maxFeatures_;
+	float _wordsMaxDepth; // 0=inf
+	float _wordsMinDepth;
+	std::vector<float> _roiRatios; // size 4
+	int _subPixWinSize;
+	int _subPixIterations;
+	double _subPixEps;
+	// Stereo stuff
+	Stereo * _stereo;
 };
 
 //SURF
@@ -198,7 +211,6 @@ private:
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
-	int nfeatures_;
 	int nOctaveLayers_;
 	double contrastThreshold_;
 	double edgeThreshold_;
@@ -222,7 +234,6 @@ private:
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
-	int nFeatures_;
 	float scaleFactor_;
 	int nLevels_;
 	int edgeThreshold_;
@@ -258,7 +269,6 @@ private:
 	double gpuKeypointsRatio_;
 	int minThreshold_;
 	int maxThreshold_;
-	int maxTotalKeypoints_;
 	int gridRows_;
 	int gridCols_;
 
@@ -337,7 +347,6 @@ private:
 	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
 
 private:
-	int _maxCorners;
 	double _qualityLevel;
 	double _minDistance;
 	int _blockSize;

corelib/include/rtabmap/core/Memory.h

@@ -42,7 +42,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
-#include <pcl/point_types.h>
 
 namespace rtabmap {
 
@@ -147,7 +146,7 @@ public:
 	SensorData getNodeData(int nodeId, bool uncompressedData = false, bool keepLoadedDataInMemory = true);
 	void getNodeWords(int nodeId,
 			std::multimap<int, cv::KeyPoint> & words,
-			std::multimap<int, pcl::PointXYZ> & words3);
+			std::multimap<int, cv::Point3f> & words3);
 	SensorData getSignatureDataConst(int locationId) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
@@ -161,8 +160,6 @@ public:
 	const Feature2D * getFeature2D() const {return _feature2D;}
 	bool isGraphReduced() const {return _reduceGraph;}
 
-	void setRoi(const std::string & roi);
-
 	void dumpMemoryTree(const char * fileNameTree) const;
 	virtual void dumpMemory(std::string directory) const;
 	virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
@@ -172,7 +169,6 @@ public:
 
 	//keypoint stuff
 	const VWDictionary * getVWDictionary() const;
-	Feature2D::Type getFeatureType() const {return _featureType;}
 
 	// RGB-D stuff
 	void getMetricConstraints(
@@ -262,23 +258,12 @@ private:
 	//Keypoint stuff
 	VWDictionary * _vwd;
 	Feature2D * _feature2D;
-	Feature2D::Type _featureType;
 	float _badSignRatio;;
 	bool _tfIdfLikelihoodUsed;
 	bool _parallelized;
-	float _wordsMaxDepth; // 0=inf
-	float _wordsMinDepth;
-	std::vector<float> _roiRatios; // size 4
 
 	RegistrationVis * _registrationVis;
 	RegistrationIcp * _registrationIcp;
-
-	// Stereo stuff
-	Stereo * _stereo;
-
-	int _subPixWinSize;
-	int _subPixIterations;
-	double _subPixEps;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Odometry.h

@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/RegistrationVis.h>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 
@@ -59,11 +61,13 @@ public:
 	float getInlierDistance() const {return _inlierDistance;}
 	int getIterations() const {return _iterations;}
 	int getRefineIterations() const {return _refineIterations;}
+	float getMinDepth() const {return _minDepth;}
 	float getMaxDepth() const {return _maxDepth;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
 	int getEstimationType() const {return _estimationType;}
 	double getPnPReprojError() const {return _pnpReprojError;}
 	int  getPnPFlags() const {return _pnpFlags;}
+	bool  getPnPOpenCV2() const {return _pnpOpenCV2;}
 	const Transform & previousTransform() const {return previousTransform_;}
 	bool isVarianceFromInliersCount() const {return _varianceFromInliersCount;}
 
@@ -79,6 +83,7 @@ private:
 	float _inlierDistance;
 	int _iterations;
 	int _refineIterations;
+	float _minDepth;
 	float _maxDepth;
 	int _resetCountdown;
 	bool _force2D;
@@ -93,6 +98,7 @@ private:
 	int _estimationType;
 	double _pnpReprojError;
 	int _pnpFlags;
+	bool _pnpOpenCV2;
 	bool _varianceFromInliersCount;
 	float _kalmanProcessNoise;
 	float _kalmanMeasurementNoise;
@@ -119,7 +125,7 @@ public:
 	virtual ~OdometryBOW();
 
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-	const std::map<int, pcl::PointXYZ> & getLocalMap() const {return localMap_;}
+	const std::map<int, cv::Point3f> & getLocalMap() const {return localMap_;}
 	const Memory * getMemory() const {return _memory;}
 
 private:
@@ -131,20 +137,18 @@ private:
 	std::string _fixedLocalMapPath;
 
 	Memory * _memory;
-	std::map<int, pcl::PointXYZ> localMap_;
+	std::map<int, cv::Point3f> localMap_;
 };
 
-class RTABMAP_EXP OdometryOpticalFlow : public Odometry
+class RTABMAP_EXP OdometryF2F : public Odometry
 {
 public:
-	OdometryOpticalFlow(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-	virtual ~OdometryOpticalFlow();
+	OdometryF2F(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryF2F();
 
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 
-	const cv::Mat & getLastFrame() const {return refFrame_;}
-	const std::vector<cv::Point2f> & getLastCorners() const {return refCorners_;}
-	const pcl::PointCloud<pcl::PointXYZ>::Ptr & getLastCorners3D() const {return refCorners3D_;}
+	const Signature & getRefFrame() const {return refFrame_;}
 
 private:
 	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
@@ -152,27 +156,14 @@ private:
 private:
 	//Parameters:
 	int keyFrameThr_;
-	int flowWinSize_;
-	int flowIterations_;
-	double flowEps_;
-	int flowMaxLevel_;
-	bool flowGuessFromMotion_;
-
-	Stereo * stereo_;
-
-	int subPixWinSize_;
-	int subPixIterations_;
-	double subPixEps_;
-
-	Feature2D * feature2D_;
-
-	cv::Mat refFrame_;
-	std::vector<cv::Point2f> refCorners_;
-	pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
+	bool guessFromMotion_;
 
+	RegistrationVis registration_;
+	Signature refFrame_;
 	Transform motionSinceLastKeyFrame_;
 };
 
+
 class RTABMAP_EXP OdometryMono : public Odometry
 {
 public:
@@ -202,7 +193,7 @@ private:
 	cv::Mat refDepthOrRight_;
 	std::map<int, cv::Point2f> cornersMap_;
 	std::map<int, cv::Point3f> localMap_;
-	std::map<int, std::multimap<int, pcl::PointXYZ> > keyFrameWords3D_;
+	std::map<int, std::map<int, cv::Point3f> > keyFrameWords3D_;
 	std::map<int, Transform> keyFramePoses_;
 	float maxVariance_;
 };

corelib/include/rtabmap/core/OdometryInfo.h

@@ -64,15 +64,15 @@ public:
 	Transform transformGroundTruth;
 	float distanceTravelled;
 
-	int type; // 0=BOW, 1=Optical Flow, 2=ICP
+	int type; // 0=BOW, 1=F2F, 2=ICP, 3=Mono
 
-	// BOW odometry
+	// BOW
 	std::multimap<int, cv::KeyPoint> words;
 	std::vector<int> wordMatches;
 	std::vector<int> wordInliers;
 	std::map<int, cv::Point3f> localMap;
 
-	// Optical Flow odometry
+	// F2F && Mono
 	std::vector<cv::Point2f> refCorners;
 	std::vector<cv::Point2f> newCorners;
 	std::vector<int> cornerInliers;

corelib/include/rtabmap/core/Parameters.h

@@ -214,7 +214,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Kp, IncrementalFlann,      bool, true, 		"When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
 	RTABMAP_PARAM(Kp, MaxDepth,              float, 0.0, 		"Filter extracted keypoints by depth (0=inf).");
 	RTABMAP_PARAM(Kp, MinDepth,              float, 0.0, 		"Filter extracted keypoints by depth.");
-	RTABMAP_PARAM(Kp, WordsPerImage,         int, 400, 			"Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
+	RTABMAP_PARAM(Kp, MaxFeatures,           int, 400, 			"Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
 	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.5, 		"Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
 	RTABMAP_PARAM_COND(Kp, NndrRatio, 	     float, RTABMAP_NONFREE, 0.8, 0.9,		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 	RTABMAP_PARAM_COND(Kp, DetectorStrategy, int, RTABMAP_NONFREE, 0, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
@@ -357,10 +357,6 @@ class RTABMAP_EXP Parameters
 
 	// Odometry Optical Flow
 	RTABMAP_PARAM(OdomFlow, KeyFrameThr,           int, 0,        "Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
-	RTABMAP_PARAM(OdomFlow, WinSize,               int, 16,       "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
-	RTABMAP_PARAM(OdomFlow, Iterations,            int, 30,       "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
-	RTABMAP_PARAM(OdomFlow, Eps,                   double, 0.01,  "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
-	RTABMAP_PARAM(OdomFlow, MaxLevel,              int, 3,        "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
 	RTABMAP_PARAM(OdomFlow, GuessMotion,           bool, true,    "Guess optical flow from the last motion computed.");
 
 	// Common registration parameters
@@ -368,16 +364,16 @@ class RTABMAP_EXP Parameters
 	
 	// Visual registration parameters
 	RTABMAP_PARAM(Vis, EstimationType,           int, 0,    	"Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
+	RTABMAP_PARAM(Vis, ForwardEstOnly,               bool, true, 	"Forward estimation (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
 	RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    "[Vis/EstimationType = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.");
 	RTABMAP_PARAM(Vis, RefineIterations,         int, 10,       "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
 	RTABMAP_PARAM(Vis, PnPReprojError, 	         double, 5.0,   "[Vis/EstimationType = 1] PnP reprojection error.");
-	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,    	"[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
+	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,        "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
+	RTABMAP_PARAM(Vis, PnPOpenCV2,               bool, true,    "[Vis/EstimationType = 1] Use OpenCV2 solvePnPRansac() in OpenCV3.");
 	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
 	RTABMAP_PARAM(Vis, MinInliers,               int, 10, 		"Minimum feature correspondences to compute/accept the transformation.");
 	RTABMAP_PARAM(Vis, Iterations,               int, 100, 		"Maximum iterations to compute the transform.");
 	RTABMAP_PARAM(Vis, Force2D,                  bool, false,   "Force 2D transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
-	RTABMAP_PARAM(Vis, NNType, 	                 int, 3,        "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
-	RTABMAP_PARAM(Vis, NNDR,                     float, 0.8,    "NNDR: nearest neighbor distance ratio.");
 	RTABMAP_PARAM(Vis, 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(Vis, MaxFeatures,              int, 1000,     "0 no limits.");
 	RTABMAP_PARAM(Vis, MaxDepth, 	             float, 0.0,    "Max depth of the features (0 means no limit).");
@@ -386,6 +382,13 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, SubPixWinSize,            int, 3,        "See cv::cornerSubPix().");
 	RTABMAP_PARAM(Vis, SubPixIterations,         int, 0,        "See cv::cornerSubPix(). 0 disables sub pixel refining.");
 	RTABMAP_PARAM(Vis, SubPixEps,                double, 0.02,  "See cv::cornerSubPix().");
+	RTABMAP_PARAM(Vis, CorType,                  int, 0,        "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
+	RTABMAP_PARAM(Vis, CorNNType, 	             int, 3,        "[Vis/CorrespondenceType=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.");
+	RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.8,    "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.");
+	RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
+	RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
+	RTABMAP_PARAM(Vis, CorFlowEps,               double, 0.01,  "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
+	RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,        "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
 
 
 	// ICP registration parameters

corelib/include/rtabmap/core/Registration.h

@@ -41,29 +41,30 @@ public:
 	{
 		Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
 	}
-	virtual Transform computeTransformation(
+
+	Transform computeTransformation(
 			const Signature & from,
 			const Signature & to,
 			Transform guess = Transform::getIdentity(),
 			std::string * rejectedMsg = 0,
-			int * inliersOut = 0,
+			std::vector<int> * inliersOut = 0,
 			float * varianceOut = 0,
-			float * inliersRatioOut = 0) = 0;
-
-	Transform computeTransformation2(
-				const SensorData & fromData,
-				const SensorData & toData,
-				Transform guess = Transform::getIdentity(), // guess is ignored for RegistrationVis
-				std::string * rejectedMsg = 0,
-				int * inliersOut = 0,
-				float * varianceOut = 0,
-				float * inliersRatioOut = 0)
+			float * inliersRatioOut = 0) const
 	{
-		Signature fromSignature(fromData.id(), -1, 0, 0.0, "", Transform::getIdentity(), fromData);
-		Signature toSignature(toData.id(), -1, 0, 0.0, "", Transform::getIdentity(), toData);
-		return computeTransformation(fromSignature, toSignature, guess, rejectedMsg, inliersOut, varianceOut, inliersRatioOut);
+		Signature fromCopy(from);
+		Signature toCopy(to);
+		return computeTransformationMod(fromCopy, toCopy, guess, rejectedMsg, inliersOut, varianceOut, inliersRatioOut);
 	}
 
+	virtual Transform computeTransformationMod(
+			Signature & from,
+			Signature & to,
+			Transform guess = Transform::getIdentity(),
+			std::string * rejectedMsg = 0,
+			std::vector<int> * inliersOut = 0,
+			float * varianceOut = 0,
+			float * inliersRatioOut = 0) const = 0;
+
 protected:
 	Registration(const ParametersMap & parameters = ParametersMap()) :
 		_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount())

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -42,23 +42,23 @@ public:
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
-	virtual Transform computeTransformation(
-			const Signature & from,
-			const Signature & to,
+	virtual Transform computeTransformationMod(
+			Signature & from,
+			Signature & to,
 			Transform guess = Transform::getIdentity(),
 			std::string * rejectedMsg = 0,
-			int * inliersOut = 0,
+			std::vector<int> * inliersOut = 0,
 			float * varianceOut = 0,
-			float * inliersRatioOut = 0);
+			float * inliersRatioOut = 0) const;
 
 	Transform computeTransformation(
 			const SensorData & from,
 			const SensorData & to,
 			Transform guess = Transform::getIdentity(),
 			std::string * rejectedMsg = 0,
-			int * inliersOut = 0,
+			std::vector<int> * inliersOut = 0,
 			float * varianceOut = 0,
-			float * inliersRatioOut = 0);
+			float * inliersRatioOut = 0) const;
 
 private:
 	float _maxTranslation;

corelib/include/rtabmap/core/RegistrationVis.h

@@ -42,14 +42,14 @@ public:
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
-	virtual Transform computeTransformation(
-			const Signature & from,
-			const Signature & to,
+	virtual Transform computeTransformationMod(
+			Signature & from,
+			Signature & to,
 			Transform guess = Transform::getIdentity(), // guess is ignored for RegistrationVis
 			std::string * rejectedMsg = 0,
-			int * inliersOut = 0,
+			std::vector<int> * inliersOut = 0,
 			float * varianceOut = 0,
-			float * inliersRatioOut = 0);
+			float * inliersRatioOut = 0) const;
 
 	float getBowInlierDistance() const {return _inlierDistance;}
 	int getBowIterations() const {return _iterations;}
@@ -64,8 +64,15 @@ private:
 	bool _force2D;
 	float _epipolarGeometryVar;
 	int _estimationType;
+	bool _forwardEstimateOnly;
 	double _PnPReprojError;
 	int _PnPFlags;
+	bool _PnPOpenCV2;
+	int _correspondencesApproach;
+	int _flowWinSize;
+	int _flowIterations;
+	float _flowEps;
+	int _flowMaxLevel;
 
 	ParametersMap _featureParameters;
 };

corelib/include/rtabmap/core/Signature.h

@@ -57,6 +57,7 @@ public:
 			const std::string & label = std::string(),
 			const Transform & pose = Transform(),
 			const SensorData & sensorData = SensorData());
+	Signature(const SensorData & data);
 	virtual ~Signature();
 
 	/**
@@ -107,10 +108,10 @@ public:
 	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
 
 	//metric stuff
-	void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
+	void setWords3(const std::multimap<int, cv::Point3f> & words3) {_words3 = words3;}
 	void setPose(const Transform & pose) {_pose = pose;}
 
-	const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
+	const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
 	const Transform & getPose() const {return _pose;}
 	cv::Mat getPoseCovariance() const;
 
@@ -132,7 +133,7 @@ private:
 	// times in the signature, it will be 2 times in this list)
 	// Words match with the CvSeq keypoints and descriptors
 	std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
-	std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint> // in base_link frame (localTransform applied))
+	std::multimap<int, cv::Point3f> _words3; // word <id, keypoint> // in base_link frame (localTransform applied))
 	std::map<int, int> _wordsChanged; // <oldId, newId>
 	bool _enabled;
 

corelib/include/rtabmap/core/Stereo.h

@@ -36,6 +36,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 class RTABMAP_EXP Stereo {
+public:
+	static Stereo * create(const ParametersMap & parameters = ParametersMap());
+
 public:
 	Stereo(const ParametersMap & parameters = ParametersMap());
 	virtual ~Stereo() {}

corelib/include/rtabmap/core/Transform.h

@@ -72,6 +72,8 @@ public:
 
 	float & operator[](int index) {return data()[index];}
 	const float & operator[](int index) const {return data()[index];}
+	float & operator()(int row, int col) {return data()[row*4 + col];}
+	const float & operator()(int row, int col) const {return data()[row*4 + col];}
 
 	bool isNull() const;
 	bool isIdentity() const;

corelib/include/rtabmap/core/VWDictionary.h

@@ -91,7 +91,7 @@ public:
 
 	void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const;
 
-	void clear();
+	void clear(bool printWarningsIfNotEmpty = true);
 	std::vector<VisualWord *> getUnusedWords() const;
 	std::vector<int> getUnusedWordIds() const;
 	unsigned int getUnusedWordsSize() const {return (int)_unusedWords.size();}

corelib/include/rtabmap/core/util3d.h

@@ -130,16 +130,18 @@ cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ>
 cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
 
-pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
+cv::Point3f RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,
 		float disparity,
 		const StereoCameraModel & model);
 
-pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
+cv::Point3f RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,
 		const cv::Mat & disparity,
 		const StereoCameraModel & model);
 
+bool RTABMAP_EXP isFinite(const cv::Point3f & pt);
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
 		const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(
@@ -176,6 +178,11 @@ void RTABMAP_EXP savePCDWords(
 		const std::multimap<int, pcl::PointXYZ> & words,
 		const Transform & transform = Transform::getIdentity());
 
+void RTABMAP_EXP savePCDWords(
+		const std::string & fileName,
+		const std::multimap<int, cv::Point3f> & words,
+		const Transform & transform = Transform::getIdentity());
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName, int dim);
 
 } // namespace util3d

corelib/include/rtabmap/core/util3d_correspondences.h

@@ -50,18 +50,18 @@ void RTABMAP_EXP findCorrespondences(
 		std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs);
 
 void RTABMAP_EXP findCorrespondences(
-		const std::multimap<int, pcl::PointXYZ> & words1,
-		const std::multimap<int, pcl::PointXYZ> & words2,
-		pcl::PointCloud<pcl::PointXYZ> & inliers1,
-		pcl::PointCloud<pcl::PointXYZ> & inliers2,
+		const std::multimap<int, cv::Point3f> & words1,
+		const std::multimap<int, cv::Point3f> & words2,
+		std::vector<cv::Point3f> & inliers1,
+		std::vector<cv::Point3f> & inliers2,
 		float maxDepth,
 		std::vector<int> * uniqueCorrespondences = 0);
 
 void RTABMAP_EXP findCorrespondences(
-		const std::map<int, pcl::PointXYZ> & words1,
-		const std::map<int, pcl::PointXYZ> & words2,
-		pcl::PointCloud<pcl::PointXYZ> & inliers1,
-		pcl::PointCloud<pcl::PointXYZ> & inliers2,
+		const std::map<int, cv::Point3f> & words1,
+		const std::map<int, cv::Point3f> & words2,
+		std::vector<cv::Point3f> & inliers1,
+		std::vector<cv::Point3f> & inliers2,
 		float maxDepth,
 		std::vector<int> * correspondences = 0);
 

corelib/include/rtabmap/core/util3d_features.h

@@ -30,8 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/RtabmapExp.h>
 
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/CameraModel.h>
@@ -46,38 +44,39 @@ namespace util3d
 {
 
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
+std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DDepth(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
 		const CameraModel & cameraModel);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
+std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DDepth(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
 		const std::vector<CameraModel> & cameraModels);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDisparity(
+std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DDisparity(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & disparity,
 		const StereoCameraModel & stereoCameraMode);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
+std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DStereo(
 		const std::vector<cv::Point2f> & leftCorners,
 		const std::vector<cv::Point2f> & rightCorners,
 		const StereoCameraModel & model,
 		const std::vector<unsigned char> & mask = std::vector<unsigned char>());
 
-std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
-		const std::multimap<int, cv::KeyPoint> & kpts,
-		const std::multimap<int, cv::KeyPoint> & previousKpts,
+std::map<int, cv::Point3f> RTABMAP_EXP generateWords3DMono(
+		const std::map<int, cv::KeyPoint> & kpts,
+		const std::map<int, cv::KeyPoint> & previousKpts,
 		const CameraModel & cameraModel,
 		Transform & cameraTransform,
 		int pnpIterations = 100,
 		float pnpReprojError = 8.0f,
 		int pnpFlags = 0, // cv::SOLVEPNP_ITERATIVE
+		bool pnpOpenCV2 = true,
 		float ransacParam1 = 3.0f,
 		float ransacParam2 = 0.99f,
-		const std::multimap<int, pcl::PointXYZ> & refGuess3D = std::multimap<int, pcl::PointXYZ>(),
+		const std::map<int, cv::Point3f> & refGuess3D = std::map<int, cv::Point3f>(),
 		double * variance = 0);
 
 std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(

corelib/include/rtabmap/core/util3d_motion_estimation.h

@@ -30,8 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/RtabmapExp.h>
 
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/CameraModel.h>
 
@@ -42,22 +40,23 @@ namespace util3d
 {
 
 Transform RTABMAP_EXP estimateMotion3DTo2D(
-			const std::map<int, pcl::PointXYZ> & words3A,
+			const std::map<int, cv::Point3f> & words3A,
 			const std::map<int, cv::KeyPoint> & words2B,
 			const CameraModel & cameraModel,
 			int minInliers = 10,
 			int iterations = 100,
 			double reprojError = 5.,
 			int flagsPnP = 0,
+			bool pnpOpenCV2 = true,
 			const Transform & guess = Transform::getIdentity(),
-			const std::map<int, pcl::PointXYZ> & words3B = std::map<int, pcl::PointXYZ>(),
+			const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
 			double * varianceOut = 0, // mean reproj error if words3B is not set
 			std::vector<int> * matchesOut = 0,
 			std::vector<int> * inliersOut = 0);
 
 Transform RTABMAP_EXP estimateMotion3DTo3D(
-			const std::map<int, pcl::PointXYZ> & words3A,
-			const std::map<int, pcl::PointXYZ> & words3B,
+			const std::map<int, cv::Point3f> & words3A,
+			const std::map<int, cv::Point3f> & words3B,
 			int minInliers = 10,
 			double inliersDistance = 0.1,
 			int iterations = 100,
@@ -66,6 +65,21 @@ Transform RTABMAP_EXP estimateMotion3DTo3D(
 			std::vector<int> * matchesOut = 0,
 			std::vector<int> * inliersOut = 0);
 
+void RTABMAP_EXP solvePnPRansac(
+		cv::InputArray _opoints,
+		cv::InputArray _ipoints,
+		cv::InputArray _cameraMatrix,
+		cv::InputArray _distCoeffs,
+		cv::OutputArray _rvec,
+		cv::OutputArray _tvec,
+		bool useExtrinsicGuess,
+        int iterationsCount,
+        float reprojectionError,
+        int minInliersCount,
+        cv::OutputArray _inliers,
+        int flags,
+        bool opencv2version);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/include/rtabmap/core/util3d_transforms.h

@@ -53,6 +53,9 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Transform & transform);
 
+cv::Point3f RTABMAP_EXP transformPoint(
+		const cv::Point3f & pt,
+		const Transform & transform);
 pcl::PointXYZ RTABMAP_EXP transformPoint(
 		const pcl::PointXYZ & pt,
 		const Transform & transform);