Skip to content
Snippets Groups Projects
Commit 7681f10f authored by Amanda Sanjuan Sánchez's avatar Amanda Sanjuan Sánchez
Browse files

Add initialized jacobian in processor motion

parent ab10928b
No related branches found
No related tags found
2 merge requests!466devel->main,!458Resolve "Jacobian not initialized in processor motion"
Pipeline #12511 failed
This commit is part of merge request !458. Comments created here will be created in the context of that merge request.
...@@ -66,11 +66,13 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, ...@@ -66,11 +66,13 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
calib_preint_(_calib_size), calib_preint_(_calib_size),
jacobian_delta_preint_(delta_cov_size_, delta_cov_size_), jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
jacobian_delta_(delta_cov_size_, delta_cov_size_), jacobian_delta_(delta_cov_size_, delta_cov_size_),
jacobian_calib_(delta_cov_size_, calib_size_) jacobian_calib_(delta_cov_size_, calib_size_),
jacobian_delta_calib_(delta_cov_size_, calib_size_)
{ {
jacobian_delta_preint_ .setIdentity(delta_cov_size_,delta_cov_size_); // dDp'/dDp, dDv'/dDv, all zeros jacobian_delta_preint_ .setIdentity(delta_cov_size_,delta_cov_size_); // dDp'/dDp, dDv'/dDv, all zeros
jacobian_delta_ .setIdentity(delta_cov_size_,delta_cov_size_); // jacobian_delta_ .setIdentity(delta_cov_size_,delta_cov_size_); //
jacobian_calib_ .setZero(delta_cov_size_,calib_size_); jacobian_calib_ .setZero(delta_cov_size_,calib_size_);
jacobian_delta_calib_ .setZero(delta_cov_size_,calib_size_);
unmeasured_perturbation_cov_ = unmeasured_perturbation_cov_ =
params_motion_->unmeasured_perturbation_std params_motion_->unmeasured_perturbation_std
* params_motion_->unmeasured_perturbation_std * params_motion_->unmeasured_perturbation_std
......
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