/* ************************************************************************* */
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();
}
