Binary descriptors (ORB, BRIEF, FREAK) can now be used for the visual dictionnary: maybe not as discriminative as SIFT/SURF on large environments, the advantage is that RTAB-Map will work without Patent/noncommercial licenses of SIFT and SURF.

A new option is added to re-extract features when a loop closure hypothesis is found. Another new option is to force 2D (3DoF) transform on visual odometry and loop closures. git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1679 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-09-01 17:10:26 +08:00 · 2014-09-21 16:51:51 +00:00
parent 02850ec756
commit a98f819821
10 changed files with 463 additions and 34 deletions
--- a/corelib/include/rtabmap/core/Memory.h
+++ b/corelib/include/rtabmap/core/Memory.h
@@ -115,14 +115,24 @@ public:
 	int getMapId(int signatureId) const;
 	std::vector<unsigned char> getImage(int signatureId) const;
 	void getImageDepth(
-			int locationId, std::vector<unsigned char> & rgb,
+			int locationId,
+			std::vector<unsigned char> & rgb,
 			std::vector<unsigned char> & depth,
 			std::vector<unsigned char> & depth2d,
 			float & fx,
 			float & fy,
 			float & cx,
 			float & cy,
-			Transform & localTransform) const;
+			Transform & localTransform);
+	void getImageDepthRaw(
+				int locationId,
+				cv::Mat & rgb,
+				cv::Mat & depth,
+				float & fx,
+				float & fy,
+				float & cx,
+				float & cy,
+				Transform & localTransform);
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
@@ -171,8 +181,13 @@ public:
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
-	Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0) const;
-	Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0) const;
+	float getBowInlierDistance() const {return _bowInlierDistance;}
+	int getBowIterations() const {return _bowIterations;}
+	int getBowMinInliers() const {return _bowMinInliers;}
+	float getBowMaxDepth() const {return _bowMaxDepth;}
+	bool getBowForce2D() const {return _bowForce2D;}
+	Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0) const;
+	Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 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(
@@ -252,6 +267,7 @@ private:
 	float _bowInlierDistance;
 	int _bowIterations;
 	float _bowMaxDepth;
+	bool _bowForce2D;
 	int _icpDecimation;
 	float _icpMaxDepth;
 	float _icpVoxelSize;
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -273,6 +273,12 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.01, 	"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, 5.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(LccReextract, LoopClosureFeatures, bool, false, 	"Re-extract 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.7, 	"NNDR: nearest neighbor distance ratio.");
+	RTABMAP_PARAM(LccReextract, FeatureType, int, 4, 		"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF.");
+	RTABMAP_PARAM(LccReextract, MaxWords, 	int, 0, 		"0 no limits.");

 	RTABMAP_PARAM(LccIcp3, Decimation,      int, 8, 		"Depth image decimation.");
 	RTABMAP_PARAM(LccIcp3, MaxDepth,        float, 4.0, 	"Max cloud depth.");
--- a/corelib/include/rtabmap/core/Rtabmap.h
+++ b/corelib/include/rtabmap/core/Rtabmap.h
@@ -167,6 +167,11 @@ private:
 	int _toroIterations;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
+	bool _reextractLoopClosureFeatures;
+	int _reextractNNType;
+	float _reextractNNDR;
+	int _reextractFeatureType;
+	int _reextractMaxWords;

 	int _lcHypothesisId;
 	float _lcHypothesisValue;
--- a/corelib/include/rtabmap/core/Signature.h
+++ b/corelib/include/rtabmap/core/Signature.h
@@ -114,6 +114,8 @@ public:
 	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
 	void setImage(const std::vector<unsigned char> & image) {_image = image;}
 	const std::vector<unsigned char> & getImage() const {return _image;}
+	void setImageRaw(const cv::Mat & image) {_imageRaw = image;}
+	const cv::Mat & getImageRaw() const {return _imageRaw;}

 	//metric stuff
 	void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
@@ -130,6 +132,8 @@ public:
 	float getDepthCy() const {return _cy;}
 	const Transform & getPose() const {return _pose;}
 	const Transform & getLocalTransform() const {return _localTransform;}
+	void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
+	const cv::Mat & getDepthRaw() const {return _depthRaw;}

 private:
 	int _id;
@@ -159,6 +163,9 @@ private:
 	Transform _pose;
 	Transform _localTransform; // camera_link -> base_link
 	std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
+
+	cv::Mat _imageRaw;
+	cv::Mat _depthRaw;
 };

 } // namespace rtabmap
--- a/corelib/include/rtabmap/core/Statistics.h
+++ b/corelib/include/rtabmap/core/Statistics.h
@@ -58,6 +58,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, ReactivateId,);
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
 	RTABMAP_STATS(Loop, Hypothesis_reactivated,);
+	RTABMAP_STATS(Loop, VisualInliers,);
 	RTABMAP_STATS(Loop, Last_loop_closure_parent,);
 	RTABMAP_STATS(Loop, Last_loop_closure_child,);

--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -84,6 +84,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_bowInlierDistance(Parameters::defaultLccBowInlierDistance()),
 	_bowIterations(Parameters::defaultLccBowIterations()),
 	_bowMaxDepth(Parameters::defaultLccBowMaxDepth()),
+	_bowForce2D(Parameters::defaultLccBowForce2D()),

 	_icpDecimation(Parameters::defaultLccIcp3Decimation()),
 	_icpMaxDepth(Parameters::defaultLccIcp3MaxDepth()),
@@ -324,6 +325,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kLccBowInlierDistance(), _bowInlierDistance);
 	Parameters::parse(parameters, Parameters::kLccBowIterations(), _bowIterations);
 	Parameters::parse(parameters, Parameters::kLccBowMaxDepth(), _bowMaxDepth);
+	Parameters::parse(parameters, Parameters::kLccBowForce2D(), _bowForce2D);
 	Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), _icpDecimation);
 	Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), _icpMaxDepth);
 	Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), _icpVoxelSize);
@@ -1636,7 +1638,7 @@ void Memory::rejectLoopClosure(int oldId, int newId)
 }

 // compute transform newId -> oldId
-Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rejectedMsg) const
+Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rejectedMsg, int * inliers) const
 {
 	const Signature * oldS = this->getSignature(oldId);
 	const Signature * newS = this->getSignature(newId);
@@ -1645,7 +1647,7 @@ Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rej

 	if(oldS && newId)
 	{
-		return computeVisualTransform(*oldS, *newS, rejectedMsg);
+		return computeVisualTransform(*oldS, *newS, rejectedMsg, inliers);
 	}
 	else
 	{
@@ -1660,7 +1662,7 @@ Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rej
 }

 // compute transform newId -> oldId
-Transform Memory::computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg) const
+Transform Memory::computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg, int * inliers) const
 {
 	Transform transform;
 	std::string msg;
@@ -1690,6 +1692,13 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
 			if(!t.isNull() && inliersCount >= _bowMinInliers)
 			{
 				transform = t;
+				if(_bowForce2D)
+				{
+					UDEBUG("Forcing 2D...");
+					float x,y,z,r,p,yaw;
+					transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
+					transform = util3d::transformFromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
+				}
 			}
 			else if(inliersCount < _bowMinInliers)
 			{
@@ -1701,6 +1710,11 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
 				msg = uFormat("Rejected identity with full inliers.");
 				UINFO(msg.c_str());
 			}
+
+			if(inliers)
+			{
+				*inliers = inliersCount;
+			}
 		}
 		else
 		{
@@ -2489,9 +2503,9 @@ void Memory::getImageDepth(
 		float & fy,
 		float & cx,
 		float & cy,
-		Transform & localTransform) const
+		Transform & localTransform)
 {
-	const Signature * s = this->getSignature(locationId);
+	Signature * s = this->_getSignature(locationId);
 	if(s)
 	{
 		rgb = s->getImage();
@@ -2506,6 +2520,79 @@ void Memory::getImageDepth(
 	if(rgb.empty() && this->isRawDataKept() && _dbDriver)
 	{
 		_dbDriver->getNodeData(locationId, rgb, depth, depth2d, fx, fy, cx, cy, localTransform);
+
+		if(s)
+		{
+			// keep in cache
+			if(!rgb.empty())
+			{
+				s->setImage(rgb);
+			}
+			if(!depth.empty())
+			{
+				s->setDepth(depth, fx, fy, cx, cy);
+			}
+			if(!depth2d.empty())
+			{
+				s->setDepth2D(depth2d);
+			}
+			if(!localTransform.isNull())
+			{
+				s->setLocalTransform(localTransform);
+			}
+		}
+	}
+}
+
+void Memory::getImageDepthRaw(
+		int locationId,
+		cv::Mat & rgb,
+		cv::Mat & depth,
+		float & fx,
+		float & fy,
+		float & cx,
+		float & cy,
+		Transform & localTransform)
+{
+	Signature * s = this->_getSignature(locationId);
+	if(s)
+	{
+		rgb = s->getImageRaw();
+		depth = s->getDepthRaw();
+		fx = s->getDepthFx();
+		fy = s->getDepthFy();
+		cx = s->getDepthCx();
+		cy = s->getDepthCy();
+		localTransform = s->getLocalTransform();
+	}
+	if(rgb.empty())
+	{
+		std::vector<unsigned char> compressedRgb;
+		std::vector<unsigned char> compressedDepth;
+		std::vector<unsigned char> comressedDepth2d;
+		getImageDepth(locationId, compressedRgb, compressedDepth, comressedDepth2d, fx, fy, cx, cy, localTransform);
+
+		//uncomressed data
+		util3d::CompressionThread ctImage(compressedRgb, true);
+		util3d::CompressionThread ctDepth(compressedDepth, true);
+		ctImage.start();
+		ctDepth.start();
+		ctImage.join();
+		ctDepth.join();
+		rgb = ctImage.getUncompressedData();
+		depth = ctDepth.getUncompressedData();
+		if(s)
+		{
+			//save it uncompressed in the signature
+			if(!rgb.empty())
+			{
+				s->setImageRaw(rgb);
+			}
+			if(!depth.empty())
+			{
+				s->setDepthRaw(depth);
+			}
+		}
 	}
 }

@@ -3110,6 +3197,8 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 			data.depthCx(),
 			data.depthCy(),
 			data.localTransform());
+		s->setImageRaw(data.image());
+		s->setDepthRaw(data.depth());
 	}
 	else
 	{
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -100,6 +100,11 @@ Rtabmap::Rtabmap() :
 	_toroIterations(Parameters::defaultRGBDToroIterations()),
 	_databasePath(""),
 	_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
+	_reextractLoopClosureFeatures(Parameters::defaultLccReextractLoopClosureFeatures()),
+	_reextractNNType(Parameters::defaultLccReextractNNType()),
+	_reextractNNDR(Parameters::defaultLccReextractNNDR()),
+	_reextractFeatureType(Parameters::defaultLccReextractFeatureType()),
+	_reextractMaxWords(Parameters::defaultLccReextractMaxWords()),
 	_lcHypothesisId(0),
 	_lcHypothesisValue(0),
 	_retrievedId(0),
@@ -349,6 +354,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
 	Parameters::parse(parameters, Parameters::kRGBDToroIterations(), _toroIterations);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
+	Parameters::parse(parameters, Parameters::kLccReextractLoopClosureFeatures(), _reextractLoopClosureFeatures);
+	Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
+	Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR);
+	Parameters::parse(parameters, Parameters::kLccReextractFeatureType(), _reextractFeatureType);
+	Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords);

 	// RGB-D SLAM stuff
 	if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
@@ -1254,6 +1264,7 @@ bool Rtabmap::process(const SensorData & data)
 	// Update loop closure links
 	// (updated: place this after retrieval to be sure that neighbors of the loop closure are in RAM)
 	//=============================================================
+	int loopClosureVisualInliers = 0; // for statistics
 	if(_lcHypothesisId>0)
 	{
 		//Compute transform if metric data are present
@@ -1261,7 +1272,65 @@ bool Rtabmap::process(const SensorData & data)
 		if(_rgbdSlamMode)
 		{
 			std::string rejectedMsg;
-			transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg);
+			if(_reextractLoopClosureFeatures)
+			{
+				ParametersMap customParameters;
+				customParameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(_memory->getBowInlierDistance())));
+				customParameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(_memory->getBowIterations())));
+				customParameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(_memory->getBowMinInliers())));
+				customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_memory->getBowMaxDepth())));
+				customParameters.insert(ParametersPair(Parameters::kLccBowForce2D(), uNumber2Str(_memory->getBowForce2D())));
+				customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+				customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
+				customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
+				customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
+				customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
+				customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
+				customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
+
+				Memory memory(customParameters);
+
+				UTimer timeT;
+
+				// Add signatures
+				float fxA, fyA, cxA, cyA;
+				float fxB, fyB, cxB, cyB;
+				rtabmap::Transform localTransformA, localTransformB;
+
+				cv::Mat imageA, depthA;
+				_memory->getImageDepthRaw(signature->id(), imageA, depthA, fxA, fyA, cxA, cyA, localTransformA);
+				SensorData dataFrom(imageA, depthA, fxA, fyA, cxA, cyA, Transform::getIdentity(), localTransformA, 1);
+
+				UDEBUG("timeA = %fs", timeT.ticks());
+
+				cv::Mat imageB, depthB;
+				_memory->getImageDepthRaw(_lcHypothesisId, imageB, depthB, fxB, fyB, cxB, cyB, localTransformB);
+				SensorData dataTo(imageB, depthB, fxB, fyB, cxB, cyB, Transform::getIdentity(), localTransformB, 2);
+
+				UDEBUG("timeB = %fs", timeT.ticks());
+
+				if(dataFrom.isValid() && dataFrom.isMetric() && dataTo.isValid() && dataTo.isMetric())
+				{
+					memory.update(dataFrom);
+					UDEBUG("timeUpA = %fs", timeT.ticks());
+					memory.update(dataTo);
+					UDEBUG("timeUpB = %fs", timeT.ticks());
+
+					transform = memory.computeVisualTransform(2, 1, &rejectedMsg, &loopClosureVisualInliers);
+					UDEBUG("timeTransform = %fs", timeT.ticks());
+				}
+				else
+				{
+					// Fallback to normal way (raw data not kept in database...)
+					UWARN("Loop closure: Some images not found in memory for re-extracting "
+						  "features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
+					transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers);
+				}
+			}
+			else
+			{
+				transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers);
+			}
 			if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 			{
 				Transform icpTransform  = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg);
@@ -1451,6 +1520,7 @@ bool Rtabmap::process(const SensorData & data)
 			statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
 			statistics_.addStatistic(Statistics::kLoopReactivateId(), _retrievedId);
 			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
+			statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers);

 			statistics_.addStatistic(Statistics::kLocalLoopOdom_corrected(), scanMatchingSuccess?1:0);
 			statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);