From f9ff78626e693c4189623f6e0e448010264f58ed Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 13 Jun 2019 11:49:33 +0200
Subject: [PATCH 1/9] Compiling

---
 include/laser/capture/capture_laser_2D.h      |   2 +-
 .../processor_tracker_feature_polyline_2D.h   |  82 +++++++---
 src/landmark/landmark_polyline_2D.cpp         | 148 ++++++++----------
 .../processor_tracker_feature_polyline_2D.cpp | 120 +++++++-------
 4 files changed, 194 insertions(+), 158 deletions(-)

diff --git a/include/laser/capture/capture_laser_2D.h b/include/laser/capture/capture_laser_2D.h
index 03faa523d..4a439dae6 100644
--- a/include/laser/capture/capture_laser_2D.h
+++ b/include/laser/capture/capture_laser_2D.h
@@ -8,8 +8,8 @@ class SensorLaser2D;
 }
 
 //wolf includes
-#include "core/capture/capture_base.h"
 #include "laser/sensor/sensor_laser_2D.h"
+#include "core/capture/capture_base.h"
 
 //laserscanutils includes
 #include "laser_scan_utils/laser_scan.h"
diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h
index 802634edd..5a9a360da 100644
--- a/include/laser/processor/processor_tracker_feature_polyline_2D.h
+++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h
@@ -75,13 +75,18 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
         virtual void configure(SensorBasePtr _sensor) override {};
 
     protected:
-        /** \brief Track provided features from \b last to \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
+
+        /** \brief Track provided features in \b _capture
+         * \param _features_in input list of features in \b last to track
+         * \param _capture the capture in which the _features_in should be searched
+         * \param _features_out returned list of features found in \b _capture
          * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+         *
+         * \return the number of features tracked
          */
-        virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in,
-                                           FeatureBasePtrList& _features_incoming_out,
+        virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+                                           const CaptureBasePtr& _capture,
+                                           FeatureBasePtrList& _features_out,
                                            FeatureMatchMap& _feature_correspondences) override;
 
         /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -109,48 +114,70 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
 
         /** \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.
+         * \param _capture The capture in which the new features should be detected.
+         * \param _features_out The list of detected Features in _capture.
          * \return The number of detected Features.
          *
          * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+         * The function is called in processNew() to set the member new_features_last_,
          * the list of newly detected features of the capture last_ptr_.
          */
         virtual unsigned int detectNewFeatures(const int& _max_new_features,
-                                               FeatureBasePtrList& _features_last_out) override;
+                                               const CaptureBasePtr& _capture,
+                                               FeatureBasePtrList& _features_out) override;
 
-        /** \brief Create a new factor and link it to the wolf tree
+        /** \brief Emplaces a new factor
          * \param _feature_ptr pointer to the parent Feature
          * \param _feature_other_ptr pointer to the other feature constrained.
          *
          * Implement this method in derived classes.
          *
-         * This function creates a factor of the appropriate type for the derived processor.
+         * This function emplaces a factor of the appropriate type for the derived processor.
          */
-        virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr,
-                                                   FeatureBasePtr _feature_other_ptr) override { return nullptr; };
+        virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override { return nullptr; };
 
         /** \brief Establish factors between features in Captures \b last and \b origin
          */
-        virtual void establishFactors()  override;
-
+        virtual void establishFactors() override;
+
+        /** \brief Emplaces a new point to line factor
+         * \param _polyline_feature pointer to the feature
+         * \param _polyline_landmark pointer to the landmark
+         * \param _ftr_point_id index of the feature point
+         * \param _lmk_point_id intex of the landmark point
+         * \param _lmk_prev_point_id index of the landmark previous point
+         */
         void emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
-                                          LandmarkPolyline2DPtr _polyline_landmark,
-                                          const int& _ftr_point_id,
-                                          const int& _lmk_point_id,
-                                          const int& _lmk_prev_point_id);
-
+                                      LandmarkPolyline2DPtr _polyline_landmark,
+                                      const int& _ftr_point_id,
+                                      const int& _lmk_point_id,
+                                      const int& _lmk_prev_point_id);
+
+        /** \brief Emplaces a new point to point factor
+         * \param _polyline_feature pointer to the feature
+         * \param _polyline_landmark pointer to the landmark
+         * \param _ftr_point_id index of the feature point
+         * \param _lmk_point_id intex of the landmark point
+         */
         void emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
-                                    LandmarkPolyline2DPtr _polyline_landmark,
-                                    const int& _ftr_point_id,
-                                    const int& _lmk_point_id);
+                                LandmarkPolyline2DPtr _polyline_landmark,
+                                const int& _ftr_point_id,
+                                const int& _lmk_point_id);
 
-        /** \brief create a landmark from a feature
+        /** \brief emplace a landmark corresponding to a new feature
          *
          */
-        virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
+        virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
 
+        /** \brief Modify a landmark and a landmark match according to a matched feature
+         * \param lmk_match landmark match
+         * \param pl_ftr feature
+         *
+         * Grow and/or define the extremes the landmark, according to the feature.
+         *
+         * \return if any modification was performed
+         */
         bool modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr);
 
         /** \brief advance pointers
@@ -187,6 +214,13 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
 
         void computeTransformations();
 
+        /** \brief Concatenate feature_incoming-feature_last match with feature_last-landmark match
+         * \param pl_incoming polyline feature
+         * \param ftr_match_incoming_last match feature_incoming-feature_last
+         * \param lmk_match_last match feature_last-landmark
+         *
+         * \return the concatenated match: feature_incoming-landmark
+         */
         LandmarkMatchPolyline2DPtr concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming,
                                                                      FeatureMatchPolyline2DPtr ftr_match_incoming_last,
                                                                      LandmarkMatchPolyline2DPtr lmk_match_last) const;
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index ab06878f1..05569121e 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -138,7 +138,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
         point_state_ptr_map_[++last_id_] = new_sb_ptr;
 
         if (getProblem())
-        	getProblem()->addStateBlock(new_sb_ptr);
+        	getProblem()->notifyStateBlock(new_sb_ptr,ADD);
 
         last_defined_ = _defined;
     }
@@ -152,7 +152,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
         point_state_ptr_map_[--first_id_] = new_sb_ptr;
 
         if (getProblem())
-        	getProblem()->addStateBlock(new_sb_ptr);
+        	getProblem()->notifyStateBlock(new_sb_ptr,ADD);
 
         first_defined_ = _defined;
     }
@@ -190,7 +190,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
             point_state_ptr_map_[++last_id_] = new_sb_ptr;
 
         	if (getProblem())
-        		getProblem()->addStateBlock(new_sb_ptr);
+        		getProblem()->notifyStateBlock(new_sb_ptr,ADD);
         }
         last_defined_ = _defined;
     }
@@ -206,7 +206,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
             point_state_ptr_map_[--first_id_] = new_sb_ptr;
 
             if (getProblem())
-        		getProblem()->addStateBlock(new_sb_ptr);
+        		getProblem()->notifyStateBlock(new_sb_ptr,ADD);
         }
 		first_defined_ = _defined;
     }
@@ -447,23 +447,26 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
     // Change factors from remove_state to remain_state
     FactorBasePtrList old_factors_list = getConstrainedByList();
     //std::cout << "changing " << old_factors_list.size() << " factors." << std::endl;
-    FactorBasePtr new_fac_ptr = nullptr;
     for (auto fac_ptr : old_factors_list)
     {
-        FactorPoint2DPtr fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr);
-        FactorPointToLine2DPtr fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
+        auto fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr);
+        auto fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
         if (fac_point_ptr)
         {
             // If landmark point constrained -> new factor
             if (fac_point_ptr->getLandmarkPointId() == _remove_id)
             {
-                new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
-                                                              std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
-                                                              fac_point_ptr->getProcessor(),
-                                                              fac_point_ptr->getFeaturePointId(),
-                                                              _remain_id,
-                                                              fac_point_ptr->getApplyLossFunction(),
-                                                              fac_point_ptr->getStatus());
+                // emplace new factor
+                FactorBase::emplace<FactorPoint2D>(fac_ptr->getFeature(),
+                                                   std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
+                                                   std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
+                                                   fac_point_ptr->getProcessor(),
+                                                   fac_point_ptr->getFeaturePointId(),
+                                                   _remain_id,
+                                                   fac_point_ptr->getApplyLossFunction(),
+                                                   fac_point_ptr->getStatus());
+                // remove old factor
+                fac_ptr->remove();
             }
         }
         else if (fac_point_line_ptr)
@@ -471,26 +474,34 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
             // If landmark point constrained -> new factor
             if (fac_point_line_ptr->getLandmarkPointId() == _remove_id)
             {
-                new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
-                                                                    std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
-                                                                    fac_point_line_ptr->getProcessor(),
-                                                                    fac_point_line_ptr->getFeaturePointId(),
-                                                                    _remain_id,
-                                                                    fac_point_line_ptr->getLandmarkPointAuxId(),
-                                                                    fac_point_line_ptr->getApplyLossFunction(),
-                                                                    fac_point_line_ptr->getStatus());
+                // emplace new factor
+                FactorBase::emplace<FactorPointToLine2D>(fac_ptr->getFeature(),
+                                                         std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
+                                                         std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
+                                                         fac_point_line_ptr->getProcessor(),
+                                                         fac_point_line_ptr->getFeaturePointId(),
+                                                         _remain_id,
+                                                         fac_point_line_ptr->getLandmarkPointAuxId(),
+                                                         fac_point_line_ptr->getApplyLossFunction(),
+                                                         fac_point_line_ptr->getStatus());
+                // remove old factor
+                fac_ptr->remove();
             }
             // If landmark point is aux point -> new factor
             else if (fac_point_line_ptr->getLandmarkPointAuxId() == _remove_id)
             {
-                new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
-                                                                    std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
-                                                                    fac_point_line_ptr->getProcessor(),
-                                                                    fac_point_line_ptr->getFeaturePointId(),
-                                                                    fac_point_line_ptr->getLandmarkPointId(),
-                                                                    _remain_id,
-                                                                    fac_point_line_ptr->getApplyLossFunction(),
-                                                                    fac_point_line_ptr->getStatus());
+                // emplace new factor
+                FactorBase::emplace<FactorPointToLine2D>(fac_ptr->getFeature(),
+                                                         std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
+                                                         std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
+                                                         fac_point_line_ptr->getProcessor(),
+                                                         fac_point_line_ptr->getFeaturePointId(),
+                                                         fac_point_line_ptr->getLandmarkPointId(),
+                                                         _remain_id,
+                                                         fac_point_line_ptr->getApplyLossFunction(),
+                                                         fac_point_line_ptr->getStatus());
+                // remove old factor
+                fac_ptr->remove();
             }
         }
         else
@@ -498,22 +509,6 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
             WOLF_ERROR("polyline factor of unknown type");
             throw std::runtime_error ("polyline factor of unknown type");
         }
-
-        // If new factor
-        if (new_fac_ptr)
-        {
-            //std::cout << "created new factor: " << new_fac_ptr->id() << std::endl;
-            //std::cout << "deleting factor: " << fac_ptr->id() << std::endl;
-
-            // add new factor
-            fac_ptr->getFeature()->addFactor(new_fac_ptr);
-            addConstrainedBy(new_fac_ptr);
-
-            // remove factor
-            fac_ptr->remove();
-
-            new_fac_ptr = nullptr;
-        }
     }
 
     // If removed was a not defined extreme, set defined extreme
@@ -524,7 +519,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
 
     // Remove remove_state
     if (getProblem() != nullptr)
-        getProblem()->removeStateBlock(remove_state);
+        getProblem()->notifyStateBlock(remove_state, REMOVE);
     //std::cout << "state removed " << std::endl;
 
     // remove element from deque
@@ -821,14 +816,13 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
     // ------------------------- COPY FACTORS
     FactorBasePtrList old_factors_list = _merged_lmk->getConstrainedByList();
     //std::cout << "\tchanging " << old_factors_list.size() << " factors." << std::endl;
-    FactorBasePtr new_fac_ptr = nullptr;
     for (auto fac_ptr : old_factors_list)
     {
         //std::cout << "\t\tfactor " << fac_ptr->id() << " to landmark " << fac_ptr->getLandmarkOther()->id() << std::endl;
         assert(fac_ptr->getLandmarkOther() == _merged_lmk);
 
-        FactorPoint2DPtr fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr);
-        FactorPointToLine2DPtr fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
+        auto fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr);
+        auto fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
 
         // point 2 point
         if (fac_point_ptr)
@@ -836,13 +830,17 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
             //std::cout << "\t\tpoint-point factor to id: " << fac_point_ptr->getLandmarkPointId() << std::endl;
             assert(merged_id_to_id.find(fac_point_ptr->getLandmarkPointId()) != merged_id_to_id.end());
 
-            new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
-                                                          std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
-                                                          fac_point_ptr->getProcessor(),
-                                                          fac_point_ptr->getFeaturePointId(),
-                                                          merged_id_to_id[fac_point_ptr->getLandmarkPointId()],
-                                                          fac_point_ptr->getApplyLossFunction(),
-                                                          fac_point_ptr->getStatus());
+            // emplace new factor
+            FactorBase::emplace<FactorPoint2D>(fac_ptr->getFeature(),
+                                               std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
+                                               std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
+                                               fac_point_ptr->getProcessor(),
+                                               fac_point_ptr->getFeaturePointId(),
+                                               merged_id_to_id[fac_point_ptr->getLandmarkPointId()],
+                                               fac_point_ptr->getApplyLossFunction(),
+                                               fac_point_ptr->getStatus());
+            // remove old factor
+            fac_ptr->remove();
 
         }
         // point 2 line
@@ -852,14 +850,18 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
             assert(merged_id_to_id.find(fac_point_line_ptr->getLandmarkPointId()) != merged_id_to_id.end());
             assert(merged_id_to_id.find(fac_point_line_ptr->getLandmarkPointAuxId()) != merged_id_to_id.end());
 
-            new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
-                                                                std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
-                                                                fac_point_line_ptr->getProcessor(),
-                                                                fac_point_line_ptr->getFeaturePointId(),
-                                                                merged_id_to_id[fac_point_line_ptr->getLandmarkPointId()],
-                                                                merged_id_to_id[fac_point_line_ptr->getLandmarkPointAuxId()],
-                                                                fac_point_line_ptr->getApplyLossFunction(),
-                                                                fac_point_line_ptr->getStatus());
+            // emplace new factor
+            FactorBase::emplace<FactorPointToLine2D>(fac_ptr->getFeature(),
+                                                     std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
+                                                     std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
+                                                     fac_point_line_ptr->getProcessor(),
+                                                     fac_point_line_ptr->getFeaturePointId(),
+                                                     merged_id_to_id[fac_point_line_ptr->getLandmarkPointId()],
+                                                     merged_id_to_id[fac_point_line_ptr->getLandmarkPointAuxId()],
+                                                     fac_point_line_ptr->getApplyLossFunction(),
+                                                     fac_point_line_ptr->getStatus());
+            // remove old factor
+            fac_ptr->remove();
 
         }
         else
@@ -867,18 +869,6 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
             WOLF_ERROR("polyline factor of unknown type");
             throw std::runtime_error ("polyline factor of unknown type");
         }
-        // Establish new factor
-        assert(new_fac_ptr != nullptr);
-        //std::cout << "\t\tcreated new factor: " << new_fac_ptr->id() << std::endl;
-        //std::cout << "\t\tdeleting factor: " << fac_ptr->id() << std::endl;
-
-        // add new factor
-        fac_ptr->getFeature()->addFactor(new_fac_ptr);
-        addConstrainedBy(new_fac_ptr);
-
-        // remove factor
-        fac_ptr->remove();
-        //std::cout << "\t\tdeleted\n";
     }
 
     // ------------------------- REMOVE _merged_lmk
@@ -892,7 +882,7 @@ void LandmarkPolyline2D::registerNewStateBlocks()
     LandmarkBase::registerNewStateBlocks();
 	if (getProblem())
 		for (auto state_it : point_state_ptr_map_)
-			getProblem()->addStateBlock(state_it.second);
+			getProblem()->notifyStateBlock(state_it.second, ADD);
 }
 
 void LandmarkPolyline2D::removeStateBlocks()
@@ -901,7 +891,7 @@ void LandmarkPolyline2D::removeStateBlocks()
     while (sbp_pair != point_state_ptr_map_.end())
     {
         if (getProblem())
-            getProblem()->removeStateBlock(sbp_pair->second);
+            getProblem()->notifyStateBlock(sbp_pair->second, REMOVE);
 
         sbp_pair = point_state_ptr_map_.erase(sbp_pair);
     }
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index 46f839049..a314cc1fa 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -23,13 +23,14 @@ ProcessorTrackerFeaturePolyline2D::~ProcessorTrackerFeaturePolyline2D()
 {
 }
 
-unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBasePtrList& _features_last_in,
-                                                            FeatureBasePtrList& _features_incoming_out,
-                                                            FeatureMatchMap& _feature_correspondences)
+unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBasePtrList& _features_in,
+                                                              const CaptureBasePtr& _capture,
+                                                              FeatureBasePtrList& _features_out,
+                                                              FeatureMatchMap& _feature_correspondences)
 {
-    WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_last_in.size());
+    WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_in.size());
 
-    if (_features_last_in.empty())
+    if (_features_in.empty())
         return 0;
 
     // prior transformations
@@ -51,7 +52,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
 
         // Check matching with all features in last
         // Store all matches consistent with T_last_incoming_prior in best_matches sorted by difference from T_last_incoming_prior
-        for (auto ftr_last_ : _features_last_in)
+        for (auto ftr_last_ : _features_in)
             tryMatchWithFeature(best_matches, std::static_pointer_cast<FeaturePolyline2D>(ftr_last_), pl_incoming, T_last_incoming_prior);
 
         std::cout << "\t" << best_matches.size() << " matches with features last found\n";
@@ -113,7 +114,10 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
             _feature_correspondences[pl_incoming] = best_ftr_match;
 
             // add feature to list of tracked features
-            _features_incoming_out.push_back(pl_incoming);
+            _features_out.push_back(pl_incoming);
+
+            // link the feature with the capture (since they weren't emplaced in preProcess())
+            pl_incoming->link(_capture);
 
             // match for this feature has been found
             matched = true;
@@ -164,7 +168,7 @@ bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame()
 unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_features)
 {
     WOLF_DEBUG("PTFP ", getName(), "::processNew: ");
-    unsigned int n = ProcessorTrackerFeature::processNew(_max_features);
+    unsigned int n = ProcessorTrackerFeature::processNew(_max_features); // implicit call to detectNewFeatures
 
     WOLF_DEBUG("Processing ", n, " new last features");
 
@@ -194,7 +198,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
         if (best_lmk_matches.empty())
         {
             // create a landmark
-            auto new_lmk_ptr = createLandmark(pl_ftr);
+            auto new_lmk_ptr = emplaceLandmark(pl_ftr);
 
             // Add a new match to landmark_match_map_
             auto new_lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
@@ -209,8 +213,6 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
             new_lmk_match->T_feature_landmark_.topLeftCorner(2,2) = R_sensor_world_last_;
             new_lmk_match->T_feature_landmark_.topRightCorner(2,1) = t_sensor_world_last_;
 
-            // Add the new landmark to the map
-            getProblem()->addLandmark(new_lmk_ptr);
             // Store in new_landmarks
             new_landmarks.push_back(new_lmk_ptr);
             // store match in map
@@ -262,17 +264,25 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
 }
 
 unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _max_new_features,
-                                                                  FeatureBasePtrList& _features_last_out)
+                                                                  const CaptureBasePtr& _capture,
+                                                                  FeatureBasePtrList& _features_out)
 {
     WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)");
 
-    _features_last_out = std::move(all_features_last_);
-    if (_max_new_features != -1 && _features_last_out.size() > _max_new_features)
-        _features_last_out.resize(_max_new_features);
+    // move all already created features (in preProcess())
+    _features_out = std::move(all_features_last_);
+
+    // resize to max_new_features
+    if (_max_new_features != -1 && _features_out.size() > _max_new_features)
+        _features_out.resize(_max_new_features);
+
+    // link detected features (only created in preProcess())
+    for (auto ftr : _features_out)
+        ftr->link(_capture);
 
-    WOLF_DEBUG(_features_last_out.size(), " were provided");
+    WOLF_DEBUG(_features_out.size(), " were provided");
 
-    return _features_last_out.size();
+    return _features_out.size();
 }
 
 void ProcessorTrackerFeaturePolyline2D::establishFactors()
@@ -281,14 +291,14 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors()
     unsigned int N_factors = 0;
 
     // Create a factor for each match in last features
-    auto ftr_it = last_ptr_->getFeatureList().begin();
-    while (ftr_it != last_ptr_->getFeatureList().end())
+    FeatureBasePtrList last_features = last_ptr_->getFeatureList();
+    for (auto ftr : last_features)
     {
-        WOLF_DEBUG("\tLast feature: ", (*ftr_it)->id());
-        assert(landmark_match_map_.find(*ftr_it) != landmark_match_map_.end() && "feature without landmark match in last features");
-        auto lmk_match = landmark_match_map_[*ftr_it];
+        WOLF_DEBUG("\tLast feature: ", ftr->id());
+        assert(landmark_match_map_.find(ftr) != landmark_match_map_.end() && "feature without landmark match in last features");
+        auto lmk_match = landmark_match_map_[ftr];
         auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_);
-        auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
+        auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(ftr);
         WOLF_DEBUG("\tLandmark: ", pl_lmk->id());
 
         // LANDMARK CHANGED: update match
@@ -301,9 +311,9 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors()
             {
                 std::cout << "\t\tNot success: removing feature " << pl_ftr->id() << " and landmark match\n";
                 // remove from match map
-                landmark_match_map_.erase(*ftr_it);
-                // remove from last feature list
-                ftr_it = last_ptr_->getFeatureList().erase(ftr_it);
+                landmark_match_map_.erase(ftr);
+                // remove feature
+                ftr->remove();
                 continue;
             }
         }
@@ -316,9 +326,9 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors()
             {
                 std::cout << "\t\tLandmark was removed: removing feature " << pl_ftr->id() << " and landmark match\n";
                 // remove from match map
-                landmark_match_map_.erase(*ftr_it);
-                // remove from last feature list
-                ftr_it = last_ptr_->getFeatureList().erase(ftr_it);
+                landmark_match_map_.erase(ftr);
+                // remove feature
+                ftr->remove();
                 continue;
             }
         }
@@ -330,7 +340,7 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors()
             if (modifyLandmarkAndMatch(lmk_match, pl_ftr))
                 modified_lmks_.push_back(pl_lmk);
 
-        // ESTABLISH CONSTRAINTS
+        // ESTABLISH FACTORS
         WOLF_DEBUG("\tEstablishing factors..");
         // checks
         assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
@@ -396,26 +406,25 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors()
             if (ftr_point_id < pl_ftr->getNPoints()-1)
                 lmk_point_id=pl_lmk->getNextValidId(lmk_point_id);
         }
-        // next ftr
-        ftr_it++;
     }
     std::cout << N_factors << " factors established" << std::endl;
 }
 
 void ProcessorTrackerFeaturePolyline2D::emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
-                                                                     LandmarkPolyline2DPtr _polyline_landmark,
-                                                                     const int& _ftr_point_id,
-                                                                     const int& _lmk_point_id,
-                                                                     const int& _lmk_prev_point_id)
+                                                                 LandmarkPolyline2DPtr _polyline_landmark,
+                                                                 const int& _ftr_point_id,
+                                                                 const int& _lmk_point_id,
+                                                                 const int& _lmk_prev_point_id)
 {
     assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id));
 
-    // CREATE CONSTRAINT --------------------
-    FactorBasePtr new_fac = std::make_shared<FactorPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id);
-
-    // ADD CONSTRAINT --------------------
-    _polyline_feature->addFactor(new_fac);
-    _polyline_landmark->addConstrainedBy(new_fac);
+    FactorBasePtr new_fac = FactorBase::emplace<FactorPointToLine2D>(_polyline_feature,
+                                                                     _polyline_feature,
+                                                                     _polyline_landmark,
+                                                                     shared_from_this(),
+                                                                     _ftr_point_id,
+                                                                     _lmk_point_id,
+                                                                     _lmk_prev_point_id);
 }
 
 void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
@@ -423,15 +432,17 @@ void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr
                                                                const int& _ftr_point_id,
                                                                const int& _lmk_point_id)
 {
-    // CREATE CONSTRAINT --------------------
-    FactorBasePtr new_fac = std::make_shared<FactorPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id);
-
-    // ADD CONSTRAINT --------------------
-    _polyline_feature->addFactor(new_fac);
-    _polyline_landmark->addConstrainedBy(new_fac);
+    assert(_polyline_landmark->isValidId(_lmk_point_id));
+
+    FactorBasePtr new_fac = FactorBase::emplace<FactorPoint2D>(_polyline_feature,
+                                                               _polyline_feature,
+                                                               _polyline_landmark,
+                                                               shared_from_this(),
+                                                               _ftr_point_id,
+                                                               _lmk_point_id);
 }
 
-LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::createLandmark(FeatureBasePtr _feature_ptr)
+LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePtr _feature_ptr)
 {
     WOLF_DEBUG("PTFP ", getName(), "::createLandmark: ");
     //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
@@ -450,11 +461,12 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::createLandmark(FeatureBasePtr
     // std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl;
 
     // Create new landmark
-    return std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true),
-                                                std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true),
-                                                points_global,
-                                                polyline_ptr->isFirstDefined(),
-                                                polyline_ptr->isLastDefined());
+    return LandmarkBase::emplace<LandmarkPolyline2D>(getProblem()->getMap(),
+                                                     std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true),
+                                                     std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true),
+                                                     points_global,
+                                                     polyline_ptr->isFirstDefined(),
+                                                     polyline_ptr->isLastDefined());
     //std::cout << "done" << std::endl;
 }
 
-- 
GitLab


From 813b51f123658480ae40cb5e7bdcb8502ccbc40a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 21 Jun 2019 10:06:29 +0200
Subject: [PATCH 2/9] improved check() in LandmarkMatchPolyline2D

---
 .../landmark/landmark_match_polyline_2D.h     |  2 +-
 src/landmark/landmark_match_polyline_2D.cpp   | 61 +++++++++++++------
 2 files changed, 44 insertions(+), 19 deletions(-)

diff --git a/include/laser/landmark/landmark_match_polyline_2D.h b/include/laser/landmark/landmark_match_polyline_2D.h
index 4e846418e..12e716a49 100644
--- a/include/laser/landmark/landmark_match_polyline_2D.h
+++ b/include/laser/landmark/landmark_match_polyline_2D.h
@@ -26,7 +26,7 @@ struct LandmarkMatchPolyline2D : public LandmarkMatch
     int landmark_version_;
     Eigen::Matrix3s T_feature_landmark_;
 
-    bool check() const;
+    bool check(FeatureBasePtr ftr) const;
 };
 
 }//end namespace
diff --git a/src/landmark/landmark_match_polyline_2D.cpp b/src/landmark/landmark_match_polyline_2D.cpp
index 318255a73..f490eb455 100644
--- a/src/landmark/landmark_match_polyline_2D.cpp
+++ b/src/landmark/landmark_match_polyline_2D.cpp
@@ -7,35 +7,59 @@
 
 #include "laser/landmark/landmark_match_polyline_2D.h"
 #include "laser/landmark/landmark_polyline_2D.h"
+#include "laser/feature/feature_polyline_2D.h"
 
 namespace wolf {
 
-bool LandmarkMatchPolyline2D::check() const
+bool LandmarkMatchPolyline2D::check(FeatureBasePtr ftr) const
 {
-    LandmarkPolyline2DPtr pl_lmk = std::dynamic_pointer_cast<LandmarkPolyline2D>(this->landmark_ptr_);
+    auto pl_lmk = std::dynamic_pointer_cast<LandmarkPolyline2D>(this->landmark_ptr_);
+    auto pl_ftr = std::dynamic_pointer_cast<FeaturePolyline2D>(ftr);
+
+    // landmark polyline type
     if (pl_lmk == nullptr)
     {
         WOLF_ERROR("LandmarkMatchPolyline2D landmark is not polyline 2D");
         return false;
     }
-    if (!pl_lmk->isClosed())
+    // feature polyline type
+    if (pl_ftr == nullptr)
     {
-        if (landmark_from_id_ > landmark_to_id_)
-        {
-            WOLF_ERROR("LandmarkMatchPolyline2D lmk_from > lmk_to with a not closed lmk");
-            return false;
-        }
-        if (landmark_from_id_ < pl_lmk->getFirstId())
-        {
-            WOLF_ERROR("LandmarkMatchPolyline2D lmk_from < lmk_first with a not closed lmk");
-            return false;
-        }
-        if (landmark_to_id_ > pl_lmk->getLastId())
-        {
-            WOLF_ERROR("LandmarkMatchPolyline2D lmk_to < lmk_last with a not closed lmk");
-            return false;
-        }
+        WOLF_ERROR("LandmarkMatchPolyline2D feature is not polyline 2D");
+        return false;
+    }
+    // Check idxs
+    if (!pl_lmk->isClosed() && landmark_from_id_ > landmark_to_id_)
+    {
+        WOLF_ERROR("LandmarkMatchPolyline2D lmk_from > lmk_to with a not closed lmk");
+        return false;
+    }
+    if (!pl_lmk->isClosed() && landmark_from_id_ < pl_lmk->getFirstId())
+    {
+        WOLF_ERROR("LandmarkMatchPolyline2D lmk_from < lmk_first with a not closed lmk");
+        return false;
+    }
+    if (!pl_lmk->isClosed() && landmark_to_id_ > pl_lmk->getLastId())
+    {
+        WOLF_ERROR("LandmarkMatchPolyline2D lmk_to < lmk_last with a not closed lmk");
+        return false;
+    }
+    if (feature_from_id_ != 0 && feature_to_id_ != pl_ftr->getNPoints()-1)
+    {
+        WOLF_ERROR("LandmarkMatchPolyline2D feature_from_id_ != 0 && feature_to_id_ != pl_ftr->getNPoints()-1 partial match");
+        return false;
     }
+    if (feature_from_id_ != 0 && landmark_from_id_ != pl_lmk->getFirstId())
+    {
+        WOLF_ERROR("LandmarkMatchPolyline2D feature_from_id_ != 0 && landmark_from_id_ != pl_lmk->getFirstId() partial match");
+        return false;
+    }
+    if (feature_to_id_ != pl_ftr->getNPoints()-1 && landmark_to_id_ != pl_lmk->getLastId())
+    {
+        WOLF_ERROR("LandmarkMatchPolyline2D feature_to_id_ != pl_ftr->getNPoints()-1 && landmark_to_id_ != pl_lmk->getLastId() partial match");
+        return false;
+    }
+    // correspondences
     int lmk_id = landmark_from_id_;
     int ftr_id = feature_from_id_;
     while (1)
@@ -60,6 +84,7 @@ bool LandmarkMatchPolyline2D::check() const
             ftr_id++;
         }
     }
+
     return true;
 }
 
-- 
GitLab


From 9f24255625c813003de843a8e080295006c9a088 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 21 Jun 2019 10:07:12 +0200
Subject: [PATCH 3/9] removed first_id_ and last_id_, using only getFirstId()
 and getLastId()

---
 include/laser/landmark/landmark_polyline_2D.h |  14 +--
 src/landmark/landmark_polyline_2D.cpp         | 108 +++++++-----------
 2 files changed, 49 insertions(+), 73 deletions(-)

diff --git a/include/laser/landmark/landmark_polyline_2D.h b/include/laser/landmark/landmark_polyline_2D.h
index 16dcdb9c5..665c1150a 100644
--- a/include/laser/landmark/landmark_polyline_2D.h
+++ b/include/laser/landmark/landmark_polyline_2D.h
@@ -77,8 +77,6 @@ class LandmarkPolyline2D : public LandmarkBase
         std::map<int,StateBlockPtr> point_state_ptr_map_; ///< polyline points state blocks
         bool first_defined_;            ///< Wether the first point is an extreme of a line or the line may continue
         bool last_defined_;             ///< Wether the last point is an extreme of a line or the line may continue
-        int first_id_;
-        int last_id_;
         bool closed_;                   ///< Wether the polyline is closed or not
         PolylineRectangularClass classification_; ///< The classification of the landmark
         int version_;                   ///< Integer increased each time a modification in landmark occurs (added points, merged points, closed, classified)
@@ -299,21 +297,21 @@ inline int LandmarkPolyline2D::getLastId() const {
 
 inline int LandmarkPolyline2D::getNextValidId(const int& i) const
 {
-    assert(!(i == last_id_ && !closed_) && "Calling getNextValidId of last_id in an open landmark");
+    assert(!(i == getLastId() && !closed_) && "Calling getNextValidId of last_id in an open landmark");
     assert(isValidId(i) && "Calling getNextValidId of an invalid id");
 
-    if (i == last_id_ && closed_)
-        return first_id_;
+    if (i == getLastId() && closed_)
+        return getFirstId();
     return std::next(point_state_ptr_map_.find(i))->first;
 }
 
 inline int LandmarkPolyline2D::getPrevValidId(const int& i) const
 {
-    assert(!(i == first_id_ && !closed_) && "Calling getPrevValidId of first_id in an open landmark");
+    assert(!(i == getFirstId() && !closed_) && "Calling getPrevValidId of first_id in an open landmark");
     assert(isValidId(i) && "Calling getPrevValidId of an invalid id");
 
-    if (i == first_id_ && closed_)
-        return last_id_;
+    if (i == getFirstId() && closed_)
+        return getLastId();
     return std::prev(point_state_ptr_map_.find(i))->first;
 }
 
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index 05569121e..a36666765 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -37,10 +37,7 @@ LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_pt
     if (!last_defined_)
         lastStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_ptr_map_.rbegin())->second));
 
-    first_id_ = getFirstId();
-    last_id_  = getLastId();
-
-    //std::cout << "LandmarkPolyline2D " << id() << " created. First: " << first_id_ << " last: "<< last_id_ << std::endl;
+    //std::cout << "LandmarkPolyline2D " << id() << " created. First: " << getFirstId() << " last: "<< getLastId() << std::endl;
 }
 
 LandmarkPolyline2D::~LandmarkPolyline2D()
@@ -135,7 +132,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
                                                                 (!_defined ?
                                                                         std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
                                                                         nullptr));
-        point_state_ptr_map_[++last_id_] = new_sb_ptr;
+        point_state_ptr_map_[getLastId()+1]= new_sb_ptr;
 
         if (getProblem())
         	getProblem()->notifyStateBlock(new_sb_ptr,ADD);
@@ -149,7 +146,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
                                                                 (!_defined ?
                                                                         std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
                                                                         nullptr));
-        point_state_ptr_map_[--first_id_] = new_sb_ptr;
+        point_state_ptr_map_[getFirstId()-1] = new_sb_ptr;
 
         if (getProblem())
         	getProblem()->notifyStateBlock(new_sb_ptr,ADD);
@@ -159,9 +156,6 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
 
     // new version
     version_++;
-
-    assert(getFirstId() == first_id_);
-    assert(getLastId() == last_id_);
 }
 
 void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigned int& _idx, const bool& _defined,
@@ -187,7 +181,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
                                                                     (i == _points.cols()-1 && !_defined ?
                                                                             std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
                                                                             nullptr));
-            point_state_ptr_map_[++last_id_] = new_sb_ptr;
+            point_state_ptr_map_[getLastId()+1] = new_sb_ptr;
 
         	if (getProblem())
         		getProblem()->notifyStateBlock(new_sb_ptr,ADD);
@@ -203,7 +197,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
                                                                     (i == 0 && !_defined ?
                                                                             std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
                                                                             nullptr));
-            point_state_ptr_map_[--first_id_] = new_sb_ptr;
+            point_state_ptr_map_[getFirstId()-1] = new_sb_ptr;
 
             if (getProblem())
         		getProblem()->notifyStateBlock(new_sb_ptr,ADD);
@@ -213,10 +207,6 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
 
     // new version
     version_++;
-
-    //std::cout << "final number of points: " << point_state_ptr_vector_.size() << std::endl;
-    assert(getFirstId() == first_id_);
-    assert(getLastId() == last_id_);
 }
 
 void LandmarkPolyline2D::defineFirst()
@@ -275,8 +265,8 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
     std::vector<std::pair<int, int> > overlapped_ids;
 
     // Check first defined point against the rest defined points
-    int first_point_id = (first_defined_ ? first_id_ : getNextValidId(first_id_));
-    int with_point_id = (last_defined_ ? last_id_ : getPrevValidId(last_id_));
+    int first_point_id = (first_defined_ ? getFirstId() : getNextValidId(getFirstId()));
+    int with_point_id = (last_defined_ ? getLastId() : getPrevValidId(getLastId()));
     int last_with_id  = first_point_id; // limit for avoiding more than one overlapping per defined point (full overlapping of defined points)
     bool found = false;
 
@@ -312,10 +302,10 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
             }
 
             // overlapped points from corresponding first point match to match with last point
-            while (with_point_id <= last_id_)
+            while (with_point_id <= getLastId())
             {
                 overlapped_ids.push_back(std::pair<int,int>(first_point_id, with_point_id));
-                if (with_point_id == last_id_)
+                if (with_point_id == getLastId())
                     break;
                 first_point_id = getNextValidId(first_point_id);
                 with_point_id = getNextValidId(with_point_id);
@@ -330,14 +320,14 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
                     Scalar sq_dist=1e6;
 
                     // 2 defined points: point-point
-                    if ( (overlapped_ids[i].first != first_id_ || first_defined_) &&
-                         (overlapped_ids[i].second != last_id_ || last_defined_) )
+                    if ( (overlapped_ids[i].first != getFirstId() || first_defined_) &&
+                         (overlapped_ids[i].second != getLastId() || last_defined_) )
                         sq_dist = (getPointVector(overlapped_ids[i].first)-getPointVector(overlapped_ids[i].second)).squaredNorm();
                     // defined with not defined: not defined point-segment
-                    else if (overlapped_ids[i].first != first_id_ || first_defined_)
+                    else if (overlapped_ids[i].first != getFirstId() || first_defined_)
                         sq_dist = sqDistPoint2Segment(getPointVector(overlapped_ids[i].second), getPointVector(overlapped_ids[i].first), getPointVector(getNextValidId(overlapped_ids[i].first)));
                     // not defined with defined: not defined point-segment
-                    else if (overlapped_ids[i].second != last_id_ || last_defined_)
+                    else if (overlapped_ids[i].second != getLastId() || last_defined_)
                         sq_dist = sqDistPoint2Segment(getPointVector(overlapped_ids[i].first),getPointVector(overlapped_ids[i].second), getPointVector(getPrevValidId(overlapped_ids[i].second)));
                     else
                         throw std::runtime_error("when trying to close, overlapping two not defined points in a landmark");
@@ -345,13 +335,13 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
                     if (sq_dist > _dist_tol*_dist_tol)
                     {
                         WOLF_TRACE("\nBad overlapping: ", sq_dist,
-                                   "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == first_id_ && !first_defined_ ? "NOT DEFINED" : ""),
-                                   "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == last_id_ && !last_defined_ ? "NOT DEFINED" : ""));
+                                   "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == getFirstId() && !first_defined_ ? "NOT DEFINED" : ""),
+                                   "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == getLastId() && !last_defined_ ? "NOT DEFINED" : ""));
                         return false;
                     }
                     WOLF_TRACE("\nGood overlapping: ", sq_dist,
-                               "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == first_id_ && !first_defined_ ? "NOT DEFINED" : ""),
-                               "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == last_id_ && !last_defined_ ? "NOT DEFINED" : ""));
+                               "\n\tpoint\t", overlapped_ids[i].first, " (", getPointVector(overlapped_ids[i].first).transpose(), ")", (overlapped_ids[i].first == getFirstId() && !first_defined_ ? "NOT DEFINED" : ""),
+                               "\n\twith \t", overlapped_ids[i].second," (", getPointVector(overlapped_ids[i].second).transpose(),")", (overlapped_ids[i].second == getLastId() && !last_defined_ ? "NOT DEFINED" : ""));
                 }
             }
         }
@@ -363,10 +353,10 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
         WOLF_TRACE("No overlapping found with defined points, trying with not defined extremes");
 
         // Check for both not defined extremes that the distance to the segment of first and last defined points is below the threshold
-        Eigen::Vector2s first_not_def   = getPointVector(first_id_);
-        Eigen::Vector2s last_not_def    = getPointVector(last_id_);
-        Eigen::Vector2s first_def       = getPointVector(getNextValidId(first_id_));
-        Eigen::Vector2s last_def        = getPointVector(getPrevValidId(last_id_));
+        Eigen::Vector2s first_not_def   = getPointVector(getFirstId());
+        Eigen::Vector2s last_not_def    = getPointVector(getLastId());
+        Eigen::Vector2s first_def       = getPointVector(getNextValidId(getFirstId()));
+        Eigen::Vector2s last_def        = getPointVector(getPrevValidId(getLastId()));
 
         if(sqDistPoint2Segment(first_not_def, last_def, first_def) < _dist_tol*_dist_tol &&
            sqDistPoint2Segment(last_not_def,  last_def, first_def) < _dist_tol*_dist_tol)
@@ -388,7 +378,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
                 WOLF_TRACE("The not defined extremes are overlapped",
                            "\n\tlast_proj_2_first_def:  ", last_proj_2_first_def,
                            "\n\tfirst_proj_2_first_def: ", first_proj_2_first_def);
-                overlapped_ids.push_back(std::pair<int,int>(first_id_, last_id_));
+                overlapped_ids.push_back(std::pair<int,int>(getFirstId(), getLastId()));
             }
         }
     }
@@ -402,7 +392,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
         while (!overlapped_ids.empty())
         {
             // not defined points are merged into defined ones
-            if ((overlapped_ids.back().first == first_id_ && !first_defined_) || (overlapped_ids.back().first == last_id_ && !last_defined_))
+            if ((overlapped_ids.back().first == getFirstId() && !first_defined_) || (overlapped_ids.back().first == getLastId() && !last_defined_))
                 mergePoints(overlapped_ids.back().first,overlapped_ids.back().second);
             else
                 mergePoints(overlapped_ids.back().second,overlapped_ids.back().first);
@@ -413,9 +403,6 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
 
         WOLF_TRACE("Landmark ",id()," was closed.");
 
-        assert(getFirstId() == first_id_);
-        assert(getLastId() == last_id_);
-
         return true;
     }
     return false;
@@ -439,7 +426,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
     assert(!(_remain_id == getLastId() && !last_defined_) && "in merging points, the remaining point must be defined");
     assert(!(_remain_id == getFirstId() && !first_defined_) && "in merging points, the remaining point must be defined");
 
-    //std::cout << "merge points: remove " << _remove_id << " and keep " << _remain_id << " (ids: " << first_id_ << " to " << getLastId() << ")" << std::endl;
+    //std::cout << "merge points: remove " << _remove_id << " and keep " << _remain_id << " (ids: " << getFirstId() << " to " << getLastId() << ")" << std::endl;
 
     StateBlockPtr remove_state = getPointStateBlock(_remove_id);
     //std::cout << "state block to remove " << remove_state->getState().transpose() << std::endl;
@@ -512,9 +499,9 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
     }
 
     // If removed was a not defined extreme, set defined extreme
-    if (_remove_id == first_id_ && !first_defined_)
+    if (_remove_id == getFirstId() && !first_defined_)
         first_defined_ = true;
-    if (_remove_id == last_id_ && !last_defined_)
+    if (_remove_id == getLastId() && !last_defined_)
         last_defined_ = true;
 
     // Remove remove_state
@@ -526,15 +513,8 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
     point_state_ptr_map_.erase(_remove_id);
     //std::cout << "state removed from point vector " << std::endl;
 
-    // reset first_id_ & last_id_
-    first_id_ = getFirstId();
-    last_id_ = getLastId();
-
     // new version
     version_++;
-
-    assert(getFirstId() == first_id_);
-    assert(getLastId() == last_id_);
 }
 
 bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<PolylineRectangularClass> _classes)
@@ -628,7 +608,7 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
             if (getNPoints() < 4)
             {
                 addPoint(Eigen::Vector2s::Zero(), true, true);
-                points_ids.push_back(last_id_);
+                points_ids.push_back(getLastId());
                 //std::cout << "ADDING POINT\n";
                 //std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl;
             }
@@ -642,14 +622,14 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
                 {
                     //std::cout << "4 DEFINED POINTS\n";
                     if (!last_defined_)
-                        mergePoints(first_id_, getPrevValidId(last_id_));
+                        mergePoints(getFirstId(), getPrevValidId(getLastId()));
                     else
-                        mergePoints(first_id_, last_id_);
+                        mergePoints(getFirstId(), getLastId());
                 }
                 else // define
                 {
                     defineFirst();
-                    points_ids.push_back(first_id_);
+                    points_ids.push_back(getFirstId());
                 }
                 //std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl;
             }
@@ -662,12 +642,12 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
                 if (getNDefinedPoints() == 4) // merge
                 {
                     //std::cout << "4 DEFINED POINTS\n";
-                    mergePoints(last_id_, first_id_);
+                    mergePoints(getLastId(), getFirstId());
                 }
                 else // define
                 {
                     defineLast();
-                    points_ids.push_back(last_id_); // it is points_ids[3]
+                    points_ids.push_back(getLastId()); // it is points_ids[3]
                 }
                 //std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl;
             }
@@ -675,8 +655,6 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
             assert(getNDefinedPoints() == 4 && "classified landmark must have 4 defined points");
             assert(getNPoints() == 4 && "classified landmark must have 4 points");
             assert(points_ids.size() == 4 && "classified landmark must have 4 points");
-            assert(getFirstId() == first_id_);
-            assert(getLastId() == last_id_);
         }
 
         // Close
@@ -749,17 +727,17 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
         // ADD FRONT POINTS
         if (_merged_from > _merged_lmk->getFirstId())
         {
-            assert(_from == first_id_);
+            assert(_from == getFirstId());
             for (int merged_id = _merged_lmk->getPrevValidId(_merged_from); ; merged_id = _merged_lmk->getPrevValidId(merged_id))
             {
                 //std::cout << "adding front point: " << _merged_lmk->getPointVector(merged_id).transpose() << std::endl;
-                //std::cout << "Until current first point: " << first_id_ << "\n";
+                //std::cout << "Until current first point: " << getFirstId() << "\n";
 
                 // Add point
                 addPoint(_merged_lmk->getPointVector(merged_id), merged_id != _merged_lmk->getFirstId() || _merged_lmk->isFirstDefined() ,false);
                 //std::cout << "added\n";
-                //std::cout << "adding correspondence" << merged_id << " with " << first_id_ << "\n";
-                merged_id_to_id[merged_id] = first_id_;
+                //std::cout << "adding correspondence" << merged_id << " with " << getFirstId() << "\n";
+                merged_id_to_id[merged_id] = getFirstId();
                 //std::cout << "added correspondence\n";
 
                 // exit loop
@@ -770,17 +748,17 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
         // ADD BACK POINTS
         if (_merged_to < _merged_lmk->getLastId())
         {
-            assert(_to == last_id_);
+            assert(_to == getLastId());
             for (int merged_id = _merged_lmk->getNextValidId(_merged_to); ; merged_id = _merged_lmk->getNextValidId(merged_id))
             {
                 //std::cout << "adding back point: " << _merged_lmk->getPointVector(merged_id).transpose() << std::endl;
-                //std::cout << "Until current last point: " << last_id_ << "\n";
+                //std::cout << "Until current last point: " << getLastId() << "\n";
 
                 // Add point
                 addPoint(_merged_lmk->getPointVector(merged_id), merged_id != _merged_lmk->getLastId() || _merged_lmk->isLastDefined() ,true);
                 //std::cout << "added\n";
-                //std::cout << "adding correspondence" << merged_id << " with " << last_id_ << "\n";
-                merged_id_to_id[merged_id] = last_id_;
+                //std::cout << "adding correspondence" << merged_id << " with " << getLastId() << "\n";
+                merged_id_to_id[merged_id] = getLastId();
                 //std::cout << "added correspondence\n";
 
                 // exit loop
@@ -799,9 +777,9 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
         merged_id_to_id[merged_id] = id;
 
         // Set Defined extremes if it's the case
-        if (id == first_id_ && !first_defined_ && (merged_id != _merged_lmk->getFirstId() || _merged_lmk->isFirstDefined()))
+        if (id == getFirstId() && !first_defined_ && (merged_id != _merged_lmk->getFirstId() || _merged_lmk->isFirstDefined()))
             defineFirst();
-        if (id == last_id_ && !last_defined_ && (merged_id != _merged_lmk->getLastId() || _merged_lmk->isLastDefined()))
+        if (id == getLastId() && !last_defined_ && (merged_id != _merged_lmk->getLastId() || _merged_lmk->isLastDefined()))
             defineLast();
 
         // exit loop
@@ -945,7 +923,7 @@ YAML::Node LandmarkPolyline2D::saveToYaml() const
     YAML::Node node = LandmarkBase::saveToYaml();
 
     // Then add specific things
-    node["first_id"]            = first_id_;
+    node["first_id"]            = getFirstId();
     node["first_defined"]       = first_defined_;
     node["last_defined"]        = last_defined_;
     node["classification_type"] = static_cast<int>(classification_.type);
-- 
GitLab


From 12ef59da6bee95ecdb802096dda60913a920c077 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 21 Jun 2019 10:11:50 +0200
Subject: [PATCH 4/9] changed name all_features by untracked_features

---
 .../processor_tracker_feature_polyline_2D.h   |  4 +-
 .../processor_tracker_feature_polyline_2D.cpp | 65 ++++++++++++-------
 2 files changed, 42 insertions(+), 27 deletions(-)

diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h
index 5a9a360da..918a17961 100644
--- a/include/laser/processor/processor_tracker_feature_polyline_2D.h
+++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h
@@ -52,7 +52,7 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
         ProcessorParamsTrackerFeaturePolyline2DPtr params_tracker_feature_polyline_;
         laserscanutils::LineFinderIterative line_finder_;
 
-        FeatureBasePtrList all_features_incoming_, all_features_last_;
+        FeatureBasePtrList untracked_features_incoming_, untracked_features_last_;
         LandmarkMatchPolyline2DMap landmark_match_map_;
         LandmarkPolyline2DPtrList   modified_lmks_;
         std::list<LandmarkPolyline2DPtrList> merge_candidates_list_;
@@ -234,7 +234,7 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
 
         const FeatureBasePtrList& getLastNewFeatures() const
         {
-            return all_features_last_;
+            return untracked_features_last_;
         }
 };
 
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index a314cc1fa..dea8f4e34 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -38,11 +38,11 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
     T_last_incoming_prior.topLeftCorner(2,2)  = R_last_incoming_;
     T_last_incoming_prior.topRightCorner(2,1) = t_last_incoming_;
 
-    // ALL AGAINST ALL: nearest neighbor matching (already detected incoming features stored in: all_features_incoming_)
-    auto ftr_it = all_features_incoming_.begin();
+    // ALL AGAINST ALL: nearest neighbor matching (already detected incoming features stored in: untracked_features_incoming_)
+    auto ftr_it = untracked_features_incoming_.begin();
 
-    // iterate over all polylines features
-    while (ftr_it != all_features_incoming_.end())
+    // iterate over all untracked_features_incoming_
+    while (ftr_it != untracked_features_incoming_.end())
     {
         bool matched = false;
         auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
@@ -127,7 +127,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
 
         // next feature
         if (matched)
-            ftr_it = all_features_incoming_.erase(ftr_it); // remove from all_features_incoming
+            ftr_it = untracked_features_incoming_.erase(ftr_it); // remove feature from untracked features list
         else
             ftr_it++;
     }
@@ -267,10 +267,10 @@ unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _ma
                                                                   const CaptureBasePtr& _capture,
                                                                   FeatureBasePtrList& _features_out)
 {
-    WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)");
+    WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", untracked_features_last_.size(), " in untracked_features_last_)");
 
     // move all already created features (in preProcess())
-    _features_out = std::move(all_features_last_);
+    _features_out = std::move(untracked_features_last_);
 
     // resize to max_new_features
     if (_max_new_features != -1 && _features_out.size() > _max_new_features)
@@ -659,11 +659,20 @@ void ProcessorTrackerFeaturePolyline2D::advanceDerived()
             landmark_match_map_.erase(ftr);
         }
 
-    WOLF_DEBUG("removing ", all_features_last_.size() , " features in all_features_last_");
-    all_features_last_.clear();
-    all_features_last_.splice(all_features_last_.end(),all_features_incoming_);
-    if (last_ptr_)
-        WOLF_DEBUG("all_features_last_ has ", all_features_last_.size() , " features (prev. all_features_incoming_)");
+    // check all not tracked features are not linked
+    assert(std::all_of(untracked_features_last_.begin(), untracked_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() == nullptr;}) &&
+           "any linked feature in untracked_features_last_");
+    assert(std::all_of(untracked_features_incoming_.begin(), untracked_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() == nullptr;}) &&
+           "any linked feature in untracked_features_incoming_");
+
+    // remove untracked features
+    WOLF_DEBUG("PTF ", getName(), ": ", "removing ", untracked_features_last_.size() , " features in untracked_features_last_");
+    untracked_features_last_.clear();
+
+    // move untracked features incoming to last
+    untracked_features_last_.splice(untracked_features_last_.end(),untracked_features_incoming_);
+    WOLF_DEBUG("PTF ", getName(), ": untracked_features_last_ has ", untracked_features_last_.size() , " features (prev. untracked_features_incoming_)");
+    WOLF_DEBUG_COND(last_ptr_!= nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
 
     ProcessorTrackerFeature::advanceDerived();
 }
@@ -679,12 +688,20 @@ void ProcessorTrackerFeaturePolyline2D::resetDerived()
             landmark_match_map_.erase(ftr);
         }
 
-    WOLF_DEBUG("PTF ", getName(), ": ", "removing ", all_features_last_.size() , " features in all_features_last_");
-    all_features_last_.clear();
-    all_features_last_.splice(all_features_last_.end(),all_features_incoming_);
-    WOLF_DEBUG("PTF ", getName(), ": ", "all_features_last_ has ", all_features_last_.size() , " features (prev. all_features_incoming_)");
-    if (last_ptr_)
-        WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
+    // check all not tracked features are not linked
+    assert(std::all_of(untracked_features_last_.begin(), untracked_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() == nullptr;}) &&
+           "any linked feature in untracked_features_last_");
+    assert(std::all_of(untracked_features_incoming_.begin(), untracked_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() == nullptr;}) &&
+           "any linked feature in untracked_features_incoming_");
+
+    // remove untracked features
+    WOLF_DEBUG("PTF ", getName(), ": ", "removing ", untracked_features_last_.size() , " features in untracked_features_last_");
+    untracked_features_last_.clear();
+
+    // move untracked features incoming to last
+    untracked_features_last_.splice(untracked_features_last_.end(),untracked_features_incoming_);
+    WOLF_DEBUG("PTF ", getName(), ": untracked_features_last_ has ", untracked_features_last_.size() , " features (prev. untracked_features_incoming_)");
+    WOLF_DEBUG_COND(last_ptr_!= nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
 
     ProcessorTrackerFeature::resetDerived();
 }
@@ -703,7 +720,7 @@ void ProcessorTrackerFeaturePolyline2D::preProcess()
     {
         //WOLF_DEBUG("new polyline detected: Defined", pl.first_defined_ , pl.last_defined_ );
         //std::cout << "covs: " << std::endl << pl.covs_ << std::endl;
-        all_features_incoming_.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_));
+        untracked_features_incoming_.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_));
         //WOLF_DEBUG("new polyline detected: ");
     }
 
@@ -711,9 +728,8 @@ void ProcessorTrackerFeaturePolyline2D::preProcess()
     if (last_ptr_ != nullptr && incoming_ptr_ != nullptr)
         computeTransformations();
 
-    WOLF_DEBUG("PTF ", getName(), ": ", "all_features_last_ has ", all_features_last_.size() , " features");
-    if (last_ptr_)
-        WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
+    WOLF_DEBUG("PTF ", getName(), ": ", "untracked_features_last_ has ", untracked_features_last_.size() , " features");
+    WOLF_DEBUG_COND(last_ptr_!=nullptr, "PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
 }
 
 void ProcessorTrackerFeaturePolyline2D::postProcess()
@@ -757,9 +773,8 @@ void ProcessorTrackerFeaturePolyline2D::postProcess()
         // Merge landmarks candidates and accumulate the correspondence of merged to the remaining ones
         LandmarkPolyline2D::tryMergeLandmarks(merge_candidates, params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th);
     }
-    WOLF_DEBUG("PTF ", getName(), ": ", "all_features_last_ has ", all_features_last_.size() , " features");
-    if (last_ptr_)
-        WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
+    WOLF_DEBUG("PTF ", getName(), ": ", "untracked_features_last_ has ", untracked_features_last_.size() , " features");
+    WOLF_DEBUG_COND(last_ptr_!=nullptr,"PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
 }
 
 void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
-- 
GitLab


From cafd6d3d957f0fa092ae8665eda36ecda13be4ef Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 21 Jun 2019 10:15:15 +0200
Subject: [PATCH 5/9] adapted to emplace

---
 .../processor_tracker_feature_polyline_2D.cpp | 44 +++++++++----------
 1 file changed, 22 insertions(+), 22 deletions(-)

diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index dea8f4e34..15e6a3001 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -84,9 +84,9 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
                     if (!updateMatchWithLandmark(landmark_match_map_[pl_last],pl_lmk,pl_last))
                     {
                         // not successful update -> remove match and feature
-                        std::cout << "\t\t\tNot success: removing feature " << pl_last->id() << " and landmark match\n";
+                        std::cout << "\t\t\tNot success: removing last feature " << pl_last->id() << " and removing landmark match\n";
                         landmark_match_map_.erase(pl_last);
-                        last_ptr_->getFeatureList().remove(pl_last);
+                        pl_last->remove();
                         continue;
                     }
                 }
@@ -98,25 +98,27 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
                 auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_incoming,
                                                                                best_ftr_match,
                                                                                landmark_match_map_[pl_last]);
-                // Add the incoming match to landmark_match_map
-                if (pl_lmk_match_incoming != nullptr)
-                    landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
-                else
+
+                // if concatenation did not succees -> next ftr match
+                if (pl_lmk_match_incoming == nullptr)
                 {
                     WOLF_DEBUG("PTFP ", getName(), "::trackFeatures: incoming track lost common points with landmark");
                     continue;
                 }
+
+                // Add the incoming match to landmark_match_map
+                landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
             }
 
             // TRACK valid
             std::cout << "\t\tvalid match, storing...\n";
-            // add best match to match_map _feature_correspondences[feature_out_ptr] = feature_in_ptr
+            // add best match to feature-feature correspondences
             _feature_correspondences[pl_incoming] = best_ftr_match;
 
             // add feature to list of tracked features
             _features_out.push_back(pl_incoming);
 
-            // link the feature with the capture (since they weren't emplaced in preProcess())
+            // link the incoming eature with the capture (since it was just created in preProcess())
             pl_incoming->link(_capture);
 
             // match for this feature has been found
@@ -168,7 +170,7 @@ bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame()
 unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_features)
 {
     WOLF_DEBUG("PTFP ", getName(), "::processNew: ");
-    unsigned int n = ProcessorTrackerFeature::processNew(_max_features); // implicit call to detectNewFeatures
+    unsigned int n = ProcessorTrackerFeature::processNew(_max_features); // implicit call to detectNewFeatures() and trackFeatures()
 
     WOLF_DEBUG("Processing ", n, " new last features");
 
@@ -177,13 +179,11 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
     T_sensor_world_last.topLeftCorner(2,2)  = R_sensor_world_last_;
     T_sensor_world_last.topRightCorner(2,1) = t_sensor_world_last_;
 
-    // For each new feature: Either create a landmark or match with an existent landmark
+    // For each new last feature: Either create a landmark or match with an existent landmark
     LandmarkBasePtrList new_landmarks;
-    for (auto ftr_it = std::prev(last_ptr_->getFeatureList().end(),n);
-              ftr_it != last_ptr_->getFeatureList().end();
-              ftr_it++)
+    for (auto ftr : new_features_last_)
     {
-        auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
+        auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(ftr);
         LandmarkMatchPolyline2DScalarMap best_lmk_matches;
         WOLF_DEBUG("Processing feature: ", pl_ftr->id());
 
@@ -236,13 +236,13 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
     }
 
     // store in landmark_match_map_ the incoming features
-    auto ftr_it = std::prev(incoming_ptr_->getFeatureList().end(),n);
-    while (ftr_it != incoming_ptr_->getFeatureList().end())
+    auto ftr_it = new_features_incoming_.begin();
+    while (ftr_it != new_features_incoming_.end())
     {
         auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
-        assert(matches_last_from_incoming_.find(pl_incoming) != matches_last_from_incoming_.end() && "last-incoming match not found");
-        auto pl_ftr_match = std::static_pointer_cast<FeatureMatchPolyline2D>(matches_last_from_incoming_[pl_incoming]);
-        assert(landmark_match_map_.find(pl_ftr_match->feature_ptr_) != landmark_match_map_.end() && "last-lmk match not found");
+        assert(matches_last_from_incoming_.count(*ftr_it) && "last-incoming match not found");
+        auto pl_ftr_match = std::static_pointer_cast<FeatureMatchPolyline2D>(matches_last_from_incoming_[*ftr_it]);
+        assert(landmark_match_map_.count(pl_ftr_match->feature_ptr_) && "last-lmk match not found");
         auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_incoming,
                                                                        pl_ftr_match,
                                                                        landmark_match_map_[pl_ftr_match->feature_ptr_]);
@@ -255,9 +255,9 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
         {
             WOLF_DEBUG("PTFP ", getName(), "::processNew: incoming track lost common points with landmark");
             matches_last_from_incoming_.erase(pl_incoming);
-            // move feature from known to all features
-            all_features_incoming_.push_back(pl_incoming);
-            ftr_it = incoming_ptr_->getFeatureList().erase(ftr_it);
+            // remove feature (unlink not allowed, we could create a new one via copy..)
+            pl_incoming->remove();
+            ftr_it = new_features_incoming_.erase(ftr_it);
         }
     }
     return n;
-- 
GitLab


From 4c9b8aac74fa8f333a77baac3cf2c59acb51e365 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 21 Jun 2019 10:16:12 +0200
Subject: [PATCH 6/9] more asserts, debuging comments and documentation

---
 .../processor_tracker_feature_polyline_2D.cpp | 119 ++++++++++--------
 1 file changed, 68 insertions(+), 51 deletions(-)

diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index 15e6a3001..ce350fc5f 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -65,11 +65,11 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
 
             std::cout << "\t\tconfirming match with feature last: " << pl_last->id() << "\n";
 
-            // If it has not been created in processNew: check/update/remove landmark match
-            if(find(new_features_last_.begin(), new_features_last_.end(), pl_last) == new_features_last_.end())
+            // check/update/remove landmark match (only if not recently created in processNew)
+            if(!std::binary_search(new_features_last_.begin(), new_features_last_.end(), pl_last))
             {
-                // the match with this last_ feature was removed after any updateMatchWithLandmark
-                if(landmark_match_map_.find(pl_last) == landmark_match_map_.end())
+                // Check if the match with this last_ feature was removed after any updateMatchWithLandmark
+                if(landmark_match_map_.count(pl_last) == 0)
                 {
                     std::cout << "\t\tremoved feature last\n";
                     continue;
@@ -77,10 +77,10 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
 
                 auto pl_lmk  = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[pl_last]->landmark_ptr_);
 
-                // Landmark changed or was merged -> Redo match with last feature
+                // Landmark changed or was merged -> update match with last feature
                 if (landmark_match_map_[pl_last]->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
                 {
-                    std::cout << "\t\tupdating match...\n";
+                    std::cout << "\t\tupdating match last-landmark...\n";
                     if (!updateMatchWithLandmark(landmark_match_map_[pl_last],pl_lmk,pl_last))
                     {
                         // not successful update -> remove match and feature
@@ -295,7 +295,7 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors()
     for (auto ftr : last_features)
     {
         WOLF_DEBUG("\tLast feature: ", ftr->id());
-        assert(landmark_match_map_.find(ftr) != landmark_match_map_.end() && "feature without landmark match in last features");
+        assert(landmark_match_map_.count(ftr) && "feature without landmark match in last features");
         auto lmk_match = landmark_match_map_[ftr];
         auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_);
         auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(ftr);
@@ -305,7 +305,7 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors()
         WOLF_DEBUG("\tLandmark changed?");
         if (lmk_match->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
         {
-            WOLF_DEBUG("\tLandmark changed");
+            WOLF_DEBUG("\tLandmark changed or was merged");
             // not successful update
             if (!updateMatchWithLandmark(lmk_match,pl_lmk,pl_ftr))
             {
@@ -346,7 +346,7 @@ void ProcessorTrackerFeaturePolyline2D::establishFactors()
         assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
         assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
         assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
-        assert(lmk_match->check() && "Landmark didn't grow properly");
+        assert(lmk_match->check(pl_ftr) && "Landmark didn't grow properly");
 
         std::cout << "Establishing factor between" << std::endl;
         std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
@@ -474,7 +474,6 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
 {
     WOLF_DEBUG("PTFP ", getName(), "::modifyLandmarkAndMatch: ");
     auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_);
-
     bool print = true;
     bool modified = false;
 
@@ -486,7 +485,11 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
         std::cout << "\tfirst defined " << pl_ftr->isFirstDefined() << std::endl;
         std::cout << "\tlast defined " << pl_ftr->isLastDefined() << std::endl;
         std::cout << "landmark " << pl_lmk->id() << ": " << std::endl;
-        std::cout << "\tpoints " << pl_lmk->getNPoints() << std::endl;
+        std::cout << "\tpoints " << pl_lmk->getNPoints() << " ( ";
+        std::vector<int> lmk_ids = pl_lmk->getValidIds();
+        for (auto id : lmk_ids)
+            std::cout << id << " ";
+        std::cout << ")" << std::endl;
         std::cout << "\tfirst defined " << pl_lmk->isFirstDefined() << std::endl;
         std::cout << "\tlast defined " << pl_lmk->isLastDefined() << std::endl << std::endl;
         std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl;
@@ -494,6 +497,11 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
         std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl;
         std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl;
     }
+    assert(lmk_match->landmark_version_ == pl_lmk->getVersion() && "Different landmark version");
+    assert(lmk_match->check(pl_ftr) && "Bad landmark match provided");
+    assert((lmk_match->landmark_from_id_ == pl_lmk->getFirstId() || lmk_match->feature_from_id_ == 0) && "Partial match");
+    assert((lmk_match->landmark_to_id_ == pl_lmk->getLastId() || lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1)  && "Partial match");
+
     //Eigen::MatrixXs points_global = R_world_sensor_last_ * pl_ftr->getPoints().topRows<2>() +
     //                                t_world_sensor_last_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose();
     Eigen::MatrixXs points_global = lmk_match->T_feature_landmark_.inverse() * pl_ftr->getPoints();
@@ -531,8 +539,8 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
 
         // checks
         assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
-        assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
-        assert(lmk_match->check() && "Landmark didn't grow properly");
+        assert(lmk_match->landmark_from_id_ == pl_lmk->getFirstId() && "Landmark didn't grow properly");
+        assert(lmk_match->check(pl_ftr) && "Landmark didn't grow properly");
     }
     // -----------------CHANGE Front-----------------
     // Change first not defined point if landmark first not defined matched with first feature point that:
@@ -540,13 +548,6 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
     // b. would extend the line (check if angle is bigger than 90º)
     else if (lmk_match->landmark_from_id_ == pl_lmk->getFirstId() && !pl_lmk->isFirstDefined()) // && pl_match->feature_from_id_ == 0
     {
-        if (print)
-        {
-            std::cout << "Change first point. Defined: " << pl_ftr->isFirstDefined() << std::endl;
-            std::cout << "\tpoint " << pl_lmk->getFirstId() << ": " << pl_lmk->getPointVector(pl_lmk->getFirstId()).transpose() << std::endl;
-            std::cout << "\tpoint " << pl_lmk->getFirstId()+1 << ": " << pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId())).transpose() << std::endl;
-            std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl;
-        }
         if (// case a
             pl_ftr->isFirstDefined() ||
             // case b
@@ -554,15 +555,23 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
             (points_global.topLeftCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm() +
             (pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId()))-pl_lmk->getPointVector(pl_lmk->getFirstId())).squaredNorm())
         {
+            if (print)
+            {
+                std::cout << "Change first point. Defined: " << pl_ftr->isFirstDefined() << std::endl;
+                std::cout << "\tpoint " << pl_lmk->getFirstId() << ": " << pl_lmk->getPointVector(pl_lmk->getFirstId()).transpose() << std::endl;
+                std::cout << "\tpoint " << pl_lmk->getFirstId()+1 << ": " << pl_lmk->getPointVector(pl_lmk->getNextValidId(pl_lmk->getFirstId())).transpose() << std::endl;
+                std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl;
+            }
+
             pl_lmk->setFirst(points_global.col(0), pl_ftr->isFirstDefined());
             modified = true;
             lmk_match->landmark_version_ = pl_lmk->getVersion();
-        }
 
-        // checks
-        assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
-        assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
-        assert(lmk_match->check() && "Landmark didn't grow properly");
+            // checks
+            assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
+            assert(lmk_match->landmark_from_id_ == pl_lmk->getFirstId() && "Landmark didn't grow properly");
+            assert(lmk_match->check(pl_ftr) && "Landmark didn't grow properly");
+        }
     }
     // -----------------GROW Back-----------------
     // Add new back points (if any feature point not matched)
@@ -594,10 +603,9 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
             std::cout << "\tland to   " << lmk_match->landmark_to_id_ << std::endl;
         }
         // checks
-        assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
         assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
-        assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
-        assert(lmk_match->check() && "Landmark didn't grow properly");
+        assert(lmk_match->landmark_to_id_ == pl_lmk->getLastId() && "Landmark didn't grow properly");
+        assert(lmk_match->check(pl_ftr) && "Landmark didn't grow properly");
     }
     // -----------------CHANGE Back-----------------
     // Change last not defined point if landmark last not defined matched with last feature point that:
@@ -605,13 +613,6 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
     // b. would extend the line (check if angle is bigger than 90º)
     else if (lmk_match->landmark_to_id_ == pl_lmk->getLastId() && !pl_lmk->isLastDefined()) //&& pl_match->feature_to_id_ == pl_ftr->getNPoints()-1
     {
-        if (print)
-        {
-            std::cout << "Change last point. Defined: " << (pl_ftr->isLastDefined() ? 1 : 0) << std::endl;
-            std::cout << "\tpoint " << pl_lmk->getLastId() << ": " << pl_lmk->getPointVector(pl_lmk->getLastId()).transpose() << std::endl;
-            std::cout << "\tpoint " << pl_lmk->getLastId()-1 << ": " << pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId())).transpose() << std::endl;
-            std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl;
-        }
         if (// case a
             pl_ftr->isLastDefined() ||
             // case b
@@ -619,18 +620,29 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
             (points_global.topRightCorner(2,1)-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm() +
             (pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId()))-pl_lmk->getPointVector(pl_lmk->getLastId())).squaredNorm())
         {
+            if (print)
+            {
+                std::cout << "Change last point. Defined: " << (pl_ftr->isLastDefined() ? 1 : 0) << std::endl;
+                std::cout << "\tpoint " << pl_lmk->getLastId() << ": " << pl_lmk->getPointVector(pl_lmk->getLastId()).transpose() << std::endl;
+                std::cout << "\tpoint " << pl_lmk->getLastId()-1 << ": " << pl_lmk->getPointVector(pl_lmk->getPrevValidId(pl_lmk->getLastId())).transpose() << std::endl;
+                std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl;
+            }
+
             pl_lmk->setLast(points_global.rightCols(1), pl_ftr->isLastDefined());
             lmk_match->landmark_version_ = pl_lmk->getVersion();
             modified = true;
-        }
 
-        // checks
-        assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
-        assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
-        assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
-        assert(lmk_match->check() && "Landmark didn't grow properly");
+            // checks
+            assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
+            assert(lmk_match->landmark_to_id_ == pl_lmk->getLastId() && "Landmark didn't grow properly");
+            assert(lmk_match->check(pl_ftr) && "Landmark didn't grow properly");
+        }
     }
+    // all checks
+    assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
     assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
+    assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
+    assert(lmk_match->check(pl_ftr) && "Landmark didn't grow properly (should have stopped in a previous assert)");
 
     if (print)
     {
@@ -651,11 +663,11 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
 void ProcessorTrackerFeaturePolyline2D::advanceDerived()
 {
     WOLF_DEBUG("PTFP ", getName(), "::advanceDerived: ");
-    // remove last features from landmark_match_map_
+    // remove landmark matches with last features
     if (last_ptr_ != nullptr)
         for (auto ftr : last_ptr_->getFeatureList())
         {
-            assert(landmark_match_map_.find(ftr) != landmark_match_map_.end());
+            assert(landmark_match_map_.count(ftr));
             landmark_match_map_.erase(ftr);
         }
 
@@ -680,11 +692,11 @@ void ProcessorTrackerFeaturePolyline2D::advanceDerived()
 void ProcessorTrackerFeaturePolyline2D::resetDerived()
 {
     WOLF_DEBUG("PTFP ", getName(), "::resetDerived: ");
-    // remove last features from landmark_match_map_
+    // remove landmark matches with last features
     if (last_ptr_ != nullptr)
         for (auto ftr : last_ptr_->getFeatureList())
         {
-            assert(landmark_match_map_.find(ftr) != landmark_match_map_.end());
+            assert(landmark_match_map_.count(ftr));
             landmark_match_map_.erase(ftr);
         }
 
@@ -824,6 +836,10 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
         ftr_match->normalized_score_ = best_match->normalized_score_;
         // feature correspondence
         ftr_match->feature_ptr_ = _ftr_L;
+
+        assert(ftr_match->feature_incoming_from_id_ == 0 || ftr_match->feature_last_from_id_ == 0);
+        assert(ftr_match->feature_incoming_to_id_ == _ftr_I->getNPoints() -1 || ftr_match->feature_incoming_to_id_ == _ftr_L->getNPoints() -1);
+
         // store in list
         ftr_matches[ftr_match->normalized_score_] = ftr_match;
         WOLF_DEBUG("match stored!");
@@ -927,11 +943,12 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
 
         assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
         assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
+        assert(lmk_match->feature_from_id_ == 0 || lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1);
 
         // store in list
         lmk_matches[lmk_match->normalized_score_]= lmk_match;
         WOLF_DEBUG("match stored!");
-        assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
+        assert(lmk_match->check(_feat_ptr) && "tryMatchWithLandmark: wrong match");
     }
 
 //    // compute best rigid transformation matching
@@ -982,7 +999,7 @@ bool ProcessorTrackerFeaturePolyline2D::updateMatchWithLandmark(LandmarkMatchPol
                                                                 LandmarkPolyline2DPtr& _lmk_ptr,
                                                                 FeaturePolyline2DPtr& _feat_ptr) const
 {
-    assert(_lmk_match->check() && "updateMatchWithLandmark: input _lmk_match wrong");
+    assert(_lmk_match->check(_feat_ptr) && "updateMatchWithLandmark: input _lmk_match wrong");
     WOLF_DEBUG("PTFP ", getName(), "::updateMatchWithLandmark: ");
     assert(_lmk_ptr->id() == _lmk_match->landmark_ptr_->id());
 
@@ -1002,8 +1019,8 @@ bool ProcessorTrackerFeaturePolyline2D::updateMatchWithLandmark(LandmarkMatchPol
     }
 
     // valid match: update landmark match of last
-    assert(_lmk_match->check() && "updateMatchWithLandmark: wrongly updated");
-    _lmk_match = new_lmk_matches.begin()->second; // the frist is the one with smallest difference from movement of the old match
+    assert(_lmk_match->check(_feat_ptr) && "updateMatchWithLandmark: wrongly updated");
+    _lmk_match = new_lmk_matches.begin()->second; // the first is the one with smallest difference from movement of the old match
     return true;
 }
 
@@ -1011,7 +1028,7 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature
                                                                                                 FeatureMatchPolyline2DPtr ftr_match,
                                                                                                 LandmarkMatchPolyline2DPtr lmk_match_last) const
 {
-    assert(lmk_match_last->check() && "input lmk_match_last wrong");
+    assert(lmk_match_last->check(ftr_match->feature_ptr_) && "input lmk_match_last wrong");
     WOLF_DEBUG("PTFP ", getName(), "::concatenateFeatureLandmarkMatches: ");
     std::cout << "ftr_match:";
     std::cout << "\n\tincoming_from:    " << ftr_match->feature_incoming_from_id_;
@@ -1075,7 +1092,7 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature
     lmk_match_incoming->normalized_score_ = ftr_match->normalized_score_;
     lmk_match_incoming->T_feature_landmark_ = ftr_match->T_incoming_last_ * lmk_match_last->T_feature_landmark_;
 
-    assert(lmk_match_incoming->check() && "lmk_match_incoming wrongly concatenated");
+    assert(lmk_match_incoming->check(ftr_match->feature_ptr_) && "lmk_match_incoming wrongly concatenated");
 
     return lmk_match_incoming;
 }
-- 
GitLab


From 71724149c0fd2c9c66602c540f3abf6db800275b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 21 Jun 2019 10:16:39 +0200
Subject: [PATCH 7/9] developing correctFeatureDrift

---
 .../processor_tracker_feature_polyline_2D.cpp | 27 ++++++++++++++++---
 1 file changed, 24 insertions(+), 3 deletions(-)

diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index ce350fc5f..b6440603d 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -143,9 +143,30 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
                                                             FeatureBasePtr _incoming_feature)
 {
     //WOLF_DEBUG("PTFP ", getName(), "::correctFeatureDrift: ");
-    // If incoming observed new landmark points & landmark has points
-    // Check if match is still valid and update the match
-    // TODO
+    /** After finding a valid feature match last-incoming, we have to check that the concatenated match
+     *  incoming-landmark is also valid.
+     *
+     *  We will only have to check IN BOTH EXTREMES if:
+     *  1. There are exceeding points in incoming-last match.
+     *  AND
+     *  2. Landmark has exceeding points in the same extreme OR is closed.
+     *
+     *  It this is the case, we have to check that the exceeding points match with the corresponding existing landmark points.
+     */
+
+    assert(matches_last_from_incoming_.count(_incoming_feature) != 0);
+    assert(landmark_match_map_.count(_last_feature) != 0);
+
+    auto ftr_match      = std::static_pointer_cast<FeatureMatchPolyline2D>(matches_last_from_incoming_[_incoming_feature]);
+    auto lmk_last_match = std::static_pointer_cast<LandmarkMatchPolyline2D>(landmark_match_map_[_last_feature]);
+    auto pl_lmk         = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[_last_feature]->landmark_ptr_);
+
+    // FRONT POINTS
+    if (ftr_match->feature_incoming_from_id_ != 0 &&
+        (lmk_last_match->landmark_from_id_ > pl_lmk->getFirstId() || pl_lmk->isClosed()))
+    {
+
+    }
 
 //    int& new_first_points = best_ftr_match->feature_incoming_from_id_;
 //    int  new_last_points  = pl_incoming->getNPoints() - best_ftr_match->feature_incoming_to_id_ - 1;
-- 
GitLab


From 826df69d1a5619724eb42d11849187d5c3725dc3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 21 Jun 2019 15:02:51 +0200
Subject: [PATCH 8/9] implementing correctFeatureDrift

---
 .../processor_tracker_feature_polyline_2D.cpp | 36 +++++++++++++------
 1 file changed, 26 insertions(+), 10 deletions(-)

diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index b6440603d..b28e0e43e 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -155,22 +155,38 @@ bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr
      */
 
     assert(matches_last_from_incoming_.count(_incoming_feature) != 0);
-    assert(landmark_match_map_.count(_last_feature) != 0);
+    assert(landmark_match_map_.count(_incoming_feature) != 0);
 
-    auto ftr_match      = std::static_pointer_cast<FeatureMatchPolyline2D>(matches_last_from_incoming_[_incoming_feature]);
-    auto lmk_last_match = std::static_pointer_cast<LandmarkMatchPolyline2D>(landmark_match_map_[_last_feature]);
-    auto pl_lmk         = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[_last_feature]->landmark_ptr_);
+    auto lmk_inc_match = std::static_pointer_cast<LandmarkMatchPolyline2D>(landmark_match_map_[_incoming_feature]);
+    auto pl_lmk        = std::static_pointer_cast<LandmarkPolyline2D>(lmk_inc_match->landmark_ptr_);
 
     // FRONT POINTS
-    if (ftr_match->feature_incoming_from_id_ != 0 &&
-        (lmk_last_match->landmark_from_id_ > pl_lmk->getFirstId() || pl_lmk->isClosed()))
+    while (lmk_inc_match->feature_from_id_ != 0 &&
+           (lmk_inc_match->landmark_from_id_ > pl_lmk->getFirstId() || pl_lmk->isClosed()))
     {
+        auto exceeding_inc = lmk_inc_match->feature_from_id_-1;
+        auto exceeding_lmk = pl_lmk->getPrevValidId(lmk_inc_match->landmark_from_id_);
 
-    }
+        // check good overlapping
+        Eigen::Vector3s lmk_point_expected(Eigen::Vector3s::Ones());
+        lmk_point_expected.head(2) = pl_lmk->getPointVector(exceeding_lmk);
+        Eigen::Vector3s ftr_point = std::static_pointer_cast<FeaturePolyline2D>(_incoming_feature)->getPoints().col(exceeding_inc);
+
+        Scalar sq_dist = (lmk_point_expected.head(2) - ftr_point.head(2)).squaredNorm();
 
-//    int& new_first_points = best_ftr_match->feature_incoming_from_id_;
-//    int  new_last_points  = pl_incoming->getNPoints() - best_ftr_match->feature_incoming_to_id_ - 1;
-//    int overlapping  = best_ftr_match->feature_incoming_to_id_ - best_ftr_match->feature_incoming_from_id_;
+        // exceeding point not matched -> return not succeed
+        if (sq_dist > params_tracker_feature_polyline_->match_landmark_position_sq_norm_th)
+        {
+            WOLF_WARN("correctFeatureDrift: any point didn't match while checking incoming-lmk front exceeding point: feature point ", exceeding_inc, " - landmark point ", exceeding_lmk);
+            return false;
+        }
+        // add exceeding point to the landmark match
+        else
+        {
+            lmk_inc_match->feature_from_id_  = exceeding_inc;
+            lmk_inc_match->landmark_from_id_ = exceeding_lmk;
+        }
+    }
 
     return true;
 }
-- 
GitLab


From 9a82e395f4288f4410ed33a8b9afc97f90ee80db Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 21 Jun 2019 15:03:01 +0200
Subject: [PATCH 9/9] probabilistic matching

---
 .../processor_tracker_feature_polyline_2D.h   |   8 +-
 .../processor_tracker_feature_polyline_2D.cpp | 130 +++++++++++++++---
 2 files changed, 113 insertions(+), 25 deletions(-)

diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h
index 918a17961..9d4b448a4 100644
--- a/include/laser/processor/processor_tracker_feature_polyline_2D.h
+++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h
@@ -37,9 +37,11 @@ typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScala
 struct ProcessorParamsTrackerFeaturePolyline2D : public ProcessorParamsTrackerFeature
 {
     laserscanutils::LineFinderIterativeParams line_finder_params;
-    Scalar match_alignment_position_sq_norm_th;
-    Scalar match_landmark_pose_sq_norm_th;
-    Scalar match_feature_pose_sq_norm_th;
+    Scalar match_feature_position_sq_norm_th;
+    Scalar match_feature_orientation_sq_norm_th;
+    Scalar match_landmark_position_sq_norm_th;
+    Scalar match_landmark_orientation_sq_norm_th;
+    bool use_probabilistic_match;
     Scalar class_position_error_th;
     unsigned int new_features_th;
     Scalar loop_time_th;
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index b28e0e43e..d2d71a9a8 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -833,17 +833,60 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
 {
     //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithFeature: ");
 
-    // compute best sq distance matching
-    Eigen::MatrixXs last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
-    MatchPolyline2DPtr best_match = computeBestSqDist(_ftr_I->getPoints(),       // <--feature incoming points
-                                                      _ftr_I->isFirstDefined(),
-                                                      _ftr_I->isLastDefined(),
-                                                      false,                     // <--feature is not closed for sure
-                                                      last_expected_points,      // <--feature last points
-                                                      _ftr_L->isFirstDefined(),
-                                                      _ftr_L->isLastDefined(),
-                                                      false,                     // <--feature is not closed for sure
-                                                      params_tracker_feature_polyline_->match_feature_pose_sq_norm_th);
+    MatchPolyline2DPtr best_match = nullptr;
+
+    if (params_tracker_feature_polyline_->use_probabilistic_match)
+    {
+        // Compute Covariance of projected points (only considering noise in T_last_incoming) and computing Jacobian using the mean of the points
+        Eigen::Vector2s mean_last_points = _ftr_L->getPoints().rowwise().mean().head(2);
+        Eigen::Vector2s J_th;
+        Scalar cos_th = _T_last_incoming_prior(0,0);
+        Scalar sin_th = _T_last_incoming_prior(1,0);
+        J_th << -sin_th * mean_last_points(0) - cos_th * mean_last_points(1),
+                 cos_th * mean_last_points(0) - sin_th * mean_last_points(1);
+        Eigen::Matrix2s Sigma = Eigen::Matrix2s::Identity()*params_tracker_feature_polyline_->match_feature_position_sq_norm_th +
+                                J_th*params_tracker_feature_polyline_->match_feature_orientation_sq_norm_th*J_th.transpose();
+
+        // sq root of inverse
+        Eigen::Matrix2s sqrtOmega = Sigma.llt().matrixL().inverse();
+
+        // last expected points
+        Eigen::Matrix<Scalar, 3, Eigen::Dynamic> last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
+
+        // Shape points in the equiprobabilistic space
+        Eigen::Matrix<Scalar, 3, Eigen::Dynamic> last_shaped = last_expected_points;
+        Eigen::Matrix<Scalar, 3, Eigen::Dynamic> inc_shaped  = _ftr_I->getPoints();
+        last_shaped.topRows<2>() = sqrtOmega*last_shaped.topRows<2>();
+        inc_shaped.topRows<2>()  = sqrtOmega*inc_shaped.topRows<2>();
+
+        // compute best sq Mahalanobis distance matching
+        best_match = computeBestSqDist(inc_shaped, // <--feature incoming points
+                                       _ftr_I->isFirstDefined(),
+                                       _ftr_I->isLastDefined(),
+                                       false,                     // <--feature is not closed for sure
+                                       last_shaped, // <--feature last points
+                                       _ftr_L->isFirstDefined(),
+                                       _ftr_L->isLastDefined(),
+                                       false,                     // <--feature is not closed for sure
+                                       1);
+    }
+    else // not probabilistic match
+    {
+        // last expected points
+        Eigen::Matrix<Scalar, 3, Eigen::Dynamic> last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
+
+        // compute best sq Mahalanobis distance matching
+        best_match = computeBestSqDist(_ftr_I->getPoints(),       // <--feature incoming points
+                                       _ftr_I->isFirstDefined(),
+                                       _ftr_I->isLastDefined(),
+                                       false,                     // <--feature is not closed for sure
+                                       last_expected_points,      // <--feature last points
+                                       _ftr_L->isFirstDefined(),
+                                       _ftr_L->isLastDefined(),
+                                       false,                     // <--feature is not closed for sure
+                                       params_tracker_feature_polyline_->match_feature_position_sq_norm_th);
+    }
+
     //valid match
     if (best_match != nullptr)
     {
@@ -932,17 +975,60 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
 {
     //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: ");
 
-    // compute best sq distance matching
-    Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
-    MatchPolyline2DPtr best_match = computeBestSqDist(lmk_expected_points,  // <--landmark points in local coordinates
-                                                      _lmk_ptr->isFirstDefined(),
-                                                      _lmk_ptr->isLastDefined(),
-                                                      _lmk_ptr->isClosed(),
-                                                      _feat_ptr->getPoints(),        // <--feature points
-                                                      _feat_ptr->isFirstDefined(),
-                                                      _feat_ptr->isLastDefined(),
-                                                      false,                      // <--feature is not closed for sure
-                                                      params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th);
+    MatchPolyline2DPtr best_match = nullptr;
+
+    if (params_tracker_feature_polyline_->use_probabilistic_match)
+    {
+        // Compute Covariance of projected points (only considering noise in _T_feature_landmark_prior) and computing Jacobian using the mean of the points
+        Eigen::Vector2s mean_last_points = _lmk_ptr->computePointsGlobal().rowwise().mean().head(2);
+        Eigen::Vector2s J_th;
+        Scalar cos_th = _T_feature_landmark_prior(0,0);
+        Scalar sin_th = _T_feature_landmark_prior(1,0);
+        J_th << -sin_th * mean_last_points(0) - cos_th * mean_last_points(1),
+                 cos_th * mean_last_points(0) - sin_th * mean_last_points(1);
+        Eigen::Matrix2s Sigma = Eigen::Matrix2s::Identity()*params_tracker_feature_polyline_->match_feature_position_sq_norm_th +
+                                J_th*params_tracker_feature_polyline_->match_feature_orientation_sq_norm_th*J_th.transpose();
+
+        // sq root of inverse
+        Eigen::Matrix2s sqrtOmega = Sigma.llt().matrixL().inverse();
+
+        // landmark expected points
+        Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
+
+        // Shape points in the equiprobabilistic space
+        Eigen::Matrix<Scalar, 3, Eigen::Dynamic> lmk_shaped = lmk_expected_points;
+        Eigen::Matrix<Scalar, 3, Eigen::Dynamic> feat_shaped  = _feat_ptr->getPoints();
+        lmk_shaped.topRows<2>() = sqrtOmega*lmk_shaped.topRows<2>();
+        feat_shaped.topRows<2>()  = sqrtOmega*feat_shaped.topRows<2>();
+
+        // compute best sq Mahalanobis distance matching
+        best_match = computeBestSqDist(lmk_shaped, // <--landmark points in local coordinates
+                                       _lmk_ptr->isFirstDefined(),
+                                       _lmk_ptr->isLastDefined(),
+                                       _lmk_ptr->isClosed(),
+                                       feat_shaped,             // <--feature last points
+                                       _feat_ptr->isFirstDefined(),
+                                       _feat_ptr->isLastDefined(),
+                                       false,                     // <--feature is not closed for sure
+                                       1);
+    }
+    else // not probabilistic match
+    {
+        // landmark expected points
+        Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
+
+        // compute best sq distance matching
+        best_match = computeBestSqDist(lmk_expected_points,  // <--landmark points in local coordinates
+                                       _lmk_ptr->isFirstDefined(),
+                                       _lmk_ptr->isLastDefined(),
+                                       _lmk_ptr->isClosed(),
+                                       _feat_ptr->getPoints(),        // <--feature points
+                                       _feat_ptr->isFirstDefined(),
+                                       _feat_ptr->isLastDefined(),
+                                       false,                      // <--feature is not closed for sure
+                                       params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th);
+    }
+
     //valid match
     if (best_match != nullptr)
     {
-- 
GitLab