diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h index 676cb6cd05d51240cad7a5008ca301ccb5af67e3..438dcc33b0e02f62c241b6852cd5d4fd29c4184e 100644 --- a/include/core/processor/processor_odom_3d.h +++ b/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 diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp index c7be4d48c4d086d5d4f594dfa7849419d8cc5c30..8d8b190eef3ca6836e75b2868e40c56afea83fb4 100644 --- a/src/processor/processor_odom_3d.cpp +++ b/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, diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp index dc24beddd0643d7d64d21deeb59c2068752d50ec..876a4450d65e0da3231f1f9b91b9e56e3b97ac69 100644 --- a/test/gtest_processor_odom_3d.cpp +++ b/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)