Merge branch 'devel' into Apriltag

README.md

@@ -139,7 +139,7 @@ We are shipping the CMAKE file `FindYamlCpp.cmake` together with Wolf. Find it a
 
 #### spdlog
 
-Wolf used spdlog macros. To install it:
+Wolf uses spdlog macros. Right now Wolf is only compatible with spdlog version 0.17.0. To install it:
 
 -   Git clone the source:
 
@@ -148,6 +148,7 @@ Wolf used spdlog macros. To install it:
 -   Build and install with:
 
         $ cd spdlog
+        $ git checkout v0.17.0
         $ mkdir build
         $ cd build
         $ cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" ..

src/ceres_wrapper/ceres_manager.cpp

@@ -351,6 +351,26 @@ void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& sta
         addConstraint(ctr);
 }
 
+bool CeresManager::hasConverged()
+{
+    return summary_.termination_type == ceres::CONVERGENCE;
+}
+
+SizeStd CeresManager::iterations()
+{
+    return summary_.iterations.size();
+}
+
+Scalar CeresManager::initialCost()
+{
+    return Scalar(summary_.initial_cost);
+}
+
+Scalar CeresManager::finalCost()
+{
+    return Scalar(summary_.final_cost);
+}
+
 ceres::CostFunctionPtr CeresManager::createCostFunction(const ConstraintBasePtr& _ctr_ptr)
 {
     assert(_ctr_ptr != nullptr);

src/ceres_wrapper/ceres_manager.h

@@ -53,6 +53,14 @@ class CeresManager : public SolverManager
 
         virtual void computeCovariances(const StateBlockList& st_list) override;
 
+        virtual bool hasConverged();
+
+        virtual SizeStd iterations();
+
+        virtual Scalar initialCost();
+
+        virtual Scalar finalCost();
+
         ceres::Solver::Options& getSolverOptions();
 
         void check();

src/constraint_AHP.h

@@ -185,18 +185,14 @@ inline bool ConstraintAHP::operator ()(const T* const _current_frame_p,
 }
 
 inline ConstraintAHPPtr ConstraintAHP::create(const FeatureBasePtr&   _ftr_ptr,
-                                                    const LandmarkAHPPtr&   _lmk_ahp_ptr,
-                                                    const ProcessorBasePtr& _processor_ptr,
-                                                    bool             _apply_loss_function,
-                                                    ConstraintStatus _status)
+                                              const LandmarkAHPPtr&   _lmk_ahp_ptr,
+                                              const ProcessorBasePtr& _processor_ptr,
+                                              bool             _apply_loss_function,
+                                              ConstraintStatus _status)
 {
     // construct constraint
     ConstraintAHPPtr ctr_ahp = std::make_shared<ConstraintAHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status);
 
-    // link it to wolf tree <-- these pointers cannot be set at construction time
-    _lmk_ahp_ptr->getAnchorFrame()->addConstrainedBy(ctr_ahp);
-    _lmk_ahp_ptr->addConstrainedBy(ctr_ahp);
-
     return ctr_ahp;
 }
 

src/processor_tracker_feature.cpp

@@ -148,9 +148,20 @@ void ProcessorTrackerFeature::establishConstraints()
         FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first;
         FeatureBasePtr feature_in_last   = pair_trkid_pair.second.second;
 
-        auto constraint  = createConstraint(feature_in_last, feature_in_origin);
-        feature_in_last  ->addConstraint(constraint);
-        feature_in_origin->addConstrainedBy(constraint);
+        auto ctr_ptr  = createConstraint(feature_in_last, feature_in_origin);
+        feature_in_last  ->addConstraint(ctr_ptr);
+        feature_in_origin->addConstrainedBy(ctr_ptr);
+
+        if (ctr_ptr != nullptr) // constraint links
+        {
+            FrameBasePtr frm = ctr_ptr->getFrameOtherPtr();
+            if (frm)
+                frm->addConstrainedBy(ctr_ptr);
+            CaptureBasePtr cap = ctr_ptr->getCaptureOtherPtr();
+            if (cap)
+                cap->addConstrainedBy(ctr_ptr);
+        }
+
 
         WOLF_DEBUG( "Constraint: track: " , feature_in_last->trackId(),
                     " origin: "           , feature_in_origin->id() ,

src/processor_tracker_feature.h

@@ -114,11 +114,11 @@ class ProcessorTrackerFeature : public ProcessorTracker
         virtual unsigned int processKnown();
 
         /** \brief Track provided features from \b last to \b incoming
-         * \param _feature_list_in input list of features in \b last to track
-         * \param _feature_list_out returned list of features found in \b incoming
+         * \param _features_last_in input list of features in \b last to track
+         * \param _features_incoming_out returned list of features found in \b incoming
          * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
          */
-        virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_correspondences) = 0;
+        virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) = 0;
 
         /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
          * \param _origin_feature input feature in origin capture tracked
@@ -153,7 +153,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
          *
          * The function sets the member new_features_last_, the list of newly detected features.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out) = 0;
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) = 0;
 
         /** \brief Create a new constraint and link it to the wolf tree
          * \param _feature_ptr pointer to the parent Feature

src/processor_tracker_feature_corner.cpp

@@ -70,14 +70,14 @@ void ProcessorTrackerFeatureCorner::advanceDerived()
     corners_last_ = std::move(corners_incoming_);
 }
 
-unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& _feature_list_in,
-                                                         FeatureBaseList& _feature_list_out,
+unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& _features_last_in,
+                                                         FeatureBaseList& _features_incoming_out,
                                                          FeatureMatchMap& _feature_correspondences)
 {
-    std::cout << "tracking " << _feature_list_in.size() << " features..." << std::endl;
+    std::cout << "tracking " << _features_last_in.size() << " features..." << std::endl;
 
     Eigen::Vector3s expected_feature_pose;
-    for (auto feat_in_ptr : _feature_list_in)
+    for (auto feat_in_ptr : _features_last_in)
     {
         expected_feature_pose = R_current_prev_ * feat_in_ptr->getMeasurement().head<3>() + t_current_prev_;
         
@@ -91,7 +91,7 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList&
                 _feature_correspondences[*feat_out_it] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
                 
                 // move matched feature to list
-                _feature_list_out.splice(_feature_list_out.end(), corners_incoming_, feat_out_it);
+                _features_incoming_out.splice(_features_incoming_out.end(), corners_incoming_, feat_out_it);
                 
                 std::cout << "feature " << feat_in_ptr->id() << " tracked!" << std::endl;
             }
@@ -99,7 +99,7 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList&
         }
     }
 
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
 bool ProcessorTrackerFeatureCorner::voteForKeyFrame()
@@ -107,11 +107,11 @@ bool ProcessorTrackerFeatureCorner::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th;
 }
 
-unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
 {
     // in corners_last_ remain all not tracked corners
-    _feature_list_out = std::move(corners_last_);
-    return _feature_list_out.size();
+    _features_incoming_out = std::move(corners_last_);
+    return _features_incoming_out.size();
 }
 
 ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr _feature_ptr,

src/processor_tracker_feature_corner.h

@@ -83,11 +83,11 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
         void advanceDerived();
 
         /** \brief Track provided features from \b last to \b incoming
-         * \param _feature_list_in input list of features in \b last to track
-         * \param _feature_list_out returned list of features found in \b incoming
+         * \param _features_last_in input list of features in \b last to track
+         * \param _features_incoming_out returned list of features found in \b incoming
          * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
          */
-        virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out,
                                            FeatureMatchMap& _feature_correspondences);
 
         /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -116,7 +116,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
          * The function sets the member new_features_list_, the list of newly detected features,
          * to be used for landmark initialization.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
 
         virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 

src/processor_tracker_feature_dummy.cpp

@@ -13,13 +13,13 @@ namespace wolf
 
 unsigned int ProcessorTrackerFeatureDummy::count_ = 0;
 
-unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& _feature_list_in,
-                                                         FeatureBaseList& _feature_list_out,
+unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& _features_last_in,
+                                                         FeatureBaseList& _features_incoming_out,
                                                          FeatureMatchMap& _feature_correspondences)
 {
-    WOLF_INFO("tracking " , _feature_list_in.size() , " features...");
+    WOLF_INFO("tracking " , _features_last_in.size() , " features...");
 
-    for (auto feat_in : _feature_list_in)
+    for (auto feat_in : _features_last_in)
     {
         if (++count_ % 3 == 2) // lose one every 3 tracks
         {
@@ -28,14 +28,14 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
         else
         {
             FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE", feat_in->getMeasurement(), feat_in->getMeasurementCovariance()));
-            _feature_list_out.push_back(ftr);
-            _feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
+            _features_incoming_out.push_back(ftr);
+            _feature_correspondences[_features_incoming_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
 
             WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
         }
     }
 
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
 bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
@@ -49,7 +49,7 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
 }
 
-unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
 {
     WOLF_INFO("Detecting " , _max_features , " new features..." );
 
@@ -60,14 +60,14 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int&
         FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE",
                                                          n_feature_* Eigen::Vector1s::Ones(),
                                                          Eigen::MatrixXs::Ones(1, 1)));
-        _feature_list_out.push_back(ftr);
+        _features_incoming_out.push_back(ftr);
 
         WOLF_INFO("feature " , ftr->id() , " detected!" );
     }
 
-    WOLF_INFO(_feature_list_out.size() , " features detected!");
+    WOLF_INFO(_features_incoming_out.size() , " features detected!");
 
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
 ConstraintBasePtr ProcessorTrackerFeatureDummy::createConstraint(FeatureBasePtr _feature_ptr,

src/processor_tracker_feature_dummy.h

@@ -32,11 +32,11 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
         unsigned int n_feature_;
 
         /** \brief Track provided features from \b last to \b incoming
-         * \param _feature_list_in input list of features in \b last to track
-         * \param _feature_list_out returned list of features found in \b incoming
+         * \param _features_last_in input list of features in \b last to track
+         * \param _features_incoming_out returned list of features found in \b incoming
          * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
          */
-        virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out,
                                            FeatureMatchMap& _feature_correspondences);
 
         /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -65,7 +65,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
          * The function sets the member new_features_list_, the list of newly detected features,
          * to be used for landmark initialization.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
 
         virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 

src/processor_tracker_feature_image.cpp

@@ -135,14 +135,14 @@ void ProcessorTrackerFeatureImage::postProcess()
 {
 }
 
-unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out,
+unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out,
                                            FeatureMatchMap& _feature_matches)
 {
     KeyPointVector candidate_keypoints;
     cv::Mat candidate_descriptors;
     DMatchVector cv_matches;
 
-    for (auto feature_base_ptr : _feature_list_in)
+    for (auto feature_base_ptr : _features_last_in)
     {
         FeaturePointImagePtr feature_ptr = std::static_pointer_cast<FeaturePointImage>(feature_base_ptr);
 
@@ -167,7 +167,7 @@ unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList&
                         (candidate_descriptors.row(cv_matches[0].trainIdx)),
                         Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
                 incoming_point_ptr->setIsKnown(feature_ptr->isKnown());
-                _feature_list_out.push_back(incoming_point_ptr);
+                _features_incoming_out.push_back(incoming_point_ptr);
 
                 _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_base_ptr, normalized_score}));
             }
@@ -185,8 +185,8 @@ unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList&
             tracker_roi_.pop_back();
         }
     }
-//    std::cout << "TrackFeatures - Number of Features tracked: " << _feature_list_out.size() << std::endl;
-    return _feature_list_out.size();
+//    std::cout << "TrackFeatures - Number of Features tracked: " << _features_incoming_out.size() << std::endl;
+    return _features_incoming_out.size();
 }
 
 bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
@@ -244,7 +244,7 @@ bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _ori
     }
 }
 
-unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out)
 {
     cv::Rect roi;
     KeyPointVector new_keypoints;
@@ -277,7 +277,7 @@ unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int&
                             new_descriptors.row(index),
                             Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
                     point_ptr->setIsKnown(false);
-                    _feature_list_out.push_back(point_ptr);
+                    _features_incoming_out.push_back(point_ptr);
 
                     active_search_ptr_->hitCell(new_keypoints[0]);
 

src/processor_tracker_feature_image.h

@@ -91,7 +91,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
             image_last_ = image_incoming_;
         }
 
-        virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out,
                                            FeatureMatchMap& _feature_correspondences);
 
         /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -119,7 +119,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
          *
          * \return The number of detected Features.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out);
 
         virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 

src/processor_tracker_landmark.cpp

@@ -120,15 +120,6 @@ void ProcessorTrackerLandmark::createNewLandmarks()
 
 unsigned int ProcessorTrackerLandmark::processKnown()
 {
-
-    //std::cout << "ProcessorTrackerLandmark::processKnown:" << std::endl;
-    //std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl;
-    //std::cout << "\tlast features: " << (last_ptr_ == nullptr ? 0 : last_ptr_->getFeatureList().size()) << std::endl;
-    //std::cout << "\tlast new features: " << new_features_last_.size() << std::endl;
-    //std::cout << "\tincoming correspondences: " << matches_landmark_from_incoming_.size() << std::endl;
-    //std::cout << "\tincoming features: " << (incoming_ptr_ == nullptr ? 0 : incoming_ptr_->getFeatureList().size()) << std::endl;
-    //std::cout << "\tincoming new features: " << new_features_incoming_.size() << std::endl;
-
     // Find landmarks in incoming_ptr_
     FeatureBaseList known_features_list_incoming;
     unsigned int n = findLandmarks(getProblem()->getMapPtr()->getLandmarkList(),
@@ -136,22 +127,12 @@ unsigned int ProcessorTrackerLandmark::processKnown()
     // Append found incoming features
     incoming_ptr_->addFeatureList(known_features_list_incoming);
 
-    //std::cout << "end of processKnown:" << std::endl;
-    //std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl;
-    //std::cout << "\tlast features: " << (last_ptr_ == nullptr ? 0 : last_ptr_->getFeatureList().size()) << std::endl;
-    //std::cout << "\tlast new features: " << new_features_last_.size() << std::endl;
-    //std::cout << "\tincoming correspondences: " << matches_landmark_from_incoming_.size() << std::endl;
-    //std::cout << "\tincoming features: " << (incoming_ptr_ == nullptr ? 0 : incoming_ptr_->getFeatureList().size()) << std::endl;
-    //std::cout << "\tincoming new features: " << new_features_incoming_.size() << std::endl;
     return n;
-
 }
 
 void ProcessorTrackerLandmark::establishConstraints()
 {
-
-    //std::cout << "\tfeatures:" << last_ptr_->getFeatureList().size() << std::endl;
-    //std::cout << "\tcorrespondences: " << matches_landmark_from_last_.size() << std::endl;
+    // Loop all features in last_ptr_
     for (auto last_feature : last_ptr_->getFeatureList())
     {
         auto lmk = matches_landmark_from_last_[last_feature]->landmark_ptr_;
@@ -164,6 +145,9 @@ void ProcessorTrackerLandmark::establishConstraints()
             FrameBasePtr frm = ctr_ptr->getFrameOtherPtr();
             if (frm)
                 frm->addConstrainedBy(ctr_ptr);
+            CaptureBasePtr cap = ctr_ptr->getCaptureOtherPtr();
+            if (cap)
+                cap->addConstrainedBy(ctr_ptr);
         }
     }
 }

src/processor_tracker_landmark.h

@@ -94,33 +94,25 @@ class ProcessorTrackerLandmark : public ProcessorTracker
         /** \brief Tracker function
          * \return The number of successful tracks.
          *
+         * Find Features in \b incoming Capture corresponding to known landmarks in the \b Map.
+         *
          * This is the tracker function to be implemented in derived classes.
          * It operates on the \b incoming capture pointed by incoming_ptr_.
          *
-         * This should do one of the following, depending on the design of the tracker:
-         *   - Track Features against other Features in the \b origin Capture. Tips:
-         *     - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
-         *     - Once tracked against last, then the link to Features in \b origin is provided by the Features' Constraints in \b last.
-         *     - If required, correct the drift by re-comparing against the Features in origin.
-         *     - The Constraints in \b last need to be transferred to \b incoming (moved, not copied).
-         *
-         * The function must generate the necessary Features in the \b incoming Capture,
-         * of the correct type, derived from FeatureBase.
-         *
-         * It must also generate the constraints, of the correct type, derived from ConstraintBase
-         * (through ConstraintAnalytic or ConstraintSparse).
+         * The function must populate the list of Features in the \b incoming Capture.
+         * Features need to be of the correct type, derived from FeatureBase.
          */
         virtual unsigned int processKnown();
 
-        /** \brief Find provided landmarks in the incoming Capture
-         * \param _landmark_list_in input list of landmarks to be found in incoming
-         * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in
+        /** \brief Find provided landmarks in the incoming capture
+         * \param _landmarks_in input list of landmarks to be found in incoming
+         * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
          * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
          * \return the number of landmarks found
          */
-        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in,
-                                           FeatureBaseList& _feature_list_out,
-                                           LandmarkMatchMap& _feature_landmark_correspondences) = 0;
+        virtual unsigned int findLandmarks(const LandmarkBaseList&  _landmarks_in,
+                                           FeatureBaseList&         _features_incoming_out,
+                                           LandmarkMatchMap&        _feature_landmark_correspondences) = 0;
 
         /** \brief Vote for KeyFrame generation
          *
@@ -149,7 +141,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
          * The function sets the member new_features_list_, the list of newly detected features
          * in last_ptr_ to be used for landmark initialization.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) = 0;
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) = 0;
 
         /** \brief Creates a landmark for each of new_features_last_
          **/

src/processor_tracker_landmark_corner.cpp

@@ -375,11 +375,11 @@ LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _f
                                               _feature_ptr->getMeasurement()(3));
 }
 
-unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
 {
     // already computed since each scan is computed in preprocess()
-    _feature_list_out = std::move(corners_last_);
-    return _feature_list_out.size();
+    _features_incoming_out = std::move(corners_last_);
+    return _features_incoming_out.size();
 }
 
 ConstraintBasePtr ProcessorTrackerLandmarkCorner::createConstraint(FeatureBasePtr _feature_ptr,

src/processor_tracker_landmark_corner.h

@@ -120,11 +120,11 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
         }
 
         /** \brief Find provided landmarks in the incoming capture
-         * \param _landmark_list_in input list of landmarks to be found in incoming
-         * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in
+         * \param _landmarks_in input list of landmarks to be found in incoming
+         * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
          * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
          */
-        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out,
                                            LandmarkMatchMap& _feature_landmark_correspondences);
 
         /** \brief Vote for KeyFrame generation
@@ -146,7 +146,7 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
          * The function sets the member new_features_list_, the list of newly detected features,
          * to be used for landmark initialization.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
 
         /** \brief Create one landmark
          *

src/processor_tracker_landmark_dummy.cpp

@@ -26,16 +26,16 @@ ProcessorTrackerLandmarkDummy::~ProcessorTrackerLandmarkDummy()
     //
 }
 
-unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList& _landmark_list_in,
-                                                          FeatureBaseList& _feature_list_out,
+unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList& _landmarks_in,
+                                                          FeatureBaseList& _features_incoming_out,
                                                           LandmarkMatchMap& _feature_landmark_correspondences)
 {
     std::cout << "\tProcessorTrackerLandmarkDummy::findLandmarks"  << std::endl;
-    std::cout << "\t\t"  << _landmark_list_in.size() << " landmarks..." << std::endl;
+    std::cout << "\t\t"  << _landmarks_in.size() << " landmarks..." << std::endl;
 
     // loosing the track of the first 2 features
     auto landmarks_lost = 0;
-    for (auto landmark_in_ptr : _landmark_list_in)
+    for (auto landmark_in_ptr : _landmarks_in)
     {
         if (landmark_in_ptr->getDescriptor(0) <= landmark_idx_non_visible_)
         {
@@ -44,15 +44,15 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList
         }
         else
         {
-            _feature_list_out.push_back(std::make_shared<FeatureBase>(
+            _features_incoming_out.push_back(std::make_shared<FeatureBase>(
                     "POINT IMAGE",
                     landmark_in_ptr->getDescriptor(),
                     Eigen::MatrixXs::Identity(1,1)));
-            _feature_landmark_correspondences[_feature_list_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
+            _feature_landmark_correspondences[_features_incoming_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
             std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " found!" << std::endl;
         }
     }
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
 bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
@@ -63,7 +63,7 @@ bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < 4;
 }
 
-unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
 {
     std::cout << "\tProcessorTrackerLandmarkDummy::detectNewFeatures" << std::endl;
 
@@ -71,11 +71,11 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int
     for (unsigned int i = 1; i <= _max_features; i++)
     {
         n_feature_++;
-        _feature_list_out.push_back(
+        _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 " << _feature_list_out.back()->getMeasurement() << " detected!" << std::endl;
+        std::cout << "\t\tfeature " << _features_incoming_out.back()->getMeasurement() << " detected!" << std::endl;
     }
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
 LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _feature_ptr)

src/processor_tracker_landmark_dummy.h

@@ -31,11 +31,11 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
         virtual void postProcess(); // implemented
 
         /** \brief Find provided landmarks in the incoming capture
-         * \param _landmark_list_in input list of landmarks to be found in incoming
-         * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in
+         * \param _landmarks_in input list of landmarks to be found in incoming
+         * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
          * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
          */
-        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out,
                                            LandmarkMatchMap& _feature_landmark_correspondences);
 
         /** \brief Vote for KeyFrame generation
@@ -56,7 +56,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
          * The function sets the member new_features_list_, the list of newly detected features,
          * to be used for landmark initialization.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
 
         /** \brief Create one landmark
          *

src/processor_tracker_landmark_image.cpp

@@ -144,8 +144,8 @@ void ProcessorTrackerLandmarkImage::postProcess()
     feat_lmk_found_.clear();
 }
 
-unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList& _landmark_list_in,
-                                                         FeatureBaseList&  _feature_list_out,
+unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList& _landmarks_in,
+                                                         FeatureBaseList&  _features_incoming_out,
                                                          LandmarkMatchMap& _feature_landmark_correspondences)
 {
     KeyPointVector candidate_keypoints;
@@ -154,7 +154,7 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList
 
     Eigen::VectorXs current_state = getProblem()->getState(incoming_ptr_->getTimeStamp());
 
-    for (auto landmark_in_ptr : _landmark_list_in)
+    for (auto landmark_in_ptr : _landmarks_in)
     {
 
         // project landmark into incoming capture
@@ -194,7 +194,7 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList
                     incoming_point_ptr->setScore(normalized_score);
                     incoming_point_ptr->setExpectation(pixel);
 
-                    _feature_list_out.push_back(incoming_point_ptr);
+                    _features_incoming_out.push_back(incoming_point_ptr);
 
 
                     _feature_landmark_correspondences[incoming_point_ptr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, normalized_score);
@@ -215,9 +215,9 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList
 //        else
 //            std::cout << "NOT in the image" << std::endl;
     }
-//    std::cout << "\tNumber of Features tracked: " << _feature_list_out.size() << std::endl;
-    landmarks_tracked_ = _feature_list_out.size();
-    return _feature_list_out.size();
+//    std::cout << "\tNumber of Features tracked: " << _features_incoming_out.size() << std::endl;
+    landmarks_tracked_ = _features_incoming_out.size();
+    return _features_incoming_out.size();
 }
 
 bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
@@ -226,7 +226,7 @@ bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
 //    return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe;
 }
 
-unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
 {
     cv::Rect roi;
     KeyPointVector new_keypoints;
@@ -261,7 +261,7 @@ unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int
                     point_ptr->setIsKnown(false);
                     point_ptr->setTrackId(point_ptr->id());
                     point_ptr->setExpectation(Eigen::Vector2s(new_keypoints[0].pt.x,new_keypoints[0].pt.y));
-                    _feature_list_out.push_back(point_ptr);
+                    _features_incoming_out.push_back(point_ptr);
 
                     active_search_ptr_->hitCell(new_keypoints[0]);
 

src/processor_tracker_landmark_image.h

@@ -112,11 +112,11 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
 
         //Pure virtual
         /** \brief Find provided landmarks in the incoming capture
-         * \param _landmark_list_in input list of landmarks to be found in incoming
-         * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in
+         * \param _landmarks_in input list of landmarks to be found in incoming
+         * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
          * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
          */
-        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out,
                                            LandmarkMatchMap& _feature_landmark_correspondences);
 
         /** \brief Vote for KeyFrame generation
@@ -136,7 +136,7 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
          *
          * \return The number of detected Features.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out);
 
         /** \brief Create one landmark
          *