From 51c4a50c2305c426bb352a8690499346899bed9a Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Fri, 5 Dec 2014 09:28:10 -0500 Subject: [PATCH 01/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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 9991ae04f3713cc508314d506f9a76339195d54c Mon Sep 17 00:00:00 2001 From: Paul Drews Date: Wed, 25 Feb 2015 10:59:25 -0500 Subject: [PATCH 18/20] 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 19/20] 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 20/20] 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