/* ************************************************************************* */
Unit3 Rot3::rotate(const Unit3& p,
OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
Matrix32 Dp;
Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0)));
if (Hp) *Hp = q.basis().transpose() * matrix() * Dp;
if (HR) *HR = -q.basis().transpose() * matrix() * p.skew();
return q;
}
/* ************************************************************************* */
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
Matrix23 Bt = basis().transpose();
Vector2 xi = Bt * q.p_;
if (H_q) {
*H_q = Bt * q.basis();
}
return xi;
}
/* ************************************************************************* */
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
// 3*3 Derivative of representation with respect to point is 3*3:
Matrix3 D_p_point;
Point3 normalized = point.normalize(H ? &D_p_point : 0);
Unit3 direction;
direction.p_ = normalized.vector();
if (H)
*H << direction.basis().transpose() * D_p_point;
return direction;
}
