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