Merge branch 'feature/int_max_new_features' into 'devel'

int ProcessorParamsTracker::max_new_feature_

Closes #91

See merge request mobile_robotics/wolf_projects/wolf_lib/wolf!265

include/base/processor/processor_tracker.h

@@ -18,7 +18,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTracker);
 struct ProcessorParamsTracker : public ProcessorParamsBase
 {
         unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
-        unsigned int max_new_features;
+        int max_new_features;                   ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0: none.)
 };
 
 WOLF_PTR_TYPEDEFS(ProcessorTracker);
@@ -181,7 +181,7 @@ class ProcessorTracker : public ProcessorBase
         /**\brief Process new Features or Landmarks
          *
          */
-        virtual unsigned int processNew(const unsigned int& _max_features) = 0;
+        virtual unsigned int processNew(const int& _max_features) = 0;
 
         /**\brief Creates and adds factors from last_ to origin_
          *

include/base/processor/processor_tracker_feature.h

@@ -143,17 +143,19 @@ class ProcessorTrackerFeature : public ProcessorTracker
         /**\brief Process new Features
          *
          */
-        virtual unsigned int processNew(const unsigned int& _max_features);
+        virtual unsigned int processNew(const int& _max_features);
 
         /** \brief Detect new Features
-         * \param _max_features maximum number of features detected
+         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+         * \param _features_last_out The list of detected Features.
          * \return The number of detected Features.
          *
          * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * The function sets the member new_features_last_, the list of newly detected features.
+         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out) = 0;
+        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out) = 0;
 
         /** \brief Create a new factor and link it to the wolf tree
          * \param _feature_ptr pointer to the parent Feature

include/base/processor/processor_tracker_feature_corner.h

@@ -106,16 +106,16 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
         virtual bool voteForKeyFrame();
 
         /** \brief Detect new Features
-         * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
-         * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
+         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+         * \param _features_last_out The list of detected Features.
          * \return The number of detected Features.
          *
          * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * The function sets the member new_features_list_, the list of newly detected features,
-         * to be used for landmark initialization.
+         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
 
         virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 

include/base/processor/processor_tracker_feature_dummy.h

@@ -56,16 +56,16 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
         virtual bool voteForKeyFrame();
 
         /** \brief Detect new Features
-         * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
-         * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
+         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+         * \param _features_last_out The list of detected Features.
          * \return The number of detected Features.
          *
          * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * The function sets the member new_features_list_, the list of newly detected features,
-         * to be used for landmark initialization.
+         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
 
         virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 

include/base/processor/processor_tracker_feature_image.h

@@ -112,14 +112,16 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
         virtual bool voteForKeyFrame();
 
         /** \brief Detect new Features
+         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+         * \param _features_last_out The list of detected Features.
+         * \return The number of detected Features.
          *
-         * This is intended to create Features that are not among the Features already known in the Map.
-         *
-         * This function sets new_features_last_, the list of newly detected features.
+         * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * \return The number of detected Features.
+         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
 
         virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 

include/base/processor/processor_tracker_feature_trifocal.h

@@ -85,14 +85,16 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
         virtual bool voteForKeyFrame() override;
 
         /** \brief Detect new Features
-         * \param _max_features maximum number of features detected
+         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+         * \param _features_last_out The list of detected Features.
          * \return The number of detected Features.
          *
          * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * The function sets the member new_features_last_, the list of newly detected features.
+         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out) override;
+        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out) override;
 
         /** \brief Create a new factor and link it to the wolf tree
          * \param _feature_ptr pointer to the parent Feature

include/base/processor/processor_tracker_landmark.h

@@ -128,18 +128,19 @@ class ProcessorTrackerLandmark : public ProcessorTracker
         /** \brief Process new Features
          *
          */
-        unsigned int processNew(const unsigned int& _max_features);
+        unsigned int processNew(const int& _max_features);
 
         /** \brief Detect new Features
-         * \param _max_features The maximum number of features to detect.
+         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+         * \param _features_last_out The list of detected Features.
          * \return The number of detected Features.
          *
          * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * The function sets the member new_features_list_, the list of newly detected features,
-         * to be used for landmark initialization.
+         * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out) = 0;
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out) = 0;
 
         /** \brief Creates a landmark for each of new_features_last_
          **/

include/base/processor/processor_tracker_landmark_corner.h

@@ -135,16 +135,16 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
         virtual bool voteForKeyFrame();
 
         /** \brief Detect new Features
-         * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
-         * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
+         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+         * \param _features_last_out The list of detected Features.
          * \return The number of detected Features.
          *
          * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * The function sets the member new_features_list_, the list of newly detected features,
-         * to be used for landmark initialization.
+         * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
 
         /** \brief Create one landmark
          *

include/base/processor/processor_tracker_landmark_dummy.h

@@ -48,15 +48,16 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
         virtual bool voteForKeyFrame();
 
         /** \brief Detect new Features
-         * \param _max_features maximum number of features to detect.
+         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+         * \param _features_last_out The list of detected Features.
          * \return The number of detected Features.
          *
          * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * The function sets the member new_features_list_, the list of newly detected features,
-         * to be used for landmark initialization.
+         * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
 
         /** \brief Create one landmark
          *

include/base/processor/processor_tracker_landmark_image.h

@@ -128,14 +128,16 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
         virtual bool voteForKeyFrame();
 
         /** \brief Detect new Features
+         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+         * \param _features_last_out The list of detected Features.
+         * \return The number of detected Features.
          *
-         * This is intended to create Features that are not among the Features already known in the Map.
-         *
-         * This function sets new_features_last_, the list of newly detected features.
+         * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * \return The number of detected Features.
+         * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
 
         /** \brief Create one landmark
          *

src/processor/processor_tracker_feature.cpp

@@ -21,7 +21,7 @@ ProcessorTrackerFeature::~ProcessorTrackerFeature()
 {
 }
 
-unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_features)
+unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
 {
     /* Rationale: A keyFrame will be created using the last Capture.
      * First, we work on the last Capture to detect new Features,

src/processor/processor_tracker_feature_corner.cpp

@@ -107,10 +107,13 @@ bool ProcessorTrackerFeatureCorner::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th;
 }
 
-unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
 {
     // in corners_last_ remain all not tracked corners
     _features_incoming_out = std::move(corners_last_);
+    if (_max_features != -1 && _features_incoming_out.size() > _max_features)
+        _features_incoming_out.resize(_max_features);
+
     return _features_incoming_out.size();
 }
 

src/processor/processor_tracker_feature_dummy.cpp

@@ -49,12 +49,19 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
 }
 
-unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
 {
+    unsigned int max_features = _max_features;
+
+    if (max_features == -1)
+    {
+        max_features = 10;
+        WOLF_INFO("max_features unlimited, setting it to " , max_features);
+    }
     WOLF_INFO("Detecting " , _max_features , " new features..." );
 
     // detecting new features
-    for (unsigned int i = 1; i <= _max_features; i++)
+    for (unsigned int i = 0; i < max_features; i++)
     {
         n_feature_++;
         FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE",

src/processor/processor_tracker_feature_image.cpp

@@ -244,7 +244,7 @@ bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _ori
     }
 }
 
-unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
 {
     cv::Rect roi;
     KeyPointVector new_keypoints;
@@ -252,7 +252,7 @@ unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int&
     cv::KeyPointsFilter keypoint_filter;
     unsigned int n_new_features = 0;
 
-    for (unsigned int n_iterations = 0; _max_new_features == 0 || n_iterations < _max_new_features; n_iterations++)
+    for (unsigned int n_iterations = 0; _max_new_features == -1 || n_iterations < _max_new_features; n_iterations++)
     {
 
         if (active_search_ptr_->pickEmptyRoi(roi))

src/processor/processor_tracker_feature_trifocal.cpp

@@ -134,7 +134,7 @@ bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, con
 }
 
 
-unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
 {
 //    // DEBUG =====================================
 //    clock_t debug_tStart;
@@ -143,7 +143,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned i
 //    WOLF_TRACE("======== DetectNewFeatures =========");
 //    // ===========================================
 
-    for (unsigned int n_iterations = 0; n_iterations < _max_new_features; ++n_iterations)
+    for (unsigned int n_iterations = 0; _max_new_features == -1 || n_iterations < _max_new_features; ++n_iterations)
     {
         Eigen::Vector2i cell_last;
         if (capture_last_->grid_features_->pickEmptyTrackingCell(cell_last))

src/processor/processor_tracker_landmark.cpp

@@ -60,7 +60,7 @@ void ProcessorTrackerLandmark::resetDerived()
     //            std::cout << "\t" << match.first->id() << " to " << match.second.landmark_ptr_->id() << std::endl;
 }
 
-unsigned int ProcessorTrackerLandmark::processNew(const unsigned int& _max_features)
+unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features)
 {
     /* Rationale: A keyFrame will be created using the last Capture.
      * First, we work on this Capture to detect new Features,

src/processor/processor_tracker_landmark_corner.cpp

@@ -375,10 +375,13 @@ LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _f
                                               _feature_ptr->getMeasurement()(3));
 }
 
-unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
 {
-    // already computed since each scan is computed in preprocess()
+    // in corners_last_ remain all not tracked corners, already computed in preprocess()
     _features_incoming_out = std::move(corners_last_);
+    if (_max_features != -1 && _features_incoming_out.size() > _max_features)
+        _features_incoming_out.resize(_max_features);
+
     return _features_incoming_out.size();
 }
 

src/processor/processor_tracker_landmark_dummy.cpp

@@ -63,16 +63,25 @@ bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < 4;
 }
 
-unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
 {
     std::cout << "\tProcessorTrackerLandmarkDummy::detectNewFeatures" << std::endl;
 
-    // detecting 5 new features
-    for (unsigned int i = 1; i <= _max_features; i++)
+    unsigned int max_features = _max_features;
+
+    if (max_features == -1)
+    {
+        max_features = 10;
+        WOLF_INFO("max_features unlimited, setting it to " , max_features);
+    }
+
+    // detecting new features
+    for (unsigned int i = 1; i <= max_features; i++)
     {
         n_feature_++;
-        _features_incoming_out.push_back(
-                std::make_shared<FeatureBase>("POINT IMAGE", n_feature_ * Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1)));
+        _features_incoming_out.push_back(std::make_shared<FeatureBase>("POINT IMAGE",
+                                                                       n_feature_ * Eigen::Vector1s::Ones(),
+                                                                       Eigen::MatrixXs::Ones(1, 1)));
         std::cout << "\t\tfeature " << _features_incoming_out.back()->getMeasurement() << " detected!" << std::endl;
     }
     return _features_incoming_out.size();

src/processor/processor_tracker_landmark_image.cpp

@@ -225,7 +225,7 @@ bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
 //    return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe;
 }
 
-unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
 {
     cv::Rect roi;
     KeyPointVector new_keypoints;
@@ -233,7 +233,7 @@ unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int
     cv::KeyPointsFilter keypoint_filter;
     unsigned int n_new_features = 0;
 
-    for (unsigned int n_iterations = 0; n_iterations < _max_features; n_iterations++)
+    for (unsigned int n_iterations = 0; _max_features == -1 || n_iterations < _max_features; n_iterations++)
     {
         if (active_search_ptr_->pickEmptyRoi(roi))
         {