Skip to content
Snippets Groups Projects
Commit d68a2ad2 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Merge branch 'feature/remove_deprecated_polylines' into devel

parents 02d27021 0c730fbe
No related branches found
No related tags found
1 merge request!250Remove deprecated polyline processor
Pipeline #2756 passed
Stage: build
    Expand all Hide whitespace changes
    Inline Side-by-side
    CMakeLists.txt
    + 0
    − 2
    View file @ d68a2ad2
    ......@@ -603,7 +603,6 @@ IF (laser_scan_utils_FOUND)
    SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
    include/base/processor/processor_tracker_feature_corner.h
    include/base/processor/processor_tracker_landmark_corner.h
    include/base/processor/processor_tracker_landmark_polyline.h
    )
    SET(HDRS_SENSOR ${HDRS_SENSOR}
    include/base/sensor/sensor_laser_2D.h
    ......@@ -612,7 +611,6 @@ IF (laser_scan_utils_FOUND)
    src/sensor/sensor_laser_2D.cpp
    src/processor/processor_tracker_feature_corner.cpp
    src/processor/processor_tracker_landmark_corner.cpp
    src/processor/processor_tracker_landmark_polyline.cpp
    )
    ENDIF(laser_scan_utils_FOUND)
    ......
    include/base/processor/processor_tracker_landmark_polyline.h deleted 100644 → 0
    + 0
    − 248
    View file @ 02d27021
    /*
    * processor_tracker_landmark_polyline.h
    *
    * Created on: May 26, 2016
    * Author: jvallve
    */
    #ifndef SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_
    #define SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_
    // Wolf includes
    #include "base/sensor/sensor_laser_2D.h"
    #include "base/capture/capture_laser_2D.h"
    #include "base/feature/feature_polyline_2D.h"
    #include "base/landmark/landmark_polyline_2D.h"
    #include "base/factor/factor_point_2D.h"
    #include "base/factor/factor_point_to_line_2D.h"
    #include "base/state_block.h"
    #include "base/association/association_tree.h"
    #include "base/processor/processor_tracker_landmark.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
    {
    //some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level
    const Scalar position_error_th_ = 1;
    const Scalar min_features_ratio_th_ = 0.5;
    //forward declaration to typedef class pointers
    struct LandmarkPolylineMatch;
    typedef std::shared_ptr<LandmarkPolylineMatch> LandmarkPolylineMatchPtr;
    //forward declaration to typedef class pointers
    struct ProcessorParamsPolyline;
    typedef std::shared_ptr<ProcessorParamsPolyline> ProcessorParamsPolylinePtr;
    WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkPolyline);
    // Match Feature - Landmark
    struct LandmarkPolylineMatch : public LandmarkMatch
    {
    int landmark_match_from_id_;
    int feature_match_from_id_;
    int landmark_match_to_id_;
    int feature_match_to_id_;
    // std::vector<unsigned int> landmark_points_match_;
    // std::vector<unsigned int> feature_points_match_;
    // std::vector<unsigned int> feature_points_add_front_;
    // std::vector<unsigned int> feature_points_add_back_;
    LandmarkPolylineMatch() :
    landmark_match_from_id_(0),
    feature_match_from_id_(0),
    landmark_match_to_id_(0),
    feature_match_to_id_(0)
    {
    //
    }
    LandmarkPolylineMatch(int _landmark_match_from_id,
    int _feature_match_from_id,
    int _landmark_match_to_id,
    int _feature_match_to_id) :
    landmark_match_from_id_(_landmark_match_from_id),
    feature_match_from_id_(_feature_match_from_id),
    landmark_match_to_id_(_landmark_match_to_id),
    feature_match_to_id_(_landmark_match_to_id)
    {
    //
    }
    };
    WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsPolyline);
    struct ProcessorParamsPolyline : public ProcessorParamsTrackerLandmark
    {
    laserscanutils::LineFinderIterativeParams line_finder_params;
    Scalar position_error_th;
    unsigned int new_features_th;
    unsigned int loop_frames_th;
    // 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;
    };
    class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
    {
    private:
    laserscanutils::LineFinderIterative line_finder_;
    ProcessorParamsPolylinePtr params_;
    FeatureBasePtrList polylines_incoming_;
    FeatureBasePtrList polylines_last_;
    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)
    ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params);
    virtual ~ProcessorTrackerLandmarkPolyline();
    virtual void configure(SensorBasePtr _sensor) { };
    const FeatureBasePtrList& getLastPolylines() const;
    protected:
    virtual void preProcess();
    void computeTransformations(const TimeStamp& _ts);
    virtual void postProcess();
    void advanceDerived();
    void resetDerived();
    /** \brief Find provided landmarks in the incoming capture
    * \param _landmarks_in input list of landmarks to be found in incoming
    * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
    * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
    */
    virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
    LandmarkMatchMap& _feature_landmark_correspondences);
    /** \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 _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
    * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
    * \return The number of detected Features.
    *
    * This function detects Features that do not correspond to known Features/Landmarks in the system.
    *
    * The function sets the member new_features_list_, the list of newly detected features,
    * to be used for landmark initialization.
    */
    virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
    /** \brief Creates a landmark for each of new_features_last_
    **/
    virtual void createNewLandmarks();
    /** \brief Create one landmark
    *
    * Implement in derived classes to build the type of landmark you need for this tracker.
    */
    virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
    /** \brief Establish factors between features in Captures \b last and \b origin
    */
    virtual void establishFactors();
    /** \brief look for known objects in the list of unclassified polylines
    */
    void classifyPolilines(LandmarkBasePtrList& _lmk_list);
    /** \brief Create a new factor
    * \param _feature_ptr pointer to the Feature to constrain
    * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
    *
    * Implement this method in derived classes.
    */
    virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
    private:
    void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list);
    void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
    Eigen::MatrixXs& expected_feature_cov_);
    Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
    const Eigen::Matrix2s& _feature_cov,
    const Eigen::Vector2s& _expected_feature,
    const Eigen::Matrix2s& _expected_feature_cov,
    const Eigen::MatrixXs& _mu);
    Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B,
    bool _A_extreme, bool _B_extreme);
    // Factory method
    public:
    static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
    };
    inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params) :
    ProcessorTrackerLandmark("TRACKER LANDMARK POLYLINE", _params),
    line_finder_(_params->line_finder_params),
    params_(_params),
    extrinsics_transformation_computed_(false)
    {
    }
    inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
    {
    // already computed since each scan is computed in preprocess()
    _features_incoming_out = std::move(polylines_last_);
    return _features_incoming_out.size();
    }
    inline void ProcessorTrackerLandmarkPolyline::advanceDerived()
    {
    //std::cout << "\tProcessorTrackerLandmarkPolyline::advance:" << std::endl;
    //std::cout << "\t\tcorners_last: " << polylines_last_.size() << std::endl;
    //std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl;
    ProcessorTrackerLandmark::advanceDerived();
    for (auto polyline : polylines_last_)
    polyline->remove();
    polylines_last_ = std::move(polylines_incoming_);
    //std::cout << "advanced" << std::endl;
    }
    inline void ProcessorTrackerLandmarkPolyline::resetDerived()
    {
    //std::cout << "\tProcessorTrackerLandmarkPolyline::reset:" << std::endl;
    //std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl;
    //std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl;
    ProcessorTrackerLandmark::resetDerived();
    polylines_last_ = std::move(polylines_incoming_);
    }
    inline const FeatureBasePtrList& ProcessorTrackerLandmarkPolyline::getLastPolylines() const
    {
    return polylines_last_;
    }
    } // namespace wolf
    #endif /* SRC_PROCESSOR_TRACKER_LASER_H_ */
    src/processor/processor_tracker_landmark_polyline.cpp deleted 100644 → 0
    + 0
    − 1020
    View file @ 02d27021
    This diff is collapsed.
    0% or .
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or to comment