Merge branch 'remove-sensor-from-processors-constructors' into...

Merge branch 'remove-sensor-from-processors-constructors' into 248-processor-constructors-and-creators-requiring-a-sensor-pointer

demos/processor_odom_3D.yaml

-processor type: "ODOM 3D"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main odometer"        # This is ignored. The name provided to the SensorFactory prevails
+type: "ODOM 3D"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main odometer"        # This is ignored. The name provided to the SensorFactory prevails
 
-time tolerance:         0.01  # seconds
+time_tolerance:         0.01  # seconds
+unmeasured_perturbation_std: 0.001
+
+voting_active:          false
+voting_aux_active:      false
+max_time_span:          0.2   # seconds
+max_buff_length:        10    # motion deltas
+dist_traveled:          0.5   # meters
+angle_turned:           0.1   # radians (1 rad approx 57 deg, approx 60 deg)
 
-voting_active:        false
-voting_aux_active:    false
-k_max_time span:      0.2   # seconds
-k_max_buff_length:    10    # motion deltas
-k_dist_traveled:      0.5   # meters
-k_angle_turned:       0.1   # radians (1 rad approx 57 deg, approx 60 deg)
\ No newline at end of file

hello_wolf/processor_range_bearing.cpp

@@ -14,10 +14,10 @@
 namespace wolf
 {
 
-ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params) :
+ProcessorRangeBearing::ProcessorRangeBearing(ProcessorParamsBasePtr _params) :
         ProcessorBase("RANGE BEARING", _params)
 {
-    H_r_s   = transform(_sensor_ptr->getP()->getState(), _sensor_ptr->getO()->getState());
+    //
 }
 
 void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
@@ -103,11 +103,10 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name,
                                                const ProcessorParamsBasePtr _params,
                                                const SensorBasePtr _sen_ptr)
 {
-    SensorRangeBearingPtr       sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sen_ptr);
-    ProcessorParamsRangeBearingPtr params = std::static_pointer_cast<ProcessorParamsRangeBearing>(_params);
+    auto params = std::static_pointer_cast<ProcessorParamsRangeBearing>(_params);
 
     // construct processor
-    ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params);
+    auto prc    = std::make_shared<ProcessorRangeBearing>(params);
 
     // setup processor
     prc->setName(_unique_name);
@@ -115,22 +114,6 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name,
     return prc;
 }
 
-//ProcessorBasePtr ProcessorRangeBearing::createAutoConf(const std::string& _unique_name,
-//                                                       const ParamsServer& _server,
-//                                                       const SensorBasePtr _sensor_ptr)
-//{
-//    SensorRangeBearingPtr       sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sensor_ptr);
-//    ProcessorParamsRangeBearingPtr params = std::make_shared<ProcessorParamsRangeBearing>(_unique_name, _server);
-//
-//    // construct processor
-//    ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params);
-//
-//    // setup processor
-//    prc->setName(_unique_name);
-//
-//    return prc;
-//}
-//
 Eigen::Vector2s ProcessorRangeBearing::observe(const Eigen::Vector2s& lmk_w) const
 {
     return polar(toSensor(lmk_w));

hello_wolf/processor_range_bearing.h

@@ -45,9 +45,9 @@ class ProcessorRangeBearing : public ProcessorBase
     public:
         typedef Eigen::Transform<Scalar, 2, Eigen::Isometry> Trf;
 
-        ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params);
+        ProcessorRangeBearing(ProcessorParamsBasePtr _params);
         virtual ~ProcessorRangeBearing() {/* empty */}
-        virtual void configure(SensorBasePtr _sensor) override { }
+        virtual void configure(SensorBasePtr _sensor) override;
 
         // Factory method for high level API
         static ProcessorBasePtr create(const std::string& _unique_name,
@@ -85,6 +85,11 @@ class ProcessorRangeBearing : public ProcessorBase
         Eigen::Vector2s rect        (Scalar range, Scalar bearing) const;
 };
 
+inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor)
+{
+    H_r_s = transform(_sensor->getP()->getState(), _sensor->getO()->getState());
+}
+
 } /* namespace wolf */
 
 #endif /* HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_ */

include/core/processor/processor_odom_3D.h

@@ -60,7 +60,7 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3D);
 class ProcessorOdom3D : public ProcessorMotion
 {
     public:
-        ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DPtr _sensor_ptr = nullptr);
+        ProcessorOdom3D(ProcessorParamsOdom3DPtr _params);
         virtual ~ProcessorOdom3D();
         virtual void configure(SensorBasePtr _sensor) override;
 

src/processor/processor_odom_3D.cpp

@@ -2,18 +2,19 @@
 namespace wolf
 {
 
-ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DPtr _sensor_ptr) :
-                ProcessorMotion("ODOM 3D", 7, 7, 6, 6, 0, _params),
-                params_odom_3D_(_params),
-                p1_(nullptr), p2_(nullptr), p_out_(nullptr),
-                q1_(nullptr), q2_(nullptr), q_out_(nullptr)
-        {
-            configure(_sensor_ptr);
-            delta_ = deltaZero();
-            delta_integrated_ = deltaZero();
-            jacobian_delta_preint_.setZero(delta_cov_size_, delta_cov_size_);
-            jacobian_delta_.setZero(delta_cov_size_, delta_cov_size_);
-        }
+ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params) :
+                        ProcessorMotion("ODOM 3D", 7, 7, 6, 6, 0, _params),
+                        params_odom_3D_ (_params),
+                        k_disp_to_disp_ (0),
+                        k_disp_to_rot_  (0),
+                        k_rot_to_rot_   (0),
+                        min_disp_var_   (0.1),          // around 10cm error
+                        min_rot_var_    (0.1),          // around 6 degrees error
+                        p1_(nullptr), p2_(nullptr), p_out_(nullptr),
+                        q1_(nullptr), q2_(nullptr), q_out_(nullptr)
+{
+    //
+}
 
 ProcessorOdom3D::~ProcessorOdom3D()
 {
@@ -21,25 +22,16 @@ ProcessorOdom3D::~ProcessorOdom3D()
 
 void ProcessorOdom3D::configure(SensorBasePtr _sensor)
 {
-    if (_sensor)
-    {
-        SensorOdom3DPtr sen_ptr = std::static_pointer_cast<SensorOdom3D>(_sensor);
-        // we steal the parameters from the provided odom3D sensor.
-        k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor();
-        k_disp_to_rot_ = sen_ptr->getDispVarToRotNoiseFactor();
-        k_rot_to_rot_ = sen_ptr->getRotVarToRotNoiseFactor();
-        min_disp_var_ = sen_ptr->getMinDispVar();
-        min_rot_var_ = sen_ptr->getMinRotVar();
-    }
-    else
-    {
-        // we put default params.
-        k_disp_to_disp_ =   0;
-        k_disp_to_rot_  =   0;
-        k_rot_to_rot_   =   0;
-        min_disp_var_   = 0.1; // around 30cm error
-        min_rot_var_    = 0.1; // around 9 degrees error
-    }
+    assert (_sensor && "Trying to configure processor with a sensor but provided sensor is nullptr.");
+
+    SensorOdom3DPtr sen_ptr = std::static_pointer_cast<SensorOdom3D>(_sensor);
+
+    // we steal the parameters from the provided odom3D sensor.
+    k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor();
+    k_disp_to_rot_  = sen_ptr->getDispVarToRotNoiseFactor();
+    k_rot_to_rot_   = sen_ptr->getRotVarToRotNoiseFactor();
+    min_disp_var_   = sen_ptr->getMinDispVar();
+    min_rot_var_    = sen_ptr->getMinRotVar();
 }
 
 void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data,
@@ -344,12 +336,10 @@ Motion ProcessorOdom3D::interpolate(const Motion& _ref1,
 ProcessorBasePtr ProcessorOdom3D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
 {
     // cast inputs to the correct type
-    std::shared_ptr<ProcessorParamsOdom3D> prc_odo_params = std::static_pointer_cast<ProcessorParamsOdom3D>(_params);
-
-    SensorOdom3DPtr sen_odo =std::static_pointer_cast<SensorOdom3D>(_sen_ptr);
+    auto params     = std::static_pointer_cast<ProcessorParamsOdom3D>(_params);
 
     // construct processor
-    ProcessorOdom3DPtr prc_odo = std::make_shared<ProcessorOdom3D>(prc_odo_params, sen_odo);
+    auto prc_odo    = std::make_shared<ProcessorOdom3D>(params);
 
     // setup processor
     prc_odo->setName(_unique_name);