Skip to content
Snippets Groups Projects
Commit f585e678 authored by Jeremie Deray's avatar Jeremie Deray
Browse files

add theta th in ProcessorOdom2D

parent d1a28acf
No related branches found
No related tags found
1 merge request!117add theta th in ProcessorOdom2D
This commit is part of merge request !117. Comments created here will be created in the context of that merge request.
...@@ -5,24 +5,31 @@ namespace wolf ...@@ -5,24 +5,31 @@ namespace wolf
ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr) ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
{ {
Scalar dist_traveled_th ; Scalar dist_traveled_th ;
Scalar theta_traveled_th ;
Scalar cov_det_th ; Scalar cov_det_th ;
Scalar elapsed_time_th ; Scalar elapsed_time_th ;
if (_params) if (_params)
{ {
std::shared_ptr<ProcessorParamsOdom2D> params = std::static_pointer_cast<ProcessorParamsOdom2D>(_params); std::shared_ptr<ProcessorParamsOdom2D> params = std::static_pointer_cast<ProcessorParamsOdom2D>(_params);
dist_traveled_th = params->dist_traveled_th_; dist_traveled_th = params->dist_traveled_th_;
cov_det_th = params->cov_det_th_; theta_traveled_th = params->theta_traveled_th_;
elapsed_time_th = params->elapsed_time_th_; cov_det_th = params->cov_det_th_;
elapsed_time_th = params->elapsed_time_th_;
} }
else else
{ {
std::cout << __FILE__ << ":" << __FUNCTION__ << "() : No parameters provided. Using dummy set." << std::endl; std::cout << __FILE__ << ":" << __FUNCTION__ << "() : No parameters provided. Using dummy set." << std::endl;
dist_traveled_th = 1; dist_traveled_th = 1;
theta_traveled_th = 0.17;
cov_det_th = 1; cov_det_th = 1;
elapsed_time_th = 1; elapsed_time_th = 1;
} }
ProcessorOdom2DPtr prc_ptr = std::make_shared<ProcessorOdom2D>(dist_traveled_th, cov_det_th, elapsed_time_th);
ProcessorOdom2DPtr prc_ptr = std::make_shared<ProcessorOdom2D>(dist_traveled_th, theta_traveled_th,
cov_det_th, elapsed_time_th);
prc_ptr->setName(_unique_name); prc_ptr->setName(_unique_name);
return prc_ptr; return prc_ptr;
} }
......
...@@ -21,6 +21,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D); ...@@ -21,6 +21,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D);
struct ProcessorParamsOdom2D : public ProcessorParamsBase struct ProcessorParamsOdom2D : public ProcessorParamsBase
{ {
Scalar dist_traveled_th_; Scalar dist_traveled_th_;
Scalar theta_traveled_th_;
Scalar cov_det_th_; Scalar cov_det_th_;
Scalar elapsed_time_th_; Scalar elapsed_time_th_;
}; };
...@@ -28,7 +29,8 @@ struct ProcessorParamsOdom2D : public ProcessorParamsBase ...@@ -28,7 +29,8 @@ struct ProcessorParamsOdom2D : public ProcessorParamsBase
class ProcessorOdom2D : public ProcessorMotion class ProcessorOdom2D : public ProcessorMotion
{ {
public: public:
ProcessorOdom2D(const Scalar& _traveled_dist_th, const Scalar& _cov_det_th, const Scalar& _elapsed_time_th); ProcessorOdom2D(const Scalar& _dist_traveled_th, const Scalar& _theta_traveled_th,
const Scalar& _cov_det_th, const Scalar& _elapsed_time_th);
virtual ~ProcessorOdom2D(); virtual ~ProcessorOdom2D();
virtual bool voteForKeyFrame(); virtual bool voteForKeyFrame();
...@@ -61,6 +63,7 @@ class ProcessorOdom2D : public ProcessorMotion ...@@ -61,6 +63,7 @@ class ProcessorOdom2D : public ProcessorMotion
protected: protected:
Scalar dist_traveled_th_; Scalar dist_traveled_th_;
Scalar theta_traveled_th_;
Scalar cov_det_th_; Scalar cov_det_th_;
Scalar elapsed_time_th_; Scalar elapsed_time_th_;
...@@ -69,9 +72,11 @@ class ProcessorOdom2D : public ProcessorMotion ...@@ -69,9 +72,11 @@ class ProcessorOdom2D : public ProcessorMotion
static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr); static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
}; };
inline ProcessorOdom2D::ProcessorOdom2D(const Scalar& _traveled_dist_th, const Scalar& _cov_det_th, const Scalar& _elapsed_time_th) : inline ProcessorOdom2D::ProcessorOdom2D(const Scalar& _dist_traveled_th, const Scalar& _theta_traveled_th,
const Scalar& _cov_det_th, const Scalar& _elapsed_time_th) :
ProcessorMotion("ODOM 2D", 3, 3, 3, 2), ProcessorMotion("ODOM 2D", 3, 3, 3, 2),
dist_traveled_th_(_traveled_dist_th), dist_traveled_th_(_dist_traveled_th),
theta_traveled_th_(_theta_traveled_th),
cov_det_th_(_cov_det_th), cov_det_th_(_cov_det_th),
elapsed_time_th_(_elapsed_time_th) elapsed_time_th_(_elapsed_time_th)
{ {
...@@ -230,6 +235,13 @@ inline bool ProcessorOdom2D::voteForKeyFrame() ...@@ -230,6 +235,13 @@ inline bool ProcessorOdom2D::voteForKeyFrame()
return true; return true;
} }
if (/*std::abs*/(getBuffer().get().back().delta_integr_.tail<1>().norm()) > theta_traveled_th_)
{
// std::cout << "ProcessorOdom2D:: " << id() << " - VOTE FOR KEY FRAME traveled distance "
// << getBuffer().get().back().delta_integr_.head<2>().norm() << std::endl;
return true;
}
// Uncertainty criterion // Uncertainty criterion
delta_integrated_cov_ = getBuffer().get().back().jacobian_delta_integr_ * delta_integrated_cov_ * getBuffer().get().back().jacobian_delta_integr_.transpose() + getBuffer().get().back().jacobian_delta_ * getBuffer().get().back().delta_cov_ * getBuffer().get().back().jacobian_delta_.transpose(); delta_integrated_cov_ = getBuffer().get().back().jacobian_delta_integr_ * delta_integrated_cov_ * getBuffer().get().back().jacobian_delta_integr_.transpose() + getBuffer().get().back().jacobian_delta_ * getBuffer().get().back().delta_cov_ * getBuffer().get().back().jacobian_delta_.transpose();
if (delta_integrated_cov_.determinant() > cov_det_th_) if (delta_integrated_cov_.determinant() > cov_det_th_)
......
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