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

Remove noise params from ProcessorOdom3d

parent a688748f
No related branches found
No related tags found
1 merge request!388Resolve "Simplifying delta covariance in ProcessorOdom2/3D"
......@@ -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
......
......@@ -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,
......
......@@ -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)
......
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