/* ************************************************************************* */ 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; }
/* ************************************************************************* */ 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; }
/* ************************************************************************* */ 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; }
/* ************************************************************************* */ 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; }
/* ************************************************************************* */ Rot3 Rot3::AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p) { // if a_p is already aligned with b_p, return the identity rotation if (std::abs(a_p.dot(b_p)) > 0.999999999) { return Rot3(); } // Check axis was not degenerate cross product const Vector3 z = axis.unitVector(); if (z.hasNaN()) throw std::runtime_error("AlignSinglePair: axis has Nans"); // Now, calculate rotation that takes b_p to a_p const Matrix3 P = I_3x3 - z * z.transpose(); // orthogonal projector const Vector3 a_po = P * a_p.unitVector(); // point in a orthogonal to axis const Vector3 b_po = P * b_p.unitVector(); // point in b orthogonal to axis const Vector3 x = a_po.normalized(); // x-axis in axis-orthogonal plane, along a_p vector const Vector3 y = z.cross(x); // y-axis in axis-orthogonal plane const double u = x.dot(b_po); // x-coordinate for b_po const double v = y.dot(b_po); // y-coordinate for b_po double angle = std::atan2(v, u); return Rot3::AxisAngle(z, -angle); }
/* ************************************************************************* */ 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; }
/* ************************************************************************* */ Rot3 Rot3::AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, // const Unit3& a_q, const Unit3& b_q) { // there are three frames in play: // a: the first frame in which p and q are measured // b: the second frame in which p and q are measured // i: intermediate, after aligning first pair // First, find rotation around that aligns a_p and b_p Rot3 i_R_b = AlignPair(a_p.cross(b_p), a_p, b_p); // Rotate points in frame b to the intermediate frame, // in which we expect the point p to be aligned now Unit3 i_q = i_R_b * b_q; assert(assert_equal(a_p, i_R_b * b_p, 1e-6)); // Now align second pair: we need to align i_q to a_q Rot3 a_R_i = AlignPair(a_p, a_q, i_q); assert(assert_equal(a_p, a_R_i * a_p, 1e-6)); assert(assert_equal(a_q, a_R_i * i_q, 1e-6)); // The desired rotation is the product of both Rot3 a_R_b = a_R_i * i_R_b; return a_R_b; }
//******************************************************************************* Point3 point3_(const Unit3& p) { return p.point3(); }