Merge branch 'odom_2d' into 'master'

add theta th in ProcessorOdom2D

See merge request !117

src/processor_odom_2D.cpp

@@ -5,24 +5,31 @@ namespace wolf
 ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
 {
     Scalar dist_traveled_th ;
+    Scalar theta_traveled_th ;
     Scalar cov_det_th       ;
     Scalar elapsed_time_th  ;
+
     if (_params)
     {
         std::shared_ptr<ProcessorParamsOdom2D> params = std::static_pointer_cast<ProcessorParamsOdom2D>(_params);
-        dist_traveled_th = params->dist_traveled_th_;
-        cov_det_th       = params->cov_det_th_;
-        elapsed_time_th  = params->elapsed_time_th_;
+        dist_traveled_th  = params->dist_traveled_th_;
+        theta_traveled_th = params->theta_traveled_th_;
+        cov_det_th        = params->cov_det_th_;
+        elapsed_time_th   = params->elapsed_time_th_;
     }
     else
     {
         std::cout << __FILE__ << ":" << __FUNCTION__ << "() : No parameters provided. Using dummy set." << std::endl;
         dist_traveled_th = 1;
+        theta_traveled_th = 0.17;
         cov_det_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);
+
     return prc_ptr;
 }
 

src/processor_odom_2D.h

@@ -21,6 +21,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D);
 struct ProcessorParamsOdom2D : public ProcessorParamsBase
 {
     Scalar dist_traveled_th_;
+    Scalar theta_traveled_th_;
     Scalar cov_det_th_;
     Scalar elapsed_time_th_;
 };
@@ -28,7 +29,8 @@ struct ProcessorParamsOdom2D : public ProcessorParamsBase
 class ProcessorOdom2D : public ProcessorMotion
 {
     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 bool voteForKeyFrame();
 
@@ -61,6 +63,7 @@ class ProcessorOdom2D : public ProcessorMotion
 
     protected:
         Scalar dist_traveled_th_;
+        Scalar theta_traveled_th_;
         Scalar cov_det_th_;
         Scalar elapsed_time_th_;
 
@@ -69,9 +72,11 @@ class ProcessorOdom2D : public ProcessorMotion
         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),
-        dist_traveled_th_(_traveled_dist_th),
+        dist_traveled_th_(_dist_traveled_th),
+        theta_traveled_th_(_theta_traveled_th),
         cov_det_th_(_cov_det_th),
         elapsed_time_th_(_elapsed_time_th)
 {
@@ -230,6 +235,13 @@ inline bool ProcessorOdom2D::voteForKeyFrame()
         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
     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_)