From 1e1826a4c0c6899f1eb08a1792bc0ce94ad19b88 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Mon, 11 Mar 2019 16:35:48 +0100
Subject: [PATCH] new processor for polylines WIP

---
 CMakeLists.txt                                |   2 +
 .../processor_tracker_feature_polyline.h      | 110 ++++++++++++++++++
 .../processor_tracker_feature_polyline.cpp    |  72 ++++++++++++
 3 files changed, 184 insertions(+)
 create mode 100644 include/base/processor/processor_tracker_feature_polyline.h
 create mode 100644 src/processor/processor_tracker_feature_polyline.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index c52edc232..8e7d93a2b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -414,6 +414,7 @@ SET(HDRS_PROCESSOR
   include/base/processor/processor_tracker.h
   include/base/processor/processor_gnss_fix.h
   include/base/processor/processor_gnss_single_diff.h
+  include/base/processor/processor_tracker_feature_polyline.h
   )
 SET(HDRS_SENSOR
   include/base/sensor/sensor_base.h
@@ -566,6 +567,7 @@ SET(SRCS_PROCESSOR
   src/processor/processor_tracker_landmark.cpp
   src/processor/processor_gnss_fix.cpp
   src/processor/processor_gnss_single_diff.cpp
+  src/processor/processor_tracker_feature_polyline.cpp
   )
 SET(SRCS_SENSOR
   src/sensor/sensor_camera.cpp
diff --git a/include/base/processor/processor_tracker_feature_polyline.h b/include/base/processor/processor_tracker_feature_polyline.h
new file mode 100644
index 000000000..f7a3f034a
--- /dev/null
+++ b/include/base/processor/processor_tracker_feature_polyline.h
@@ -0,0 +1,110 @@
+/*
+ * processor_tracker_feature_polyline.h
+ *
+ *  Created on: Mar 11, 2019
+ *      Author: jvallve
+ */
+
+#ifndef PROCESSOR_TRACKER_FEATURE_POLYLINE_H_
+#define PROCESSOR_TRACKER_FEATURE_POLYLINE_H_
+
+#include "base/processor/processor_tracker_feature.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline);
+
+struct ProcessorParamsTrackerFeaturePolyline : public ProcessorParamsTrackerFeature
+{
+    //
+};
+
+class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
+{
+    protected:
+        ProcessorParamsTrackerFeaturePolylinePtr params_;
+
+    public:
+        ProcessorTrackerFeaturePolyline(ProcessorParamsTrackerFeaturePolylinePtr _params);
+
+        virtual ~ProcessorTrackerFeaturePolyline();
+
+    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
+         * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+         */
+        virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in,
+                                           FeatureBaseList& _features_incoming_out,
+                                           FeatureMatchMap& _feature_correspondences);
+
+        /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
+         * \param _origin_feature input feature in origin capture tracked
+         * \param _incoming_feature input/output feature in incoming capture to be corrected
+         * \return false if the the process discards the correspondence with origin's feature
+         */
+        virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
+                                         const FeatureBasePtr _last_feature,
+                                         FeatureBasePtr _incoming_feature);
+
+        /** \brief Vote for KeyFrame generation
+         *
+         * If a KeyFrame criterion is validated, this function returns true,
+         * meaning that it wants to create a KeyFrame at the \b last Capture.
+         *
+         * WARNING! This function only votes! It does not create KeyFrames!
+         */
+        virtual bool voteForKeyFrame();
+
+        /** \brief Detect new Features
+         * \param _max_features maximum number of features detected
+         * \return The number of detected Features.
+         *
+         * This function detects Features that do not correspond to known Features/Landmarks in the system.
+         *
+         * The function sets _features_last_out, the list of newly detected features in Capture last.
+         */
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features,
+                                               FeatureBaseList& _features_last_out);
+
+        /** \brief Create a new constraint and link it to the wolf tree
+         * \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 constraint of the appropriate type for the derived processor.
+         */
+        virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr,
+                                                   FeatureBasePtr _feature_other_ptr);
+
+        /** \brief Pre-process
+         *
+         */
+        virtual void preProcess() override;
+
+        /** \brief Post-process
+         *
+         */
+        virtual void postProcess() override;
+
+    public:
+
+        /// @brief Factory method
+        static ProcessorBasePtr create(const std::string& _unique_name,
+                                       const ProcessorParamsBasePtr _params,
+                                       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_ */
diff --git a/src/processor/processor_tracker_feature_polyline.cpp b/src/processor/processor_tracker_feature_polyline.cpp
new file mode 100644
index 000000000..d6bbcce31
--- /dev/null
+++ b/src/processor/processor_tracker_feature_polyline.cpp
@@ -0,0 +1,72 @@
+/*
+ * processor_tracker_feature_polyline.cpp
+ *
+ *  Created on: Mar 11, 2019
+ *      Author: jvallve
+ */
+
+#include "base/processor/processor_tracker_feature_polyline.h"
+
+namespace wolf
+{
+
+ProcessorTrackerFeaturePolyline::ProcessorTrackerFeaturePolyline(ProcessorParamsTrackerFeaturePolylinePtr _params) :
+     ProcessorTrackerFeature("TRACKER FEATURE POLYLINES",_params),
+     params_(_params)
+{
+    // TODO Auto-generated constructor stub
+
+}
+
+ProcessorTrackerFeaturePolyline::~ProcessorTrackerFeaturePolyline()
+{
+    // TODO Auto-generated destructor stub
+}
+
+unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseList& _features_last_in,
+                                                            FeatureBaseList& _features_incoming_out,
+                                                            FeatureMatchMap& _feature_correspondences)
+{
+    return 0;
+}
+
+bool ProcessorTrackerFeaturePolyline::voteForKeyFrame()
+{
+    return false;
+}
+
+unsigned int ProcessorTrackerFeaturePolyline::detectNewFeatures(const unsigned int& _max_new_features,
+                                                                FeatureBaseList& _features_last_out)
+{
+    return 0;
+}
+
+ConstraintBasePtr ProcessorTrackerFeaturePolyline::createConstraint(FeatureBasePtr _feature_ptr,
+                                                                    FeatureBasePtr _feature_other_ptr)
+{
+    return nullptr;
+}
+
+ProcessorBasePtr ProcessorTrackerFeaturePolyline::create(const std::string& _unique_name,
+                                                         const ProcessorParamsBasePtr _params,
+                                                         const SensorBasePtr _sensor_ptr)
+{
+  const auto params = std::static_pointer_cast<ProcessorParamsTrackerFeaturePolyline>(_params);
+
+  ProcessorBasePtr prc_ptr = std::make_shared<ProcessorTrackerFeaturePolyline>(params);
+  prc_ptr->setName(_unique_name);
+  prc_ptr->configure(_sensor_ptr);
+  return prc_ptr;
+}
+
+void ProcessorTrackerFeaturePolyline::preProcess()
+{
+
+}
+
+void ProcessorTrackerFeaturePolyline::postProcess()
+{
+
+}
+
+} /* namespace wolf */
-- 
GitLab