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

Improve code with better API for composites

parent dd382151
No related branches found
No related tags found
1 merge request!358WIP: Resolve "Complete state vector new data structure?"
Pipeline #5362 failed
......@@ -65,19 +65,18 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXd& _data,
* dth : arc angle [rad]
*/
const double& k = radians_per_tick_;
const auto& calib = _calib.at("I"); // sensor intrinsics
const double& r_l = calib(0); // wheel radii
const auto& calib = _calib.at("I"); // sensor intrinsics
const double& r_l = calib(0); // wheel radii
const double& r_r = calib(1);
const double& d = calib(2); // wheel separation
double dl = k * (_data(1) * r_r + _data(0) * r_l) / 2.0;
double dth = k * (_data(1) * r_r - _data(0) * r_l) / d;
const double& d = calib(2); // wheel separation
double dl = k * (_data(1) * r_r + _data(0) * r_l) / 2.0; // length of arc of trajectory
double dth = k * (_data(1) * r_r - _data(0) * r_l) / d; // angle of arc of trajectory
/// 1. Tangent space: calibrate and go to se2 tangent -----------------------------------------------
VectorComposite tangent;
tangent.emplace("P", Vector2d(dl,0));
tangent.emplace("O", Vector1d(dth));
VectorComposite tangent ("PO", { Vector2d( dl, 0 ),
Vector1d( dth ) } );
MatrixComposite J_tangent_calib;
......@@ -90,29 +89,32 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXd& _data,
0.0, // d ds / d r_l
0.0, // d ds / d r_r
0.0 // d ds / d d
).finished());
).finished());
J_tangent_calib.emplace("O", "I", (Matrix<double,1,3>() << - k * _data(0) / d, // d dth / d r_l
+ k * _data(1) / d, // d dth / d r_r
- dth / d // d dth / d d
).finished());
).finished());
MatrixComposite J_tangent_data;
J_tangent_data.emplace("P", "d", (Matrix<double,2,2>() << k * r_l / 2.0, // d dl / d data(0)
k * r_r / 2.0, // d dl / d data(1)
0.0, // d ds / d data(0)
0.0 // d ds / d data(1)
).finished());
).finished());
J_tangent_data.emplace("O", "d", (Matrix<double,1,2>() << - k * r_l / d, // d dth / d data(0)
+ k * r_r / d // d dth / d data(1)
).finished());
).finished());
MatrixComposite J_tangent_side;
J_tangent_side.emplace("P", "s", (Matrix<double,2,1>() << 0.0, // d dl / d ds
1.0 // d ds / d ds
).finished());
).finished());
J_tangent_side.emplace("O", "s", Matrix1d( 0.0 )); // d dth / d ds
/// 2. Current delta: go to SE2 manifold -----------------------------------------------
MatrixComposite J_delta_tangent;
......@@ -122,12 +124,14 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXd& _data,
/// 3. delta covariance -----------------------------------------------
const auto& side_cov = unmeasured_perturbation_cov_; // rename for better reading
MatrixComposite data_cov;
data_cov.emplace("d","d",_data_cov);
_delta_cov = (J_delta_tangent * J_tangent_data).propagate(data_cov) + (J_delta_tangent * J_tangent_side).propagate(side_cov);
/// 4. Jacobian of delta wrt calibration params -----------------------------------------------
_jacobian_calib = J_delta_tangent * J_tangent_calib;
......
......@@ -12,10 +12,7 @@ ProcessorOdom2d::ProcessorOdom2d(ParamsProcessorOdom2dPtr _params) :
{
double unmeasured_var = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std;
unmeasured_perturbation_cov_.emplace("P","P", Matrix2d::Identity() * unmeasured_var);
unmeasured_perturbation_cov_.emplace("P","O", Vector2d::Zero());
unmeasured_perturbation_cov_.emplace("O","P", RowVector2d::Zero());
unmeasured_perturbation_cov_.emplace("O","P", Matrix1d::Identity() * unmeasured_var);
unmeasured_perturbation_cov_ = MatrixComposite(unmeasured_var*Matrix3d::Identity(), "PO", {2,1}, "PO", {2,1});
}
ProcessorOdom2d::~ProcessorOdom2d()
......
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