diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index a74e39f47..a4153643e 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -15,12 +15,12 @@ * @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 #include // for GTSAM_USE_TBB #ifdef __clang__ @@ -62,12 +62,10 @@ 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(); - Unit3 result; - result.p_ = Point3(d[0], d[1], d[2]); - return result; + return Unit3(d[0], d[1], d[2]); } #ifdef GTSAM_USE_TBB @@ -85,26 +83,24 @@ const Matrix32& Unit3::basis() const { return *B_; // Get the axis of rotation with the minimum projected length of the point - Point3 axis; + Vector3 axis; double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z()); if ((mx <= my) && (mx <= mz)) - axis = Point3(1.0, 0.0, 0.0); + axis = Vector3(1.0, 0.0, 0.0); else if ((my <= mx) && (my <= mz)) - axis = Point3(0.0, 1.0, 0.0); + axis = Vector3(0.0, 1.0, 0.0); else if ((mz <= mx) && (mz <= my)) - axis = Point3(0.0, 0.0, 1.0); + axis = Vector3(0.0, 0.0, 1.0); else assert(false); // Create the two basis vectors - Point3 b1 = p_.cross(axis); - b1 = b1 / b1.norm(); - Point3 b2 = p_.cross(b1); - b2 = b2 / b2.norm(); + Vector3 b1 = p_.cross(axis).normalized(); + Vector3 b2 = p_.cross(b1).normalized(); // Create the basis matrix B_.reset(Matrix32()); - (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + (*B_) << b1, b2; return *B_; } @@ -122,10 +118,9 @@ Matrix3 Unit3::skew() const { /* ************************************************************************* */ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) 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_.vector(); + Vector2 xi = basis().transpose() * q.p_; if (H) - *H = Bt * q.basis(); + *H = basis().transpose() * q.basis(); return xi; } @@ -142,44 +137,34 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { /* ************************************************************************* */ Unit3 Unit3::retract(const Vector2& v) const { - // 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); - + Vector3 xi_hat = basis() * v; 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()); + // When v is the so small and approximate as a direction + if (xi_hat_norm < 1e-8) { + return Unit3(cos(xi_hat_norm) * p_ + xi_hat); } - Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p + 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& y) const { - Vector3 p = p_.vector(), q = y.p_.vector(); - double dot = p.dot(q); + double dot = p_.dot(y.p_); // Check for special cases if (std::abs(dot - 1.0) < 1e-16) - return Vector2(0, 0); + return Vector2(0.0, 0.0); else if (std::abs(dot + 1.0) < 1e-16) - return Vector2(M_PI, 0); + return Vector2(M_PI, 0.0); else { // no special case - double theta = acos(dot); - Vector3 result_hat = (theta / sin(theta)) * (q - p * dot); + const double theta = acos(dot); + Vector3 result_hat = (theta / sin(theta)) * (y.p_ - p_ * dot); return basis().transpose() * result_hat; } } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index f8cea092e..9d0444844 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -20,11 +20,16 @@ #pragma once -#include #include +#include +#include +#include -#include #include +#include +#include + +#include namespace gtsam { @@ -33,7 +38,7 @@ class GTSAM_EXPORT Unit3 { private: - Point3 p_; ///< The location of the point on the unit sphere + Vector3 p_; ///< The location of the point on the unit sphere mutable boost::optional B_; ///< Cached basis public: @@ -52,18 +57,18 @@ public: /// Construct from point explicit Unit3(const Point3& p) : - p_(p / p.norm()) { + p_(p.vector().normalized()) { } /// Construct from a vector3 explicit Unit3(const Vector3& p) : - p_(p / p.norm()) { + p_(p.normalized()) { } /// Construct from x,y,z Unit3(double x, double y, double z) : p_(x, y, z) { - p_ = p_ / p_.norm(); + p_.normalize(); } /// Named constructor from Point3 with optional Jacobian @@ -83,7 +88,7 @@ public: /// The equals function with tolerance bool equals(const Unit3& s, double tol = 1e-9) const { - return p_.equals(s.p_, tol); + return equal_with_abs_tol(p_, s.p_, tol); } /// @} @@ -101,22 +106,22 @@ public: Matrix3 skew() const; /// Return unit-norm Point3 - const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const { + Point3 point3(OptionalJacobian<3, 2> H = boost::none) const { + if (H) + *H = basis(); + return Point3(p_); + } + + /// Return unit-norm Vector + const Vector3& unitVector(boost::optional H = boost::none) const { if (H) *H = basis(); return p_; } - /// Return unit-norm Vector - Vector3 unitVector(boost::optional H = boost::none) const { - if (H) - *H = basis(); - return (p_.vector()); - } - /// Return scaled direction as Point3 friend Point3 operator*(double s, const Unit3& d) { - return s * d.p_; + return Point3(s * d.p_); } /// Signed, vector-valued error between two directions diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 1d0c28ded..16c7cd0e7 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -41,6 +41,7 @@ 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) @@ -66,7 +67,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); @@ -90,8 +91,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); @@ -113,7 +114,6 @@ 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); @@ -153,31 +153,44 @@ TEST(Unit3, distance) { } //******************************************************************************* -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, localCoordinates) { + { + Unit3 p; + Vector2 actual = p.localCoordinates(p); + EXPECT(assert_equal(zero(2), actual, 1e-8)); + } + { + Unit3 p, q(1, 6.12385e-21, 0); + Vector2 actual = p.localCoordinates(q); + CHECK(assert_equal(zero(2), actual, 1e-8)); + } + { + Unit3 p, q(-1, 0, 0); + Vector2 expected(M_PI, 0); + Vector2 actual = p.localCoordinates(q); + CHECK(assert_equal(expected, 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)); + 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, basis) { Unit3 p; - Matrix expected(3, 2); + Matrix32 expected; expected << 0, 0, 0, -1, 1, 0; Matrix actual = p.basis(); EXPECT(assert_equal(expected, actual, 1e-8)); @@ -185,20 +198,27 @@ TEST(Unit3, basis) { //******************************************************************************* TEST(Unit3, retract) { - 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)); + { + 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)); + } } //******************************************************************************* TEST(Unit3, retract_expmap) { Unit3 p; - Vector v(2); - v << (M_PI / 2.0), 0; + Vector2 v((M_PI / 2.0), 0); Unit3 expected(Point3(0, 0, 1)); Unit3 actual = p.retract(v); EXPECT(assert_equal(expected, actual, 1e-8)); @@ -228,9 +248,11 @@ inline static Vector randomVector(const Vector& minLimits, 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); + Vector3 minSphereLimit(-1.0, -1.0, -1.0); + Vector3 maxSphereLimit(1.0, 1.0, 1.0); + Vector2 minXiLimit(-1.0, -1.0); + Vector2 maxXiLimit(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). @@ -246,9 +268,9 @@ TEST(Unit3, localCoordinates_retract) { // 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)); + EXPECT(assert_equal(v12, actual_v12, 1e-8)); Unit3 actual_s2 = s1.retract(actual_v12); - EXPECT(assert_equal(s2, actual_s2, 1e-3)); + EXPECT(assert_equal(s2, actual_s2, 1e-8)); } } @@ -258,30 +280,26 @@ TEST(Unit3, localCoordinates_retract) { 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(); + Vector3 minSphereLimit = Vector3(-1.0, -1.0, -1.0); + Vector3 maxSphereLimit = Vector3(1.0, 1.0, 1.0); + Vector2 minXiLimit = Vector2(-M_PI, -M_PI); + Vector2 maxXiLimit = Vector2(M_PI, M_PI); + 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); + Vector v = 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); + if (v.norm() > M_PI) + v = v / M_PI; + Unit3 s2 = s1.retract(v); - // 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)); + EXPECT(assert_equal(v, s1.localCoordinates(s1.retract(v)), 1e-6)); + EXPECT(assert_equal(s2, s1.retract(s1.localCoordinates(s2)), 1e-6)); } } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index f2f8a01cf..2a4d7c746 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -241,7 +242,7 @@ namespace gtsam { virtual Matrix R() const { return thisR();} /// Compute information matrix - virtual Matrix information() const { return thisR().transpose() * thisR(); } + virtual Matrix information() const { return R().transpose() * R(); } /// Compute covariance matrix virtual Matrix covariance() const { return information().inverse(); } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index b2afb1709..37b55fc03 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -33,19 +33,12 @@ using namespace gtsam; using namespace noiseModel; using namespace boost::assign; -static const double kSigma = 2, s_1=1.0/kSigma, kVariance = kSigma*kSigma, prc = 1.0/kVariance; -static const Matrix R = (Matrix(3, 3) << - s_1, 0.0, 0.0, - 0.0, s_1, 0.0, - 0.0, 0.0, s_1).finished(); -static const Matrix kCovariance = (Matrix(3, 3) << - kVariance, 0.0, 0.0, - 0.0, kVariance, 0.0, - 0.0, 0.0, kVariance).finished(); +static const double kSigma = 2, kInverseSigma = 1.0 / kSigma, + kVariance = kSigma * kSigma, prc = 1.0 / kVariance; +static const Matrix R = Matrix3::Identity() * kInverseSigma; +static const Matrix kCovariance = Matrix3::Identity() * kVariance; static const Vector3 kSigmas(kSigma, kSigma, kSigma); -//static double inf = numeric_limits::infinity(); - /* ************************************************************************* */ TEST(NoiseModel, constructors) { @@ -58,8 +51,8 @@ TEST(NoiseModel, constructors) m.push_back(Gaussian::Covariance(kCovariance,false)); m.push_back(Gaussian::Information(kCovariance.inverse(),false)); m.push_back(Diagonal::Sigmas(kSigmas,false)); - m.push_back(Diagonal::Variances((Vector(3) << kVariance, kVariance, kVariance).finished(),false)); - m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished(),false)); + m.push_back(Diagonal::Variances((Vector3(kVariance, kVariance, kVariance)),false)); + m.push_back(Diagonal::Precisions(Vector3(prc, prc, prc),false)); m.push_back(Isotropic::Sigma(3, kSigma,false)); m.push_back(Isotropic::Variance(3, kVariance,false)); m.push_back(Isotropic::Precision(3, prc,false)); @@ -82,25 +75,23 @@ TEST(NoiseModel, constructors) DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9); // test R matrix - Matrix expectedR((Matrix(3, 3) << - s_1, 0.0, 0.0, - 0.0, s_1, 0.0, - 0.0, 0.0, s_1).finished()); - BOOST_FOREACH(Gaussian::shared_ptr mi, m) - EXPECT(assert_equal(expectedR,mi->R())); + EXPECT(assert_equal(R,mi->R())); + + // test covariance + BOOST_FOREACH(Gaussian::shared_ptr mi, m) + EXPECT(assert_equal(kCovariance,mi->covariance())); + + // test covariance + BOOST_FOREACH(Gaussian::shared_ptr mi, m) + EXPECT(assert_equal(kCovariance.inverse(),mi->information())); // test Whiten operator Matrix H((Matrix(3, 4) << 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 1.0).finished()); - - Matrix expected((Matrix(3, 4) << - 0.0, 0.0, s_1, s_1, - 0.0, s_1, 0.0, s_1, - s_1, 0.0, 0.0, s_1).finished()); - + Matrix expected = kInverseSigma * H; BOOST_FOREACH(Gaussian::shared_ptr mi, m) EXPECT(assert_equal(expected,mi->Whiten(H))); @@ -122,7 +113,7 @@ TEST(NoiseModel, equals) { Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), g2 = Gaussian::SqrtInformation(eye(3,3)); - Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << kSigma, kSigma, kSigma).finished()), + Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector3(kSigma, kSigma, kSigma)), d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma), i2 = Isotropic::Sigma(3, 0.7); @@ -141,7 +132,7 @@ TEST(NoiseModel, equals) ///* ************************************************************************* */ //TEST(NoiseModel, ConstrainedSmart ) //{ -// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma), true); +// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector3(sigma, 0.0, sigma), true); // Diagonal::shared_ptr n1 = boost::dynamic_pointer_cast(nonconstrained); // Constrained::shared_ptr n2 = boost::dynamic_pointer_cast(nonconstrained); // EXPECT(n1); @@ -160,8 +151,8 @@ TEST(NoiseModel, ConstrainedConstructors ) Constrained::shared_ptr actual; size_t d = 3; double m = 100.0; - Vector sigmas = (Vector(3) << kSigma, 0.0, 0.0).finished(); - Vector mu = Vector3(200.0, 300.0, 400.0); + Vector3 sigmas(kSigma, 0.0, 0.0); + Vector3 mu(200.0, 300.0, 400.0); actual = Constrained::All(d); // TODO: why should this be a thousand ??? Dummy variable? EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu())); @@ -187,7 +178,7 @@ TEST(NoiseModel, ConstrainedMixed ) { Vector feasible = Vector3(1.0, 0.0, 1.0), infeasible = Vector3(1.0, 1.0, 1.0); - Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << kSigma, 0.0, kSigma).finished()); + Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector3(kSigma, 0.0, kSigma)); // NOTE: we catch constrained variables elsewhere, so whitening does nothing EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible))); EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible))); @@ -346,7 +337,7 @@ TEST(NoiseModel, robustNoise) { const double k = 10.0, error1 = 1.0, error2 = 100.0; Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0).finished(); - Vector b = (Vector(2) << error1, error2).finished(); + Vector b = Vector2(error1, error2); const Robust::shared_ptr robust = Robust::Create( mEstimator::Huber::Create(k, mEstimator::Huber::Scalar), Unit::Create(2));