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