diff --git a/gtsam.h b/gtsam.h index c42c4582a..614db91c7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2292,8 +2292,10 @@ virtual class NonlinearOptimizerParams { }; bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); +bool checkConvergence(const gtsam::NonlinearOptimizerParams& params, + double currentError, double newError); #include virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { @@ -2529,7 +2531,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2552,7 +2554,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include -template +template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); @@ -2793,8 +2795,10 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, // std::vector::shared_ptr> class BetweenFactorPose3s { + BetweenFactorPose3s(); size_t size() const; gtsam::BetweenFactorPose3* at(size_t i) const; + void push_back(const gtsam::BetweenFactorPose3* factor); }; #include diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 26c8a80d2..fe730c934 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -170,7 +170,7 @@ namespace internal { /// Assumes existence of: identity, dimension, localCoordinates, retract, /// and additionally Logmap, Expmap, compose, between, and inverse template -struct LieGroupTraits { +struct LieGroupTraits: GetDimensionImpl { typedef lie_group_tag structure_category; /// @name Group @@ -186,8 +186,6 @@ struct LieGroupTraits { typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; - static int GetDimension(const Class&) {return dimension;} - static TangentVector Local(const Class& origin, const Class& other, ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { return origin.localCoordinates(other, Horigin, Hother); diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 21df90513..f3653f099 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -72,7 +72,7 @@ struct HasManifoldPrereqs { /// Extra manifold traits for fixed-dimension types template -struct ManifoldImpl { +struct GetDimensionImpl { // Compile-time dimensionality static int GetDimension(const Class&) { return N; @@ -81,7 +81,7 @@ struct ManifoldImpl { /// Extra manifold traits for variable-dimension types template -struct ManifoldImpl { +struct GetDimensionImpl { // Run-time dimensionality static int GetDimension(const Class& m) { return m.dim(); @@ -92,7 +92,7 @@ struct ManifoldImpl { /// To use this for your class type, define: /// template<> struct traits : public internal::ManifoldTraits { }; template -struct ManifoldTraits: ManifoldImpl { +struct ManifoldTraits: GetDimensionImpl { // Check that Class has the necessary machinery BOOST_CONCEPT_ASSERT((HasManifoldPrereqs)); diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 19c016795..c86b9b860 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -261,13 +261,13 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special - if (std::abs(tr + 1.0) < 1e-10) { - if (std::abs(R33 + 1.0) > 1e-10) + if (tr + 1.0 < 1e-10) { + if (std::abs(R33 + 1.0) > 1e-5) omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); - else if (std::abs(R22 + 1.0) > 1e-10) + else if (std::abs(R22 + 1.0) > 1e-5) omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); else - // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit + // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); } else { double magnitude; diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp index 9b6e4217a..8a78bb83f 100644 --- a/gtsam/geometry/SO4.cpp +++ b/gtsam/geometry/SO4.cpp @@ -33,9 +33,10 @@ using namespace std; namespace gtsam { +// TODO(frank): is this better than SOn::Random? // /* ************************************************************************* // */ static Vector3 randomOmega(boost::mt19937 &rng) { -// static boost::uniform_real randomAngle(-M_PI, M_PI); +// static std::uniform_real_distribution randomAngle(-M_PI, M_PI); // return Unit3::Random(rng).unitVector() * randomAngle(rng); // } @@ -58,9 +59,9 @@ Matrix4 SO4::Hat(const Vector6& xi) { Y(0, 1) = -xi(5); Y(0, 2) = +xi(4); Y(1, 2) = -xi(3); - Y(0, 3) = -xi(2); - Y(1, 3) = +xi(1); - Y(2, 3) = -xi(0); + Y(0, 3) = +xi(2); + Y(1, 3) = -xi(1); + Y(2, 3) = +xi(0); return Y - Y.transpose(); } @@ -72,9 +73,9 @@ Vector6 SO4::Vee(const Matrix4& X) { xi(5) = -X(0, 1); xi(4) = +X(0, 2); xi(3) = -X(1, 2); - xi(2) = -X(0, 3); - xi(1) = +X(1, 3); - xi(0) = -X(2, 3); + xi(2) = +X(0, 3); + xi(1) = -X(1, 3); + xi(0) = +X(2, 3); return xi; } @@ -208,9 +209,9 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) { if (H) { const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2), q = R.topRightCorner<3, 1>(); - *H << Z_3x1, Z_3x1, q, Z_3x1, -m3, m2, // - Z_3x1, -q, Z_3x1, m3, Z_3x1, -m1, // - q, Z_3x1, Z_3x1, -m2, m1, Z_3x1; + *H << Z_3x1, Z_3x1, -q, Z_3x1, -m3, m2, // + Z_3x1, q, Z_3x1, m3, Z_3x1, -m1, // + -q, Z_3x1, Z_3x1, -m2, m1, Z_3x1; } return M; } @@ -221,9 +222,9 @@ GTSAM_EXPORT Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) { const Matrix43 M = R.leftCols<3>(); if (H) { const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3); - *H << Z_4x1, Z_4x1, q, Z_4x1, -m3, m2, // - Z_4x1, -q, Z_4x1, m3, Z_4x1, -m1, // - q, Z_4x1, Z_4x1, -m2, m1, Z_4x1; + *H << Z_4x1, Z_4x1, -q, Z_4x1, -m3, m2, // + Z_4x1, q, Z_4x1, m3, Z_4x1, -m1, // + -q, Z_4x1, Z_4x1, -m2, m1, Z_4x1; } return M; } diff --git a/gtsam/geometry/SOn.cpp b/gtsam/geometry/SOn.cpp index 0676b4a67..37b6c1784 100644 --- a/gtsam/geometry/SOn.cpp +++ b/gtsam/geometry/SOn.cpp @@ -38,11 +38,12 @@ Matrix SOn::Hat(const Vector& xi) { const size_t dmin = (n - 1) * (n - 2) / 2; X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin)); + // determine sign of last element (signs alternate) + double sign = pow(-1.0, xi.size()); // Now fill last row and column - double sign = 1.0; for (size_t i = 0; i < n - 1; i++) { const size_t j = n - 2 - i; - X(n - 1, j) = sign * xi(i); + X(n - 1, j) = -sign * xi(i); X(j, n - 1) = -X(n - 1, j); sign = -sign; } @@ -67,10 +68,10 @@ Vector SOn::Vee(const Matrix& X) { Vector xi(d); // Fill first n-1 spots from last row of X - double sign = 1.0; + double sign = pow(-1.0, xi.size()); for (size_t i = 0; i < n - 1; i++) { const size_t j = n - 2 - i; - xi(i) = sign * X(n - 1, j); + xi(i) = -sign * X(n - 1, j); sign = -sign; } diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 117c2336e..5313d3018 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -24,10 +24,11 @@ #include -#include +#include // TODO(frank): how to avoid? #include #include #include +#include namespace gtsam { @@ -93,6 +94,16 @@ class SO : public LieGroup, internal::DimensionSO(N)> { return SO(R); } + /// Named constructor from lower dimensional matrix + template > + static SO Lift(size_t n, const Eigen::MatrixBase &R) { + Matrix Q = Matrix::Identity(n, n); + size_t p = R.rows(); + assert(p < n && R.cols() == p); + Q.topLeftCorner(p, p) = R; + return SO(Q); + } + /// Construct dynamic SO(n) from Fixed SO template > explicit SO(const SO& R) : matrix_(R.matrix()) {} @@ -207,11 +218,11 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * etc... For example, the vector-space isomorphic to so(5) is laid out as: * a b c d | u v w | x y | z * where the latter elements correspond to "telescoping" sub-algebras: - * 0 -z y -w d - * z 0 -x v -c - * -y x 0 -u b - * w -v u 0 -a - * -d c -b a 0 + * 0 -z y w -d + * z 0 -x -v c + * -y x 0 u -b + * -w v -u 0 a + * d -c b -a 0 * This scheme behaves exactly as expected for SO(2) and SO(3). */ static MatrixNN Hat(const TangentVector& xi); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 182db59b0..598c57b24 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -181,63 +181,81 @@ TEST( Rot3, retract) } /* ************************************************************************* */ -TEST(Rot3, log) -{ +TEST(Rot3, log) { static const double PI = boost::math::constants::pi(); Vector w; Rot3 R; -#define CHECK_OMEGA(X,Y,Z) \ - w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ - R = Rot3::Rodrigues(w); \ - EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); +#define CHECK_OMEGA(X, Y, Z) \ + w = (Vector(3) << X, Y, Z).finished(); \ + R = Rot3::Rodrigues(w); \ + EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12)); // Check zero - CHECK_OMEGA( 0, 0, 0) + CHECK_OMEGA(0, 0, 0) // create a random direction: - double norm=sqrt(1.0+16.0+4.0); - double x=1.0/norm, y=4.0/norm, z=2.0/norm; + double norm = sqrt(1.0 + 16.0 + 4.0); + double x = 1.0 / norm, y = 4.0 / norm, z = 2.0 / norm; // Check very small rotation for Taylor expansion // Note that tolerance above is 1e-12, so Taylor is pretty good ! double d = 0.0001; - CHECK_OMEGA( d, 0, 0) - CHECK_OMEGA( 0, d, 0) - CHECK_OMEGA( 0, 0, d) - CHECK_OMEGA(x*d, y*d, z*d) + CHECK_OMEGA(d, 0, 0) + CHECK_OMEGA(0, d, 0) + CHECK_OMEGA(0, 0, d) + CHECK_OMEGA(x * d, y * d, z * d) // check normal rotation d = 0.1; - CHECK_OMEGA( d, 0, 0) - CHECK_OMEGA( 0, d, 0) - CHECK_OMEGA( 0, 0, d) - CHECK_OMEGA(x*d, y*d, z*d) + CHECK_OMEGA(d, 0, 0) + CHECK_OMEGA(0, d, 0) + CHECK_OMEGA(0, 0, d) + CHECK_OMEGA(x * d, y * d, z * d) // Check 180 degree rotations - CHECK_OMEGA( PI, 0, 0) - CHECK_OMEGA( 0, PI, 0) - CHECK_OMEGA( 0, 0, PI) + CHECK_OMEGA(PI, 0, 0) + CHECK_OMEGA(0, PI, 0) + CHECK_OMEGA(0, 0, PI) // Windows and Linux have flipped sign in quaternion mode -#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS) - w = (Vector(3) << x*PI, y*PI, z*PI).finished(); +#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS) + w = (Vector(3) << x * PI, y * PI, z * PI).finished(); R = Rot3::Rodrigues(w); - EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12)); + EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12)); #else - CHECK_OMEGA(x*PI,y*PI,z*PI) + CHECK_OMEGA(x * PI, y * PI, z * PI) #endif // Check 360 degree rotations -#define CHECK_OMEGA_ZERO(X,Y,Z) \ - w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ - R = Rot3::Rodrigues(w); \ - EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R))); +#define CHECK_OMEGA_ZERO(X, Y, Z) \ + w = (Vector(3) << X, Y, Z).finished(); \ + R = Rot3::Rodrigues(w); \ + EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R))); - CHECK_OMEGA_ZERO( 2.0*PI, 0, 0) - CHECK_OMEGA_ZERO( 0, 2.0*PI, 0) - CHECK_OMEGA_ZERO( 0, 0, 2.0*PI) - CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) + CHECK_OMEGA_ZERO(2.0 * PI, 0, 0) + CHECK_OMEGA_ZERO(0, 2.0 * PI, 0) + CHECK_OMEGA_ZERO(0, 0, 2.0 * PI) + CHECK_OMEGA_ZERO(x * 2. * PI, y * 2. * PI, z * 2. * PI) + + // Check problematic case from Lund dataset vercingetorix.g2o + // This is an almost rotation with determinant not *quite* 1. + Rot3 Rlund(-0.98582676, -0.03958746, -0.16303092, // + -0.03997006, -0.88835923, 0.45740671, // + -0.16293753, 0.45743998, 0.87418537); + + // Rot3's Logmap returns different, but equivalent compacted + // axis-angle vectors depending on whether Rot3 is implemented + // by Quaternions or SO3. + #if defined(GTSAM_USE_QUATERNIONS) + // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees + EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211), + (Vector)Rot3::Logmap(Rlund), 1e-8)); + #else + // SO3 does not bound angle resulting in ~180.1 degrees + EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314), + (Vector)Rot3::Logmap(Rlund), 1e-8)); + #endif } /* ************************************************************************* */ @@ -536,16 +554,15 @@ TEST( Rot3, logmapStability ) { TEST(Rot3, quaternion) { // NOTE: This is also verifying the ability to convert Vector to Quaternion Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782); - Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) << - 0.271018623057411, 0.278786459830371, 0.921318086098018, - 0.578529366719085, 0.717799701969298, -0.387385285854279, - -0.769319620053772, 0.637998195662053, 0.033250932803219).finished()); + Rot3 R1(0.271018623057411, 0.278786459830371, 0.921318086098018, + 0.578529366719085, 0.717799701969298, -0.387385285854279, + -0.769319620053772, 0.637998195662053, 0.033250932803219); - Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053); - Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) << - -0.207341903877828, 0.250149415542075, 0.945745528564780, - 0.881304914479026, -0.371869043667957, 0.291573424846290, - 0.424630407073532, 0.893945571198514, -0.143353873763946).finished()); + Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, + 0.599136268678053); + Rot3 R2(-0.207341903877828, 0.250149415542075, 0.945745528564780, + 0.881304914479026, -0.371869043667957, 0.291573424846290, + 0.424630407073532, 0.893945571198514, -0.143353873763946); // Check creating Rot3 from quaternion EXPECT(assert_equal(R1, Rot3(q1))); diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index eb84a4d55..1cf8caed2 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -46,7 +46,7 @@ TEST(SOn, SO0) { EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); EXPECT_LONGS_EQUAL(0, R.dim()); - EXPECT_LONGS_EQUAL(-1, traits::GetDimension(R)); + EXPECT_LONGS_EQUAL(0, traits::GetDimension(R)); } //****************************************************************************** @@ -56,7 +56,7 @@ TEST(SOn, SO5) { EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); EXPECT_LONGS_EQUAL(10, R.dim()); - EXPECT_LONGS_EQUAL(-1, traits::GetDimension(R)); + EXPECT_LONGS_EQUAL(10, traits::GetDimension(R)); } //****************************************************************************** @@ -95,32 +95,42 @@ TEST(SOn, Random) { //****************************************************************************** TEST(SOn, HatVee) { - Vector6 v; - v << 1, 2, 3, 4, 5, 6; + Vector10 v; + v << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; Matrix expected2(2, 2); expected2 << 0, -1, 1, 0; const auto actual2 = SOn::Hat(v.head<1>()); - CHECK(assert_equal(expected2, actual2)); - CHECK(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2))); + EXPECT(assert_equal(expected2, actual2)); + EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2))); Matrix expected3(3, 3); - expected3 << 0, -3, 2, // - 3, 0, -1, // - -2, 1, 0; + expected3 << 0, -3, 2, // + 3, 0, -1, // + -2, 1, 0; const auto actual3 = SOn::Hat(v.head<3>()); - CHECK(assert_equal(expected3, actual3)); - CHECK(assert_equal(skewSymmetric(1, 2, 3), actual3)); - CHECK(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3))); + EXPECT(assert_equal(expected3, actual3)); + EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3)); + EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3))); Matrix expected4(4, 4); - expected4 << 0, -6, 5, -3, // - 6, 0, -4, 2, // - -5, 4, 0, -1, // - 3, -2, 1, 0; - const auto actual4 = SOn::Hat(v); - CHECK(assert_equal(expected4, actual4)); - CHECK(assert_equal((Vector)v, SOn::Vee(actual4))); + expected4 << 0, -6, 5, 3, // + 6, 0, -4, -2, // + -5, 4, 0, 1, // + -3, 2, -1, 0; + const auto actual4 = SOn::Hat(v.head<6>()); + EXPECT(assert_equal(expected4, actual4)); + EXPECT(assert_equal((Vector)v.head<6>(), SOn::Vee(actual4))); + + Matrix expected5(5, 5); + expected5 << 0,-10, 9, 7, -4, // + 10, 0, -8, -6, 3, // + -9, 8, 0, 5, -2, // + -7, 6, -5, 0, 1, // + 4, -3, 2, -1, 0; + const auto actual5 = SOn::Hat(v); + EXPECT(assert_equal(expected5, actual5)); + EXPECT(assert_equal((Vector)v, SOn::Vee(actual5))); } //******************************************************************************