WIP

include/base/feature/feature_polyline_2D.h

@@ -13,6 +13,7 @@
 namespace wolf
 {
 WOLF_PTR_TYPEDEFS(FeaturePolyline2D);
+WOLF_LIST_TYPEDEFS(FeaturePolyline2D);
 
 //class
 class FeaturePolyline2D : public FeatureBase

include/base/processor/processor_tracker_feature_polyline.h

@@ -9,21 +9,81 @@
 #define PROCESSOR_TRACKER_FEATURE_POLYLINE_H_
 
 #include "base/processor/processor_tracker_feature.h"
+#include "base/landmark/landmark_match.h"
+#include "base/landmark/landmark_polyline_2D.h"
+#include "base/sensor/sensor_laser_2D.h"
+#include "base/capture/capture_laser_2D.h"
+#include "base/feature/feature_polyline_2D.h"
+#include "base/constraint/constraint_point_2D.h"
+#include "base/constraint/constraint_point_to_line_2D.h"
+
+//laser_scan_utils
+#include "laser_scan_utils/laser_scan.h"
+#include "laser_scan_utils/line_finder_iterative.h"
+#include "laser_scan_utils/polyline.h"
 
 namespace wolf
 {
 
 WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline);
+WOLF_STRUCT_PTR_TYPEDEFS(FeaturePolylineMatch);
+WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline);
 
 struct ProcessorParamsTrackerFeaturePolyline : public ProcessorParamsTrackerFeature
 {
-    //
+        laserscanutils::LineFinderIterativeParams line_finder_params;
+        Scalar match_position_error_th;
+        Scalar class_position_error_th;
+        unsigned int new_features_th;
+        Scalar loop_time_th;
+        std::vector<PolylineRectangularClass> polyline_classes;
+
+        // These values below are constant and defined within the class -- provide a setter or accept them at construction time if you need to configure them
+        Scalar aperture_error_th_;// = 20.0 * M_PI / 180.; //20 degrees
+        Scalar angular_error_th_;// = 10.0 * M_PI / 180.; //10 degrees;
+        Scalar position_error_th_;// = 1;
+        Scalar min_features_ratio_th_;// = 0.5;
+};
+
+// Match Feature - Feature
+struct FeaturePolylineMatch : public FeatureMatch
+{
+    int feature_last_from_id_;
+    int feature_last_to_id_;
+    int feature_incoming_from_id_;
+    int feature_incoming_to_id_;
+};
+
+// Match Feature - Landmark
+WOLF_STRUCT_PTR_TYPEDEFS(LandmarkPolylineMatch);
+typedef std::map<FeatureBasePtr, LandmarkPolylineMatchPtr> LandmarkPolylineMatchMap;
+struct LandmarkPolylineMatch : public LandmarkMatch
+{
+    int landmark_from_id_;
+    int feature_from_id_;
+    int landmark_to_id_;
+    int feature_to_id_;
+    int landmark_version_;
 };
 
 class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
 {
     protected:
-        ProcessorParamsTrackerFeaturePolylinePtr params_;
+        ProcessorParamsTrackerFeaturePolylinePtr params_tracker_feature_polyline_;
+        laserscanutils::LineFinderIterative line_finder_;
+
+        FeatureBaseList all_features_incoming_, all_features_last_;
+        LandmarkPolylineMatchMap landmark_match_map_;
+
+        Eigen::Matrix2s R_sensor_world_last_, R_world_sensor_last_;
+        Eigen::Vector2s t_sensor_world_last_, t_world_sensor_last_;
+        Eigen::Matrix2s R_sensor_world_incoming_, R_world_sensor_incoming_;
+        Eigen::Vector2s t_sensor_world_incoming_, t_world_sensor_incoming_;
+        Eigen::Matrix2s R_robot_sensor_;
+        Eigen::Vector2s t_robot_sensor_;
+        Eigen::Matrix2s R_last_incoming_, R_incoming_last_;
+        Eigen::Vector2s t_last_incoming_, t_incoming_last_;
+        bool extrinsics_transformation_computed_;
 
     public:
         ProcessorTrackerFeaturePolyline(ProcessorParamsTrackerFeaturePolylinePtr _params);
@@ -58,6 +118,11 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
          */
         virtual bool voteForKeyFrame();
 
+        /**\brief Process new Features
+         *
+         */
+        virtual unsigned int processNew(const unsigned int& _max_features);
+
         /** \brief Detect new Features
          * \param _max_features maximum number of features detected
          * \return The number of detected Features.
@@ -78,7 +143,35 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
          * This function creates a constraint of the appropriate type for the derived processor.
          */
         virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr,
-                                                   FeatureBasePtr _feature_other_ptr);
+                                                   FeatureBasePtr _feature_other_ptr){ return nullptr; };
+
+        /** \brief Create a new constraint and link it to the wolf tree
+         * \param _feature_ptr pointer to the feature
+         * \param _landmark_ptr pointer to the landmark.
+         *
+         * This function creates a constraint of the appropriate type for the derived processor.
+         */
+        virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr,
+                                                   LandmarkBasePtr _landmark_ptr) const;
+
+        /** \brief Establish constraints between features in Captures \b last and \b origin
+         */
+        virtual void establishConstraints();
+
+        /** \brief create a landmark from a feature
+         *
+         */
+        virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
+
+        /** \brief advance pointers
+         *
+         */
+        virtual void advanceDerived() override;
+
+        /** \brief reset pointers and match lists at KF creation
+         *
+         */
+        virtual void resetDerived() override;
 
         /** \brief Pre-process
          *
@@ -90,6 +183,16 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
          */
         virtual void postProcess() override;
 
+        FeaturePolylineMatchPtr computeBestMatch(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const;
+
+        FeaturePolylineMatchConstPtr bestMatch(const FeaturePolylineMatchConstPtr& _match_A,
+                                               const FeaturePolylineMatchConstPtr& _match_B) const;
+
+        Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
+                                 const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict) const
+
+        void computeIncomingTransformations(const TimeStamp& _ts);
+
     public:
 
         /// @brief Factory method
@@ -98,13 +201,6 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
                                        const SensorBasePtr _sensor_ptr);
 };
 
-inline bool ProcessorTrackerFeaturePolyline::correctFeatureDrift(const FeatureBasePtr _origin_feature,
-                                                                 const FeatureBasePtr _last_feature,
-                                                                 FeatureBasePtr _incoming_feature)
-{
-    return true;
-}
-
 } /* namespace wolf */
 
 #endif /* PROCESSOR_TRACKER_FEATURE_POLYLINE_H_ */