Added autoconf to processor_tracker_feature_polyline_2D

include/laser/landmark/landmark_polyline_2D.h

 /**
- * \file landmark_poliyline_2D.h
+ * \file landmark_polyline_2D.h
  *
  *  Created on: May 26, 2016
  *      \author: jvallve

include/laser/processor/processor_tracker_feature_polyline_2D.h

@@ -46,6 +46,9 @@ struct ProcessorParamsTrackerFeaturePolyline2D : public ProcessorParamsTrackerFe
     unsigned int new_features_th;
     Scalar loop_time_th;
     std::vector<PolylineRectangularClass> polyline_classes;
+    ProcessorParamsTrackerFeaturePolyline2D() = default;
+    ProcessorParamsTrackerFeaturePolyline2D(std::string _unique_name, const paramsServer& _server);
+    std::string print();
 };
 
 class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
@@ -235,6 +238,8 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
                                        const ProcessorParamsBasePtr _params,
                                        const SensorBasePtr _sensor_ptr);
 
+        static ProcessorBasePtr createAutoConf(const std::string& _unique_name, const paramsServer& _server, const SensorBasePtr sensor_ptr);
+
         const FeatureBasePtrList& getLastNewFeatures() const
         {
             return untracked_features_last_;

include/laser/sensor/sensor_laser_2D.h

@@ -20,6 +20,31 @@ struct IntrinsicsLaser2D : public IntrinsicsBase
         virtual ~IntrinsicsLaser2D() = default;
 
         laserscanutils::LaserScanParams scan_params;
+        IntrinsicsLaser2D() = default;
+        IntrinsicsLaser2D(std::string _unique_name, const paramsServer& _server):
+            IntrinsicsBase(_unique_name, _server)
+            {
+                scan_params.angle_min_ = _server.getParam<double>(_unique_name + "/angle_min"); //radians
+                scan_params.angle_max_ = _server.getParam<double>(_unique_name + "/angle_max"); //radians
+                scan_params.angle_step_ = _server.getParam<double>(_unique_name + "/angle_step"); //radians
+                scan_params.scan_time_ = _server.getParam<double>(_unique_name + "/scan_time"); //time from the measuring the first ray up to measuring the last one, seconds
+                scan_params.range_min_ = _server.getParam<double>(_unique_name + "/range_min"); //meters
+                scan_params.range_max_ = _server.getParam<double>(_unique_name + "/range_max"); //meters
+                scan_params.range_std_dev_ = _server.getParam<double>(_unique_name + "/range_std_dev"); //standard deviation measurement noise in range, meters
+                scan_params.angle_std_dev_ = _server.getParam<double>(_unique_name + "/angle_std_dev"); //standard deviation measurement noise in range, meters
+            }
+        std::string print()
+        {
+            return "\n" + IntrinsicsBase::print()
+                + "angle_min_: " + std::to_string(scan_params.angle_min_) + "\n"
+                + "angle_max_: " + std::to_string(scan_params.angle_max_) + "\n"
+                + "angle_step_: " + std::to_string(scan_params.angle_step_) + "\n"
+                + "scan_time_: " + std::to_string(scan_params.scan_time_) + "\n"
+                + "range_min_: " + std::to_string(scan_params.range_min_) + "\n"
+                + "range_max_: " + std::to_string(scan_params.range_max_) + "\n"
+                + "range_std_dev_: " + std::to_string(scan_params.range_std_dev_) + "\n"
+                + "angle_std_dev_: " + std::to_string(scan_params.angle_std_dev_) + "\n";
+        }
 };
 
 WOLF_PTR_TYPEDEFS(SensorLaser2D);

src/processor/processor_tracker_feature_polyline_2D.cpp

@@ -9,6 +9,86 @@
 
 namespace wolf
 {
+ProcessorParamsTrackerFeaturePolyline2D::ProcessorParamsTrackerFeaturePolyline2D(std::string _unique_name, const paramsServer& _server):
+    ProcessorParamsTrackerFeature(_unique_name, _server)
+{
+    line_finder_params.max_fit_error_ = _server.getParam<Scalar>(_unique_name + "/max_fit_error");
+    line_finder_params.min_supports_ = _server.getParam<unsigned int>(_unique_name + "/min_supports");
+    line_finder_params.range_jump_segment_ = _server.getParam<Scalar>(_unique_name + "/range_jump_segment");
+    line_finder_params.dist_th_defined_ = _server.getParam<Scalar>(_unique_name + "/dist_th_defined");
+    line_finder_params.std_dev_points_ = _server.getParam<Scalar>(_unique_name + "/std_dev_points");
+    match_feature_position_sq_norm_th = _server.getParam<Scalar>(_unique_name + "/match_feature_position_sq_norm_th");
+    match_feature_orientation_sq_norm_th = _server.getParam<Scalar>(_unique_name + "/match_feature_orientation_sq_norm_th");
+    match_landmark_position_sq_norm_th = _server.getParam<Scalar>(_unique_name + "/match_landmark_position_sq_norm_th");
+    match_landmark_orientation_sq_norm_th = _server.getParam<Scalar>(_unique_name + "/match_landmark_orientation_sq_norm_th");
+    class_position_error_th = _server.getParam<Scalar>(_unique_name + "/class_position_error_th");
+    new_features_th = _server.getParam<unsigned int>(_unique_name + "/new_features_th");
+    loop_time_th = _server.getParam<Scalar>(_unique_name + "/loop_time_th");
+    //TODO: how to read polyline classes?
+    std::vector<std::string> polylineRectangularClass;
+    polylineRectangularClass = _server.getParam<std::vector<std::string>>(_unique_name + "/polylines");
+    for(auto it : polylineRectangularClass){
+        if(it == "CONTAINER") polyline_classes.push_back(PolylineClassContainer());
+        else if(it == "SMALL_CONTAINER") polyline_classes.push_back(PolylineClassSmallContainer());
+        else if(it == "PALLET") polyline_classes.push_back(PolylineClassPallet());
+        else{
+            auto polyline_class = converter<std::vector<std::string>>::convert(it);
+            PolylineClassType type;
+            std::string str_type = polyline_class[0];
+            if(str_type == "CONTAINER") type = PolylineClassType::CONTAINER;
+            else if(str_type == "SMALL_CONTAINER") type = PolylineClassType::SMALL_CONTAINER;
+            else if(str_type == "PALLET") type = PolylineClassType::PALLET;
+            else if(str_type == "OTHER") type = PolylineClassType::OTHER;
+            else type = PolylineClassType::UNCLASSIFIED;
+            polyline_classes.push_back(PolylineRectangularClass(type,
+                                                                converter<Scalar>::convert(polyline_class[1]),
+                                                                converter<Scalar>::convert(polyline_class[2])));
+        }
+
+    }
+}
+
+std::string ProcessorParamsTrackerFeaturePolyline2D::print()
+{
+    std::string polyline_string = "";
+    for(auto it : polyline_classes){
+        std::string aux = "\nType: ";
+        switch(it.type){
+        case CONTAINER:
+            aux += "Container";
+            break;
+        case SMALL_CONTAINER:
+            aux += "Small Container";
+            break;
+        case PALLET:
+            aux += "Pallet";
+            break;
+        case OTHER:
+            aux += "Other";
+            break;
+        default:
+            aux += "Unclassified";
+            break;
+        }
+        aux += ", Long side: " + std::to_string(it.L) + ", Short side: " + std::to_string(it.W) + ", Diagonal length: " + std::to_string(it.D);
+        polyline_string += aux;
+    }
+
+    return "\n" + ProcessorParamsTrackerFeature::print()
+        + "line_finder_params.max_fit_error_: " + std::to_string(line_finder_params.max_fit_error_) + "\n"
+        + "line_finder_params.min_supports_: " + std::to_string(line_finder_params.min_supports_) + "\n"
+        + "line_finder_params.range_jump_segment_: " + std::to_string(line_finder_params.range_jump_segment_) + "\n"
+        + "line_finder_params.dist_th_defined_: " + std::to_string(line_finder_params.dist_th_defined_) + "\n"
+        + "line_finder_params.std_dev_points_: " + std::to_string(line_finder_params.std_dev_points_) + "\n"
+        + "match_feature_position_sq_norm_th: " + std::to_string(match_feature_position_sq_norm_th) + "\n"
+        + "match_feature_orientation_sq_norm_th: " + std::to_string(match_feature_orientation_sq_norm_th) + "\n"
+        + "match_landmark_position_sq_norm_th: " + std::to_string(match_landmark_position_sq_norm_th) + "\n"
+        + "match_landmark_orientation_sq_norm_th: " + std::to_string(match_landmark_orientation_sq_norm_th) + "\n"
+        + "class_position_error_th: " + std::to_string(class_position_error_th) + "\n"
+        + "new_features_th: " + std::to_string(new_features_th) + "\n"
+        + "loop_time_th: " + std::to_string(loop_time_th) + "\n"
+        + "Polyline Classes: " + polyline_string + "\n";
+}
 
 ProcessorTrackerFeaturePolyline2D::ProcessorTrackerFeaturePolyline2D(ProcessorParamsTrackerFeaturePolyline2DPtr _params) :
      ProcessorTrackerFeature("TRACKER FEATURE POLYLINE",_params),
@@ -1414,10 +1494,18 @@ ProcessorBasePtr ProcessorTrackerFeaturePolyline2D::create(const std::string& _u
     return prc_ptr;
 }
 
+ProcessorBasePtr ProcessorTrackerFeaturePolyline2D::createAutoConf(const std::string& _unique_name, const paramsServer& _server, const SensorBasePtr sensor_ptr)
+{
+    auto params = std::make_shared<ProcessorParamsTrackerFeaturePolyline2D>(_unique_name, _server);
+    WOLF_TRACE("\n", params->print());
+    return nullptr;
+}
 } /* namespace wolf */
 
 // Register in the SensorFactory
 #include "core/processor/processor_factory.h"
+#include "core/processor/autoconf_processor_factory.h"
 namespace wolf {
 WOLF_REGISTER_PROCESSOR("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline2D)
+WOLF_REGISTER_PROCESSOR_AUTO("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline2D)
 } /* namespace wolf */