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