Added parameter "Mem/UseOdometryFeatures=false" to use directly features already extracted from odometry for the vocabulary (issue #32)

corelib/include/rtabmap/core/Memory.h

@@ -246,6 +246,7 @@ private:
 	float _rehearsalMaxAngle;
 	bool _rehearsalWeightIgnoredWhileMoving;
 	bool _useDepthAsMask;
+	bool _useOdometryFeatures;
 
 	int _idCount;
 	int _idMapCount;

corelib/include/rtabmap/core/Odometry.h

@@ -54,7 +54,7 @@ public:
 
 public:
 	virtual ~Odometry();
-	Transform process(const SensorData & data, OdometryInfo * info = 0);
+	Transform process(SensorData & data, OdometryInfo * info = 0);
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 
 	//getters
@@ -63,7 +63,7 @@ public:
 	const Transform & previousTransform() const {return previousTransform_;}
 
 private:
-	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
+	virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0) = 0;
 
 	void initKalmanFilter();
 	void updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw);

corelib/include/rtabmap/core/OdometryF2F.h

@@ -46,7 +46,7 @@ public:
 	const Signature & getRefFrame() const {return refFrame_;}
 
 private:
-	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
+	virtual Transform computeTransform(SensorData & image, OdometryInfo * info = 0);
 
 private:
 	//Parameters:

corelib/include/rtabmap/core/OdometryF2M.h

@@ -45,7 +45,7 @@ public:
 	const std::multimap<int, cv::Point3f> & getLocalMap() const;
 
 private:
-	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
+	virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0);
 
 private:
 	//Parameters

corelib/include/rtabmap/core/OdometryMono.h

@@ -43,7 +43,7 @@ public:
 	virtual void reset(const Transform & initialPose);
 
 private:
-	virtual Transform computeTransform(const SensorData & data, OdometryInfo * info = 0);
+	virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0);
 private:
 	//Parameters:
 	int flowWinSize_;

corelib/include/rtabmap/core/Parameters.h

@@ -209,6 +209,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, ImageDecimation,         int, 1,          "Image decimation (>=1) when creating a signature.");
 	RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,  "If > 1, downsample the laser scans when creating a signature.");
 	RTABMAP_PARAM(Mem, UseDepthAsMask,          bool, false,   "Use depth image as mask for features detection.");
+	RTABMAP_PARAM(Mem, UseOdomFeatures,         bool, false,   "Use odometry features.");
 
 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM(Kp, NNStrategy,            int, 1,            "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -269,7 +270,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(GFTT, K, double, 0.04, "");
 
 	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, NLevels,              int, 8,       "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.");
 	RTABMAP_PARAM(ORB, FirstLevel,           int, 0,       "It should be 0 in the current implementation.");
 	RTABMAP_PARAM(ORB, WTA_K,                int, 2,       "The number of points that produce each element of the oriented BRIEF descriptor. The default value 2 means the BRIEF where we take a random point pair and compare their brightnesses, so we get 0/1 response. Other possible values are 3 and 4. For example, 3 means that we take 3 random points (of course, those point coordinates are random, but they are generated from the pre-defined seed, so each element of BRIEF descriptor is computed deterministically from the pixel rectangle), find point of maximum brightness and output index of the winner (0, 1 or 2). Such output will occupy 2 bits, and therefore it will need a special variant of Hamming distance, denoted as NORM_HAMMING2 (2 bits per bin). When WTA_K=4, we take 4 random points to compute each bin (that will also occupy 2 bits with possible values 0, 1, 2 or 3).");

corelib/src/Memory.cpp

@@ -90,6 +90,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
 	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
 	_useDepthAsMask(Parameters::defaultMemUseDepthAsMask()),
+	_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -404,6 +405,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
 	Parameters::parse(parameters, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
 	Parameters::parse(parameters, Parameters::kMemUseDepthAsMask(), _useDepthAsMask);
+	Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
 
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -3102,10 +3104,11 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 
 	std::vector<cv::Point3f> keypoints3D;
-	if(data.keypoints().size() == 0)
+	if(!_useOdometryFeatures || data.keypoints().size() == 0)
 	{
 		if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
 		{
+			UINFO("Extract features");
 			cv::Mat imageMono;
 			if(data.imageRaw().channels() == 3)
 			{
@@ -3166,10 +3169,38 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 	else if(!isIntermediateNode)
 	{
+		UINFO("Use odometry features");
 		keypoints = data.keypoints();
 		descriptors = data.descriptors().clone();
 
-		keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
+		UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
+
+		if(keypoints.size() > _feature2D->getMaxFeatures())
+		{
+			_feature2D->limitKeypoints(keypoints, descriptors, _feature2D->getMaxFeatures());
+		}
+
+		if(descriptors.empty())
+		{
+			descriptors = _feature2D->generateDescriptors(data.imageRaw(), keypoints);
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
+			UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
+		}
+
+		UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
+		if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
+		{
+			descriptors = cv::Mat();
+		}
+		else if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
+				(!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
+		{
+			keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
+		}
 	}
 
 	if(_parallelized)

corelib/src/Odometry.cpp

@@ -192,7 +192,7 @@ void Odometry::reset(const Transform & initialPose)
 	}
 }
 
-Transform Odometry::process(const SensorData & data, OdometryInfo * info)
+Transform Odometry::process(SensorData & data, OdometryInfo * info)
 {
 	if(_pose.isNull())
 	{

corelib/src/OdometryF2F.cpp

@@ -59,7 +59,7 @@ void OdometryF2F::reset(const Transform & initialPose)
 
 // return not null transform if odometry is correctly computed
 Transform OdometryF2F::computeTransform(
-		const SensorData & data,
+		SensorData & data,
 		OdometryInfo * info)
 {
 	UTimer timer;
@@ -87,6 +87,8 @@ Transform OdometryF2F::computeTransform(
 				guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
 				&regInfo);
 
+		data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
+
 		if(info && this->isInfoDataFilled())
 		{
 			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
@@ -140,6 +142,8 @@ Transform OdometryF2F::computeTransform(
 						newFrame,
 						dummy);
 				features = (int)newFrame.sensorData().keypoints().size();
+
+				data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
 			}
 
 			if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&

corelib/src/OdometryF2M.cpp

@@ -168,7 +168,7 @@ const std::multimap<int, cv::Point3f> & OdometryF2M::getLocalMap() const
 
 // return not null transform if odometry is correctly computed
 Transform OdometryF2M::computeTransform(
-		const SensorData & data,
+		SensorData & data,
 		OdometryInfo * info)
 {
 	UTimer timer;
@@ -191,6 +191,8 @@ Transform OdometryF2M::computeTransform(
 			Transform guess = this->previousTransform().isIdentity()||this->previousTransform().isNull()?Transform():this->getPose()*this->previousTransform();
 			Transform transform = regVis_->computeTransformationMod(*map_, newSignature, guess, &regInfo);
 
+			data.setFeatures(newSignature.sensorData().keypoints(), newSignature.sensorData().descriptors());
+
 			if(!transform.isNull())
 			{
 				// make it incremental
@@ -271,6 +273,8 @@ Transform OdometryF2M::computeTransform(
 					newSignature,
 					dummy);
 
+			data.setFeatures(newSignature.sensorData().keypoints(), newSignature.sensorData().descriptors());
+
 			if(fixedMapPath_.empty() && (int)newSignature.getWords3().size() >= regVis_->getMinInliers())
 			{
 				output.setIdentity();
@@ -293,6 +297,7 @@ Transform OdometryF2M::computeTransform(
 
 		map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat()); // clear sensorData features
 
+		nFeatures = newSignature.getWords().size();
 		if(this->isInfoDataFilled() && info)
 		{
 			info->words = newSignature.getWords();

corelib/src/OdometryMono.cpp

@@ -170,7 +170,7 @@ void OdometryMono::reset(const Transform & initialPose)
 	keyFramePoses_.clear();
 }
 
-Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * info)
+Transform OdometryMono::computeTransform(SensorData & data, OdometryInfo * info)
 {
 	Transform output;