From 0c4fa21ae3a2b3157906b9b73b9ad8c744d15867 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 14 Jan 2019 11:42:28 +0100
Subject: [PATCH] Add automatic addConstrainedBy() in establishConstraints()
 ...

- Rename some feature lists for easier reading
---
 src/constraint_AHP.h                          | 12 +++------
 src/processor_tracker_feature.cpp             | 17 +++++++++---
 src/processor_tracker_feature.h               |  8 +++---
 src/processor_tracker_feature_corner.cpp      | 18 ++++++-------
 src/processor_tracker_feature_corner.h        |  8 +++---
 src/processor_tracker_feature_dummy.cpp       | 22 +++++++--------
 src/processor_tracker_feature_dummy.h         |  8 +++---
 src/processor_tracker_feature_image.cpp       | 14 +++++-----
 src/processor_tracker_feature_image.h         |  4 +--
 src/processor_tracker_landmark.cpp            | 24 +++--------------
 src/processor_tracker_landmark.h              | 27 +++++++------------
 src/processor_tracker_landmark_corner.cpp     |  6 ++---
 src/processor_tracker_landmark_corner.h       |  8 +++---
 src/processor_tracker_landmark_dummy.cpp      | 22 +++++++--------
 src/processor_tracker_landmark_dummy.h        |  8 +++---
 src/processor_tracker_landmark_image.cpp      | 18 ++++++-------
 src/processor_tracker_landmark_image.h        |  8 +++---
 src/processor_tracker_landmark_polyline.h     | 14 +++++-----
 .../processor_tracker_feature_trifocal.cpp    | 18 ++++++-------
 .../processor_tracker_feature_trifocal.h      |  8 +++---
 20 files changed, 128 insertions(+), 144 deletions(-)

diff --git a/src/constraint_AHP.h b/src/constraint_AHP.h
index e7acfc0a3..a97ac9840 100644
--- a/src/constraint_AHP.h
+++ b/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;
 }
 
diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp
index 049e0bf5c..0ce945f30 100644
--- a/src/processor_tracker_feature.cpp
+++ b/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() ,
diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h
index e51bd7312..fc35c5465 100644
--- a/src/processor_tracker_feature.h
+++ b/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
diff --git a/src/processor_tracker_feature_corner.cpp b/src/processor_tracker_feature_corner.cpp
index 5876bdd2b..387333fd7 100644
--- a/src/processor_tracker_feature_corner.cpp
+++ b/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,
diff --git a/src/processor_tracker_feature_corner.h b/src/processor_tracker_feature_corner.h
index 93c4c7add..efc40a145 100644
--- a/src/processor_tracker_feature_corner.h
+++ b/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);
 
diff --git a/src/processor_tracker_feature_dummy.cpp b/src/processor_tracker_feature_dummy.cpp
index b0679cf6a..dc81126f9 100644
--- a/src/processor_tracker_feature_dummy.cpp
+++ b/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,
diff --git a/src/processor_tracker_feature_dummy.h b/src/processor_tracker_feature_dummy.h
index 09ae7df58..3c163d276 100644
--- a/src/processor_tracker_feature_dummy.h
+++ b/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);
 
diff --git a/src/processor_tracker_feature_image.cpp b/src/processor_tracker_feature_image.cpp
index 397a39cf6..1a860c217 100644
--- a/src/processor_tracker_feature_image.cpp
+++ b/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]);
 
diff --git a/src/processor_tracker_feature_image.h b/src/processor_tracker_feature_image.h
index c8eeaa4be..792c5d645 100644
--- a/src/processor_tracker_feature_image.h
+++ b/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);
 
diff --git a/src/processor_tracker_landmark.cpp b/src/processor_tracker_landmark.cpp
index e68101044..8e3f5d0d9 100644
--- a/src/processor_tracker_landmark.cpp
+++ b/src/processor_tracker_landmark.cpp
@@ -116,15 +116,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(),
@@ -132,22 +123,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_;
@@ -160,6 +141,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);
         }
     }
 }
diff --git a/src/processor_tracker_landmark.h b/src/processor_tracker_landmark.h
index 479d0d8ea..bae94066b 100644
--- a/src/processor_tracker_landmark.h
+++ b/src/processor_tracker_landmark.h
@@ -94,31 +94,24 @@ 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
+         * \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,
-                                           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
          *
@@ -147,7 +140,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
          * 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) = 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_
          **/
diff --git a/src/processor_tracker_landmark_corner.cpp b/src/processor_tracker_landmark_corner.cpp
index c69b9964e..e687e5fcd 100644
--- a/src/processor_tracker_landmark_corner.cpp
+++ b/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,
diff --git a/src/processor_tracker_landmark_corner.h b/src/processor_tracker_landmark_corner.h
index 56619fcf1..bb9eff8db 100644
--- a/src/processor_tracker_landmark_corner.h
+++ b/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
          *
diff --git a/src/processor_tracker_landmark_dummy.cpp b/src/processor_tracker_landmark_dummy.cpp
index c4a45acb2..e806c23e5 100644
--- a/src/processor_tracker_landmark_dummy.cpp
+++ b/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)
diff --git a/src/processor_tracker_landmark_dummy.h b/src/processor_tracker_landmark_dummy.h
index 6df2393fd..410b9c72c 100644
--- a/src/processor_tracker_landmark_dummy.h
+++ b/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
          *
diff --git a/src/processor_tracker_landmark_image.cpp b/src/processor_tracker_landmark_image.cpp
index 00d90d7ff..da3f1c7b4 100644
--- a/src/processor_tracker_landmark_image.cpp
+++ b/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]);
 
diff --git a/src/processor_tracker_landmark_image.h b/src/processor_tracker_landmark_image.h
index 8d8b8fd42..6d86249e7 100644
--- a/src/processor_tracker_landmark_image.h
+++ b/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
          *
diff --git a/src/processor_tracker_landmark_polyline.h b/src/processor_tracker_landmark_polyline.h
index 56d7bad9e..3c4c69031 100644
--- a/src/processor_tracker_landmark_polyline.h
+++ b/src/processor_tracker_landmark_polyline.h
@@ -132,11 +132,11 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
         void resetDerived();
 
         /** \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
@@ -158,7 +158,7 @@ class ProcessorTrackerLandmarkPolyline : 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 Creates a landmark for each of new_features_last_
          **/
@@ -213,11 +213,11 @@ inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(Proces
 {
 }
 
-inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
+inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
 {
     // already computed since each scan is computed in preprocess()
-    _feature_list_out = std::move(polylines_last_);
-    return _feature_list_out.size();
+    _features_incoming_out = std::move(polylines_last_);
+    return _features_incoming_out.size();
 }
 
 inline void ProcessorTrackerLandmarkPolyline::advanceDerived()
diff --git a/src/processors/processor_tracker_feature_trifocal.cpp b/src/processors/processor_tracker_feature_trifocal.cpp
index 13ea679d2..7ffb99375 100644
--- a/src/processors/processor_tracker_feature_trifocal.cpp
+++ b/src/processors/processor_tracker_feature_trifocal.cpp
@@ -137,7 +137,7 @@ bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, con
 }
 
 
-unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out)
 {
 //    // DEBUG =====================================
 //    clock_t debug_tStart;
@@ -177,7 +177,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned i
                                 capture_last_->descriptors_.row(index_last),
                                 Eigen::Matrix2s::Identity() * params_tracker_feature_trifocal_->pixel_noise_std * params_tracker_feature_trifocal_->pixel_noise_std);
 
-                        _feature_list_out.push_back(last_point_ptr);
+                        _features_incoming_out.push_back(last_point_ptr);
 
                         // hit cell to acknowledge there's a tracked point in that cell
                         capture_last_->grid_features_->hitTrackingCell(kp_last);
@@ -195,17 +195,17 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned i
             break; // There are no empty cells
     }
 
-    WOLF_TRACE("DetectNewFeatures - Number of new features detected: " , _feature_list_out.size() );
+    WOLF_TRACE("DetectNewFeatures - Number of new features detected: " , _features_incoming_out.size() );
 
 //    // DEBUG
 //    debug_comp_time_ = (double)(clock() - debug_tStart) / CLOCKS_PER_SEC;
 //    WOLF_TRACE("--> TIME: detect new features: TOTAL ",debug_comp_time_);
 //    WOLF_TRACE("======== ========= =========");
 
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
-unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_matches)
+unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_matches)
 {
 //    // DEBUG =====================================
 //    clock_t debug_tStart;
@@ -214,7 +214,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseLis
 //    WOLF_TRACE("======== TrackFeatures =========");
 //    // ===========================================
 
-    for (auto feature_base_last_ : _feature_list_in)
+    for (auto feature_base_last_ : _features_last_in)
     {
         FeaturePointImagePtr feature_last_ = std::static_pointer_cast<FeaturePointImage>(feature_base_last_);
 
@@ -236,7 +236,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseLis
                             capture_incoming_->descriptors_.row(index_kp_incoming),
                             Eigen::Matrix2s::Identity() * params_tracker_feature_trifocal_->pixel_noise_std * params_tracker_feature_trifocal_->pixel_noise_std);
 
-                    _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_last_, capture_incoming_->matches_normalized_scores_.at(index_kp_incoming)}));
 
@@ -252,9 +252,9 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseLis
 //    WOLF_TRACE("--> TIME: track: ",debug_comp_time_);
 //    WOLF_TRACE("======== ========= =========");
 
-    WOLF_TRACE("TrAckFeatures - Number of features tracked: " , _feature_list_out.size() );
+    WOLF_TRACE("TrAckFeatures - Number of features tracked: " , _features_incoming_out.size() );
 
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
 bool ProcessorTrackerFeatureTrifocal::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
diff --git a/src/processors/processor_tracker_feature_trifocal.h b/src/processors/processor_tracker_feature_trifocal.h
index e31ee6f5f..9065c718c 100644
--- a/src/processors/processor_tracker_feature_trifocal.h
+++ b/src/processors/processor_tracker_feature_trifocal.h
@@ -62,11 +62,11 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
         virtual void configure(SensorBasePtr _sensor) override;
 
         /** \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_matches returned map of correspondences: _feature_matches[feature_out_ptr] = feature_in_ptr
          */
-        virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_matches) override;
+        virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_matches) override;
 
         /** \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
@@ -92,7 +92,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
          *
          * 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) override;
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) override;
 
         /** \brief Create a new constraint and link it to the wolf tree
          * \param _feature_ptr pointer to the parent Feature
-- 
GitLab