Merge branch '5-adapt-to-the-new-emplace-api' into 'devel'

Resolve "Adapt to the new emplace() API"

Closes #5

See merge request !7

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"

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

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;
 }
 

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;
@@ -52,7 +54,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_;
@@ -75,13 +77,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 +116,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 +216,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;
@@ -200,7 +236,7 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
 
         const FeatureBasePtrList& getLastNewFeatures() const
         {
-            return all_features_last_;
+            return untracked_features_last_;
         }
 };
 

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;
 }