diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 7729bd354..f53be3b40 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -15,12 +15,14 @@ * @author Can Erdogan * @author Frank Dellaert * @author Alex Trevor - * @author Zhaoyang Lv * @brief The Unit3 class - basically a point on a unit sphere */ #include #include +#include // GTSAM_USE_TBB + +#include #include // for GTSAM_USE_TBB #ifdef __clang__ @@ -32,13 +34,8 @@ # pragma clang diagnostic pop #endif -#ifdef GTSAM_USE_TBB -#include -#endif - #include #include -#include using namespace std; @@ -46,14 +43,12 @@ namespace gtsam { /* ************************************************************************* */ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { - Unit3 direction(point); - if (H) { - // 3*3 Derivative of representation with respect to point is 3*3: - Matrix3 D_p_point; - point.normalize(D_p_point); // TODO, this calculates norm a second time :-( - // Calculate the 2*3 Jacobian + // 3*3 Derivative of representation with respect to point is 3*3: + Matrix3 D_p_point; + Unit3 direction; + direction.p_ = point.normalize(H ? &D_p_point : 0); + if (H) *H << direction.basis().transpose() * D_p_point; - } return direction; } @@ -63,49 +58,105 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { boost::uniform_on_sphere randomDirection(3); // This variate_generator object is required for versions of boost somewhere // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng). - boost::variate_generator > generator( - rng, randomDirection); + boost::variate_generator > + generator(rng, randomDirection); vector d = generator(); - return Unit3(d[0], d[1], d[2]); + Unit3 result; + result.p_ = Point3(d[0], d[1], d[2]); + return result; } -#ifdef GTSAM_USE_TBB -tbb::mutex unit3BasisMutex; -#endif - /* ************************************************************************* */ -const Matrix32& Unit3::basis() const { +const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB - tbb::mutex::scoped_lock lock(unit3BasisMutex); + // NOTE(hayk): At some point it seemed like this reproducably resulted in deadlock. However, I + // can't see the reason why and I can no longer reproduce it. It may have been a red herring, or + // there is still a latent bug to watch out for. + tbb::mutex::scoped_lock lock(B_mutex_); #endif - // Return cached version if exists - if (B_) return *B_; + // Return cached basis if available and the Jacobian isn't needed. + if (B_ && !H) { + return *B_; + } + + // Return cached basis and derivatives if available. + if (B_ && H && H_B_) { + *H = *H_B_; + return *B_; + } + + // Get the unit vector and derivative wrt this. + // NOTE(hayk): We can't call point3(), because it would recursively call basis(). + const Point3& n = p_; // Get the axis of rotation with the minimum projected length of the point - Vector3 axis; - double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z()); - if ((mx <= my) && (mx <= mz)) - axis = Vector3(1.0, 0.0, 0.0); - else if ((my <= mx) && (my <= mz)) - axis = Vector3(0.0, 1.0, 0.0); - else if ((mz <= mx) && (mz <= my)) - axis = Vector3(0.0, 0.0, 1.0); - else + Point3 axis; + double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); + if ((mx <= my) && (mx <= mz)) { + axis = Point3(1.0, 0.0, 0.0); + } else if ((my <= mx) && (my <= mz)) { + axis = Point3(0.0, 1.0, 0.0); + } else if ((mz <= mx) && (mz <= my)) { + axis = Point3(0.0, 0.0, 1.0); + } else { assert(false); + } - // Create the two basis vectors - Vector3 b1 = p_.cross(axis).normalized(); - Vector3 b2 = p_.cross(b1).normalized(); + // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with + // the chosen axis. + Matrix33 H_B1_n; + Point3 B1 = n.cross(axis, H ? &H_B1_n : nullptr); - // Create the basis matrix + // Normalize result to get a unit vector: b1 = B1 / |B1|. + Matrix33 H_b1_B1; + Point3 b1 = B1.normalize(H ? &H_b1_B1 : nullptr); + + // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. + // No need to normalize this, p and b1 are orthogonal unit vectors. + Matrix33 H_b2_n, H_b2_b1; + Point3 b2 = n.cross(b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); + + // Create the basis by stacking b1 and b2. B_.reset(Matrix32()); - (*B_) << b1, b2; + (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + + if (H) { + // Chain rule tomfoolery to compute the derivative. + const Matrix32& H_n_p = *B_; + Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; + Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + + // Cache the derivative and fill the result. + H_B_.reset(Matrix62()); + (*H_B_) << H_b1_p, H_b2_p; + *H = *H_B_; + } + return *B_; } /* ************************************************************************* */ -/// The print fuction +const Point3& Unit3::point3(OptionalJacobian<3, 2> H) const { + if (H) + *H = basis(); + return p_; +} + +/* ************************************************************************* */ +Vector3 Unit3::unitVector(boost::optional H) const { + if (H) + *H = basis(); + return (p_.vector()); +} + +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Unit3& pair) { + os << pair.p_ << endl; + return os; +} + +/* ************************************************************************* */ void Unit3::print(const std::string& s) const { cout << s << ":" << p_ << endl; } @@ -116,11 +167,72 @@ Matrix3 Unit3::skew() const { } /* ************************************************************************* */ -Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { +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; + const Point3& pn = point3(H_p ? &H_pn_p : 0); + + Matrix32 H_qn_q; + const 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 : nullptr, H_q ? &H_dot_qn : nullptr); + + if (H_p) { + (*H_p) << H_dot_pn * H_pn_p; + } + + if (H_q) { + (*H_q) = H_dot_qn * H_qn_q; + } + + return d; +} + +/* ************************************************************************* */ +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 - Vector2 xi = basis().transpose() * q.p_; - if (H) - *H = basis().transpose() * q.basis(); + Matrix23 Bt = basis().transpose(); + Vector2 xi = Bt * q.p_.vector(); + if (H_q) { + *H_q = Bt * q.basis(); + } + return xi; +} + +/* ************************************************************************* */ +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; + const 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 : nullptr).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; } @@ -136,39 +248,46 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { /* ************************************************************************* */ Unit3 Unit3::retract(const Vector2& v) const { - // Compute the 3D xi_hat vector - Vector3 xi_hat = basis() * v; - double theta = xi_hat.norm(); - // Treat case of very small v differently - if (theta < std::numeric_limits::epsilon()) { - return Unit3(cos(theta) * p_ + xi_hat); + // Get the vector form of the point and the basis matrix + Vector3 p = p_.vector(); + Matrix32 B = basis(); + + // Compute the 3D xi_hat vector + Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); + + double xi_hat_norm = xi_hat.norm(); + + // Avoid nan + if (xi_hat_norm == 0.0) { + if (v.norm() == 0.0) + return Unit3(point3()); + else + return Unit3(-point3()); } - Vector3 exp_p_xi_hat = - cos(theta) * p_ + xi_hat * (sin(theta) / theta); + Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p + + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); return Unit3(exp_p_xi_hat); } /* ************************************************************************* */ -Vector2 Unit3::localCoordinates(const Unit3& other) const { - const double x = p_.dot(other.p_); - // Crucial quantity here is y = theta/sin(theta) with theta=acos(x) - // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2) - // We treat the special case 1 and -1 below - const double x2 = x * x; - const double z = 1 - x2; - double y; - if (z < std::numeric_limits::epsilon()) { - if (x > 0) // first order expansion at x=1 - y = 1.0 - (x - 1.0) / 3.0; - else // cop out - return Vector2(M_PI, 0.0); - } else { +Vector2 Unit3::localCoordinates(const Unit3& y) const { + + Vector3 p = p_.vector(), q = y.p_.vector(); + double dot = p.dot(q); + + // Check for special cases + if (dot > 1.0 - 1e-16) + return Vector2(0, 0); + else if (dot < -1.0 + 1e-16) + return Vector2(M_PI, 0); + else { // no special case - y = acos(x) / sqrt(z); + double theta = acos(dot); + Vector3 result_hat = (theta / sin(theta)) * (q - p * dot); + return basis().transpose() * result_hat; } - return basis().transpose() * y * (other.p_ - x * p_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index b7e243419..784e5c5e1 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -20,16 +20,16 @@ #pragma once -#include #include -#include -#include +#include +#include -#include #include -#include +#include -#include +#ifdef GTSAM_USE_TBB +#include +#endif namespace gtsam { @@ -38,8 +38,13 @@ class GTSAM_EXPORT Unit3 { private: - Vector3 p_; ///< The location of the point on the unit sphere + Point3 p_; ///< The location of the point on the unit sphere mutable boost::optional B_; ///< Cached basis + mutable boost::optional H_B_; ///< Cached basis derivative + +#ifdef GTSAM_USE_TBB + mutable tbb::mutex B_mutex_; ///< Mutex to protect the cached basis. +#endif public: @@ -57,18 +62,23 @@ public: /// Construct from point explicit Unit3(const Point3& p) : - p_(p.vector().normalized()) { + p_(p.normalize()) { } /// Construct from a vector3 - explicit Unit3(const Vector3& p) : - p_(p.normalized()) { + explicit Unit3(const Vector3& v) : + p_(v / v.norm()) { } /// Construct from x,y,z Unit3(double x, double y, double z) : - p_(x, y, z) { - p_.normalize(); + p_(Point3(x, y, z).normalize()) { + } + + /// Construct from 2D point in plane at focal length f + /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point + explicit Unit3(const Point2& p, double f=1.0) : + p_(Point3(p.x(), p.y(), f).normalize()) { } /// Named constructor from Point3 with optional Jacobian @@ -83,12 +93,14 @@ public: /// @name Testable /// @{ + friend std::ostream& operator<<(std::ostream& os, const Unit3& pair); + /// The print fuction void print(const std::string& s = std::string()) const; /// The equals function with tolerance bool equals(const Unit3& s, double tol = 1e-9) const { - return equal_with_abs_tol(p_, s.p_, tol); + return p_.equals(s.p_, tol); } /// @} @@ -99,37 +111,50 @@ public: * Returns the local coordinate frame to tangent plane * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions * tangent to the sphere at the current direction. + * Provides derivatives of the basis with the two basis vectors stacked up as a 6x1. */ - const Matrix32& basis() const; + const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const; /// Return skew-symmetric associated with 3D point on unit sphere Matrix3 skew() const; /// Return unit-norm Point3 - Point3 point3(OptionalJacobian<3, 2> H = boost::none) const { - if (H) - *H = basis(); - return Point3(p_); - } + const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const; /// Return unit-norm Vector - const Vector3& unitVector(boost::optional H = boost::none) const { - if (H) - *H = basis(); - return p_; - } + Vector3 unitVector(boost::optional H = boost::none) const; /// Return scaled direction as Point3 friend Point3 operator*(double s, const Unit3& d) { - return Point3(s * d.p_); + return s * d.p_; } + /// Return dot product with q + double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, // + OptionalJacobian<1,2> H2 = boost::none) const; + /// Signed, vector-valued error between two directions - Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; + /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second. + Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const; + + /// Signed, vector-valued error between two directions + /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal. + Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, // + OptionalJacobian<2, 2> H_q = boost::none) const; /// Distance between two directions double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; + /// Cross-product between two Unit3s + Unit3 cross(const Unit3& q) const { + return Unit3(p_.cross(q.p_)); + } + + /// Cross-product w Point3 + Point3 cross(const Point3& q) const { + return Point3(p_.vector().cross(q.vector())); + } + /// @} /// @name Manifold @@ -147,7 +172,7 @@ public: enum CoordinatesMode { EXPMAP, ///< Use the exponential map to retract - RENORM ///< Retract with vector addition and renormalize. + RENORM ///< Retract with vector addtion and renormalize. }; /// The retract function @@ -167,6 +192,13 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(p_); + // homebrew serialize Eigen Matrix + ar & boost::serialization::make_nvp("B11", (*B_)(0, 0)); + ar & boost::serialization::make_nvp("B12", (*B_)(0, 1)); + ar & boost::serialization::make_nvp("B21", (*B_)(1, 0)); + ar & boost::serialization::make_nvp("B22", (*B_)(1, 1)); + ar & boost::serialization::make_nvp("B31", (*B_)(2, 0)); + ar & boost::serialization::make_nvp("B32", (*B_)(2, 1)); } /// @} diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index e55caaa3c..26fbf42bb 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -22,19 +22,26 @@ #include #include #include -#include +#include +#include +#include +#include +#include +#include +#include #include - #include #include #include +//#include #include #include using namespace boost::assign; using namespace gtsam; using namespace std; +using gtsam::symbol_shorthand::U; GTSAM_CONCEPT_TESTABLE_INST(Unit3) GTSAM_CONCEPT_MANIFOLD_INST(Unit3) @@ -43,7 +50,6 @@ GTSAM_CONCEPT_MANIFOLD_INST(Unit3) Point3 point3_(const Unit3& p) { return p.point3(); } - TEST(Unit3, point3) { vector ps; ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0) @@ -69,7 +75,7 @@ TEST(Unit3, rotate) { Unit3 actual = R * p; EXPECT(assert_equal(expected, actual, 1e-8)); Matrix actualH, expectedH; - + // Use numerical derivatives to calculate the expected Jacobian { expectedH = numericalDerivative21(rotate_, R, p); R.rotate(p, actualH, boost::none); @@ -93,8 +99,8 @@ TEST(Unit3, unrotate) { Unit3 expected = Unit3(1, 1, 0); Unit3 actual = R.unrotate(p); EXPECT(assert_equal(expected, actual, 1e-8)); - Matrix actualH, expectedH; + // Use numerical derivatives to calculate the expected Jacobian { expectedH = numericalDerivative21(unrotate_, R, p); R.unrotate(p, actualH, boost::none); @@ -107,6 +113,37 @@ TEST(Unit3, unrotate) { } } +TEST(Unit3, dot) { + Unit3 p(1, 0.2, 0.3); + Unit3 q = p.retract(Vector2(0.5, 0)); + Unit3 r = p.retract(Vector2(0.8, 0)); + Unit3 t = p.retract(Vector2(0, 0.3)); + EXPECT(assert_equal(1.0, p.dot(p), 1e-8)); + EXPECT(assert_equal(0.877583, p.dot(q), 1e-5)); + EXPECT(assert_equal(0.696707, p.dot(r), 1e-5)); + EXPECT(assert_equal(0.955336, p.dot(t), 1e-5)); + + // Use numerical derivatives to calculate the expected Jacobians + Matrix H1, H2; + boost::function f = boost::bind(&Unit3::dot, _1, _2, // + boost::none, boost::none); + { + p.dot(q, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-9)); + } + { + p.dot(r, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-9)); + } + { + p.dot(t, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, t), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, t), H2, 1e-9)); + } +} + //******************************************************************************* TEST(Unit3, error) { Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // @@ -116,6 +153,7 @@ TEST(Unit3, error) { EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), q); @@ -130,6 +168,45 @@ TEST(Unit3, error) { } } +//******************************************************************************* +TEST(Unit3, error2) { + Unit3 p(0.1, -0.2, 0.8); + Unit3 q = p.retract(Vector2(0.2, -0.1)); + Unit3 r = p.retract(Vector2(0.8, 0)); + + // Hard-coded as simple regression values + EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-8)); + EXPECT(assert_equal((Vector)(Vector2(0.198337495, -0.0991687475)), p.errorVector(q), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5)); + + Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian + { + expected = numericalDerivative21( + boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); + p.errorVector(q, actual, boost::none); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative21( + boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); + p.errorVector(r, actual, boost::none); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative22( + boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); + p.errorVector(q, boost::none, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative22( + boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); + p.errorVector(r, boost::none, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } +} + //******************************************************************************* TEST(Unit3, distance) { Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // @@ -155,107 +232,211 @@ TEST(Unit3, distance) { } //******************************************************************************* - -TEST(Unit3, localCoordinates) { - { - Unit3 p, q; - Vector2 expected = Vector2::Zero(); - Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(zero(2), actual, 1e-8)); - EXPECT(assert_equal(q, p.retract(expected), 1e-8)); - } - { - Unit3 p, q(1, 6.12385e-21, 0); - Vector2 expected = Vector2::Zero(); - Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(zero(2), actual, 1e-8)); - EXPECT(assert_equal(q, p.retract(expected), 1e-8)); - } - { - Unit3 p, q(-1, 0, 0); - Vector2 expected(M_PI, 0); - Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(q, p.retract(expected), 1e-8)); - } - { - Unit3 p, q(0, 1, 0); - Vector2 expected(0,-M_PI_2); - Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(q, p.retract(expected), 1e-8)); - } - { - Unit3 p, q(0, -1, 0); - Vector2 expected(0, M_PI_2); - Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(q, p.retract(expected), 1e-8)); - } - { - Unit3 p(0,1,0), q(0,-1,0); - Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(q, p.retract(actual), 1e-8)); - } - { - Unit3 p(0,0,1), q(0,0,-1); - Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(q, p.retract(actual), 1e-8)); - } - - double twist = 1e-4; - { - Unit3 p(0, 1, 0), q(0 - twist, -1 + twist, 0); - Vector2 actual = p.localCoordinates(q); - EXPECT(actual(0) < 1e-2); - EXPECT(actual(1) > M_PI - 1e-2) - } - { - Unit3 p(0, 1, 0), q(0 + twist, -1 - twist, 0); - Vector2 actual = p.localCoordinates(q); - EXPECT(actual(0) < 1e-2); - EXPECT(actual(1) < -M_PI + 1e-2) - } +TEST(Unit3, localCoordinates0) { + Unit3 p; + Vector actual = p.localCoordinates(p); + EXPECT(assert_equal(zero(2), actual, 1e-8)); } //******************************************************************************* +TEST(Unit3, localCoordinates1) { + Unit3 p, q(1, 6.12385e-21, 0); + Vector actual = p.localCoordinates(q); + CHECK(assert_equal(zero(2), actual, 1e-8)); +} + +//******************************************************************************* +TEST(Unit3, localCoordinates2) { + Unit3 p, q(-1, 0, 0); + Vector expected = (Vector(2) << M_PI, 0).finished(); + Vector actual = p.localCoordinates(q); + CHECK(assert_equal(expected, actual, 1e-8)); +} + +//******************************************************************************* +// Wrapper to make basis return a vector6 so we can test numerical derivatives. +Vector6 BasisTest(const Unit3& p, OptionalJacobian<6, 2> H) { + Matrix32 B = p.basis(H); + Vector6 B_vec; + B_vec << B; + return B_vec; +} + TEST(Unit3, basis) { - Unit3 p; - Matrix32 expected; - expected << 0, 0, 0, -1, 1, 0; - Matrix actual = p.basis(); - EXPECT(assert_equal(expected, actual, 1e-8)); + Unit3 p(0.1, -0.2, 0.9); + + Matrix expected(3, 2); + expected << 0.0, -0.994169047, 0.97618706, + -0.0233922129, 0.216930458, 0.105264958; + + Matrix62 actualH; + Matrix actual = p.basis(actualH); + EXPECT(assert_equal(expected, actual, 1e-6)); + + Matrix62 expectedH = numericalDerivative11( + boost::bind(BasisTest, _1, boost::none), p); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +//******************************************************************************* +/// Check the basis derivatives of a bunch of random Unit3s. +TEST(Unit3, basis_derivatives) { + int num_tests = 100; + boost::mt19937 rng(42); + for (int i = 0; i < num_tests; i++) { + Unit3 p = Unit3::Random(rng); + + Matrix62 actualH; + p.basis(actualH); + + Matrix62 expectedH = numericalDerivative11( + boost::bind(BasisTest, _1, boost::none), p); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); + } } //******************************************************************************* TEST(Unit3, retract) { - { - Unit3 p; - Vector2 v(0.5, 0); - Unit3 expected(0.877583, 0, 0.479426); - Unit3 actual = p.retract(v); - EXPECT(assert_equal(expected, actual, 1e-6)); - EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); - } - { - Unit3 p; - Vector2 v(0, 0); - Unit3 actual = p.retract(v); - EXPECT(assert_equal(p, actual, 1e-6)); - EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); - } + Unit3 p; + Vector v(2); + v << 0.5, 0; + Unit3 expected(0.877583, 0, 0.479426); + Unit3 actual = p.retract(v); + EXPECT(assert_equal(expected, actual, 1e-6)); + EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); } //******************************************************************************* TEST(Unit3, retract_expmap) { Unit3 p; - Vector2 v((M_PI / 2.0), 0); + Vector v(2); + v << (M_PI / 2.0), 0; Unit3 expected(Point3(0, 0, 1)); Unit3 actual = p.retract(v); EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); } +//******************************************************************************* +/// Returns a random vector +inline static Vector randomVector(const Vector& minLimits, + const Vector& maxLimits) { + + // Get the number of dimensions and create the return vector + size_t numDims = dim(minLimits); + Vector vector = zero(numDims); + + // Create the random vector + for (size_t i = 0; i < numDims; i++) { + double range = maxLimits(i) - minLimits(i); + vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); + } + return vector; +} + +//******************************************************************************* +// Let x and y be two Unit3's. +// The equality x.localCoordinates(x.retract(v)) == v should hold. +TEST(Unit3, localCoordinates_retract) { + + size_t numIterations = 10000; + Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit = + Vector3(1.0, 1.0, 1.0); + Vector minXiLimit = Vector2(-1.0, -1.0), maxXiLimit = Vector2(1.0, 1.0); + for (size_t i = 0; i < numIterations; i++) { + + // Sleep for the random number generator (TODO?: Better create all of them first). +// boost::this_thread::sleep(boost::posix_time::milliseconds(0)); + + // Create the two Unit3s. + // NOTE: You can not create two totally random Unit3's because you cannot always compute + // between two any Unit3's. (For instance, they might be at the different sides of the circle). + Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); +// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); + Vector v12 = randomVector(minXiLimit, maxXiLimit); + Unit3 s2 = s1.retract(v12); + + // Check if the local coordinates and retract return the same results. + Vector actual_v12 = s1.localCoordinates(s2); + EXPECT(assert_equal(v12, actual_v12, 1e-3)); + Unit3 actual_s2 = s1.retract(actual_v12); + EXPECT(assert_equal(s2, actual_s2, 1e-3)); + } +} + +//******************************************************************************* +// Let x and y be two Unit3's. +// The equality x.localCoordinates(x.retract(v)) == v should hold. +TEST(Unit3, localCoordinates_retract_expmap) { + + size_t numIterations = 10000; + Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit = + Vector3(1.0, 1.0, 1.0); + Vector minXiLimit = (Vector(2) << -M_PI, -M_PI).finished(), maxXiLimit = (Vector(2) << M_PI, M_PI).finished(); + for (size_t i = 0; i < numIterations; i++) { + + // Sleep for the random number generator (TODO?: Better create all of them first). +// boost::this_thread::sleep(boost::posix_time::milliseconds(0)); + + // Create the two Unit3s. + // Unlike the above case, we can use any two Unit3's. + Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); +// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); + Vector v12 = randomVector(minXiLimit, maxXiLimit); + + // Magnitude of the rotation can be at most pi + if (v12.norm() > M_PI) + v12 = v12 / M_PI; + Unit3 s2 = s1.retract(v12); + + // Check if the local coordinates and retract return the same results. + Vector actual_v12 = s1.localCoordinates(s2); + EXPECT(assert_equal(v12, actual_v12, 1e-3)); + Unit3 actual_s2 = s1.retract(actual_v12); + EXPECT(assert_equal(s2, actual_s2, 1e-3)); + } +} + +//******************************************************************************* +//TEST( Pose2, between ) +//{ +// // < +// // +// // ^ +// // +// // *--0--*--* +// Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y +// Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x +// +// Matrix actualH1,actualH2; +// Pose2 expected(M_PI/2.0, Point2(2,2)); +// Pose2 actual1 = gT1.between(gT2); +// Pose2 actual2 = gT1.between(gT2,actualH1,actualH2); +// EXPECT(assert_equal(expected,actual1)); +// EXPECT(assert_equal(expected,actual2)); +// +// Matrix expectedH1 = (Matrix(3,3) << +// 0.0,-1.0,-2.0, +// 1.0, 0.0,-2.0, +// 0.0, 0.0,-1.0 +// ); +// Matrix numericalH1 = numericalDerivative21(testing::between, gT1, gT2); +// EXPECT(assert_equal(expectedH1,actualH1)); +// EXPECT(assert_equal(numericalH1,actualH1)); +// // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx +// EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1)); +// +// Matrix expectedH2 = (Matrix(3,3) << +// 1.0, 0.0, 0.0, +// 0.0, 1.0, 0.0, +// 0.0, 0.0, 1.0 +// ); +// Matrix numericalH2 = numericalDerivative22(testing::between, gT1, gT2); +// EXPECT(assert_equal(expectedH2,actualH2)); +// EXPECT(assert_equal(numericalH2,actualH2)); +// +//} + //******************************************************************************* TEST(Unit3, Random) { boost::mt19937 rng(42); @@ -267,26 +448,6 @@ TEST(Unit3, Random) { EXPECT(assert_equal(expectedMean,actualMean,0.1)); } -//******************************************************************************* -// New test that uses Unit3::Random -TEST(Unit3, localCoordinates_retract) { - boost::mt19937 rng(42); - size_t numIterations = 10000; - - for (size_t i = 0; i < numIterations; i++) { - // Create two random Unit3s - const Unit3 s1 = Unit3::Random(rng); - const Unit3 s2 = Unit3::Random(rng); - // Check that they are not at opposite ends of the sphere, which is ill defined - if (s1.unitVector().dot(s2.unitVector())<-0.9) continue; - - // Check if the local coordinates and retract return consistent results. - Vector v12 = s1.localCoordinates(s2); - Unit3 actual_s2 = s1.retract(v12); - EXPECT(assert_equal(s2, actual_s2, 1e-9)); - } -} - //************************************************************************* TEST (Unit3, FromPoint3) { Matrix actualH; @@ -298,12 +459,42 @@ TEST (Unit3, FromPoint3) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } -/* ************************************************************************* */ -TEST(actualH, Serialization) { - Unit3 p(0, 1, 0); - EXPECT(serializationTestHelpers::equalsObj(p)); - EXPECT(serializationTestHelpers::equalsXML(p)); - EXPECT(serializationTestHelpers::equalsBinary(p)); +//******************************************************************************* +TEST(Unit3, ErrorBetweenFactor) { + std::vector data = {Unit3(1.0, 0.0, 0.0), Unit3(0.0, 0.0, 1.0)}; + + NonlinearFactorGraph graph; + Values initial_values; + + // Add prior factors. + SharedNoiseModel R_prior = noiseModel::Unit::Create(2); + for (int i = 0; i < data.size(); i++) { + graph.add(PriorFactor(U(i), data[i], R_prior)); + } + + // Add process factors using the dot product error function. + SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01); + for (int i = 0; i < data.size() - 1; i++) { + Expression exp(Expression(U(i)), &Unit3::errorVector, Expression(U(i + 1))); + graph.addExpressionFactor(R_process, Vector2::Zero(), exp); + } + + // Add initial values. Since there is no identity, just pick something. + for (int i = 0; i < data.size(); i++) { + initial_values.insert(U(i), Unit3(0.0, 1.0, 0.0)); + } + + Values values = GaussNewtonOptimizer(graph, initial_values).optimize(); + + // Check that the y-value is very small for each. + for (int i = 0; i < data.size(); i++) { + EXPECT(assert_equal(0.0, values.at(U(i)).unitVector().y(), 1e-3)); + } + + // Check that the dot product between variables is close to 1. + for (int i = 0; i < data.size() - 1; i++) { + EXPECT(assert_equal(1.0, values.at(U(i)).dot(values.at(U(i + 1))), 1e-2)); + } } /* ************************************************************************* */