Refactored OdometryBOW class to handle binary descriptors too (removed OdometryBin).

Odometry: added "local map history" option to increase precision
Added new Nearest neighbor options (LSH, brute Force, GPU brute Force)
Added FREAK/ORB features
Increased version to 0.6.5

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1431 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/src/Odometry.cpp

@@ -18,11 +18,14 @@
 #include <rtabmap/core/Features2d.h>
 
 #include <rtabmap/core/Memory.h>
+#include <rtabmap/core/VWDictionary.h>
 #include "rtabmap/core/Signature.h"
 
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/transforms.h>
 
+#include <opencv2/gpu/gpu.hpp>
+
 #if _MSC_VER
 	#define ISFINITE(value) _finite(value)
 #else
@@ -31,31 +34,6 @@
 
 namespace rtabmap {
 
-Odometry::Odometry(
-		float inlierDistance,
-		int maxWords,
-		int minInliers,
-		int iterations,
-		float wordsRatio,
-		float maxDepth,
-		float linearUpdate,
-		float angularUpdate,
-		int resetCoutdown) :
-			_maxFeatures(maxWords),
-			_minInliers(minInliers),
-			_inlierDistance(inlierDistance),
-			_iterations(iterations),
-			_wordsRatio(wordsRatio),
-			_maxDepth(maxDepth),
-			_linearUpdate(linearUpdate),
-			_angularUpdate(angularUpdate),
-			_resetCountdown(resetCoutdown),
-			_pose(Transform::getIdentity()),
-			_resetCurrentCount(0)
-{
-
-}
-
 Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_maxFeatures(Parameters::defaultOdomMaxWords()),
 		_minInliers(Parameters::defaultOdomMinInliers()),
@@ -66,6 +44,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_linearUpdate(Parameters::defaultOdomLinearUpdate()),
 		_angularUpdate(Parameters::defaultOdomAngularUpdate()),
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
+		_localHistory(Parameters::defaultOdomLocalHistory()),
 		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0)
 {
@@ -78,6 +57,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomWordsRatio(), _wordsRatio);
 	Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
 	Parameters::parse(parameters, Parameters::kOdomMaxWords(), _maxFeatures);
+	Parameters::parse(parameters, Parameters::kOdomLocalHistory(), _localHistory);
 }
 
 void Odometry::reset()
@@ -116,57 +96,71 @@ Transform Odometry::process(Image & image, int * quality)
 	}
 	return Transform();
 }
-OdometryBinary::OdometryBinary(
-		float inlierDistance,
-		int maxWords,
-		int minInliers,
-		int iterations,
-		float wordsRatio,
-		float maxDepth,
-		float linearUpdate,
-		float angularUpdate,
-		int resetCoutdown,
-		int briefBytes,
-		int fastThreshold,
-		bool fastNonmaxSuppression,
-		bool bruteForceMatching) :
-	Odometry(inlierDistance, maxWords, minInliers, iterations, wordsRatio, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
- 	_briefBytes(briefBytes),
-	_fastThreshold(fastThreshold),
-	_fastNonmaxSuppression(fastNonmaxSuppression),
-	_bruteForceMatching(bruteForceMatching)
-{
 
-}
-
-OdometryBinary::OdometryBinary(const ParametersMap & parameters) :
+//OdometryBOW
+OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	Odometry(parameters),
- 	_briefBytes(Parameters::defaultOdomBinBriefBytes()),
-	_fastThreshold(Parameters::defaultOdomBinFastThreshold()),
-	_fastNonmaxSuppression(Parameters::defaultOdomBinFastNonmaxSuppression()),
-	_bruteForceMatching(Parameters::defaultOdomBinBruteForceMatching())
+	_memory(0)
 {
-	Parameters::parse(parameters, Parameters::kOdomBinBriefBytes(), _briefBytes);
-	Parameters::parse(parameters, Parameters::kOdomBinFastThreshold(), _fastThreshold);
-	Parameters::parse(parameters, Parameters::kOdomBinFastNonmaxSuppression(), _fastNonmaxSuppression);
-	Parameters::parse(parameters, Parameters::kOdomBinBruteForceMatching(), _bruteForceMatching);
+	ParametersMap customParameters;
+	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(this->getMaxFeatures()))); // hack
+	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
+	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
+	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
+	int nn = Parameters::defaultOdomNearestNeighbor();
+	float nndr = Parameters::defaultOdomNNDR();
+	int odomType = Parameters::defaultOdomType();
+	Parameters::parse(parameters, Parameters::kOdomNearestNeighbor(), nn);
+	Parameters::parse(parameters, Parameters::kOdomNNDR(), nndr);
+	Parameters::parse(parameters, Parameters::kOdomType(), odomType);
+	customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
+	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
+	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(odomType)));
+
+	// add only feature stuff
+	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string group = uSplit(iter->first, '/').front();
+		if(group.compare("SURF") == 0 ||
+			group.compare("SIFT") == 0 ||
+			group.compare("BRIEF") == 0 ||
+			group.compare("FAST") == 0 ||
+			group.compare("ORB") == 0 ||
+			group.compare("FREAK") == 0)
+		{
+			customParameters.insert(*iter);
+		}
+	}
+
+	_memory = new Memory(customParameters);
+	if(!_memory->init("", false, ParametersMap(), false))
+	{
+		UERROR("Error initializing the memory for BOW Odometry.");
+	}
 }
 
-void OdometryBinary::reset()
+OdometryBOW::~OdometryBOW()
+{
+	delete _memory;
+}
+
+
+void OdometryBOW::reset()
 {
 	Odometry::reset();
-	_lastKeypoints.clear();
-	_lastDescriptors = cv::Mat();
-	_lastDepth = cv::Mat();
+	_memory->init("", false, ParametersMap(), false);
+	localMap_.clear();
 }
 
 
 // return not null transform if odometry is correctly computed
-Transform OdometryBinary::computeTransform(Image & image, int * quality)
+Transform OdometryBOW::computeTransform(Image & image, int * quality)
 {
 	UTimer timer;
-	cv::Mat imageMono;
 	Transform output;
+
+	cv::Mat imageMono;
 	// convert to grayscale
 	if(image.image().channels() > 1)
 	{
@@ -177,274 +171,19 @@ Transform OdometryBinary::computeTransform(Image & image, int * quality)
 		imageMono = image.image();
 	}
 
-	cv::FastFeatureDetector detector(_fastThreshold, _fastNonmaxSuppression);
-	std::vector<cv::KeyPoint> newKeypoints;
-	detector.detect(imageMono, newKeypoints);
-
-	limitKeypoints(newKeypoints, this->getMaxFeatures());
-
-	cv::BriefDescriptorExtractor extractor(_briefBytes);
-	cv::Mat newDescriptors;
-	extractor.compute(imageMono, newKeypoints, newDescriptors);
-
-	int inliers = 0;
-	int correspondences = 0;
-
-	if(_lastKeypoints.size())
-	{
-		if(newDescriptors.rows && newDescriptors.rows > (int)(getWordsRatio() * float(_lastKeypoints.size()))) // at least 50% keypoints
-		{
-			cv::Mat results;
-			cv::Mat dists;
-			int k=1; // find the 1 nearest neighbor
-			std::vector<std::vector<cv::DMatch> > matches;
-
-			if(_bruteForceMatching)
-			{
-				cv::BFMatcher matcher(cv::NORM_HAMMING);
-				  matcher.knnMatch(newDescriptors, _lastDescriptors, matches, k);
-			}
-			else
-			{
-				 // Create Flann LSH index
-				cv::flann::Index flannIndex(_lastDescriptors, cv::flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);
-				results = cv::Mat(newDescriptors.rows, k, CV_32SC1);
-				dists = cv::Mat(newDescriptors.rows, k, CV_32FC1);
-
-				// search (nearest neighbor)
-				flannIndex.knnSearch(newDescriptors, results, dists, k, cv::flann::SearchParams() );
-			}
-
-			pcl::PointCloud<pcl::PointXYZ>::Ptr mpts_1(new pcl::PointCloud<pcl::PointXYZ>);
-			pcl::PointCloud<pcl::PointXYZ>::Ptr mpts_2(new pcl::PointCloud<pcl::PointXYZ>);
-			std::vector<int> indexes_1, indexes_2;
-			std::vector<uchar> outlier_mask;
-			// Check if this descriptor matches with those of the objects
-			mpts_1->resize(newDescriptors.rows);
-			mpts_2->resize(newDescriptors.rows);
-			UDEBUG("newDescriptors=%d _lastKeypoints=%d time=%fs", newDescriptors.rows, _lastKeypoints.size(), timer.elapsed());
-			int oi = 0;
-			if(_bruteForceMatching)
-			{
-				for(unsigned int i=0; i<matches.size(); ++i)
-				{
-					pcl::PointXYZ pt1 = util3d::getDepth(image.depth(),
-							int(newKeypoints.at(matches.at(i).at(0).queryIdx).pt.x+0.5f),
-							int(newKeypoints.at(matches.at(i).at(0).queryIdx).pt.y+0.5f),
-							(float)imageMono.cols/2,
-							(float)imageMono.rows/2,
-							1.0f/image.depthConstant(),
-							1.0f/image.depthConstant());
-					if(matches.at(i).at(0).trainIdx >=0)
-					{
-						pcl::PointXYZ pt2 = util3d::getDepth(_lastDepth,
-								int(_lastKeypoints.at(matches.at(i).at(0).trainIdx).pt.x+0.5f),
-								int(_lastKeypoints.at(matches.at(i).at(0).trainIdx).pt.y+0.5f),
-								(float)imageMono.cols/2,
-								(float)imageMono.rows/2,
-								1.0f/image.depthConstant(),
-								1.0f/image.depthConstant());
-
-						if(uIsFinite(pt1.z) && uIsFinite(pt2.z) &&
-						   (this->getMaxDepth() <= 0 || (pt1.z < this->getMaxDepth() && pt2.z < this->getMaxDepth())))
-						{
-							mpts_1->at(oi) = pt1;
-							mpts_2->at(oi) = pt2;
-							++oi;
-						}
-					}
-					else
-					{
-						UWARN("Index = %d for i=%d ?!?", results.at<int>(i,0), i);
-					}
-				}
-			}
-			else
-			{
-				for(int i=0; i<newDescriptors.rows; ++i)
-				{
-					pcl::PointXYZ pt1 = util3d::getDepth(image.depth(),
-							int(newKeypoints.at(i).pt.x+0.5f),
-							int(newKeypoints.at(i).pt.y+0.5f),
-							(float)imageMono.cols/2,
-							(float)imageMono.rows/2,
-							1.0f/image.depthConstant(),
-							1.0f/image.depthConstant());
-					if(results.at<int>(i,0) >=0)
-					{
-						pcl::PointXYZ pt2 = util3d::getDepth(_lastDepth,
-								int(_lastKeypoints.at(results.at<int>(i,0)).pt.x+0.5f),
-								int(_lastKeypoints.at(results.at<int>(i,0)).pt.y+0.5f),
-								(float)imageMono.cols/2,
-								(float)imageMono.rows/2,
-								1.0f/image.depthConstant(),
-								1.0f/image.depthConstant());
-
-						if(uIsFinite(pt1.z) && uIsFinite(pt2.z) &&
-						   (this->getMaxDepth() <= 0 || (pt1.z < this->getMaxDepth() && pt2.z < this->getMaxDepth())))
-						{
-							mpts_1->at(oi) = pt1;
-							mpts_2->at(oi) = pt2;
-							++oi;
-						}
-					}
-					else
-					{
-						UWARN("Index = %d for i=%d ?!?", results.at<int>(i,0), i);
-					}
-				}
-			}
-			mpts_1->resize(oi);
-			mpts_2->resize(oi);
-
-			UDEBUG("Correspondences = %d", oi);
-
-			if(oi >= this->getMinInliers())
-			{
-				mpts_1 = util3d::transformPointCloud(mpts_1, image.localTransform()); // new
-				mpts_2 = util3d::transformPointCloud(mpts_2, image.localTransform()); // previous
-				correspondences = mpts_2->size();
-				Transform t = util3d::transformFromXYZCorrespondences(
-						mpts_1,
-						mpts_2,
-						this->getInlierDistance(),
-						this->getIterations(),
-						&inliers);
-				float x,y,z, roll,pitch,yaw;
-				pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(t), x,y,z, roll,pitch,yaw);
-
-				if(quality)
-				{
-					*quality = inliers;
-				}
-
-				// Large transforms may be erroneous computed transforms, so keep under 1 m
-				if(inliers >= this->getMinInliers())
-				{
-					if(isLargeEnoughTransform(t))
-					{
-						_lastKeypoints = newKeypoints;
-						_lastDescriptors = newDescriptors;
-						_lastDepth = image.depth().clone();
-						output = t;
-					}
-					else
-					{
-						output.setIdentity();
-					}
-				}
-				else
-				{
-					UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
-				}
-			}
-			else
-			{
-				UWARN("Not enough inliers %d < %d", oi, this->getMinInliers());
-			}
-		}
-		else if(newDescriptors.rows)
-		{
-			UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
-					getWordsRatio()*100.0f, newDescriptors.rows, _lastKeypoints.size());
-		}
-		else
-		{
-			UWARN("No feature extracted!");
-		}
-	}
-	else
-	{
-		_lastKeypoints = newKeypoints;
-		_lastDescriptors = newDescriptors;
-		_lastDepth = image.depth().clone();
-		output.setIdentity();
-	}
-
-	UINFO("Odom update time = %fs features=%d inliers=%d/%d",
-			timer.elapsed(),
-			newDescriptors.rows,
-			inliers,
-			correspondences);
-
-	return output;
-}
-
-//OdometryBOW
-
-OdometryBOW::OdometryBOW(
-		int detectorType, // SURF or SIFT
-		float inlierDistance,
-		int maxWords,
-		int minInliers,
-		int iterations,
-		float wordsRatio,
-		float maxDepth,
-		float linearUpdate,
-		float angularUpdate,
-		int resetCoutdown,
-		float surfHessianThreshold,
-		float nndr) : // nearest neighbor distance ratio
-	Odometry(inlierDistance, maxWords, minInliers, iterations, wordsRatio, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
-	_memory(new Memory())
-{
-	ParametersMap customParameters;
-	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxWords)));
-	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(maxDepth)));
-	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(detectorType)));
-	customParameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), uNumber2Str(surfHessianThreshold)));
-	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
-	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
-	if(!_memory->init("", false, customParameters, false))
-	{
-		UERROR("Error initializing the memory for BOW Odometry.");
-	}
-}
-
-OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
-	Odometry(parameters),
-	_memory(new Memory(parameters))
-{
-	ParametersMap customParameters;
-	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(this->getMaxFeatures()))); // hack
-	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
-	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
-	if(!_memory->init("", false, customParameters, false))
-	{
-		UERROR("Error initializing the memory for BOW Odometry.");
-	}
-}
-
-OdometryBOW::~OdometryBOW()
-{
-	UDEBUG("");
-	delete _memory;
-	UDEBUG("");
-}
-
-
-void OdometryBOW::reset()
-{
-	Odometry::reset();
-	_memory->init("", false, ParametersMap(), false);
-}
-
-
-// return not null transform if odometry is correctly computed
-Transform OdometryBOW::computeTransform(Image & image, int * quality)
-{
-	UTimer timer;
-	Transform output;
-
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat descriptors;
-	_memory->extractKeypointsAndDescriptors(image.image(), image.depth(), image.depthConstant(), keypoints, descriptors);
+	_memory->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthConstant(), keypoints, descriptors);
 
-	image.setDescriptors(descriptors);
+	image.setDescriptors(descriptors, _memory->getFeatureType());
 	image.setKeypoints(keypoints);
 
+	if(this->getLocalHistory() && this->getLocalHistory() < descriptors.rows)
+	{
+		UWARN("Local history words size (%d) is smaller than extracted features from the current frame (%d).",
+				this->getLocalHistory(), descriptors.rows);
+	}
+
 	int inliers = 0;
 	int correspondences = 0;
 
@@ -455,22 +194,24 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
 		if(previousSignature && newSignature)
 		{
 			Transform transform;
+			std::set<int> uniqueCorrespondences;
 			if(newSignature->getWords3().size() < (unsigned int)(getWordsRatio() * float(previousSignature->getWords3().size())))
 			{
 				UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
 						getWordsRatio()*100.0f, newSignature->getWords3().size(), previousSignature->getWords3().size());
 			}
-			else if(!previousSignature->getWords3().empty() && !newSignature->getWords3().empty())
+			else if(!localMap_.empty() && !newSignature->getWords3().empty())
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
 				pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
 
 				util3d::findCorrespondences(
-						previousSignature->getWords3(),
+						localMap_,
 						newSignature->getWords3(),
 						*inliers1,
 						*inliers2,
-						this->getMaxDepth());
+						this->getMaxDepth(),
+						&uniqueCorrespondences);
 
 				if((int)inliers1->size() >= this->getMinInliers())
 				{
@@ -493,10 +234,6 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
 						transform.setNull();
 						UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
 					}
-					//else if(!transform.isNull() && true)
-					//{
-					//	transform = _memory->computeIcpTransform(*newSignature, *previousSignature, transform, true);
-					//}
 				}
 				else
 				{
@@ -508,46 +245,121 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
 			{
 				_memory->deleteLocation(newSignature->id());
 			}
-			else if(!isLargeEnoughTransform(transform))
-			{
-				output.setIdentity();
-				_memory->deleteLocation(newSignature->id());
-			}
 			else
 			{
-				output = transform;
-				_memory->deleteLocation(previousSignature->id());
+				if(this->getLocalHistory()<=0)
+				{
+					output = this->getPose().inverse() * transform; // make it incremental
+					if(!isLargeEnoughTransform(transform))
+					{
+						// Transform not large enough, keep the old signature
+						_memory->deleteLocation(newSignature->id());
+					}
+					else
+					{
+						_memory->deleteLocation(previousSignature->id());
+						localMap_.clear();
+						// update local map
+						std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
+						for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
+						{
+							if(newSignature->getWords3().count(*iter) == 1)
+							{
+								const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
+								if(pcl::isFinite(pt) &&
+								   (pt.x != 0 || pt.y != 0 || pt.z != 0) &&
+								   (this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
+								{
+									pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
+									localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2));
+								}
+							}
+						}
+					}
+				}
+				else
+				{
+					output = this->getPose().inverse() * transform; // make it incremental
+
+					if(isLargeEnoughTransform(transform))
+					{
+						// update local map only if transform is large enough
+						std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
+						for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
+						{
+							if(newSignature->getWords3().count(*iter) == 1 &&
+							   uniqueCorrespondences.find(*iter) == uniqueCorrespondences.end())
+							{
+								const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
+								if(pcl::isFinite(pt) &&
+								   (pt.x != 0 || pt.y != 0 || pt.z != 0) &&
+								   (this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
+								{
+									pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
+									localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2));
+								}
+							}
+						}
+						while(localMap_.size() && (int)localMap_.size() > this->getLocalHistory() && _memory->getStMem().size()>1)
+						{
+							std::list<int> deletedWords;
+							_memory->deleteLocation(*_memory->getStMem().begin(), &deletedWords);
+							for(std::list<int>::iterator iter = deletedWords.begin(); iter!=deletedWords.end(); ++iter)
+							{
+								localMap_.erase(*iter);
+							}
+						}
+					}
+					else
+					{
+						_memory->deleteLocation(newSignature->id());
+					}
+				}
 			}
 		}
 		else if(!previousSignature && newSignature)
 		{
+			localMap_.clear();
 			output.setIdentity();
+
+			std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
+			for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
+			{
+				if(newSignature->getWords3().count(*iter) == 1)
+				{
+					const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
+					if(pcl::isFinite(pt) &&
+					   (pt.x != 0 || pt.y != 0 || pt.z != 0) &&
+					   (this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
+					{
+						localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt));
+					}
+				}
+			}
 		}
 
 		_memory->emptyTrash();
 	}
 
-	UINFO("Odom update time = %fs features=%d inliers=%d/%d",
+	UINFO("Odom update time = %fs features=%d inliers=%d/%d dict=%d nodes=%d",
 			timer.elapsed(),
 			descriptors.rows,
 			inliers,
-			correspondences);
+			correspondences,
+			(int)_memory->getVWDictionary()->getVisualWords().size(),
+			(int)_memory->getStMem().size());
 	return output;
 }
 
 // OdometryICP
-OdometryICP::OdometryICP(
-		int decimation,
+OdometryICP::OdometryICP(int decimation,
 		float voxelSize,
-		float samples,
+		int samples,
 		float maxCorrespondenceDistance,
-		int	maxIterations,
+		int maxIterations,
 		float maxFitness,
-		float maxDepth,
-		float linearUpdate,
-		float angularUpdate,
-		int resetCoutdown) :
-	Odometry(0, 0, 0, 0, 0, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
+		const ParametersMap & odometryParameter) :
+	Odometry(odometryParameter),
 	_decimation(decimation),
 	_voxelSize(voxelSize),
 	_samples(samples),
@@ -556,25 +368,6 @@ OdometryICP::OdometryICP(
 	_maxFitness(maxFitness),
 	_previousCloud(new pcl::PointCloud<pcl::PointNormal>)
 {
-
-}
-
-OdometryICP::OdometryICP(const ParametersMap & parameters) :
-	Odometry(parameters),
-	_decimation(Parameters::defaultOdomICPDecimation()),
-	_voxelSize(Parameters::defaultOdomICPVoxelSize()),
-	_samples(Parameters::defaultOdomICPSamples()),
-	_maxCorrespondenceDistance(Parameters::defaultOdomICPCorrespondencesDistance()),
-	_maxIterations(Parameters::defaultOdomICPIterations()),
-	_maxFitness(Parameters::defaultOdomICPMaxFitness()),
-	_previousCloud(new pcl::PointCloud<pcl::PointNormal>)
-{
-	Parameters::parse(parameters, Parameters::kOdomICPDecimation(), _decimation);
-	Parameters::parse(parameters, Parameters::kOdomICPVoxelSize(), _voxelSize);
-	Parameters::parse(parameters, Parameters::kOdomICPSamples(), _samples);
-	Parameters::parse(parameters, Parameters::kOdomICPCorrespondencesDistance(), _maxCorrespondenceDistance);
-	Parameters::parse(parameters, Parameters::kOdomICPIterations(), _maxIterations);
-	Parameters::parse(parameters, Parameters::kOdomICPMaxFitness(), _maxFitness);
 }
 
 void OdometryICP::reset()