Fixed maximum features ignored (always set to 400)

corelib/src/DBReader.cpp

@@ -102,10 +102,10 @@ bool DBReader::init(int startIndex)
 	_currentId = _ids.begin();
 	if(startIndex>0 && _ids.size())
 	{
-		std::set<int>::iterator iter = _ids.lower_bound(startIndex);
+		std::set<int>::iterator iter = uIteratorAt(_ids, startIndex);
 		if(iter == _ids.end())
 		{
-			UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", startIndex, *_ids.rbegin());
+			UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", startIndex, _ids.size()-1);
 		}
 		else
 		{

corelib/src/Features2d.cpp

@@ -322,6 +322,7 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
 Feature2D::Feature2D(const ParametersMap & parameters) :
 		maxFeatures_(Parameters::defaultKpWordsPerImage())
 {
+	this->parseParameters(parameters);
 }
 void Feature2D::parseParameters(const ParametersMap & parameters)
 {
@@ -454,6 +455,8 @@ SURF::~SURF()
 
 void SURF::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kSURFExtended(), extended_);
 	Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), hessianThreshold_);
 	Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), nOctaveLayers_);
@@ -574,6 +577,8 @@ SIFT::~SIFT()
 
 void SIFT::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), contrastThreshold_);
 	Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold_);
 	Parameters::parse(parameters, Parameters::kSIFTNFeatures(), nfeatures_);
@@ -653,6 +658,8 @@ ORB::~ORB()
 
 void ORB::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), nFeatures_);
 	Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
 	Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
@@ -771,6 +778,8 @@ FAST::~FAST()
 
 void FAST::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kFASTThreshold(), threshold_);
 	Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppression_);
 	Parameters::parse(parameters, Parameters::kFASTGpu(), gpu_);
@@ -931,6 +940,8 @@ GFTT::~GFTT()
 
 void GFTT::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), _maxCorners);
 	Parameters::parse(parameters, Parameters::kGFTTQualityLevel(), _qualityLevel);
 	Parameters::parse(parameters, Parameters::kGFTTMinDistance(), _minDistance);
@@ -1065,6 +1076,8 @@ BRISK::~BRISK()
 
 void BRISK::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kBRISKThresh(), thresh_);
 	Parameters::parse(parameters, Parameters::kBRISKOctaves(), octaves_);
 	Parameters::parse(parameters, Parameters::kBRISKPatternScale(), patternScale_);

utilite/include/rtabmap/utilite/UStl.h

@@ -308,6 +308,42 @@ inline typename std::list<V>::const_iterator uIteratorAt(const std::list<V> & li
 	return iter;
 }
 
+/**
+ * Get the iterator at a specified position in a std::set. If the position
+ * is out of range, the result is the end iterator of the set.
+ * @param set the set
+ * @param pos the index position in the set
+ * @return the iterator at the specified index
+ */
+template<class V>
+inline typename std::set<V>::iterator uIteratorAt(std::set<V> & set, const unsigned int & pos)
+{
+	typename std::set<V>::iterator iter = set.begin();
+	for(unsigned int i = 0; i<pos && iter != set.end(); ++i )
+	{
+		++iter;
+	}
+	return iter;
+}
+
+/**
+ * Get the iterator at a specified position in a std::set. If the position
+ * is out of range, the result is the end iterator of the set.
+ * @param set the set
+ * @param pos the index position in the set
+ * @return the iterator at the specified index
+ */
+template<class V>
+inline typename std::set<V>::const_iterator uIteratorAt(const std::set<V> & set, const unsigned int & pos)
+{
+	typename std::set<V>::const_iterator iter = set.begin();
+	for(unsigned int i = 0; i<pos && iter != set.end(); ++i )
+	{
+		++iter;
+	}
+	return iter;
+}
+
 /**
  * Get the iterator at a specified position in a std::vector. If the position
  * is out of range, the result is the end iterator of the vector.
@@ -321,6 +357,19 @@ inline typename std::vector<V>::iterator uIteratorAt(std::vector<V> & v, const u
 	return v.begin() + pos;
 }
 
+/**
+ * Get the iterator at a specified position in a std::vector. If the position
+ * is out of range, the result is the end iterator of the vector.
+ * @param v the vector
+ * @param pos the index position in the vector
+ * @return the iterator at the specified index
+ */
+template<class V>
+inline typename std::vector<V>::const_iterator uIteratorAt(const std::vector<V> & v, const unsigned int & pos)
+{
+	return v.begin() + pos;
+}
+
 /**
  * Get the value at a specified position in a std::list. If the position
  * is out of range, the result is undefined.