Remove noise params from ProcessorOdom3d

include/core/processor/processor_odom_3d.h

@@ -114,13 +114,6 @@ class ProcessorOdom3d : public ProcessorMotion
     protected:
         ParamsProcessorOdom3dPtr params_odom_3d_;
 
-        // noise parameters (stolen from owner SensorOdom3d)
-        double k_disp_to_disp_; // displacement variance growth per meter of linear motion
-        double k_disp_to_rot_;  // orientation  variance growth per meter of linear motion
-        double k_rot_to_rot_;   // orientation  variance growth per radian of rotational motion
-        double min_disp_var_;   // floor displacement variance when no  motion
-        double min_rot_var_;    // floor orientation variance when no motion
-
 };
 
 inline Eigen::VectorXd ProcessorOdom3d::deltaZero() const

src/processor/processor_odom_3d.cpp

@@ -6,12 +6,7 @@ namespace wolf
 
 ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) :
                         ProcessorMotion("ProcessorOdom3d", "PO", 3, 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
+                        params_odom_3d_ (_params)
 {
      // Set constant parts of Jacobians
     jacobian_delta_preint_.setIdentity(6,6);
@@ -29,13 +24,6 @@ void ProcessorOdom3d::configure(SensorBasePtr _sensor)
     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::VectorXd& _data,

test/gtest_processor_odom_3d.cpp

@@ -44,18 +44,10 @@ class ProcessorOdom3dTest : public ProcessorOdom3d
 {
     public:
         ProcessorOdom3dTest();
-
-        // getters :-D !!
-        double& kdd() {return k_disp_to_disp_;}
-        double& kdr() {return k_disp_to_rot_;}
-        double& krr() {return k_rot_to_rot_;}
-        double& dvar_min() {return min_disp_var_;}
-        double& rvar_min() {return min_rot_var_;}
 };
 ProcessorOdom3dTest::ProcessorOdom3dTest() : ProcessorOdom3d(std::make_shared<ParamsProcessorOdom3d>())
 {
-    dvar_min() = 0.5;
-    rvar_min() = 0.25;
+    //
 }
 
 TEST(ProcessorOdom3d, computeCurrentDelta)
@@ -76,11 +68,8 @@ TEST(ProcessorOdom3d, computeCurrentDelta)
     delta.head<3>() = delta_dp;
     delta.tail<4>() = delta_dq.coeffs();
 
-    // construct covariance from processor parameters and motion magnitudes
-    double disp = data_dp.norm();
-    double rot = data_do.norm();
-    double dvar = prc.dvar_min() + prc.kdd()*disp;
-    double rvar = prc.rvar_min() + prc.kdr()*disp + prc.krr()*rot;
+    double dvar = 0.1;
+    double rvar = 0.2;
     Vector6d diag; diag << dvar, dvar, dvar, rvar, rvar, rvar;
     Matrix6d data_cov = diag.asDiagonal();
     Matrix6d delta_cov = data_cov;
@@ -119,8 +108,6 @@ TEST(ProcessorOdom3d, deltaPlusDelta)
     prc.deltaPlusDelta(D, d, dt, D_int);
 
     ASSERT_MATRIX_APPROX(D_int , D_int_check, 1e-10);
-//        << "\nDpd  : " << D_int.transpose()
-//        << "\ncheck: " << D_int_check.transpose();
 }
 
 TEST(ProcessorOdom3d, deltaPlusDelta_Jac)