diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h
index 5be0bbd2b3ff0ace7264b83db249f382740ba362..d9b0b58454132c8abaa11d50a5847d18b829337f 100644
--- a/include/imu/math/imu_tools.h
+++ b/include/imu/math/imu_tools.h
@@ -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>