/* ************************************************************************* */ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const { // Get the point3 of this, and the derivative. Matrix32 H_qn_q; Point3 qn = q.point3(H_q ? &H_qn_q : 0); // 2D error here is projecting q into the tangent plane of this (p). Matrix62 H_B_p; Matrix23 Bt = basis(H_p ? &H_B_p : 0).transpose(); Vector2 xi = Bt * qn.vector(); if (H_p) { // Derivatives of each basis vector. const Matrix32& H_b1_p = H_B_p.block<3, 2>(0, 0); const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0); // Derivatives of the two entries of xi wrt the basis vectors. Matrix13 H_xi1_b1 = qn.vector().transpose(); Matrix13 H_xi2_b2 = qn.vector().transpose(); // Assemble dxi/dp = dxi/dB * dB/dp. Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p; Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p; *H_p << H_xi1_p, H_xi2_p; } if (H_q) { // dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q. Matrix23 H_xi_qu = Bt; *H_q = H_xi_qu * H_qn_q; } return xi; }
/* ************************************************************************* */ Unit3 Rot3::unrotate(const Unit3& p, OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { Matrix32 Dp; Unit3 q = Unit3(unrotate(p.point3(Dp))); if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp; if (HR) *HR = q.basis().transpose() * q.skew(); return q; }
/* ************************************************************************* */ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const { // Get the unit vectors of each, and the derivative. Matrix32 H_pn_p; Point3 pn = point3(H_p ? &H_pn_p : 0); Matrix32 H_qn_q; Point3 qn = q.point3(H_q ? &H_qn_q : 0); // Compute the dot product of the Point3s. Matrix13 H_dot_pn, H_dot_qn; double d = pn.dot(qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); if (H_p) { (*H_p) << H_dot_pn * H_pn_p; } if (H_q) { (*H_q) = H_dot_qn * H_qn_q; } return d; }
//******************************************************************************* Point3 point3_(const Unit3& p) { return p.point3(); }