From 8552752839105a1255d71951250e302ec6c0a8cc Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 19 Feb 2021 16:08:43 -0500 Subject: [PATCH 01/30] Start moving Sim(3) functionality into Python wrapper --- gtsam_unstable/gtsam_unstable.i | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index e18d32b59..b5d9133cd 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -163,6 +163,15 @@ class BearingS2 { void serializable() const; // enabling serialization functionality }; +#include +class Similarity3 { + Similarity3(); + Similarity3(double s); + Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s); + Similarity3(const Matrix& R, const Vector& t, double s); + Similarity3(const Matrix& T); +}; + #include class SimWall2D { SimWall2D(); From 0effe69df2ddbac8c26c851c8e7c53d97601278b Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 24 Feb 2021 15:26:13 -0500 Subject: [PATCH 02/30] add sim3 Point3 align to wrapper --- gtsam_unstable/geometry/Similarity3.cpp | 2 +- gtsam_unstable/gtsam_unstable.i | 11 +++++++++++ python/CMakeLists.txt | 1 + python/gtsam/specializations.h | 1 + 4 files changed, 14 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 819c51fee..8e3e97067 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -24,7 +24,7 @@ namespace gtsam { using std::vector; -using PointPairs = vector; +typedef PointPairs = vector; namespace { /// Subtract centroids from point pairs. diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index b5d9133cd..1ca73bbe6 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -164,12 +164,23 @@ class BearingS2 { }; #include +class PointPairs +{ + PointPairs(); + size_t size() const; + bool empty() const; + gtsam::Point3Pair at(size_t n) const; + void push_back(const gtsam::Point3Pair& point_pair); +}; + class Similarity3 { Similarity3(); Similarity3(double s); Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s); Similarity3(const Matrix& R, const Vector& t, double s); Similarity3(const Matrix& T); + + static Similarity3 Align(const gtsam::PointPairs & abPointPairs); }; #include diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index b50701464..3d6bf1702 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -41,6 +41,7 @@ set(ignore gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s gtsam::Point2Vector + gtsam::PointPairs gtsam::Pose3Vector gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 431697aac..04c1aa82c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -6,6 +6,7 @@ py::bind_vector > >(m_, " py::bind_vector >(m_, "KeyVector"); #endif py::bind_vector > >(m_, "Point2Vector"); +py::bind_vector >(m_, "PointPairs"); py::bind_vector >(m_, "Pose3Vector"); py::bind_vector > > >(m_, "BetweenFactorPose3s"); py::bind_vector > > >(m_, "BetweenFactorPose2s"); From bbd7ed4f6969f177b18e021cb64abb0ddd95b3a5 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 24 Feb 2021 15:42:58 -0500 Subject: [PATCH 03/30] Fix typo in using -> typedef conversion --- gtsam_unstable/geometry/Similarity3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 8e3e97067..7252095a4 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -24,7 +24,7 @@ namespace gtsam { using std::vector; -typedef PointPairs = vector; +typedef vector PointPairs; namespace { /// Subtract centroids from point pairs. From 0a2deab5b6380e4c24d2e692775545e844e1be0e Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 24 Feb 2021 17:39:16 -0500 Subject: [PATCH 04/30] move sim3 to stable version --- .../geometry/Similarity3.cpp | 2 +- .../geometry/Similarity3.h | 0 gtsam/gtsam.i | 23 +++++++++++++++++++ gtsam_unstable/gtsam_unstable.i | 19 --------------- 4 files changed, 24 insertions(+), 20 deletions(-) rename {gtsam_unstable => gtsam}/geometry/Similarity3.cpp (99%) rename {gtsam_unstable => gtsam}/geometry/Similarity3.h (100%) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp similarity index 99% rename from gtsam_unstable/geometry/Similarity3.cpp rename to gtsam/geometry/Similarity3.cpp index 7252095a4..5cb1466b4 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -15,7 +15,7 @@ * @author Paul Drews */ -#include +#include #include #include diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h similarity index 100% rename from gtsam_unstable/geometry/Similarity3.h rename to gtsam/geometry/Similarity3.h diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 22c2cc17d..b6b3ecac4 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1058,6 +1058,29 @@ class PinholeCamera { void serialize() const; }; + +#include +class PointPairs +{ + PointPairs(); + size_t size() const; + bool empty() const; + gtsam::Point3Pair at(size_t n) const; + void push_back(const gtsam::Point3Pair& point_pair); +}; + +class Similarity3 { + Similarity3(); + Similarity3(double s); + Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s); + Similarity3(const Matrix& R, const Vector& t, double s); + Similarity3(const Matrix& T); + + static Similarity3 Align(const gtsam::PointPairs & abPointPairs); +}; + + + // Forward declaration of PinholeCameraCalX is defined here. #include // Some typedefs for common camera types diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 1ca73bbe6..8c9345147 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -163,25 +163,6 @@ class BearingS2 { void serializable() const; // enabling serialization functionality }; -#include -class PointPairs -{ - PointPairs(); - size_t size() const; - bool empty() const; - gtsam::Point3Pair at(size_t n) const; - void push_back(const gtsam::Point3Pair& point_pair); -}; - -class Similarity3 { - Similarity3(); - Similarity3(double s); - Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s); - Similarity3(const Matrix& R, const Vector& t, double s); - Similarity3(const Matrix& T); - - static Similarity3 Align(const gtsam::PointPairs & abPointPairs); -}; #include class SimWall2D { From 98faf54f9b338506d98df225a183b707b40f68af Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 24 Feb 2021 22:12:00 -0500 Subject: [PATCH 05/30] move unit test out of gtsam unstable --- {gtsam_unstable => gtsam}/geometry/tests/testSimilarity3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename {gtsam_unstable => gtsam}/geometry/tests/testSimilarity3.cpp (99%) diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp similarity index 99% rename from gtsam_unstable/geometry/tests/testSimilarity3.cpp rename to gtsam/geometry/tests/testSimilarity3.cpp index 937761f02..abfab9451 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -16,7 +16,7 @@ * @author Zhaoyang Lv */ -#include +#include #include #include #include From 149b0adfb19915da484fa3e1b4a7c3bc48539ab9 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 25 Feb 2021 18:13:53 -0500 Subject: [PATCH 06/30] move typedef to header file --- gtsam/geometry/Similarity3.cpp | 1 - gtsam/geometry/Similarity3.h | 2 ++ 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index 5cb1466b4..b086a5b5c 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -24,7 +24,6 @@ namespace gtsam { using std::vector; -typedef vector PointPairs; namespace { /// Subtract centroids from point pairs. diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index b82862ddb..79180985d 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -30,6 +30,8 @@ namespace gtsam { // Forward declarations class Pose3; +typedef std::vector PointPairs; + /** * 3D similarity transform */ From 7d90e5040b9c6cb4d9a4557348012c17564559c3 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 25 Feb 2021 20:51:33 -0500 Subject: [PATCH 07/30] add Align() for pose3pairs --- gtsam/geometry/Pose3.h | 1 + gtsam/gtsam.i | 13 +++++++++++++ python/CMakeLists.txt | 1 + python/gtsam/specializations.h | 1 + 4 files changed, 16 insertions(+) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 4c8973996..318baab3d 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -389,6 +389,7 @@ inline Matrix wedge(const Vector& xi) { // Convenience typedef using Pose3Pair = std::pair; +using Pose3Pairs = std::vector >; // For MATLAB wrapper typedef std::vector Pose3Vector; diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index b6b3ecac4..9bdbe298d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -766,6 +766,16 @@ class Pose3 { void serialize() const; }; +#include +class Pose3Pairs +{ + Pose3Pairs(); + size_t size() const; + bool empty() const; + gtsam::Pose3Pair at(size_t n) const; + void push_back(const gtsam::Pose3Pair& pose_pair); +}; + // std::vector #include class Pose3Vector @@ -1077,6 +1087,9 @@ class Similarity3 { Similarity3(const Matrix& T); static Similarity3 Align(const gtsam::PointPairs & abPointPairs); + static Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs); + + }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 3d6bf1702..e87fb70e7 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -42,6 +42,7 @@ set(ignore gtsam::BetweenFactorPose3s gtsam::Point2Vector gtsam::PointPairs + gtsam::Pose3Pairs gtsam::Pose3Vector gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 04c1aa82c..2ed105ef2 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -7,6 +7,7 @@ py::bind_vector >(m_, "KeyVector"); #endif py::bind_vector > >(m_, "Point2Vector"); py::bind_vector >(m_, "PointPairs"); +py::bind_vector >(m_, "Pose3Pairs"); py::bind_vector >(m_, "Pose3Vector"); py::bind_vector > > >(m_, "BetweenFactorPose3s"); py::bind_vector > > >(m_, "BetweenFactorPose2s"); From 06e6aa918f1c499bff50127e22ef0183b655e14d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 26 Feb 2021 08:25:28 -0500 Subject: [PATCH 08/30] add standard interface for Sim3 in wrapper --- gtsam/gtsam.i | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 9bdbe298d..5ec31eeed 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1080,6 +1080,7 @@ class PointPairs }; class Similarity3 { + // Standard Constructors Similarity3(); Similarity3(double s); Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s); @@ -1089,7 +1090,11 @@ class Similarity3 { static Similarity3 Align(const gtsam::PointPairs & abPointPairs); static Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs); - + // Standard Interface + const Matrix matrix() const; + const gtsam::Rot3& rotation(); + const gtsam::Point3& translation(); + double scale() const; }; From 7caaf69ae58af3314c2ea649d8b5bd6470634d1d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 26 Feb 2021 10:44:54 -0500 Subject: [PATCH 09/30] add interface for transformFrom --- gtsam/gtsam.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 5ec31eeed..2d050f90a 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1087,6 +1087,7 @@ class Similarity3 { Similarity3(const Matrix& R, const Vector& t, double s); Similarity3(const Matrix& T); + gtsam::Pose3 transformFrom(const gtsam::Pose3& T); static Similarity3 Align(const gtsam::PointPairs & abPointPairs); static Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs); From 89dfdf082f58cbe0b2c45da42787d26bbc7fa7b0 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 3 Mar 2021 00:16:08 -0500 Subject: [PATCH 10/30] PointPairs to Point3Pairs, and move to Point3.h --- gtsam/geometry/Point3.h | 2 ++ gtsam/geometry/Similarity3.cpp | 16 ++++++++-------- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 001218ff7..3ff78442b 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -38,6 +38,8 @@ typedef Vector3 Point3; typedef std::pair Point3Pair; GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); +using Point3Pairs = std::vector; + /// distance between two points GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index b086a5b5c..ea6ed2563 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -27,9 +27,9 @@ using std::vector; namespace { /// Subtract centroids from point pairs. -static PointPairs subtractCentroids(const PointPairs &abPointPairs, +static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, const Point3Pair ¢roids) { - PointPairs d_abPointPairs; + Point3Pairs d_abPointPairs; for (const Point3Pair& abPair : abPointPairs) { Point3 da = abPair.first - centroids.first; Point3 db = abPair.second - centroids.second; @@ -39,7 +39,7 @@ static PointPairs subtractCentroids(const PointPairs &abPointPairs, } /// Form inner products x and y and calculate scale. -static const double calculateScale(const PointPairs &d_abPointPairs, +static const double calculateScale(const Point3Pairs &d_abPointPairs, const Rot3 &aRb) { double x = 0, y = 0; Point3 da, db; @@ -54,7 +54,7 @@ static const double calculateScale(const PointPairs &d_abPointPairs, } /// Form outer product H. -static Matrix3 calculateH(const PointPairs &d_abPointPairs) { +static Matrix3 calculateH(const Point3Pairs &d_abPointPairs) { Matrix3 H = Z_3x3; for (const Point3Pair& d_abPair : d_abPointPairs) { H += d_abPair.first * d_abPair.second.transpose(); @@ -63,7 +63,7 @@ static Matrix3 calculateH(const PointPairs &d_abPointPairs) { } /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. -static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb, +static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb, const Point3Pair ¢roids) { const double s = calculateScale(d_abPointPairs, aRb); const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s; @@ -72,7 +72,7 @@ static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb, /// This method estimates the similarity transform from point pairs, given a known or estimated rotation. // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 -static Similarity3 alignGivenR(const PointPairs &abPointPairs, +static Similarity3 alignGivenR(const Point3Pairs &abPointPairs, const Rot3 &aRb) { auto centroids = means(abPointPairs); auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); @@ -153,7 +153,7 @@ Point3 Similarity3::operator*(const Point3& p) const { return transformFrom(p); } -Similarity3 Similarity3::Align(const PointPairs &abPointPairs) { +Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) { // Refer to Chapter 3 of // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf if (abPointPairs.size() < 3) @@ -173,7 +173,7 @@ Similarity3 Similarity3::Align(const vector &abPosePairs) { // calculate rotation vector rotations; - PointPairs abPointPairs; + Point3Pairs abPointPairs; rotations.reserve(n); abPointPairs.reserve(n); Pose3 wTa, wTb; From b839444d17f967e02d0a72687fba21bac75a747c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 3 Mar 2021 00:16:31 -0500 Subject: [PATCH 11/30] move PointPairs to Point3.h --- gtsam/geometry/Similarity3.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index 79180985d..65efdd1ca 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -30,8 +30,6 @@ namespace gtsam { // Forward declarations class Pose3; -typedef std::vector PointPairs; - /** * 3D similarity transform */ @@ -128,6 +126,7 @@ public: /** * Create Similarity3 by aligning at least two pose pairs + * */ GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPosePairs); From 7604633c43b7bb14007afbac701e82c6da8cacf6 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 3 Mar 2021 00:21:54 -0500 Subject: [PATCH 12/30] update the docstring --- gtsam/geometry/Similarity3.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index 65efdd1ca..ccea0c33b 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -125,8 +125,10 @@ public: GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPointPairs); /** - * Create Similarity3 by aligning at least two pose pairs - * + * Create Similarity3 by aligning at least two pose pairs + * Given a list of pairs in world frame w1, and a list of pairs in another world + * frame w2, will compute the best-fit Similarity3 transformation to align them. + * `w2Sw1` will returned for pairs [ (w2Ti1,w1Ti1), (w2Ti2,w1Ti2), (w2Ti3,w1Ti3) ] */ GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPosePairs); From 104031dca32579a3de71767e468e5ef768c12024 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 3 Mar 2021 00:25:04 -0500 Subject: [PATCH 13/30] Rename PointPairs to Point3Pairs everywhere per popular demand --- gtsam/gtsam.i | 6 +++--- python/CMakeLists.txt | 2 +- python/gtsam/specializations.h | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 2d050f90a..5c7d7f946 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1070,9 +1070,9 @@ class PinholeCamera { #include -class PointPairs +class Point3Pairs { - PointPairs(); + Point3Pairs(); size_t size() const; bool empty() const; gtsam::Point3Pair at(size_t n) const; @@ -1088,7 +1088,7 @@ class Similarity3 { Similarity3(const Matrix& T); gtsam::Pose3 transformFrom(const gtsam::Pose3& T); - static Similarity3 Align(const gtsam::PointPairs & abPointPairs); + static Similarity3 Align(const gtsam::Point3Pairs & abPointPairs); static Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs); // Standard Interface diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e87fb70e7..8d1c07d7b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -41,7 +41,7 @@ set(ignore gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s gtsam::Point2Vector - gtsam::PointPairs + gtsam::Point3Pairs gtsam::Pose3Pairs gtsam::Pose3Vector gtsam::KeyVector diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 2ed105ef2..98143160e 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -6,7 +6,7 @@ py::bind_vector > >(m_, " py::bind_vector >(m_, "KeyVector"); #endif py::bind_vector > >(m_, "Point2Vector"); -py::bind_vector >(m_, "PointPairs"); +py::bind_vector >(m_, "Point3Pairs"); py::bind_vector >(m_, "Pose3Pairs"); py::bind_vector >(m_, "Pose3Vector"); py::bind_vector > > >(m_, "BetweenFactorPose3s"); From 0bb4d68487c80701a99e6f1e6bb56dbf412dc894 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 3 Mar 2021 00:44:07 -0500 Subject: [PATCH 14/30] add a unit test for line case --- python/gtsam/tests/test_Sim3.py | 52 +++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 python/gtsam/tests/test_Sim3.py diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py new file mode 100644 index 000000000..3f4329eb4 --- /dev/null +++ b/python/gtsam/tests/test_Sim3.py @@ -0,0 +1,52 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Sim3 unit tests. +Author: John Lambert +""" +# pylint: disable=no-name-in-module +import math +import unittest + +import numpy as np + +import gtsam +from gtsam import Point3, Pose3, Pose3Pairs, Rot3, Similarity3 +from gtsam.utils.test_case import GtsamTestCase + + +class TestSim3(GtsamTestCase): + """Test selected Sim3 methods.""" + + def test_align_poses_along_straight_line(self): + """Test Align Pose3Pairs method.""" + + Rx90 = Rot3.Rx(np.deg2rad(90)) + + w1Ti0 = Pose3(Rot3(), np.array([5, 0, 0])) + w1Ti1 = Pose3(Rot3(), np.array([10, 0, 0])) + w1Ti2 = Pose3(Rot3(), np.array([15, 0, 0])) + + w1Ti_list = [w1Ti0, w1Ti1, w1Ti2] + + w2Ti0 = Pose3(Rx90, np.array([-10, 0, 0])) + w2Ti1 = Pose3(Rx90, np.array([-5, 0, 0])) + w2Ti2 = Pose3(Rx90, np.array([0, 0, 0])) + + w2Ti_list = [w2Ti0, w2Ti1, w2Ti2] + + pairs = list(zip(w2Ti_list, w1Ti_list)) + pose_pairs = Pose3Pairs(pairs) + + w2Sw1 = Similarity3.Align(pose_pairs) + + for w1Ti, w2Ti in zip(w1Ti_list, w2Ti_list): + self.gtsamAssertEquals(w2Ti, w2Sw1.transformFrom(w1Ti)) + + +if __name__ == "__main__": + unittest.main() From f5504d064525c807adebc32b0a3dcd7ddc00e25b Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 3 Mar 2021 00:56:17 -0500 Subject: [PATCH 15/30] add another unit test, but this one fails --- python/gtsam/tests/test_Sim3.py | 45 ++++++++++++++++++++++++++++++++- 1 file changed, 44 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py index 3f4329eb4..81b190e5a 100644 --- a/python/gtsam/tests/test_Sim3.py +++ b/python/gtsam/tests/test_Sim3.py @@ -23,7 +23,14 @@ class TestSim3(GtsamTestCase): """Test selected Sim3 methods.""" def test_align_poses_along_straight_line(self): - """Test Align Pose3Pairs method.""" + """Test Align Pose3Pairs method. + + Scenario: + 3 camera frames + same scale (no gauge ambiguity) + world frame 2 (w2) has poses rotated about x-axis (90 degree roll) + world frames translated by 15 meters + """ Rx90 = Rot3.Rx(np.deg2rad(90)) @@ -48,5 +55,41 @@ class TestSim3(GtsamTestCase): self.gtsamAssertEquals(w2Ti, w2Sw1.transformFrom(w1Ti)) + + def test_align_poses_along_straight_line_gauge(self): + """Test Align Pose3Pairs method. + + Scenario: + 3 camera frames + with gauge ambiguity (2x scale) + world frame 2 (w2) has poses rotated about z-axis (90 degree yaw) + world frames translated by 10 meters + """ + + Rz90 = Rot3.Rz(np.deg2rad(90)) + + w1Ti0 = Pose3(Rot3(), np.array([1, 0, 0])) + w1Ti1 = Pose3(Rot3(), np.array([2, 0, 0])) + w1Ti2 = Pose3(Rot3(), np.array([4, 0, 0])) + + w1Ti_list = [w1Ti0, w1Ti1, w1Ti2] + + w2Ti0 = Pose3(Rz90, np.array([12, 0, 0])) + w2Ti1 = Pose3(Rz90, np.array([14, 0, 0])) + w2Ti2 = Pose3(Rz90, np.array([18, 0, 0])) + + w2Ti_list = [w2Ti0, w2Ti1, w2Ti2] + + pairs = list(zip(w2Ti_list, w1Ti_list)) + pose_pairs = Pose3Pairs(pairs) + + w2Sw1 = Similarity3.Align(pose_pairs) + + for w1Ti, w2Ti in zip(w1Ti_list, w2Ti_list): + self.gtsamAssertEquals(w2Ti, w2Sw1.transformFrom(w1Ti)) + + + + if __name__ == "__main__": unittest.main() From 30edfe9658902835111b82dbb992bdf60b73d7a7 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 3 Mar 2021 01:54:27 -0500 Subject: [PATCH 16/30] move Point3Pairs to Point3.h part of gtsam.i --- gtsam/gtsam.i | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 5c7d7f946..92a7b1834 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -448,6 +448,16 @@ class Point3 { void serialize() const; }; +#include +class Point3Pairs +{ + Point3Pairs(); + size_t size() const; + bool empty() const; + gtsam::Point3Pair at(size_t n) const; + void push_back(const gtsam::Point3Pair& point_pair); +}; + #include class Rot2 { // Standard Constructors and Named Constructors @@ -1070,15 +1080,6 @@ class PinholeCamera { #include -class Point3Pairs -{ - Point3Pairs(); - size_t size() const; - bool empty() const; - gtsam::Point3Pair at(size_t n) const; - void push_back(const gtsam::Point3Pair& point_pair); -}; - class Similarity3 { // Standard Constructors Similarity3(); From 943879c6cc7b61a85eb23769f62afa719ee9bb6c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 3 Mar 2021 01:56:13 -0500 Subject: [PATCH 17/30] fix notation --- gtsam/geometry/tests/testSimilarity3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index abfab9451..17637584a 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -346,7 +346,7 @@ TEST(Similarity3, AlignPoint3_3) { //****************************************************************************** // Align with Pose3 Pairs TEST(Similarity3, AlignPose3) { - Similarity3 expected_aSb(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + Similarity3 expected_bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); // Create source poses Pose3 Ta1(Rot3(), Point3(0, 0, 0)); @@ -364,8 +364,8 @@ TEST(Similarity3, AlignPose3) { // Cayley transform cannot accommodate 180 degree rotations, // hence we only test for Expmap #ifdef GTSAM_ROT3_EXPMAP - Similarity3 actual_aSb = Similarity3::Align(correspondences); - EXPECT(assert_equal(expected_aSb, actual_aSb)); + Similarity3 actual_bSa = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_bSa, actual_bSa)); #endif } From 3121604f6364a9181054c153a919c1eca89da707 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 3 Mar 2021 11:25:55 -0500 Subject: [PATCH 18/30] fix bugs in Karcher mean --- gtsam/geometry/Similarity3.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index ea6ed2563..c426fef09 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -179,12 +179,13 @@ Similarity3 Similarity3::Align(const vector &abPosePairs) { Pose3 wTa, wTb; for (const Pose3Pair &abPair : abPosePairs) { std::tie(wTa, wTb) = abPair; - rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); + Rot3 aRb = wTa.rotation().between(wTb.rotation()); + rotations.emplace_back(aRb); abPointPairs.emplace_back(wTa.translation(), wTb.translation()); } - const Rot3 aRb = FindKarcherMean(rotations); + const Rot3 aRb_estimate = FindKarcherMean(rotations); - return alignGivenR(abPointPairs, aRb); + return alignGivenR(abPointPairs, aRb_estimate); } Matrix4 Similarity3::wedge(const Vector7 &xi) { From 9f5acb18902788a5727ae0299034f12bb97f1dcf Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 3 Mar 2021 11:26:23 -0500 Subject: [PATCH 19/30] switch typedef to using per popular request --- gtsam/geometry/Point3.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 3ff78442b..835c53d7c 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -35,7 +35,7 @@ namespace gtsam { typedef Vector3 Point3; // Convenience typedef -typedef std::pair Point3Pair; +using Point3Pair = std::pair; GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); using Point3Pairs = std::vector; From 96cce19f79437b6d53f0ace8d507d46c1c795afc Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 3 Mar 2021 12:17:14 -0500 Subject: [PATCH 20/30] update author list --- gtsam/geometry/Similarity3.cpp | 1 + gtsam/geometry/Similarity3.h | 1 + 2 files changed, 2 insertions(+) diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index c426fef09..4e8d6e6b7 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -13,6 +13,7 @@ * @file Similarity3.cpp * @brief Implementation of Similarity3 transform * @author Paul Drews + * @author John Lambert */ #include diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index ccea0c33b..780301c1b 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -13,6 +13,7 @@ * @file Similarity3.h * @brief Implementation of Similarity3 transform * @author Paul Drews + * @author John Lambert */ #pragma once From 4d079e19054fc614b4428fb32f06d2bd607be42b Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 5 Mar 2021 14:17:00 -0500 Subject: [PATCH 21/30] fix notations --- gtsam/geometry/Similarity3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index 4e8d6e6b7..b69cfa991 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -177,10 +177,10 @@ Similarity3 Similarity3::Align(const vector &abPosePairs) { Point3Pairs abPointPairs; rotations.reserve(n); abPointPairs.reserve(n); - Pose3 wTa, wTb; + Pose3 aTi, bTi; for (const Pose3Pair &abPair : abPosePairs) { - std::tie(wTa, wTb) = abPair; - Rot3 aRb = wTa.rotation().between(wTb.rotation()); + std::tie(aTi, bTi) = abPair; + Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse()); rotations.emplace_back(aRb); abPointPairs.emplace_back(wTa.translation(), wTb.translation()); } From 063a41a3f8ac27b4f044a91a6c35ea2e5b886fc2 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 5 Mar 2021 14:17:29 -0500 Subject: [PATCH 22/30] clean up Sim(3) notations --- gtsam/geometry/tests/testSimilarity3.cpp | 48 +++++++++++++----------- 1 file changed, 27 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index 17637584a..cac4dafa7 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -260,18 +260,22 @@ TEST(Similarity3, GroupAction) { //****************************************************************************** // Group action on Pose3 TEST(Similarity3, GroupActionPose3) { - Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + // Suppose we know the pose of the egovehicle in the world frame + Similarity3 wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); - // Create source poses - Pose3 Ta1(Rot3(), Point3(0, 0, 0)); - Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); + // Create source poses (two objects o1 and o2 living in the egovehicle "e" frame) + // Suppose they are 3d cuboids detected by onboard sensor, in the egovehicle frame + Pose3 eTo1(Rot3(), Point3(0, 0, 0)); + Pose3 eTo2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); - // Create destination poses - Pose3 expected_Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); - Pose3 expected_Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); + // Create destination poses (two objects now living in the world/city "w" frame) + // Desired to place the objects into the world frame for tracking + Pose3 expected_wTo1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 expected_wTo2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); - EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1))); - EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2))); + // objects now live in the world frame, instead of in the egovehicle frame + EXPECT(assert_equal(expected_wTo1, wSe.transformFrom(eTo1))); + EXPECT(assert_equal(expected_wTo2, wSe.transformFrom(eTo2))); } // Test left group action compatibility. @@ -346,26 +350,28 @@ TEST(Similarity3, AlignPoint3_3) { //****************************************************************************** // Align with Pose3 Pairs TEST(Similarity3, AlignPose3) { - Similarity3 expected_bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + Similarity3 expected_wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); - // Create source poses - Pose3 Ta1(Rot3(), Point3(0, 0, 0)); - Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); + // Create source poses (two objects o1 and o2 living in the egovehicle "e" frame) + // Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + Pose3 eTo1(Rot3(), Point3(0, 0, 0)); + Pose3 eTo2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); - // Create destination poses - Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); - Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); + // Create destination poses (same two objects, but instead living in the world/city "w" frame) + Pose3 wTo1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 wTo2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); - Pose3Pair bTa1(make_pair(Tb1, Ta1)); - Pose3Pair bTa2(make_pair(Tb2, Ta2)); + Pose3Pair wTe1(make_pair(wTo1, eTo1)); + Pose3Pair wTe2(make_pair(wTo2, eTo2)); - vector correspondences{bTa1, bTa2}; + vector correspondences{wTe1, wTe2}; // Cayley transform cannot accommodate 180 degree rotations, // hence we only test for Expmap #ifdef GTSAM_ROT3_EXPMAP - Similarity3 actual_bSa = Similarity3::Align(correspondences); - EXPECT(assert_equal(expected_bSa, actual_bSa)); + // Recover the transformation wSe (i.e. world_S_egovehicle) + Similarity3 actual_wSe = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_wSe, actual_wSe)); #endif } From eaf457e625fcac32a9182d2f1f7dc33897c6e1d1 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 5 Mar 2021 14:26:37 -0500 Subject: [PATCH 23/30] update test notation to have just 1 world frame, and fix typo in abPointPairs --- gtsam/geometry/Similarity3.cpp | 2 +- python/gtsam/tests/test_Sim3.py | 79 +++++++++++++++++---------------- 2 files changed, 42 insertions(+), 39 deletions(-) diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index b69cfa991..10cfbfc2b 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -182,7 +182,7 @@ Similarity3 Similarity3::Align(const vector &abPosePairs) { std::tie(aTi, bTi) = abPair; Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse()); rotations.emplace_back(aRb); - abPointPairs.emplace_back(wTa.translation(), wTb.translation()); + abPointPairs.emplace_back(aTi.translation(), bTi.translation()); } const Rot3 aRb_estimate = FindKarcherMean(rotations); diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py index 81b190e5a..2b9ad4df8 100644 --- a/python/gtsam/tests/test_Sim3.py +++ b/python/gtsam/tests/test_Sim3.py @@ -26,69 +26,72 @@ class TestSim3(GtsamTestCase): """Test Align Pose3Pairs method. Scenario: - 3 camera frames + 3 object poses same scale (no gauge ambiguity) - world frame 2 (w2) has poses rotated about x-axis (90 degree roll) - world frames translated by 15 meters + world frame has poses rotated about x-axis (90 degree roll) + world and egovehicle frame translated by 15 meters w.r.t. each other """ - Rx90 = Rot3.Rx(np.deg2rad(90)) - w1Ti0 = Pose3(Rot3(), np.array([5, 0, 0])) - w1Ti1 = Pose3(Rot3(), np.array([10, 0, 0])) - w1Ti2 = Pose3(Rot3(), np.array([15, 0, 0])) + # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) + # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + eTo0 = Pose3(Rot3(), np.array([5, 0, 0])) + eTo1 = Pose3(Rot3(), np.array([10, 0, 0])) + eTo2 = Pose3(Rot3(), np.array([15, 0, 0])) - w1Ti_list = [w1Ti0, w1Ti1, w1Ti2] + eToi_list = [eTo0, eTo1, eTo2] - w2Ti0 = Pose3(Rx90, np.array([-10, 0, 0])) - w2Ti1 = Pose3(Rx90, np.array([-5, 0, 0])) - w2Ti2 = Pose3(Rx90, np.array([0, 0, 0])) + # Create destination poses + # (same three objects, but instead living in the world/city "w" frame) + wTo0 = Pose3(Rx90, np.array([-10, 0, 0])) + wTo1 = Pose3(Rx90, np.array([-5, 0, 0])) + wTo2 = Pose3(Rx90, np.array([0, 0, 0])) - w2Ti_list = [w2Ti0, w2Ti1, w2Ti2] + wToi_list = [wTo0, wTo1, wTo2] - pairs = list(zip(w2Ti_list, w1Ti_list)) - pose_pairs = Pose3Pairs(pairs) + wTe_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) - w2Sw1 = Similarity3.Align(pose_pairs) - - for w1Ti, w2Ti in zip(w1Ti_list, w2Ti_list): - self.gtsamAssertEquals(w2Ti, w2Sw1.transformFrom(w1Ti)) + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity3.Align(wTe_pairs) + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) def test_align_poses_along_straight_line_gauge(self): - """Test Align Pose3Pairs method. + """Test if Align Pose3Pairs method can account for gauge ambiguity. Scenario: - 3 camera frames + 3 object poses with gauge ambiguity (2x scale) - world frame 2 (w2) has poses rotated about z-axis (90 degree yaw) - world frames translated by 10 meters + world frame has poses rotated about z-axis (90 degree yaw) + world and egovehicle frame translated by 11 meters w.r.t. each other """ - Rz90 = Rot3.Rz(np.deg2rad(90)) - w1Ti0 = Pose3(Rot3(), np.array([1, 0, 0])) - w1Ti1 = Pose3(Rot3(), np.array([2, 0, 0])) - w1Ti2 = Pose3(Rot3(), np.array([4, 0, 0])) + # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) + # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + eTo0 = Pose3(Rot3(), np.array([1, 0, 0])) + eTo1 = Pose3(Rot3(), np.array([2, 0, 0])) + eTo2 = Pose3(Rot3(), np.array([4, 0, 0])) - w1Ti_list = [w1Ti0, w1Ti1, w1Ti2] + eToi_list = [eTo0, eTo1, eTo2] - w2Ti0 = Pose3(Rz90, np.array([12, 0, 0])) - w2Ti1 = Pose3(Rz90, np.array([14, 0, 0])) - w2Ti2 = Pose3(Rz90, np.array([18, 0, 0])) + # Create destination poses + # (same three objects, but instead living in the world/city "w" frame) + wTo0 = Pose3(Rz90, np.array([12, 0, 0])) + wTo1 = Pose3(Rz90, np.array([14, 0, 0])) + wTo2 = Pose3(Rz90, np.array([18, 0, 0])) - w2Ti_list = [w2Ti0, w2Ti1, w2Ti2] + wToi_list = [wTo0, wTo1, wTo2] - pairs = list(zip(w2Ti_list, w1Ti_list)) - pose_pairs = Pose3Pairs(pairs) - - w2Sw1 = Similarity3.Align(pose_pairs) - - for w1Ti, w2Ti in zip(w1Ti_list, w2Ti_list): - self.gtsamAssertEquals(w2Ti, w2Sw1.transformFrom(w1Ti)) + wTe_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity3.Align(wTe_pairs) + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) if __name__ == "__main__": From 5ab7af0a098c79a61bd7c674bcaea5d2ca04fd9e Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 5 Mar 2021 17:58:43 -0500 Subject: [PATCH 24/30] dont conflate notation on aTb --- python/gtsam/tests/test_Sim3.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py index 2b9ad4df8..c74a009ec 100644 --- a/python/gtsam/tests/test_Sim3.py +++ b/python/gtsam/tests/test_Sim3.py @@ -49,10 +49,10 @@ class TestSim3(GtsamTestCase): wToi_list = [wTo0, wTo1, wTo2] - wTe_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) + we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) # Recover the transformation wSe (i.e. world_S_egovehicle) - wSe = Similarity3.Align(wTe_pairs) + wSe = Similarity3.Align(we_pairs) for wToi, eToi in zip(wToi_list, eToi_list): self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) @@ -85,10 +85,10 @@ class TestSim3(GtsamTestCase): wToi_list = [wTo0, wTo1, wTo2] - wTe_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) + we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) # Recover the transformation wSe (i.e. world_S_egovehicle) - wSe = Similarity3.Align(wTe_pairs) + wSe = Similarity3.Align(we_pairs) for wToi, eToi in zip(wToi_list, eToi_list): self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) From 5da50a5a6f8e2f70761a2a0a08dbf3ff24067b10 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 10 Mar 2021 08:53:43 -0500 Subject: [PATCH 25/30] improve docstring --- gtsam/geometry/Similarity3.cpp | 5 +++- python/gtsam/tests/test_Sim3.py | 46 ++++++++++++++++++++++++++++++--- 2 files changed, 46 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index 10cfbfc2b..ded917933 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -63,10 +63,13 @@ static Matrix3 calculateH(const Point3Pairs &d_abPointPairs) { return H; } -/// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. +/// This method estimates the similarity transform from differences point pairs, +// given a known or estimated rotation and point centroids. static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb, const Point3Pair ¢roids) { const double s = calculateScale(d_abPointPairs, aRb); + // dividing aTb by s is required because the registration cost function + // minimizes ||a - sRb - t||, whereas Sim(3) computes s(Rb + t) const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s; return Similarity3(aRb, aTb, s); } diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py index c74a009ec..ff478306e 100644 --- a/python/gtsam/tests/test_Sim3.py +++ b/python/gtsam/tests/test_Sim3.py @@ -9,7 +9,6 @@ Sim3 unit tests. Author: John Lambert """ # pylint: disable=no-name-in-module -import math import unittest import numpy as np @@ -79,9 +78,9 @@ class TestSim3(GtsamTestCase): # Create destination poses # (same three objects, but instead living in the world/city "w" frame) - wTo0 = Pose3(Rz90, np.array([12, 0, 0])) - wTo1 = Pose3(Rz90, np.array([14, 0, 0])) - wTo2 = Pose3(Rz90, np.array([18, 0, 0])) + wTo0 = Pose3(Rz90, np.array([0, 12, 0])) + wTo1 = Pose3(Rz90, np.array([0, 14, 0])) + wTo2 = Pose3(Rz90, np.array([0, 18, 0])) wToi_list = [wTo0, wTo1, wTo2] @@ -94,5 +93,44 @@ class TestSim3(GtsamTestCase): self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) + def test_align_poses_scaled_squares(self): + """Test if Align Pose3Pairs method can account for gauge ambiguity. + + Make sure a big and small square can be aligned. + The u's represent a big square (10x10), and v's represents a small square (4x4). + + Scenario: + 4 object poses + with gauge ambiguity (2.5x scale) + """ + # 0, 90, 180, and 270 degrees yaw + R0 = Rot3.Rz(np.deg2rad(0)) + R90 = Rot3.Rz(np.deg2rad(90)) + R180 = Rot3.Rz(np.deg2rad(180)) + R270 = Rot3.Rz(np.deg2rad(270)) + + aTi0 = Pose3(R0, np.array([2, 3, 0])) + aTi1 = Pose3(R90, np.array([12, 3, 0])) + aTi2 = Pose3(R180, np.array([12, 13, 0])) + aTi3 = Pose3(R270, np.array([2, 13, 0])) + + aTi_list = [aTi0, aTi1, aTi2, aTi3] + + bTi0 = Pose3(R0, np.array([4, 3, 0])) + bTi1 = Pose3(R90, np.array([8, 3, 0])) + bTi2 = Pose3(R180, np.array([8, 7, 0])) + bTi3 = Pose3(R270, np.array([4, 7, 0])) + + bTi_list = [bTi0, bTi1, bTi2, bTi3] + + ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + aSb = Similarity3.Align(ab_pairs) + + for aTi, bTi in zip(aTi_list, bTi_list): + self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi)) + + if __name__ == "__main__": unittest.main() From 2c1593c020f316b07499a156efb03f973f3f316d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 10 Mar 2021 08:53:55 -0500 Subject: [PATCH 26/30] improve docstring --- gtsam/geometry/Similarity3.h | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index 780301c1b..d4a478310 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -126,10 +126,14 @@ public: GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPointPairs); /** - * Create Similarity3 by aligning at least two pose pairs - * Given a list of pairs in world frame w1, and a list of pairs in another world - * frame w2, will compute the best-fit Similarity3 transformation to align them. - * `w2Sw1` will returned for pairs [ (w2Ti1,w1Ti1), (w2Ti2,w1Ti2), (w2Ti3,w1Ti3) ] + * Create the Similarity3 object that aligns at least two pose pairs. + * Each pair is of the form (aTi, bTi). + * Given a list of pairs in frame a, and a list of pairs in frame b, Align() + * will compute the best-fit Similarity3 aSb transformation to align them. + * First, the rotation aRb will be computed as the average (Karcher mean) of + * many estimates aRb (from each pair). Afterwards, the scale factor will be computed + * using the algorithm described here: + * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf */ GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPosePairs); From bddd7e68eba3b85daf8f1ae8d4888acd83916c62 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 10 Mar 2021 09:45:19 -0500 Subject: [PATCH 27/30] add const on Rot3 --- gtsam/geometry/Similarity3.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index ded917933..c76b8eb32 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -180,10 +180,11 @@ Similarity3 Similarity3::Align(const vector &abPosePairs) { Point3Pairs abPointPairs; rotations.reserve(n); abPointPairs.reserve(n); + // note that frame "i" is the i'th object/camera/etc body frame Pose3 aTi, bTi; for (const Pose3Pair &abPair : abPosePairs) { std::tie(aTi, bTi) = abPair; - Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse()); + const Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse()); rotations.emplace_back(aRb); abPointPairs.emplace_back(aTi.translation(), bTi.translation()); } From 10b2465351f517c29cb27dba0240eef2b469a31e Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 10 Mar 2021 10:23:11 -0500 Subject: [PATCH 28/30] improve docstring --- gtsam/geometry/tests/testSimilarity3.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index cac4dafa7..8f466e21b 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -259,6 +259,10 @@ TEST(Similarity3, GroupAction) { //****************************************************************************** // Group action on Pose3 +// Estimate Sim(3) object "aSb" from pose pairs {(aTi, bTi)} +// In the example below, let the "a" frame be the "world" frame below, +// and let the "b" frame be the "egovehicle" frame. +// Suppose within the egovehicle frame, we know the poses of two objects "o1" and "o2" TEST(Similarity3, GroupActionPose3) { // Suppose we know the pose of the egovehicle in the world frame Similarity3 wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); From 4428148961a184279daa5e9e93adfbc34dca70c3 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 10 Mar 2021 10:23:27 -0500 Subject: [PATCH 29/30] reformat with black --- python/gtsam/tests/test_Sim3.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py index ff478306e..001321e2c 100644 --- a/python/gtsam/tests/test_Sim3.py +++ b/python/gtsam/tests/test_Sim3.py @@ -56,7 +56,6 @@ class TestSim3(GtsamTestCase): for wToi, eToi in zip(wToi_list, eToi_list): self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) - def test_align_poses_along_straight_line_gauge(self): """Test if Align Pose3Pairs method can account for gauge ambiguity. @@ -92,7 +91,6 @@ class TestSim3(GtsamTestCase): for wToi, eToi in zip(wToi_list, eToi_list): self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) - def test_align_poses_scaled_squares(self): """Test if Align Pose3Pairs method can account for gauge ambiguity. From a27679e803b687f75e04fb76884ea93d39776449 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 10 Mar 2021 12:34:40 -0500 Subject: [PATCH 30/30] use different brace indent format --- gtsam/geometry/Similarity3.cpp | 2 +- gtsam/gtsam.i | 6 ++---- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index c76b8eb32..fcaf0c874 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -180,7 +180,7 @@ Similarity3 Similarity3::Align(const vector &abPosePairs) { Point3Pairs abPointPairs; rotations.reserve(n); abPointPairs.reserve(n); - // note that frame "i" is the i'th object/camera/etc body frame + // Below denotes the pose of the i'th object/camera/etc in frame "a" or frame "b" Pose3 aTi, bTi; for (const Pose3Pair &abPair : abPosePairs) { std::tie(aTi, bTi) = abPair; diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 2ca57504c..007686360 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -470,8 +470,7 @@ class Point3 { }; #include -class Point3Pairs -{ +class Point3Pairs { Point3Pairs(); size_t size() const; bool empty() const; @@ -810,8 +809,7 @@ class Pose3 { }; #include -class Pose3Pairs -{ +class Pose3Pairs { Pose3Pairs(); size_t size() const; bool empty() const;