From 51c4a50c2305c426bb352a8690499346899bed9a Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Fri, 5 Dec 2014 09:28:10 -0500 Subject: [PATCH 01/28] Initial broken commit of Similarity Transform --- .../geometry/tests/testSimilarity3.cpp | 125 ++++++++++++++++++ 1 file changed, 125 insertions(+) create mode 100644 gtsam_unstable/geometry/tests/testSimilarity3.cpp diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp new file mode 100644 index 000000000..a71308b7a --- /dev/null +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -0,0 +1,125 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSimilarity3.cpp + * @brief Unit tests for Similarity3 class + */ + +#include +#include +#include + +namespace gtsam { + +/** + * 3D similarity transform + */ +class Similarity3: private Matrix4 { + Similarity3() : + Matrix4::Identity() { + } + + /// Construct pure scaling + Similarity3(double s) : + Matrix4::Identity() { + this->topLeftCorner<3, 3>() = s * Matrix3::Identity(); + } + + /// Construct from Eigen types + Similarity3(const Matrix3& R, const Vector3& t, double s) { + *this << s * R, t, 0, 0, 0, 1; + } + + /// Construct from GTSAM types + Similarity3(const Rot3& R, const Point3& t, double s) { + *this << s * R.matrix(), t.vector, 0, 0, 0, 1; + } + + /// @name Manifold + /// @{ + + /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes + inline static size_t Dim() { + return 7; + } + + /// Dimensionality of tangent space = 3 DOF + inline size_t dim() const { + return 7; + } + + /// Update Similarity transform via 7-dim vector in tangent space + Similarity3 retract(const Vector& v) const { + + // Get rotation - translation - scale from 4*4 + Rot3 R(this->topLeftCorner<3, 3>); + Vector3 t(this->topRightCorner<3, 1>); + double s(this->at(3, 3)); + + return Similarity3( // + R.retract(v.head<3>()), // retract rotation using v[0,1,2] + t + v.vector.segment < 3 > (3), // retract translation via v[3,4,5] + s + v[6]); // finally, update scale using v[6] + } + + /// 7-dimensional vector v in tangent space that makes other = this->retract(v) + Vector localCoordinates(const Similarity3& other) const { + return Vector7::Zero(); + } + + /// @} + + /// @name Lie Group + /// @{ + + // compose T1*T2 + // between T2*inverse(T1) + // identity I4 + // inverse inverse(T) + + /// @} + +}; +} + +#include + +using namespace gtsam; +using namespace std; + +//****************************************************************************** +TEST(Similarity3, constructors) { + Similarity3 test; +} + +//****************************************************************************** +TEST(Similarity3, manifold) { + EXPECT_LONGS_EQUAL(7, Similarity3::Dim()); + Vector7 z = Vector7::Zero(); + Similarity3 sim; + EXPECT(sim.retract(z)==sim); + + Vector7 v = Vector7::Zero(); + v(6) = 2; + Similarity3 sim; + EXPECT(sim.retract(z)==sim); + + // TODO add unit tests for retract and localCoordinates +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + From f71513b3bfc21f8319e6530c2735dde07674e17e Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 5 Dec 2014 15:50:09 +0100 Subject: [PATCH 02/28] Compiles and test runs --- .cproject | 106 +++++++++--------- gtsam/geometry/Rot3.h | 3 - gtsam/geometry/Rot3M.cpp | 7 -- gtsam/geometry/Rot3Q.cpp | 4 - .../geometry/tests/testSimilarity3.cpp | 39 ++++--- 5 files changed, 75 insertions(+), 84 deletions(-) diff --git a/.cproject b/.cproject index bcf690995..d081a750a 100644 --- a/.cproject +++ b/.cproject @@ -542,6 +542,14 @@ true true + + make + -j4 + testSimilarity3.run + true + true + true + make -j2 @@ -592,7 +600,6 @@ make - tests/testBayesTree.run true false @@ -600,7 +607,6 @@ make - testBinaryBayesNet.run true false @@ -648,7 +654,6 @@ make - testSymbolicBayesNet.run true false @@ -656,7 +661,6 @@ make - tests/testSymbolicFactor.run true false @@ -664,7 +668,6 @@ make - testSymbolicFactorGraph.run true false @@ -680,7 +683,6 @@ make - tests/testBayesTree true false @@ -1128,7 +1130,6 @@ make - testErrors.run true false @@ -1358,46 +1359,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1480,6 +1441,7 @@ make + testSimulated2DOriented.run true false @@ -1519,6 +1481,7 @@ make + testSimulated2D.run true false @@ -1526,6 +1489,7 @@ make + testSimulated3D.run true false @@ -1539,6 +1503,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1796,7 +1800,6 @@ cpack - -G DEB true false @@ -1804,7 +1807,6 @@ cpack - -G RPM true false @@ -1812,7 +1814,6 @@ cpack - -G TGZ true false @@ -1820,7 +1821,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2675,7 +2675,6 @@ make - testGraph.run true false @@ -2683,7 +2682,6 @@ make - testJunctionTree.run true false @@ -2691,7 +2689,6 @@ make - testSymbolicBayesNetB.run true false @@ -3235,6 +3232,7 @@ make + tests/testGaussianISAM2 true false diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index ef2d19750..80ecd6d81 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -93,9 +93,6 @@ namespace gtsam { /** constructor from a rotation matrix */ Rot3(const Matrix3& R); - /** constructor from a rotation matrix */ - Rot3(const Matrix& R); - /** Constructor from a quaternion. This can also be called using a plain * Vector, due to implicit conversion from Vector to Quaternion * @param q The quaternion diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index d0c7e95f3..2c446c6d7 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -56,13 +56,6 @@ Rot3::Rot3(const Matrix3& R) { rot_ = R; } -/* ************************************************************************* */ -Rot3::Rot3(const Matrix& R) { - if (R.rows()!=3 || R.cols()!=3) - throw invalid_argument("Rot3 constructor expects 3*3 matrix"); - rot_ = R; -} - /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) { } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 19de92ca2..3f21c2a80 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -52,10 +52,6 @@ namespace gtsam { Rot3::Rot3(const Matrix3& R) : quaternion_(R) {} - /* ************************************************************************* */ - Rot3::Rot3(const Matrix& R) : - quaternion_(Matrix3(R)) {} - /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : quaternion_(q) { diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index a71308b7a..7526250af 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -24,24 +24,31 @@ namespace gtsam { * 3D similarity transform */ class Similarity3: private Matrix4 { - Similarity3() : - Matrix4::Identity() { - } - - /// Construct pure scaling - Similarity3(double s) : - Matrix4::Identity() { - this->topLeftCorner<3, 3>() = s * Matrix3::Identity(); - } /// Construct from Eigen types Similarity3(const Matrix3& R, const Vector3& t, double s) { *this << s * R, t, 0, 0, 0, 1; } +public: + + Similarity3() { + setIdentity(); + } + + /// Construct pure scaling + Similarity3(double s) { + setIdentity(); + this->topLeftCorner<3, 3>() = s * Matrix3::Identity(); + } + /// Construct from GTSAM types Similarity3(const Rot3& R, const Point3& t, double s) { - *this << s * R.matrix(), t.vector, 0, 0, 0, 1; + *this << s * R.matrix(), t.vector(), 0, 0, 0, 1; + } + + bool operator==(const Similarity3& other) const { + return Matrix4::operator==(other); } /// @name Manifold @@ -61,13 +68,13 @@ class Similarity3: private Matrix4 { Similarity3 retract(const Vector& v) const { // Get rotation - translation - scale from 4*4 - Rot3 R(this->topLeftCorner<3, 3>); - Vector3 t(this->topRightCorner<3, 1>); - double s(this->at(3, 3)); + Rot3 R(this->topLeftCorner<3, 3>()); + Vector3 t(this->topRightCorner<3, 1>()); + double s((*this)(3, 3)); return Similarity3( // R.retract(v.head<3>()), // retract rotation using v[0,1,2] - t + v.vector.segment < 3 > (3), // retract translation via v[3,4,5] + Point3(t + v.segment < 3 > (3)), // retract translation via v[3,4,5] s + v[6]); // finally, update scale using v[6] } @@ -110,8 +117,8 @@ TEST(Similarity3, manifold) { Vector7 v = Vector7::Zero(); v(6) = 2; - Similarity3 sim; - EXPECT(sim.retract(z)==sim); + Similarity3 sim2; + EXPECT(sim2.retract(z)==sim2); // TODO add unit tests for retract and localCoordinates } From 8c4468185a9969bf1aeb116b4484dabbc9029e83 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Thu, 11 Dec 2014 22:54:02 -0500 Subject: [PATCH 03/28] Take a stab at localCoordinates --- .../geometry/tests/testSimilarity3.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 7526250af..92c028bdf 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -72,6 +72,8 @@ public: Vector3 t(this->topRightCorner<3, 1>()); double s((*this)(3, 3)); + // Will retracting or localCoordinating R work if R is not a unit rotation? + // Also, how do we actually get s out? Seems like we need to store it somewhere. return Similarity3( // R.retract(v.head<3>()), // retract rotation using v[0,1,2] Point3(t + v.segment < 3 > (3)), // retract translation via v[3,4,5] @@ -80,7 +82,16 @@ public: /// 7-dimensional vector v in tangent space that makes other = this->retract(v) Vector localCoordinates(const Similarity3& other) const { - return Vector7::Zero(); + Rot3 R(this->topLeftCorner<3,3>()); + Vector3 t(this->topRightCorner<3,1>()); + Vector3 o(other.topRightCorner<3,1>()); + double s((*this)(3,3)); + + Vector7 v; + v.head<3>() = R.localCoordinates(other.topLeftCorner<3,3>()); + v.segment<3>(3) = o - t; + v[6] = other(3,3) - (*this)(3,3); + return v; } /// @} @@ -120,6 +131,11 @@ TEST(Similarity3, manifold) { Similarity3 sim2; EXPECT(sim2.retract(z)==sim2); + Vector7 vzero = Vector7::Zero(); + EXPECT(sim2.localCoordinates(sim) == vzero); + + EXPECT(sim2.localCoordinates(sim)) + // TODO add unit tests for retract and localCoordinates } From 69f27b94884676896edea45fdfb6829ec5e72d06 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Fri, 12 Dec 2014 08:43:57 -0500 Subject: [PATCH 04/28] Additional unit test --- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 92c028bdf..2bf31cbfa 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -131,10 +131,11 @@ TEST(Similarity3, manifold) { Similarity3 sim2; EXPECT(sim2.retract(z)==sim2); - Vector7 vzero = Vector7::Zero(); - EXPECT(sim2.localCoordinates(sim) == vzero); + EXPECT(sim2.localCoordinates(sim) == zero); - EXPECT(sim2.localCoordinates(sim)) + Similarity3 sim3 = Similarity3(Matrix3::Identity(), {1,1,1}, 1); + Vector7 v3 = {0,0,0,1,1,1,0}; + EXPECT(sim2.localCoordinates(sim3)==v3); // TODO add unit tests for retract and localCoordinates } From 42d5b02cb63e4adb7e52885815197ca91997b677 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Fri, 12 Dec 2014 09:08:44 -0500 Subject: [PATCH 05/28] ignore .settings --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 60633adaf..e583e3ab0 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ /examples/Data/dubrovnik-3-7-pre-rewritten.txt /examples/Data/pose2example-rewritten.txt /examples/Data/pose3example-rewritten.txt +/.settings/ From c6e4cd5e03ca613bd00d4561ca55695d3117c047 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Fri, 12 Dec 2014 10:12:58 -0500 Subject: [PATCH 06/28] Operational Sim3 with basic unit tests --- .../geometry/tests/testSimilarity3.cpp | 91 +++++++++++++------ 1 file changed, 63 insertions(+), 28 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 2bf31cbfa..2011cb755 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -14,9 +14,9 @@ * @brief Unit tests for Similarity3 class */ -#include #include #include +#include namespace gtsam { @@ -30,6 +30,16 @@ class Similarity3: private Matrix4 { *this << s * R, t, 0, 0, 0, 1; } + /// Return the translation + const Eigen::Block t() const { + return this->topRightCorner<3, 1>(); + } + + /// Return the rotation matrix + const Eigen::Block R() const { + return this->topLeftCorner<3, 3>(); + } + public: Similarity3() { @@ -44,7 +54,7 @@ public: /// Construct from GTSAM types Similarity3(const Rot3& R, const Point3& t, double s) { - *this << s * R.matrix(), t.vector(), 0, 0, 0, 1; + *this << R.matrix(), t.vector(), 0, 0, 0, 1.0/s; } bool operator==(const Similarity3& other) const { @@ -64,33 +74,40 @@ public: return 7; } - /// Update Similarity transform via 7-dim vector in tangent space - Similarity3 retract(const Vector& v) const { + /// Return the rotation matrix + Rot3 rotation() const { + return R().eval(); + } - // Get rotation - translation - scale from 4*4 - Rot3 R(this->topLeftCorner<3, 3>()); - Vector3 t(this->topRightCorner<3, 1>()); - double s((*this)(3, 3)); + /// Return the translation + Point3 translation() const { + Vector3 t = this->t(); + return Point3::Expmap(t); + } + + /// Return the scale + double scale() const { + return 1.0 / (*this)(3, 3); + } + + /// Update Similarity transform via 7-dim vector in tangent space + Similarity3 retract(const Vector7& v) const { // Will retracting or localCoordinating R work if R is not a unit rotation? // Also, how do we actually get s out? Seems like we need to store it somewhere. return Similarity3( // - R.retract(v.head<3>()), // retract rotation using v[0,1,2] - Point3(t + v.segment < 3 > (3)), // retract translation via v[3,4,5] - s + v[6]); // finally, update scale using v[6] + rotation().retract(v.head<3>()), // retract rotation using v[0,1,2] + translation().retract(v.segment<3>(3)), + scale() + v[6]); // finally, update scale using v[6] } /// 7-dimensional vector v in tangent space that makes other = this->retract(v) - Vector localCoordinates(const Similarity3& other) const { - Rot3 R(this->topLeftCorner<3,3>()); - Vector3 t(this->topRightCorner<3,1>()); - Vector3 o(other.topRightCorner<3,1>()); - double s((*this)(3,3)); + Vector7 localCoordinates(const Similarity3& other) const { Vector7 v; - v.head<3>() = R.localCoordinates(other.topLeftCorner<3,3>()); - v.segment<3>(3) = o - t; - v[6] = other(3,3) - (*this)(3,3); + v.head<3>() = rotation().localCoordinates(other.rotation()); + v.segment<3>(3) = translation().localCoordinates(other.translation()); + v[6] = other.scale() - scale(); return v; } @@ -109,33 +126,51 @@ public: }; } +#include #include using namespace gtsam; using namespace std; //****************************************************************************** -TEST(Similarity3, constructors) { +TEST(Similarity3, Constructors) { Similarity3 test; } //****************************************************************************** -TEST(Similarity3, manifold) { +TEST(Similarity3, Getters) { + Similarity3 test; + EXPECT(assert_equal(Rot3(), test.rotation())); + EXPECT(assert_equal(Point3(), test.translation())); + EXPECT_DOUBLES_EQUAL(1.0, test.scale(), 1e-9); +} + +//****************************************************************************** +TEST(Similarity3, Getters2) { + Similarity3 test(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7); + EXPECT(assert_equal(Rot3::ypr(1, 2, 3), test.rotation())); + EXPECT(assert_equal(Point3(4, 5, 6), test.translation())); + EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9); +} + +//****************************************************************************** +TEST(Similarity3, Manifold) { EXPECT_LONGS_EQUAL(7, Similarity3::Dim()); - Vector7 z = Vector7::Zero(); + Vector z = Vector7::Zero(); Similarity3 sim; - EXPECT(sim.retract(z)==sim); + EXPECT(sim.retract(z) == sim); Vector7 v = Vector7::Zero(); v(6) = 2; Similarity3 sim2; - EXPECT(sim2.retract(z)==sim2); + EXPECT(sim2.retract(z) == sim2); - EXPECT(sim2.localCoordinates(sim) == zero); + EXPECT(assert_equal(z, sim2.localCoordinates(sim))); - Similarity3 sim3 = Similarity3(Matrix3::Identity(), {1,1,1}, 1); - Vector7 v3 = {0,0,0,1,1,1,0}; - EXPECT(sim2.localCoordinates(sim3)==v3); + Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 1, 1), 1); + Vector v3(7); + v3 << 0, 0, 0, 1, 1, 1, 0; + EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); // TODO add unit tests for retract and localCoordinates } From a7b8e60272a9146c5ede61209a8ad0dedc92b178 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Fri, 12 Dec 2014 10:38:59 -0500 Subject: [PATCH 07/28] Unit test failure, either retraction or localCoordinates is wrong for rotation --- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 2011cb755..2f130b67f 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -54,7 +54,7 @@ public: /// Construct from GTSAM types Similarity3(const Rot3& R, const Point3& t, double s) { - *this << R.matrix(), t.vector(), 0, 0, 0, 1.0/s; + *this << R.matrix(), t.vector(), 0, 0, 0, 1.0 / s; } bool operator==(const Similarity3& other) const { @@ -97,8 +97,7 @@ public: // Also, how do we actually get s out? Seems like we need to store it somewhere. return Similarity3( // rotation().retract(v.head<3>()), // retract rotation using v[0,1,2] - translation().retract(v.segment<3>(3)), - scale() + v[6]); // finally, update scale using v[6] + translation().retract(v.segment<3>(3)), scale() + v[6]); // finally, update scale using v[6] } /// 7-dimensional vector v in tangent space that makes other = this->retract(v) @@ -172,6 +171,11 @@ TEST(Similarity3, Manifold) { v3 << 0, 0, 0, 1, 1, 1, 0; EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); + Similarity3 other = Similarity3(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7); +// Similarity3 other = Similarity3(Rot3(),Point3(4,5,6),1); + + EXPECT(sim.retract(sim.localCoordinates(other)) == other); + // TODO add unit tests for retract and localCoordinates } From 89460fe93169be9a6a40cc2b32391a93b2327fa1 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Fri, 2 Jan 2015 09:02:43 -0500 Subject: [PATCH 08/28] Unit tests still fail, but believed to be correct. --- .../geometry/tests/testSimilarity3.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 2f130b67f..7d3f3f553 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -97,7 +97,8 @@ public: // Also, how do we actually get s out? Seems like we need to store it somewhere. return Similarity3( // rotation().retract(v.head<3>()), // retract rotation using v[0,1,2] - translation().retract(v.segment<3>(3)), scale() + v[6]); // finally, update scale using v[6] + translation().retract(R() * v.segment<3>(3)), // Retract the translation + scale() + v[6]); //finally, update scale using v[6] } /// 7-dimensional vector v in tangent space that makes other = this->retract(v) @@ -105,7 +106,8 @@ public: Vector7 v; v.head<3>() = rotation().localCoordinates(other.rotation()); - v.segment<3>(3) = translation().localCoordinates(other.translation()); + v.segment<3>(3) = rotation().unrotate(other.translation() - translation()).vector(); + //v.segment<3>(3) = translation().localCoordinates(other.translation()); v[6] = other.scale() - scale(); return v; } @@ -166,14 +168,20 @@ TEST(Similarity3, Manifold) { EXPECT(assert_equal(z, sim2.localCoordinates(sim))); - Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 1, 1), 1); + Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1); Vector v3(7); - v3 << 0, 0, 0, 1, 1, 1, 0; + v3 << 0, 0, 0, 1, 2, 3, 0; EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); - Similarity3 other = Similarity3(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7); + Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); // Similarity3 other = Similarity3(Rot3(),Point3(4,5,6),1); + std::cout << "Local Coords: " << sim.localCoordinates(other) << std::endl; + std::cout << "Retracted: \n" + << sim.retract(sim.localCoordinates(other)).rotation().matrix() << std::endl + << sim.retract(sim.localCoordinates(other)).translation().vector() << std::endl + << sim.retract(sim.localCoordinates(other)).scale() << std::endl; + EXPECT(sim.retract(sim.localCoordinates(other)) == other); // TODO add unit tests for retract and localCoordinates From a88b10eacc4d9b17eaa900324109bcd1f078b6fb Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Wed, 7 Jan 2015 09:57:48 -0500 Subject: [PATCH 09/28] Working similarity3 transform with unit tests --- .../geometry/tests/testSimilarity3.cpp | 68 ++++++++++++++++--- 1 file changed, 59 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 7d3f3f553..c1e424a2d 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -61,6 +61,19 @@ public: return Matrix4::operator==(other); } + /// Compare with tolerance + bool equals(const Similarity3& sim, double tol) const { + return rotation().equals(sim.rotation(), tol) && translation().equals(sim.translation(), tol) + && scale() < (sim.scale()+tol) && scale() > (sim.scale()-tol); + } + + void print(const std::string& s) const { + std::cout << s; + rotation().print("R:\n"); + translation().print("t: "); + std::cout << "s: " << scale() << std::endl; + } + /// @name Manifold /// @{ @@ -127,12 +140,19 @@ public: }; } + #include #include using namespace gtsam; using namespace std; +static Point3 P(0.2,0.7,-2); +static Rot3 R = Rot3::rodriguez(0.3,0,0); +static Similarity3 T(R,Point3(3.5,-8.2,4.2),1); +static Similarity3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1); +static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1); + //****************************************************************************** TEST(Similarity3, Constructors) { Similarity3 test; @@ -168,25 +188,55 @@ TEST(Similarity3, Manifold) { EXPECT(assert_equal(z, sim2.localCoordinates(sim))); - Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1); + Similarity3 sim3 = Similarity3(Rot3().ypr(0.5,1,1.5), Point3(1, 2, 3), 1); Vector v3(7); v3 << 0, 0, 0, 1, 2, 3, 0; EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); - Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); -// Similarity3 other = Similarity3(Rot3(),Point3(4,5,6),1); +// Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); + Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03),Point3(4,5,6),1); + Rot3 R = Rot3::rodriguez(0.3,0,0); - std::cout << "Local Coords: " << sim.localCoordinates(other) << std::endl; - std::cout << "Retracted: \n" - << sim.retract(sim.localCoordinates(other)).rotation().matrix() << std::endl - << sim.retract(sim.localCoordinates(other)).translation().vector() << std::endl - << sim.retract(sim.localCoordinates(other)).scale() << std::endl; + Vector vlocal = sim.localCoordinates(other); - EXPECT(sim.retract(sim.localCoordinates(other)) == other); + EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); // TODO add unit tests for retract and localCoordinates } +/* ************************************************************************* */ +TEST( Similarity3, retract_first_order) +{ + Similarity3 id; + Vector v = zero(7); + v(0) = 0.3; + EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v),1e-2)); + v(3)=0.2;v(4)=0.7;v(5)=-2; + EXPECT(assert_equal(Similarity3(R, P, 1),id.retract(v),1e-2)); +} + +/* ************************************************************************* */ +TEST(Similarity3, localCoordinates_first_order) +{ + Vector d12 = repeat(7,0.1); + d12(6) = 1.0; + Similarity3 t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} + +/* ************************************************************************* */ +TEST(Similarity3, manifold_first_order) +{ + Similarity3 t1 = T; + Similarity3 t2 = T3; + Similarity3 origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); +} + + //****************************************************************************** int main() { TestResult tr; From bade68fa339aee6d88e905f86a2c56a373c23f05 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Mon, 12 Jan 2015 12:58:17 -0500 Subject: [PATCH 10/28] Simple single prior optimization --- .../geometry/tests/testSimilarity3.cpp | 38 +++++++++++++++++-- 1 file changed, 35 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index c1e424a2d..26a469c92 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -140,6 +140,11 @@ public: }; } +#include +#include +#include +#include +#include #include #include @@ -188,19 +193,25 @@ TEST(Similarity3, Manifold) { EXPECT(assert_equal(z, sim2.localCoordinates(sim))); - Similarity3 sim3 = Similarity3(Rot3().ypr(0.5,1,1.5), Point3(1, 2, 3), 1); + Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1); Vector v3(7); v3 << 0, 0, 0, 1, 2, 3, 0; EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); // Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); - Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03),Point3(4,5,6),1); - Rot3 R = Rot3::rodriguez(0.3,0,0); + Similarity3 other = Similarity3(Rot3::ypr(0.1, 0.2, 0.3),Point3(4,5,6),1); Vector vlocal = sim.localCoordinates(other); EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); + Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1); + Rot3 R = Rot3::rodriguez(0.3,0,0); + + Vector vlocal2 = sim.localCoordinates(other2); + + EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2)); + // TODO add unit tests for retract and localCoordinates } @@ -236,6 +247,27 @@ TEST(Similarity3, manifold_first_order) EXPECT(assert_equal(t1, t2.retract(d21))); } +TEST(Similarity3, Optimization) { + Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); + prior.print("goal angle"); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); + Symbol key('x',1); + PriorFactor factor(key, prior, model); + + NonlinearFactorGraph graph; + graph.push_back(factor); + graph.print("full graph"); + + Values initial; + initial.insert(key, Similarity3()); + initial.print("initial estimate"); + + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + result.print("final result"); + + EXPECT(assert_equal(prior, result, 1e-2)); +} + //****************************************************************************** int main() { From a87a3dd9878c636680e2bef0dfc1ef2779b30ee4 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Sat, 24 Jan 2015 22:42:48 -0500 Subject: [PATCH 11/28] Rotation works, translation and scale incorrect --- .../geometry/tests/testSimilarity3.cpp | 51 +++++++++++-------- 1 file changed, 29 insertions(+), 22 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 06ac9ab80..dd6e24f9a 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -91,12 +91,13 @@ public: return R_ * (s_ * p) + t_; } - Matrix7 adjointMap() const{ + Matrix7 AdjointMap() const{ const Matrix3 R = R_.matrix(); const Vector3 t = t_.vector(); Matrix3 A = s_ * skewSymmetric(t) * R; Matrix7 adj; adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Z_3x3, Z_3x3, 1; + return adj; } /** syntactic sugar for transform_from */ @@ -104,16 +105,18 @@ public: return transform_from(p); } - /*Similarity3 inverse() const { + Similarity3 inverse() const { Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt * (-t_)); - }*/ + Point3 sRt = R_.inverse() * (s_ * t_); + return Similarity3(Rt, sRt, 1.0/s_); + } Similarity3 operator*(const Similarity3& T) const { return Similarity3(R_ * T.R_, ((1.0/s_)*t_) + R_ * T.t_, s_*T.s_); } void print(const std::string& s) const { + std::cout << std::endl; std::cout << s; rotation().print("R:\n"); translation().print("t: "); @@ -149,26 +152,31 @@ public: } /// Update Similarity transform via 7-dim vector in tangent space -/* Similarity3 retract(const Vector7& v) const { + struct ChartAtOrigin { + static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) { // Will retracting or localCoordinating R work if R is not a unit rotation? // Also, how do we actually get s out? Seems like we need to store it somewhere. + Rot3 r; //Create a zero rotation to do our retraction. return Similarity3( // - R_.retract(v.head<3>()), // retract rotation using v[0,1,2] - t_.retract(R() * v.segment<3>(3)), // Retract the translation - scale() + v[6]); //finally, update scale using v[6] + r.retract(v.head<3>()), // retract rotation using v[0,1,2] + Point3(v.segment<3>(3)), // Retract the translation + v[6]); //finally, update scale using v[6] } /// 7-dimensional vector v in tangent space that makes other = this->retract(v) - Vector7 localCoordinates(const Similarity3& other) const { - + static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) { + Rot3 r; //Create a zero rotation to do the retraction Vector7 v; - v.head<3>() = R_.localCoordinates(other.R_); - v.segment<3>(3) = R_.unrotate(other.t_ - t_).vector(); + v.head<3>() = r.localCoordinates(other.R_); + v.segment<3>(3) = other.t_.vector(); //v.segment<3>(3) = translation().localCoordinates(other.translation()); - v[6] = other.s_ - s_; + v[6] = other.s_; return v; - }*/ + } + }; + + using LieGroup::inverse; // version with derivative /// @} @@ -200,7 +208,7 @@ struct traits : public internal::LieGroupTraits {}; using namespace gtsam; using namespace std; -//GTSAM_CONCEPT_POSE_INST(Similarity3); +GTSAM_CONCEPT_TESTABLE_INST(Similarity3) static Point3 P(0.2,0.7,-2); static Rot3 R = Rot3::rodriguez(0.3,0,0); @@ -299,24 +307,23 @@ TEST(Similarity3, manifold_first_order) TEST(Similarity3, Optimization) { Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); - prior.print("goal angle"); + prior.print("Goal Transform"); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); Symbol key('x',1); - Symbol key2('x',2); PriorFactor factor(key, prior, model); NonlinearFactorGraph graph; graph.push_back(factor); - graph.print("full graph"); + graph.print("Full Graph"); Values initial; initial.insert(key, Similarity3()); - initial.print("initial estimate"); + initial.print("Initial Estimate"); Values result; - result.insert(key2, LevenbergMarquardtOptimizer(graph, initial).optimize()); - result.print("final result"); - EXPECT(assert_equal(prior, result.at(key2), 1e-2)); + result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + result.print("Optimized Estimate"); + EXPECT(assert_equal(prior, result.at(key))); } From c6b3535dda50af4dfbcf7f5d8807ba6a4e143e95 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Sun, 25 Jan 2015 01:28:16 -0500 Subject: [PATCH 12/28] retract works, local coordiantes still broken --- .../geometry/tests/testSimilarity3.cpp | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index dd6e24f9a..809d7846e 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -96,7 +96,7 @@ public: const Vector3 t = t_.vector(); Matrix3 A = s_ * skewSymmetric(t) * R; Matrix7 adj; - adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Z_3x3, Z_3x3, 1; + adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), 1; return adj; } @@ -161,7 +161,7 @@ public: return Similarity3( // r.retract(v.head<3>()), // retract rotation using v[0,1,2] Point3(v.segment<3>(3)), // Retract the translation - v[6]); //finally, update scale using v[6] + 1.0 + v[6]); //finally, update scale using v[6] } /// 7-dimensional vector v in tangent space that makes other = this->retract(v) @@ -171,7 +171,7 @@ public: v.head<3>() = r.localCoordinates(other.R_); v.segment<3>(3) = other.t_.vector(); //v.segment<3>(3) = translation().localCoordinates(other.translation()); - v[6] = other.s_; + v[6] = other.s_ - 1.0; return v; } }; @@ -237,6 +237,19 @@ TEST(Similarity3, Getters2) { EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9); } +TEST(Similarity3, AdjointMap) { + Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Matrix7 result; + result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000, + 6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000, + -2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000, + 0, 0, 0, -0.2248, -0.3502, -0.9093, 0, + 0, 0, 0, 0.9024, -0.4269, -0.0587, 0, + 0, 0, 0, -0.3676, -0.8337, 0.4120, 0, + 0, 0, 0, 0, 0, 0, 1.0000; + EXPECT(assert_equal(result, test.AdjointMap(), 1e-3)); +} + //****************************************************************************** TEST(Similarity3, Manifold) { EXPECT_LONGS_EQUAL(7, Similarity3::Dim()); From 10b56a115c58885e0eb61a66e9fdbae7d57a0727 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Mon, 26 Jan 2015 13:38:32 -0500 Subject: [PATCH 13/28] Working Similarity3 transform with unit tests. --- .../geometry/tests/testSimilarity3.cpp | 39 +++++++++++++++---- 1 file changed, 32 insertions(+), 7 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 809d7846e..7cced8a96 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -107,12 +107,12 @@ public: Similarity3 inverse() const { Rot3 Rt = R_.inverse(); - Point3 sRt = R_.inverse() * (s_ * t_); + Point3 sRt = R_.inverse() * (-s_ * t_); return Similarity3(Rt, sRt, 1.0/s_); } Similarity3 operator*(const Similarity3& T) const { - return Similarity3(R_ * T.R_, ((1.0/s_)*t_) + R_ * T.t_, s_*T.s_); + return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_); } void print(const std::string& s) const { @@ -250,6 +250,29 @@ TEST(Similarity3, AdjointMap) { EXPECT(assert_equal(result, test.AdjointMap(), 1e-3)); } +TEST(Similarity3, inverse) { + Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Matrix3 Re; + Re << -0.2248, 0.9024, -0.3676, + -0.3502, -0.4269, -0.8337, + -0.9093, -0.0587, 0.4120; + Vector3 te(-9.8472, 59.7640, 10.2125); + Similarity3 expected(Re, te, 1.0/7.0); + EXPECT(assert_equal(expected, test.inverse(), 1e-3)); +} + +TEST(Similarity3, multiplication) { + Similarity3 test1(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Similarity3 test2(Rot3::ypr(1,2,3).inverse(), Point3(8,9,10), 11); + Matrix3 re; + re << 0.0688, 0.9863, -0.1496, + -0.5665, -0.0848, -0.8197, + -0.8211, 0.1412, 0.5530; + Vector3 te(-13.6797, 3.2441, -5.7794); + Similarity3 expected(re, te, 77); + EXPECT(assert_equal(expected, test1*test2, 1e-2)); +} + //****************************************************************************** TEST(Similarity3, Manifold) { EXPECT_LONGS_EQUAL(7, Similarity3::Dim()); @@ -320,23 +343,25 @@ TEST(Similarity3, manifold_first_order) TEST(Similarity3, Optimization) { Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); - prior.print("Goal Transform"); + //prior.print("Goal Transform"); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); Symbol key('x',1); PriorFactor factor(key, prior, model); NonlinearFactorGraph graph; graph.push_back(factor); - graph.print("Full Graph"); + //graph.print("Full Graph"); Values initial; initial.insert(key, Similarity3()); - initial.print("Initial Estimate"); + //initial.print("Initial Estimate"); Values result; + LevenbergMarquardtParams params; + params.setVerbosityLM("TRYCONFIG"); result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - result.print("Optimized Estimate"); - EXPECT(assert_equal(prior, result.at(key))); + //result.print("Optimized Estimate"); + EXPECT(assert_equal(prior, result.at(key), 1e-4)); } From db73a0dd1daae8a56346f7dcc77131010e457b33 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Wed, 28 Jan 2015 14:55:13 -0500 Subject: [PATCH 14/28] Working, with stub log/expmap and identity --- .../geometry/tests/testSimilarity3.cpp | 56 +++++++++++++++++++ 1 file changed, 56 insertions(+) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 7cced8a96..d7d12a580 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -70,6 +70,20 @@ public: s_ = s; } + static Similarity3 identity() { + std::cout << "Identity!" << std::endl; + return Similarity3(); } + + static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none) { + std::cout << "Logmap!" << std::endl; + return Vector7(); + } + + static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none) { + std::cout << "Expmap!" << std::endl; + return Similarity3(); + } + bool operator==(const Similarity3& other) const { return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); } @@ -198,6 +212,7 @@ struct traits : public internal::LieGroupTraits {}; #include #include +#include #include #include #include @@ -207,6 +222,7 @@ struct traits : public internal::LieGroupTraits {}; using namespace gtsam; using namespace std; +using symbol_shorthand::X; GTSAM_CONCEPT_TESTABLE_INST(Similarity3) @@ -364,6 +380,46 @@ TEST(Similarity3, Optimization) { EXPECT(assert_equal(prior, result.at(key), 1e-4)); } +TEST(Similarity3, Optimization2) { + Similarity3 prior = Similarity3();//Rot3::ypr(0, 0, 0), Point3(1, 2, 3), 4); + Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1.0, 0, 0), 1.0); + Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1.0, 0, 0), 1.0); + Similarity3 m3 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(0.9, 0, 0), 1.0); + Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(0.9, 0, 0), 1.0); + + //prior.print("Goal Transform"); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); + SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.010).finished()); + PriorFactor factor(X(1), prior, model); + BetweenFactor b1(X(1), X(2), m1, betweenNoise); + BetweenFactor b2(X(2), X(3), m2, betweenNoise); + BetweenFactor b3(X(3), X(4), m3, betweenNoise); + BetweenFactor b4(X(4), X(1), m4, betweenNoise); + + + NonlinearFactorGraph graph; + graph.push_back(factor); + graph.push_back(b1); + graph.push_back(b2); + graph.push_back(b3); + graph.push_back(b4); + + graph.print("Full Graph"); + + Values initial; + initial.insert(X(1), Similarity3()); + initial.insert(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.0)); + initial.insert(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(1, 1.5, 0), 1.0)); + initial.insert(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.0)); + + initial.print("Initial Estimate"); + + Values result; + result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + result.print("Optimized Estimate"); + //EXPECT(assert_equal(prior, result.at(key), 1e-4)); +} //****************************************************************************** int main() { From 42d8e1fcb2d7607fe032f75b255696842bb82f35 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Fri, 13 Feb 2015 09:06:08 -0500 Subject: [PATCH 15/28] Working Sim3 Transform --- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index d7d12a580..9cae0ba5d 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -383,9 +383,9 @@ TEST(Similarity3, Optimization) { TEST(Similarity3, Optimization2) { Similarity3 prior = Similarity3();//Rot3::ypr(0, 0, 0), Point3(1, 2, 3), 4); Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1.0, 0, 0), 1.0); - Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1.0, 0, 0), 1.0); - Similarity3 m3 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(0.9, 0, 0), 1.0); - Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(0.9, 0, 0), 1.0); + Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(0.9, 0, 0), 1.0); + Similarity3 m3 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(0.8, 0, 0), 1.0); + Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(0.7, 0, 0), 1.42); //prior.print("Goal Transform"); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); From f9ccd23d4a039575eb03bd9a53f47ec7c1ebfb96 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Thu, 19 Feb 2015 01:21:20 -0500 Subject: [PATCH 16/28] Refactored into class and unit test --- gtsam_unstable/geometry/Similarity3.cpp | 154 +++++++++++++ gtsam_unstable/geometry/Similarity3.h | 118 ++++++++++ .../geometry/tests/testSimilarity3.cpp | 213 +----------------- 3 files changed, 280 insertions(+), 205 deletions(-) create mode 100644 gtsam_unstable/geometry/Similarity3.cpp create mode 100644 gtsam_unstable/geometry/Similarity3.h diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp new file mode 100644 index 000000000..c805d8aab --- /dev/null +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -0,0 +1,154 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Similarity3.cpp + * @brief Implementation of Similarity3 transform + */ + +#include +#include +#include +#include + +namespace gtsam { + +Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) { + R_ = R; + t_ = t; + s_ = s; +} + +/// Return the translation +const Vector3 Similarity3::t() const { + return t_.vector(); +} + +/// Return the rotation matrix +const Matrix3 Similarity3::R() const { + return R_.matrix(); +} + +Similarity3::Similarity3() : + R_(), t_(), s_(1){ +} + +/// Construct pure scaling +Similarity3::Similarity3(double s) { + s_ = s; +} + +/// Construct from GTSAM types +Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) { + R_ = R; + t_ = t; + s_ = s; +} + +Similarity3 Similarity3::identity() { + std::cout << "Identity!" << std::endl; + return Similarity3(); } + +Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { + std::cout << "Logmap!" << std::endl; + return Vector7(); +} + +Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { + std::cout << "Expmap!" << std::endl; + return Similarity3(); +} + +bool Similarity3::operator==(const Similarity3& other) const { + return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); +} + +/// Compare with tolerance +bool Similarity3::equals(const Similarity3& sim, double tol) const { + return rotation().equals(sim.rotation(), tol) && translation().equals(sim.translation(), tol) + && scale() < (sim.scale()+tol) && scale() > (sim.scale()-tol); +} + +Point3 Similarity3::transform_from(const Point3& p) const { + return R_ * (s_ * p) + t_; +} + +Matrix7 Similarity3::AdjointMap() const{ + const Matrix3 R = R_.matrix(); + const Vector3 t = t_.vector(); + Matrix3 A = s_ * skewSymmetric(t) * R; + Matrix7 adj; + adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), 1; + return adj; +} + +/** syntactic sugar for transform_from */ +inline Point3 Similarity3::operator*(const Point3& p) const { + return transform_from(p); +} + +Similarity3 Similarity3::inverse() const { + Rot3 Rt = R_.inverse(); + Point3 sRt = R_.inverse() * (-s_ * t_); + return Similarity3(Rt, sRt, 1.0/s_); +} + +Similarity3 Similarity3::operator*(const Similarity3& T) const { + return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_); +} + +void Similarity3::print(const std::string& s) const { + std::cout << std::endl; + std::cout << s; + rotation().print("R:\n"); + translation().print("t: "); + std::cout << "s: " << scale() << std::endl; +} + +/// Return the rotation matrix +Rot3 Similarity3::rotation() const { + return R_; +} + +/// Return the translation +Point3 Similarity3::translation() const { + return t_; +} + +/// Return the scale +double Similarity3::scale() const { + return s_; +} + +/// Update Similarity transform via 7-dim vector in tangent space +Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) { + // Will retracting or localCoordinating R work if R is not a unit rotation? + // Also, how do we actually get s out? Seems like we need to store it somewhere. + Rot3 r; //Create a zero rotation to do our retraction. + return Similarity3( // + r.retract(v.head<3>()), // retract rotation using v[0,1,2] + Point3(v.segment<3>(3)), // Retract the translation + 1.0 + v[6]); //finally, update scale using v[6] +} + +/// 7-dimensional vector v in tangent space that makes other = this->retract(v) +Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) { + Rot3 r; //Create a zero rotation to do the retraction + Vector7 v; + v.head<3>() = r.localCoordinates(other.R_); + v.segment<3>(3) = other.t_.vector(); + //v.segment<3>(3) = translation().localCoordinates(other.translation()); + v[6] = other.s_ - 1.0; + return v; +} +} + + diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h new file mode 100644 index 000000000..8d7ccf82f --- /dev/null +++ b/gtsam_unstable/geometry/Similarity3.h @@ -0,0 +1,118 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Similarity3.h + * @brief Implementation of Similarity3 transform + */ + +#ifndef GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ +#define GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ + + +#include +#include +#include + +namespace gtsam { + +/** + * 3D similarity transform + */ +class Similarity3: public LieGroup { + + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Translation; + +private: + Rot3 R_; + Point3 t_; + double s_; + +public: + + Similarity3(); + + /// Construct pure scaling + Similarity3(double s); + + /// Construct from GTSAM types + Similarity3(const Rot3& R, const Point3& t, double s); + + /// Construct from Eigen types + Similarity3(const Matrix3& R, const Vector3& t, double s); + + /// Return the translation + const Vector3 t() const; + + /// Return the rotation matrix + const Matrix3 R() const; + + static Similarity3 identity(); + + static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none); + + static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none); + + bool operator==(const Similarity3& other) const; + + /// Compare with tolerance + bool equals(const Similarity3& sim, double tol) const; + + Point3 transform_from(const Point3& p) const; + + Matrix7 AdjointMap() const; + + /** syntactic sugar for transform_from */ + inline Point3 operator*(const Point3& p) const; + + Similarity3 inverse() const; + + Similarity3 operator*(const Similarity3& T) const; + + void print(const std::string& s) const; + + /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes + inline static size_t Dim() { + return 7; + }; + + /// Dimensionality of tangent space = 7 DOF + inline size_t dim() const { + return 7; + }; + + /// Return the rotation matrix + Rot3 rotation() const; + + /// Return the translation + Point3 translation() const; + + /// Return the scale + double scale() const; + + /// Update Similarity transform via 7-dim vector in tangent space + struct ChartAtOrigin { + static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none); + + /// 7-dimensional vector v in tangent space that makes other = this->retract(v) + static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none); + }; + + using LieGroup::inverse; // version with derivative +}; + +template<> +struct traits : public internal::LieGroupTraits {}; +} + +#endif /* GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ */ diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 9cae0ba5d..51125eb2d 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -14,202 +14,7 @@ * @brief Unit tests for Similarity3 class */ -#include -#include -#include - -namespace gtsam { - -/** - * 3D similarity transform - */ -class Similarity3: public LieGroup { - - /** Pose Concept requirements */ - typedef Rot3 Rotation; - typedef Point3 Translation; - -private: - Rot3 R_; - Point3 t_; - double s_; - -public: - /// Construct from Eigen types - Similarity3(const Matrix3& R, const Vector3& t, double s) { - R_ = R; - t_ = t; - s_ = s; - } - - /// Return the translation - const Vector3 t() const { - return t_.vector(); - } - - /// Return the rotation matrix - const Matrix3 R() const { - return R_.matrix(); - } - -public: - - Similarity3() : - R_(), t_(), s_(1){ - } - - /// Construct pure scaling - Similarity3(double s) { - s_ = s; - } - - /// Construct from GTSAM types - Similarity3(const Rot3& R, const Point3& t, double s) { - R_ = R; - t_ = t; - s_ = s; - } - - static Similarity3 identity() { - std::cout << "Identity!" << std::endl; - return Similarity3(); } - - static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none) { - std::cout << "Logmap!" << std::endl; - return Vector7(); - } - - static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none) { - std::cout << "Expmap!" << std::endl; - return Similarity3(); - } - - bool operator==(const Similarity3& other) const { - return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); - } - - /// Compare with tolerance - bool equals(const Similarity3& sim, double tol) const { - return rotation().equals(sim.rotation(), tol) && translation().equals(sim.translation(), tol) - && scale() < (sim.scale()+tol) && scale() > (sim.scale()-tol); - } - - Point3 transform_from(const Point3& p) const { - /*if (Dpose) { - const Matrix3 R = R_.matrix(); - Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); - (*Dpose) << DR, R; - } - if (Dpoint) - *Dpoint = R_.matrix();*/ - return R_ * (s_ * p) + t_; - } - - Matrix7 AdjointMap() const{ - const Matrix3 R = R_.matrix(); - const Vector3 t = t_.vector(); - Matrix3 A = s_ * skewSymmetric(t) * R; - Matrix7 adj; - adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), 1; - return adj; - } - - /** syntactic sugar for transform_from */ - inline Point3 operator*(const Point3& p) const { - return transform_from(p); - } - - Similarity3 inverse() const { - Rot3 Rt = R_.inverse(); - Point3 sRt = R_.inverse() * (-s_ * t_); - return Similarity3(Rt, sRt, 1.0/s_); - } - - Similarity3 operator*(const Similarity3& T) const { - return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_); - } - - void print(const std::string& s) const { - std::cout << std::endl; - std::cout << s; - rotation().print("R:\n"); - translation().print("t: "); - std::cout << "s: " << scale() << std::endl; - } - - /// @name Manifold - /// @{ - - /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes - inline static size_t Dim() { - return 7; - } - - /// Dimensionality of tangent space = 7 DOF - inline size_t dim() const { - return 7; - } - - /// Return the rotation matrix - Rot3 rotation() const { - return R_; - } - - /// Return the translation - Point3 translation() const { - return t_; - } - - /// Return the scale - double scale() const { - return s_; - } - - /// Update Similarity transform via 7-dim vector in tangent space - struct ChartAtOrigin { - static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) { - - // Will retracting or localCoordinating R work if R is not a unit rotation? - // Also, how do we actually get s out? Seems like we need to store it somewhere. - Rot3 r; //Create a zero rotation to do our retraction. - return Similarity3( // - r.retract(v.head<3>()), // retract rotation using v[0,1,2] - Point3(v.segment<3>(3)), // Retract the translation - 1.0 + v[6]); //finally, update scale using v[6] - } - - /// 7-dimensional vector v in tangent space that makes other = this->retract(v) - static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) { - Rot3 r; //Create a zero rotation to do the retraction - Vector7 v; - v.head<3>() = r.localCoordinates(other.R_); - v.segment<3>(3) = other.t_.vector(); - //v.segment<3>(3) = translation().localCoordinates(other.translation()); - v[6] = other.s_ - 1.0; - return v; - } - }; - - using LieGroup::inverse; // version with derivative - - /// @} - - /// @name Lie Group - /// @{ - - // compose T1*T2 - // between T2*inverse(T1) - // identity I4 - // inverse inverse(T) - - /// @} - -}; - -template<> -struct traits : public internal::LieGroupTraits {}; -} - +#include #include #include #include @@ -359,24 +164,20 @@ TEST(Similarity3, manifold_first_order) TEST(Similarity3, Optimization) { Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); - //prior.print("Goal Transform"); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); Symbol key('x',1); PriorFactor factor(key, prior, model); NonlinearFactorGraph graph; graph.push_back(factor); - //graph.print("Full Graph"); Values initial; initial.insert(key, Similarity3()); - //initial.print("Initial Estimate"); Values result; LevenbergMarquardtParams params; params.setVerbosityLM("TRYCONFIG"); result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - //result.print("Optimized Estimate"); EXPECT(assert_equal(prior, result.at(key), 1e-4)); } @@ -390,12 +191,14 @@ TEST(Similarity3, Optimization2) { //prior.print("Goal Transform"); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( - (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.010).finished()); + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1).finished()); + SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.01).finished()); PriorFactor factor(X(1), prior, model); BetweenFactor b1(X(1), X(2), m1, betweenNoise); BetweenFactor b2(X(2), X(3), m2, betweenNoise); BetweenFactor b3(X(3), X(4), m3, betweenNoise); - BetweenFactor b4(X(4), X(1), m4, betweenNoise); + BetweenFactor b4(X(4), X(1), m4, betweenNoise2); NonlinearFactorGraph graph; @@ -409,9 +212,9 @@ TEST(Similarity3, Optimization2) { Values initial; initial.insert(X(1), Similarity3()); - initial.insert(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.0)); - initial.insert(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(1, 1.5, 0), 1.0)); - initial.insert(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.0)); + initial.insert(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1)); + initial.insert(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); + initial.insert(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3)); initial.print("Initial Estimate"); From fbfbe1b9e26d51bfe9fd5a6232f1e26a42f54fc9 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Fri, 20 Feb 2015 01:12:39 -0500 Subject: [PATCH 17/28] re-added project make target --- .cproject | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.cproject b/.cproject index ab1d0cdfc..ed61e4c4b 100644 --- a/.cproject +++ b/.cproject @@ -532,6 +532,14 @@ true true + + make + -j4 + testSimilarity3.run + true + true + true + make -j2 From 0a5f2205343eb95f75d63c70f09cdb049985243e Mon Sep 17 00:00:00 2001 From: alexhagiopol Date: Fri, 20 Feb 2015 14:33:15 -0500 Subject: [PATCH 18/28] 10% reduction in lines of code included by Vector.h. --- gtsam/base/Matrix.h | 6 ++++++ gtsam/base/Vector.cpp | 5 +++-- gtsam/base/Vector.h | 12 +++++++----- 3 files changed, 16 insertions(+), 7 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index c3cbfa341..294b5e63a 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -27,6 +27,12 @@ #include #include +#include +#include +#include +#include + + /** * Matrix is a typedef in the gtsam namespace * TODO: make a version to work with matlab wrapping diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index a9ef8fe10..5368e708e 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -16,9 +16,10 @@ * @author Frank Dellaert */ + #include #include -#include +//#include #include #include #include @@ -27,7 +28,7 @@ #include #include #include - +#include //added by alex #include diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 74cd248a1..58e1d2ff6 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -18,14 +18,16 @@ // \callgraph + #pragma once - #include -#include -#include +//#include +//#include #include -#include - +//#include +//#include +//#include +#include namespace gtsam { // Vector is just a typedef of the Eigen dynamic vector type From 7dca1d76a55fd614ffb96c01685acea0d67e809f Mon Sep 17 00:00:00 2001 From: alexhagiopol Date: Fri, 20 Feb 2015 17:09:17 -0500 Subject: [PATCH 19/28] More reductions in included lines. --- gtsam/base/FastSet.h | 2 +- gtsam/base/GenericValue.h | 2 +- gtsam/base/Matrix.h | 1 - gtsam/base/Vector.cpp | 3 +-- gtsam/base/Vector.h | 5 ----- gtsam/base/types.cpp | 2 +- gtsam/base/types.h | 5 +++-- gtsam/geometry/EssentialMatrix.h | 2 +- 8 files changed, 8 insertions(+), 14 deletions(-) diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 69e841e57..810a48c60 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index c67f7dd61..dd0811d8b 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -26,7 +26,7 @@ #include #include -#include +#include #include // operator typeid namespace gtsam { diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 294b5e63a..d5c433d35 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -21,7 +21,6 @@ // \callgraph #pragma once - #include #include #include diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 5368e708e..1b145a116 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -19,7 +19,6 @@ #include #include -//#include #include #include #include @@ -28,7 +27,7 @@ #include #include #include -#include //added by alex +#include #include diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 58e1d2ff6..d19fee298 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -21,12 +21,7 @@ #pragma once #include -//#include -//#include #include -//#include -//#include -//#include #include namespace gtsam { diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp index 3f86dc0c1..9a0d6c627 100644 --- a/gtsam/base/types.cpp +++ b/gtsam/base/types.cpp @@ -18,7 +18,7 @@ */ #include - +//#include #include #include diff --git a/gtsam/base/types.h b/gtsam/base/types.h index aca04a14b..839016fd9 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -21,10 +21,11 @@ #include #include - +//#include +#include #include #include -#include +#include #include #include #include diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 606f62f87..9ebcbcf5c 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -10,7 +10,7 @@ #include #include #include -#include +#include namespace gtsam { From 3f4558cacb2a8b86230e3cfcb2940240211ce227 Mon Sep 17 00:00:00 2001 From: alexhagiopol Date: Mon, 23 Feb 2015 18:45:25 -0500 Subject: [PATCH 20/28] SVD removed from Matrix.h --- gtsam/base/Matrix.cpp | 16 ++++++++++++++++ gtsam/base/Matrix.h | 19 ++----------------- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 7bcd32b9f..7136db768 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -32,6 +32,9 @@ #include #include +#include +#include + using namespace std; namespace gtsam { @@ -697,6 +700,19 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, return ss.str(); } +void inplace_QR(Matrix& A){ + size_t rows = A.rows(); + size_t cols = A.cols(); + size_t size = std::min(rows,cols); + typedef Eigen::internal::plain_diag_type::type HCoeffsType; + typedef Eigen::internal::plain_row_type::type RowVectorType; + HCoeffsType hCoeffs(size); + RowVectorType temp(cols); + + Eigen::internal::householder_qr_inplace_blocked::run(A, hCoeffs, 48, temp.data()); + + zeroBelowDiagonal(A); +} } // namespace gtsam diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index d5c433d35..ee3c84464 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -25,10 +25,8 @@ #include #include #include - #include #include -#include #include @@ -372,21 +370,8 @@ GTSAM_EXPORT std::pair qr(const Matrix& A); * @param A is the input matrix, and is the output * @param clear_below_diagonal enables zeroing out below diagonal */ -template -void inplace_QR(MATRIX& A) { - size_t rows = A.rows(); - size_t cols = A.cols(); - size_t size = std::min(rows,cols); - - typedef Eigen::internal::plain_diag_type::type HCoeffsType; - typedef Eigen::internal::plain_row_type::type RowVectorType; - HCoeffsType hCoeffs(size); - RowVectorType temp(cols); - - Eigen::internal::householder_qr_inplace_blocked::run(A, hCoeffs, 48, temp.data()); - - zeroBelowDiagonal(A); -} +//template +void inplace_QR(Matrix& A); /** * Imperative algorithm for in-place full elimination with From c13bde99f2e03644ef19bd22ffc817843c116583 Mon Sep 17 00:00:00 2001 From: alexhagiopol Date: Tue, 24 Feb 2015 00:19:13 -0500 Subject: [PATCH 21/28] More header bloat reduction. --- gtsam/base/Matrix.h | 1 - gtsam/base/types.cpp | 1 - gtsam/base/types.h | 1 - gtsam/nonlinear/Values.h | 6 ------ 4 files changed, 9 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index ee3c84464..fc9f27099 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -370,7 +370,6 @@ GTSAM_EXPORT std::pair qr(const Matrix& A); * @param A is the input matrix, and is the output * @param clear_below_diagonal enables zeroing out below diagonal */ -//template void inplace_QR(Matrix& A); /** diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp index 9a0d6c627..4f61c6a1d 100644 --- a/gtsam/base/types.cpp +++ b/gtsam/base/types.cpp @@ -18,7 +18,6 @@ */ #include -//#include #include #include diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 839016fd9..d0ba526c2 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -21,7 +21,6 @@ #include #include -//#include #include #include #include diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 73d0711be..ad727f45e 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -26,14 +26,9 @@ #include #include -#include #include - -#include -#include #include #include -#include #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" @@ -43,7 +38,6 @@ #pragma GCC diagnostic pop #endif #include -#include #include #include From 77770b48b51135660bad396a3ddacee2f18735bc Mon Sep 17 00:00:00 2001 From: alexhagiopol Date: Tue, 24 Feb 2015 14:39:08 -0500 Subject: [PATCH 22/28] Include in types.h only when TBB is on. --- gtsam/base/FastSet.h | 3 +-- gtsam/base/Vector.h | 1 + gtsam/base/types.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 810a48c60..29fb3fcd9 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -19,9 +19,8 @@ #pragma once #include - #include -#include +#include #include #include #include diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index d19fee298..1c433eb4a 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -23,6 +23,7 @@ #include #include #include +#include namespace gtsam { // Vector is just a typedef of the Eigen dynamic vector type diff --git a/gtsam/base/types.h b/gtsam/base/types.h index d0ba526c2..122bc18a0 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -21,7 +21,6 @@ #include #include -#include #include #include #include @@ -33,6 +32,7 @@ #include #include #include +#include #endif #ifdef GTSAM_USE_EIGEN_MKL_OPENMP From 8a88f101db2529330754993d46c5a653a1d9debd Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Tue, 24 Feb 2015 21:57:50 -0500 Subject: [PATCH 23/28] Fix deprecation warnings --- gtsam.h | 6 +- gtsam/base/LieMatrix.cpp | 2 +- gtsam/base/LieMatrix.h | 137 +--------------- gtsam/base/LieMatrix_Deprecated.h | 151 ++++++++++++++++++ gtsam/base/LieScalar.cpp | 2 +- gtsam/base/LieScalar.h | 69 +------- gtsam/base/LieScalar_Deprecated.h | 85 ++++++++++ gtsam/base/LieVector.cpp | 2 +- gtsam/base/LieVector.h | 102 +----------- gtsam/base/LieVector_Deprecated.h | 116 ++++++++++++++ gtsam/base/tests/testLieMatrix.cpp | 2 +- gtsam/base/tests/testLieScalar.cpp | 2 +- gtsam/base/tests/testLieVector.cpp | 2 +- gtsam/base/tests/testTestableAssertions.cpp | 2 +- gtsam_unstable/gtsam_unstable.h | 2 +- gtsam_unstable/slam/serialization.cpp | 5 +- .../tests/testGaussMarkov1stOrderFactor.cpp | 3 +- tests/testGaussianISAM2.cpp | 3 +- 18 files changed, 375 insertions(+), 318 deletions(-) create mode 100644 gtsam/base/LieMatrix_Deprecated.h create mode 100644 gtsam/base/LieScalar_Deprecated.h create mode 100644 gtsam/base/LieVector_Deprecated.h diff --git a/gtsam.h b/gtsam.h index f8e0af28e..1aee996dc 100644 --- a/gtsam.h +++ b/gtsam.h @@ -156,7 +156,7 @@ virtual class Value { size_t dim() const; }; -#include +#include class LieScalar { // Standard constructors LieScalar(); @@ -185,7 +185,7 @@ class LieScalar { static Vector Logmap(const gtsam::LieScalar& p); }; -#include +#include class LieVector { // Standard constructors LieVector(); @@ -217,7 +217,7 @@ class LieVector { void serialize() const; }; -#include +#include class LieMatrix { // Standard constructors LieMatrix(); diff --git a/gtsam/base/LieMatrix.cpp b/gtsam/base/LieMatrix.cpp index 69054fc1c..cf5b57536 100644 --- a/gtsam/base/LieMatrix.cpp +++ b/gtsam/base/LieMatrix.cpp @@ -15,7 +15,7 @@ * @author Richard Roberts and Alex Cunningham */ -#include +#include namespace gtsam { diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 90b7207a2..e24f339e4 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -11,147 +11,16 @@ /** * @file LieMatrix.h - * @brief A wrapper around Matrix providing Lie compatibility - * @author Richard Roberts and Alex Cunningham + * @brief External deprecation warning, see LieMatrix_Deprecated.h for details + * @author Paul Drews */ #pragma once -#include - #ifdef _MSC_VER #pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.") #else #warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead." #endif -#include -#include - -namespace gtsam { - -/** - * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as - * we can directly add double, Vector, and Matrix into values now, because of - * gtsam::traits. - */ -struct LieMatrix : public Matrix { - - /// @name Constructors - /// @{ - enum { dimension = Eigen::Dynamic }; - - /** default constructor - only for serialize */ - LieMatrix() {} - - /** initialize from a normal matrix */ - LieMatrix(const Matrix& v) : Matrix(v) {} - - template - LieMatrix(const M& v) : Matrix(v) {} - -// Currently TMP constructor causes ICE on MSVS 2013 -#if (_MSC_VER < 1800) - /** initialize from a fixed size normal vector */ - template - LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} -#endif - - /** constructor with size and initial data, row order ! */ - LieMatrix(size_t m, size_t n, const double* const data) : - Matrix(Eigen::Map(data, m, n)) {} - - /// @} - /// @name Testable interface - /// @{ - - /** print @param s optional string naming the object */ - GTSAM_EXPORT void print(const std::string& name="") const; - - /** equality up to tolerance */ - inline bool equals(const LieMatrix& expected, double tol=1e-5) const { - return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); - } - - /// @} - /// @name Standard Interface - /// @{ - - /** get the underlying matrix */ - inline Matrix matrix() const { - return static_cast(*this); - } - - /// @} - - /// @name Group - /// @{ - LieMatrix compose(const LieMatrix& q) { return (*this)+q;} - LieMatrix between(const LieMatrix& q) { return q-(*this);} - LieMatrix inverse() { return -(*this);} - /// @} - - /// @name Manifold - /// @{ - Vector localCoordinates(const LieMatrix& q) { return between(q).vector();} - LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));} - /// @} - - /// @name Lie Group - /// @{ - static Vector Logmap(const LieMatrix& p) {return p.vector();} - static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);} - /// @} - - /// @name VectorSpace requirements - /// @{ - - /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return size(); } - - /** Convert to vector, is done row-wise - TODO why? */ - inline Vector vector() const { - Vector result(size()); - typedef Eigen::Matrix RowMajor; - Eigen::Map(&result(0), rows(), cols()) = *this; - return result; - } - - /** identity - NOTE: no known size at compile time - so zero length */ - inline static LieMatrix identity() { - throw std::runtime_error("LieMatrix::identity(): Don't use this function"); - return LieMatrix(); - } - /// @} - -private: - - // Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Matrix", - boost::serialization::base_object(*this)); - - } - -}; - - -template<> -struct traits : public internal::VectorSpace { - - // Override Retract, as the default version does not know how to initialize - static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { - if (H1) *H1 = Eye(origin); - if (H2) *H2 = Eye(origin); - typedef const Eigen::Matrix RowMajor; - return origin + Eigen::Map(&v(0), origin.rows(), origin.cols()); - } - -}; - -} // \namespace gtsam +#include "gtsam/base/LieMatrix_Deprecated.h" diff --git a/gtsam/base/LieMatrix_Deprecated.h b/gtsam/base/LieMatrix_Deprecated.h new file mode 100644 index 000000000..5dbe9935f --- /dev/null +++ b/gtsam/base/LieMatrix_Deprecated.h @@ -0,0 +1,151 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LieMatrix.h + * @brief A wrapper around Matrix providing Lie compatibility + * @author Richard Roberts and Alex Cunningham + */ + +#pragma once + +#include + +#include +#include + +namespace gtsam { + +/** + * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * we can directly add double, Vector, and Matrix into values now, because of + * gtsam::traits. + */ +struct LieMatrix : public Matrix { + + /// @name Constructors + /// @{ + enum { dimension = Eigen::Dynamic }; + + /** default constructor - only for serialize */ + LieMatrix() {} + + /** initialize from a normal matrix */ + LieMatrix(const Matrix& v) : Matrix(v) {} + + template + LieMatrix(const M& v) : Matrix(v) {} + +// Currently TMP constructor causes ICE on MSVS 2013 +#if (_MSC_VER < 1800) + /** initialize from a fixed size normal vector */ + template + LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} +#endif + + /** constructor with size and initial data, row order ! */ + LieMatrix(size_t m, size_t n, const double* const data) : + Matrix(Eigen::Map(data, m, n)) {} + + /// @} + /// @name Testable interface + /// @{ + + /** print @param s optional string naming the object */ + GTSAM_EXPORT void print(const std::string& name="") const; + + /** equality up to tolerance */ + inline bool equals(const LieMatrix& expected, double tol=1e-5) const { + return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); + } + + /// @} + /// @name Standard Interface + /// @{ + + /** get the underlying matrix */ + inline Matrix matrix() const { + return static_cast(*this); + } + + /// @} + + /// @name Group + /// @{ + LieMatrix compose(const LieMatrix& q) { return (*this)+q;} + LieMatrix between(const LieMatrix& q) { return q-(*this);} + LieMatrix inverse() { return -(*this);} + /// @} + + /// @name Manifold + /// @{ + Vector localCoordinates(const LieMatrix& q) { return between(q).vector();} + LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));} + /// @} + + /// @name Lie Group + /// @{ + static Vector Logmap(const LieMatrix& p) {return p.vector();} + static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);} + /// @} + + /// @name VectorSpace requirements + /// @{ + + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return size(); } + + /** Convert to vector, is done row-wise - TODO why? */ + inline Vector vector() const { + Vector result(size()); + typedef Eigen::Matrix RowMajor; + Eigen::Map(&result(0), rows(), cols()) = *this; + return result; + } + + /** identity - NOTE: no known size at compile time - so zero length */ + inline static LieMatrix identity() { + throw std::runtime_error("LieMatrix::identity(): Don't use this function"); + return LieMatrix(); + } + /// @} + +private: + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Matrix", + boost::serialization::base_object(*this)); + + } + +}; + + +template<> +struct traits : public internal::VectorSpace { + + // Override Retract, as the default version does not know how to initialize + static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(origin); + if (H2) *H2 = Eye(origin); + typedef const Eigen::Matrix RowMajor; + return origin + Eigen::Map(&v(0), origin.rows(), origin.cols()); + } + +}; + +} // \namespace gtsam diff --git a/gtsam/base/LieScalar.cpp b/gtsam/base/LieScalar.cpp index 5f59c29bc..4c5a6a257 100644 --- a/gtsam/base/LieScalar.cpp +++ b/gtsam/base/LieScalar.cpp @@ -8,7 +8,7 @@ -#include +#include namespace gtsam { void LieScalar::print(const std::string& name) const { diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 9f6c56b28..650e2bb21 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -11,7 +11,7 @@ /** * @file LieScalar.h - * @brief A wrapper around scalar providing Lie compatibility + * @brief External deprecation warning, see LieScalar_Deprecated.h for details * @author Kai Ni */ @@ -23,69 +23,4 @@ #warning "LieScalar.h is deprecated. Please use double/float instead." #endif -#include -#include - -namespace gtsam { - - /** - * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as - * we can directly add double, Vector, and Matrix into values now, because of - * gtsam::traits. - */ - struct GTSAM_EXPORT LieScalar { - - enum { dimension = 1 }; - - /** default constructor */ - LieScalar() : d_(0.0) {} - - /** wrap a double */ - /*explicit*/ LieScalar(double d) : d_(d) {} - - /** access the underlying value */ - double value() const { return d_; } - - /** Automatic conversion to underlying value */ - operator double() const { return d_; } - - /** convert vector */ - Vector1 vector() const { Vector1 v; v< - struct traits : public internal::ScalarTraits {}; - -} // \namespace gtsam +#include diff --git a/gtsam/base/LieScalar_Deprecated.h b/gtsam/base/LieScalar_Deprecated.h new file mode 100644 index 000000000..6ffc76d37 --- /dev/null +++ b/gtsam/base/LieScalar_Deprecated.h @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LieScalar.h + * @brief A wrapper around scalar providing Lie compatibility + * @author Kai Ni + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * we can directly add double, Vector, and Matrix into values now, because of + * gtsam::traits. + */ + struct GTSAM_EXPORT LieScalar { + + enum { dimension = 1 }; + + /** default constructor */ + LieScalar() : d_(0.0) {} + + /** wrap a double */ + /*explicit*/ LieScalar(double d) : d_(d) {} + + /** access the underlying value */ + double value() const { return d_; } + + /** Automatic conversion to underlying value */ + operator double() const { return d_; } + + /** convert vector */ + Vector1 vector() const { Vector1 v; v< + struct traits : public internal::ScalarTraits {}; + +} // \namespace gtsam diff --git a/gtsam/base/LieVector.cpp b/gtsam/base/LieVector.cpp index 84afdabc8..3e4eeecf2 100644 --- a/gtsam/base/LieVector.cpp +++ b/gtsam/base/LieVector.cpp @@ -15,8 +15,8 @@ * @author Alex Cunningham */ +#include #include -#include using namespace std; diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index c2003df3c..813431775 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -11,8 +11,8 @@ /** * @file LieVector.h - * @brief A wrapper around vector providing Lie compatibility - * @author Alex Cunningham + * @brief Deprecation warning for LieVector_Deprecated.h, see LieVector_Deprecated.h for details. + * @author Paul Drews */ #pragma once @@ -23,100 +23,4 @@ #warning "LieVector.h is deprecated. Please use Eigen::Vector instead." #endif -#include - -namespace gtsam { - -/** - * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as - * we can directly add double, Vector, and Matrix into values now, because of - * gtsam::traits. - */ -struct LieVector : public Vector { - - enum { dimension = Eigen::Dynamic }; - - /** default constructor - should be unnecessary */ - LieVector() {} - - /** initialize from a normal vector */ - LieVector(const Vector& v) : Vector(v) {} - - template - LieVector(const V& v) : Vector(v) {} - -// Currently TMP constructor causes ICE on MSVS 2013 -#if (_MSC_VER < 1800) - /** initialize from a fixed size normal vector */ - template - LieVector(const Eigen::Matrix& v) : Vector(v) {} -#endif - - /** wrap a double */ - LieVector(double d) : Vector((Vector(1) << d).finished()) {} - - /** constructor with size and initial data, row order ! */ - GTSAM_EXPORT LieVector(size_t m, const double* const data); - - /// @name Testable - /// @{ - GTSAM_EXPORT void print(const std::string& name="") const; - bool equals(const LieVector& expected, double tol=1e-5) const { - return gtsam::equal(vector(), expected.vector(), tol); - } - /// @} - - /// @name Group - /// @{ - LieVector compose(const LieVector& q) { return (*this)+q;} - LieVector between(const LieVector& q) { return q-(*this);} - LieVector inverse() { return -(*this);} - /// @} - - /// @name Manifold - /// @{ - Vector localCoordinates(const LieVector& q) { return between(q).vector();} - LieVector retract(const Vector& v) {return compose(LieVector(v));} - /// @} - - /// @name Lie Group - /// @{ - static Vector Logmap(const LieVector& p) {return p.vector();} - static LieVector Expmap(const Vector& v) { return LieVector(v);} - /// @} - - /// @name VectorSpace requirements - /// @{ - - /** get the underlying vector */ - Vector vector() const { - return static_cast(*this); - } - - /** Returns dimensionality of the tangent space */ - size_t dim() const { return this->size(); } - - /** identity - NOTE: no known size at compile time - so zero length */ - static LieVector identity() { - throw std::runtime_error("LieVector::identity(): Don't use this function"); - return LieVector(); - } - - /// @} - -private: - - // Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Vector", - boost::serialization::base_object(*this)); - } -}; - - -template<> -struct traits : public internal::VectorSpace {}; - -} // \namespace gtsam +#include diff --git a/gtsam/base/LieVector_Deprecated.h b/gtsam/base/LieVector_Deprecated.h new file mode 100644 index 000000000..3cb73319d --- /dev/null +++ b/gtsam/base/LieVector_Deprecated.h @@ -0,0 +1,116 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LieVector.h + * @brief A wrapper around vector providing Lie compatibility + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * we can directly add double, Vector, and Matrix into values now, because of + * gtsam::traits. + */ +struct LieVector : public Vector { + + enum { dimension = Eigen::Dynamic }; + + /** default constructor - should be unnecessary */ + LieVector() {} + + /** initialize from a normal vector */ + LieVector(const Vector& v) : Vector(v) {} + + template + LieVector(const V& v) : Vector(v) {} + +// Currently TMP constructor causes ICE on MSVS 2013 +#if (_MSC_VER < 1800) + /** initialize from a fixed size normal vector */ + template + LieVector(const Eigen::Matrix& v) : Vector(v) {} +#endif + + /** wrap a double */ + LieVector(double d) : Vector((Vector(1) << d).finished()) {} + + /** constructor with size and initial data, row order ! */ + GTSAM_EXPORT LieVector(size_t m, const double* const data); + + /// @name Testable + /// @{ + GTSAM_EXPORT void print(const std::string& name="") const; + bool equals(const LieVector& expected, double tol=1e-5) const { + return gtsam::equal(vector(), expected.vector(), tol); + } + /// @} + + /// @name Group + /// @{ + LieVector compose(const LieVector& q) { return (*this)+q;} + LieVector between(const LieVector& q) { return q-(*this);} + LieVector inverse() { return -(*this);} + /// @} + + /// @name Manifold + /// @{ + Vector localCoordinates(const LieVector& q) { return between(q).vector();} + LieVector retract(const Vector& v) {return compose(LieVector(v));} + /// @} + + /// @name Lie Group + /// @{ + static Vector Logmap(const LieVector& p) {return p.vector();} + static LieVector Expmap(const Vector& v) { return LieVector(v);} + /// @} + + /// @name VectorSpace requirements + /// @{ + + /** get the underlying vector */ + Vector vector() const { + return static_cast(*this); + } + + /** Returns dimensionality of the tangent space */ + size_t dim() const { return this->size(); } + + /** identity - NOTE: no known size at compile time - so zero length */ + static LieVector identity() { + throw std::runtime_error("LieVector::identity(): Don't use this function"); + return LieVector(); + } + + /// @} + +private: + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Vector", + boost::serialization::base_object(*this)); + } +}; + + +template<> +struct traits : public internal::VectorSpace {}; + +} // \namespace gtsam diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 7cc649e66..09caadabd 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -15,10 +15,10 @@ */ #include +#include #include #include -#include using namespace gtsam; diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 060087c2a..141b55761 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -15,10 +15,10 @@ */ #include +#include #include #include -#include using namespace gtsam; diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index 81e03c63c..ed3afac8c 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -15,10 +15,10 @@ */ #include +#include #include #include -#include using namespace gtsam; diff --git a/gtsam/base/tests/testTestableAssertions.cpp b/gtsam/base/tests/testTestableAssertions.cpp index d8c62b121..364834e2a 100644 --- a/gtsam/base/tests/testTestableAssertions.cpp +++ b/gtsam/base/tests/testTestableAssertions.cpp @@ -15,8 +15,8 @@ */ #include +#include -#include #include using namespace gtsam; diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 95e635516..9aa32e1c3 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -477,7 +477,7 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model); }; -#include +#include #include virtual class VelocityConstraint3 : gtsam::NonlinearFactor { diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 31995e769..a598fb689 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -5,6 +5,8 @@ * @author Alex Cunningham */ +#include +#include #include #include @@ -22,9 +24,6 @@ #include #include #include -#include -#include -//#include #include #include #include diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index ff7307840..a86306a8d 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -20,9 +20,8 @@ #include #include #include -#include - #include +#include using namespace std; using namespace gtsam; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index b43f0d3ef..4e14d49b3 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -23,10 +23,9 @@ #include #include #include -#include - #include #include +#include using namespace boost::assign; #include namespace br { using namespace boost::adaptors; using namespace boost::range; } From 9991ae04f3713cc508314d506f9a76339195d54c Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Wed, 25 Feb 2015 10:59:25 -0500 Subject: [PATCH 24/28] Fixed unit tests --- gtsam_unstable/geometry/Similarity3.cpp | 6 ++- gtsam_unstable/geometry/Similarity3.h | 4 ++ .../geometry/tests/testSimilarity3.cpp | 45 +++++++++++++------ 3 files changed, 41 insertions(+), 14 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index c805d8aab..b8fcd8ce0 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -53,8 +53,12 @@ Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) { s_ = s; } +Similarity3::operator Pose3() const { + return Pose3(R_, s_*t_); +} + Similarity3 Similarity3::identity() { - std::cout << "Identity!" << std::endl; + //std::cout << "Identity!" << std::endl; return Similarity3(); } Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 8d7ccf82f..59425bb72 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -20,6 +20,7 @@ #include #include +#include #include namespace gtsam { @@ -51,6 +52,9 @@ public: /// Construct from Eigen types Similarity3(const Matrix3& R, const Vector3& t, double s); + /// Convert to a rigid body pose + operator Pose3() const; + /// Return the translation const Vector3 t() const; diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 51125eb2d..6741b7481 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -182,23 +182,26 @@ TEST(Similarity3, Optimization) { } TEST(Similarity3, Optimization2) { - Similarity3 prior = Similarity3();//Rot3::ypr(0, 0, 0), Point3(1, 2, 3), 4); - Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1.0, 0, 0), 1.0); - Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(0.9, 0, 0), 1.0); - Similarity3 m3 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(0.8, 0, 0), 1.0); - Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(0.7, 0, 0), 1.42); + Similarity3 prior = Similarity3(); + Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0); + Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0); + Similarity3 m3 = Similarity3(Rot3::ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0); + Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0); + Similarity3 loop = Similarity3(1.42); //prior.print("Goal Transform"); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 0.01); SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( - (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1).finished()); + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( - (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.01).finished()); + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); PriorFactor factor(X(1), prior, model); BetweenFactor b1(X(1), X(2), m1, betweenNoise); BetweenFactor b2(X(2), X(3), m2, betweenNoise); BetweenFactor b3(X(3), X(4), m3, betweenNoise); - BetweenFactor b4(X(4), X(1), m4, betweenNoise2); + BetweenFactor b4(X(4), X(5), m4, betweenNoise); + BetweenFactor lc(X(5), X(1), loop, betweenNoise2); + NonlinearFactorGraph graph; @@ -207,21 +210,37 @@ TEST(Similarity3, Optimization2) { graph.push_back(b2); graph.push_back(b3); graph.push_back(b4); + graph.push_back(lc); - graph.print("Full Graph"); + //graph.print("Full Graph\n"); Values initial; initial.insert(X(1), Similarity3()); initial.insert(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1)); initial.insert(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); initial.insert(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3)); + initial.insert(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0)); - initial.print("Initial Estimate"); + //initial.print("Initial Estimate\n"); Values result; result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - result.print("Optimized Estimate"); - //EXPECT(assert_equal(prior, result.at(key), 1e-4)); + //result.print("Optimized Estimate\n"); + Pose3 p1, p2, p3, p4, p5; + p1 = Pose3(result.at(X(1))); + p2 = Pose3(result.at(X(2))); + p3 = Pose3(result.at(X(3))); + p4 = Pose3(result.at(X(4))); + p5 = Pose3(result.at(X(5))); + + //p1.print("Pose1"); + //p2.print("Pose2"); + //p3.print("Pose3"); + //p4.print("Pose4"); + //p5.print("Pose5"); + + Similarity3 expected(0.7); + EXPECT(assert_equal(expected, result.at(X(5)), 0.4)); } //****************************************************************************** From 5c40d62b5676bf32215e7a120c69fe5908c6f472 Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Wed, 25 Feb 2015 14:20:26 -0500 Subject: [PATCH 25/28] Fixed accidentally deleted function --- gtsam/geometry/Rot3.h | 3 +++ gtsam/geometry/Rot3M.cpp | 7 +++++++ gtsam/geometry/Rot3Q.cpp | 4 ++++ 3 files changed, 14 insertions(+) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 93810d436..d35fa52f4 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -87,6 +87,9 @@ namespace gtsam { /** constructor from a rotation matrix */ Rot3(const Matrix3& R); + /** constructor from a rotation matrix */ + Rot3(const Matrix& R); + /** Constructor from a quaternion. This can also be called using a plain * Vector, due to implicit conversion from Vector to Quaternion * @param q The quaternion diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 9fffb0c2e..b4c79de3b 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -55,6 +55,13 @@ Rot3::Rot3(const Matrix3& R) { rot_ = R; } +/* ************************************************************************* */ +Rot3::Rot3(const Matrix& R) { + if (R.rows()!=3 || R.cols()!=3) + throw invalid_argument("Rot3 constructor expects 3*3 matrix"); + rot_ = R; +} + /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) { } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 81ea77fb7..52fb4f262 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -52,6 +52,10 @@ namespace gtsam { Rot3::Rot3(const Matrix3& R) : quaternion_(R) {} + /* ************************************************************************* */ + Rot3::Rot3(const Matrix& R) : + quaternion_(Matrix3(R)) {} + /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : quaternion_(q) { From 6faa881de641939a36d9f0001e9921f355d7e06a Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Thu, 26 Feb 2015 15:10:59 -0500 Subject: [PATCH 26/28] Make the code prettier. --- gtsam_unstable/geometry/Similarity3.cpp | 90 +++++---------- gtsam_unstable/geometry/Similarity3.h | 106 +++++++++++------- .../geometry/tests/testSimilarity3.cpp | 4 + 3 files changed, 99 insertions(+), 101 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index b8fcd8ce0..cfc508ac7 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -12,45 +12,33 @@ /** * @file Similarity3.cpp * @brief Implementation of Similarity3 transform + * @author Paul Drews */ +#include + #include #include +#include + #include -#include namespace gtsam { -Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) { - R_ = R; - t_ = t; - s_ = s; -} - -/// Return the translation -const Vector3 Similarity3::t() const { - return t_.vector(); -} - -/// Return the rotation matrix -const Matrix3 Similarity3::R() const { - return R_.matrix(); +Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : + R_(R), t_(t), s_(s) { } Similarity3::Similarity3() : - R_(), t_(), s_(1){ + R_(), t_(), s_(1){ } -/// Construct pure scaling -Similarity3::Similarity3(double s) { - s_ = s; +Similarity3::Similarity3(double s) : + s_ (s) { } -/// Construct from GTSAM types -Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) { - R_ = R; - t_ = t; - s_ = s; +Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) : + R_ (R), t_ (t), s_ (s) { } Similarity3::operator Pose3() const { @@ -58,30 +46,26 @@ Similarity3::operator Pose3() const { } Similarity3 Similarity3::identity() { - //std::cout << "Identity!" << std::endl; return Similarity3(); } -Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { - std::cout << "Logmap!" << std::endl; - return Vector7(); -} - -Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { - std::cout << "Expmap!" << std::endl; - return Similarity3(); -} +//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { +// return Vector7(); +//} +// +//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { +// return Similarity3(); +//} bool Similarity3::operator==(const Similarity3& other) const { return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); } -/// Compare with tolerance bool Similarity3::equals(const Similarity3& sim, double tol) const { - return rotation().equals(sim.rotation(), tol) && translation().equals(sim.translation(), tol) - && scale() < (sim.scale()+tol) && scale() > (sim.scale()-tol); + return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol) + && s_ < (sim.s_+tol) && s_ > (sim.s_-tol); } -Point3 Similarity3::transform_from(const Point3& p) const { +Similarity3::Translation Similarity3::transform_from(const Translation& p) const { return R_ * (s_ * p) + t_; } @@ -94,14 +78,13 @@ Matrix7 Similarity3::AdjointMap() const{ return adj; } -/** syntactic sugar for transform_from */ -inline Point3 Similarity3::operator*(const Point3& p) const { +inline Similarity3::Translation Similarity3::operator*(const Translation& p) const { return transform_from(p); } Similarity3 Similarity3::inverse() const { - Rot3 Rt = R_.inverse(); - Point3 sRt = R_.inverse() * (-s_ * t_); + Rotation Rt = R_.inverse(); + Translation sRt = R_.inverse() * (-s_ * t_); return Similarity3(Rt, sRt, 1.0/s_); } @@ -117,35 +100,18 @@ void Similarity3::print(const std::string& s) const { std::cout << "s: " << scale() << std::endl; } -/// Return the rotation matrix -Rot3 Similarity3::rotation() const { - return R_; -} - -/// Return the translation -Point3 Similarity3::translation() const { - return t_; -} - -/// Return the scale -double Similarity3::scale() const { - return s_; -} - -/// Update Similarity transform via 7-dim vector in tangent space Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) { // Will retracting or localCoordinating R work if R is not a unit rotation? // Also, how do we actually get s out? Seems like we need to store it somewhere. - Rot3 r; //Create a zero rotation to do our retraction. + Rotation r; //Create a zero rotation to do our retraction. return Similarity3( // r.retract(v.head<3>()), // retract rotation using v[0,1,2] - Point3(v.segment<3>(3)), // Retract the translation + Translation(v.segment<3>(3)), // Retract the translation 1.0 + v[6]); //finally, update scale using v[6] } -/// 7-dimensional vector v in tangent space that makes other = this->retract(v) Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) { - Rot3 r; //Create a zero rotation to do the retraction + Rotation r; //Create a zero rotation to do the retraction Vector7 v; v.head<3>() = r.localCoordinates(other.R_); v.segment<3>(3) = other.t_.vector(); diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 59425bb72..89f1010c4 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -12,19 +12,21 @@ /** * @file Similarity3.h * @brief Implementation of Similarity3 transform + * @author Paul Drews */ -#ifndef GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ -#define GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ - +#pragma once #include #include -#include +#include #include namespace gtsam { +// Forward declarations +class Pose3; + /** * 3D similarity transform */ @@ -35,55 +37,77 @@ class Similarity3: public LieGroup { typedef Point3 Translation; private: - Rot3 R_; - Point3 t_; + Rotation R_; + Translation t_; double s_; public: + /// @name Constructors + /// @{ + Similarity3(); /// Construct pure scaling Similarity3(double s); /// Construct from GTSAM types - Similarity3(const Rot3& R, const Point3& t, double s); + Similarity3(const Rotation& R, const Translation& t, double s); /// Construct from Eigen types Similarity3(const Matrix3& R, const Vector3& t, double s); - /// Convert to a rigid body pose - operator Pose3() const; - - /// Return the translation - const Vector3 t() const; - - /// Return the rotation matrix - const Matrix3 R() const; - - static Similarity3 identity(); - - static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none); - - static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none); - - bool operator==(const Similarity3& other) const; + /// @} + /// @name Testable + /// @{ /// Compare with tolerance bool equals(const Similarity3& sim, double tol) const; - Point3 transform_from(const Point3& p) const; + /// Compare with standard tolerance + bool operator==(const Similarity3& other) const; - Matrix7 AdjointMap() const; + /// Print with optional string + void print(const std::string& s) const; + + /// @} + /// @name Group + /// @{ + + /// Return an identity transform + static Similarity3 identity(); + + /// Return the inverse + Similarity3 inverse() const; + + Translation transform_from(const Translation& p) const; /** syntactic sugar for transform_from */ - inline Point3 operator*(const Point3& p) const; - - Similarity3 inverse() const; + inline Translation operator*(const Translation& p) const; Similarity3 operator*(const Similarity3& T) const; - void print(const std::string& s) const; + /// @} + /// @name Standard interface + /// @{ + + /// Return a GTSAM rotation + const Rotation& rotation() const { + return R_; + }; + + /// Return a GTSAM translation + const Translation& translation() const { + return t_; + }; + + /// Return the scale + double scale() const { + return s_; + }; + + /// Convert to a rigid body pose + operator Pose3() const; /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes inline static size_t Dim() { @@ -95,14 +119,9 @@ public: return 7; }; - /// Return the rotation matrix - Rot3 rotation() const; - - /// Return the translation - Point3 translation() const; - - /// Return the scale - double scale() const; + /// @} + /// @name Chart + /// @{ /// Update Similarity transform via 7-dim vector in tangent space struct ChartAtOrigin { @@ -112,11 +131,20 @@ public: static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none); }; + /// Project from one tangent space to another + Matrix7 AdjointMap() const; + + /// @} + /// @name Stubs + /// @{ + + /// Not currently implemented, required because this is a lie group + static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none); + static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none); + using LieGroup::inverse; // version with derivative }; template<> struct traits : public internal::LieGroupTraits {}; } - -#endif /* GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ */ diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 6741b7481..b2b5d5702 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -12,9 +12,13 @@ /** * @file testSimilarity3.cpp * @brief Unit tests for Similarity3 class + * @author Paul Drews */ #include +#include +#include +#include #include #include #include From d8468567b2533ca9a684a7f59f7593fd75836e0c Mon Sep 17 00:00:00 2001 From: alexhagiopol Date: Mon, 2 Mar 2015 20:19:59 -0500 Subject: [PATCH 27/28] MOre bloat reduction and header ordering. --- gtsam/base/FastSet.h | 8 ++++---- gtsam/base/Matrix.cpp | 12 +++++++++--- gtsam/base/Matrix.h | 13 +++++++++---- gtsam/base/Value.h | 5 +++-- gtsam/base/Vector.cpp | 17 +++++++++++------ gtsam/base/Vector.h | 14 ++++++++++---- gtsam/base/types.cpp | 2 +- gtsam/base/types.h | 6 +++--- 8 files changed, 50 insertions(+), 27 deletions(-) diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 29fb3fcd9..4ef963f5d 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -19,14 +19,14 @@ #pragma once #include -#include -#include -#include -#include #include #include #include #include +#include +#include +#include +#include BOOST_MPL_HAS_XXX_TRAIT_DEF(print) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 7136db768..7fa0992ca 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -32,9 +34,6 @@ #include #include -#include -#include - using namespace std; namespace gtsam { @@ -183,6 +182,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec } /* ************************************************************************* */ +//3 argument call void print(const Matrix& A, const string &s, ostream& stream) { size_t m = A.rows(), n = A.cols(); @@ -201,6 +201,12 @@ void print(const Matrix& A, const string &s, ostream& stream) { stream << "];" << endl; } +/* ************************************************************************* */ +//1 or 2 argument call +void print(const Matrix& A, const string &s){ + print(A, s, cout); +} + /* ************************************************************************* */ void save(const Matrix& A, const string &s, const string& filename) { fstream stream(filename.c_str(), fstream::out | fstream::app); diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index fc9f27099..27b7ec8f7 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -22,12 +22,12 @@ #pragma once #include -#include -#include #include #include #include #include +#include +#include /** @@ -201,9 +201,14 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) { } /** - * print a matrix + * print without optional string, must specify cout yourself */ -GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "", std::ostream& stream = std::cout); +GTSAM_EXPORT void print(const Matrix& A, const std::string& s, std::ostream& stream); + +/** + * print with optional string to cout + */ +GTSAM_EXPORT void print(const Matrix& A, const std::string& s = ""); /** * save a matrix to file, which can be loaded by matlab diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index a12f453f4..70677cad1 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -18,9 +18,10 @@ #pragma once -#include -#include #include +#include +#include + namespace gtsam { diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 1b145a116..0a708427a 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -16,20 +16,18 @@ * @author Frank Dellaert */ - +#include +#include +#include +#include #include #include #include #include #include #include -#include -#include -#include #include #include -#include - using namespace std; @@ -55,6 +53,7 @@ Vector delta(size_t n, size_t i, double value) { } /* ************************************************************************* */ +//3 argument call void print(const Vector& v, const string& s, ostream& stream) { size_t n = v.size(); @@ -65,6 +64,12 @@ void print(const Vector& v, const string& s, ostream& stream) { stream << "];" << endl; } +/* ************************************************************************* */ +//1 or 2 argument call +void print(const Vector& v, const string& s) { + print(v, s, cout); +} + /* ************************************************************************* */ void save(const Vector& v, const string &s, const string& filename) { fstream stream(filename.c_str(), fstream::out | fstream::app); diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 1c433eb4a..2d333848b 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -20,10 +20,11 @@ #pragma once -#include #include #include -#include +#include +#include + namespace gtsam { // Vector is just a typedef of the Eigen dynamic vector type @@ -95,9 +96,14 @@ GTSAM_EXPORT bool zero(const Vector& v); inline size_t dim(const Vector& v) { return v.size(); } /** - * print with optional string + * print without optional string, must specify cout yourself */ -GTSAM_EXPORT void print(const Vector& v, const std::string& s = "", std::ostream& stream = std::cout); +GTSAM_EXPORT void print(const Vector& v, const std::string& s, std::ostream& stream); + +/** + * print with optional string to cout + */ +GTSAM_EXPORT void print(const Vector& v, const std::string& s = ""); /** * save a vector to file, which can be loaded by matlab diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp index 4f61c6a1d..03e7fd120 100644 --- a/gtsam/base/types.cpp +++ b/gtsam/base/types.cpp @@ -17,9 +17,9 @@ * @addtogroup base */ -#include #include #include +#include namespace gtsam { diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 122bc18a0..a66db13c8 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -21,12 +21,12 @@ #include #include -#include -#include -#include #include #include #include +#include +#include +#include #ifdef GTSAM_USE_TBB #include From 3ab4c329e8faf0d91f90b5ffbb99605b264fd9e7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 4 Mar 2015 12:10:43 -0800 Subject: [PATCH 28/28] Check explicit poses rather than printing them --- .../tests/testSmartProjectionPoseFactor.cpp | 178 ++++++++++++------ 1 file changed, 122 insertions(+), 56 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 474009cfb..07c49008d 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -296,8 +296,12 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); LevenbergMarquardtParams params; if (isDebugTest) @@ -306,7 +310,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_ (SmartProjectionPoseFactor); + gttic_(SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); @@ -316,8 +320,6 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { // VectorValues delta = GFG->optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); if (isDebugTest) tictoc_print_(); @@ -416,8 +418,6 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { result = optimizer.optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); EXPECT(assert_equal(bodyPose3, result.at(x3))); // Check derivatives @@ -447,15 +447,16 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Factors ){ +TEST( SmartProjectionPoseFactor, Factors ) { // Default cameras for simple derivatives Rot3 R; static Cal3_S2::shared_ptr K(new Cal3_S2(100, 100, 0, 0, 0)); - SimpleCamera cam1(Pose3(R,Point3(0,0,0)),*K), cam2(Pose3(R,Point3(1,0,0)),*K); + SimpleCamera cam1(Pose3(R, Point3(0, 0, 0)), *K), cam2( + Pose3(R, Point3(1, 0, 0)), *K); // one landmarks 1m in front of camera - Point3 landmark1(0,0,10); + Point3 landmark1(0, 0, 10); vector measurements_cam1; @@ -476,12 +477,12 @@ TEST( SmartProjectionPoseFactor, Factors ){ cameras.push_back(cam2); // Make sure triangulation works - LONGS_EQUAL(2,smartFactor1->triangulateSafe(cameras)); + LONGS_EQUAL(2, smartFactor1->triangulateSafe(cameras)); CHECK(!smartFactor1->isDegenerate()); CHECK(!smartFactor1->isPointBehindCamera()); boost::optional p = smartFactor1->point(); CHECK(p); - EXPECT(assert_equal(landmark1,*p)); + EXPECT(assert_equal(landmark1, *p)); // After eliminating the point, A1 and A2 contain 2-rank information on cameras: Matrix16 A1, A2; @@ -489,12 +490,14 @@ TEST( SmartProjectionPoseFactor, Factors ){ A2 << 1000, 0, 100, 0, -100, 0; { // createHessianFactor - Matrix66 G11 = 0.5*A1.transpose()*A1; - Matrix66 G12 = 0.5*A1.transpose()*A2; - Matrix66 G22 = 0.5*A2.transpose()*A2; + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; - Vector6 g1; g1.setZero(); - Vector6 g2; g2.setZero(); + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); double f = 0; @@ -507,13 +510,32 @@ TEST( SmartProjectionPoseFactor, Factors ){ } { - Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; - Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; - Matrix43 E; E.setZero(); E(0,0)=100; E(1,1)=100; E(2,0)=100; E(2,2)=10;E(3,1)=100; + Matrix26 F1; + F1.setZero(); + F1(0, 1) = -100; + F1(0, 3) = -10; + F1(1, 0) = 100; + F1(1, 4) = -10; + Matrix26 F2; + F2.setZero(); + F2(0, 1) = -101; + F2(0, 3) = -10; + F2(0, 5) = -1; + F2(1, 0) = 100; + F2(1, 2) = 10; + F2(1, 4) = -10; + Matrix43 E; + E.setZero(); + E(0, 0) = 100; + E(1, 1) = 100; + E(2, 0) = 100; + E(2, 2) = 10; + E(3, 1) = 100; const vector > Fblocks = list_of > // - (make_pair(x1, 10*F1))(make_pair(x2, 10*F2)); + (make_pair(x1, 10 * F1))(make_pair(x2, 10 * F2)); Matrix3 P = (E.transpose() * E).inverse(); - Vector4 b; b.setZero(); + Vector4 b; + b.setZero(); // createRegularImplicitSchurFactor RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); @@ -524,7 +546,7 @@ TEST( SmartProjectionPoseFactor, Factors ){ CHECK(assert_equal(expected, *actual)); // createJacobianQFactor - JacobianFactorQ < 6, 2 > expectedQ(Fblocks, E, P, b); + JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b); boost::shared_ptr > actualQ = smartFactor1->createJacobianQFactor(cameras, 0.0); @@ -534,9 +556,10 @@ TEST( SmartProjectionPoseFactor, Factors ){ { // createJacobianSVDFactor - Vector1 b; b.setZero(); + Vector1 b; + b.setZero(); double s = sin(M_PI_4); - JacobianFactor expected(x1, s*A1, x2, s*A2, b); + JacobianFactor expected(x1, s * A1, x2, s * A2, b); boost::shared_ptr actual = smartFactor1->createJacobianSVDFactor(cameras, 0.0); @@ -604,8 +627,13 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, + -0.0313952598), Point3(0.1, -0.1, 1.9)), + values.at(x3))); LevenbergMarquardtParams params; if (isDebugTest) @@ -614,15 +642,13 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_ (SmartProjectionPoseFactor); + gttic_(SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); EXPECT(assert_equal(pose3, result.at(x3), 1e-7)); if (isDebugTest) tictoc_print_(); @@ -1050,8 +1076,14 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); boost::shared_ptr hessianFactor1 = smartFactor1->linearize( values); @@ -1143,8 +1175,14 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac values.insert(x2, pose2 * noise_pose); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814, + -0.572308662, -0.324093872, 0.639358349, -0.521617766, + -0.564921063), + Point3(0.145118171, -0.252907438, 0.819956033)), + values.at(x3))); LevenbergMarquardtParams params; if (isDebugTest) @@ -1153,19 +1191,24 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_ (SmartProjectionPoseFactor); + gttic_(SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; - // EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT( + assert_equal( + Pose3( + Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814, + -0.572308662, -0.324093872, 0.639358349, -0.521617766, + -0.564921063), + Point3(0.145118171, -0.252907438, 0.819956033)), + result.at(x3))); if (isDebugTest) tictoc_print_(); } @@ -1240,8 +1283,14 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); LevenbergMarquardtParams params; if (isDebugTest) @@ -1250,19 +1299,24 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_ (SmartProjectionPoseFactor); + gttic_(SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; - // EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + result.at(x3))); if (isDebugTest) tictoc_print_(); } @@ -1543,9 +1597,12 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); - + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); LevenbergMarquardtParams params; if (isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; @@ -1553,15 +1610,12 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_ (SmartProjectionPoseFactor); + gttic_(SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); - // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); EXPECT(assert_equal(pose3, result.at(x3), 1e-6)); if (isDebugTest) tictoc_print_(); @@ -1653,8 +1707,14 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); LevenbergMarquardtParams params; if (isDebugTest) @@ -1663,15 +1723,21 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_ (SmartProjectionPoseFactor); + gttic_(SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl;