BEGONE ONCE AND FOR ALL PROCESSOR_POLYLINE

CMakeLists.txt

@@ -181,7 +181,6 @@ SET(HDRS_LANDMARK
   )
 SET(HDRS_PROCESSOR
   include/laser/processor/polyline_2D_utils.h
-  include/laser/processor/processor_polyline.h
   include/laser/processor/processor_tracker_feature_polyline_2D.h
   )
 SET(HDRS_SENSOR
@@ -216,9 +215,7 @@ SET(SRCS_LANDMARK
   )
 SET(SRCS_PROCESSOR
   src/processor/polyline_2D_utils.cpp
-  src/processor/processor_polyline.cpp
   src/processor/processor_tracker_feature_polyline_2D.cpp
-  src/processor/processor_polyline.cpp
   )
 SET(SRCS_SENSOR
   src/sensor/sensor_laser_2D.cpp

include/laser/processor/processor_polyline.h

-/*
- * processor_polyline.h
- *
- *  Created on: Sep 14, 2017
- *      Author: jvallve
- */
-
-#ifndef SRC_PROCESSOR_POLYLINE_H_
-#define SRC_PROCESSOR_POLYLINE_H_
-
-// Wolf includes
-#include "laser/sensor/sensor_laser_2D.h"
-#include "laser/capture/capture_laser_2D.h"
-#include "laser/feature/feature_polyline_2D.h"
-#include "core/landmark/landmark_match.h"
-#include "laser/landmark/landmark_polyline_2D.h"
-#include "laser/factor/factor_point_2D.h"
-#include "laser/factor/factor_point_to_line_2D.h"
-#include "core/state_block/state_block.h"
-#include "core/processor/processor_base.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(ProcessorParamsPolyline);
-WOLF_PTR_TYPEDEFS(ProcessorPolyline);
-
-struct ProcessorParamsPolyline : public ProcessorParamsBase
-{
-        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;
-        Scalar position_error_th_;
-        Scalar min_features_ratio_th_;
-};
-
-class ProcessorPolyline : public ProcessorBase
-{
-    protected:
-        CaptureBasePtr last_ptr_;               ///< Pointer to the last tracked capture.
-        CaptureBasePtr incoming_ptr_;           ///< Pointer to the incoming capture being processed.
-        laserscanutils::LineFinderIterative line_finder_;
-
-        ProcessorParamsPolylinePtr params_;
-
-        FeatureBasePtrList new_features_incoming_, new_features_last_, known_features_incoming_, known_features_last_;
-        LandmarkBasePtrList new_landmarks_;        ///< List of new detected landmarks
-        LandmarkMatchMap matches_landmark_from_incoming_;
-        LandmarkMatchMap matches_landmark_from_last_;
-
-        std::list<LandmarkPolyline2DPtrList> merge_candidates_list_;
-
-        Eigen::Matrix2s R_sensor_world_, R_world_sensor_;
-        Eigen::Matrix2s R_robot_sensor_;
-        Eigen::Matrix2s R_current_prev_;
-        Eigen::Vector2s t_sensor_world_, t_world_sensor_, t_world_sensor_prev_, t_sensor_world_prev_;
-        Eigen::Vector2s t_robot_sensor_;
-        Eigen::Vector2s t_current_prev_;
-        Eigen::Vector2s t_world_robot_;
-        bool extrinsics_transformation_computed_;
-
-
-    public:
-        EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
-        ProcessorPolyline(ProcessorParamsPolylinePtr _params);
-
-        virtual ~ProcessorPolyline();
-
-        virtual void configure(SensorBasePtr _sensor) { };
-
-        // Algorithm
-        virtual void process(CaptureBasePtr _capture_ptr);
-
-        void processNew();
-
-        virtual bool voteForKeyFrame();
-
-        // Implementation
-        void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list);
-        unsigned int findLandmarks(const LandmarkBasePtrList& _landmark_list_in, FeatureBasePtrList& _feature_list_out, LandmarkMatchMap& _feature_landmark_correspondences);
-
-        virtual void createNewLandmarks();
-        virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
-
-        virtual void establishFactors();
-        void emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
-                                          LandmarkPolyline2DPtr _polyline_landmark,
-                                          const int& _ftr_point_id,
-                                          const int& _lmk_point_id,
-                                          const int& _lmk_prev_point_id);
-        void emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
-                                    LandmarkPolyline2DPtr _polyline_landmark,
-                                    const int& _ftr_point_id,
-                                    const int& _lmk_point_id);
-        bool rejectOutlier(FactorBasePtr& fac_ptr);
-        void classifyPolylineLandmarks(LandmarkBasePtrList& _lmk_list);
-        LandmarkMatchPolyline2DPtr tryMatch(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr);
-        LandmarkMatchPolyline2DPtr tryMatch(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
-
-        // Gets/Sets
-        const FeatureBasePtrList& getLastNewPolylines() const;
-        const FeatureBasePtrList& getLastKnownPolylines() const;
-        virtual void setKeyFrame(CaptureBasePtr _capture_ptr);
-        virtual CaptureBasePtr getLast();
-
-        // Maths
-        void computeTransformations(const TimeStamp& _ts);
-        void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
-                             Eigen::MatrixXs& expected_feature_cov_);
-
-    // Factory method
-    public:
-        static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-};
-
-inline void ProcessorPolyline::emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
-                                                        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);
-}
-
-inline void ProcessorPolyline::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
-                                                 LandmarkPolyline2DPtr _polyline_landmark,
-                                                 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);
-}
-
-inline const FeatureBasePtrList& ProcessorPolyline::getLastNewPolylines() const
-{
-    return new_features_last_;
-}
-
-inline const FeatureBasePtrList& ProcessorPolyline::getLastKnownPolylines() const
-{
-    if (last_ptr_->getFrame()->isKey())
-        return last_ptr_->getFeatureList();
-    else
-        return known_features_last_;
-}
-
-inline CaptureBasePtr ProcessorPolyline::getLast()
-{
-    return last_ptr_;
-}
-
-} /* namespace wolf */
-
-#endif /* SRC_PROCESSOR_POLYLINE_H_ */