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

Key string --> char

parent 00abe629
No related branches found
No related tags found
2 merge requests!39release after RAL,!38After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
include/imu/math/imu_tools.h
+ 41
− 68
View file @ 3c0243ec
......@@ -80,9 +80,9 @@ inline Matrix<T, 10, 1> identity()
inline VectorComposite identityComposite()
{
VectorComposite D;
D.emplace("P", Vector3d::Zero());
D.emplace("O", Quaterniond::Identity().coeffs());
D.emplace("V", Vector3d::Zero());
D.emplace('P', Vector3d::Zero());
D.emplace('O', Quaterniond::Identity().coeffs());
D.emplace('V', Vector3d::Zero());
return D;
}
......@@ -194,13 +194,13 @@ inline void compose(const MatrixBase<D1>& d1,
inline void compose(const VectorComposite& d1, const VectorComposite& d2, double dt, VectorComposite& dc)
{
compose(d1.at("P"), d1.at("O"), d1.at("V"), d2.at("P"), d2.at("O"), d2.at("V"), dt, dc["P"], dc["O"], dc["V"]);
compose(d1.at('P'), d1.at('O'), d1.at('V'), d2.at('P'), d2.at('O'), d2.at('V'), dt, dc['P'], dc['O'], dc['V']);
}
inline VectorComposite compose(const VectorComposite& d1, const VectorComposite& d2, double dt)
{
VectorComposite dc("POV", {3,4,3});
compose(d1.at("P"), d1.at("O"), d1.at("V"), d2.at("P"), d2.at("O"), d2.at("V"), dt, dc["P"], dc["O"], dc["V"]);
compose(d1.at('P'), d1.at('O'), d1.at('V'), d2.at('P'), d2.at('O'), d2.at('V'), dt, dc['P'], dc['O'], dc['V']);
return dc;
}
......@@ -258,33 +258,33 @@ inline void compose(const VectorComposite& d1,
{
// Some useful temporaries
Matrix3d dR1 = q2R(d1.at("O")); //dq1.matrix(); // First Delta, DR
Matrix3d dR2 = q2R(d2.at("O")); //dq2.matrix(); // Second delta, dR
Matrix3d dR1 = q2R(d1.at('O')); //dq1.matrix(); // First Delta, DR
Matrix3d dR2 = q2R(d2.at('O')); //dq2.matrix(); // Second delta, dR
// Jac wrt first delta // TODO find optimal way to re-use memory allocation!!!
J_sum_d1.clear();
J_sum_d1.emplace("P","P", Matrix3d::Identity()); // dDp'/dDp = I
J_sum_d1.emplace("P","O", - dR1 * skew(d2.at("P"))) ; // dDp'/dDo
J_sum_d1.emplace("P","V", Matrix3d::Identity() * dt); // dDp'/dDv = I*dt
J_sum_d1.emplace("O","P", Matrix3d::Zero());
J_sum_d1.emplace("O","O", dR2.transpose()); // dDo'/dDo
J_sum_d1.emplace("O","V", Matrix3d::Zero());
J_sum_d1.emplace("V","P", Matrix3d::Zero());
J_sum_d1.emplace("V","O", - dR1 * skew(d2.at("V"))) ; // dDv'/dDo
J_sum_d1.emplace("V","V", Matrix3d::Identity()); // dDv'/dDv = I
J_sum_d1.emplace('P','P', Matrix3d::Identity()); // dDp'/dDp = I
J_sum_d1.emplace('P','O', - dR1 * skew(d2.at('P'))) ; // dDp'/dDo
J_sum_d1.emplace('P','V', Matrix3d::Identity() * dt); // dDp'/dDv = I*dt
J_sum_d1.emplace('O','P', Matrix3d::Zero());
J_sum_d1.emplace('O','O', dR2.transpose()); // dDo'/dDo
J_sum_d1.emplace('O','V', Matrix3d::Zero());
J_sum_d1.emplace('V','P', Matrix3d::Zero());
J_sum_d1.emplace('V','O', - dR1 * skew(d2.at('V'))) ; // dDv'/dDo
J_sum_d1.emplace('V','V', Matrix3d::Identity()); // dDv'/dDv = I
// Jac wrt second delta
J_sum_d2.clear();
J_sum_d2.emplace("P","P", dR1); // dDp'/ddp
J_sum_d2.emplace("P","O", Matrix3d::Zero()) ; // dDp'/ddo
J_sum_d2.emplace("P","V", Matrix3d::Zero()); // dDp'/ddv
J_sum_d2.emplace("O","P", Matrix3d::Zero());
J_sum_d2.emplace("O","O", Matrix3d::Identity());// dDo'/ddo
J_sum_d2.emplace("O","V", Matrix3d::Zero());
J_sum_d2.emplace("V","P", Matrix3d::Zero());
J_sum_d2.emplace("V","O", Matrix3d::Zero()) ; // dDv'/ddo
J_sum_d2.emplace("V","V", dR1); // dDv'/ddv
J_sum_d2.emplace('P','P', dR1); // dDp'/ddp
J_sum_d2.emplace('P','O', Matrix3d::Zero()) ; // dDp'/ddo
J_sum_d2.emplace('P','V', Matrix3d::Zero()); // dDp'/ddv
J_sum_d2.emplace('O','P', Matrix3d::Zero());
J_sum_d2.emplace('O','O', Matrix3d::Identity());// dDo'/ddo
J_sum_d2.emplace('O','V', Matrix3d::Zero());
J_sum_d2.emplace('V','P', Matrix3d::Zero());
J_sum_d2.emplace('V','O', Matrix3d::Zero()) ; // dDv'/ddo
J_sum_d2.emplace('V','V', dR1); // dDv'/ddv
// compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
compose(d1, d2, dt, sum);
......@@ -428,14 +428,14 @@ inline void composeOverState(const VectorComposite& x,
T dt,
VectorComposite& x_plus_d)
{
assert(x_plus_d.count("P") && "provided reference does not have key 'P'");
assert(x_plus_d.count("O") && "provided reference does not have key 'O'");
assert(x_plus_d.count("V") && "provided reference does not have key 'V'");
assert(x_plus_d.count('P') && "provided reference does not have key 'P'");
assert(x_plus_d.count('O') && "provided reference does not have key 'O'");
assert(x_plus_d.count('V') && "provided reference does not have key 'V'");
composeOverState(x.at("P"), x.at("O"), x.at("V"),
d.at("P"), d.at("O"), d.at("V"),
composeOverState(x.at('P'), x.at('O'), x.at('V'),
d.at('P'), d.at('O'), d.at('V'),
dt,
x_plus_d["P"], x_plus_d["O"], x_plus_d["V"]);
x_plus_d['P'], x_plus_d['O'], x_plus_d['V']);
}
template<class T>
......@@ -592,7 +592,7 @@ inline Matrix<typename D1::Scalar, 10, 1> plus(const MatrixBase<D1>& d1,
inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
{
plus(x.at("P"), x.at("O"), x.at("V"), tau.at("P"), tau.at("O"), tau.at("V"), res.at("P"), res.at("O"), res.at("V"));
plus(x.at('P'), x.at('O'), x.at('V'), tau.at('P'), tau.at('O'), tau.at('V'), res.at('P'), res.at('O'), res.at('V'));
}
inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
......@@ -755,22 +755,12 @@ inline void body2delta(const MatrixBase<D1>& body,
{
MatrixSizeCheck<6,1>::check(body);
// _delta["P"] = Vector3d();
// _delta["O"] = Vector4d();
// _delta["V"] = Vector3d();
body2delta(body.block(0,0,3,1),
body.block(3,0,3,1),
dt,
_delta["P"],
_delta["O"],
_delta["V"]);
// body2delta(body.block(0,0,3,1),
// body.block(3,0,3,1),
// dt,
// _delta.at("P"),
// _delta.at("O"),
// _delta.at("V"));
_delta['P'],
_delta['O'],
_delta['V']);
}
template<typename D1, typename D2, typename D3>
......@@ -808,29 +798,12 @@ inline void body2delta(const MatrixBase<D1>& body,
Matrix<T, 3, 1> w = body.block(3,0,3,1);
// jac_body.setZero();
// jac_body.block(0,0,3,3) = 0.5 * dt * dt * Matrix<T, 3, 3>::Identity();
// jac_body.block(3,3,3,3) = dt * jac_SO3_right(w * dt);
// jac_body.block(6,0,3,3) = dt * Matrix<T, 3, 3>::Identity();
// Matrix<double, 3, 6> J_dp_data;
// Matrix<double, 3, 6> J_do_data;
// Matrix<double, 3, 6> J_dv_data;
// jac_body.emplace("P","I", MatrixXd(3,6));
// jac_body.emplace("O","I", MatrixXd(3,6));
// jac_body.emplace("V","I", MatrixXd(3,6));
//
// jac_body.at("P","I") << 0.5 * dt * dt * Matrix3d::Identity() , Matrix3d::Zero() ;
// jac_body.at("O","I") << Matrix3d::Zero() , dt * jac_SO3_right(w * dt) ;
// jac_body.at("V","I") << dt * Matrix3d::Identity() , Matrix3d::Zero() ;
jac_body.emplace("P","a", 0.5 * dt * dt * Matrix3d::Identity()); // 0,0
jac_body.emplace("P","w", Matrix3d::Zero()); // 0,3
jac_body.emplace("O","a", Matrix3d::Zero()); // 3,0
jac_body.emplace("O","w", dt * jac_SO3_right(w * dt)); // 3,3
jac_body.emplace("V","a", dt * Matrix3d::Identity()); // 6,0
jac_body.emplace("V","w", Matrix3d::Zero()); // 6,6
jac_body.emplace('P','a', 0.5 * dt * dt * Matrix3d::Identity()); // 0,0
jac_body.emplace('P','w', Matrix3d::Zero()); // 0,3
jac_body.emplace('O','a', Matrix3d::Zero()); // 3,0
jac_body.emplace('O','w', dt * jac_SO3_right(w * dt)); // 3,3
jac_body.emplace('V','a', dt * Matrix3d::Identity()); // 6,0
jac_body.emplace('V','w', Matrix3d::Zero()); // 6,6
}
template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment