Skip to content
Snippets Groups Projects
Commit 52fa98ca authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

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
parents ea0f2ec5 99fa8038
No related branches found
No related tags found
1 merge request!313WIP: Resolve "Processor constructors and creators requiring a sensor pointer?"
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
......@@ -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));
......
......@@ -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_ */
......@@ -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;
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment