Refactoring Registration: added RegistrationInfo class, pipeline registration. Camera source: create laser scan from depth image. Removed OdometryICP as the same behavior can be achieved with OdometryF2F and "Reg/Strategy" = 1 (ICP) or 2 (Vis+ICP).

corelib/include/rtabmap/core/CameraEvent.h

@@ -48,7 +48,7 @@ public:
 		UEvent(kCodeData),
 		data_(image, seq, stamp)
 	{
-		cameraInfo_.cameraName_ = cameraName;
+		cameraInfo_.cameraName = cameraName;
 	}
 
 	CameraEvent() :
@@ -66,7 +66,7 @@ public:
 		UEvent(kCodeData),
 		data_(data)
 	{
-		cameraInfo_.cameraName_ = cameraName;
+		cameraInfo_.cameraName = cameraName;
 	}
 	CameraEvent(const SensorData & data, const CameraInfo & cameraInfo) :
 		UEvent(kCodeData),
@@ -77,7 +77,7 @@ public:
 
 	// Image or descriptors
 	const SensorData & data() const {return data_;}
-	const std::string & cameraName() const {return cameraInfo_.cameraName_;}
+	const std::string & cameraName() const {return cameraInfo_.cameraName;}
 	const CameraInfo & info() const {return cameraInfo_;}
 
 	virtual ~CameraEvent() {}

corelib/include/rtabmap/core/CameraInfo.h

@@ -37,20 +37,22 @@ class CameraInfo
 
 public:
 	CameraInfo() :
-		cameraName_(""),
-		id_(0),
-		timeCapture_(0.0),
-		timeDisparity_(0.0),
-		timeMirroring_(0.0)
+		cameraName(""),
+		id(0),
+		timeCapture(0.0f),
+		timeDisparity(0.0f),
+		timeMirroring(0.0f),
+		timeScanFromDepth(0.0f)
 	{
 	}
 	virtual ~CameraInfo() {}
 
-	std::string cameraName_;
-	int id_;
-	float timeCapture_;
-	float timeDisparity_;
-	float timeMirroring_;
+	std::string cameraName;
+	int id;
+	float timeCapture;
+	float timeDisparity;
+	float timeMirroring;
+	float timeScanFromDepth;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraThread.h

@@ -55,6 +55,8 @@ public:
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
 	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
+	void setScanFromDepth(bool enabled, int decimation=4, float maxDepth=4.0f)
+	{_scanFromDepth = enabled; _scanDecimation=decimation; _scanMaxDepth = maxDepth;}
 
 	//getters
 	bool isPaused() const {return !this->isRunning();}
@@ -72,6 +74,9 @@ private:
 	bool _mirroring;
 	bool _colorOnly;
 	bool _stereoToDepth;
+	bool _scanFromDepth;
+	int _scanDecimation;
+	float _scanMaxDepth;
 	StereoDense * _stereoDense;
 };
 

corelib/include/rtabmap/core/Memory.h

@@ -51,7 +51,8 @@ class VWDictionary;
 class VisualWord;
 class Feature2D;
 class Statistics;
-class RegistrationVis;
+class Registration;
+class RegistrationInfo;
 class RegistrationIcp;
 class Stereo;
 
@@ -178,15 +179,13 @@ public:
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
 
-	Transform computeVisualTransform(int fromId, int toId, std::string * rejectedMsg = 0, int * inliers = 0, float * variance = 0);
-	Transform computeIcpTransform(int fromId, int toId, Transform guess, std::string * rejectedMsg = 0, int * correspondences = 0, float * variance = 0, float * correspondencesRatio = 0);
+	Transform computeTransform(int fromId, int toId, RegistrationInfo * info = 0);
+	Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
 	Transform computeIcpTransformMulti(
 			int newId,
 			int oldId,
 			const std::map<int, Transform> & poses,
-			std::string * rejectedMsg = 0,
-			int * inliers = 0,
-			float * variance = 0);
+			RegistrationInfo * info = 0);
 
 private:
 	void preUpdate();
@@ -264,7 +263,7 @@ private:
 	bool _tfIdfLikelihoodUsed;
 	bool _parallelized;
 
-	RegistrationVis * _registrationVis;
+	Registration * _registrationPipeline;
 	RegistrationIcp * _registrationIcp;
 };
 

corelib/include/rtabmap/core/Odometry.h

@@ -50,7 +50,7 @@ public:
 
 public:
 	static Odometry * create(const ParametersMap & parameters);
-	static Odometry * create(Type & type, const ParametersMap & parameters);
+	static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap());
 
 public:
 	virtual ~Odometry();

corelib/include/rtabmap/core/OdometryF2F.h

@@ -29,10 +29,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYF2F_H_
 
 #include <rtabmap/core/Odometry.h>
-#include <rtabmap/core/RegistrationVis.h>
+#include <rtabmap/core/Signature.h>
 
 namespace rtabmap {
 
+class Registration;
+
 class RTABMAP_EXP OdometryF2F : public Odometry
 {
 public:
@@ -51,7 +53,7 @@ private:
 	int keyFrameThr_;
 	bool guessFromMotion_;
 
-	RegistrationVis registration_;
+	Registration * registrationPipeline_;
 	Signature refFrame_;
 	Transform motionSinceLastKeyFrame_;
 };

corelib/include/rtabmap/core/OdometryICP.h

@@ -1,68 +0,0 @@
-/*
-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 ODOMETRYICP_H_
-#define ODOMETRYICP_H_
-
-#include <rtabmap/core/Odometry.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
-namespace rtabmap {
-
-class RTABMAP_EXP OdometryICP : public Odometry
-{
-public:
-	OdometryICP(int decimation = 4,
-			float voxelSize = 0.005f,
-			int samples = 0,
-			float maxCorrespondenceDistance = 0.05f,
-			int maxIterations = 30,
-			float correspondenceRatio = 0.7f,
-			bool pointToPlane = true,
-			const ParametersMap & odometryParameter = rtabmap::ParametersMap());
-	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-
-private:
-	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
-
-private:
-	int _decimation;
-	float _voxelSize;
-	float _samples;
-	float _maxCorrespondenceDistance;
-	int	_maxIterations;
-	float _correspondenceRatio;
-	bool _pointToPlane;
-
-	pcl::PointCloud<pcl::PointNormal>::Ptr _previousCloudNormal; // for point ot plane
-	pcl::PointCloud<pcl::PointXYZ>::Ptr _previousCloud; // for point to point
-};
-
-}
-
-#endif /* ODOMETRYICP_H_ */

corelib/include/rtabmap/core/Parameters.h

@@ -311,7 +311,6 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, LocalImmunizationRatio,   float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
 	RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true,      "Save scan matching IDs in link's user data.");
 	RTABMAP_PARAM(RGBD, NeighborLinkRefining,          bool, false,  "When a new node is added to the graph, the transformation of its neighbor link to the previous node is refined using ICP (laser scans required!).");
-	RTABMAP_PARAM(RGBD, LoopClosureLinkRefining,      bool, false, 	"If the estimated loop closure transformation is refined using ICP (laser scans required!).");
 	RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures,  bool, true,    "Extract features even if there are some already in the nodes.");
 
 	// Local/Proximity loop closure detection
@@ -361,6 +360,8 @@ class RTABMAP_EXP Parameters
 
 	// Common registration parameters
 	RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
+	RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
+	RTABMAP_PARAM(Reg, Force3DoF,                bool, false,   "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
 	
 	// Visual registration parameters
 	RTABMAP_PARAM(Vis, EstimationType,           int, 0,    	"Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
@@ -373,7 +374,6 @@ class RTABMAP_EXP Parameters
 	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, 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).");
@@ -394,7 +394,6 @@ class RTABMAP_EXP Parameters
 	// ICP registration parameters
 	RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
 	RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");
-	RTABMAP_PARAM(Icp, 2D,                        bool, true, 	"If 2D ICP is done (only 3Dof -> x,y,yaw).");
 	RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.025, "Uniform sampling voxel size (0=disabled).");
 	RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1, 	    "Downsampling step size (1=no sampling). This is done before uniform sampling.");
 	RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");

corelib/include/rtabmap/core/Registration.h

@@ -32,50 +32,75 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Signature.h>
+#include <rtabmap/core/RegistrationInfo.h>
 
 namespace rtabmap {
 
 class RTABMAP_EXP Registration
 {
 public:
-	virtual ~Registration() {}
-	virtual void parseParameters(const ParametersMap & parameters)
-	{
-		Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
-	}
+	enum Type {
+		kTypeUndef = -1,
+		kTypeVis = 0,
+		kTypeIcp = 1,
+		kTypeVisIcp = 2
+	};
+
+public:
+	static Registration * create(const ParametersMap & parameters);
+	static Registration * create(Type & type, const ParametersMap & parameters = ParametersMap());
+
+public:
+	virtual ~Registration();
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	bool isImageRequired() const;
+	bool isScanRequired() const;
+	bool isUserDataRequired() const;
+
+	bool varianceFromInliersCount() const {return varianceFromInliersCount_;}
+	bool force3DoF() const {return force3DoF_;}
+
+	// take ownership!
+	void setChildRegistration(Registration * child);
 
 	Transform computeTransformation(
 			const Signature & from,
 			const Signature & to,
 			Transform guess = Transform::getIdentity(),
-			std::string * rejectedMsg = 0,
-			std::vector<int> * inliersOut = 0,
-			float * varianceOut = 0,
-			float * inliersRatioOut = 0) const
-	{
-		Signature fromCopy(from);
-		Signature toCopy(to);
-		return computeTransformationMod(fromCopy, toCopy, guess, rejectedMsg, inliersOut, varianceOut, inliersRatioOut);
-	}
+			RegistrationInfo * info = 0) const;
+	Transform computeTransformation(
+			const SensorData & from,
+			const SensorData & to,
+			Transform SensorData = Transform::getIdentity(),
+			RegistrationInfo * info = 0) const;
 
-	virtual Transform computeTransformationMod(
+	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;
+			RegistrationInfo * info = 0) const;
 
 protected:
-	Registration(const ParametersMap & parameters = ParametersMap()) :
-		_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount())
-	{
-		this->parseParameters(parameters);
-	}
+	// take ownership of child
+	Registration(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
 
-protected:
-	bool _varianceFromInliersCount;
+	// It is safe to modify the signatures in the implementation, if so, the
+	// child registration will use these modifications.
+	virtual Transform computeTransformationImpl(
+			Signature & from,
+			Signature & to,
+			Transform guess,
+			RegistrationInfo & info) const = 0;
+
+	virtual bool isImageRequiredImpl() const = 0;
+	virtual bool isScanRequiredImpl() const = 0;
+	virtual bool isUserDataRequiredImpl() const = 0;
+
+private:
+	bool varianceFromInliersCount_;
+	bool force3DoF_;
+	Registration * child_;
 
 };
 

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -39,33 +39,25 @@ namespace rtabmap {
 class RTABMAP_EXP RegistrationIcp : public Registration
 {
 public:
-	RegistrationIcp(const ParametersMap & parameters = ParametersMap());
+	// take ownership of child
+	RegistrationIcp(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
 	virtual ~RegistrationIcp() {}
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
-	virtual Transform computeTransformationMod(
+protected:
+	virtual Transform computeTransformationImpl(
 			Signature & from,
 			Signature & to,
-			Transform guess = Transform::getIdentity(),
-			std::string * rejectedMsg = 0,
-			std::vector<int> * inliersOut = 0,
-			float * varianceOut = 0,
-			float * inliersRatioOut = 0) const;
-
-	Transform computeTransformation(
-			const SensorData & from,
-			const SensorData & to,
-			Transform guess = Transform::getIdentity(),
-			std::string * rejectedMsg = 0,
-			std::vector<int> * inliersOut = 0,
-			float * varianceOut = 0,
-			float * inliersRatioOut = 0) const;
+			Transform guess,
+			RegistrationInfo & info) const;
+	virtual bool isImageRequiredImpl() const {return false;}
+	virtual bool isScanRequiredImpl() const {return true;}
+	virtual bool isUserDataRequiredImpl() const {return false;}
 
 private:
 	float _maxTranslation;
 	float _maxRotation;
-	bool _icp2D;
 	float _voxelSize;
 	int _downsamplingStep;
 	float _maxCorrespondenceDistance;

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -0,0 +1,33 @@
+/*
+ * RegistrationInfo.h
+ *
+ *  Created on: Jan 5, 2016
+ *      Author: mathieu
+ */
+
+#ifndef REGISTRATIONINFO_H_
+#define REGISTRATIONINFO_H_
+
+
+namespace rtabmap {
+
+class RegistrationInfo
+{
+public:
+	RegistrationInfo() :
+		variance(0),
+		inliers(0),
+		inliersRatio(0)
+	{
+	}
+
+	float variance;
+	int inliers;
+	float inliersRatio;
+	std::vector<int> inliersIndexes_;
+	std::string rejectedMsg_;
+};
+
+}
+
+#endif /* REGISTRATIONINFO_H_ */

corelib/include/rtabmap/core/RegistrationVis.h

@@ -39,31 +39,32 @@ namespace rtabmap {
 class RTABMAP_EXP RegistrationVis : public Registration
 {
 public:
-	RegistrationVis(const ParametersMap & parameters = ParametersMap());
+	// take ownership of child
+	RegistrationVis(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
 	virtual ~RegistrationVis();
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
-	virtual Transform computeTransformationMod(
-			Signature & from,
-			Signature & to,
-			Transform guess = Transform::getIdentity(), // guess is ignored for RegistrationVis
-			std::string * rejectedMsg = 0,
-			std::vector<int> * inliersOut = 0,
-			float * varianceOut = 0,
-			float * inliersRatioOut = 0) const;
-
 	float getBowInlierDistance() const {return _inlierDistance;}
 	int getBowIterations() const {return _iterations;}
 	int getBowMinInliers() const {return _minInliers;}
-	bool getBowForce2D() const {return _force2D;}
+
+protected:
+	virtual Transform computeTransformationImpl(
+			Signature & from,
+			Signature & to,
+			Transform guess,
+			RegistrationInfo & info) const;
+
+	virtual bool isImageRequiredImpl() const {return true;}
+	virtual bool isScanRequiredImpl() const {return false;}
+	virtual bool isUserDataRequiredImpl() const {return false;}
 
 private:
 	int _minInliers;
 	float _inlierDistance;
 	int _iterations;
 	int _refineIterations;
-	bool _force2D;
 	float _epipolarGeometryVar;
 	int _estimationType;
 	bool _forwardEstimateOnly;

corelib/include/rtabmap/core/Rtabmap.h

@@ -184,7 +184,6 @@ private:
 	float _rgbdLinearUpdate;
 	float _rgbdAngularUpdate;
 	float _newMapOdomChangeDistance;
-	bool _loopClosureRefining;
 	bool _neighborLinkRefining;
 	bool _proximityByTime;
 	bool _proximityBySpace;

corelib/include/rtabmap/core/Transform.h

@@ -97,6 +97,7 @@ public:
 	Transform inverse() const;
 	Transform rotation() const;
 	Transform translation() const;
+	Transform to3DoF() const;
 
 	void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
 	void getEulerAngles(float & roll, float & pitch, float & yaw) const;