Refactoring Odometry

corelib/include/rtabmap/core/Odometry.h

@@ -59,18 +59,7 @@ public:
 
 	//getters
 	const Transform & getPose() const {return _pose;}
-	const std::string & getRoiRatios() const {return _roiRatios;}
-	int getMinInliers() const {return _minInliers;}
-	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;}
-	int getPnPRefineIterations() const {return _pnpRefineIterations;}
 	const Transform & previousTransform() const {return previousTransform_;}
 	bool isVarianceFromInliersCount() const {return _varianceFromInliersCount;}
 
@@ -81,15 +70,8 @@ private:
 	void updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw);
 
 private:
-	std::string _roiRatios;
-	int _minInliers;
-	float _inlierDistance;
-	int _iterations;
-	int _refineIterations;
-	float _minDepth;
-	float _maxDepth;
 	int _resetCountdown;
-	bool _force2D;
+	bool _force3DoF;
 	bool _holonomic;
 	int _filteringStrategy;
 	int _particleSize;
@@ -98,10 +80,6 @@ private:
 	float _particleNoiseR;
 	float _particleLambdaR;
 	bool _fillInfoData;
-	int _estimationType;
-	double _pnpReprojError;
-	int _pnpFlags;
-	int _pnpRefineIterations;
 	bool _varianceFromInliersCount;
 	float _kalmanProcessNoise;
 	float _kalmanMeasurementNoise;

corelib/include/rtabmap/core/OdometryLocalMap.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 class Memory;
+class RegistrationVis;
 
 class RTABMAP_EXP OdometryLocalMap : public Odometry
 {
@@ -41,19 +42,20 @@ public:
 	virtual ~OdometryLocalMap();
 
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-	const std::map<int, cv::Point3f> & getLocalMap() const {return localMap_;}
-	const Memory * getMemory() const {return _memory;}
+	const std::multimap<int, cv::Point3f> & getLocalMap() const {return localMap_;}
+	const Memory * getMemory() const {return memory_;}
 
 private:
 	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
 
 private:
 	//Parameters
-	int _localHistoryMaxSize;
-	std::string _fixedLocalMapPath;
+	int localHistoryMaxSize_;
+	std::string fixedLocalMapPath_;
 
-	Memory * _memory;
-	std::map<int, cv::Point3f> localMap_;
+	Memory * memory_;
+	RegistrationVis * regVis_;
+	std::multimap<int, cv::Point3f> localMap_;
 };
 
 }

corelib/include/rtabmap/core/OdometryMono.h

@@ -50,6 +50,11 @@ private:
 	int flowIterations_;
 	double flowEps_;
 	int flowMaxLevel_;
+	int minInliers_;
+	int iterations_;
+	double pnpReprojError_;
+	int pnpFlags_;
+	int pnpRefineIterations_;
 
 	Stereo * stereo_;
 

corelib/include/rtabmap/core/Registration.h

@@ -58,6 +58,9 @@ public:
 	bool isScanRequired() const;
 	bool isUserDataRequired() const;
 
+	int getMinVisualCorrespondences() const;
+	float getMinGeometryCorrespondencesRatio() const;
+
 	bool varianceFromInliersCount() const {return varianceFromInliersCount_;}
 	bool force3DoF() const {return force3DoF_;}
 
@@ -93,9 +96,11 @@ protected:
 			Transform guess,
 			RegistrationInfo & info) const = 0;
 
-	virtual bool isImageRequiredImpl() const = 0;
-	virtual bool isScanRequiredImpl() const = 0;
-	virtual bool isUserDataRequiredImpl() const = 0;
+	virtual bool isImageRequiredImpl() const {return false;}
+	virtual bool isScanRequiredImpl() const {return false;}
+	virtual bool isUserDataRequiredImpl() const {return false;}
+	virtual int getMinVisualCorrespondencesImpl() const {return 0;}
+	virtual float getMinGeometryCorrespondencesRatioImpl() const {return 0.0f;}
 
 private:
 	bool varianceFromInliersCount_;

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -51,9 +51,8 @@ protected:
 			Signature & to,
 			Transform guess,
 			RegistrationInfo & info) const;
-	virtual bool isImageRequiredImpl() const {return false;}
 	virtual bool isScanRequiredImpl() const {return true;}
-	virtual bool isUserDataRequiredImpl() const {return false;}
+	virtual float getMinGeometryCorrespondencesRatioImpl() const {return _correspondenceRatio;}
 
 private:
 	float _maxTranslation;

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -17,15 +17,18 @@ public:
 	RegistrationInfo() :
 		variance(0),
 		inliers(0),
-		inliersRatio(0)
+		inliersRatio(0),
+		matches(0)
 	{
 	}
 
 	float variance;
 	int inliers;
 	float inliersRatio;
-	std::vector<int> inliersIndexes_;
-	std::string rejectedMsg_;
+	std::vector<int> inliersIDs;
+	int matches;
+	std::vector<int> matchesIDs;
+	std::string rejectedMsg;
 };
 
 }

corelib/include/rtabmap/core/RegistrationVis.h

@@ -45,9 +45,9 @@ public:
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
-	float getBowInlierDistance() const {return _inlierDistance;}
-	int getBowIterations() const {return _iterations;}
-	int getBowMinInliers() const {return _minInliers;}
+	float getInlierDistance() const {return _inlierDistance;}
+	int getIterations() const {return _iterations;}
+	int getMinInliers() const {return _minInliers;}
 
 protected:
 	virtual Transform computeTransformationImpl(
@@ -57,8 +57,7 @@ protected:
 			RegistrationInfo & info) const;
 
 	virtual bool isImageRequiredImpl() const {return true;}
-	virtual bool isScanRequiredImpl() const {return false;}
-	virtual bool isUserDataRequiredImpl() const {return false;}
+	virtual int getMinVisualCorrespondencesImpl() const {return _minInliers;}
 
 private:
 	int _minInliers;