diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp index 5685c411998415689aa8d795b4f8c61417382f3d..2a267f12a091713d6ceea0a412afe1797d5d0f80 100644 --- a/hello_wolf/processor_range_bearing.cpp +++ b/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); @@ -119,11 +118,10 @@ ProcessorBasePtr ProcessorRangeBearing::createAutoConf(const std::string& _uniqu 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); + auto params = std::make_shared<ProcessorParamsRangeBearing>(_unique_name, _server); // construct processor - ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params); + auto prc = std::make_shared<ProcessorRangeBearing>(params); // setup processor prc->setName(_unique_name); diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h index 2bc4767c14f0c9619534d3bd6f6ae1dc5d4f506b..b5e1a5f74929c6ac076c9dde033fa2f9ea644c73 100644 --- a/hello_wolf/processor_range_bearing.h +++ b/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, @@ -84,6 +84,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_ */ diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h index 759912f27680d5947352d599a889985b0e81d2b3..7eab40c33609139afab9e59301c816400bdea95f 100644 --- a/include/core/processor/processor_odom_3D.h +++ b/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; diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp index 3168276f93aa8da7c5220aaba098503863ff0e68..6d60447c261f6244f4bcf7adf38d469a326ef56d 100644 --- a/src/processor/processor_odom_3D.cpp +++ b/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);