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

Merge branch 'odom_2d' into 'master'

add theta th in ProcessorOdom2D

See merge request !117
parents 7e37f246 f585e678
No related branches found
No related tags found
1 merge request!117add theta th in ProcessorOdom2D
...@@ -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