From df5ecac604b396139c72eafc0ad806532cdcde67 Mon Sep 17 00:00:00 2001 From: David Date: Tue, 16 Jun 2020 11:30:44 +1000 Subject: [PATCH 001/106] Add unit test for oriented plane 3 factor jacobian --- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 81f67f1ee..36ae57201 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -122,6 +122,42 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } +double randDouble(double max = 1) { + return static_cast(rand()) / RAND_MAX * max; +} + +TEST( OrientedPlane3Factor, Derivatives ) { + for (int i=0; i<100; i++) { + // Random measurement + OrientedPlane3 p(randDouble(), randDouble(), randDouble(), randDouble()); + + // Random linearisation point + OrientedPlane3 pLin(randDouble(), randDouble(), randDouble(), randDouble()); + gtsam::Point3 pointLin = gtsam::Point3(randDouble(100), randDouble(100), randDouble(100)); + gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(randDouble(2*M_PI), randDouble(2*M_PI), randDouble(2*M_PI)); + Pose3 poseLin(rotationLin, pointLin); + + // Factor + Key planeKey(1), poseKey(2); + SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey); + + // Calculate numerical derivatives + boost::function f = boost::bind( + &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none); + Matrix numericalH1 = numericalDerivative21(f, poseLin, pLin); + Matrix numericalH2 = numericalDerivative22(f, poseLin, pLin); + + // Use the factor to calculate the derivative + Matrix actualH1, actualH2; + factor.evaluateError(poseLin, pLin, actualH1, actualH2); + + // Verify we get the expected error + EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); + EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); + } +} + // ************************************************************************* TEST( OrientedPlane3DirectionPrior, Constructor ) { From 75eb859ee7dc2ea0ff44ef7b26431f115a0d2e7e Mon Sep 17 00:00:00 2001 From: David Date: Mon, 22 Jun 2020 14:39:56 +1000 Subject: [PATCH 002/106] Fix OrientedPlane3Factor jacobian using numericalDerivative --- gtsam/geometry/OrientedPlane3.h | 4 ++++ gtsam/slam/OrientedPlane3Factor.h | 24 ++++++++++++++++--- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 6 ++--- 3 files changed, 28 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 61d8a30d2..ad6053234 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -114,6 +114,10 @@ public: */ Vector3 error(const OrientedPlane3& plane) const; + static Vector3 Error(const OrientedPlane3& plane1, const OrientedPlane3& plane2) { + return plane1.error(plane2); + } + /** Computes the error between the two planes, with derivatives. * This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses * Unit3::localCoordinates. This one has correct derivatives. diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index 1f56adc45..d1f2329c4 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -7,6 +7,7 @@ #pragma once +#include #include #include @@ -48,10 +49,27 @@ public: virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, - H2); Vector err(3); - err << predicted_plane.error(measured_p_); + + if (H1 || H2) { + Matrix H1_1, H2_1; + OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1_1, H2_1); + err << predicted_plane.error(measured_p_); + // Numerically calculate the derivative since this function doesn't provide one. + auto f = boost::bind(&OrientedPlane3::Error, _1, _2); + Matrix H1_2 = numericalDerivative21(f, predicted_plane, measured_p_); + + // Apply the chain rule to calculate the derivatives. + if (H1) { + *H1 = H1_2 * H1_1; + } + if (H2) { + *H2 = H1_2 * H2_1; + } + } else { + OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2); + err << predicted_plane.error(measured_p_); + } return (err); } ; diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 36ae57201..65491648f 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -83,7 +83,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { // Tests one pose, two measurements of the landmark that differ in angle only. // Normal along -x, 3m away Symbol lm_sym('p', 0); - OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); + OrientedPlane3 test_lm0(-1.0/sqrt(1.01), 0.1/sqrt(1.01), 0.0, 3.0); ISAM2 isam2; Values new_values; @@ -153,8 +153,8 @@ TEST( OrientedPlane3Factor, Derivatives ) { factor.evaluateError(poseLin, pLin, actualH1, actualH2); // Verify we get the expected error - EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); - EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); + EXPECT(assert_equal(numericalH1, actualH1, 1e-7)); + EXPECT(assert_equal(numericalH2, actualH2, 1e-7)); } } From 58a0f82cba0acb9e458ed8d09d5403c8904d7108 Mon Sep 17 00:00:00 2001 From: David Date: Tue, 23 Jun 2020 19:02:51 +1000 Subject: [PATCH 003/106] Address Frank's comments and clean up changes --- gtsam/geometry/OrientedPlane3.cpp | 10 +++- gtsam/geometry/OrientedPlane3.h | 4 +- gtsam/slam/OrientedPlane3Factor.cpp | 27 ++++++++++ gtsam/slam/OrientedPlane3Factor.h | 27 +--------- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 50 ++++++++----------- 5 files changed, 62 insertions(+), 56 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 5cfa80779..76a569ffa 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -20,6 +20,7 @@ #include #include #include +#include using namespace std; @@ -58,7 +59,14 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> } /* ************************************************************************* */ -Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const { +Vector3 OrientedPlane3::error(const OrientedPlane3& plane, + OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { + // Numerically calculate the derivative since this function doesn't provide one. + auto f = boost::bind(&OrientedPlane3::Error, _1, _2); + if (H1) *H1 = numericalDerivative21(f, *this, plane); + if (H2) *H2 = numericalDerivative22(f, *this, plane); + Vector2 n_error = -n_.localCoordinates(plane.n_); return Vector3(n_error(0), n_error(1), d_ - plane.d_); } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index ad6053234..e56e650ab 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -112,7 +112,9 @@ public: * The error is a norm 1 difference in tangent space. * @param the other plane */ - Vector3 error(const OrientedPlane3& plane) const; + Vector3 error(const OrientedPlane3& plane, + OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H2 = boost::none) const; static Vector3 Error(const OrientedPlane3& plane1, const OrientedPlane3& plane2) { return plane1.error(plane2); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 56968301a..699148e92 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -19,6 +19,33 @@ void OrientedPlane3Factor::print(const string& s, this->noiseModel_->print(" noise model: "); } +//*************************************************************************** +Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, + const OrientedPlane3& plane, boost::optional H1, + boost::optional H2) const { + Vector err(3); + + if (H1 || H2) { + Matrix36 predicted_H_pose; + Matrix33 predicted_H_plane, error_H_predicted; + + OrientedPlane3 predicted_plane = plane.transform(pose, predicted_H_plane, predicted_H_pose); + err << predicted_plane.error(measured_p_, error_H_predicted); + + // Apply the chain rule to calculate the derivatives. + if (H1) { + *H1 = error_H_predicted * predicted_H_pose; + } + if (H2) { + *H2 = error_H_predicted * predicted_H_plane; + } + } else { + OrientedPlane3 predicted_plane = plane.transform(pose); + err << predicted_plane.error(measured_p_); + } + return (err); +} + //*************************************************************************** void OrientedPlane3DirectionPrior::print(const string& s, const KeyFormatter& keyFormatter) const { diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index d1f2329c4..282873b4e 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -7,7 +7,6 @@ #pragma once -#include #include #include @@ -48,31 +47,7 @@ public: /// evaluateError virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - Vector err(3); - - if (H1 || H2) { - Matrix H1_1, H2_1; - OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1_1, H2_1); - err << predicted_plane.error(measured_p_); - // Numerically calculate the derivative since this function doesn't provide one. - auto f = boost::bind(&OrientedPlane3::Error, _1, _2); - Matrix H1_2 = numericalDerivative21(f, predicted_plane, measured_p_); - - // Apply the chain rule to calculate the derivatives. - if (H1) { - *H1 = H1_2 * H1_1; - } - if (H2) { - *H2 = H1_2 * H2_1; - } - } else { - OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2); - err << predicted_plane.error(measured_p_); - } - return (err); - } - ; + boost::none) const; }; // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 65491648f..6744dcd50 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -122,40 +122,34 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } -double randDouble(double max = 1) { - return static_cast(rand()) / RAND_MAX * max; -} - TEST( OrientedPlane3Factor, Derivatives ) { - for (int i=0; i<100; i++) { - // Random measurement - OrientedPlane3 p(randDouble(), randDouble(), randDouble(), randDouble()); + // Measurement + OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5); - // Random linearisation point - OrientedPlane3 pLin(randDouble(), randDouble(), randDouble(), randDouble()); - gtsam::Point3 pointLin = gtsam::Point3(randDouble(100), randDouble(100), randDouble(100)); - gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(randDouble(2*M_PI), randDouble(2*M_PI), randDouble(2*M_PI)); - Pose3 poseLin(rotationLin, pointLin); + // Linearisation point + OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7); + gtsam::Point3 pointLin = gtsam::Point3(1, 2, -4); + gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI); + Pose3 poseLin(rotationLin, pointLin); - // Factor - Key planeKey(1), poseKey(2); - SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); - OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey); + // Factor + Key planeKey(1), poseKey(2); + SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey); - // Calculate numerical derivatives - boost::function f = boost::bind( - &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none); - Matrix numericalH1 = numericalDerivative21(f, poseLin, pLin); - Matrix numericalH2 = numericalDerivative22(f, poseLin, pLin); + // Calculate numerical derivatives + boost::function f = boost::bind( + &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none); + Matrix numericalH1 = numericalDerivative21(f, poseLin, pLin); + Matrix numericalH2 = numericalDerivative22(f, poseLin, pLin); - // Use the factor to calculate the derivative - Matrix actualH1, actualH2; - factor.evaluateError(poseLin, pLin, actualH1, actualH2); + // Use the factor to calculate the derivative + Matrix actualH1, actualH2; + factor.evaluateError(poseLin, pLin, actualH1, actualH2); - // Verify we get the expected error - EXPECT(assert_equal(numericalH1, actualH1, 1e-7)); - EXPECT(assert_equal(numericalH2, actualH2, 1e-7)); - } + // Verify we get the expected error + EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); + EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); } // ************************************************************************* From 795380d5d7c57f999a715f2c304e103d24dbfa3c Mon Sep 17 00:00:00 2001 From: David Date: Wed, 24 Jun 2020 09:32:24 +1000 Subject: [PATCH 004/106] Update style and switch to errorVector() --- gtsam/slam/OrientedPlane3Factor.cpp | 32 ++++++++++++----------------- 1 file changed, 13 insertions(+), 19 deletions(-) diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 699148e92..fc8bb3436 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -23,27 +23,21 @@ void OrientedPlane3Factor::print(const string& s, Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, const OrientedPlane3& plane, boost::optional H1, boost::optional H2) const { - Vector err(3); + Matrix36 predicted_H_pose; + Matrix33 predicted_H_plane, error_H_predicted; + OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr, + H1 ? &predicted_H_pose : nullptr); + Vector3 err = predicted_plane.errorVector(measured_p_, (H1 || H2) ? &error_H_predicted : nullptr); - if (H1 || H2) { - Matrix36 predicted_H_pose; - Matrix33 predicted_H_plane, error_H_predicted; - - OrientedPlane3 predicted_plane = plane.transform(pose, predicted_H_plane, predicted_H_pose); - err << predicted_plane.error(measured_p_, error_H_predicted); - - // Apply the chain rule to calculate the derivatives. - if (H1) { - *H1 = error_H_predicted * predicted_H_pose; - } - if (H2) { - *H2 = error_H_predicted * predicted_H_plane; - } - } else { - OrientedPlane3 predicted_plane = plane.transform(pose); - err << predicted_plane.error(measured_p_); + // Apply the chain rule to calculate the derivatives. + if (H1) { + *H1 = error_H_predicted * predicted_H_pose; } - return (err); + if (H2) { + *H2 = error_H_predicted * predicted_H_plane; + } + + return err; } //*************************************************************************** From a897ee786328154abb98950c21c1a24b2175456f Mon Sep 17 00:00:00 2001 From: David Date: Wed, 15 Jul 2020 19:18:39 +1000 Subject: [PATCH 005/106] Fix all unit tests. Only remaining item is the analytical Jacobian for Unit3::localCoordinates. --- gtsam/geometry/OrientedPlane3.cpp | 13 +++++++++--- gtsam/geometry/tests/testOrientedPlane3.cpp | 23 ++++++++++++++++++++- gtsam/geometry/tests/testUnit3.cpp | 1 + gtsam/slam/OrientedPlane3Factor.cpp | 2 +- 4 files changed, 34 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 76a569ffa..274d8220c 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -63,11 +63,18 @@ Vector3 OrientedPlane3::error(const OrientedPlane3& plane, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { // Numerically calculate the derivative since this function doesn't provide one. - auto f = boost::bind(&OrientedPlane3::Error, _1, _2); - if (H1) *H1 = numericalDerivative21(f, *this, plane); - if (H2) *H2 = numericalDerivative22(f, *this, plane); + const auto f = boost::bind(&Unit3::localCoordinates, _1, _2); Vector2 n_error = -n_.localCoordinates(plane.n_); + + if (H1) { + *H1 = I_3x3; + H1->block<2,2>(0,0) = -numericalDerivative21(f, n_, plane.n_);; + } + if (H2) { + *H2 = -I_3x3; + H2->block<2,2>(0,0) = -numericalDerivative22(f, n_, plane.n_);; + } return Vector3(n_error(0), n_error(1), d_ - plane.d_); } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index dc5851319..36e566dca 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -138,7 +138,7 @@ TEST(OrientedPlane3, localCoordinates_retract) { } //******************************************************************************* -TEST (OrientedPlane3, error2) { +TEST (OrientedPlane3, errorVector) { OrientedPlane3 plane1(-1, 0.1, 0.2, 5); OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4); @@ -161,6 +161,27 @@ TEST (OrientedPlane3, error2) { EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } +//******************************************************************************* +TEST (OrientedPlane3, error) { + // Hard-coded regression values, to ensure the result doesn't change. + OrientedPlane3 plane1(-1, 0.1, 0.2, 5); + OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4); + + // Test the jacobians of transform + Matrix33 actualH1, expectedH1, actualH2, expectedH2; + Vector3 actual = plane1.error(plane2, actualH1, actualH2); + + EXPECT(assert_equal((Vector) Z_3x1, plane1.error(plane1), 1e-8)); + EXPECT(assert_equal(Vector3(0.0678852, 0.0761865, -0.4), actual, 1e-5)); + + boost::function f = // + boost::bind(&OrientedPlane3::error, _1, _2, boost::none, boost::none); + expectedH1 = numericalDerivative21(f, plane1, plane2); + expectedH2 = numericalDerivative22(f, plane1, plane2); + EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); +} + //******************************************************************************* TEST (OrientedPlane3, jacobian_retract) { OrientedPlane3 plane(-1, 0.1, 0.2, 5); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index a25ab25c7..ada735ed4 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -492,6 +492,7 @@ TEST(actualH, Serialization) { EXPECT(serializationTestHelpers::equalsBinary(p)); } + /* ************************************************************************* */ int main() { srand(time(nullptr)); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index fc8bb3436..7fd225e60 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -27,7 +27,7 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, Matrix33 predicted_H_plane, error_H_predicted; OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr, H1 ? &predicted_H_pose : nullptr); - Vector3 err = predicted_plane.errorVector(measured_p_, (H1 || H2) ? &error_H_predicted : nullptr); + Vector3 err = predicted_plane.error(measured_p_, (H1 || H2) ? &error_H_predicted : nullptr); // Apply the chain rule to calculate the derivatives. if (H1) { From 17081627236884124f8b29060ef9b2ece1df86d2 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Mon, 12 Oct 2020 15:36:46 -0400 Subject: [PATCH 006/106] Fixed some warnings --- gtsam/geometry/OrientedPlane3.h | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 61d8a30d2..024fa2778 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -24,6 +24,9 @@ #include #include +#include +#include + namespace gtsam { /** @@ -58,9 +61,8 @@ public: } /// Construct from a vector of plane coefficients - OrientedPlane3(const Vector4& vec) : - n_(vec(0), vec(1), vec(2)), d_(vec(3)) { - } + explicit OrientedPlane3(const Vector4& vec) + : n_(vec(0), vec(1), vec(2)), d_(vec(3)) {} /// Construct from four numbers of plane coeffcients (a, b, c, d) OrientedPlane3(double a, double b, double c, double d) { From 5df3eebd2eae856ee4b0ce2c813188d360a66775 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Mon, 12 Oct 2020 15:37:02 -0400 Subject: [PATCH 007/106] Printing now transpose --- gtsam/geometry/OrientedPlane3.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 5cfa80779..ff48ae5dc 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -19,6 +19,7 @@ #include #include + #include using namespace std; @@ -28,7 +29,7 @@ namespace gtsam { /* ************************************************************************* */ void OrientedPlane3::print(const string& s) const { Vector4 coeffs = planeCoefficients(); - cout << s << " : " << coeffs << endl; + cout << s << " : " << coeffs.transpose() << endl; } /* ************************************************************************* */ @@ -42,15 +43,14 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> double pred_d = n_.unitVector().dot(xr.translation()) + d_; if (Hr) { - *Hr = Matrix::Zero(3,6); + Hr->setZero(); Hr->block<2, 3>(0, 0) = D_rotated_plane; Hr->block<1, 3>(2, 3) = unit_vec; } if (Hp) { - Vector2 hpp = n_.basis().transpose() * xr.translation(); - *Hp = Z_3x3; + Hp->setZero(); Hp->block<2, 2>(0, 0) = D_rotated_pose; - Hp->block<1, 2>(2, 0) = hpp; + Hp->block<1, 2>(2, 0) = n_.basis().transpose() * xr.translation(); (*Hp)(2, 2) = 1; } From 4adca52daa1885b5bde0b14013ca3416c63f805a Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Mon, 12 Oct 2020 15:37:24 -0400 Subject: [PATCH 008/106] Simplified evaluateError --- gtsam/slam/OrientedPlane3Factor.h | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index e83464b1e..7abf5547b 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -45,16 +45,13 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /// evaluateError - Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const override { - OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, - H2); - Vector err(3); - err << predicted_plane.error(measured_p_); - return (err); + Vector evaluateError( + const Pose3& pose, const OrientedPlane3& plane, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + auto predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2); + return predicted_plane.error(measured_p_); } - ; }; // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior From 6909c2b2acba53fc65217c8fe761ba694933a879 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Mon, 12 Oct 2020 15:37:39 -0400 Subject: [PATCH 009/106] Use keyFormatter --- gtsam/slam/OrientedPlane3Factor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 56968301a..cfefe1f22 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -14,7 +14,7 @@ namespace gtsam { //*************************************************************************** void OrientedPlane3Factor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << "OrientedPlane3Factor Factor on " << landmarkKey_ << "\n"; + cout << "OrientedPlane3Factor Factor on " << keyFormatter(landmarkKey_) << "\n"; measured_p_.print("Measured Plane"); this->noiseModel_->print(" noise model: "); } @@ -22,7 +22,7 @@ void OrientedPlane3Factor::print(const string& s, //*************************************************************************** void OrientedPlane3DirectionPrior::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << "Prior Factor on " << landmarkKey_ << "\n"; + cout << "Prior Factor on " << keyFormatter(landmarkKey_) << "\n"; measured_p_.print("Measured Plane"); this->noiseModel_->print(" noise model: "); } From adbd9a68acd7e3c77bf0332f80fc06e43b4fee89 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Mon, 12 Oct 2020 15:39:49 -0400 Subject: [PATCH 010/106] Added unit test for issue 561 --- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 81 ++++++++++++++++++- 1 file changed, 77 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 81f67f1ee..314b43ad9 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -10,20 +10,24 @@ * -------------------------------------------------------------------------- */ /* - * @file testOrientedPlane3.cpp + * @file testOrientedPlane3Factor.cpp * @date Dec 19, 2012 * @author Alex Trevor * @brief Tests the OrientedPlane3Factor class */ #include -#include -#include + #include +#include +#include +#include #include -#include + #include +#include +#include using namespace boost::assign; using namespace gtsam; @@ -170,6 +174,75 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { EXPECT(assert_equal(expectedH3, actualH3, 1e-8)); } +/* ************************************************************************* */ +// Test by Marco Camurri to debug issue #561 +TEST(OrientedPlane3Factor, Issue561) { + // Typedefs + using symbol_shorthand::P; //< Planes + using symbol_shorthand::X; //< Pose3 (x,y,z,r,p,y) + using Plane = OrientedPlane3; + + NonlinearFactorGraph graph; + + // Setup prior factors + Pose3 x0_prior( + Rot3(0.799903913, -0.564527097, 0.203624376, 0.552537226, 0.82520195, + 0.117236322, -0.234214312, 0.0187322547, 0.972004505), + Vector3{-91.7500013, -0.47569366, -2.2}); + auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); + graph.addPrior(X(0), x0_prior, x0_noise); + +// Plane p1_prior(0.211098835, 0.214292752, 0.95368543, 26.4269514); +// auto p1_noise = +// noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5}); +// graph.addPrior(P(1), p1_prior, p1_noise); + +// ADDING THIS PRIOR MAKES OPTIMIZATION FAIL +// Plane p2_prior(0.301901811, 0.151751467, 0.941183717, 33.4388229); +// auto p2_noise = +// noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5}); +// graph.addPrior(P(2), p2_prior, p2_noise); + + // First plane factor + Plane p1_meas = Plane(0.0638967294, 0.0755284627, 0.995094297, 2.55956073); + const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.0451801); + graph.emplace_shared(p1_meas.planeCoefficients(), + x0_p1_noise, X(0), P(1)); + + // Second plane factor + Plane p2_meas = Plane(0.104902077, -0.0275756528, 0.994100165, 1.32765088); + const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.0322889); + graph.emplace_shared(p2_meas.planeCoefficients(), + x0_p2_noise, X(0), P(2)); + + // Optimize + try { + // Initial values + Values initialEstimate; + Plane p1(0.211098835, 0.214292752, 0.95368543, 26.4269514); + Plane p2(0.301901811, 0.151751467, 0.941183717, 33.4388229); + Pose3 x0( + Rot3(0.799903913, -0.564527097, 0.203624376, 0.552537226, 0.82520195, + 0.117236322, -0.234214312, 0.0187322547, 0.972004505), + Vector3{-91.7500013, -0.47569366, -2.2}); + initialEstimate.insert(P(1), p1); + initialEstimate.insert(P(2), p2); + initialEstimate.insert(X(0), x0); + + GaussNewtonParams params; + GTSAM_PRINT(graph); + Ordering ordering = list_of(P(1))(P(2))(X(0)); // make sure P1 eliminated first + params.setOrdering(ordering); + params.setLinearSolverType("SEQUENTIAL_QR"); // abundance of caution + params.setVerbosity("TERMINATION"); // show info about stopping conditions + GaussNewtonOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); + } catch (IndeterminantLinearSystemException e) { + std::cerr << "CAPTURED THE EXCEPTION: " << e.nearbyVariable() << std::endl; + } +} + /* ************************************************************************* */ int main() { srand(time(nullptr)); From af4005a52cfc4d8a5548e4411f0790d707dc2b9c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 15 Nov 2020 18:24:55 -0500 Subject: [PATCH 011/106] pass in params to optimizer --- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 314b43ad9..fd240f4ba 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -235,7 +235,7 @@ TEST(OrientedPlane3Factor, Issue561) { params.setOrdering(ordering); params.setLinearSolverType("SEQUENTIAL_QR"); // abundance of caution params.setVerbosity("TERMINATION"); // show info about stopping conditions - GaussNewtonOptimizer optimizer(graph, initialEstimate); + GaussNewtonOptimizer optimizer(graph, initialEstimate, params); Values result = optimizer.optimize(); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); } catch (IndeterminantLinearSystemException e) { From 534fa6bb31e9a00f76c160d47adbe4553439685b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 6 Dec 2020 18:20:53 -0500 Subject: [PATCH 012/106] formatting and small fixes --- gtsam/geometry/OrientedPlane3.cpp | 4 ++-- gtsam/geometry/OrientedPlane3.h | 9 ++++----- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 274d8220c..6af79b5d1 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -82,8 +82,8 @@ Vector3 OrientedPlane3::error(const OrientedPlane3& plane, Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { Matrix22 H_n_error_this, H_n_error_other; - Vector2 n_error = n_.errorVector(other.normal(), H1 ? &H_n_error_this : 0, - H2 ? &H_n_error_other : 0); + Vector2 n_error = n_.errorVector(other.n_, H1 ? &H_n_error_this : 0, + H2 ? &H_n_error_other : 0); double d_error = d_ - other.d_; diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index e56e650ab..20fa7c112 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -53,8 +53,8 @@ public: } /// Construct from a Unit3 and a distance - OrientedPlane3(const Unit3& s, double d) : - n_(s), d_(d) { + OrientedPlane3(const Unit3& n, double d) : + n_(n), d_(d) { } /// Construct from a vector of plane coefficients @@ -64,8 +64,7 @@ public: /// Construct from four numbers of plane coeffcients (a, b, c, d) OrientedPlane3(double a, double b, double c, double d) { - Point3 p(a, b, c); - n_ = Unit3(p); + n_ = Unit3(a, b, c); d_ = d; } @@ -110,7 +109,7 @@ public: /** Computes the error between two planes. * The error is a norm 1 difference in tangent space. - * @param the other plane + * @param plane The other plane */ Vector3 error(const OrientedPlane3& plane, OptionalJacobian<3,3> H1 = boost::none, From 50768371dc7bb017befec88e38d21eb9d732be17 Mon Sep 17 00:00:00 2001 From: David Wisth Date: Tue, 19 Jan 2021 17:41:12 +0000 Subject: [PATCH 013/106] Add a simplifed version of the minimal failing example --- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 66 ++++++++++++++++++- 1 file changed, 65 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index fd240f4ba..49c1b9c07 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -238,8 +238,72 @@ TEST(OrientedPlane3Factor, Issue561) { GaussNewtonOptimizer optimizer(graph, initialEstimate, params); Values result = optimizer.optimize(); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); - } catch (IndeterminantLinearSystemException e) { + } catch (const IndeterminantLinearSystemException &e) { std::cerr << "CAPTURED THE EXCEPTION: " << e.nearbyVariable() << std::endl; + EXPECT(false); // fail if this happens + } +} + +/* ************************************************************************* */ +// Simplified version of the test by Marco Camurri to debug issue #561 +TEST(OrientedPlane3Factor, Issue561Simplified) { + // Typedefs + using symbol_shorthand::P; //< Planes + using symbol_shorthand::X; //< Pose3 (x,y,z,r,p,y) + using Plane = OrientedPlane3; + + NonlinearFactorGraph graph; + + // Setup prior factors + Pose3 x0_prior(Rot3::identity(), Vector3(99, 0, 0)); + auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); + graph.addPrior(X(0), x0_prior, x0_noise); + + // Two horizontal planes with different heights. + const Plane p1(0,0,1,1), p2(0,0,1,2); + + auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); + graph.addPrior(P(1), p1, p1_noise); + + // ADDING THIS PRIOR MAKES OPTIMIZATION FAIL + auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); + graph.addPrior(P(2), p2, p2_noise); + + // First plane factor + const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); + graph.emplace_shared(p1.planeCoefficients(), + x0_p1_noise, X(0), P(1)); + + // Second plane factor + const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05); + graph.emplace_shared(p2.planeCoefficients(), + x0_p2_noise, X(0), P(2)); + + // Initial values + // Just offset the initial pose by 1m. This is what we are trying to optimize. + Values initialEstimate; + Pose3 x0 = x0_prior.compose(Pose3(Rot3::identity(), Vector3(1,0,0))); + initialEstimate.insert(P(1), p1); + initialEstimate.insert(P(2), p2); + initialEstimate.insert(X(0), x0); + + // Optimize + try { + GaussNewtonParams params; + //GTSAM_PRINT(graph); + //Ordering ordering = list_of(P(1))(P(2))(X(0)); // make sure P1 eliminated first + //params.setOrdering(ordering); + // params.setLinearSolverType("SEQUENTIAL_QR"); // abundance of caution + params.setVerbosity("TERMINATION"); // show info about stopping conditions + GaussNewtonOptimizer optimizer(graph, initialEstimate, params); + Values result = optimizer.optimize(); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); + EXPECT(x0_prior.equals(result.at(X(0)))); + EXPECT(p1.equals(result.at(P(1)))); + EXPECT(p2.equals(result.at(P(2)))); + } catch (const IndeterminantLinearSystemException &e) { + std::cerr << "CAPTURED THE EXCEPTION: " << e.nearbyVariable() << std::endl; + EXPECT(false); // fail if this happens } } From 62119d8076b323580a7cb92a100d5c1c03d0dc24 Mon Sep 17 00:00:00 2001 From: David Wisth Date: Tue, 19 Jan 2021 20:51:31 +0000 Subject: [PATCH 014/106] Add hessian calculation --- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 40 ++++++++++++++++++- 1 file changed, 38 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 49c1b9c07..9f94ab3ac 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -239,7 +239,7 @@ TEST(OrientedPlane3Factor, Issue561) { Values result = optimizer.optimize(); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); } catch (const IndeterminantLinearSystemException &e) { - std::cerr << "CAPTURED THE EXCEPTION: " << e.nearbyVariable() << std::endl; + std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl; EXPECT(false); // fail if this happens } } @@ -287,6 +287,42 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { initialEstimate.insert(P(2), p2); initialEstimate.insert(X(0), x0); + // For testing only + HessianFactor::shared_ptr hessianFactor = graph.linearizeToHessianFactor(initialEstimate); + const auto hessian = hessianFactor->hessianBlockDiagonal(); + + Matrix hessianP1 = hessian.at(P(1)), + hessianP2 = hessian.at(P(2)), + hessianX0 = hessian.at(X(0)); + + Eigen::JacobiSVD svdP1(hessianP1, Eigen::ComputeThinU), + svdP2(hessianP2, Eigen::ComputeThinU), + svdX0(hessianX0, Eigen::ComputeThinU); + + double conditionNumberP1 = svdP1.singularValues()[0] / svdP1.singularValues()[2], + conditionNumberP2 = svdP2.singularValues()[0] / svdP2.singularValues()[2], + conditionNumberX0 = svdX0.singularValues()[0] / svdX0.singularValues()[5]; + + std::cout << "Hessian P1:\n" << hessianP1 << "\n" + << "Condition number:\n" << conditionNumberP1 << "\n" + << "Singular values:\n" << svdP1.singularValues().transpose() << "\n" + << "SVD U:\n" << svdP1.matrixU() << "\n" << std::endl; + + std::cout << "Hessian P2:\n" << hessianP2 << "\n" + << "Condition number:\n" << conditionNumberP2 << "\n" + << "Singular values:\n" << svdP2.singularValues().transpose() << "\n" + << "SVD U:\n" << svdP2.matrixU() << "\n" << std::endl; + + std::cout << "Hessian X0:\n" << hessianX0 << "\n" + << "Condition number:\n" << conditionNumberX0 << "\n" + << "Singular values:\n" << svdX0.singularValues().transpose() << "\n" + << "SVD U:\n" << svdX0.matrixU() << "\n" << std::endl; + + // std::cout << "Hessian P2:\n" << hessianP2 << std::endl; + // std::cout << "Hessian X0:\n" << hessianX0 << std::endl; + + // For testing only + // Optimize try { GaussNewtonParams params; @@ -302,7 +338,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { EXPECT(p1.equals(result.at(P(1)))); EXPECT(p2.equals(result.at(P(2)))); } catch (const IndeterminantLinearSystemException &e) { - std::cerr << "CAPTURED THE EXCEPTION: " << e.nearbyVariable() << std::endl; + std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl; EXPECT(false); // fail if this happens } } From 6b11c9fe83242b0bea484bef40cbb012f38f0704 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 21 Jan 2021 10:46:23 -0500 Subject: [PATCH 015/106] use transform rather than deprecated static function --- gtsam/slam/OrientedPlane3Factor.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index 7abf5547b..c42ed222a 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -47,9 +47,9 @@ public: /// evaluateError Vector evaluateError( const Pose3& pose, const OrientedPlane3& plane, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { - auto predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2); + boost::optional Hpose = boost::none, + boost::optional Hplane = boost::none) const override { + auto predicted_plane = plane.transform(pose, Hplane, Hpose); return predicted_plane.error(measured_p_); } }; From 68887f2da67fcc97beeb1ac36935e9de9c4e352c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 21 Jan 2021 14:52:58 -0500 Subject: [PATCH 016/106] Got rid of extra keys --- gtsam/slam/OrientedPlane3Factor.cpp | 2 +- gtsam/slam/OrientedPlane3Factor.h | 17 ++++++++++------- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 7c86e68cc..c1ad037e2 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -14,7 +14,7 @@ namespace gtsam { //*************************************************************************** void OrientedPlane3Factor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << "OrientedPlane3Factor Factor on " << keyFormatter(landmarkKey_) << "\n"; + cout << "OrientedPlane3Factor Factor on " << keyFormatter(key2()) << "\n"; measured_p_.print("Measured Plane"); this->noiseModel_->print(" noise model: "); } diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index a6ab5528b..1b1d1d4a0 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -18,8 +18,6 @@ namespace gtsam { class OrientedPlane3Factor: public NoiseModelFactor2 { protected: - Key poseKey_; - Key landmarkKey_; Vector measured_coeffs_; OrientedPlane3 measured_p_; @@ -32,11 +30,16 @@ public: } virtual ~OrientedPlane3Factor() {} - /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol - OrientedPlane3Factor(const Vector&z, const SharedGaussian& noiseModel, - const Key& pose, const Key& landmark) : - Base(noiseModel, pose, landmark), poseKey_(pose), landmarkKey_(landmark), measured_coeffs_( - z) { + /** Constructor with measured plane (a,b,c,d) coefficients + * @param z measured plane (a,b,c,d) coefficients as 4D vector + * @param noiseModel noiseModel Gaussian noise model + * @param poseKey Key or symbol for unknown pose + * @param landmarkKey Key or symbol for unknown planar landmark + * @return the transformed plane + */ + OrientedPlane3Factor(const Vector& z, const SharedGaussian& noiseModel, + Key poseKey, Key landmarkKey) + : Base(noiseModel, poseKey, landmarkKey), measured_coeffs_(z) { measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3)); } From 6c3973087230b0b28504b9cac5fb9d95b31206ae Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 21 Jan 2021 14:53:13 -0500 Subject: [PATCH 017/106] Got rid of static versions of methods --- gtsam/geometry/OrientedPlane3.cpp | 33 ++++++++++------- gtsam/geometry/OrientedPlane3.h | 27 +++----------- gtsam/geometry/tests/testOrientedPlane3.cpp | 39 ++++++--------------- 3 files changed, 35 insertions(+), 64 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index c682e4aa4..fa92905ff 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -17,11 +17,11 @@ * @brief A plane, represented by a normal direction and perpendicular distance */ +#include #include #include #include -#include using namespace std; @@ -34,8 +34,9 @@ void OrientedPlane3::print(const string& s) const { } /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hp, - OptionalJacobian<3, 6> Hr) const { +OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, + OptionalJacobian<3, 3> Hp, + OptionalJacobian<3, 6> Hr) const { Matrix23 D_rotated_plane; Matrix22 D_rotated_pose; Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose); @@ -60,38 +61,44 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> /* ************************************************************************* */ Vector3 OrientedPlane3::error(const OrientedPlane3& plane, - OptionalJacobian<3,3> H1, - OptionalJacobian<3,3> H2) const { - // Numerically calculate the derivative since this function doesn't provide one. + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + // Numerically calculate the derivative since this function doesn't provide + // one. const auto f = boost::bind(&Unit3::localCoordinates, _1, _2); Vector2 n_error = -n_.localCoordinates(plane.n_); if (H1) { *H1 = I_3x3; - H1->block<2,2>(0,0) = -numericalDerivative21(f, n_, plane.n_);; + H1->block<2, 2>(0, 0) = + -numericalDerivative21(f, n_, plane.n_); + ; } if (H2) { *H2 = -I_3x3; - H2->block<2,2>(0,0) = -numericalDerivative22(f, n_, plane.n_);; + H2->block<2, 2>(0, 0) = + -numericalDerivative22(f, n_, plane.n_); + ; } return Vector3(n_error(0), n_error(1), d_ - plane.d_); } /* ************************************************************************* */ -Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1, +Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { Matrix22 H_n_error_this, H_n_error_other; Vector2 n_error = n_.errorVector(other.n_, H1 ? &H_n_error_this : 0, - H2 ? &H_n_error_other : 0); + H2 ? &H_n_error_other : 0); double d_error = d_ - other.d_; if (H1) { - *H1 << H_n_error_this, Vector2::Zero(), 0, 0, 1; + *H1 << H_n_error_this, Z_2x1, 0, 0, 1; } if (H2) { - *H2 << H_n_error_other, Vector2::Zero(), 0, 0, -1; + *H2 << H_n_error_other, Z_2x1, 0, 0, -1; } return Vector3(n_error(0), n_error(1), d_error); @@ -103,7 +110,7 @@ OrientedPlane3 OrientedPlane3::retract(const Vector3& v, Matrix22 H_n; Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr)); if (H) { - *H << H_n, Vector2::Zero(), 0, 0, 1; + *H << H_n, Z_2x1, 0, 0, 1; } return OrientedPlane3(n_retract, d_ + v(2)); } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 446fc83c7..aaacca78a 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -91,35 +91,16 @@ public: * @return the transformed plane */ OrientedPlane3 transform(const Pose3& xr, - OptionalJacobian<3, 3> Hp = boost::none, - OptionalJacobian<3, 6> Hr = boost::none) const; - - /** - * @deprecated the static method has wrong Jacobian order, - * please use the member method transform() - * @param The raw plane - * @param xr a transformation in current coordiante - * @param Hr optional jacobian wrpt the pose transformation - * @param Hp optional Jacobian wrpt the destination plane - * @return the transformed plane - */ - static OrientedPlane3 Transform(const OrientedPlane3& plane, - const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, - OptionalJacobian<3, 3> Hp = boost::none) { - return plane.transform(xr, Hp, Hr); - } + OptionalJacobian<3, 3> Hp = boost::none, + OptionalJacobian<3, 6> Hr = boost::none) const; /** Computes the error between two planes. * The error is a norm 1 difference in tangent space. * @param plane The other plane */ Vector3 error(const OrientedPlane3& plane, - OptionalJacobian<3,3> H1 = boost::none, - OptionalJacobian<3,3> H2 = boost::none) const; - - static Vector3 Error(const OrientedPlane3& plane1, const OrientedPlane3& plane2) { - return plane1.error(plane2); - } + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; /** Computes the error between the two planes, with derivatives. * This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 36e566dca..e164382d7 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -50,44 +50,27 @@ TEST (OrientedPlane3, getMethods) { //******************************************************************************* -OrientedPlane3 Transform_(const OrientedPlane3& plane, const Pose3& xr) { - return OrientedPlane3::Transform(plane, xr); -} - OrientedPlane3 transform_(const OrientedPlane3& plane, const Pose3& xr) { return plane.transform(xr); } -TEST (OrientedPlane3, transform) { +TEST(OrientedPlane3, transform) { gtsam::Pose3 pose(gtsam::Rot3::Ypr(-M_PI / 4.0, 0.0, 0.0), - gtsam::Point3(2.0, 3.0, 4.0)); + gtsam::Point3(2.0, 3.0, 4.0)); OrientedPlane3 plane(-1, 0, 0, 5); OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); - OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose, - none, none); - OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none); - EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-5)); - EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-5)); + OrientedPlane3 transformedPlane = plane.transform(pose, none, none); + EXPECT(assert_equal(expectedPlane, transformedPlane, 1e-5)); // Test the jacobians of transform Matrix actualH1, expectedH1, actualH2, expectedH2; - { - // because the Transform class uses a wrong order of Jacobians in interface - OrientedPlane3::Transform(plane, pose, actualH1, none); - expectedH1 = numericalDerivative22(Transform_, plane, pose); - EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); - OrientedPlane3::Transform(plane, pose, none, actualH2); - expectedH2 = numericalDerivative21(Transform_, plane, pose); - EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); - } - { - plane.transform(pose, actualH1, none); - expectedH1 = numericalDerivative21(transform_, plane, pose); - EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); - plane.transform(pose, none, actualH2); - expectedH2 = numericalDerivative22(Transform_, plane, pose); - EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); - } + expectedH1 = numericalDerivative21(transform_, plane, pose); + plane.transform(pose, actualH1, none); + EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); + + expectedH2 = numericalDerivative22(transform_, plane, pose); + plane.transform(pose, none, actualH2); + EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } //******************************************************************************* From 1b36c7497e7cad3ad02d20d1ef24b8a43a09afa8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 21 Jan 2021 15:02:54 -0500 Subject: [PATCH 018/106] Cleaned up derivative code --- gtsam/geometry/OrientedPlane3.cpp | 23 ++++++++--------------- gtsam/geometry/OrientedPlane3.h | 6 +++--- 2 files changed, 11 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index fa92905ff..f3e3a039f 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -60,28 +60,21 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, } /* ************************************************************************* */ -Vector3 OrientedPlane3::error(const OrientedPlane3& plane, +Vector3 OrientedPlane3::error(const OrientedPlane3& other, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - // Numerically calculate the derivative since this function doesn't provide - // one. + Vector2 n_error = -n_.localCoordinates(other.n_); + const auto f = boost::bind(&Unit3::localCoordinates, _1, _2); - - Vector2 n_error = -n_.localCoordinates(plane.n_); - if (H1) { - *H1 = I_3x3; - H1->block<2, 2>(0, 0) = - -numericalDerivative21(f, n_, plane.n_); - ; + Matrix2 H_n_error_this = -numericalDerivative21(f, n_, other.n_); + *H1 << H_n_error_this, Z_2x1, 0, 0, 1; } if (H2) { - *H2 = -I_3x3; - H2->block<2, 2>(0, 0) = - -numericalDerivative22(f, n_, plane.n_); - ; + Matrix H_n_error_other = -numericalDerivative22(f, n_, other.n_); + *H2 << H_n_error_other, Z_2x1, 0, 0, -1; } - return Vector3(n_error(0), n_error(1), d_ - plane.d_); + return Vector3(n_error(0), n_error(1), d_ - other.d_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index aaacca78a..2550761b1 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -96,9 +96,9 @@ public: /** Computes the error between two planes. * The error is a norm 1 difference in tangent space. - * @param plane The other plane + * @param other The other plane */ - Vector3 error(const OrientedPlane3& plane, + Vector3 error(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; @@ -106,7 +106,7 @@ public: * This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses * Unit3::localCoordinates. This one has correct derivatives. * NOTE(hayk): The derivatives are zero when normals are exactly orthogonal. - * @param the other plane + * @param other the other plane */ Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const; From 79d42479d0316b4d2f90525d21e1958a6b38207b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 21 Jan 2021 15:21:29 -0500 Subject: [PATCH 019/106] Cleanup --- gtsam/slam/OrientedPlane3Factor.cpp | 31 ++++++++++------------------- gtsam/slam/OrientedPlane3Factor.h | 31 ++++++++++------------------- 2 files changed, 22 insertions(+), 40 deletions(-) diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index c1ad037e2..918831a30 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -27,7 +27,7 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, Matrix33 predicted_H_plane, error_H_predicted; OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr, H1 ? &predicted_H_pose : nullptr); - Vector3 err = predicted_plane.error( + Vector3 err = predicted_plane.errorVector( measured_p_, (H1 || H2) ? &error_H_predicted : nullptr); // Apply the chain rule to calculate the derivatives. @@ -44,7 +44,7 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, //*************************************************************************** void OrientedPlane3DirectionPrior::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << "Prior Factor on " << keyFormatter(landmarkKey_) << "\n"; + cout << "Prior Factor on " << keyFormatter(key()) << "\n"; measured_p_.print("Measured Plane"); this->noiseModel_->print(" noise model: "); } @@ -58,26 +58,17 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, } //*************************************************************************** - -Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, - boost::optional H) const { - +Vector OrientedPlane3DirectionPrior::evaluateError( + const OrientedPlane3& plane, boost::optional H) const { + Unit3 n_hat_p = measured_p_.normal(); + Unit3 n_hat_q = plane.normal(); + Matrix2 H_p; + Vector e = n_hat_p.error(n_hat_q, H ? &H_p : nullptr); if (H) { - Matrix H_p; - Unit3 n_hat_p = measured_p_.normal(); - Unit3 n_hat_q = plane.normal(); - Vector e = n_hat_p.error(n_hat_q, H_p); H->resize(2, 3); - H->block<2, 2>(0, 0) << H_p; - H->block<2, 1>(0, 2) << Z_2x1; - return e; - } else { - Unit3 n_hat_p = measured_p_.normal(); - Unit3 n_hat_q = plane.normal(); - Vector e = n_hat_p.error(n_hat_q); - return e; + *H << H_p, Z_2x1; } - -} + return e; } +} // namespace gtsam diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index 1b1d1d4a0..b7152ab73 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -16,15 +16,11 @@ namespace gtsam { * Factor to measure a planar landmark from a given pose */ class OrientedPlane3Factor: public NoiseModelFactor2 { - -protected: - Vector measured_coeffs_; + protected: OrientedPlane3 measured_p_; - typedef NoiseModelFactor2 Base; -public: - + public: /// Constructor OrientedPlane3Factor() { } @@ -37,11 +33,9 @@ public: * @param landmarkKey Key or symbol for unknown planar landmark * @return the transformed plane */ - OrientedPlane3Factor(const Vector& z, const SharedGaussian& noiseModel, + OrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel, Key poseKey, Key landmarkKey) - : Base(noiseModel, poseKey, landmarkKey), measured_coeffs_(z) { - measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3)); - } + : Base(noiseModel, poseKey, landmarkKey), measured_p_(z) {} /// print void print(const std::string& s = "OrientedPlane3Factor", @@ -55,24 +49,21 @@ public: }; // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior -class OrientedPlane3DirectionPrior: public NoiseModelFactor1 { -protected: - OrientedPlane3 measured_p_; /// measured plane parameters - Key landmarkKey_; +class OrientedPlane3DirectionPrior : public NoiseModelFactor1 { + protected: + OrientedPlane3 measured_p_; /// measured plane parameters typedef NoiseModelFactor1 Base; -public: + public: typedef OrientedPlane3DirectionPrior This; /// Constructor OrientedPlane3DirectionPrior() { } /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol - OrientedPlane3DirectionPrior(Key key, const Vector&z, - const SharedGaussian& noiseModel) : - Base(noiseModel, key), landmarkKey_(key) { - measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3)); - } + OrientedPlane3DirectionPrior(Key key, const Vector4& z, + const SharedGaussian& noiseModel) + : Base(noiseModel, key), measured_p_(z) {} /// print void print(const std::string& s = "OrientedPlane3DirectionPrior", From 46464aa357b15ed4151159e137e0d378b0896ce7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 28 Jan 2021 23:22:50 -0500 Subject: [PATCH 020/106] cleanup --- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 203 ++++++------------ 1 file changed, 68 insertions(+), 135 deletions(-) diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index b0592709f..0a68dcbad 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -36,89 +36,86 @@ using namespace std; GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) +using symbol_shorthand::P; //< Planes +using symbol_shorthand::X; //< Pose3 + // ************************************************************************* -TEST (OrientedPlane3Factor, lm_translation_error) { +TEST(OrientedPlane3Factor, lm_translation_error) { // Tests one pose, two measurements of the landmark that differ in range only. // Normal along -x, 3m away - Symbol lm_sym('p', 0); OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); - ISAM2 isam2; - Values new_values; - NonlinearFactorGraph new_graph; + NonlinearFactorGraph graph; - // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose - Symbol init_sym('x', 0); + // Init pose and prior. Pose Prior is needed since a single plane measurement + // does not fully constrain the pose Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); - Vector sigmas(6); + Vector6 sigmas; sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; - new_graph.addPrior( - init_sym, init_pose, noiseModel::Diagonal::Sigmas(sigmas)); - new_values.insert(init_sym, init_pose); + graph.addPrior(X(0), init_pose, noiseModel::Diagonal::Sigmas(sigmas)); // Add two landmark measurements, differing in range - new_values.insert(lm_sym, test_lm0); - Vector sigmas3(3); - sigmas3 << 0.1, 0.1, 0.1; - Vector test_meas0_mean(4); - test_meas0_mean << -1.0, 0.0, 0.0, 3.0; - OrientedPlane3Factor test_meas0(test_meas0_mean, - noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); - new_graph.add(test_meas0); - Vector test_meas1_mean(4); - test_meas1_mean << -1.0, 0.0, 0.0, 1.0; - OrientedPlane3Factor test_meas1(test_meas1_mean, - noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); - new_graph.add(test_meas1); + Vector3 sigmas3 {0.1, 0.1, 0.1}; + Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; + OrientedPlane3Factor factor0( + measurement0, noiseModel::Diagonal::Sigmas(sigmas3), X(0), P(0)); + graph.add(factor0); + Vector4 measurement1 {-1.0, 0.0, 0.0, 1.0}; + OrientedPlane3Factor factor1( + measurement1, noiseModel::Diagonal::Sigmas(sigmas3), X(0), P(0)); + graph.add(factor1); + + // Initial Estimate + Values values; + values.insert(X(0), init_pose); + values.insert(P(0), test_lm0); // Optimize - ISAM2Result result = isam2.update(new_graph, new_values); + ISAM2 isam2; + ISAM2Result result = isam2.update(graph, values); Values result_values = isam2.calculateEstimate(); - OrientedPlane3 optimized_plane_landmark = result_values.at( - lm_sym); + OrientedPlane3 optimized_plane_landmark = + result_values.at(P(0)); // Given two noisy measurements of equal weight, expect result between the two OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0); EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } -// ************************************************************************* +// // ************************************************************************* TEST (OrientedPlane3Factor, lm_rotation_error) { // Tests one pose, two measurements of the landmark that differ in angle only. // Normal along -x, 3m away - Symbol lm_sym('p', 0); OrientedPlane3 test_lm0(-1.0/sqrt(1.01), 0.1/sqrt(1.01), 0.0, 3.0); - ISAM2 isam2; - Values new_values; - NonlinearFactorGraph new_graph; + NonlinearFactorGraph graph; // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose - Symbol init_sym('x', 0); Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); - new_graph.addPrior(init_sym, init_pose, + graph.addPrior(X(0), init_pose, noiseModel::Diagonal::Sigmas( (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); - new_values.insert(init_sym, init_pose); -// // Add two landmark measurements, differing in angle - new_values.insert(lm_sym, test_lm0); - Vector test_meas0_mean(4); - test_meas0_mean << -1.0, 0.0, 0.0, 3.0; - OrientedPlane3Factor test_meas0(test_meas0_mean, - noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym); - new_graph.add(test_meas0); - Vector test_meas1_mean(4); - test_meas1_mean << 0.0, -1.0, 0.0, 3.0; - OrientedPlane3Factor test_meas1(test_meas1_mean, - noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym); - new_graph.add(test_meas1); + // Add two landmark measurements, differing in angle + Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; + OrientedPlane3Factor factor0(measurement0, + noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), X(0), P(0)); + graph.add(factor0); + Vector4 measurement1 {0.0, -1.0, 0.0, 3.0}; + OrientedPlane3Factor factor1(measurement1, + noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), X(0), P(0)); + graph.add(factor1); + + // Initial Estimate + Values values; + values.insert(X(0), init_pose); + values.insert(P(0), test_lm0); // Optimize - ISAM2Result result = isam2.update(new_graph, new_values); + ISAM2 isam2; + ISAM2Result result = isam2.update(graph, values); Values result_values = isam2.calculateEstimate(); - OrientedPlane3 optimized_plane_landmark = result_values.at( - lm_sym); + auto optimized_plane_landmark = result_values.at(P(0)); // Given two noisy measurements of equal weight, expect result between the two OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, @@ -126,6 +123,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } +// ************************************************************************* TEST( OrientedPlane3Factor, Derivatives ) { // Measurement OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5); @@ -163,7 +161,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { // If pitch and roll are zero for an aerospace frame, // that means Z is pointing down, i.e., direction of Z = (0,0,-1) - Vector planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin + Vector4 planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin // Factor Key key(1); @@ -171,9 +169,9 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { OrientedPlane3DirectionPrior factor(key, planeOrientation, model); // Create a linearization point at the zero-error point - Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0); - Vector theta2 = Vector4(0.0, 0.1, -0.8, 10.0); - Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0); + Vector4 theta1 {0.0, 0.02, -1.2, 10.0}; + Vector4 theta2 {0.0, 0.1, -0.8, 10.0}; + Vector4 theta3 {0.0, 0.2, -0.9, 10.0}; OrientedPlane3 T1(theta1); OrientedPlane3 T2(theta2); @@ -204,90 +202,18 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { EXPECT(assert_equal(expectedH3, actualH3, 1e-8)); } -/* ************************************************************************* */ -// Test by Marco Camurri to debug issue #561 -TEST(OrientedPlane3Factor, Issue561) { - // Typedefs - using symbol_shorthand::P; //< Planes - using symbol_shorthand::X; //< Pose3 (x,y,z,r,p,y) - using Plane = OrientedPlane3; - - NonlinearFactorGraph graph; - - // Setup prior factors - Pose3 x0_prior( - Rot3(0.799903913, -0.564527097, 0.203624376, 0.552537226, 0.82520195, - 0.117236322, -0.234214312, 0.0187322547, 0.972004505), - Vector3{-91.7500013, -0.47569366, -2.2}); - auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); - graph.addPrior(X(0), x0_prior, x0_noise); - -// Plane p1_prior(0.211098835, 0.214292752, 0.95368543, 26.4269514); -// auto p1_noise = -// noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5}); -// graph.addPrior(P(1), p1_prior, p1_noise); - -// ADDING THIS PRIOR MAKES OPTIMIZATION FAIL -// Plane p2_prior(0.301901811, 0.151751467, 0.941183717, 33.4388229); -// auto p2_noise = -// noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5}); -// graph.addPrior(P(2), p2_prior, p2_noise); - - // First plane factor - Plane p1_meas = Plane(0.0638967294, 0.0755284627, 0.995094297, 2.55956073); - const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.0451801); - graph.emplace_shared(p1_meas.planeCoefficients(), - x0_p1_noise, X(0), P(1)); - - // Second plane factor - Plane p2_meas = Plane(0.104902077, -0.0275756528, 0.994100165, 1.32765088); - const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.0322889); - graph.emplace_shared(p2_meas.planeCoefficients(), - x0_p2_noise, X(0), P(2)); - - // Optimize - try { - // Initial values - Values initialEstimate; - Plane p1(0.211098835, 0.214292752, 0.95368543, 26.4269514); - Plane p2(0.301901811, 0.151751467, 0.941183717, 33.4388229); - Pose3 x0( - Rot3(0.799903913, -0.564527097, 0.203624376, 0.552537226, 0.82520195, - 0.117236322, -0.234214312, 0.0187322547, 0.972004505), - Vector3{-91.7500013, -0.47569366, -2.2}); - initialEstimate.insert(P(1), p1); - initialEstimate.insert(P(2), p2); - initialEstimate.insert(X(0), x0); - - GaussNewtonParams params; - GTSAM_PRINT(graph); - Ordering ordering = list_of(P(1))(P(2))(X(0)); // make sure P1 eliminated first - params.setOrdering(ordering); - params.setLinearSolverType("SEQUENTIAL_QR"); // abundance of caution - params.setVerbosity("TERMINATION"); // show info about stopping conditions - GaussNewtonOptimizer optimizer(graph, initialEstimate, params); - Values result = optimizer.optimize(); - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); - } catch (const IndeterminantLinearSystemException &e) { - std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl; - EXPECT(false); // fail if this happens - } -} - /* ************************************************************************* */ // Simplified version of the test by Marco Camurri to debug issue #561 TEST(OrientedPlane3Factor, Issue561Simplified) { // Typedefs - using symbol_shorthand::P; //< Planes - using symbol_shorthand::X; //< Pose3 (x,y,z,r,p,y) using Plane = OrientedPlane3; NonlinearFactorGraph graph; // Setup prior factors - Pose3 x0_prior(Rot3::identity(), Vector3(99, 0, 0)); + Pose3 x0(Rot3::identity(), Vector3(0, 0, 10)); auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); - graph.addPrior(X(0), x0_prior, x0_noise); + graph.addPrior(X(0), x0, x0_noise); // Two horizontal planes with different heights. const Plane p1(0,0,1,1), p2(0,0,1,2); @@ -300,22 +226,29 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { graph.addPrior(P(2), p2, p2_noise); // First plane factor + auto p1_measured_from_x0 = p1.transform(x0); const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); - graph.emplace_shared(p1.planeCoefficients(), - x0_p1_noise, X(0), P(1)); + graph.emplace_shared( + p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1)); // Second plane factor + auto p2_measured_from_x0 = p2.transform(x0); const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05); - graph.emplace_shared(p2.planeCoefficients(), - x0_p2_noise, X(0), P(2)); + graph.emplace_shared( + p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), P(2)); + + GTSAM_PRINT(graph); // Initial values // Just offset the initial pose by 1m. This is what we are trying to optimize. Values initialEstimate; - Pose3 x0 = x0_prior.compose(Pose3(Rot3::identity(), Vector3(1,0,0))); + Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0))); initialEstimate.insert(P(1), p1); initialEstimate.insert(P(2), p2); - initialEstimate.insert(X(0), x0); + initialEstimate.insert(X(0), x0_initial); + + // Print Jacobian + cout << graph.linearize(initialEstimate)->augmentedJacobian() << endl << endl; // For testing only HessianFactor::shared_ptr hessianFactor = graph.linearizeToHessianFactor(initialEstimate); @@ -364,7 +297,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { GaussNewtonOptimizer optimizer(graph, initialEstimate, params); Values result = optimizer.optimize(); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); - EXPECT(x0_prior.equals(result.at(X(0)))); + EXPECT(x0.equals(result.at(X(0)))); EXPECT(p1.equals(result.at(P(1)))); EXPECT(p2.equals(result.at(P(2)))); } catch (const IndeterminantLinearSystemException &e) { From 9eb626775a6b79057f010d5ff4b15ec58753b1a1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 3 Feb 2021 13:40:43 -0500 Subject: [PATCH 021/106] Comments --- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 0a68dcbad..3736743ae 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -215,7 +215,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); graph.addPrior(X(0), x0, x0_noise); - // Two horizontal planes with different heights. + // Two horizontal planes with different heights, in the world frame. const Plane p1(0,0,1,1), p2(0,0,1,2); auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); @@ -226,13 +226,13 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { graph.addPrior(P(2), p2, p2_noise); // First plane factor - auto p1_measured_from_x0 = p1.transform(x0); + auto p1_measured_from_x0 = p1.transform(x0); // transform p1 to pose x0 as a measurement const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); graph.emplace_shared( p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1)); // Second plane factor - auto p2_measured_from_x0 = p2.transform(x0); + auto p2_measured_from_x0 = p2.transform(x0); // transform p2 to pose x0 as a measurement const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05); graph.emplace_shared( p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), P(2)); From ffc4e59bfce83d0377a47c7bf58d5a25907c37b9 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Fri, 5 Feb 2021 02:24:40 +0100 Subject: [PATCH 022/106] Avoid target collision if gtsam used as submodule --- cmake/HandleUninstall.cmake | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/cmake/HandleUninstall.cmake b/cmake/HandleUninstall.cmake index 1859b0273..dccb1905e 100644 --- a/cmake/HandleUninstall.cmake +++ b/cmake/HandleUninstall.cmake @@ -6,5 +6,11 @@ configure_file( "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" IMMEDIATE @ONLY) -add_custom_target(uninstall - "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake") +if (NOT TARGET uninstall) # avoid duplicating this target + add_custom_target(uninstall + "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake") +else() + add_custom_target(uninstall_gtsam + "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake") + add_dependencies(uninstall uninstall_gtsam) +endif() From a62bdd45e8959a9b9c147c94a54d957beeb0cc2a Mon Sep 17 00:00:00 2001 From: David Wisth Date: Mon, 15 Feb 2021 13:15:11 +0000 Subject: [PATCH 023/106] Add new oriented plane 3 factors with local linearisation point --- .../slam/LocalOrientedPlane3Factor.cpp | 82 +++++ .../slam/LocalOrientedPlane3Factor.h | 69 +++++ .../tests/testLocalOrientedPlane3Factor.cpp | 289 ++++++++++++++++++ 3 files changed, 440 insertions(+) create mode 100644 gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp create mode 100644 gtsam_unstable/slam/LocalOrientedPlane3Factor.h create mode 100644 gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp new file mode 100644 index 000000000..87d94d957 --- /dev/null +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp @@ -0,0 +1,82 @@ +/* + * LocalOrientedPlane3Factor.cpp + * + * Author: David Wisth + * Created on: February 12, 2021 + */ + +#include "LocalOrientedPlane3Factor.h" + +using namespace std; + +namespace gtsam { + +//*************************************************************************** +void LocalOrientedPlane3Factor::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << (s == "" ? "" : "\n"); + cout << "LocalOrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", " + << keyFormatter(key2()) << ", " << keyFormatter(key3()) << ")\n"; + measured_p_.print("Measured Plane"); + this->noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& basePose, + const Pose3& anchorPose, const OrientedPlane3& plane, + boost::optional H1, boost::optional H2, + boost::optional H3) const { + + Matrix66 pose_H_anchorPose, pose_H_basePose; + Matrix36 predicted_H_pose; + Matrix33 predicted_H_plane, error_H_predicted; + + // T_LB = inv(T_WL) * T_WB + const Pose3 relativePose = anchorPose.transformPoseTo(basePose, + H2 ? &pose_H_anchorPose : nullptr, + H1 ? &pose_H_basePose : nullptr); + + const OrientedPlane3 predicted_plane = plane.transform(relativePose, + H2 ? &predicted_H_plane : nullptr, + (H1 || H3) ? &predicted_H_pose : nullptr); + + const Vector3 err = measured_p_.error(predicted_plane, + boost::none, (H1 || H2 || H3) ? &error_H_predicted : nullptr); + + // const Vector3 err = predicted_plane.errorVector(measured_p_, + // (H1 || H2 || H3) ? &error_H_predicted : nullptr); + + // Apply the chain rule to calculate the derivatives. + if (H1) { + *H1 = error_H_predicted * predicted_H_pose * pose_H_basePose; + // std::cout << "H1:\n" << *H1 << std::endl; + } + + if (H2) { + *H2 = error_H_predicted * predicted_H_pose * pose_H_anchorPose; + // std::cout << "H2:\n" << *H2 << std::endl; + } + + if (H3) { + *H3 = error_H_predicted * predicted_H_plane; + // std::cout << "H3:\n" << *H3 << std::endl; + + // measured_p_.print(); + // predicted_plane.print(); + + // std::cout << "pose_H_anchorPose:\n" << pose_H_anchorPose << std::endl; + // std::cout << "pose_H_basePose:\n" << pose_H_basePose << std::endl; + // std::cout << "predicted_H_pose:\n" << predicted_H_pose << std::endl; + // std::cout << "error_H_predicted:\n" << error_H_predicted << std::endl; + // std::cout << "predicted_H_plane:\n" << predicted_H_plane << std::endl; + + std::cout << "H3^T x error:\n" << (*H3).transpose() * err << std::endl; + // std::cout << "H3:\n" << *H3 << std::endl; + } + + // std::cout << "Error: " << err.transpose() << std::endl; + + return err; +} + +} // namespace gtsam diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h new file mode 100644 index 000000000..7bec7e12e --- /dev/null +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h @@ -0,0 +1,69 @@ +/* + * @file LocalOrientedPlane3Factor.h + * @brief LocalOrientedPlane3 Factor class + * @author David Wisth + * @date February 12, 2021 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Factor to measure a planar landmark from a given pose, with a given local + * linearization point. + * + * This factor is based on the relative plane factor formulation proposed in: + * Equation 25, + * M. Kaess, "Simultaneous Localization and Mapping with Infinite Planes", + * IEEE International Conference on Robotics and Automation, 2015. + * + * Note: This uses the retraction from the OrientedPlane3, not the quaternion- + * based representation proposed by Kaess. + */ +class LocalOrientedPlane3Factor: public NoiseModelFactor3 { +protected: + OrientedPlane3 measured_p_; + typedef NoiseModelFactor3 Base; +public: + /// Constructor + LocalOrientedPlane3Factor() {} + + virtual ~LocalOrientedPlane3Factor() {} + + /** Constructor with measured plane (a,b,c,d) coefficients + * @param z measured plane (a,b,c,d) coefficients as 4D vector + * @param noiseModel noiseModel Gaussian noise model + * @param poseKey Key or symbol for unknown pose + * @param anchorPoseKey Key or symbol for the plane's linearization point. + * @param landmarkKey Key or symbol for unknown planar landmark + * + * Note: The anchorPoseKey can simply be chosen as the first pose a plane + * is observed. + */ + LocalOrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel, + Key poseKey, Key anchorPoseKey, Key landmarkKey) + : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {} + + LocalOrientedPlane3Factor(const OrientedPlane3& z, const SharedGaussian& noiseModel, + Key poseKey, Key anchorPoseKey, Key landmarkKey) + : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {} + + /// print + void print(const std::string& s = "LocalOrientedPlane3Factor", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + + /// evaluateError + Vector evaluateError( + const Pose3& basePose, const Pose3& anchorPose, const OrientedPlane3& plane, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const override; +}; + +} // gtsam + diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp new file mode 100644 index 000000000..e3eec80c7 --- /dev/null +++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp @@ -0,0 +1,289 @@ +/* ---------------------------------------------------------------------------- + + * 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 testOrientedPlane3Factor.cpp + * @date Dec 19, 2012 + * @author Alex Trevor + * @brief Tests the OrientedPlane3Factor class + */ + +#include + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +using namespace boost::assign; +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) +GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) + +using symbol_shorthand::P; //< Planes +using symbol_shorthand::X; //< Pose3 + +// ************************************************************************* +TEST(LocalOrientedPlane3Factor, lm_translation_error) { + // Tests one pose, two measurements of the landmark that differ in range only. + // Normal along -x, 3m away + OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); + + NonlinearFactorGraph graph; + + // Init pose and prior. Pose Prior is needed since a single plane measurement + // does not fully constrain the pose + Pose3 init_pose = Pose3::identity(); + graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); + + // Add two landmark measurements, differing in range + Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; + Vector4 measurement1 {-1.0, 0.0, 0.0, 1.0}; + LocalOrientedPlane3Factor factor0( + measurement0, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); + LocalOrientedPlane3Factor factor1( + measurement1, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); + graph.add(factor0); + graph.add(factor1); + + // Initial Estimate + Values values; + values.insert(X(0), init_pose); + values.insert(P(0), test_lm0); + + // Optimize + ISAM2 isam2; + isam2.update(graph, values); + Values result_values = isam2.calculateEstimate(); + auto optimized_plane_landmark = result_values.at(P(0)); + + // Given two noisy measurements of equal weight, expect result between the two + OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0); + EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); +} + +// ************************************************************************* +TEST (LocalOrientedPlane3Factor, lm_rotation_error) { + // Tests one pose, two measurements of the landmark that differ in angle only. + // Normal along -x, 3m away + OrientedPlane3 test_lm0(-1.0/sqrt(1.01), -0.1/sqrt(1.01), 0.0, 3.0); + + NonlinearFactorGraph graph; + + // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose + Pose3 init_pose = Pose3::identity(); + graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); + + // Add two landmark measurements, differing in angle + Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; + Vector4 measurement1 {0.0, -1.0, 0.0, 3.0}; + LocalOrientedPlane3Factor factor0(measurement0, + noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); + LocalOrientedPlane3Factor factor1(measurement1, + noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); + graph.add(factor0); + graph.add(factor1); + + // Initial Estimate + Values values; + values.insert(X(0), init_pose); + values.insert(P(0), test_lm0); + + // Optimize + ISAM2 isam2; + isam2.update(graph, values); + Values result_values = isam2.calculateEstimate(); + auto optimized_plane_landmark = result_values.at(P(0)); + + values.print(); + result_values.print(); + + // HessianFactor::shared_ptr hessianFactor = graph.linearizeToHessianFactor(values); + // const auto hessian = hessianFactor->hessianBlockDiagonal(); + + // Matrix hessianP0 = hessian.at(P(0)), hessianX0 = hessian.at(X(0)); + + // Eigen::JacobiSVD svdP0(hessianP0, Eigen::ComputeThinU), + // svdX0(hessianX0, Eigen::ComputeThinU); + + // double conditionNumberP0 = svdP0.singularValues()[0] / svdP0.singularValues()[2], + // conditionNumberX0 = svdX0.singularValues()[0] / svdX0.singularValues()[5]; + + // std::cout << "Hessian P0:\n" << hessianP0 << "\n" + // << "Condition number:\n" << conditionNumberP0 << "\n" + // << "Singular values:\n" << svdP0.singularValues().transpose() << "\n" + // << "SVD U:\n" << svdP0.matrixU() << "\n" << std::endl; + + // std::cout << "Hessian X0:\n" << hessianX0 << "\n" + // << "Condition number:\n" << conditionNumberX0 << "\n" + // << "Singular values:\n" << svdX0.singularValues().transpose() << "\n" + // << "SVD U:\n" << svdX0.matrixU() << "\n" << std::endl; + + // Given two noisy measurements of equal weight, expect result between the two + OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, + 0.0, 3.0); + EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); +} + +// ************************************************************************* +TEST(LocalOrientedPlane3Factor, Derivatives) { + // Measurement + OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5); + + // Linearisation point + OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7); + Pose3 poseLin(Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI), Point3(1, 2, -4)); + Pose3 anchorPoseLin(Rot3::RzRyRx(-0.1*M_PI, 0.2*M_PI, 1.0*M_PI), Point3(-5, 0, 1)); + + // Factor + Key planeKey(1), poseKey(2), anchorPoseKey(3); + SharedGaussian noise = noiseModel::Isotropic::Sigma(3, 0.1); + LocalOrientedPlane3Factor factor(p, noise, poseKey, anchorPoseKey, planeKey); + + // Calculate numerical derivatives + auto f = boost::bind(&LocalOrientedPlane3Factor::evaluateError, factor, + _1, _2, _3, boost::none, boost::none, boost::none); + Matrix numericalH1 = numericalDerivative31(f, poseLin, anchorPoseLin, pLin); + Matrix numericalH2 = numericalDerivative32(f, poseLin, anchorPoseLin, pLin); + Matrix numericalH3 = numericalDerivative33(f, poseLin, anchorPoseLin, pLin); + + // Use the factor to calculate the derivative + Matrix actualH1, actualH2, actualH3; + factor.evaluateError(poseLin, anchorPoseLin, pLin, actualH1, actualH2, actualH3); + + // Verify we get the expected error + EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); + EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); + EXPECT(assert_equal(numericalH3, actualH3, 1e-8)); +} + + +// /* ************************************************************************* */ +// // Simplified version of the test by Marco Camurri to debug issue #561 +// TEST(OrientedPlane3Factor, Issue561Simplified) { +// // Typedefs +// using Plane = OrientedPlane3; + +// NonlinearFactorGraph graph; + +// // Setup prior factors +// Pose3 x0(Rot3::identity(), Vector3(0, 0, 10)); +// auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); +// graph.addPrior(X(0), x0, x0_noise); + +// // Two horizontal planes with different heights, in the world frame. +// const Plane p1(0,0,1,1), p2(0,0,1,2); + +// auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); +// graph.addPrior(P(1), p1, p1_noise); + +// // ADDING THIS PRIOR MAKES OPTIMIZATION FAIL +// auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); +// graph.addPrior(P(2), p2, p2_noise); + +// // First plane factor +// auto p1_measured_from_x0 = p1.transform(x0); // transform p1 to pose x0 as a measurement +// const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); +// graph.emplace_shared( +// p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1)); + +// // Second plane factor +// auto p2_measured_from_x0 = p2.transform(x0); // transform p2 to pose x0 as a measurement +// const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05); +// graph.emplace_shared( +// p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), P(2)); + +// GTSAM_PRINT(graph); + +// // Initial values +// // Just offset the initial pose by 1m. This is what we are trying to optimize. +// Values initialEstimate; +// Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0))); +// initialEstimate.insert(P(1), p1); +// initialEstimate.insert(P(2), p2); +// initialEstimate.insert(X(0), x0_initial); + +// // Print Jacobian +// cout << graph.linearize(initialEstimate)->augmentedJacobian() << endl << endl; + +// // For testing only +// HessianFactor::shared_ptr hessianFactor = graph.linearizeToHessianFactor(initialEstimate); +// const auto hessian = hessianFactor->hessianBlockDiagonal(); + +// Matrix hessianP1 = hessian.at(P(1)), +// hessianP2 = hessian.at(P(2)), +// hessianX0 = hessian.at(X(0)); + +// Eigen::JacobiSVD svdP1(hessianP1, Eigen::ComputeThinU), +// svdP2(hessianP2, Eigen::ComputeThinU), +// svdX0(hessianX0, Eigen::ComputeThinU); + +// double conditionNumberP1 = svdP1.singularValues()[0] / svdP1.singularValues()[2], +// conditionNumberP2 = svdP2.singularValues()[0] / svdP2.singularValues()[2], +// conditionNumberX0 = svdX0.singularValues()[0] / svdX0.singularValues()[5]; + +// std::cout << "Hessian P1:\n" << hessianP1 << "\n" +// << "Condition number:\n" << conditionNumberP1 << "\n" +// << "Singular values:\n" << svdP1.singularValues().transpose() << "\n" +// << "SVD U:\n" << svdP1.matrixU() << "\n" << std::endl; + +// std::cout << "Hessian P2:\n" << hessianP2 << "\n" +// << "Condition number:\n" << conditionNumberP2 << "\n" +// << "Singular values:\n" << svdP2.singularValues().transpose() << "\n" +// << "SVD U:\n" << svdP2.matrixU() << "\n" << std::endl; + +// std::cout << "Hessian X0:\n" << hessianX0 << "\n" +// << "Condition number:\n" << conditionNumberX0 << "\n" +// << "Singular values:\n" << svdX0.singularValues().transpose() << "\n" +// << "SVD U:\n" << svdX0.matrixU() << "\n" << std::endl; + +// // std::cout << "Hessian P2:\n" << hessianP2 << std::endl; +// // std::cout << "Hessian X0:\n" << hessianX0 << std::endl; + +// // For testing only + +// // Optimize +// try { +// GaussNewtonParams params; +// //GTSAM_PRINT(graph); +// //Ordering ordering = list_of(P(1))(P(2))(X(0)); // make sure P1 eliminated first +// //params.setOrdering(ordering); +// // params.setLinearSolverType("SEQUENTIAL_QR"); // abundance of caution +// params.setVerbosity("TERMINATION"); // show info about stopping conditions +// GaussNewtonOptimizer optimizer(graph, initialEstimate, params); +// Values result = optimizer.optimize(); +// EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); +// EXPECT(x0.equals(result.at(X(0)))); +// EXPECT(p1.equals(result.at(P(1)))); +// EXPECT(p2.equals(result.at(P(2)))); +// } catch (const IndeterminantLinearSystemException &e) { +// std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl; +// EXPECT(false); // fail if this happens +// } +// } + +/* ************************************************************************* */ +int main() { + srand(time(nullptr)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 0ad488c56790de1d531b5393d8ea2c5e858994f3 Mon Sep 17 00:00:00 2001 From: David Wisth Date: Mon, 15 Feb 2021 13:15:38 +0000 Subject: [PATCH 024/106] Update print method of OrientedPlane3Factor --- gtsam/slam/OrientedPlane3Factor.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 918831a30..185fae73f 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -14,7 +14,9 @@ namespace gtsam { //*************************************************************************** void OrientedPlane3Factor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << "OrientedPlane3Factor Factor on " << keyFormatter(key2()) << "\n"; + cout << s << (s == "" ? "" : "\n"); + cout << "OrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", " + << keyFormatter(key2()) << ")\n"; measured_p_.print("Measured Plane"); this->noiseModel_->print(" noise model: "); } @@ -25,8 +27,9 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, boost::optional H2) const { Matrix36 predicted_H_pose; Matrix33 predicted_H_plane, error_H_predicted; - OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr, - H1 ? &predicted_H_pose : nullptr); + OrientedPlane3 predicted_plane = plane.transform(pose, + H2 ? &predicted_H_plane : nullptr, H1 ? &predicted_H_pose : nullptr); + Vector3 err = predicted_plane.errorVector( measured_p_, (H1 || H2) ? &error_H_predicted : nullptr); @@ -44,7 +47,8 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, //*************************************************************************** void OrientedPlane3DirectionPrior::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << "Prior Factor on " << keyFormatter(key()) << "\n"; + cout << s << (s == "" ? "" : "\n"); + cout << s << "Prior Factor on " << keyFormatter(key()) << "\n"; measured_p_.print("Measured Plane"); this->noiseModel_->print(" noise model: "); } From 5087d36ab1987ccf661d2f21e0aa3f257a6899fb Mon Sep 17 00:00:00 2001 From: David Wisth Date: Mon, 15 Feb 2021 14:43:28 +0000 Subject: [PATCH 025/106] remove deprecated Unit3::error() which is replaced by Unit3::errorVector() --- gtsam/geometry/Unit3.cpp | 12 +----------- gtsam/geometry/Unit3.h | 4 ---- gtsam/geometry/tests/testUnit3.cpp | 28 ++-------------------------- gtsam/navigation/AttitudeFactor.cpp | 4 ++-- gtsam/slam/OrientedPlane3Factor.cpp | 2 +- gtsam/slam/RotateFactor.h | 2 +- 6 files changed, 7 insertions(+), 45 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 6f70d840e..1756b055d 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -187,16 +187,6 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1, 2> H_p, return d; } -/* ************************************************************************* */ -Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2, 2> H_q) const { - // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 - const Vector2 xi = basis().transpose() * q.p_; - if (H_q) { - *H_q = basis().transpose() * q.basis(); - } - return xi; -} - /* ************************************************************************* */ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const { @@ -236,7 +226,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, /* ************************************************************************* */ double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const { Matrix2 H_xi_q; - const Vector2 xi = error(q, H ? &H_xi_q : nullptr); + const Vector2 xi = errorVector(q, boost::none, H ? &H_xi_q : nullptr); const double theta = xi.norm(); if (H) *H = (xi.transpose() / theta) * H_xi_q; diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 27d41a014..158e3e9dd 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -152,10 +152,6 @@ public: GTSAM_EXPORT double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, // OptionalJacobian<1,2> H2 = boost::none) const; - /// Signed, vector-valued error between two directions - /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second. - GTSAM_EXPORT Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const; - /// Signed, vector-valued error between two directions /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal. GTSAM_EXPORT Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, // diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 9929df21a..35206b6ab 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -145,30 +145,6 @@ TEST(Unit3, dot) { } } -//******************************************************************************* -TEST(Unit3, error) { - Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // - r = p.retract(Vector2(0.8, 0)); - EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-5)); - EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5)); - EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); - - Matrix actual, expected; - // Use numerical derivatives to calculate the expected Jacobian - { - expected = numericalDerivative11( - boost::bind(&Unit3::error, &p, _1, boost::none), q); - p.error(q, actual); - EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); - } - { - expected = numericalDerivative11( - boost::bind(&Unit3::error, &p, _1, boost::none), r); - p.error(r, actual); - EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); - } -} - //******************************************************************************* TEST(Unit3, error2) { Unit3 p(0.1, -0.2, 0.8); @@ -487,10 +463,10 @@ TEST(Unit3, ErrorBetweenFactor) { TEST(Unit3, CopyAssign) { Unit3 p{1, 0.2, 0.3}; - EXPECT(p.error(p).isZero()); + EXPECT(p.errorVector(p).isZero()); p = Unit3{-1, 2, 8}; - EXPECT(p.error(p).isZero()); + EXPECT(p.errorVector(p).isZero()); } /* ************************************************************************* */ diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 7f335152e..512a257e7 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -29,13 +29,13 @@ Vector AttitudeFactor::attitudeError(const Rot3& nRb, Matrix23 D_nRef_R; Matrix22 D_e_nRef; Unit3 nRef = nRb.rotate(bRef_, D_nRef_R); - Vector e = nZ_.error(nRef, D_e_nRef); + Vector e = nZ_.errorVector(nRef, boost::none, D_e_nRef); (*H) = D_e_nRef * D_nRef_R; return e; } else { Unit3 nRef = nRb * bRef_; - return nZ_.error(nRef); + return nZ_.errorVector(nRef); } } diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 185fae73f..00f6b7b62 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -67,7 +67,7 @@ Vector OrientedPlane3DirectionPrior::evaluateError( Unit3 n_hat_p = measured_p_.normal(); Unit3 n_hat_q = plane.normal(); Matrix2 H_p; - Vector e = n_hat_p.error(n_hat_q, H ? &H_p : nullptr); + Vector e = n_hat_p.errorVector(n_hat_q, boost::none, H ? &H_p : nullptr); if (H) { H->resize(2, 3); *H << H_p, Z_2x1; diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 9e4091111..3891bb7c9 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -104,7 +104,7 @@ public: /// vector of errors returns 2D vector Vector evaluateError(const Rot3& iRc, boost::optional H = boost::none) const override { Unit3 i_q = iRc * c_z_; - Vector error = i_p_.error(i_q, H); + Vector error = i_p_.errorVector(i_q, boost::none, H); if (H) { Matrix DR; iRc.rotate(c_z_, DR); From 7480d149c8d5dd1ba074f61be622585709e7d51d Mon Sep 17 00:00:00 2001 From: David Wisth Date: Mon, 15 Feb 2021 15:08:57 +0000 Subject: [PATCH 026/106] Update documentation on new factor --- .../slam/LocalOrientedPlane3Factor.cpp | 48 ++++++------------- .../slam/LocalOrientedPlane3Factor.h | 19 ++++++-- .../tests/testLocalOrientedPlane3Factor.cpp | 23 +-------- 3 files changed, 32 insertions(+), 58 deletions(-) diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp index 87d94d957..5a92dd7ef 100644 --- a/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp @@ -22,60 +22,42 @@ void LocalOrientedPlane3Factor::print(const string& s, } //*************************************************************************** -Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& basePose, - const Pose3& anchorPose, const OrientedPlane3& plane, +Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi, + const Pose3& wTwa, const OrientedPlane3& a_plane, boost::optional H1, boost::optional H2, boost::optional H3) const { - Matrix66 pose_H_anchorPose, pose_H_basePose; - Matrix36 predicted_H_pose; + Matrix66 aTai_H_wTwa, aTai_H_wTwi; + Matrix36 predicted_H_aTai; Matrix33 predicted_H_plane, error_H_predicted; - // T_LB = inv(T_WL) * T_WB - const Pose3 relativePose = anchorPose.transformPoseTo(basePose, - H2 ? &pose_H_anchorPose : nullptr, - H1 ? &pose_H_basePose : nullptr); + // Find the relative transform from anchor to sensor frame. + const Pose3 aTai = wTwa.transformPoseTo(wTwi, + H2 ? &aTai_H_wTwa : nullptr, + H1 ? &aTai_H_wTwi : nullptr); - const OrientedPlane3 predicted_plane = plane.transform(relativePose, + // Transform the plane measurement into sensor frame. + const OrientedPlane3 i_plane = a_plane.transform(aTai, H2 ? &predicted_H_plane : nullptr, - (H1 || H3) ? &predicted_H_pose : nullptr); + (H1 || H3) ? &predicted_H_aTai : nullptr); - const Vector3 err = measured_p_.error(predicted_plane, + // Calculate the error between measured and estimated planes in sensor frame. + const Vector3 err = measured_p_.errorVector(i_plane, boost::none, (H1 || H2 || H3) ? &error_H_predicted : nullptr); - // const Vector3 err = predicted_plane.errorVector(measured_p_, - // (H1 || H2 || H3) ? &error_H_predicted : nullptr); - // Apply the chain rule to calculate the derivatives. if (H1) { - *H1 = error_H_predicted * predicted_H_pose * pose_H_basePose; - // std::cout << "H1:\n" << *H1 << std::endl; + *H1 = error_H_predicted * predicted_H_aTai * aTai_H_wTwi; } if (H2) { - *H2 = error_H_predicted * predicted_H_pose * pose_H_anchorPose; - // std::cout << "H2:\n" << *H2 << std::endl; + *H2 = error_H_predicted * predicted_H_aTai * aTai_H_wTwa; } if (H3) { *H3 = error_H_predicted * predicted_H_plane; - // std::cout << "H3:\n" << *H3 << std::endl; - - // measured_p_.print(); - // predicted_plane.print(); - - // std::cout << "pose_H_anchorPose:\n" << pose_H_anchorPose << std::endl; - // std::cout << "pose_H_basePose:\n" << pose_H_basePose << std::endl; - // std::cout << "predicted_H_pose:\n" << predicted_H_pose << std::endl; - // std::cout << "error_H_predicted:\n" << error_H_predicted << std::endl; - // std::cout << "predicted_H_plane:\n" << predicted_H_plane << std::endl; - - std::cout << "H3^T x error:\n" << (*H3).transpose() * err << std::endl; - // std::cout << "H3:\n" << *H3 << std::endl; } - // std::cout << "Error: " << err.transpose() << std::endl; - return err; } diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h index 7bec7e12e..586874248 100644 --- a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h @@ -39,7 +39,8 @@ public: * @param z measured plane (a,b,c,d) coefficients as 4D vector * @param noiseModel noiseModel Gaussian noise model * @param poseKey Key or symbol for unknown pose - * @param anchorPoseKey Key or symbol for the plane's linearization point. + * @param anchorPoseKey Key or symbol for the plane's linearization point, + (called the "anchor pose"). * @param landmarkKey Key or symbol for unknown planar landmark * * Note: The anchorPoseKey can simply be chosen as the first pose a plane @@ -57,9 +58,19 @@ public: void print(const std::string& s = "LocalOrientedPlane3Factor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; - /// evaluateError - Vector evaluateError( - const Pose3& basePose, const Pose3& anchorPose, const OrientedPlane3& plane, + /*** + * Vector of errors + * @brief Error = measured_plane_.error(a_plane.transform(inv(wTwa) * wTwi)) + * + * This is the error of the measured and predicted plane in the current + * sensor frame, i. The plane is represented in the anchor pose, a. + * + * @param wTwi The pose of the sensor in world coordinates + * @param wTwa The pose of the anchor frame in world coordinates + * @param a_plane The estimated plane in anchor frame. + */ + Vector evaluateError(const Pose3& wTwi, const Pose3& wTwa, + const OrientedPlane3& a_plane, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const override; diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp index e3eec80c7..7c00824eb 100644 --- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp @@ -111,32 +111,13 @@ TEST (LocalOrientedPlane3Factor, lm_rotation_error) { ISAM2 isam2; isam2.update(graph, values); Values result_values = isam2.calculateEstimate(); + isam2.getDelta().print(); + auto optimized_plane_landmark = result_values.at(P(0)); values.print(); result_values.print(); - // HessianFactor::shared_ptr hessianFactor = graph.linearizeToHessianFactor(values); - // const auto hessian = hessianFactor->hessianBlockDiagonal(); - - // Matrix hessianP0 = hessian.at(P(0)), hessianX0 = hessian.at(X(0)); - - // Eigen::JacobiSVD svdP0(hessianP0, Eigen::ComputeThinU), - // svdX0(hessianX0, Eigen::ComputeThinU); - - // double conditionNumberP0 = svdP0.singularValues()[0] / svdP0.singularValues()[2], - // conditionNumberX0 = svdX0.singularValues()[0] / svdX0.singularValues()[5]; - - // std::cout << "Hessian P0:\n" << hessianP0 << "\n" - // << "Condition number:\n" << conditionNumberP0 << "\n" - // << "Singular values:\n" << svdP0.singularValues().transpose() << "\n" - // << "SVD U:\n" << svdP0.matrixU() << "\n" << std::endl; - - // std::cout << "Hessian X0:\n" << hessianX0 << "\n" - // << "Condition number:\n" << conditionNumberX0 << "\n" - // << "Singular values:\n" << svdX0.singularValues().transpose() << "\n" - // << "SVD U:\n" << svdX0.matrixU() << "\n" << std::endl; - // Given two noisy measurements of equal weight, expect result between the two OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3.0); From cd4c0c6dbd6cb46110336adc26259b816b6a2613 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 15 Feb 2021 19:40:40 -0500 Subject: [PATCH 027/106] Fix GTSAM MATLAB --- cmake/GtsamMatlabWrap.cmake | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index 2be0c61a8..993af47c4 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -14,6 +14,14 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX) endif() endif() +# Fixup the Python paths +if(GTWRAP_DIR) + # packaged + set(GTWRAP_PACKAGE_DIR ${GTWRAP_DIR}) +else() + set(GTWRAP_PACKAGE_DIR ${CMAKE_SOURCE_DIR}/wrap) +endif() + # Set up cache options option(GTSAM_MEX_BUILD_STATIC_MODULE "Build MATLAB wrapper statically (increases build time)" OFF) set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation") @@ -239,7 +247,10 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex set(_ignore gtsam::Point2 - gtsam::Point3) + gtsam::Point3 + gtsam::BearingRangeFactor + gtsam::BearingRangeFactor2D + gtsam::BearingRangeFactorPose2) # set the matlab wrapping script variable set(MATLAB_WRAP_SCRIPT "${GTSAM_SOURCE_DIR}/wrap/scripts/matlab_wrap.py") @@ -247,7 +258,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex add_custom_command( OUTPUT ${generated_cpp_file} DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects} - COMMAND + COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" ${PYTHON_EXECUTABLE} ${MATLAB_WRAP_SCRIPT} --src ${interfaceHeader} From 8d49d7dc40f35298da8a0317b1ce7fbcc2cc88b9 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 15 Feb 2021 19:42:16 -0500 Subject: [PATCH 028/106] Squashed 'wrap/' changes from 85d34351c..b28b3570d b28b3570d Merge pull request #30 from borglab/feature/remove_install cc2b07193 Cleanup 610ca176b Allow GTWRAP to be installed in a prefix 193b922c6 Merge pull request #29 from borglab/feature/remove_install 6d2b6ace6 fix path to package e5f220759 clean up some leftover code b0b158a0a install python package as a directory 3f4a7c775 Allow usage without install into global env 5040ba415 Merge pull request #28 from borglab/readme-update 14a7452fe updated README Getting Started section git-subtree-dir: wrap git-subtree-split: b28b3570d221b89f3568f44ed439d3a444903570 --- CMakeLists.txt | 27 ++++++--------------------- README.md | 8 ++++---- cmake/PybindWrap.cmake | 20 +++++++++++++++++--- cmake/gtwrapConfig.cmake | 18 +++++++++++------- gtwrap/matlab_wrapper.py | 9 +++++++-- 5 files changed, 45 insertions(+), 37 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c5e8c46de..5ffb66ad0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,30 +26,15 @@ install(FILES cmake/gtwrapConfig.cmake cmake/PybindWrap.cmake cmake/GtwrapUtils.cmake DESTINATION "${SCRIPT_INSTALL_DIR}/gtwrap") +include(GNUInstallDirs) +# Install the gtwrap python package as a directory so it can be found for +# wrapping. +install(DIRECTORY gtwrap DESTINATION "${CMAKE_INSTALL_DATADIR}/gtwrap") + # Install wrapping scripts as binaries to `CMAKE_INSTALL_PREFIX/bin` so they can # be invoked for wrapping. install(PROGRAMS scripts/pybind_wrap.py scripts/matlab_wrap.py TYPE BIN) # Install pybind11 directory to `CMAKE_INSTALL_PREFIX/lib/pybind11` This will # allow the gtwrapConfig.cmake file to load it later. -install(DIRECTORY pybind11 TYPE LIB) - -# ############################################################################## -# Install the Python package -find_package( - Python ${WRAP_PYTHON_VERSION} - COMPONENTS Interpreter - EXACT) - -# Detect virtualenv and set Pip args accordingly -# https://www.scivision.dev/cmake-install-python-package/ -if(DEFINED ENV{VIRTUAL_ENV} OR DEFINED ENV{CONDA_PREFIX}) - set(_pip_args) -else() - set(_pip_args "--user") -endif() -#TODO add correct flags for virtualenv - -# Finally install the gtwrap python package. -execute_process(COMMAND ${Python_EXECUTABLE} -m pip install . ${_pip_args} - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) +install(DIRECTORY pybind11 DESTINATION "${CMAKE_INSTALL_LIBDIR}/gtwrap") diff --git a/README.md b/README.md index 347cca601..5b5cbb902 100644 --- a/README.md +++ b/README.md @@ -23,18 +23,18 @@ cmake .. make install # use sudo if needed ``` -Using `wrap` in your project is straightforward from here. In you `CMakeLists.txt` file, you just need to add the following: +Using `wrap` in your project is straightforward from here. In your `CMakeLists.txt` file, you just need to add the following: ```cmake -include(PybindWrap) +find_package(gtwrap) pybind_wrap(${PROJECT_NAME}_py # target ${PROJECT_SOURCE_DIR}/cpp/${PROJECT_NAME}.h # interface header file "${PROJECT_NAME}.cpp" # the generated cpp "${PROJECT_NAME}" # module_name - "gtsam" # top namespace in the cpp file + "${PROJECT_MODULE_NAME}" # top namespace in the cpp file e.g. gtsam "${ignore}" # ignore classes - ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.tpl + ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.tpl # the wrapping template file ${PROJECT_NAME} # libs "${PROJECT_NAME}" # dependencies ON # use boost diff --git a/cmake/PybindWrap.cmake b/cmake/PybindWrap.cmake index a94dbc5cc..c7c47a662 100644 --- a/cmake/PybindWrap.cmake +++ b/cmake/PybindWrap.cmake @@ -1,5 +1,12 @@ set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION}) +if(GTWRAP_PYTHON_PACKAGE_DIR) + # packaged + set(GTWRAP_PACKAGE_DIR "${GTWRAP_PYTHON_PACKAGE_DIR}") +else() + set(GTWRAP_PACKAGE_DIR ${CMAKE_CURRENT_LIST_DIR}/..) +endif() + # User-friendly Pybind11 wrapping and installing function. # Builds a Pybind11 module from the provided interface_header. # For example, for the interface header gtsam.h, this will @@ -35,9 +42,16 @@ function(pybind_wrap else(USE_BOOST) set(_WRAP_BOOST_ARG "") endif(USE_BOOST) - + + if (UNIX) + set(GTWRAP_PATH_SEPARATOR ":") + else() + set(GTWRAP_PATH_SEPARATOR ";") + endif() + add_custom_command(OUTPUT ${generated_cpp} - COMMAND ${PYTHON_EXECUTABLE} + COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} ${PYBIND_WRAP_SCRIPT} --src ${interface_header} @@ -119,7 +133,7 @@ function(install_python_scripts else() set(build_type_tag "") endif() - # Split up filename to strip trailing '/' in WRAP_CYTHON_INSTALL_PATH if + # Split up filename to strip trailing '/' in GTSAM_PY_INSTALL_PATH if # there is one get_filename_component(location "${dest_directory}" PATH) get_filename_component(name "${dest_directory}" NAME) diff --git a/cmake/gtwrapConfig.cmake b/cmake/gtwrapConfig.cmake index 48bd4772d..15c5ea921 100644 --- a/cmake/gtwrapConfig.cmake +++ b/cmake/gtwrapConfig.cmake @@ -5,9 +5,13 @@ set(GTWRAP_DIR "${CMAKE_CURRENT_LIST_DIR}") list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}") if(WIN32 AND NOT CYGWIN) - set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/CMake") + set(GTWRAP_CMAKE_DIR "${GTWRAP_DIR}") + set(GTWRAP_SCRIPT_DIR ${GTWRAP_CMAKE_DIR}/../../../bin) + set(GTWRAP_PYTHON_PACKAGE_DIR ${GTWRAP_CMAKE_DIR}/../../../share/gtwrap) else() - set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/cmake") + set(GTWRAP_CMAKE_DIR "${GTWRAP_DIR}") + set(GTWRAP_SCRIPT_DIR ${GTWRAP_CMAKE_DIR}/../../../bin) + set(GTWRAP_PYTHON_PACKAGE_DIR ${GTWRAP_CMAKE_DIR}/../../../share/gtwrap) endif() # Standard includes @@ -16,12 +20,12 @@ include(CMakePackageConfigHelpers) include(CMakeDependentOption) # Load all the CMake scripts from the standard location -include(${SCRIPT_INSTALL_DIR}/gtwrap/PybindWrap.cmake) -include(${SCRIPT_INSTALL_DIR}/gtwrap/GtwrapUtils.cmake) +include(${GTWRAP_CMAKE_DIR}/PybindWrap.cmake) +include(${GTWRAP_CMAKE_DIR}/GtwrapUtils.cmake) # Set the variables for the wrapping scripts to be used in the build. -set(PYBIND_WRAP_SCRIPT "${CMAKE_INSTALL_FULL_BINDIR}/pybind_wrap.py") -set(MATLAB_WRAP_SCRIPT "${CMAKE_INSTALL_FULL_BINDIR}/matlab_wrap.py") +set(PYBIND_WRAP_SCRIPT "${GTWRAP_SCRIPT_DIR}/pybind_wrap.py") +set(MATLAB_WRAP_SCRIPT "${GTWRAP_SCRIPT_DIR}/matlab_wrap.py") # Load the pybind11 code from the library installation path -add_subdirectory(${CMAKE_INSTALL_FULL_LIBDIR}/pybind11 pybind11) +add_subdirectory(${GTWRAP_CMAKE_DIR}/../../gtwrap/pybind11 pybind11) diff --git a/gtwrap/matlab_wrapper.py b/gtwrap/matlab_wrapper.py index 355913ba7..669bf474f 100755 --- a/gtwrap/matlab_wrapper.py +++ b/gtwrap/matlab_wrapper.py @@ -1010,7 +1010,8 @@ class MatlabWrapper(object): file_name = self._clean_class_name(instantiated_class) namespace_file_name = namespace_name + file_name - if instantiated_class.cpp_class() in self.ignore_classes: + uninstantiated_name = "::".join(instantiated_class.namespaces()[1:]) + "::" + instantiated_class.name + if uninstantiated_name in self.ignore_classes: return None # Class comment @@ -1518,7 +1519,11 @@ class MatlabWrapper(object): ptr_ctor_frag = '' for cls in self.classes: - if cls.cpp_class().strip() in self.ignore_classes: + uninstantiated_name = "::".join(cls.namespaces()[1:]) + "::" + cls.name + self._debug("Cls: {} -> {}".format(cls.name, uninstantiated_name)) + + if uninstantiated_name in self.ignore_classes: + self._debug("Ignoring: {} -> {}".format(cls.name, uninstantiated_name)) continue def _has_serialization(cls): From 501c140286a27566b677de32fddcb3397f5ed766 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 15 Feb 2021 20:08:16 -0500 Subject: [PATCH 029/106] Use latest Boost --- .github/workflows/build-macos.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 69873980a..435936bdf 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -35,9 +35,8 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew tap ProfFan/robotics brew install cmake ninja - brew install ProfFan/robotics/boost + brew install boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV From 1771a5699ae50733526810fa6f2c559c505b539a Mon Sep 17 00:00:00 2001 From: David Wisth Date: Tue, 16 Feb 2021 10:16:21 +0000 Subject: [PATCH 030/106] Revert "remove deprecated Unit3::error() which is replaced by Unit3::errorVector()" This reverts commit 5087d36ab1987ccf661d2f21e0aa3f257a6899fb. --- gtsam/geometry/Unit3.cpp | 12 +++++++++++- gtsam/geometry/Unit3.h | 4 ++++ gtsam/geometry/tests/testUnit3.cpp | 28 ++++++++++++++++++++++++++-- gtsam/navigation/AttitudeFactor.cpp | 4 ++-- gtsam/slam/OrientedPlane3Factor.cpp | 2 +- gtsam/slam/RotateFactor.h | 2 +- 6 files changed, 45 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 1756b055d..6f70d840e 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -187,6 +187,16 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1, 2> H_p, return d; } +/* ************************************************************************* */ +Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2, 2> H_q) const { + // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 + const Vector2 xi = basis().transpose() * q.p_; + if (H_q) { + *H_q = basis().transpose() * q.basis(); + } + return xi; +} + /* ************************************************************************* */ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const { @@ -226,7 +236,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, /* ************************************************************************* */ double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const { Matrix2 H_xi_q; - const Vector2 xi = errorVector(q, boost::none, H ? &H_xi_q : nullptr); + const Vector2 xi = error(q, H ? &H_xi_q : nullptr); const double theta = xi.norm(); if (H) *H = (xi.transpose() / theta) * H_xi_q; diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 158e3e9dd..27d41a014 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -152,6 +152,10 @@ public: GTSAM_EXPORT double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, // OptionalJacobian<1,2> H2 = boost::none) const; + /// Signed, vector-valued error between two directions + /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second. + GTSAM_EXPORT Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const; + /// Signed, vector-valued error between two directions /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal. GTSAM_EXPORT Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, // diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 35206b6ab..9929df21a 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -145,6 +145,30 @@ TEST(Unit3, dot) { } } +//******************************************************************************* +TEST(Unit3, error) { + Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // + r = p.retract(Vector2(0.8, 0)); + EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); + + Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian + { + expected = numericalDerivative11( + boost::bind(&Unit3::error, &p, _1, boost::none), q); + p.error(q, actual); + EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); + } + { + expected = numericalDerivative11( + boost::bind(&Unit3::error, &p, _1, boost::none), r); + p.error(r, actual); + EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); + } +} + //******************************************************************************* TEST(Unit3, error2) { Unit3 p(0.1, -0.2, 0.8); @@ -463,10 +487,10 @@ TEST(Unit3, ErrorBetweenFactor) { TEST(Unit3, CopyAssign) { Unit3 p{1, 0.2, 0.3}; - EXPECT(p.errorVector(p).isZero()); + EXPECT(p.error(p).isZero()); p = Unit3{-1, 2, 8}; - EXPECT(p.errorVector(p).isZero()); + EXPECT(p.error(p).isZero()); } /* ************************************************************************* */ diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 512a257e7..7f335152e 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -29,13 +29,13 @@ Vector AttitudeFactor::attitudeError(const Rot3& nRb, Matrix23 D_nRef_R; Matrix22 D_e_nRef; Unit3 nRef = nRb.rotate(bRef_, D_nRef_R); - Vector e = nZ_.errorVector(nRef, boost::none, D_e_nRef); + Vector e = nZ_.error(nRef, D_e_nRef); (*H) = D_e_nRef * D_nRef_R; return e; } else { Unit3 nRef = nRb * bRef_; - return nZ_.errorVector(nRef); + return nZ_.error(nRef); } } diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 00f6b7b62..185fae73f 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -67,7 +67,7 @@ Vector OrientedPlane3DirectionPrior::evaluateError( Unit3 n_hat_p = measured_p_.normal(); Unit3 n_hat_q = plane.normal(); Matrix2 H_p; - Vector e = n_hat_p.errorVector(n_hat_q, boost::none, H ? &H_p : nullptr); + Vector e = n_hat_p.error(n_hat_q, H ? &H_p : nullptr); if (H) { H->resize(2, 3); *H << H_p, Z_2x1; diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 3891bb7c9..9e4091111 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -104,7 +104,7 @@ public: /// vector of errors returns 2D vector Vector evaluateError(const Rot3& iRc, boost::optional H = boost::none) const override { Unit3 i_q = iRc * c_z_; - Vector error = i_p_.errorVector(i_q, boost::none, H); + Vector error = i_p_.error(i_q, H); if (H) { Matrix DR; iRc.rotate(c_z_, DR); From 71f39ab2e0da6eba1222b010876f17656a93f6da Mon Sep 17 00:00:00 2001 From: David Wisth Date: Tue, 16 Feb 2021 11:26:16 +0000 Subject: [PATCH 031/106] remove the error() function from OrientedPlane3 (it had incorrect derivatives) --- gtsam/geometry/OrientedPlane3.cpp | 20 -------------------- gtsam/geometry/OrientedPlane3.h | 12 ------------ gtsam/geometry/tests/testOrientedPlane3.cpp | 21 --------------------- 3 files changed, 53 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index f3e3a039f..156f7e03f 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -17,9 +17,7 @@ * @brief A plane, represented by a normal direction and perpendicular distance */ -#include #include -#include #include @@ -59,24 +57,6 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d); } -/* ************************************************************************* */ -Vector3 OrientedPlane3::error(const OrientedPlane3& other, - OptionalJacobian<3, 3> H1, - OptionalJacobian<3, 3> H2) const { - Vector2 n_error = -n_.localCoordinates(other.n_); - - const auto f = boost::bind(&Unit3::localCoordinates, _1, _2); - if (H1) { - Matrix2 H_n_error_this = -numericalDerivative21(f, n_, other.n_); - *H1 << H_n_error_this, Z_2x1, 0, 0, 1; - } - if (H2) { - Matrix H_n_error_other = -numericalDerivative22(f, n_, other.n_); - *H2 << H_n_error_other, Z_2x1, 0, 0, -1; - } - return Vector3(n_error(0), n_error(1), d_ - other.d_); -} - /* ************************************************************************* */ Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1, diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 2550761b1..1ff21cd9c 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -20,13 +20,9 @@ #pragma once -#include #include #include -#include -#include - namespace gtsam { /** @@ -94,14 +90,6 @@ public: OptionalJacobian<3, 3> Hp = boost::none, OptionalJacobian<3, 6> Hr = boost::none) const; - /** Computes the error between two planes. - * The error is a norm 1 difference in tangent space. - * @param other The other plane - */ - Vector3 error(const OrientedPlane3& other, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; - /** Computes the error between the two planes, with derivatives. * This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses * Unit3::localCoordinates. This one has correct derivatives. diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index e164382d7..6f62ab4e2 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -144,27 +144,6 @@ TEST (OrientedPlane3, errorVector) { EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } -//******************************************************************************* -TEST (OrientedPlane3, error) { - // Hard-coded regression values, to ensure the result doesn't change. - OrientedPlane3 plane1(-1, 0.1, 0.2, 5); - OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4); - - // Test the jacobians of transform - Matrix33 actualH1, expectedH1, actualH2, expectedH2; - Vector3 actual = plane1.error(plane2, actualH1, actualH2); - - EXPECT(assert_equal((Vector) Z_3x1, plane1.error(plane1), 1e-8)); - EXPECT(assert_equal(Vector3(0.0678852, 0.0761865, -0.4), actual, 1e-5)); - - boost::function f = // - boost::bind(&OrientedPlane3::error, _1, _2, boost::none, boost::none); - expectedH1 = numericalDerivative21(f, plane1, plane2); - expectedH2 = numericalDerivative22(f, plane1, plane2); - EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); - EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); -} - //******************************************************************************* TEST (OrientedPlane3, jacobian_retract) { OrientedPlane3 plane(-1, 0.1, 0.2, 5); From 960a3e1d8ebddcaf3428ffea2c31a812fc386584 Mon Sep 17 00:00:00 2001 From: David Wisth Date: Tue, 16 Feb 2021 11:48:26 +0000 Subject: [PATCH 032/106] Tidy up comments and use cpplint --- gtsam/geometry/OrientedPlane3.cpp | 6 +- gtsam/geometry/OrientedPlane3.h | 23 ++- gtsam/geometry/tests/testOrientedPlane3.cpp | 20 +- gtsam/slam/OrientedPlane3Factor.cpp | 1 + gtsam/slam/tests/testOrientedPlane3Factor.cpp | 69 +------ .../slam/LocalOrientedPlane3Factor.cpp | 2 +- .../slam/LocalOrientedPlane3Factor.h | 26 ++- .../tests/testLocalOrientedPlane3Factor.cpp | 178 ++++++++---------- 8 files changed, 132 insertions(+), 193 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 156f7e03f..c2da4bd0b 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -79,9 +79,9 @@ Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, /* ************************************************************************* */ OrientedPlane3 OrientedPlane3::retract(const Vector3& v, - OptionalJacobian<3,3> H) const { + OptionalJacobian<3, 3> H) const { Matrix22 H_n; - Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr)); + Unit3 n_retract(n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr)); if (H) { *H << H_n, Z_2x1, 0, 0, 1; } @@ -95,4 +95,4 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { return Vector3(n_local(0), n_local(1), -d_local); } -} +} // namespace gtsam diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 1ff21cd9c..d2ad4230e 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -22,19 +22,19 @@ #include #include +#include namespace gtsam { /** - * @brief Represents an infinite plane in 3D, which is composed of a planar normal and its - * perpendicular distance to the origin. - * Currently it provides a transform of the plane, and a norm 1 differencing of two planes. + * @brief Represents an infinite plane in 3D, which is composed of a planar + * normal and its perpendicular distance to the origin. + * Currently it provides a transform of the plane, and a norm 1 differencing of + * two planes. * Refer to Trevor12iros for more math details. */ class GTSAM_EXPORT OrientedPlane3 { - private: - Unit3 n_; ///< The direction of the planar normal double d_; ///< The perpendicular distance to this plane @@ -91,12 +91,14 @@ public: OptionalJacobian<3, 6> Hr = boost::none) const; /** Computes the error between the two planes, with derivatives. - * This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses - * Unit3::localCoordinates. This one has correct derivatives. + * This uses Unit3::errorVector, as opposed to the other .error() in this + * class, which uses Unit3::localCoordinates. This one has correct + * derivatives. * NOTE(hayk): The derivatives are zero when normals are exactly orthogonal. * @param other the other plane */ - Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, // + Vector3 errorVector(const OrientedPlane3& other, + OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; /// Dimensionality of tangent space = 3 DOF @@ -110,7 +112,8 @@ public: } /// The retract function - OrientedPlane3 retract(const Vector3& v, OptionalJacobian<3,3> H = boost::none) const; + OrientedPlane3 retract(const Vector3& v, + OptionalJacobian<3, 3> H = boost::none) const; /// The local coordinates function Vector3 localCoordinates(const OrientedPlane3& s) const; @@ -142,5 +145,5 @@ template<> struct traits : public internal::Manifold< OrientedPlane3> { }; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 6f62ab4e2..2461cea1a 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -31,7 +31,7 @@ GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) //******************************************************************************* -TEST (OrientedPlane3, getMethods) { +TEST(OrientedPlane3, getMethods) { Vector4 c; c << -1, 0, 0, 5; OrientedPlane3 plane1(c); @@ -92,7 +92,6 @@ inline static Vector randomVector(const Vector& minLimits, //******************************************************************************* TEST(OrientedPlane3, localCoordinates_retract) { - size_t numIterations = 10000; Vector4 minPlaneLimit, maxPlaneLimit; minPlaneLimit << -1.0, -1.0, -1.0, 0.01; @@ -102,7 +101,6 @@ TEST(OrientedPlane3, localCoordinates_retract) { minXiLimit << -M_PI, -M_PI, -10.0; maxXiLimit << M_PI, M_PI, 10.0; for (size_t i = 0; i < numIterations; i++) { - // Create a Plane OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit)); Vector v12 = randomVector(minXiLimit, maxXiLimit); @@ -121,22 +119,24 @@ TEST(OrientedPlane3, localCoordinates_retract) { } //******************************************************************************* -TEST (OrientedPlane3, errorVector) { +TEST(OrientedPlane3, errorVector) { OrientedPlane3 plane1(-1, 0.1, 0.2, 5); OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4); // Hard-coded regression values, to ensure the result doesn't change. EXPECT(assert_equal((Vector) Z_3x1, plane1.errorVector(plane1), 1e-8)); - EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), plane1.errorVector(plane2), 1e-5)); + EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), + plane1.errorVector(plane2), 1e-5)); // Test the jacobians of transform Matrix33 actualH1, expectedH1, actualH2, expectedH2; Vector3 actual = plane1.errorVector(plane2, actualH1, actualH2); - EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()), Vector2(actual[0], actual[1]))); + EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()), + Vector2(actual[0], actual[1]))); EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2])); - boost::function f = // + boost::function f = boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none); expectedH1 = numericalDerivative21(f, plane1, plane2); expectedH2 = numericalDerivative22(f, plane1, plane2); @@ -145,19 +145,19 @@ TEST (OrientedPlane3, errorVector) { } //******************************************************************************* -TEST (OrientedPlane3, jacobian_retract) { +TEST(OrientedPlane3, jacobian_retract) { OrientedPlane3 plane(-1, 0.1, 0.2, 5); Matrix33 H_actual; boost::function f = boost::bind(&OrientedPlane3::retract, plane, _1, boost::none); { - Vector3 v (-0.1, 0.2, 0.3); + Vector3 v(-0.1, 0.2, 0.3); plane.retract(v, H_actual); Matrix H_expected_numerical = numericalDerivative11(f, v); EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5)); } { - Vector3 v (0, 0, 0); + Vector3 v(0, 0, 0); plane.retract(v, H_actual); Matrix H_expected_numerical = numericalDerivative11(f, v); EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5)); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 185fae73f..d7508f6b8 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -27,6 +27,7 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, boost::optional H2) const { Matrix36 predicted_H_pose; Matrix33 predicted_H_plane, error_H_predicted; + OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr, H1 ? &predicted_H_pose : nullptr); diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 3736743ae..d1e361bc3 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -83,6 +83,7 @@ TEST(OrientedPlane3Factor, lm_translation_error) { } // // ************************************************************************* +/* TEST (OrientedPlane3Factor, lm_rotation_error) { // Tests one pose, two measurements of the landmark that differ in angle only. // Normal along -x, 3m away @@ -122,6 +123,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { 0.0, 3.0); EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } +*/ // ************************************************************************* TEST( OrientedPlane3Factor, Derivatives ) { @@ -211,34 +213,28 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { NonlinearFactorGraph graph; // Setup prior factors - Pose3 x0(Rot3::identity(), Vector3(0, 0, 10)); + // Note: If x0 is too far away from the origin (e.g. x=100) this test can fail. + Pose3 x0(Rot3::identity(), Vector3(10, -1, 1)); auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); graph.addPrior(X(0), x0, x0_noise); // Two horizontal planes with different heights, in the world frame. const Plane p1(0,0,1,1), p2(0,0,1,2); - auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); - graph.addPrior(P(1), p1, p1_noise); - - // ADDING THIS PRIOR MAKES OPTIMIZATION FAIL auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); + graph.addPrior(P(1), p1, p1_noise); graph.addPrior(P(2), p2, p2_noise); - // First plane factor + // Plane factors auto p1_measured_from_x0 = p1.transform(x0); // transform p1 to pose x0 as a measurement - const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); - graph.emplace_shared( - p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1)); - - // Second plane factor auto p2_measured_from_x0 = p2.transform(x0); // transform p2 to pose x0 as a measurement + const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05); + graph.emplace_shared( + p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1)); graph.emplace_shared( p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), P(2)); - GTSAM_PRINT(graph); - // Initial values // Just offset the initial pose by 1m. This is what we are trying to optimize. Values initialEstimate; @@ -247,54 +243,9 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { initialEstimate.insert(P(2), p2); initialEstimate.insert(X(0), x0_initial); - // Print Jacobian - cout << graph.linearize(initialEstimate)->augmentedJacobian() << endl << endl; - - // For testing only - HessianFactor::shared_ptr hessianFactor = graph.linearizeToHessianFactor(initialEstimate); - const auto hessian = hessianFactor->hessianBlockDiagonal(); - - Matrix hessianP1 = hessian.at(P(1)), - hessianP2 = hessian.at(P(2)), - hessianX0 = hessian.at(X(0)); - - Eigen::JacobiSVD svdP1(hessianP1, Eigen::ComputeThinU), - svdP2(hessianP2, Eigen::ComputeThinU), - svdX0(hessianX0, Eigen::ComputeThinU); - - double conditionNumberP1 = svdP1.singularValues()[0] / svdP1.singularValues()[2], - conditionNumberP2 = svdP2.singularValues()[0] / svdP2.singularValues()[2], - conditionNumberX0 = svdX0.singularValues()[0] / svdX0.singularValues()[5]; - - std::cout << "Hessian P1:\n" << hessianP1 << "\n" - << "Condition number:\n" << conditionNumberP1 << "\n" - << "Singular values:\n" << svdP1.singularValues().transpose() << "\n" - << "SVD U:\n" << svdP1.matrixU() << "\n" << std::endl; - - std::cout << "Hessian P2:\n" << hessianP2 << "\n" - << "Condition number:\n" << conditionNumberP2 << "\n" - << "Singular values:\n" << svdP2.singularValues().transpose() << "\n" - << "SVD U:\n" << svdP2.matrixU() << "\n" << std::endl; - - std::cout << "Hessian X0:\n" << hessianX0 << "\n" - << "Condition number:\n" << conditionNumberX0 << "\n" - << "Singular values:\n" << svdX0.singularValues().transpose() << "\n" - << "SVD U:\n" << svdX0.matrixU() << "\n" << std::endl; - - // std::cout << "Hessian P2:\n" << hessianP2 << std::endl; - // std::cout << "Hessian X0:\n" << hessianX0 << std::endl; - - // For testing only - // Optimize try { - GaussNewtonParams params; - //GTSAM_PRINT(graph); - //Ordering ordering = list_of(P(1))(P(2))(X(0)); // make sure P1 eliminated first - //params.setOrdering(ordering); - // params.setLinearSolverType("SEQUENTIAL_QR"); // abundance of caution - params.setVerbosity("TERMINATION"); // show info about stopping conditions - GaussNewtonOptimizer optimizer(graph, initialEstimate, params); + GaussNewtonOptimizer optimizer(graph, initialEstimate); Values result = optimizer.optimize(); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); EXPECT(x0.equals(result.at(X(0)))); diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp index 5a92dd7ef..25d7083f8 100644 --- a/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp @@ -22,7 +22,7 @@ void LocalOrientedPlane3Factor::print(const string& s, } //*************************************************************************** -Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi, +Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi, const Pose3& wTwa, const OrientedPlane3& a_plane, boost::optional H1, boost::optional H2, boost::optional H3) const { diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h index 586874248..5264c8f4b 100644 --- a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h @@ -9,6 +9,7 @@ #include #include +#include namespace gtsam { @@ -21,11 +22,18 @@ namespace gtsam { * M. Kaess, "Simultaneous Localization and Mapping with Infinite Planes", * IEEE International Conference on Robotics and Automation, 2015. * - * Note: This uses the retraction from the OrientedPlane3, not the quaternion- - * based representation proposed by Kaess. + * + * The main purpose of this factor is to improve the numerical stability of the + * optimization, especially compared to gtsam::OrientedPlane3Factor. This + * is especially relevant when the sensor is far from the origin (and thus + * the derivatives associated to transforming the plane are large). + * + * x0 is the current sensor pose, and x1 is the local "anchor pose" - i.e. + * a local linearisation point for the plane. The plane is representated and + * optimized in x1 frame in the optimization. */ -class LocalOrientedPlane3Factor: public NoiseModelFactor3 { +class LocalOrientedPlane3Factor: public NoiseModelFactor3 { protected: OrientedPlane3 measured_p_; typedef NoiseModelFactor3 Base; @@ -50,8 +58,9 @@ public: Key poseKey, Key anchorPoseKey, Key landmarkKey) : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {} - LocalOrientedPlane3Factor(const OrientedPlane3& z, const SharedGaussian& noiseModel, - Key poseKey, Key anchorPoseKey, Key landmarkKey) + LocalOrientedPlane3Factor(const OrientedPlane3& z, + const SharedGaussian& noiseModel, + Key poseKey, Key anchorPoseKey, Key landmarkKey) : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {} /// print @@ -68,6 +77,9 @@ public: * @param wTwi The pose of the sensor in world coordinates * @param wTwa The pose of the anchor frame in world coordinates * @param a_plane The estimated plane in anchor frame. + * + * Note: The optimized plane is represented in anchor frame, a, not the + * world frame. */ Vector evaluateError(const Pose3& wTwi, const Pose3& wTwa, const OrientedPlane3& a_plane, @@ -76,5 +88,5 @@ public: boost::optional H3 = boost::none) const override; }; -} // gtsam +} // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp index 7c00824eb..bb71d8ecc 100644 --- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp @@ -48,12 +48,12 @@ TEST(LocalOrientedPlane3Factor, lm_translation_error) { OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); NonlinearFactorGraph graph; - + // Init pose and prior. Pose Prior is needed since a single plane measurement // does not fully constrain the pose Pose3 init_pose = Pose3::identity(); graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); - + // Add two landmark measurements, differing in range Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; Vector4 measurement1 {-1.0, 0.0, 0.0, 1.0}; @@ -81,6 +81,7 @@ TEST(LocalOrientedPlane3Factor, lm_translation_error) { } // ************************************************************************* +/* TEST (LocalOrientedPlane3Factor, lm_rotation_error) { // Tests one pose, two measurements of the landmark that differ in angle only. // Normal along -x, 3m away @@ -123,6 +124,7 @@ TEST (LocalOrientedPlane3Factor, lm_rotation_error) { 0.0, 3.0); EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } +*/ // ************************************************************************* TEST(LocalOrientedPlane3Factor, Derivatives) { @@ -132,7 +134,7 @@ TEST(LocalOrientedPlane3Factor, Derivatives) { // Linearisation point OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7); Pose3 poseLin(Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI), Point3(1, 2, -4)); - Pose3 anchorPoseLin(Rot3::RzRyRx(-0.1*M_PI, 0.2*M_PI, 1.0*M_PI), Point3(-5, 0, 1)); + Pose3 anchorPoseLin(Rot3::RzRyRx(-0.1*M_PI, 0.2*M_PI, 1.0), Point3(-5, 0, 1)); // Factor Key planeKey(1), poseKey(2), anchorPoseKey(3); @@ -142,13 +144,17 @@ TEST(LocalOrientedPlane3Factor, Derivatives) { // Calculate numerical derivatives auto f = boost::bind(&LocalOrientedPlane3Factor::evaluateError, factor, _1, _2, _3, boost::none, boost::none, boost::none); - Matrix numericalH1 = numericalDerivative31(f, poseLin, anchorPoseLin, pLin); - Matrix numericalH2 = numericalDerivative32(f, poseLin, anchorPoseLin, pLin); - Matrix numericalH3 = numericalDerivative33(f, poseLin, anchorPoseLin, pLin); + Matrix numericalH1 = numericalDerivative31(f, poseLin, anchorPoseLin, pLin); + Matrix numericalH2 = numericalDerivative32(f, poseLin, anchorPoseLin, pLin); + Matrix numericalH3 = numericalDerivative33(f, poseLin, anchorPoseLin, pLin); // Use the factor to calculate the derivative Matrix actualH1, actualH2, actualH3; - factor.evaluateError(poseLin, anchorPoseLin, pLin, actualH1, actualH2, actualH3); + factor.evaluateError(poseLin, anchorPoseLin, pLin, actualH1, actualH2, + actualH3); // Verify we get the expected error EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); @@ -157,109 +163,75 @@ TEST(LocalOrientedPlane3Factor, Derivatives) { } -// /* ************************************************************************* */ -// // Simplified version of the test by Marco Camurri to debug issue #561 -// TEST(OrientedPlane3Factor, Issue561Simplified) { -// // Typedefs -// using Plane = OrientedPlane3; +/* ************************************************************************* */ +// Simplified version of the test by Marco Camurri to debug issue #561 +// +// This test provides an example of how LocalOrientedPlane3Factor works. +// x0 is the current sensor pose, and x1 is the local "anchor pose" - i.e. +// a local linearisation point for the plane. The plane is representated and +// optimized in x1 frame in the optimization. This greatly improves numerical +// stability when the pose is far from the origin. +// +TEST(LocalOrientedPlane3Factor, Issue561Simplified) { + // Typedefs + using Plane = OrientedPlane3; -// NonlinearFactorGraph graph; + NonlinearFactorGraph graph; -// // Setup prior factors -// Pose3 x0(Rot3::identity(), Vector3(0, 0, 10)); -// auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); -// graph.addPrior(X(0), x0, x0_noise); + // Setup prior factors + Pose3 x0(Rot3::identity(), Vector3(100, 30, 10)); // the "sensor pose" + Pose3 x1(Rot3::identity(), Vector3(90, 40, 5) ); // the "anchor pose" -// // Two horizontal planes with different heights, in the world frame. -// const Plane p1(0,0,1,1), p2(0,0,1,2); + auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); + auto x1_noise = noiseModel::Isotropic::Sigma(6, 0.01); + graph.addPrior(X(0), x0, x0_noise); + graph.addPrior(X(1), x1, x1_noise); -// auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); -// graph.addPrior(P(1), p1, p1_noise); + // Two horizontal planes with different heights, in the world frame. + const Plane p1(0, 0, 1, 1), p2(0, 0, 1, 2); + // Transform to x1, the "anchor frame" (i.e. local frame) + auto p1_in_x1 = p1.transform(x1); + auto p2_in_x1 = p2.transform(x1); + auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); + auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); + graph.addPrior(P(1), p1_in_x1, p1_noise); + graph.addPrior(P(2), p2_in_x1, p2_noise); -// // ADDING THIS PRIOR MAKES OPTIMIZATION FAIL -// auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); -// graph.addPrior(P(2), p2, p2_noise); + // Add plane factors, with a local linearization point. + // transform p1 to pose x0 as a measurement + auto p1_measured_from_x0 = p1.transform(x0); + // transform p2 to pose x0 as a measurement + auto p2_measured_from_x0 = p2.transform(x0); + const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); + const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05); + graph.emplace_shared( + p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), X(1), P(1)); + graph.emplace_shared( + p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), X(1), P(2)); -// // First plane factor -// auto p1_measured_from_x0 = p1.transform(x0); // transform p1 to pose x0 as a measurement -// const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); -// graph.emplace_shared( -// p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1)); + // Initial values + // Just offset the initial pose by 1m. This is what we are trying to optimize. + Values initialEstimate; + Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1, 0, 0))); + initialEstimate.insert(P(1), p1_in_x1); + initialEstimate.insert(P(2), p2_in_x1); + initialEstimate.insert(X(0), x0_initial); + initialEstimate.insert(X(1), x1); -// // Second plane factor -// auto p2_measured_from_x0 = p2.transform(x0); // transform p2 to pose x0 as a measurement -// const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05); -// graph.emplace_shared( -// p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), P(2)); - -// GTSAM_PRINT(graph); - -// // Initial values -// // Just offset the initial pose by 1m. This is what we are trying to optimize. -// Values initialEstimate; -// Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0))); -// initialEstimate.insert(P(1), p1); -// initialEstimate.insert(P(2), p2); -// initialEstimate.insert(X(0), x0_initial); - -// // Print Jacobian -// cout << graph.linearize(initialEstimate)->augmentedJacobian() << endl << endl; - -// // For testing only -// HessianFactor::shared_ptr hessianFactor = graph.linearizeToHessianFactor(initialEstimate); -// const auto hessian = hessianFactor->hessianBlockDiagonal(); - -// Matrix hessianP1 = hessian.at(P(1)), -// hessianP2 = hessian.at(P(2)), -// hessianX0 = hessian.at(X(0)); - -// Eigen::JacobiSVD svdP1(hessianP1, Eigen::ComputeThinU), -// svdP2(hessianP2, Eigen::ComputeThinU), -// svdX0(hessianX0, Eigen::ComputeThinU); - -// double conditionNumberP1 = svdP1.singularValues()[0] / svdP1.singularValues()[2], -// conditionNumberP2 = svdP2.singularValues()[0] / svdP2.singularValues()[2], -// conditionNumberX0 = svdX0.singularValues()[0] / svdX0.singularValues()[5]; - -// std::cout << "Hessian P1:\n" << hessianP1 << "\n" -// << "Condition number:\n" << conditionNumberP1 << "\n" -// << "Singular values:\n" << svdP1.singularValues().transpose() << "\n" -// << "SVD U:\n" << svdP1.matrixU() << "\n" << std::endl; - -// std::cout << "Hessian P2:\n" << hessianP2 << "\n" -// << "Condition number:\n" << conditionNumberP2 << "\n" -// << "Singular values:\n" << svdP2.singularValues().transpose() << "\n" -// << "SVD U:\n" << svdP2.matrixU() << "\n" << std::endl; - -// std::cout << "Hessian X0:\n" << hessianX0 << "\n" -// << "Condition number:\n" << conditionNumberX0 << "\n" -// << "Singular values:\n" << svdX0.singularValues().transpose() << "\n" -// << "SVD U:\n" << svdX0.matrixU() << "\n" << std::endl; - -// // std::cout << "Hessian P2:\n" << hessianP2 << std::endl; -// // std::cout << "Hessian X0:\n" << hessianX0 << std::endl; - -// // For testing only - -// // Optimize -// try { -// GaussNewtonParams params; -// //GTSAM_PRINT(graph); -// //Ordering ordering = list_of(P(1))(P(2))(X(0)); // make sure P1 eliminated first -// //params.setOrdering(ordering); -// // params.setLinearSolverType("SEQUENTIAL_QR"); // abundance of caution -// params.setVerbosity("TERMINATION"); // show info about stopping conditions -// GaussNewtonOptimizer optimizer(graph, initialEstimate, params); -// Values result = optimizer.optimize(); -// EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); -// EXPECT(x0.equals(result.at(X(0)))); -// EXPECT(p1.equals(result.at(P(1)))); -// EXPECT(p2.equals(result.at(P(2)))); -// } catch (const IndeterminantLinearSystemException &e) { -// std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl; -// EXPECT(false); // fail if this happens -// } -// } + // Optimize + try { + GaussNewtonOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); + EXPECT(x0.equals(result.at(X(0)))); + EXPECT(p1_in_x1.equals(result.at(P(1)))); + EXPECT(p2_in_x1.equals(result.at(P(2)))); + } catch (const IndeterminantLinearSystemException &e) { + cerr << "CAPTURED THE EXCEPTION: " + << DefaultKeyFormatter(e.nearbyVariable()) << endl; + EXPECT(false); // fail if this happens + } +} /* ************************************************************************* */ int main() { From 5b0bd08e7bd67da39e7897cc01723d65fc87bea6 Mon Sep 17 00:00:00 2001 From: David Wisth Date: Tue, 16 Feb 2021 18:21:53 +0000 Subject: [PATCH 033/106] small tidy and fix unit tests --- .../tests/testLocalOrientedPlane3Factor.cpp | 34 +++++++++---------- 1 file changed, 16 insertions(+), 18 deletions(-) diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp index bb71d8ecc..06a5ae688 100644 --- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp @@ -10,28 +10,22 @@ * -------------------------------------------------------------------------- */ /* - * @file testOrientedPlane3Factor.cpp - * @date Dec 19, 2012 - * @author Alex Trevor - * @brief Tests the OrientedPlane3Factor class + * @file testLocalOrientedPlane3Factor.cpp + * @date Feb 12, 2021 + * @author David Wisth + * @brief Tests the LocalOrientedPlane3Factor class */ #include -#include - #include #include -#include #include #include -#include -#include #include -using namespace boost::assign; using namespace gtsam; using namespace std; @@ -52,21 +46,24 @@ TEST(LocalOrientedPlane3Factor, lm_translation_error) { // Init pose and prior. Pose Prior is needed since a single plane measurement // does not fully constrain the pose Pose3 init_pose = Pose3::identity(); + Pose3 anchor_pose = Pose3::identity(); graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); + graph.addPrior(X(1), anchor_pose, noiseModel::Isotropic::Sigma(6, 0.001)); // Add two landmark measurements, differing in range - Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; - Vector4 measurement1 {-1.0, 0.0, 0.0, 1.0}; + Vector4 measurement0(-1.0, 0.0, 0.0, 3.0); + Vector4 measurement1(-1.0, 0.0, 0.0, 1.0); LocalOrientedPlane3Factor factor0( - measurement0, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); + measurement0, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(1), P(0)); LocalOrientedPlane3Factor factor1( - measurement1, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); + measurement1, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(1), P(0)); graph.add(factor0); graph.add(factor1); // Initial Estimate Values values; values.insert(X(0), init_pose); + values.insert(X(1), anchor_pose); values.insert(P(0), test_lm0); // Optimize @@ -94,8 +91,8 @@ TEST (LocalOrientedPlane3Factor, lm_rotation_error) { graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); // Add two landmark measurements, differing in angle - Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; - Vector4 measurement1 {0.0, -1.0, 0.0, 3.0}; + Vector4 measurement0(-1.0, 0.0, 0.0, 3.0); + Vector4 measurement1(0.0, -1.0, 0.0, 3.0); LocalOrientedPlane3Factor factor0(measurement0, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); LocalOrientedPlane3Factor factor1(measurement1, @@ -220,8 +217,9 @@ TEST(LocalOrientedPlane3Factor, Issue561Simplified) { // Optimize try { - GaussNewtonOptimizer optimizer(graph, initialEstimate); - Values result = optimizer.optimize(); + ISAM2 isam2; + isam2.update(graph, initialEstimate); + Values result = isam2.calculateEstimate(); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); EXPECT(x0.equals(result.at(X(0)))); EXPECT(p1_in_x1.equals(result.at(P(1)))); From 8552752839105a1255d71951250e302ec6c0a8cc Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 19 Feb 2021 16:08:43 -0500 Subject: [PATCH 034/106] 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 8f18ce931b43862408b770367fc505fe5b2b0824 Mon Sep 17 00:00:00 2001 From: David Wisth Date: Sat, 20 Feb 2021 23:13:43 +0000 Subject: [PATCH 035/106] Add inline comments on commented-out unit tests --- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 4 +++- gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp | 5 ++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index d1e361bc3..a0ef7b91d 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -82,7 +82,9 @@ TEST(OrientedPlane3Factor, lm_translation_error) { EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } -// // ************************************************************************* +// ************************************************************************* +// TODO As described in PR #564 after correcting the derivatives in +// OrientedPlane3Factor this test fails. It should be debugged and re-enabled. /* TEST (OrientedPlane3Factor, lm_rotation_error) { // Tests one pose, two measurements of the landmark that differ in angle only. diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp index 06a5ae688..9f6fe5b50 100644 --- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp @@ -78,6 +78,8 @@ TEST(LocalOrientedPlane3Factor, lm_translation_error) { } // ************************************************************************* +// TODO As described in PR #564 after correcting the derivatives in +// OrientedPlane3Factor this test fails. It should be debugged and re-enabled. /* TEST (LocalOrientedPlane3Factor, lm_rotation_error) { // Tests one pose, two measurements of the landmark that differ in angle only. @@ -86,7 +88,8 @@ TEST (LocalOrientedPlane3Factor, lm_rotation_error) { NonlinearFactorGraph graph; - // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose + // Init pose and prior. Pose Prior is needed since a single plane measurement + // does not fully constrain the pose Pose3 init_pose = Pose3::identity(); graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); From 8c324ccbf58efd92a7dc774827993edd23a3c6b4 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Tue, 23 Feb 2021 17:15:27 -0800 Subject: [PATCH 036/106] Initial implimentation of a Constant Velocity Constraint between NavStates --- gtsam/navigation/ConstantVelocityFactor.h | 59 +++++++++++++++ .../tests/testConstantVelocityFactor.cpp | 74 +++++++++++++++++++ 2 files changed, 133 insertions(+) create mode 100644 gtsam/navigation/ConstantVelocityFactor.h create mode 100644 gtsam/navigation/tests/testConstantVelocityFactor.cpp diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h new file mode 100644 index 000000000..8105609d1 --- /dev/null +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -0,0 +1,59 @@ +#include +#include +#include +#include +#include + +using gtsam::Key; +using gtsam::NavState; +using gtsam::NoiseModelFactor2; +using gtsam::Point3; +using gtsam::SharedNoiseModel; +using gtsam::Velocity3; + +class ConstantVelocityFactor : public NoiseModelFactor2 { + double dt_; + + public: + using Base = NoiseModelFactor2; + + public: + ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model) + : NoiseModelFactor2(model, i, j), dt_(dt) {} + ~ConstantVelocityFactor() override{}; + + gtsam::Vector evaluateError(const NavState &x1, const NavState &x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + if (H1) { + (*H1) = numericalDerivative21( + boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5); + } + if (H2) { + (*H2) = numericalDerivative22( + boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5); + } + + return evaluateError_(x1, x2, dt_); + } + + private: + static gtsam::Vector evaluateError_(const NavState &x1, const NavState &x2, double dt) { + const Velocity3 &v1 = x1.v(); + const Velocity3 &v2 = x2.v(); + const Point3 &p1 = x1.t(); + const Point3 &p2 = x2.t(); + + // trapezoidal integration constant accelleration + // const Point3 hx = p1 + Point3((v1 + v2) * dt / 2.0); + + // euler start + // const Point3 hx = p1 + Point3(v1 * dt); + + // euler end + const Point3 hx = p1 + Point3(v2 * dt); + + return p2 - hx; + } +}; + diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp new file mode 100644 index 000000000..9e3c9f929 --- /dev/null +++ b/gtsam/navigation/tests/testConstantVelocityFactor.cpp @@ -0,0 +1,74 @@ +/* ---------------------------------------------------------------------------- + + * 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 testConstantVelocityFactor.cpp + * @brief Unit test for ConstantVelocityFactor + * @author Alex Cunningham + * @author Asa Hammond + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +/* ************************************************************************* */ +TEST(ConstantVelocityFactor, VelocityFactor) { + using namespace gtsam; + + const auto tol = double{1e-5}; + + const auto x1 = Key{1}; + const auto x2 = Key{2}; + + const auto dt = double{1.0}; + const auto mu = double{1000}; + + // moving upward with groundtruth velocity" + const auto origin = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 0.0}}, Velocity3{0.0, 0.0, 0.0}}; + + const auto state0 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 0.0}}, Velocity3{0.0, 0.0, 1.0}}; + + const auto state1 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 1.0}}, Velocity3{0.0, 0.0, 1.0}}; + + const auto state2 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}}; + + const auto noise_model = noiseModel::Constrained::All(3, mu); + + const auto factor = ConstantVelocityFactor(x1, x2, dt, noise_model); + + // positions are the same, secondary state has velocity 1.0 , + EXPECT(assert_equal(Unit3{0.0, 0.0, -1.0}.unitVector(), factor.evaluateError(origin, state0), tol)); + + // same velocities, different position + // second state agrees with initial state + velocity * dt + EXPECT(assert_equal(Unit3{0.0, 0.0, 0.0}.unitVector(), factor.evaluateError(state0, state1), tol)); + + // both bodies have the same velocity, + // but state2.pose() != state0.pose() + state0.velocity * dt + // error comes from this pose difference + EXPECT(assert_equal(Unit3{0.0, 0.0, 1.0}.unitVector(), factor.evaluateError(state0, state2), tol)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 2aa7144b7ea4c67cc55b6997b64477a429684eae Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Tue, 23 Feb 2021 17:32:48 -0800 Subject: [PATCH 037/106] add gtsam namespace --- gtsam/navigation/ConstantVelocityFactor.h | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 8105609d1..2188cc76e 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -4,12 +4,7 @@ #include #include -using gtsam::Key; -using gtsam::NavState; -using gtsam::NoiseModelFactor2; -using gtsam::Point3; -using gtsam::SharedNoiseModel; -using gtsam::Velocity3; +namespace gtsam { class ConstantVelocityFactor : public NoiseModelFactor2 { double dt_; @@ -57,3 +52,4 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { } }; +} // namespace gtsam From c0cd024b2a9ba5a2bebeede6e9e53022637641c2 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Tue, 23 Feb 2021 18:27:20 -0800 Subject: [PATCH 038/106] bind a lambda instead of a static function --- gtsam/navigation/ConstantVelocityFactor.h | 46 +++++++++++------------ 1 file changed, 22 insertions(+), 24 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 2188cc76e..08cd531b6 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -20,35 +20,33 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { gtsam::Vector evaluateError(const NavState &x1, const NavState &x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { + // so we can reuse this calculation below + auto evaluateErrorInner = [dt = dt_](const NavState &x1, const NavState &x2) -> gtsam::Vector { + const Velocity3 &v1 = x1.v(); + const Velocity3 &v2 = x2.v(); + const Point3 &p1 = x1.t(); + const Point3 &p2 = x2.t(); + + // trapezoidal integration constant accelleration + // const Point3 hx = p1 + Point3((v1 + v2) * dt / 2.0); + + // euler start + // const Point3 hx = p1 + Point3(v1 * dt); + + // euler end + const Point3 hx = p1 + Point3(v2 * dt); + + return p2 - hx; + }; + if (H1) { - (*H1) = numericalDerivative21( - boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5); + (*H1) = numericalDerivative21(evaluateErrorInner, x1, x2, 1e-5); } if (H2) { - (*H2) = numericalDerivative22( - boost::bind(ConstantVelocityFactor::evaluateError_, _1, _2, dt_), x1, x2, 1e-5); + (*H2) = numericalDerivative22(evaluateErrorInner, x1, x2, 1e-5); } - return evaluateError_(x1, x2, dt_); - } - - private: - static gtsam::Vector evaluateError_(const NavState &x1, const NavState &x2, double dt) { - const Velocity3 &v1 = x1.v(); - const Velocity3 &v2 = x2.v(); - const Point3 &p1 = x1.t(); - const Point3 &p2 = x2.t(); - - // trapezoidal integration constant accelleration - // const Point3 hx = p1 + Point3((v1 + v2) * dt / 2.0); - - // euler start - // const Point3 hx = p1 + Point3(v1 * dt); - - // euler end - const Point3 hx = p1 + Point3(v2 * dt); - - return p2 - hx; + return evaluateErrorInner(x1, x2); } }; From 0effe69df2ddbac8c26c851c8e7c53d97601278b Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 24 Feb 2021 15:26:13 -0500 Subject: [PATCH 039/106] 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 040/106] 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 041/106] 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 042/106] 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 043/106] 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 044/106] 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 38ec991731136015c0e03cfe0fce1c2d615f7b28 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:09:45 -0800 Subject: [PATCH 045/106] Add docstrings, use update() to build predicted state and build the error calc --- gtsam/navigation/ConstantVelocityFactor.h | 45 ++++++++++--------- .../tests/testConstantVelocityFactor.cpp | 39 +++++++++++++--- 2 files changed, 57 insertions(+), 27 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 08cd531b6..f676108ce 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -6,6 +6,10 @@ namespace gtsam { +/** + * Binary factor for applying a constant velocity model to a moving body represented as a NavState. + * The only measurement is dt, the time delta between the states. + */ class ConstantVelocityFactor : public NoiseModelFactor2 { double dt_; @@ -17,36 +21,33 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { : NoiseModelFactor2(model, i, j), dt_(dt) {} ~ConstantVelocityFactor() override{}; + /** + * @brief Caclulate error: (x2 - x1.update(dt))) + * where X1 and X1 are NavStates and dt is + * the time difference in seconds between the states. + * @param x1 NavState for key a + * @param x2 NavState for key b + * @param H1 optional jacobian in x1 + * @param H2 optional jacobian in x2 + * @return * Vector + */ gtsam::Vector evaluateError(const NavState &x1, const NavState &x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { - // so we can reuse this calculation below - auto evaluateErrorInner = [dt = dt_](const NavState &x1, const NavState &x2) -> gtsam::Vector { - const Velocity3 &v1 = x1.v(); - const Velocity3 &v2 = x2.v(); - const Point3 &p1 = x1.t(); - const Point3 &p2 = x2.t(); + // only used to use update() below + const Vector3 b_accel{0.0, 0.0, 0.0}; + const Vector3 b_omega{0.0, 0.0, 0.0}; - // trapezoidal integration constant accelleration - // const Point3 hx = p1 + Point3((v1 + v2) * dt / 2.0); + Matrix predicted_H_x1; + NavState predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {}); - // euler start - // const Point3 hx = p1 + Point3(v1 * dt); - - // euler end - const Point3 hx = p1 + Point3(v2 * dt); - - return p2 - hx; - }; + Matrix error_H_predicted; + Vector9 error = predicted.localCoordinates(x2, error_H_predicted, H2); if (H1) { - (*H1) = numericalDerivative21(evaluateErrorInner, x1, x2, 1e-5); + *H1 = error_H_predicted * predicted_H_x1; } - if (H2) { - (*H2) = numericalDerivative22(evaluateErrorInner, x1, x2, 1e-5); - } - - return evaluateErrorInner(x1, x2); + return error; } }; diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp index 9e3c9f929..cdf05b253 100644 --- a/gtsam/navigation/tests/testConstantVelocityFactor.cpp +++ b/gtsam/navigation/tests/testConstantVelocityFactor.cpp @@ -53,17 +53,46 @@ TEST(ConstantVelocityFactor, VelocityFactor) { const auto factor = ConstantVelocityFactor(x1, x2, dt, noise_model); - // positions are the same, secondary state has velocity 1.0 , - EXPECT(assert_equal(Unit3{0.0, 0.0, -1.0}.unitVector(), factor.evaluateError(origin, state0), tol)); + // TODO make these tests way less verbose! + // ideally I could find an initializer for Vector9 like: Vector9{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}' + // positions are the same, secondary state has velocity 1.0 in z, + const auto state0_err_origin = factor.evaluateError(origin, state0); + EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).z(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).z(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state0_err_origin).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state0_err_origin).y(), tol)); + EXPECT(assert_equal(1.0, NavState::dV(state0_err_origin).z(), tol)); // same velocities, different position // second state agrees with initial state + velocity * dt - EXPECT(assert_equal(Unit3{0.0, 0.0, 0.0}.unitVector(), factor.evaluateError(state0, state1), tol)); + const auto state1_err_state0 = factor.evaluateError(state0, state1); + EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).z(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).z(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).z(), tol)); // both bodies have the same velocity, - // but state2.pose() != state0.pose() + state0.velocity * dt + // but state2.pose() does not agree with .update() // error comes from this pose difference - EXPECT(assert_equal(Unit3{0.0, 0.0, 1.0}.unitVector(), factor.evaluateError(state0, state2), tol)); + const auto state2_err_state0 = factor.evaluateError(state0, state2); + EXPECT(assert_equal(0.0, NavState::dP(state2_err_state0).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dP(state2_err_state0).y(), tol)); + EXPECT(assert_equal(1.0, NavState::dP(state2_err_state0).z(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).z(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).x(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).y(), tol)); + EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).z(), tol)); } /* ************************************************************************* */ From df76b3b51f4fcfbd4498c9e5423a4f2732a8fa4a Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:12:08 -0800 Subject: [PATCH 046/106] remove AAA style for Dellaert style on simpler types --- gtsam/navigation/tests/testConstantVelocityFactor.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp index cdf05b253..ca731f404 100644 --- a/gtsam/navigation/tests/testConstantVelocityFactor.cpp +++ b/gtsam/navigation/tests/testConstantVelocityFactor.cpp @@ -32,13 +32,12 @@ TEST(ConstantVelocityFactor, VelocityFactor) { using namespace gtsam; - const auto tol = double{1e-5}; + const double tol{1e-5}; - const auto x1 = Key{1}; - const auto x2 = Key{2}; + const Key x1 = Key{1}; + const Key x2 = Key{2}; - const auto dt = double{1.0}; - const auto mu = double{1000}; + const double dt{1.0}; // moving upward with groundtruth velocity" const auto origin = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 0.0}}, Velocity3{0.0, 0.0, 0.0}}; @@ -49,6 +48,7 @@ TEST(ConstantVelocityFactor, VelocityFactor) { const auto state2 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}}; + const double mu{1000}; const auto noise_model = noiseModel::Constrained::All(3, mu); const auto factor = ConstantVelocityFactor(x1, x2, dt, noise_model); From 34a7ce0d0d263a01b4d2216c10e6909a76d055a6 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:35:29 -0800 Subject: [PATCH 047/106] rename some vars for consistency --- gtsam/navigation/NavState.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 0181ea45f..f21a2f660 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -136,12 +136,12 @@ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { Matrix3 D_dR_R, D_dt_R, D_dv_R; const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); - const Point3 dt = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); - const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); + const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); + const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); Vector9 xi; Matrix3 D_xi_R; - xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv; + xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; if (H1) { *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // D_dt_R, -I_3x3, Z_3x3, // From d7c565d88533c415d392f908d17f6010f3011a11 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:37:10 -0800 Subject: [PATCH 048/106] static const for placeholder vars --- gtsam/navigation/ConstantVelocityFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index f676108ce..f5a35947f 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -35,8 +35,8 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { // only used to use update() below - const Vector3 b_accel{0.0, 0.0, 0.0}; - const Vector3 b_omega{0.0, 0.0, 0.0}; + static const Vector3 b_accel{0.0, 0.0, 0.0}; + static const Vector3 b_omega{0.0, 0.0, 0.0}; Matrix predicted_H_x1; NavState predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {}); From 78b92e0fbc675b4c61b48f93e28b60d7cacfe22c Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:38:45 -0800 Subject: [PATCH 049/106] fixup noisemodel so its correct dimension --- gtsam/navigation/tests/testConstantVelocityFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp index ca731f404..5bc2599c3 100644 --- a/gtsam/navigation/tests/testConstantVelocityFactor.cpp +++ b/gtsam/navigation/tests/testConstantVelocityFactor.cpp @@ -49,7 +49,7 @@ TEST(ConstantVelocityFactor, VelocityFactor) { const auto state2 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}}; const double mu{1000}; - const auto noise_model = noiseModel::Constrained::All(3, mu); + const auto noise_model = noiseModel::Constrained::All(9, mu); const auto factor = ConstantVelocityFactor(x1, x2, dt, noise_model); From aacba70562e2d93ac35ded75207a143c6e2ef5c6 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:50:42 -0800 Subject: [PATCH 050/106] cleanup some cruft, add license text --- gtsam/navigation/ConstantVelocityFactor.h | 20 ++++++++++++++++--- .../tests/testConstantVelocityFactor.cpp | 5 ----- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index f5a35947f..028026f7d 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -1,6 +1,20 @@ -#include -#include -#include +/* ---------------------------------------------------------------------------- + + * 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 NonlinearFactor.h + * @brief Non-linear factor base classes + * @author Asa Hammond + */ + #include #include diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp index 5bc2599c3..5c32fe738 100644 --- a/gtsam/navigation/tests/testConstantVelocityFactor.cpp +++ b/gtsam/navigation/tests/testConstantVelocityFactor.cpp @@ -12,20 +12,15 @@ /** * @file testConstantVelocityFactor.cpp * @brief Unit test for ConstantVelocityFactor - * @author Alex Cunningham * @author Asa Hammond */ #include -#include #include #include #include -#include #include - -#include #include /* ************************************************************************* */ From 68cdb417060f2e999bc7db330b94573febd4cb75 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 25 Feb 2021 18:55:19 -0800 Subject: [PATCH 051/106] corrected license header --- gtsam/navigation/ConstantVelocityFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 028026f7d..6cb349869 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearFactor.h - * @brief Non-linear factor base classes + * @file ConstantVelocityFactor.h + * @brief Maintain a constant velocity motion model between two NavStates * @author Asa Hammond */ From 06e6aa918f1c499bff50127e22ef0183b655e14d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 26 Feb 2021 08:25:28 -0500 Subject: [PATCH 052/106] 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 f48a2f6273e964ec0dda99f57dcdbd7b179d74a6 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Fri, 26 Feb 2021 07:11:21 -0800 Subject: [PATCH 053/106] test cleanup --- .../tests/testConstantVelocityFactor.cpp | 45 ++++++------------- 1 file changed, 14 insertions(+), 31 deletions(-) diff --git a/gtsam/navigation/tests/testConstantVelocityFactor.cpp b/gtsam/navigation/tests/testConstantVelocityFactor.cpp index 5c32fe738..540c27eab 100644 --- a/gtsam/navigation/tests/testConstantVelocityFactor.cpp +++ b/gtsam/navigation/tests/testConstantVelocityFactor.cpp @@ -43,51 +43,34 @@ TEST(ConstantVelocityFactor, VelocityFactor) { const auto state2 = NavState{Pose3{Rot3::Yaw(0), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}}; + const auto state3 = NavState{Pose3{Rot3::Yaw(M_PI_2), Point3{0.0, 0.0, 2.0}}, Velocity3{0.0, 0.0, 1.0}}; + const double mu{1000}; const auto noise_model = noiseModel::Constrained::All(9, mu); const auto factor = ConstantVelocityFactor(x1, x2, dt, noise_model); - // TODO make these tests way less verbose! - // ideally I could find an initializer for Vector9 like: Vector9{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}' // positions are the same, secondary state has velocity 1.0 in z, const auto state0_err_origin = factor.evaluateError(origin, state0); - EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state0_err_origin).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state0_err_origin).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state0_err_origin).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state0_err_origin).y(), tol)); - EXPECT(assert_equal(1.0, NavState::dV(state0_err_origin).z(), tol)); + EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0).finished(), state0_err_origin, tol)); // same velocities, different position // second state agrees with initial state + velocity * dt const auto state1_err_state0 = factor.evaluateError(state0, state1); - EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state1_err_state0).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state1_err_state0).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state1_err_state0).z(), tol)); + EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(), state1_err_state0, tol)); + + // same velocities, same position, different rotations + // second state agrees with initial state + velocity * dt + // as we assume that omega is 0.0 this is the same as the above case + // TODO: this should respect omega and actually fail in this case + const auto state3_err_state2 = factor.evaluateError(state0, state1); + EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(), state3_err_state2, tol)); // both bodies have the same velocity, - // but state2.pose() does not agree with .update() - // error comes from this pose difference + // but state2.pose() does not agree with state0.update() + // error comes from this position difference const auto state2_err_state0 = factor.evaluateError(state0, state2); - EXPECT(assert_equal(0.0, NavState::dP(state2_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dP(state2_err_state0).y(), tol)); - EXPECT(assert_equal(1.0, NavState::dP(state2_err_state0).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dR(state2_err_state0).z(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).x(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).y(), tol)); - EXPECT(assert_equal(0.0, NavState::dV(state2_err_state0).z(), tol)); + EXPECT(assert_equal((Vector9() << 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0).finished(), state2_err_state0, tol)); } /* ************************************************************************* */ From 7caaf69ae58af3314c2ea649d8b5bd6470634d1d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 26 Feb 2021 10:44:54 -0500 Subject: [PATCH 054/106] 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 055/106] 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 056/106] 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 057/106] 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 058/106] 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 059/106] 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 060/106] 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 061/106] 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 062/106] 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 063/106] 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 064/106] 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 065/106] 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 a2f99ac71cbaad5b92dad26ff2f72c4ca872a7ca Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Fri, 5 Mar 2021 07:21:38 +0000 Subject: [PATCH 066/106] unit test + fix segfault --- gtsam/sfm/TranslationRecovery.cpp | 10 ++++++++++ tests/testTranslationRecovery.cpp | 29 +++++++++++++++++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index d4100b00a..7f788ff84 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -81,6 +81,7 @@ void TranslationRecovery::addPrior( const double scale, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); + if(edge == relativeTranslations_.end()) return; graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), priorNoiseModel); graph->emplace_shared >( @@ -102,6 +103,15 @@ Values TranslationRecovery::initalizeRandomly() const { insert(edge.key1()); insert(edge.key2()); } + + // If there are no valid edges, but zero-distance edges exist, initialize one of + // the nodes in a connected component of zero-distance edges. + if(initial.empty() && !sameTranslationNodes_.empty()){ + for(const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { + Key optimizedKey = optimizedAndDuplicateKeys.first; + initial.insert(optimizedKey, Point3(0, 0, 0)); + } + } return initial; } diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 7260fd5af..9a4e2b545 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -238,6 +238,35 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) { EXPECT(assert_equal(Point3(2, -2, 0), result.at(3))); } +TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { + Values poses; + poses.insert(0, Pose3(Rot3::RzRyRx(-M_PI / 6, 0, 0), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(2, Pose3(Rot3::RzRyRx(M_PI / 6, 0, 0), Point3(0, 0, 0))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}, {2, 0}}); + + // Check simulated measurements. + for (auto& unitTranslation : relativeTranslations) { + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); + } + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + // Graph size will be zero as there no 'non-zero distance' edges. + EXPECT_LONGS_EQUAL(0, graph.size()); + + // Run translation recovery + const auto result = algorithm.run(/*scale=*/4.0); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(1))); + EXPECT(assert_equal(Point3(0, 0, 0), result.at(2))); +} + + /* ************************************************************************* */ int main() { TestResult tr; From 759dcaa1dd9bd799c6a531561563acfa9ae5032b Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Fri, 5 Mar 2021 00:06:30 -0800 Subject: [PATCH 067/106] formatting changes --- gtsam/sfm/TranslationRecovery.cpp | 10 +++++----- tests/testTranslationRecovery.cpp | 17 ++++++++--------- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 7f788ff84..f38c14ba7 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -81,7 +81,7 @@ void TranslationRecovery::addPrior( const double scale, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); - if(edge == relativeTranslations_.end()) return; + if (edge == relativeTranslations_.end()) return; graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), priorNoiseModel); graph->emplace_shared >( @@ -104,10 +104,10 @@ Values TranslationRecovery::initalizeRandomly() const { insert(edge.key2()); } - // If there are no valid edges, but zero-distance edges exist, initialize one of - // the nodes in a connected component of zero-distance edges. - if(initial.empty() && !sameTranslationNodes_.empty()){ - for(const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { + // If there are no valid edges, but zero-distance edges exist, initialize one + // of the nodes in a connected component of zero-distance edges. + if (initial.empty() && !sameTranslationNodes_.empty()) { + for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { Key optimizedKey = optimizedAndDuplicateKeys.first; initial.insert(optimizedKey, Point3(0, 0, 0)); } diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 9a4e2b545..2915a375e 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -17,7 +17,6 @@ */ #include - #include #include @@ -185,7 +184,7 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) { TranslationRecovery algorithm(relativeTranslations); const auto graph = algorithm.buildGraph(); // There is only 1 non-zero translation edge. - EXPECT_LONGS_EQUAL(1, graph.size()); + EXPECT_LONGS_EQUAL(1, graph.size()); // Run translation recovery const auto result = algorithm.run(/*scale=*/3.0); @@ -244,28 +243,28 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) { poses.insert(1, Pose3(Rot3(), Point3(0, 0, 0))); poses.insert(2, Pose3(Rot3::RzRyRx(M_PI / 6, 0, 0), Point3(0, 0, 0))); - auto relativeTranslations = TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}, {2, 0}}); + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {1, 2}, {2, 0}}); // Check simulated measurements. for (auto& unitTranslation : relativeTranslations) { - EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), - unitTranslation.measured())); + EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation), + unitTranslation.measured())); } TranslationRecovery algorithm(relativeTranslations); const auto graph = algorithm.buildGraph(); - // Graph size will be zero as there no 'non-zero distance' edges. + // Graph size will be zero as there no 'non-zero distance' edges. EXPECT_LONGS_EQUAL(0, graph.size()); - + // Run translation recovery const auto result = algorithm.run(/*scale=*/4.0); - + // Check result EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); EXPECT(assert_equal(Point3(0, 0, 0), result.at(1))); EXPECT(assert_equal(Point3(0, 0, 0), result.at(2))); } - /* ************************************************************************* */ int main() { From 5d6b9c95160c0fe01bc7dabb54d3f8afbfa838c8 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 5 Mar 2021 12:57:12 -0500 Subject: [PATCH 068/106] Use brew's boost, instead of Fan's version --- .github/workflows/build-python.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 3f9a2e98a..345af5831 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -88,7 +88,7 @@ jobs: run: | brew tap ProfFan/robotics brew install cmake ninja - brew install ProfFan/robotics/boost + brew install boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV From 4d079e19054fc614b4428fb32f06d2bd607be42b Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 5 Mar 2021 14:17:00 -0500 Subject: [PATCH 069/106] 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 070/106] 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 071/106] 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 072/106] 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 c5eb449e08022363a431c507f7ca5410190a195b Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 8 Mar 2021 14:58:50 -0500 Subject: [PATCH 073/106] Squashed 'wrap/' changes from b28b3570d..d37b8a972 d37b8a972 Merge pull request #31 from ayushbaid/feature/pickle efd4a0fb4 removing tab 42fd231f3 adding newline for test compare 0aa316150 removing extra brackets 7fe1d7d0f adding pickle code to expected file 9c3ab7a8b fixing string format for new classname 2f89284e8 moving pickle support with the serialization code 5a8abc916 adding pickle support for select classes using serialization git-subtree-dir: wrap git-subtree-split: d37b8a972f6f3aa99a657831102fc22a73b59f21 --- gtwrap/pybind_wrapper.py | 13 ++++++++++++- tests/expected-python/geometry_pybind.cpp | 20 ++++++++++++++++++++ 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/gtwrap/pybind_wrapper.py b/gtwrap/pybind_wrapper.py index c0e88e37a..ec5480785 100755 --- a/gtwrap/pybind_wrapper.py +++ b/gtwrap/pybind_wrapper.py @@ -75,7 +75,17 @@ class PybindWrapper(object): []({class_inst} self, string serialized){{ gtsam::deserialize(serialized, *self); }}, py::arg("serialized")) - '''.format(class_inst=cpp_class + '*')) + .def(py::pickle( + [](const {cpp_class} &a){{ // __getstate__ + /* Returns a string that encodes the state of the object */ + return py::make_tuple(gtsam::serialize(a)); + }}, + [](py::tuple t){{ // __setstate__ + {cpp_class} obj; + gtsam::deserialize(t[0].cast(), obj); + return obj; + }})) + '''.format(class_inst=cpp_class + '*', cpp_class=cpp_class)) is_method = isinstance(method, instantiator.InstantiatedMethod) is_static = isinstance(method, parser.StaticMethod) @@ -318,3 +328,4 @@ class PybindWrapper(object): wrapped_namespace=wrapped_namespace, boost_class_export=boost_class_export, ) + diff --git a/tests/expected-python/geometry_pybind.cpp b/tests/expected-python/geometry_pybind.cpp index 3eee55bf4..53d33eabf 100644 --- a/tests/expected-python/geometry_pybind.cpp +++ b/tests/expected-python/geometry_pybind.cpp @@ -47,6 +47,16 @@ PYBIND11_MODULE(geometry_py, m_) { [](gtsam::Point2* self, string serialized){ gtsam::deserialize(serialized, *self); }, py::arg("serialized")) +.def(py::pickle( + [](const gtsam::Point2 &a){ // __getstate__ + /* Returns a string that encodes the state of the object */ + return py::make_tuple(gtsam::serialize(a)); + }, + [](py::tuple t){ // __setstate__ + gtsam::Point2 obj; + gtsam::deserialize(t[0].cast(), obj); + return obj; + })) ; py::class_>(m_gtsam, "Point3") @@ -61,6 +71,16 @@ PYBIND11_MODULE(geometry_py, m_) { [](gtsam::Point3* self, string serialized){ gtsam::deserialize(serialized, *self); }, py::arg("serialized")) +.def(py::pickle( + [](const gtsam::Point3 &a){ // __getstate__ + /* Returns a string that encodes the state of the object */ + return py::make_tuple(gtsam::serialize(a)); + }, + [](py::tuple t){ // __setstate__ + gtsam::Point3 obj; + gtsam::deserialize(t[0].cast(), obj); + return obj; + })) .def_static("staticFunction",[](){return gtsam::Point3::staticFunction();}) .def_static("StaticFunctionRet",[]( double z){return gtsam::Point3::StaticFunctionRet(z);}, py::arg("z")); From afc9333a17167aed7a351d763e8b6fd06eb3bb3b Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 8 Mar 2021 19:53:26 -0500 Subject: [PATCH 074/106] Squashed 'wrap/' changes from d37b8a972..10e1efd6f 10e1efd6f Merge pull request #32 from ayushbaid/feature/pickle 55d5d7fbe ignoring pickle in matlab wrap 8d70c7fe2 adding newlines dee8aaee3 adding markers for pickling 46fc45d82 separating out the marker for pickle git-subtree-dir: wrap git-subtree-split: 10e1efd6f25f76b774868b3da33cb3954008b233 --- gtwrap/matlab_wrapper.py | 12 ++++++++++++ gtwrap/pybind_wrapper.py | 7 ++++++- tests/expected-python/geometry_pybind.cpp | 2 ++ tests/geometry.h | 6 ++++++ 4 files changed, 26 insertions(+), 1 deletion(-) diff --git a/gtwrap/matlab_wrapper.py b/gtwrap/matlab_wrapper.py index 669bf474f..fe4ee7e19 100755 --- a/gtwrap/matlab_wrapper.py +++ b/gtwrap/matlab_wrapper.py @@ -49,6 +49,8 @@ class MatlabWrapper(object): } """Methods that should not be wrapped directly""" whitelist = ['serializable', 'serialize'] + """Methods that should be ignored""" + ignore_methods = ['pickle'] """Datatypes that do not need to be checked in methods""" not_check_type = [] """Data types that are primitive types""" @@ -563,6 +565,8 @@ class MatlabWrapper(object): for method in methods: if method.name in self.whitelist: continue + if method.name in self.ignore_methods: + continue comment += '%{name}({args})'.format(name=method.name, args=self._wrap_args(method.args)) @@ -612,6 +616,9 @@ class MatlabWrapper(object): methods = self._group_methods(methods) for method in methods: + if method in self.ignore_methods: + continue + if globals: self._debug("[wrap_methods] wrapping: {}..{}={}".format(method[0].parent.name, method[0].name, type(method[0].parent.name))) @@ -861,6 +868,8 @@ class MatlabWrapper(object): method_name = method[0].name if method_name in self.whitelist and method_name != 'serialize': continue + if method_name in self.ignore_methods: + continue if method_name == 'serialize': serialize[0] = True @@ -932,6 +941,9 @@ class MatlabWrapper(object): format_name = list(static_method[0].name) format_name[0] = format_name[0].upper() + if static_method[0].name in self.ignore_methods: + continue + method_text += textwrap.indent(textwrap.dedent('''\ function varargout = {name}(varargin) '''.format(name=''.join(format_name))), diff --git a/gtwrap/pybind_wrapper.py b/gtwrap/pybind_wrapper.py index ec5480785..a045afcbd 100755 --- a/gtwrap/pybind_wrapper.py +++ b/gtwrap/pybind_wrapper.py @@ -75,6 +75,11 @@ class PybindWrapper(object): []({class_inst} self, string serialized){{ gtsam::deserialize(serialized, *self); }}, py::arg("serialized")) + '''.format(class_inst=cpp_class + '*')) + if cpp_method == "pickle": + if not cpp_class in self._serializing_classes: + raise ValueError("Cannot pickle a class which is not serializable") + return textwrap.dedent(''' .def(py::pickle( [](const {cpp_class} &a){{ // __getstate__ /* Returns a string that encodes the state of the object */ @@ -85,7 +90,7 @@ class PybindWrapper(object): gtsam::deserialize(t[0].cast(), obj); return obj; }})) - '''.format(class_inst=cpp_class + '*', cpp_class=cpp_class)) + '''.format(cpp_class=cpp_class)) is_method = isinstance(method, instantiator.InstantiatedMethod) is_static = isinstance(method, parser.StaticMethod) diff --git a/tests/expected-python/geometry_pybind.cpp b/tests/expected-python/geometry_pybind.cpp index 53d33eabf..be6482d89 100644 --- a/tests/expected-python/geometry_pybind.cpp +++ b/tests/expected-python/geometry_pybind.cpp @@ -47,6 +47,7 @@ PYBIND11_MODULE(geometry_py, m_) { [](gtsam::Point2* self, string serialized){ gtsam::deserialize(serialized, *self); }, py::arg("serialized")) + .def(py::pickle( [](const gtsam::Point2 &a){ // __getstate__ /* Returns a string that encodes the state of the object */ @@ -71,6 +72,7 @@ PYBIND11_MODULE(geometry_py, m_) { [](gtsam::Point3* self, string serialized){ gtsam::deserialize(serialized, *self); }, py::arg("serialized")) + .def(py::pickle( [](const gtsam::Point3 &a){ // __getstate__ /* Returns a string that encodes the state of the object */ diff --git a/tests/geometry.h b/tests/geometry.h index 40d878c9f..ec5d3b277 100644 --- a/tests/geometry.h +++ b/tests/geometry.h @@ -22,6 +22,9 @@ class Point2 { VectorNotEigen vectorConfusion(); void serializable() const; // Sets flag and creates export, but does not make serialization functions + + // enable pickling in python + void pickle() const; }; #include @@ -35,6 +38,9 @@ class Point3 { // enabling serialization functionality void serialize() const; // Just triggers a flag internally and removes actual function + + // enable pickling in python + void pickle() const; }; } From 1670e68e2f5548722837072a7176128e23fe492b Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 8 Mar 2021 20:18:09 -0500 Subject: [PATCH 075/106] enabling markers and testing pickle roundtrip for few classes --- gtsam/gtsam.i | 100 ++++++++++++++++++++++++++++++ python/gtsam/tests/test_pickle.py | 46 ++++++++++++++ python/gtsam/utils/test_case.py | 12 ++++ 3 files changed, 158 insertions(+) create mode 100644 python/gtsam/tests/test_pickle.py diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 22c2cc17d..85209ddfa 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -105,6 +105,9 @@ * virtual class MyFactor : gtsam::NoiseModelFactor {...}; * - *DO NOT* re-define overriden function already declared in the external (forward-declared) base class * - This will cause an ambiguity problem in Pybind header file + * Pickle support in Python: + * - Add "void pickle()" to a class to enable pickling via gtwrap. In the current implementation, "void serialize()" + * and a public constructor with no-arguments in needed for successful build. */ /** @@ -144,6 +147,9 @@ class KeyList { void remove(size_t key); void serialize() const; + + // enable pickling in python + void pickle() const; }; // Actually a FastSet @@ -169,6 +175,9 @@ class KeySet { bool count(size_t key) const; // returns true if value exists void serialize() const; + + // enable pickling in python + void pickle() const; }; // Actually a vector @@ -190,6 +199,9 @@ class KeyVector { void push_back(size_t key) const; void serialize() const; + + // enable pickling in python + void pickle() const; }; // Actually a FastMap @@ -361,6 +373,9 @@ class Point2 { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; // std::vector @@ -422,6 +437,9 @@ class StereoPoint2 { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -446,6 +464,9 @@ class Point3 { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -491,6 +512,9 @@ class Rot2 { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -653,6 +677,9 @@ class Rot3 { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -708,6 +735,9 @@ class Pose2 { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -764,6 +794,9 @@ class Pose3 { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; // std::vector @@ -797,6 +830,15 @@ class Unit3 { size_t dim() const; gtsam::Unit3 retract(Vector v) const; Vector localCoordinates(const gtsam::Unit3& s) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + // enabling function to compare objects + bool equals(const gtsam::Unit3& expected, double tol) const; }; #include @@ -856,6 +898,9 @@ class Cal3_S2 { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -884,6 +929,9 @@ virtual class Cal3DS2_Base { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -905,6 +953,9 @@ virtual class Cal3DS2 : gtsam::Cal3DS2_Base { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -931,6 +982,9 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -988,6 +1042,9 @@ class Cal3Bundler { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -1018,6 +1075,9 @@ class CalibratedCamera { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -1056,6 +1116,9 @@ class PinholeCamera { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; // Forward declaration of PinholeCameraCalX is defined here. @@ -1103,6 +1166,9 @@ class StereoCamera { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -1557,6 +1623,9 @@ class VectorValues { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -1618,6 +1687,9 @@ virtual class JacobianFactor : gtsam::GaussianFactor { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -1649,6 +1721,9 @@ virtual class HessianFactor : gtsam::GaussianFactor { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -1728,6 +1803,9 @@ class GaussianFactorGraph { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -2033,6 +2111,9 @@ class Ordering { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -2071,6 +2152,10 @@ class NonlinearFactorGraph { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; + void saveGraph(const string& s) const; }; @@ -2128,6 +2213,9 @@ class Values { // enabling serialization functionality void serialize() const; + // enable pickling in python + void pickle() const; + // New in 4.0, we have to specialize every insert/update/at to generate wrappers // Instead of the old: // void insert(size_t j, const gtsam::Value& value); @@ -2527,6 +2615,9 @@ virtual class PriorFactor : gtsam::NoiseModelFactor { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; @@ -2538,6 +2629,9 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { // enabling serialization functionality void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -2774,6 +2868,9 @@ class SfmTrack { // enabling serialization functionality void serialize() const; + // enable pickling in python + void pickle() const; + // enabling function to compare objects bool equals(const gtsam::SfmTrack& expected, double tol) const; }; @@ -2790,6 +2887,9 @@ class SfmData { // enabling serialization functionality void serialize() const; + // enable pickling in python + void pickle() const; + // enabling function to compare objects bool equals(const gtsam::SfmData& expected, double tol) const; }; diff --git a/python/gtsam/tests/test_pickle.py b/python/gtsam/tests/test_pickle.py new file mode 100644 index 000000000..0acbf6765 --- /dev/null +++ b/python/gtsam/tests/test_pickle.py @@ -0,0 +1,46 @@ +""" +GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests to check pickling. + +Author: Ayush Baid +""" +from gtsam import Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3, SfmTrack, Unit3 + +from gtsam.utils.test_case import GtsamTestCase + +class TestPickle(GtsamTestCase): + """Tests pickling on some of the classes.""" + + def test_cal3Bundler_roundtrip(self): + obj = Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70) + self.assertEqualityOnPickleRoundtrip(obj) + + def test_pinholeCameraCal3Bundler_roundtrip(self): + obj = PinholeCameraCal3Bundler( + Pose3(Rot3.RzRyRx(0, 0.1, -0.05), Point3(1, 1, 0)), + Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70), + ) + self.assertEqualityOnPickleRoundtrip(obj) + + def test_rot3_roundtrip(self): + obj = Rot3.RzRyRx(0, 0.05, 0.1) + self.assertEqualityOnPickleRoundtrip(obj) + + def test_pose3_roundtrip(self): + obj = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)) + self.assertEqualityOnPickleRoundtrip(obj) + + def test_sfmTrack_roundtrip(self): + obj = SfmTrack(Point3(1, 1, 0)) + obj.add_measurement(0, Point2(-1, 5)) + obj.add_measurement(1, Point2(6, 2)) + self.assertEqualityOnPickleRoundtrip(obj) + + def test_unit3_roundtrip(self): + obj = Unit3(Point3(1, 1, 0)) + self.assertEqualityOnPickleRoundtrip(obj) diff --git a/python/gtsam/utils/test_case.py b/python/gtsam/utils/test_case.py index 3effd7f65..50af004f4 100644 --- a/python/gtsam/utils/test_case.py +++ b/python/gtsam/utils/test_case.py @@ -8,6 +8,7 @@ See LICENSE for the license information TestCase class with GTSAM assert utils. Author: Frank Dellaert """ +import pickle import unittest @@ -29,3 +30,14 @@ class GtsamTestCase(unittest.TestCase): if not equal: raise self.failureException( "Values are not equal:\n{}!={}".format(actual, expected)) + + def assertEqualityOnPickleRoundtrip(self, obj: object, tol=1e-9) -> None: + """ Performs a round-trip using pickle and asserts equality. + + Usage: + self.assertEqualityOnPickleRoundtrip(obj) + Keyword Arguments: + tol {float} -- tolerance passed to 'equals', default 1e-9 + """ + roundTripObj = pickle.loads(pickle.dumps(obj)) + self.gtsamAssertEquals(roundTripObj, obj) From 6f81bdea7b078ff447a520544f04e0c2606344e3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Mar 2021 15:07:15 -0500 Subject: [PATCH 076/106] offload matlab wrapping to the wrap project --- cmake/CMakeLists.txt | 2 - cmake/GtsamMatlabWrap.cmake | 452 ++---------------------------------- python/CMakeLists.txt | 2 + 3 files changed, 19 insertions(+), 437 deletions(-) diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt index 451ca38a4..6e912dd63 100644 --- a/cmake/CMakeLists.txt +++ b/cmake/CMakeLists.txt @@ -22,5 +22,3 @@ install(FILES FindNumPy.cmake README.html DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools") - - diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index 993af47c4..a4085fd14 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -1,26 +1,5 @@ -# Check / set dependent variables for MATLAB wrapper -if(GTSAM_INSTALL_MATLAB_TOOLBOX) - find_package(Matlab COMPONENTS MEX_COMPILER REQUIRED) - if(NOT Matlab_MEX_COMPILER) - message(FATAL_ERROR "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well.") - endif() - - if(GTSAM_BUILD_TYPE_POSTFIXES) - set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) - endif() - - if(NOT BUILD_SHARED_LIBS) - message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") - endif() -endif() - -# Fixup the Python paths -if(GTWRAP_DIR) - # packaged - set(GTWRAP_PACKAGE_DIR ${GTWRAP_DIR}) -else() - set(GTWRAP_PACKAGE_DIR ${CMAKE_SOURCE_DIR}/wrap) -endif() +# Set the wrapping script variable +set(MATLAB_WRAP_SCRIPT "${GTSAM_SOURCE_DIR}/wrap/scripts/matlab_wrap.py") # Set up cache options option(GTSAM_MEX_BUILD_STATIC_MODULE "Build MATLAB wrapper statically (increases build time)" OFF) @@ -30,419 +9,22 @@ if(NOT GTSAM_TOOLBOX_INSTALL_PATH) set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox") endif() -# GTSAM_MEX_BUILD_STATIC_MODULE is not for Windows - on Windows any static -# are already compiled into the library by the linker -if(GTSAM_MEX_BUILD_STATIC_MODULE AND WIN32) - message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).") +set(WRAP_MEX_BUILD_STATIC_MODULE ${GTSAM_MEX_BUILD_STATIC_MODULE}) +set(WRAP_BUILD_MEX_BINARY_FLAGS ${GTSAM_BUILD_MEX_BINARY_FLAGS}) +set(WRAP_TOOLBOX_INSTALL_PATH ${GTSAM_TOOLBOX_INSTALL_PATH}) +set(WRAP_CUSTOM_MATLAB_PATH ${GTSAM_CUSTOM_MATLAB_PATH}) +set(WRAP_BUILD_TYPE_POSTFIXES ${GTSAM_BUILD_TYPE_POSTFIXES}) + +# Fixup the Python paths +if(GTWRAP_DIR) + # packaged + set(GTWRAP_PACKAGE_DIR ${GTWRAP_DIR}) +else() + set(GTWRAP_PACKAGE_DIR ${GTSAM_SOURCE_DIR}/wrap) endif() -set(MEX_COMMAND ${Matlab_MEX_COMPILER} CACHE PATH "Path to MATLAB MEX compiler") -set(MATLAB_ROOT ${Matlab_ROOT_DIR} CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)") +include(MatlabWrap) -# Try to automatically configure mex path from provided custom `bin` path. -if(GTSAM_CUSTOM_MATLAB_PATH) - set(matlab_bin_directory ${GTSAM_CUSTOM_MATLAB_PATH}) - - if(WIN32) - set(mex_program_name "mex.bat") - else() - set(mex_program_name "mex") - endif() - - # Run find_program explicitly putting $PATH after our predefined program - # directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents - # finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is - # on the system path. - find_program(MEX_COMMAND ${mex_program_name} - PATHS ${matlab_bin_directory} ENV PATH - NO_DEFAULT_PATH) - - mark_as_advanced(FORCE MEX_COMMAND) - # Now that we have mex, trace back to find the Matlab installation root - get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH) - get_filename_component(mex_path "${MEX_COMMAND}" PATH) - if(mex_path MATCHES ".*/win64$") - get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE) - else() - get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE) - endif() +if(NOT BUILD_SHARED_LIBS) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") endif() - - -# User-friendly wrapping function. Builds a mex module from the provided -# interfaceHeader. For example, for the interface header gtsam.h, -# this will build the wrap module 'gtsam'. -# -# Arguments: -# -# interfaceHeader: The relative path to the wrapper interface definition file. -# linkLibraries: Any *additional* libraries to link. Your project library -# (e.g. `lba`), libraries it depends on, and any necessary -# MATLAB libraries will be linked automatically. So normally, -# leave this empty. -# extraIncludeDirs: Any *additional* include paths required by dependent -# libraries that have not already been added by -# include_directories. Again, normally, leave this empty. -# extraMexFlags: Any *additional* flags to pass to the compiler when building -# the wrap code. Normally, leave this empty. -function(wrap_and_install_library interfaceHeader linkLibraries extraIncludeDirs extraMexFlags) - wrap_library_internal("${interfaceHeader}" "${linkLibraries}" "${extraIncludeDirs}" "${mexFlags}") - install_wrapped_library_internal("${interfaceHeader}") -endfunction() - - -# Internal function that wraps a library and compiles the wrapper -function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs extraMexFlags) - if(UNIX AND NOT APPLE) - if(CMAKE_SIZEOF_VOID_P EQUAL 8) - set(mexModuleExt mexa64) - else() - set(mexModuleExt mexglx) - endif() - elseif(APPLE) - set(mexModuleExt mexmaci64) - elseif(MSVC) - if(CMAKE_CL_64) - set(mexModuleExt mexw64) - else() - set(mexModuleExt mexw32) - endif() - endif() - - # Wrap codegen interface - #usage: wrap interfacePath moduleName toolboxPath headerPath - # interfacePath : *absolute* path to directory of module interface file - # moduleName : the name of the module, interface file must be called moduleName.h - # toolboxPath : the directory in which to generate the wrappers - # headerPath : path to matlab.h - - # Extract module name from interface header file name - get_filename_component(interfaceHeader "${interfaceHeader}" ABSOLUTE) - get_filename_component(modulePath "${interfaceHeader}" PATH) - get_filename_component(moduleName "${interfaceHeader}" NAME_WE) - - # Paths for generated files - set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}") - set(generated_cpp_file "${generated_files_path}/${moduleName}_wrapper.cpp") - set(compiled_mex_modules_root "${PROJECT_BINARY_DIR}/wrap/${moduleName}_mex") - - message(STATUS "Building wrap module ${moduleName}") - - # Find matlab.h in GTSAM - if(("${PROJECT_NAME}" STREQUAL "gtsam") OR - ("${PROJECT_NAME}" STREQUAL "gtsam_unstable")) - set(matlab_h_path "${PROJECT_SOURCE_DIR}") - else() - if(NOT GTSAM_INCLUDE_DIR) - message(FATAL_ERROR "You must call find_package(GTSAM) before using wrap") - endif() - list(GET GTSAM_INCLUDE_DIR 0 installed_includes_path) - set(matlab_h_path "${installed_includes_path}/wrap") - endif() - - # If building a static mex module, add all cmake-linked libraries to the - # explicit link libraries list so that the next block of code can unpack - # any static libraries - set(automaticDependencies "") - foreach(lib ${moduleName} ${linkLibraries}) - #message("MODULE NAME: ${moduleName}") - if(TARGET "${lib}") - get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES) - # message("DEPENDENT LIBRARIES: ${dependentLibraries}") - if(dependentLibraries) - list(APPEND automaticDependencies ${dependentLibraries}) - endif() - endif() - endforeach() - - ## CHRIS: Temporary fix. On my system the get_target_property above returned Not-found for gtsam module - ## This needs to be fixed!! - if(UNIX AND NOT APPLE) - list(APPEND automaticDependencies ${Boost_SERIALIZATION_LIBRARY_RELEASE} ${Boost_FILESYSTEM_LIBRARY_RELEASE} - ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE}) - if(Boost_TIMER_LIBRARY_RELEASE AND NOT GTSAM_DISABLE_NEW_TIMERS) # Only present in Boost >= 1.48.0 - list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} ${Boost_CHRONO_LIBRARY_RELEASE}) - if(GTSAM_MEX_BUILD_STATIC_MODULE) - #list(APPEND automaticDependencies -Wl,--no-as-needed -lrt) - endif() - endif() - endif() - - #message("AUTOMATIC DEPENDENCIES: ${automaticDependencies}") - ## CHRIS: End temporary fix - - # Separate dependencies - set(correctedOtherLibraries "") - set(otherLibraryTargets "") - set(otherLibraryNontargets "") - set(otherSourcesAndObjects "") - foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies}) - if(TARGET "${lib}") - if(GTSAM_MEX_BUILD_STATIC_MODULE) - get_target_property(target_sources ${lib} SOURCES) - list(APPEND otherSourcesAndObjects ${target_sources}) - else() - list(APPEND correctedOtherLibraries ${lib}) - list(APPEND otherLibraryTargets ${lib}) - endif() - else() - get_filename_component(file_extension "${lib}" EXT) - get_filename_component(lib_name "${lib}" NAME_WE) - if(file_extension STREQUAL ".a" AND GTSAM_MEX_BUILD_STATIC_MODULE) - # For building a static MEX module, unpack the static library - # and compile its object files into our module - file(MAKE_DIRECTORY "${generated_files_path}/${lib_name}_objects") - execute_process(COMMAND ar -x "${lib}" - WORKING_DIRECTORY "${generated_files_path}/${lib_name}_objects" - RESULT_VARIABLE ar_result) - if(NOT ar_result EQUAL 0) - message(FATAL_ERROR "Failed extracting ${lib}") - endif() - - # Get list of object files - execute_process(COMMAND ar -t "${lib}" - OUTPUT_VARIABLE object_files - RESULT_VARIABLE ar_result) - if(NOT ar_result EQUAL 0) - message(FATAL_ERROR "Failed listing ${lib}") - endif() - - # Add directory to object files - string(REPLACE "\n" ";" object_files_list "${object_files}") - foreach(object_file ${object_files_list}) - get_filename_component(file_extension "${object_file}" EXT) - if(file_extension STREQUAL ".o") - list(APPEND otherSourcesAndObjects "${generated_files_path}/${lib_name}_objects/${object_file}") - endif() - endforeach() - else() - list(APPEND correctedOtherLibraries ${lib}) - list(APPEND otherLibraryNontargets ${lib}) - endif() - endif() - endforeach() - - # Check libraries for conflicting versions built-in to MATLAB - set(dependentLibraries "") - if(NOT "${otherLibraryTargets}" STREQUAL "") - foreach(target ${otherLibraryTargets}) - get_target_property(dependentLibrariesOne ${target} INTERFACE_LINK_LIBRARIES) - list(APPEND dependentLibraries ${dependentLibrariesOne}) - endforeach() - endif() - list(APPEND dependentLibraries ${otherLibraryNontargets}) - check_conflicting_libraries_internal("${dependentLibraries}") - - # Set up generation of module source file - file(MAKE_DIRECTORY "${generated_files_path}") - - find_package(PythonInterp - ${GTSAM_PYTHON_VERSION} - EXACT - REQUIRED) - find_package(PythonLibs - ${GTSAM_PYTHON_VERSION} - EXACT - REQUIRED) - - - set(_ignore gtsam::Point2 - gtsam::Point3 - gtsam::BearingRangeFactor - gtsam::BearingRangeFactor2D - gtsam::BearingRangeFactorPose2) - - # set the matlab wrapping script variable - set(MATLAB_WRAP_SCRIPT "${GTSAM_SOURCE_DIR}/wrap/scripts/matlab_wrap.py") - - add_custom_command( - OUTPUT ${generated_cpp_file} - DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects} - COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} - ${MATLAB_WRAP_SCRIPT} - --src ${interfaceHeader} - --module_name ${moduleName} - --out ${generated_files_path} - --top_module_namespaces ${moduleName} - --ignore ${_ignore} - VERBATIM - WORKING_DIRECTORY ${generated_files_path}) - - # Set up building of mex module - string(REPLACE ";" " " extraMexFlagsSpaced "${extraMexFlags}") - string(REPLACE ";" " " mexFlagsSpaced "${GTSAM_BUILD_MEX_BINARY_FLAGS}") - add_library(${moduleName}_matlab_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects}) - target_link_libraries(${moduleName}_matlab_wrapper ${correctedOtherLibraries}) - target_link_libraries(${moduleName}_matlab_wrapper ${moduleName}) - set_target_properties(${moduleName}_matlab_wrapper PROPERTIES - OUTPUT_NAME "${moduleName}_wrapper" - PREFIX "" - SUFFIX ".${mexModuleExt}" - LIBRARY_OUTPUT_DIRECTORY "${compiled_mex_modules_root}" - ARCHIVE_OUTPUT_DIRECTORY "${compiled_mex_modules_root}" - RUNTIME_OUTPUT_DIRECTORY "${compiled_mex_modules_root}" - CLEAN_DIRECT_OUTPUT 1) - set_property(TARGET ${moduleName}_matlab_wrapper APPEND_STRING PROPERTY COMPILE_FLAGS " ${extraMexFlagsSpaced} ${mexFlagsSpaced} \"-I${MATLAB_ROOT}/extern/include\" -DMATLAB_MEX_FILE -DMX_COMPAT_32") - set_property(TARGET ${moduleName}_matlab_wrapper APPEND PROPERTY INCLUDE_DIRECTORIES ${extraIncludeDirs}) - # Disable build type postfixes for the mex module - we install in different directories for each build type instead - foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) - string(TOUPPER "${build_type}" build_type_upper) - set_target_properties(${moduleName}_matlab_wrapper PROPERTIES ${build_type_upper}_POSTFIX "") - endforeach() - # Set up platform-specific flags - if(MSVC) - if(CMAKE_CL_64) - set(mxLibPath "${MATLAB_ROOT}/extern/lib/win64/microsoft") - else() - set(mxLibPath "${MATLAB_ROOT}/extern/lib/win32/microsoft") - endif() - target_link_libraries(${moduleName}_matlab_wrapper "${mxLibPath}/libmex.lib" "${mxLibPath}/libmx.lib" "${mxLibPath}/libmat.lib") - set_target_properties(${moduleName}_matlab_wrapper PROPERTIES LINK_FLAGS "/export:mexFunction") - set_property(SOURCE "${generated_cpp_file}" APPEND PROPERTY COMPILE_FLAGS "/bigobj") - elseif(APPLE) - set(mxLibPath "${MATLAB_ROOT}/bin/maci64") - target_link_libraries(${moduleName}_matlab_wrapper "${mxLibPath}/libmex.dylib" "${mxLibPath}/libmx.dylib" "${mxLibPath}/libmat.dylib") - endif() - - # Hacking around output issue with custom command - # Deletes generated build folder - add_custom_target(wrap_${moduleName}_matlab_distclean - COMMAND cmake -E remove_directory ${generated_files_path} - COMMAND cmake -E remove_directory ${compiled_mex_modules_root}) -endfunction() - -# Internal function that installs a wrap toolbox -function(install_wrapped_library_internal interfaceHeader) - get_filename_component(moduleName "${interfaceHeader}" NAME_WE) - set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}") - - # NOTE: only installs .m and mex binary files (not .cpp) - the trailing slash on the directory name - # here prevents creating the top-level module name directory in the destination. - message(STATUS "Installing Matlab Toolbox to ${GTSAM_TOOLBOX_INSTALL_PATH}") - if(GTSAM_BUILD_TYPE_POSTFIXES) - foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) - string(TOUPPER "${build_type}" build_type_upper) - if(${build_type_upper} STREQUAL "RELEASE") - set(build_type_tag "") # Don't create release mode tag on installed directory - else() - set(build_type_tag "${build_type}") - endif() - # Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one - get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH) - get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME) - install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING PATTERN "*.m") - install(TARGETS ${moduleName}_matlab_wrapper - LIBRARY DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" - RUNTIME DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}") - endforeach() - else() - install(DIRECTORY "${generated_files_path}/" DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH} FILES_MATCHING PATTERN "*.m") - install(TARGETS ${moduleName}_matlab_wrapper - LIBRARY DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH} - RUNTIME DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}) - endif() -endfunction() - -# Internal function to check for libraries installed with MATLAB that may conflict -# and prints a warning to move them if problems occur. -function(check_conflicting_libraries_internal libraries) - if(UNIX) - # Set path for matlab's built-in libraries - if(APPLE) - set(mxLibPath "${MATLAB_ROOT}/bin/maci64") - else() - if(CMAKE_CL_64) - set(mxLibPath "${MATLAB_ROOT}/bin/glnxa64") - else() - set(mxLibPath "${MATLAB_ROOT}/bin/glnx86") - endif() - endif() - - # List matlab's built-in libraries - file(GLOB matlabLibs RELATIVE "${mxLibPath}" "${mxLibPath}/lib*") - - # Convert to base names - set(matlabLibNames "") - foreach(lib ${matlabLibs}) - get_filename_component(libName "${lib}" NAME_WE) - list(APPEND matlabLibNames "${libName}") - endforeach() - - # Get names of link libraries - set(linkLibNames "") - foreach(lib ${libraries}) - string(FIND "${lib}" "/" slashPos) - if(NOT slashPos EQUAL -1) - # If the name is a path, just get the library name - get_filename_component(libName "${lib}" NAME_WE) - list(APPEND linkLibNames "${libName}") - else() - # It's not a path, so see if it looks like a filename - get_filename_component(ext "${lib}" EXT) - if(NOT "${ext}" STREQUAL "") - # It's a filename, so get the base name - get_filename_component(libName "${lib}" NAME_WE) - list(APPEND linkLibNames "${libName}") - else() - # It's not a filename so it must be a short name, add the "lib" prefix - list(APPEND linkLibNames "lib${lib}") - endif() - endif() - endforeach() - - # Remove duplicates - list(REMOVE_DUPLICATES linkLibNames) - - set(conflictingLibs "") - foreach(lib ${linkLibNames}) - list(FIND matlabLibNames "${lib}" libPos) - if(NOT libPos EQUAL -1) - if(NOT conflictingLibs STREQUAL "") - set(conflictingLibs "${conflictingLibs}, ") - endif() - set(conflictingLibs "${conflictingLibs}${lib}") - endif() - endforeach() - - if(NOT "${conflictingLibs}" STREQUAL "") - message(WARNING "GTSAM links to the libraries [ ${conflictingLibs} ] on your system, but " - "MATLAB is distributed with its own versions of these libraries which may conflict. " - "If you get strange errors or crashes with the GTSAM MATLAB wrapper, move these " - "libraries out of MATLAB's built-in library directory, which is ${mxLibPath} on " - "your system. MATLAB will usually still work with these libraries moved away, but " - "if not, you'll have to compile the static GTSAM MATLAB wrapper module.") - endif() - endif() -endfunction() - -# Helper function to install MATLAB scripts and handle multiple build types where the scripts -# should be installed to all build type toolboxes -function(install_matlab_scripts source_directory patterns) - set(patterns_args "") - set(exclude_patterns "") - if(NOT GTSAM_WRAP_SERIALIZATION) - set(exclude_patterns "testSerialization.m") - endif() - - foreach(pattern ${patterns}) - list(APPEND patterns_args PATTERN "${pattern}") - endforeach() - if(GTSAM_BUILD_TYPE_POSTFIXES) - foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) - string(TOUPPER "${build_type}" build_type_upper) - if(${build_type_upper} STREQUAL "RELEASE") - set(build_type_tag "") # Don't create release mode tag on installed directory - else() - set(build_type_tag "${build_type}") - endif() - # Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one - get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH) - get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME) - install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) - endforeach() - else() - install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) - endif() - -endfunction() diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index b50701464..cfc2e39bd 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -13,6 +13,8 @@ configure_file(${PROJECT_SOURCE_DIR}/python/setup.py.in file(COPY ${PROJECT_SOURCE_DIR}/python/MANIFEST.in DESTINATION ${GTSAM_PYTHON_BUILD_DIRECTORY}) +set(WRAP_BUILD_TYPE_POSTFIXES ${GTSAM_BUILD_TYPE_POSTFIXES}) + include(PybindWrap) ############################################################ From f03b12e420e081f86943a384e480439c2bd556c0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Mar 2021 16:10:08 -0500 Subject: [PATCH 077/106] Make matlab wrapping rely completely on the wrap project --- cmake/CMakeLists.txt | 1 - cmake/GtsamMatlabWrap.cmake | 30 ------- cmake/README.md | 26 ------ gtsam/CMakeLists.txt | 15 ---- gtsam_unstable/CMakeLists.txt | 16 ---- matlab/CMakeLists.txt | 154 ++++++++++++++++++++++++++-------- 6 files changed, 117 insertions(+), 125 deletions(-) delete mode 100644 cmake/GtsamMatlabWrap.cmake diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt index 6e912dd63..2c32049a0 100644 --- a/cmake/CMakeLists.txt +++ b/cmake/CMakeLists.txt @@ -16,7 +16,6 @@ install(FILES dllexport.h.in GtsamBuildTypes.cmake GtsamMakeConfigFile.cmake - GtsamMatlabWrap.cmake GtsamTesting.cmake GtsamPrinting.cmake FindNumPy.cmake diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake deleted file mode 100644 index a4085fd14..000000000 --- a/cmake/GtsamMatlabWrap.cmake +++ /dev/null @@ -1,30 +0,0 @@ -# Set the wrapping script variable -set(MATLAB_WRAP_SCRIPT "${GTSAM_SOURCE_DIR}/wrap/scripts/matlab_wrap.py") - -# Set up cache options -option(GTSAM_MEX_BUILD_STATIC_MODULE "Build MATLAB wrapper statically (increases build time)" OFF) -set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation") -set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE PATH "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox") -if(NOT GTSAM_TOOLBOX_INSTALL_PATH) - set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox") -endif() - -set(WRAP_MEX_BUILD_STATIC_MODULE ${GTSAM_MEX_BUILD_STATIC_MODULE}) -set(WRAP_BUILD_MEX_BINARY_FLAGS ${GTSAM_BUILD_MEX_BINARY_FLAGS}) -set(WRAP_TOOLBOX_INSTALL_PATH ${GTSAM_TOOLBOX_INSTALL_PATH}) -set(WRAP_CUSTOM_MATLAB_PATH ${GTSAM_CUSTOM_MATLAB_PATH}) -set(WRAP_BUILD_TYPE_POSTFIXES ${GTSAM_BUILD_TYPE_POSTFIXES}) - -# Fixup the Python paths -if(GTWRAP_DIR) - # packaged - set(GTWRAP_PACKAGE_DIR ${GTWRAP_DIR}) -else() - set(GTWRAP_PACKAGE_DIR ${GTSAM_SOURCE_DIR}/wrap) -endif() - -include(MatlabWrap) - -if(NOT BUILD_SHARED_LIBS) - message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") -endif() diff --git a/cmake/README.md b/cmake/README.md index 7f38bbcf2..569a401b1 100644 --- a/cmake/README.md +++ b/cmake/README.md @@ -67,32 +67,6 @@ Defines two useful functions for creating CTest unit tests. Also immediately cr an empty string "" if nothing needs to be excluded. linkLibraries: The list of libraries to link to. -## GtsamMatlabWrap - - include(GtsamMatlabWrap) - -Defines functions for generating MATLAB wrappers. Also immediately creates several CMake options for configuring the wrapper. - -* `wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)` Generates wrap code and compiles the wrapper. - - Usage example: - - `wrap_and_install_library("lba.h" "" "" "")` - - Arguments: - - interfaceHeader: The relative or absolute path to the wrapper interface - definition file. - linkLibraries: Any *additional* libraries to link. Your project library - (e.g. `lba`), libraries it depends on, and any necessary - MATLAB libraries will be linked automatically. So normally, - leave this empty. - extraIncludeDirs: Any *additional* include paths required by dependent - libraries that have not already been added by - include_directories. Again, normally, leave this empty. - extraMexFlags: Any *additional* flags to pass to the compiler when building - the wrap code. Normally, leave this empty. - ## GtsamMakeConfigFile include(GtsamMakeConfigFile) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 11ae113b9..71daf0653 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -204,18 +204,3 @@ else() set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error") endif() endif() - -# Create the matlab toolbox for the gtsam library -if (GTSAM_INSTALL_MATLAB_TOOLBOX) - # Set up codegen - include(GtsamMatlabWrap) - - # Generate, build and install toolbox - set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") - if(NOT BUILD_SHARED_LIBS) - list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) - endif() - - # Wrap - wrap_and_install_library(gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}") -endif () diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index ec161baa8..13c061b9b 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -107,22 +107,6 @@ install( list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) -# Wrap version for gtsam_unstable -if (GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) - # Set up codegen - include(GtsamMatlabWrap) - - # Generate, build and install toolbox - set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") - if(NOT BUILD_SHARED_LIBS) - list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) - endif() - - # Wrap - wrap_and_install_library(gtsam_unstable.i "gtsam" "" "${mexFlags}") -endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) - - # Build examples add_subdirectory(examples) diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 52d56a2b5..037787dfa 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -1,44 +1,124 @@ # Install matlab components -include(GtsamMatlabWrap) +# Create the matlab toolbox for the gtsam library +if(GTSAM_INSTALL_MATLAB_TOOLBOX) -# Record the root dir for gtsam - needed during external builds, e.g., ROS -set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR}) -message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") + # Set the wrapping script variable + set(MATLAB_WRAP_SCRIPT "${GTSAM_SOURCE_DIR}/wrap/scripts/matlab_wrap.py") -# Tests -#message(STATUS "Installing Matlab Toolbox") -install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig") -install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "README-gtsam-toolbox.txt") + # Set up cache options + option(GTSAM_MEX_BUILD_STATIC_MODULE + "Build MATLAB wrapper statically (increases build time)" OFF) + set(GTSAM_BUILD_MEX_BINARY_FLAGS + "" + CACHE STRING "Extra flags for running Matlab MEX compilation") + set(GTSAM_TOOLBOX_INSTALL_PATH + "" + CACHE + PATH + "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox" + ) + if(NOT GTSAM_TOOLBOX_INSTALL_PATH) + set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox") + endif() -# Examples -#message(STATUS "Installing Matlab Toolbox Examples") -# Matlab files: *.m and *.fig -#install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" "*.m;*.fig") + set(WRAP_MEX_BUILD_STATIC_MODULE ${GTSAM_MEX_BUILD_STATIC_MODULE}) + set(WRAP_BUILD_MEX_BINARY_FLAGS ${GTSAM_BUILD_MEX_BINARY_FLAGS}) + set(WRAP_TOOLBOX_INSTALL_PATH ${GTSAM_TOOLBOX_INSTALL_PATH}) + set(WRAP_CUSTOM_MATLAB_PATH ${GTSAM_CUSTOM_MATLAB_PATH}) + set(WRAP_BUILD_TYPE_POSTFIXES ${GTSAM_BUILD_TYPE_POSTFIXES}) -# Utilities -#message(STATUS "Installing Matlab Toolbox Utilities") -#install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" "*.m") + # Fixup the Python paths + if(GTWRAP_DIR) + # packaged + set(GTWRAP_PACKAGE_DIR ${GTWRAP_DIR}) + else() + set(GTWRAP_PACKAGE_DIR ${GTSAM_SOURCE_DIR}/wrap) + endif() -#message(STATUS "Installing Matlab Toolbox Example Data") -# Data files: *.graph, *.mat, and *.txt -file(GLOB matlab_examples_data_graph "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph") -file(GLOB matlab_examples_data_mat "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat") -file(GLOB matlab_examples_data_txt "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt") -set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_mat} ${matlab_examples_data_txt}) -if(GTSAM_BUILD_TYPE_POSTFIXES) - foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) - string(TOUPPER "${build_type}" build_type_upper) - if(${build_type_upper} STREQUAL "RELEASE") - set(build_type_tag "") # Don't create release mode tag on installed directory - else() - set(build_type_tag "${build_type}") - endif() - # Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one - get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH) - get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME) - install(FILES ${matlab_examples_data} DESTINATION "${location}/${name}${build_type_tag}/gtsam_examples/Data" CONFIGURATIONS "${build_type}") - endforeach() -else() - install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data) -endif() + include(MatlabWrap) + + if(NOT BUILD_SHARED_LIBS) + message( + FATAL_ERROR + "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF." + "The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries." + "If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF." + ) + endif() + + # ############################################################################ + # Generate, build and install toolbox + set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") + if(NOT BUILD_SHARED_LIBS) + list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) + endif() + + # Wrap + wrap_and_install_library(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" "" + "${mexFlags}") + + # Wrap version for gtsam_unstable + if(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) + # Generate, build and install toolbox + set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") + if(NOT BUILD_SHARED_LIBS) + list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) + endif() + + # Wrap + wrap_and_install_library(${GTSAM_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i "gtsam" "" "${mexFlags}") + endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) + + # Record the root dir for gtsam - needed during external builds, e.g., ROS + set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR}) + message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") + + # Tests message(STATUS "Installing Matlab Toolbox") + install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig") + install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" + "README-gtsam-toolbox.txt") + + # Examples message(STATUS "Installing Matlab Toolbox Examples") Matlab files: + # *.m and *.fig + # install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" + # "*.m;*.fig") + + # Utilities message(STATUS "Installing Matlab Toolbox Utilities") + # install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" "*.m") + + # message(STATUS "Installing Matlab Toolbox Example Data") Data files: + # *.graph, *.mat, and *.txt + file(GLOB matlab_examples_data_graph + "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph") + file(GLOB matlab_examples_data_mat + "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat") + file(GLOB matlab_examples_data_txt + "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt") + set(matlab_examples_data + ${matlab_examples_data_graph} ${matlab_examples_data_mat} + ${matlab_examples_data_txt}) + if(GTSAM_BUILD_TYPE_POSTFIXES) + foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) + string(TOUPPER "${build_type}" build_type_upper) + if(${build_type_upper} STREQUAL "RELEASE") + set(build_type_tag "") # Don't create release mode tag on installed + # directory + else() + set(build_type_tag "${build_type}") + endif() + # Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if + # there is one + get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH) + get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME) + install( + FILES ${matlab_examples_data} + DESTINATION "${location}/${name}${build_type_tag}/gtsam_examples/Data" + CONFIGURATIONS "${build_type}") + endforeach() + else() + install(FILES ${matlab_examples_data} + DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data) + endif() + +endif(GTSAM_INSTALL_MATLAB_TOOLBOX) From 5da50a5a6f8e2f70761a2a0a08dbf3ff24067b10 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 10 Mar 2021 08:53:43 -0500 Subject: [PATCH 078/106] 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 079/106] 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 827f3feb6b9da405f1162f1fce5bf1bce5824422 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Mar 2021 09:17:22 -0500 Subject: [PATCH 080/106] update Matlab CMakeLists.txt to use the wrapper --- matlab/CMakeLists.txt | 216 ++++++++++++++++++++++-------------------- 1 file changed, 111 insertions(+), 105 deletions(-) diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 037787dfa..1e7e88612 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -1,53 +1,72 @@ # Install matlab components +# Check if flag is enabled +if(NOT GTSAM_INSTALL_MATLAB_TOOLBOX) + return() +endif() + # Create the matlab toolbox for the gtsam library -if(GTSAM_INSTALL_MATLAB_TOOLBOX) - # Set the wrapping script variable - set(MATLAB_WRAP_SCRIPT "${GTSAM_SOURCE_DIR}/wrap/scripts/matlab_wrap.py") +# Set the wrapping script variable +set(MATLAB_WRAP_SCRIPT "${GTSAM_SOURCE_DIR}/wrap/scripts/matlab_wrap.py") - # Set up cache options - option(GTSAM_MEX_BUILD_STATIC_MODULE - "Build MATLAB wrapper statically (increases build time)" OFF) - set(GTSAM_BUILD_MEX_BINARY_FLAGS - "" - CACHE STRING "Extra flags for running Matlab MEX compilation") - set(GTSAM_TOOLBOX_INSTALL_PATH - "" - CACHE - PATH - "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox" +# Set up cache options +option(GTSAM_MEX_BUILD_STATIC_MODULE + "Build MATLAB wrapper statically (increases build time)" OFF) +set(GTSAM_BUILD_MEX_BINARY_FLAGS + "" + CACHE STRING "Extra flags for running Matlab MEX compilation") +set(GTSAM_TOOLBOX_INSTALL_PATH + "" + CACHE + PATH + "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox" +) +if(NOT GTSAM_TOOLBOX_INSTALL_PATH) + set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox") +endif() + +set(WRAP_MEX_BUILD_STATIC_MODULE ${GTSAM_MEX_BUILD_STATIC_MODULE}) +set(WRAP_BUILD_MEX_BINARY_FLAGS ${GTSAM_BUILD_MEX_BINARY_FLAGS}) +set(WRAP_TOOLBOX_INSTALL_PATH ${GTSAM_TOOLBOX_INSTALL_PATH}) +set(WRAP_CUSTOM_MATLAB_PATH ${GTSAM_CUSTOM_MATLAB_PATH}) +set(WRAP_BUILD_TYPE_POSTFIXES ${GTSAM_BUILD_TYPE_POSTFIXES}) + +# Fixup the Python paths +if(GTWRAP_DIR) + # packaged + set(GTWRAP_PACKAGE_DIR ${GTWRAP_DIR}) +else() + set(GTWRAP_PACKAGE_DIR ${GTSAM_SOURCE_DIR}/wrap) +endif() + +include(MatlabWrap) + +if(NOT BUILD_SHARED_LIBS) + message( + FATAL_ERROR + "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF." + "The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries." + "If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF." ) - if(NOT GTSAM_TOOLBOX_INSTALL_PATH) - set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox") - endif() +endif() - set(WRAP_MEX_BUILD_STATIC_MODULE ${GTSAM_MEX_BUILD_STATIC_MODULE}) - set(WRAP_BUILD_MEX_BINARY_FLAGS ${GTSAM_BUILD_MEX_BINARY_FLAGS}) - set(WRAP_TOOLBOX_INSTALL_PATH ${GTSAM_TOOLBOX_INSTALL_PATH}) - set(WRAP_CUSTOM_MATLAB_PATH ${GTSAM_CUSTOM_MATLAB_PATH}) - set(WRAP_BUILD_TYPE_POSTFIXES ${GTSAM_BUILD_TYPE_POSTFIXES}) +# ############################################################################## +# Generate, build and install toolbox +set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") +if(NOT BUILD_SHARED_LIBS) + list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) +endif() - # Fixup the Python paths - if(GTWRAP_DIR) - # packaged - set(GTWRAP_PACKAGE_DIR ${GTWRAP_DIR}) - else() - set(GTWRAP_PACKAGE_DIR ${GTSAM_SOURCE_DIR}/wrap) - endif() +set(ignore gtsam::Point2 gtsam::Point3 gtsam::BearingRangeFactor + gtsam::BearingRangeFactor2D gtsam::BearingRangeFactorPose2) - include(MatlabWrap) +# Wrap +matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" + "" "${mexFlags}" "${ignore}") - if(NOT BUILD_SHARED_LIBS) - message( - FATAL_ERROR - "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF." - "The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries." - "If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF." - ) - endif() - - # ############################################################################ +# Wrap version for gtsam_unstable +if(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) # Generate, build and install toolbox set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") if(NOT BUILD_SHARED_LIBS) @@ -55,70 +74,57 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX) endif() # Wrap - wrap_and_install_library(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" "" - "${mexFlags}") + matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i "gtsam" "" + "${mexFlags}") +endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) - # Wrap version for gtsam_unstable - if(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) - # Generate, build and install toolbox - set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") - if(NOT BUILD_SHARED_LIBS) - list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) +# Record the root dir for gtsam - needed during external builds, e.g., ROS +set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR}) +message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") + +# Tests message(STATUS "Installing Matlab Toolbox") +install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig") +install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" + "README-gtsam-toolbox.txt") + +# Examples message(STATUS "Installing Matlab Toolbox Examples") Matlab files: +# *.m and *.fig +# install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" +# "*.m;*.fig") + +# Utilities message(STATUS "Installing Matlab Toolbox Utilities") +# install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" "*.m") + +# message(STATUS "Installing Matlab Toolbox Example Data") Data files: *.graph, +# *.mat, and *.txt +file(GLOB matlab_examples_data_graph + "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph") +file(GLOB matlab_examples_data_mat + "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat") +file(GLOB matlab_examples_data_txt + "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt") +set(matlab_examples_data + ${matlab_examples_data_graph} ${matlab_examples_data_mat} + ${matlab_examples_data_txt}) +if(GTSAM_BUILD_TYPE_POSTFIXES) + foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) + string(TOUPPER "${build_type}" build_type_upper) + if(${build_type_upper} STREQUAL "RELEASE") + set(build_type_tag "") # Don't create release mode tag on installed + # directory + else() + set(build_type_tag "${build_type}") endif() - - # Wrap - wrap_and_install_library(${GTSAM_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i "gtsam" "" "${mexFlags}") - endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) - - # Record the root dir for gtsam - needed during external builds, e.g., ROS - set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR}) - message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") - - # Tests message(STATUS "Installing Matlab Toolbox") - install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig") - install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" - "README-gtsam-toolbox.txt") - - # Examples message(STATUS "Installing Matlab Toolbox Examples") Matlab files: - # *.m and *.fig - # install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" - # "*.m;*.fig") - - # Utilities message(STATUS "Installing Matlab Toolbox Utilities") - # install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" "*.m") - - # message(STATUS "Installing Matlab Toolbox Example Data") Data files: - # *.graph, *.mat, and *.txt - file(GLOB matlab_examples_data_graph - "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph") - file(GLOB matlab_examples_data_mat - "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat") - file(GLOB matlab_examples_data_txt - "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt") - set(matlab_examples_data - ${matlab_examples_data_graph} ${matlab_examples_data_mat} - ${matlab_examples_data_txt}) - if(GTSAM_BUILD_TYPE_POSTFIXES) - foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) - string(TOUPPER "${build_type}" build_type_upper) - if(${build_type_upper} STREQUAL "RELEASE") - set(build_type_tag "") # Don't create release mode tag on installed - # directory - else() - set(build_type_tag "${build_type}") - endif() - # Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if - # there is one - get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH) - get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME) - install( - FILES ${matlab_examples_data} - DESTINATION "${location}/${name}${build_type_tag}/gtsam_examples/Data" - CONFIGURATIONS "${build_type}") - endforeach() - else() - install(FILES ${matlab_examples_data} - DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data) - endif() - -endif(GTSAM_INSTALL_MATLAB_TOOLBOX) + # Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if + # there is one + get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH) + get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME) + install( + FILES ${matlab_examples_data} + DESTINATION "${location}/${name}${build_type_tag}/gtsam_examples/Data" + CONFIGURATIONS "${build_type}") + endforeach() +else() + install(FILES ${matlab_examples_data} + DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data) +endif() From 9afce21ac50cbb3dc92f1f25d3ece301549424a0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Mar 2021 09:18:09 -0500 Subject: [PATCH 081/106] delete example project since it is now its own git template --- cmake/example_project/CMakeLists.txt | 49 ------------------- cmake/example_project/README.md | 32 ------------ cmake/example_project/SayGoodbye.cpp | 23 --------- cmake/example_project/SayHello.cpp | 23 --------- cmake/example_project/example.h | 31 ------------ .../example_project/example/PrintExamples.cpp | 44 ----------------- cmake/example_project/example/PrintExamples.h | 42 ---------------- cmake/example_project/tests/testExample.cpp | 43 ---------------- 8 files changed, 287 deletions(-) delete mode 100644 cmake/example_project/CMakeLists.txt delete mode 100644 cmake/example_project/README.md delete mode 100644 cmake/example_project/SayGoodbye.cpp delete mode 100644 cmake/example_project/SayHello.cpp delete mode 100644 cmake/example_project/example.h delete mode 100644 cmake/example_project/example/PrintExamples.cpp delete mode 100644 cmake/example_project/example/PrintExamples.h delete mode 100644 cmake/example_project/tests/testExample.cpp diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt deleted file mode 100644 index 620ad4811..000000000 --- a/cmake/example_project/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -# This file should be used as a template for creating new projects using the CMake tools -# This project has the following features -# - GTSAM linking -# - Unit tests via CppUnitLite -# - Scripts -# - Automatic MATLAB wrapper generation - -################################################################################### -# To create your own project, replace "example" with the actual name of your project -cmake_minimum_required(VERSION 3.0) -project(example CXX C) - -# Include GTSAM CMake tools -find_package(GTSAMCMakeTools) -include(GtsamBuildTypes) # Load build type flags and default to Debug mode -include(GtsamTesting) # Easy functions for creating unit tests and scripts -include(GtsamMatlabWrap) # Automatic MATLAB wrapper generation - -# Ensure that local folder is searched before library folders -include_directories(BEFORE "${PROJECT_SOURCE_DIR}") - -################################################################################### -# Find GTSAM components -find_package(GTSAM REQUIRED) # Uses installed package -# Note: Since Jan-2019, GTSAMConfig.cmake defines exported CMake targets -# that automatically do include the include_directories() without the need -# to call include_directories(), just target_link_libraries(NAME gtsam) -#include_directories(${GTSAM_INCLUDE_DIR}) - -################################################################################### -# Build static library from common sources -set(CONVENIENCE_LIB_NAME ${PROJECT_NAME}) -add_library(${CONVENIENCE_LIB_NAME} SHARED example/PrintExamples.h example/PrintExamples.cpp) -target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam) - -# Install library -install(TARGETS ${CONVENIENCE_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) - -################################################################################### -# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library) -gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}") - -################################################################################### -# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library) -gtsamAddExamplesGlob("*.cpp" "" "${CONVENIENCE_LIB_NAME}") - -################################################################################### -# Build MATLAB wrapper (CMake tracks the dependecy to link with GTSAM through our project's static library) -wrap_and_install_library("example.h" "${CONVENIENCE_LIB_NAME}" "" "") diff --git a/cmake/example_project/README.md b/cmake/example_project/README.md deleted file mode 100644 index a1269cf19..000000000 --- a/cmake/example_project/README.md +++ /dev/null @@ -1,32 +0,0 @@ -# MATLAB Wrapper Example Project - -This project serves as a lightweight example for demonstrating how to wrap C++ code in MATLAB using GTSAM. - -## Compiling - -We follow the regular build procedure inside the `example_project` directory: - -```sh -mkdir build && cd build -cmake .. -make -j8 -sudo make install - -sudo ldconfig # ensures the shared object file generated is correctly loaded -``` - -## Usage - -Now you can open MATLAB and add the `gtsam_toolbox` to the MATLAB path - -```matlab -addpath('/usr/local/gtsam_toolbox') -``` - -At this point you are ready to run the example project. Starting from the `example_project` directory inside MATLAB, simply run code like regular MATLAB, e.g. - -```matlab -pe = example.PrintExamples(); -pe.sayHello(); -pe.sayGoodbye(); -``` \ No newline at end of file diff --git a/cmake/example_project/SayGoodbye.cpp b/cmake/example_project/SayGoodbye.cpp deleted file mode 100644 index be1165ef6..000000000 --- a/cmake/example_project/SayGoodbye.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 SayGoodbye.cpp - * @brief Example script for example project - * @author Richard Roberts - */ - -#include - -int main(int argc, char *argv[]) { - example::PrintExamples().sayGoodbye(); - return 0; -} diff --git a/cmake/example_project/SayHello.cpp b/cmake/example_project/SayHello.cpp deleted file mode 100644 index 2da06ab32..000000000 --- a/cmake/example_project/SayHello.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 SayHello.cpp - * @brief Example script for example project - * @author Richard Roberts - */ - -#include - -int main(int argc, char *argv[]) { - example::PrintExamples().sayHello(); - return 0; -} diff --git a/cmake/example_project/example.h b/cmake/example_project/example.h deleted file mode 100644 index b0d732e14..000000000 --- a/cmake/example_project/example.h +++ /dev/null @@ -1,31 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 example.h - * @brief Example wrapper interface file - * @author Richard Roberts - */ - -// This is an interface file for automatic MATLAB wrapper generation. See -// gtsam.h for full documentation and more examples. - -#include - -namespace example { - -class PrintExamples { - PrintExamples(); - void sayHello() const; - void sayGoodbye() const; -}; - -} diff --git a/cmake/example_project/example/PrintExamples.cpp b/cmake/example_project/example/PrintExamples.cpp deleted file mode 100644 index 1e9f10713..000000000 --- a/cmake/example_project/example/PrintExamples.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 print_examples.cpp - * @brief Example library file - * @author Richard Roberts - */ - -#include - -#include - -namespace example { - -void PrintExamples::sayHello() const { - std::cout << internal::getHelloString() << std::endl; -} - -void PrintExamples::sayGoodbye() const { - std::cout << internal::getGoodbyeString() << std::endl; -} - -namespace internal { - -std::string getHelloString() { - return "Hello!"; -} - -std::string getGoodbyeString() { - return "See you soon!"; -} - -} // namespace internal - -} // namespace example diff --git a/cmake/example_project/example/PrintExamples.h b/cmake/example_project/example/PrintExamples.h deleted file mode 100644 index 25d4dd8cb..000000000 --- a/cmake/example_project/example/PrintExamples.h +++ /dev/null @@ -1,42 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 print_examples.h - * @brief Example library file - * @author Richard Roberts - */ - -#pragma once - -#include -#include - -namespace example { - -class PrintExamples { -public: - /// Print a greeting - void sayHello() const; - - /// Print a farewell - void sayGoodbye() const; -}; - -namespace internal { - -std::string getHelloString(); - -std::string getGoodbyeString(); - -} // namespace internal - -} // namespace example diff --git a/cmake/example_project/tests/testExample.cpp b/cmake/example_project/tests/testExample.cpp deleted file mode 100644 index c2a5a173b..000000000 --- a/cmake/example_project/tests/testExample.cpp +++ /dev/null @@ -1,43 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testExample.cpp - * @brief Unit tests for example - * @author Richard Roberts - */ - -#include - -#include - -#include - -using namespace gtsam; - -TEST(Example, HelloString) { - const std::string expectedString = "Hello!"; - EXPECT(assert_equal(expectedString, example::internal::getHelloString())); -} - -TEST(Example, GoodbyeString) { - const std::string expectedString = "See you soon!"; - EXPECT(assert_equal(expectedString, example::internal::getGoodbyeString())); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - - From f52b09660ea54a04ae20f2bd746570af37ee88d2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Mar 2021 09:18:36 -0500 Subject: [PATCH 082/106] Squashed 'wrap/' changes from b28b3570d..b0eb968f2 b0eb968f2 Completely handle Matlab wrapping (#34) 4ad71812a Merge pull request #33 from borglab/fix/script-destination 0c832eed7 reverted from TYPE to DESTINATION for wrapper scripts 10e1efd6f Merge pull request #32 from ayushbaid/feature/pickle 55d5d7fbe ignoring pickle in matlab wrap 8d70c7fe2 adding newlines dee8aaee3 adding markers for pickling 46fc45d82 separating out the marker for pickle d37b8a972 Merge pull request #31 from ayushbaid/feature/pickle efd4a0fb4 removing tab 42fd231f3 adding newline for test compare 0aa316150 removing extra brackets 7fe1d7d0f adding pickle code to expected file 9c3ab7a8b fixing string format for new classname 2f89284e8 moving pickle support with the serialization code 5a8abc916 adding pickle support for select classes using serialization git-subtree-dir: wrap git-subtree-split: b0eb968f29a2261700361599cab2823221dd0f9d --- CMakeLists.txt | 25 +- README.md | 79 ++-- cmake/MatlabWrap.cmake | 477 +++++++++++++++++++++ gtwrap/matlab_wrapper.py | 27 +- gtwrap/pybind_wrapper.py | 16 + tests/expected-matlab/geometry_wrapper.cpp | 2 +- tests/expected-python/geometry_pybind.cpp | 22 + tests/geometry.h | 6 + 8 files changed, 615 insertions(+), 39 deletions(-) create mode 100644 cmake/MatlabWrap.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 5ffb66ad0..163165d98 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,20 +21,27 @@ else() set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/cmake") endif() -# Install scripts to the standard CMake script directory. -install(FILES cmake/gtwrapConfig.cmake cmake/PybindWrap.cmake - cmake/GtwrapUtils.cmake +# Install CMake scripts to the standard CMake script directory. +install(FILES cmake/gtwrapConfig.cmake cmake/MatlabWrap.cmake + cmake/PybindWrap.cmake cmake/GtwrapUtils.cmake DESTINATION "${SCRIPT_INSTALL_DIR}/gtwrap") +# Needed for the CMAKE_INSTALL_X variables used below. include(GNUInstallDirs) -# Install the gtwrap python package as a directory so it can be found for -# wrapping. + +# Install the gtwrap python package as a directory so it can be found by CMake +# for wrapping. install(DIRECTORY gtwrap DESTINATION "${CMAKE_INSTALL_DATADIR}/gtwrap") # Install wrapping scripts as binaries to `CMAKE_INSTALL_PREFIX/bin` so they can -# be invoked for wrapping. -install(PROGRAMS scripts/pybind_wrap.py scripts/matlab_wrap.py TYPE BIN) +# be invoked for wrapping. We use DESTINATION (instead of TYPE) so we can +# support older CMake versions. +install(PROGRAMS scripts/pybind_wrap.py scripts/matlab_wrap.py + DESTINATION ${CMAKE_INSTALL_BINDIR}) -# Install pybind11 directory to `CMAKE_INSTALL_PREFIX/lib/pybind11` This will -# allow the gtwrapConfig.cmake file to load it later. +# Install pybind11 directory to `CMAKE_INSTALL_PREFIX/lib/gtwrap/pybind11` This +# will allow the gtwrapConfig.cmake file to load it later. install(DIRECTORY pybind11 DESTINATION "${CMAKE_INSTALL_LIBDIR}/gtwrap") + +# Install the matlab.h file to `CMAKE_INSTALL_PREFIX/lib/gtwrap/matlab.h`. +install(FILES matlab.h DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/gtwrap") diff --git a/README.md b/README.md index 5b5cbb902..1b2783a4c 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,3 @@ - # WRAP The wrap library wraps the GTSAM library into a Python library or MATLAB toolbox. @@ -43,36 +42,64 @@ pybind_wrap(${PROJECT_NAME}_py # target For more information, please follow our [tutorial](https://github.com/borglab/gtsam-project-python). -## GTSAM Python wrapper +## Python Wrapper **WARNING: On macOS, you have to statically build GTSAM to use the wrapper.** 1. Set `GTSAM_BUILD_PYTHON=ON` while configuring the build with `cmake`. 1. What you can do in the `build` folder: - 1. Just run python then import GTSAM and play around: - ``` - import gtsam - gtsam.__dir__() - ``` + 1. Just run python then import GTSAM and play around: + + ``` + import gtsam + gtsam.__dir__() + ``` + + 1. Run the unittests: + ``` + python -m unittest discover + ``` + 1. Edit the unittests in `python/gtsam/*.py` and simply rerun the test. + They were symlinked to `/gtsam/*.py` to facilitate fast development. + `python -m unittest gtsam/tests/test_Pose3.py` - NOTE: You might need to re-run `cmake ..` if files are deleted or added. - 1. Run the unittests: - ``` - python -m unittest discover - ``` - 1. Edit the unittests in `python/gtsam/*.py` and simply rerun the test. - They were symlinked to `/gtsam/*.py` to facilitate fast development. - ``` - python -m unittest gtsam/tests/test_Pose3.py - ``` - - NOTE: You might need to re-run `cmake ..` if files are deleted or added. 1. Do `make install` and `cd /python`. Here, you can: - 1. Run the unittests: - ``` - python setup.py test - ``` - 2. Install `gtsam` to your current Python environment. - ``` - python setup.py install - ``` - - NOTE: It's a good idea to create a virtual environment otherwise it will be installed in your system Python's site-packages. + 1. Run the unittests: + ``` + python setup.py test + ``` + 2. Install `gtsam` to your current Python environment. + ``` + python setup.py install + ``` + - NOTE: It's a good idea to create a virtual environment otherwise it will be installed in your system Python's site-packages. + +## Matlab Wrapper + +In the CMake, simply include the `MatlabWrap.cmake` file. + +```cmake +include(MatlabWrap) +``` + +This cmake file defines functions for generating MATLAB wrappers. + +- `wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)` Generates wrap code and compiles the wrapper. + +Usage example: + + `wrap_and_install_library("lba.h" "" "" "")` + +Arguments: + +- `interfaceHeader`: The relative or absolute path to the wrapper interface definition file. +- `linkLibraries`: Any _additional_ libraries to link. Your project library + (e.g. `lba`), libraries it depends on, and any necessary + MATLAB libraries will be linked automatically. So normally, + leave this empty. +- `extraIncludeDirs`: Any _additional_ include paths required by dependent + libraries that have not already been added by + include_directories. Again, normally, leave this empty. +- `extraMexFlags`: Any _additional_ flags to pass to the compiler when building + the wrap code. Normally, leave this empty. diff --git a/cmake/MatlabWrap.cmake b/cmake/MatlabWrap.cmake new file mode 100644 index 000000000..75654238a --- /dev/null +++ b/cmake/MatlabWrap.cmake @@ -0,0 +1,477 @@ +find_package( + Matlab + COMPONENTS MEX_COMPILER + REQUIRED) + +if(NOT Matlab_MEX_COMPILER) + message( + FATAL_ERROR + "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well." + ) +endif() + +if(WRAP_BUILD_TYPE_POSTFIXES) + set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) +endif() + +# WRAP_MEX_BUILD_STATIC_MODULE is not for Windows - on Windows any static +# are already compiled into the library by the linker +if(WRAP_MEX_BUILD_STATIC_MODULE AND WIN32) + message(FATAL_ERROR "WRAP_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).") +endif() + +set(MEX_COMMAND ${Matlab_MEX_COMPILER} CACHE PATH "Path to MATLAB MEX compiler") +set(MATLAB_ROOT ${Matlab_ROOT_DIR} CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)") + +# Try to automatically configure mex path from provided custom `bin` path. +if(WRAP_CUSTOM_MATLAB_PATH) + set(matlab_bin_directory ${WRAP_CUSTOM_MATLAB_PATH}) + + if(WIN32) + set(mex_program_name "mex.bat") + else() + set(mex_program_name "mex") + endif() + + # Run find_program explicitly putting $PATH after our predefined program + # directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents + # finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is + # on the system path. + find_program(MEX_COMMAND ${mex_program_name} + PATHS ${matlab_bin_directory} ENV PATH + NO_DEFAULT_PATH) + mark_as_advanced(FORCE MEX_COMMAND) + # Now that we have mex, trace back to find the Matlab installation root + get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH) + get_filename_component(mex_path "${MEX_COMMAND}" PATH) + if(mex_path MATCHES ".*/win64$") + get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE) + else() + get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE) + endif() +endif() + +# Consistent and user-friendly wrap function +function(matlab_wrap interfaceHeader linkLibraries + extraIncludeDirs extraMexFlags ignore_classes) + wrap_and_install_library("${interfaceHeader}" "${linkLibraries}" + "${extraIncludeDirs}" "${extraMexFlags}" + "${ignore_classes}") +endfunction() + +# Wrapping function. Builds a mex module from the provided +# interfaceHeader. For example, for the interface header gtsam.h, this will +# build the wrap module 'gtsam'. +# +# Arguments: +# +# interfaceHeader: The relative path to the wrapper interface definition file. +# linkLibraries: Any *additional* libraries to link. Your project library +# (e.g. `lba`), libraries it depends on, and any necessary MATLAB libraries will +# be linked automatically. So normally, leave this empty. +# extraIncludeDirs: Any *additional* include paths required by dependent libraries that have not +# already been added by include_directories. Again, normally, leave this empty. +# extraMexFlags: Any *additional* flags to pass to the compiler when building +# the wrap code. Normally, leave this empty. +# ignore_classes: List of classes to ignore in the wrapping. +function(wrap_and_install_library interfaceHeader linkLibraries + extraIncludeDirs extraMexFlags ignore_classes) + wrap_library_internal("${interfaceHeader}" "${linkLibraries}" + "${extraIncludeDirs}" "${mexFlags}") + install_wrapped_library_internal("${interfaceHeader}") +endfunction() + +# Internal function that wraps a library and compiles the wrapper +function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs + extraMexFlags) + if(UNIX AND NOT APPLE) + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + set(mexModuleExt mexa64) + else() + set(mexModuleExt mexglx) + endif() + elseif(APPLE) + set(mexModuleExt mexmaci64) + elseif(MSVC) + if(CMAKE_CL_64) + set(mexModuleExt mexw64) + else() + set(mexModuleExt mexw32) + endif() + endif() + + # Wrap codegen interface usage: wrap interfacePath moduleName toolboxPath + # headerPath interfacePath : *absolute* path to directory of module interface + # file moduleName : the name of the module, interface file must be called + # moduleName.h toolboxPath : the directory in which to generate the wrappers + # headerPath : path to matlab.h + + # Extract module name from interface header file name + get_filename_component(interfaceHeader "${interfaceHeader}" ABSOLUTE) + get_filename_component(modulePath "${interfaceHeader}" PATH) + get_filename_component(moduleName "${interfaceHeader}" NAME_WE) + + # Paths for generated files + set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}") + set(generated_cpp_file "${generated_files_path}/${moduleName}_wrapper.cpp") + set(compiled_mex_modules_root "${PROJECT_BINARY_DIR}/wrap/${moduleName}_mex") + + message(STATUS "Building wrap module ${moduleName}") + + # Set matlab.h in project + set(matlab_h_path "${CMAKE_SOURCE_DIR}") + + # If building a static mex module, add all cmake-linked libraries to the + # explicit link libraries list so that the next block of code can unpack any + # static libraries + set(automaticDependencies "") + foreach(lib ${moduleName} ${linkLibraries}) + # message("MODULE NAME: ${moduleName}") + if(TARGET "${lib}") + get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES) + # message("DEPENDENT LIBRARIES: ${dependentLibraries}") + if(dependentLibraries) + list(APPEND automaticDependencies ${dependentLibraries}) + endif() + endif() + endforeach() + + # CHRIS: Temporary fix. On my system the get_target_property above returned + # Not-found for gtsam module This needs to be fixed!! + if(UNIX AND NOT APPLE) + list( + APPEND + automaticDependencies + ${Boost_SERIALIZATION_LIBRARY_RELEASE} + ${Boost_FILESYSTEM_LIBRARY_RELEASE} + ${Boost_SYSTEM_LIBRARY_RELEASE} + ${Boost_THREAD_LIBRARY_RELEASE} + ${Boost_DATE_TIME_LIBRARY_RELEASE}) + # Only present in Boost >= 1.48.0 + if(Boost_TIMER_LIBRARY_RELEASE) + list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} + ${Boost_CHRONO_LIBRARY_RELEASE}) + if(WRAP_MEX_BUILD_STATIC_MODULE) + # list(APPEND automaticDependencies -Wl,--no-as-needed -lrt) + endif() + endif() + endif() + + # message("AUTOMATIC DEPENDENCIES: ${automaticDependencies}") CHRIS: End + # temporary fix + + # Separate dependencies + set(correctedOtherLibraries "") + set(otherLibraryTargets "") + set(otherLibraryNontargets "") + set(otherSourcesAndObjects "") + foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies}) + if(TARGET "${lib}") + if(WRAP_MEX_BUILD_STATIC_MODULE) + get_target_property(target_sources ${lib} SOURCES) + list(APPEND otherSourcesAndObjects ${target_sources}) + else() + list(APPEND correctedOtherLibraries ${lib}) + list(APPEND otherLibraryTargets ${lib}) + endif() + else() + get_filename_component(file_extension "${lib}" EXT) + get_filename_component(lib_name "${lib}" NAME_WE) + if(file_extension STREQUAL ".a" AND WRAP_MEX_BUILD_STATIC_MODULE) + # For building a static MEX module, unpack the static library and + # compile its object files into our module + file(MAKE_DIRECTORY "${generated_files_path}/${lib_name}_objects") + execute_process( + COMMAND ar -x "${lib}" + WORKING_DIRECTORY "${generated_files_path}/${lib_name}_objects" + RESULT_VARIABLE ar_result) + if(NOT ar_result EQUAL 0) + message(FATAL_ERROR "Failed extracting ${lib}") + endif() + + # Get list of object files + execute_process( + COMMAND ar -t "${lib}" + OUTPUT_VARIABLE object_files + RESULT_VARIABLE ar_result) + if(NOT ar_result EQUAL 0) + message(FATAL_ERROR "Failed listing ${lib}") + endif() + + # Add directory to object files + string(REPLACE "\n" ";" object_files_list "${object_files}") + foreach(object_file ${object_files_list}) + get_filename_component(file_extension "${object_file}" EXT) + if(file_extension STREQUAL ".o") + list(APPEND otherSourcesAndObjects + "${generated_files_path}/${lib_name}_objects/${object_file}") + endif() + endforeach() + else() + list(APPEND correctedOtherLibraries ${lib}) + list(APPEND otherLibraryNontargets ${lib}) + endif() + endif() + endforeach() + + # Check libraries for conflicting versions built-in to MATLAB + set(dependentLibraries "") + if(NOT "${otherLibraryTargets}" STREQUAL "") + foreach(target ${otherLibraryTargets}) + get_target_property(dependentLibrariesOne ${target} + INTERFACE_LINK_LIBRARIES) + list(APPEND dependentLibraries ${dependentLibrariesOne}) + endforeach() + endif() + list(APPEND dependentLibraries ${otherLibraryNontargets}) + check_conflicting_libraries_internal("${dependentLibraries}") + + # Set up generation of module source file + file(MAKE_DIRECTORY "${generated_files_path}") + + find_package(PythonInterp ${WRAP_PYTHON_VERSION} EXACT) + find_package(PythonLibs ${WRAP_PYTHON_VERSION} EXACT) + + add_custom_command( + OUTPUT ${generated_cpp_file} + DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets} + ${otherSourcesAndObjects} + COMMAND + ${CMAKE_COMMAND} -E env + "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} ${MATLAB_WRAP_SCRIPT} --src ${interfaceHeader} + --module_name ${moduleName} --out ${generated_files_path} + --top_module_namespaces ${moduleName} --ignore ${ignore_classes} + VERBATIM + WORKING_DIRECTORY ${generated_files_path}) + + # Set up building of mex module + string(REPLACE ";" " " extraMexFlagsSpaced "${extraMexFlags}") + string(REPLACE ";" " " mexFlagsSpaced "${WRAP_BUILD_MEX_BINARY_FLAGS}") + add_library( + ${moduleName}_matlab_wrapper MODULE + ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects}) + target_link_libraries(${moduleName}_matlab_wrapper ${correctedOtherLibraries}) + target_link_libraries(${moduleName}_matlab_wrapper ${moduleName}) + set_target_properties( + ${moduleName}_matlab_wrapper + PROPERTIES OUTPUT_NAME "${moduleName}_wrapper" + PREFIX "" + SUFFIX ".${mexModuleExt}" + LIBRARY_OUTPUT_DIRECTORY "${compiled_mex_modules_root}" + ARCHIVE_OUTPUT_DIRECTORY "${compiled_mex_modules_root}" + RUNTIME_OUTPUT_DIRECTORY "${compiled_mex_modules_root}" + CLEAN_DIRECT_OUTPUT 1) + set_property( + TARGET ${moduleName}_matlab_wrapper + APPEND_STRING + PROPERTY + COMPILE_FLAGS + " ${extraMexFlagsSpaced} ${mexFlagsSpaced} \"-I${MATLAB_ROOT}/extern/include\" -DMATLAB_MEX_FILE -DMX_COMPAT_32" + ) + set_property( + TARGET ${moduleName}_matlab_wrapper + APPEND + PROPERTY INCLUDE_DIRECTORIES ${extraIncludeDirs}) + # Disable build type postfixes for the mex module - we install in different + # directories for each build type instead + foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) + string(TOUPPER "${build_type}" build_type_upper) + set_target_properties(${moduleName}_matlab_wrapper + PROPERTIES ${build_type_upper}_POSTFIX "") + endforeach() + # Set up platform-specific flags + if(MSVC) + if(CMAKE_CL_64) + set(mxLibPath "${MATLAB_ROOT}/extern/lib/win64/microsoft") + else() + set(mxLibPath "${MATLAB_ROOT}/extern/lib/win32/microsoft") + endif() + target_link_libraries( + ${moduleName}_matlab_wrapper "${mxLibPath}/libmex.lib" + "${mxLibPath}/libmx.lib" "${mxLibPath}/libmat.lib") + set_target_properties(${moduleName}_matlab_wrapper + PROPERTIES LINK_FLAGS "/export:mexFunction") + set_property( + SOURCE "${generated_cpp_file}" + APPEND + PROPERTY COMPILE_FLAGS "/bigobj") + elseif(APPLE) + set(mxLibPath "${MATLAB_ROOT}/bin/maci64") + target_link_libraries( + ${moduleName}_matlab_wrapper "${mxLibPath}/libmex.dylib" + "${mxLibPath}/libmx.dylib" "${mxLibPath}/libmat.dylib") + endif() + + # Hacking around output issue with custom command Deletes generated build + # folder + add_custom_target( + wrap_${moduleName}_matlab_distclean + COMMAND cmake -E remove_directory ${generated_files_path} + COMMAND cmake -E remove_directory ${compiled_mex_modules_root}) +endfunction() + +# Internal function that installs a wrap toolbox +function(install_wrapped_library_internal interfaceHeader) + get_filename_component(moduleName "${interfaceHeader}" NAME_WE) + set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}") + + # NOTE: only installs .m and mex binary files (not .cpp) - the trailing slash + # on the directory name here prevents creating the top-level module name + # directory in the destination. + message(STATUS "Installing Matlab Toolbox to ${WRAP_TOOLBOX_INSTALL_PATH}") + if(WRAP_BUILD_TYPE_POSTFIXES) + foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) + string(TOUPPER "${build_type}" build_type_upper) + if(${build_type_upper} STREQUAL "RELEASE") + set(build_type_tag "") # Don't create release mode tag on installed + # directory + else() + set(build_type_tag "${build_type}") + endif() + # Split up filename to strip trailing '/' in WRAP_TOOLBOX_INSTALL_PATH if + # there is one + get_filename_component(location "${WRAP_TOOLBOX_INSTALL_PATH}" PATH) + get_filename_component(name "${WRAP_TOOLBOX_INSTALL_PATH}" NAME) + install( + DIRECTORY "${generated_files_path}/" + DESTINATION "${location}/${name}${build_type_tag}" + CONFIGURATIONS "${build_type}" + FILES_MATCHING + PATTERN "*.m") + install( + TARGETS ${moduleName}_matlab_wrapper + LIBRARY DESTINATION "${location}/${name}${build_type_tag}" + CONFIGURATIONS "${build_type}" + RUNTIME DESTINATION "${location}/${name}${build_type_tag}" + CONFIGURATIONS "${build_type}") + endforeach() + else() + install( + DIRECTORY "${generated_files_path}/" + DESTINATION ${WRAP_TOOLBOX_INSTALL_PATH} + FILES_MATCHING + PATTERN "*.m") + install( + TARGETS ${moduleName}_matlab_wrapper + LIBRARY DESTINATION ${WRAP_TOOLBOX_INSTALL_PATH} + RUNTIME DESTINATION ${WRAP_TOOLBOX_INSTALL_PATH}) + endif() +endfunction() + +# Internal function to check for libraries installed with MATLAB that may +# conflict and prints a warning to move them if problems occur. +function(check_conflicting_libraries_internal libraries) + if(UNIX) + # Set path for matlab's built-in libraries + if(APPLE) + set(mxLibPath "${MATLAB_ROOT}/bin/maci64") + else() + if(CMAKE_CL_64) + set(mxLibPath "${MATLAB_ROOT}/bin/glnxa64") + else() + set(mxLibPath "${MATLAB_ROOT}/bin/glnx86") + endif() + endif() + + # List matlab's built-in libraries + file( + GLOB matlabLibs + RELATIVE "${mxLibPath}" + "${mxLibPath}/lib*") + + # Convert to base names + set(matlabLibNames "") + foreach(lib ${matlabLibs}) + get_filename_component(libName "${lib}" NAME_WE) + list(APPEND matlabLibNames "${libName}") + endforeach() + + # Get names of link libraries + set(linkLibNames "") + foreach(lib ${libraries}) + string(FIND "${lib}" "/" slashPos) + if(NOT slashPos EQUAL -1) + # If the name is a path, just get the library name + get_filename_component(libName "${lib}" NAME_WE) + list(APPEND linkLibNames "${libName}") + else() + # It's not a path, so see if it looks like a filename + get_filename_component(ext "${lib}" EXT) + if(NOT "${ext}" STREQUAL "") + # It's a filename, so get the base name + get_filename_component(libName "${lib}" NAME_WE) + list(APPEND linkLibNames "${libName}") + else() + # It's not a filename so it must be a short name, add the "lib" prefix + list(APPEND linkLibNames "lib${lib}") + endif() + endif() + endforeach() + + # Remove duplicates + list(REMOVE_DUPLICATES linkLibNames) + + set(conflictingLibs "") + foreach(lib ${linkLibNames}) + list(FIND matlabLibNames "${lib}" libPos) + if(NOT libPos EQUAL -1) + if(NOT conflictingLibs STREQUAL "") + set(conflictingLibs "${conflictingLibs}, ") + endif() + set(conflictingLibs "${conflictingLibs}${lib}") + endif() + endforeach() + + if(NOT "${conflictingLibs}" STREQUAL "") + message( + WARNING + "The project links to the libraries [ ${conflictingLibs} ] on your system, but " + "MATLAB is distributed with its own versions of these libraries which may conflict. " + "If you get strange errors or crashes with the MATLAB wrapper, move these " + "libraries out of MATLAB's built-in library directory, which is ${mxLibPath} on " + "your system. MATLAB will usually still work with these libraries moved away, but " + "if not, you'll have to compile the static MATLAB wrapper module." + ) + endif() + endif() +endfunction() + +# Helper function to install MATLAB scripts and handle multiple build types +# where the scripts should be installed to all build type toolboxes +function(install_matlab_scripts source_directory patterns) + set(patterns_args "") + set(exclude_patterns "") + + foreach(pattern ${patterns}) + list(APPEND patterns_args PATTERN "${pattern}") + endforeach() + if(WRAP_BUILD_TYPE_POSTFIXES) + foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) + string(TOUPPER "${build_type}" build_type_upper) + if(${build_type_upper} STREQUAL "RELEASE") + set(build_type_tag "") # Don't create release mode tag on installed + # directory + else() + set(build_type_tag "${build_type}") + endif() + # Split up filename to strip trailing '/' in WRAP_TOOLBOX_INSTALL_PATH if + # there is one + get_filename_component(location "${WRAP_TOOLBOX_INSTALL_PATH}" PATH) + get_filename_component(name "${WRAP_TOOLBOX_INSTALL_PATH}" NAME) + install( + DIRECTORY "${source_directory}" + DESTINATION "${location}/${name}${build_type_tag}" + CONFIGURATIONS "${build_type}" + FILES_MATCHING ${patterns_args} + PATTERN "${exclude_patterns}" EXCLUDE) + endforeach() + else() + install( + DIRECTORY "${source_directory}" + DESTINATION "${WRAP_TOOLBOX_INSTALL_PATH}" + FILES_MATCHING ${patterns_args} + PATTERN "${exclude_patterns}" EXCLUDE) + endif() + +endfunction() diff --git a/gtwrap/matlab_wrapper.py b/gtwrap/matlab_wrapper.py index 669bf474f..aeaf221bd 100755 --- a/gtwrap/matlab_wrapper.py +++ b/gtwrap/matlab_wrapper.py @@ -49,6 +49,8 @@ class MatlabWrapper(object): } """Methods that should not be wrapped directly""" whitelist = ['serializable', 'serialize'] + """Methods that should be ignored""" + ignore_methods = ['pickle'] """Datatypes that do not need to be checked in methods""" not_check_type = [] """Data types that are primitive types""" @@ -563,6 +565,8 @@ class MatlabWrapper(object): for method in methods: if method.name in self.whitelist: continue + if method.name in self.ignore_methods: + continue comment += '%{name}({args})'.format(name=method.name, args=self._wrap_args(method.args)) @@ -587,7 +591,7 @@ class MatlabWrapper(object): file_name = self._wrapper_name() + '.cpp' wrapper_file = textwrap.dedent('''\ - # include + # include # include ''') @@ -612,6 +616,9 @@ class MatlabWrapper(object): methods = self._group_methods(methods) for method in methods: + if method in self.ignore_methods: + continue + if globals: self._debug("[wrap_methods] wrapping: {}..{}={}".format(method[0].parent.name, method[0].name, type(method[0].parent.name))) @@ -861,6 +868,8 @@ class MatlabWrapper(object): method_name = method[0].name if method_name in self.whitelist and method_name != 'serialize': continue + if method_name in self.ignore_methods: + continue if method_name == 'serialize': serialize[0] = True @@ -932,6 +941,9 @@ class MatlabWrapper(object): format_name = list(static_method[0].name) format_name[0] = format_name[0].upper() + if static_method[0].name in self.ignore_methods: + continue + method_text += textwrap.indent(textwrap.dedent('''\ function varargout = {name}(varargin) '''.format(name=''.join(format_name))), @@ -1464,7 +1476,7 @@ class MatlabWrapper(object): """Generate the c++ wrapper.""" # Includes wrapper_file = textwrap.dedent('''\ - #include + #include #include \n #include #include @@ -1473,7 +1485,16 @@ class MatlabWrapper(object): includes_list = sorted(list(self.includes.keys()), key=lambda include: include.header) - wrapper_file += reduce(lambda x, y: str(x) + '\n' + str(y), includes_list) + '\n' + # Check the number of includes. + # If no includes, do nothing, if 1 then just append newline. + # if more than one, concatenate them with newlines. + if len(includes_list) == 0: + pass + elif len(includes_list) == 1: + wrapper_file += (str(includes_list[0]) + '\n') + else: + wrapper_file += reduce(lambda x, y: str(x) + '\n' + str(y), includes_list) + wrapper_file += '\n' typedef_instances = '\n' typedef_collectors = '' diff --git a/gtwrap/pybind_wrapper.py b/gtwrap/pybind_wrapper.py index c0e88e37a..a045afcbd 100755 --- a/gtwrap/pybind_wrapper.py +++ b/gtwrap/pybind_wrapper.py @@ -76,6 +76,21 @@ class PybindWrapper(object): gtsam::deserialize(serialized, *self); }}, py::arg("serialized")) '''.format(class_inst=cpp_class + '*')) + if cpp_method == "pickle": + if not cpp_class in self._serializing_classes: + raise ValueError("Cannot pickle a class which is not serializable") + return textwrap.dedent(''' + .def(py::pickle( + [](const {cpp_class} &a){{ // __getstate__ + /* Returns a string that encodes the state of the object */ + return py::make_tuple(gtsam::serialize(a)); + }}, + [](py::tuple t){{ // __setstate__ + {cpp_class} obj; + gtsam::deserialize(t[0].cast(), obj); + return obj; + }})) + '''.format(cpp_class=cpp_class)) is_method = isinstance(method, instantiator.InstantiatedMethod) is_static = isinstance(method, parser.StaticMethod) @@ -318,3 +333,4 @@ class PybindWrapper(object): wrapped_namespace=wrapped_namespace, boost_class_export=boost_class_export, ) + diff --git a/tests/expected-matlab/geometry_wrapper.cpp b/tests/expected-matlab/geometry_wrapper.cpp index 70f673d25..40cd184b2 100644 --- a/tests/expected-matlab/geometry_wrapper.cpp +++ b/tests/expected-matlab/geometry_wrapper.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include diff --git a/tests/expected-python/geometry_pybind.cpp b/tests/expected-python/geometry_pybind.cpp index 3eee55bf4..be6482d89 100644 --- a/tests/expected-python/geometry_pybind.cpp +++ b/tests/expected-python/geometry_pybind.cpp @@ -47,6 +47,17 @@ PYBIND11_MODULE(geometry_py, m_) { [](gtsam::Point2* self, string serialized){ gtsam::deserialize(serialized, *self); }, py::arg("serialized")) + +.def(py::pickle( + [](const gtsam::Point2 &a){ // __getstate__ + /* Returns a string that encodes the state of the object */ + return py::make_tuple(gtsam::serialize(a)); + }, + [](py::tuple t){ // __setstate__ + gtsam::Point2 obj; + gtsam::deserialize(t[0].cast(), obj); + return obj; + })) ; py::class_>(m_gtsam, "Point3") @@ -62,6 +73,17 @@ PYBIND11_MODULE(geometry_py, m_) { gtsam::deserialize(serialized, *self); }, py::arg("serialized")) +.def(py::pickle( + [](const gtsam::Point3 &a){ // __getstate__ + /* Returns a string that encodes the state of the object */ + return py::make_tuple(gtsam::serialize(a)); + }, + [](py::tuple t){ // __setstate__ + gtsam::Point3 obj; + gtsam::deserialize(t[0].cast(), obj); + return obj; + })) + .def_static("staticFunction",[](){return gtsam::Point3::staticFunction();}) .def_static("StaticFunctionRet",[]( double z){return gtsam::Point3::StaticFunctionRet(z);}, py::arg("z")); diff --git a/tests/geometry.h b/tests/geometry.h index 40d878c9f..ec5d3b277 100644 --- a/tests/geometry.h +++ b/tests/geometry.h @@ -22,6 +22,9 @@ class Point2 { VectorNotEigen vectorConfusion(); void serializable() const; // Sets flag and creates export, but does not make serialization functions + + // enable pickling in python + void pickle() const; }; #include @@ -35,6 +38,9 @@ class Point3 { // enabling serialization functionality void serialize() const; // Just triggers a flag internally and removes actual function + + // enable pickling in python + void pickle() const; }; } From bddd7e68eba3b85daf8f1ae8d4888acd83916c62 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 10 Mar 2021 09:45:19 -0500 Subject: [PATCH 083/106] 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 084/106] 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 085/106] 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 086/106] 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; From faf004347ba984c6b235c3264c6f5c1fe368291b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Mar 2021 15:58:12 -0500 Subject: [PATCH 087/106] Fix Matlab tests and add saveGraph method to GaussianBayesNet --- gtsam/gtsam.i | 2 ++ gtsam/linear/GaussianBayesNet.cpp | 25 ++++++++++++++++++++++--- gtsam/linear/GaussianBayesNet.h | 10 ++++++++++ matlab/+gtsam/plotBayesNet.m | 4 +++- matlab/+gtsam/plotBayesTree.m | 4 +++- matlab/gtsam_tests/testThinBayesTree.m | 2 +- matlab/gtsam_tests/testTriangulation.m | 10 +++++----- matlab/gtsam_tests/thinBayesTree.m | 3 ++- matlab/gtsam_tests/thinTreeBayesNet.m | 13 ++++++++----- 9 files changed, 56 insertions(+), 17 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 22c2cc17d..b8cc15af2 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1791,6 +1791,8 @@ virtual class GaussianBayesNet { gtsam::KeySet keys() const; bool exists(size_t idx) const; + void saveGraph(const string& s) const; + gtsam::GaussianConditional* front() const; gtsam::GaussianConditional* back() const; void push_back(gtsam::GaussianConditional* conditional); diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 04094d593..1e790d0f1 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -12,15 +12,16 @@ /** * @file GaussianBayesNet.cpp * @brief Chordal Bayes Net, the result of eliminating a factor graph - * @author Frank Dellaert + * @author Frank Dellaert, Varun Agrawal */ +#include +#include #include #include -#include -#include #include +#include using namespace std; using namespace gtsam; @@ -204,5 +205,23 @@ namespace gtsam { } /* ************************************************************************* */ + void GaussianBayesNet::saveGraph(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::ofstream of(s.c_str()); + of << "digraph G{\n"; + + for (auto conditional : boost::adaptors::reverse(*this)) { + typename GaussianConditional::Frontals frontals = conditional->frontals(); + Key me = frontals.front(); + typename GaussianConditional::Parents parents = conditional->parents(); + for (Key p : parents) + of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl; + } + + of << "}"; + of.close(); + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 3f6d69e91..06782c3cf 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -177,6 +177,16 @@ namespace gtsam { */ VectorValues backSubstituteTranspose(const VectorValues& gx) const; + /** + * @brief Save the GaussianBayesNet as an image. Requires `dot` to be + * installed. + * + * @param s The name of the figure. + * @param keyFormatter Formatter to use for styling keys in the graph. + */ + void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + /// @} private: diff --git a/matlab/+gtsam/plotBayesNet.m b/matlab/+gtsam/plotBayesNet.m index dea9b04ad..446ffffac 100644 --- a/matlab/+gtsam/plotBayesNet.m +++ b/matlab/+gtsam/plotBayesNet.m @@ -5,4 +5,6 @@ function plotBayesNet(bayesNet) bayesNet.saveGraph('/tmp/bayesNet.dot') !dot -Tpng -o /tmp/dotImage.png /tmp/bayesNet.dot dotImage=imread('/tmp/dotImage.png'); -imshow(dotImage) \ No newline at end of file +imshow(dotImage) + +end \ No newline at end of file diff --git a/matlab/+gtsam/plotBayesTree.m b/matlab/+gtsam/plotBayesTree.m index 94628e8a5..91d0925a9 100644 --- a/matlab/+gtsam/plotBayesTree.m +++ b/matlab/+gtsam/plotBayesTree.m @@ -5,4 +5,6 @@ function plotBayesTree(bayesTree) bayesTree.saveGraph('/tmp/bayesTree.dot') !dot -Tpng -o /tmp/dotImage.png /tmp/bayesTree.dot dotImage=imread('/tmp/dotImage.png'); -imshow(dotImage) \ No newline at end of file +imshow(dotImage) + +end \ No newline at end of file diff --git a/matlab/gtsam_tests/testThinBayesTree.m b/matlab/gtsam_tests/testThinBayesTree.m index 034fc38de..747852146 100644 --- a/matlab/gtsam_tests/testThinBayesTree.m +++ b/matlab/gtsam_tests/testThinBayesTree.m @@ -20,4 +20,4 @@ %% Run the tests import gtsam.* bayesTree = thinBayesTree(3,2); -EQUALITY('7 = bayesTree.size', 7, bayesTree.size); \ No newline at end of file +EQUALITY('7 = bayesTree.size', 4, bayesTree.size); \ No newline at end of file diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m index 7116d3838..c623e7b2b 100644 --- a/matlab/gtsam_tests/testTriangulation.m +++ b/matlab/gtsam_tests/testTriangulation.m @@ -44,15 +44,15 @@ optimize = true; rank_tol = 1e-9; triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize); -CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9)); +CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-9); %% 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements = Point2Vector; -measurements.push_back(z1.retract([0.1;0.5])); -measurements.push_back(z2.retract([-0.2;0.3])); +measurements.push_back(z1 + [0.1;0.5]); +measurements.push_back(z2 + [-0.2;0.3]); triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize); -CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-2)); +CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-2); %% two Poses with Bundler Calibration bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480); @@ -67,4 +67,4 @@ measurements.push_back(z1); measurements.push_back(z2); triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize); -CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9)); +CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-9); diff --git a/matlab/gtsam_tests/thinBayesTree.m b/matlab/gtsam_tests/thinBayesTree.m index 6c2f496c6..61f89cead 100644 --- a/matlab/gtsam_tests/thinBayesTree.m +++ b/matlab/gtsam_tests/thinBayesTree.m @@ -1,5 +1,6 @@ function bayesTree = thinBayesTree(depth, width) import gtsam.* bayesNet = thinTreeBayesNet(depth, width); - bayesTree = GaussianBayesTree(bayesNet); + fg = GaussianFactorGraph(bayesNet); + bayesTree = fg.eliminateMultifrontal(); end \ No newline at end of file diff --git a/matlab/gtsam_tests/thinTreeBayesNet.m b/matlab/gtsam_tests/thinTreeBayesNet.m index d993c3fd3..b91a83ba8 100644 --- a/matlab/gtsam_tests/thinTreeBayesNet.m +++ b/matlab/gtsam_tests/thinTreeBayesNet.m @@ -6,8 +6,9 @@ bayesNet = GaussianBayesNet; tree = thinTree(depth,width); % Add root to the Bayes net -gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), 3*rand(1)); -bayesNet.push_front(gc); +model = noiseModel.Isotropic.Sigma(1, 3*rand(1)); +gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), model); +bayesNet.push_back(gc); n=tree.getNumberOfElements(); for i=2:n @@ -17,13 +18,15 @@ for i=2:n % Create and link the corresponding GaussianConditionals if tree.getW == 1 || di == 2 % Creation of single-parent GaussianConditional - gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), 5*rand(1)); + model = noiseModel.Isotropic.Sigma(1, 5*rand(1)); + gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), model); elseif tree.getW == 2 || di == 3 % GaussianConditionalj associated with the second parent - gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), 5*rand(1)); + model = noiseModel.Isotropic.Sigma(1, 5*rand(1)); + gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), model); end % Add conditional to the Bayes net - bayesNet.push_front(gc); + bayesNet.push_back(gc); end end \ No newline at end of file From b0b5b04ce8d2bad7fbb4f52dd1663374c07eeaa5 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 11 Mar 2021 09:22:40 -0800 Subject: [PATCH 088/106] Avoid derivative calcs if they aren't asked for --- gtsam/navigation/ConstantVelocityFactor.h | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 6cb349869..0e1401375 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -52,11 +52,20 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { static const Vector3 b_accel{0.0, 0.0, 0.0}; static const Vector3 b_omega{0.0, 0.0, 0.0}; + NavState predicted; Matrix predicted_H_x1; - NavState predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {}); + + if (H1) + predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {}); + else + predicted = x1.update(b_accel, b_omega, dt_, boost::none, {}, {}); Matrix error_H_predicted; - Vector9 error = predicted.localCoordinates(x2, error_H_predicted, H2); + Vector9 error; + if (H1) + error = predicted.localCoordinates(x2, error_H_predicted, H2); + else + error = predicted.localCoordinates(x2, boost::none, H2); if (H1) { *H1 = error_H_predicted * predicted_H_x1; From 11b196b52843b1a2146a509e648b02a808ddb186 Mon Sep 17 00:00:00 2001 From: Asa Hammond Date: Thu, 11 Mar 2021 11:41:16 -0800 Subject: [PATCH 089/106] Move to fixed size matrix for derivative calculations --- gtsam/navigation/ConstantVelocityFactor.h | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 0e1401375..ed68ac077 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -52,20 +52,11 @@ class ConstantVelocityFactor : public NoiseModelFactor2 { static const Vector3 b_accel{0.0, 0.0, 0.0}; static const Vector3 b_omega{0.0, 0.0, 0.0}; - NavState predicted; - Matrix predicted_H_x1; + Matrix99 predicted_H_x1; + NavState predicted = x1.update(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 : nullptr, {}, {}); - if (H1) - predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {}); - else - predicted = x1.update(b_accel, b_omega, dt_, boost::none, {}, {}); - - Matrix error_H_predicted; - Vector9 error; - if (H1) - error = predicted.localCoordinates(x2, error_H_predicted, H2); - else - error = predicted.localCoordinates(x2, boost::none, H2); + Matrix99 error_H_predicted; + Vector9 error = predicted.localCoordinates(x2, H1 ? &error_H_predicted : nullptr, H2); if (H1) { *H1 = error_H_predicted * predicted_H_x1; From e9855dcad3c1a943607a114f2f0b88f0f3f79bce Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 11 Mar 2021 15:12:49 -0500 Subject: [PATCH 090/106] temporarily turn off Python + tbb in CI because of OOM --- .github/workflows/build-python.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 345af5831..1b5293b16 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -22,7 +22,7 @@ jobs: ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9, macOS-10.15-xcode-11.3.1, - ubuntu-18.04-gcc-5-tbb, + #ubuntu-18.04-gcc-5-tbb, ] build_type: [Debug, Release] @@ -48,11 +48,11 @@ jobs: compiler: xcode version: "11.3.1" - - name: ubuntu-18.04-gcc-5-tbb - os: ubuntu-18.04 - compiler: gcc - version: "5" - flag: tbb +# - name: ubuntu-18.04-gcc-5-tbb +# os: ubuntu-18.04 +# compiler: gcc +# version: "5" +# flag: tbb steps: - name: Checkout From cfc77b7df8b97a7eea7c342a56c48193e240719f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Mar 2021 18:24:34 -0500 Subject: [PATCH 091/106] fixed inhertance --- gtsam/gtsam.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 85209ddfa..273a5e059 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1809,7 +1809,7 @@ class GaussianFactorGraph { }; #include -virtual class GaussianConditional : gtsam::GaussianFactor { +virtual class GaussianConditional : gtsam::JacobianFactor { //Constructors GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, From c37c8b794ed9dfed4d4ff1ab6299100312402a20 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Mar 2021 18:26:22 -0500 Subject: [PATCH 092/106] Added explicit header as suggested in issue #634 --- gtsam/linear/LossFunctions.h | 1 + gtsam/linear/SubgraphBuilder.h | 1 + 2 files changed, 2 insertions(+) diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 4705721a0..4e6a2b609 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -28,6 +28,7 @@ #include #include #include +#include namespace gtsam { namespace noiseModel { diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h index 5247ea2a2..88c771ba5 100644 --- a/gtsam/linear/SubgraphBuilder.h +++ b/gtsam/linear/SubgraphBuilder.h @@ -23,6 +23,7 @@ #include #include +#include #include From b890f06afe582ab35e8fec5ba5b8422e917428f7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 13 Mar 2021 12:01:05 -0500 Subject: [PATCH 093/106] clean up the CMake --- CMakeLists.txt | 27 +++++++++++++++------------ matlab/CMakeLists.txt | 3 +-- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b19ece0e6..6fc456609 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,17 +60,6 @@ include(cmake/HandleGlobalBuildFlags.cmake) # Build flags # Build CppUnitLite add_subdirectory(CppUnitLite) -# This is the new wrapper -if(GTSAM_BUILD_PYTHON) - # Need to set this for the wrap package so we don't use the default value. - set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} - CACHE STRING "The Python version to use for wrapping") - - add_subdirectory(wrap) - list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake") - add_subdirectory(python) -endif() - # Build GTSAM library add_subdirectory(gtsam) @@ -88,8 +77,22 @@ if (GTSAM_BUILD_UNSTABLE) add_subdirectory(gtsam_unstable) endif() +# This is the new wrapper +if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX) + # Need to set this for the wrap package so we don't use the default value. + set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} + CACHE STRING "The Python version to use for wrapping") + add_subdirectory(wrap) + list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake") +endif() + +# Python toolbox +if(GTSAM_BUILD_PYTHON) + add_subdirectory(python) +endif() + # Matlab toolbox -if (GTSAM_INSTALL_MATLAB_TOOLBOX) +if(GTSAM_INSTALL_MATLAB_TOOLBOX) add_subdirectory(matlab) endif() diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 1e7e88612..5de7ebac0 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -58,8 +58,7 @@ if(NOT BUILD_SHARED_LIBS) list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) endif() -set(ignore gtsam::Point2 gtsam::Point3 gtsam::BearingRangeFactor - gtsam::BearingRangeFactor2D gtsam::BearingRangeFactorPose2) +set(ignore gtsam::Point2 gtsam::Point3) # Wrap matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" From 55dade0b8e2e9d82cd06f6fdec0ce054bdc05f3a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 13 Mar 2021 12:01:28 -0500 Subject: [PATCH 094/106] Squashed 'wrap/' changes from b0eb968f2..d19cda546 d19cda546 Merge pull request #36 from borglab/fix/shared-ptr-property 13ef2485c Merge pull request #37 from borglab/feature/multiple-templates f3f40b375 add support for multiple instantiated templates 4f33353ef support for templated return types 1244045c2 cleaned up tests and added test for Pybind property shared pointer 0f3fbc428 add support for pointers as properties, and update docs 9974a73ec Merge pull request #35 from borglab/feature/docs e9c52421d Added DOCS with info about wrapping structure 627154f9f fix call to superclass init 8a4e61ead added more docs and fixed typo for holder_type b1bdec933 added docstrings to interface_parser and took care of all the errors and warnings git-subtree-dir: wrap git-subtree-split: d19cda5467f8b5cb8d4f571d8735ede328dae02d --- DOCS.md | 167 +++++++++++++ README.md | 18 +- gtwrap/interface_parser.py | 227 +++++++++++++----- gtwrap/pybind_wrapper.py | 119 +++++---- gtwrap/template_instantiator.py | 110 +++++++-- pybind_wrapper.tpl.example | 2 +- scripts/pybind_wrap.py | 8 + .../MultipleTemplatesIntDouble.m | 31 +++ .../MultipleTemplatesIntFloat.m | 31 +++ tests/expected-matlab/MyFactorPosePoint2.m | 6 +- tests/expected-matlab/aGlobalFunction.m | 2 +- tests/expected-matlab/geometry_wrapper.cpp | 154 +++++++++--- tests/expected-matlab/load2D.m | 6 +- .../overloadedGlobalFunction.m | 4 +- tests/expected-python/geometry_pybind.cpp | 9 +- tests/geometry.h | 7 + tests/pybind_wrapper.tpl | 2 +- tests/test_matlab_wrapper.py | 28 ++- tests/test_pybind_wrapper.py | 14 +- 19 files changed, 747 insertions(+), 198 deletions(-) create mode 100644 DOCS.md create mode 100644 tests/expected-matlab/MultipleTemplatesIntDouble.m create mode 100644 tests/expected-matlab/MultipleTemplatesIntFloat.m diff --git a/DOCS.md b/DOCS.md new file mode 100644 index 000000000..3acb7df4f --- /dev/null +++ b/DOCS.md @@ -0,0 +1,167 @@ +## Wrap Module Definition + +### Important + +The python wrapper supports keyword arguments for functions/methods. Hence, the argument names matter. An implementation restriction is that in overloaded methods or functions, arguments of different types *have* to have different names. + +### Requirements + +- Classes must start with an uppercase letter. + - The wrapper can wrap a typedef, e.g. `typedef TemplatedClass EasyName;`. + +- Only one Method/Constructor per line, though methods/constructors can extend across multiple lines. + +- Methods can return + - Eigen types: `Matrix`, `Vector`. + - C/C++ basic types: `string`, `bool`, `size_t`, `int`, `double`, `char`, `unsigned char`. + - `void` + - Any class with which be copied with `boost::make_shared()`. + - `boost::shared_ptr` of any object type. + +- Constructors + - Overloads are supported, but arguments of different types *have* to have different names. + - A class with no constructors can be returned from other functions but not allocated directly in MATLAB. + +- Methods + - Constness has no effect. + - Specify by-value (not reference) return types, even if C++ method returns reference. + - Must start with a letter (upper or lowercase). + - Overloads are supported. + +- Static methods + - Must start with a letter (upper or lowercase) and use the "static" keyword, e.g. `static void func()`. + - The first letter will be made uppercase in the generated MATLAB interface. + - Overloads are supported, but arguments of different types *have* to have different names. + +- Arguments to functions can be any of + - Eigen types: `Matrix`, `Vector`. + - Eigen types and classes as an optionally const reference. + - C/C++ basic types: `string`, `bool`, `size_t`, `size_t`, `double`, `char`, `unsigned char`. + - Any class with which be copied with `boost::make_shared()` (except Eigen). + - `boost::shared_ptr` of any object type (except Eigen). + +- Properties or Variables + - You can specify class variables in the interface file as long as they are in the `public` scope, e.g. + + ```cpp + class Sample { + double seed; + }; + ``` + + - Class variables are read-write so they can be updated directly in Python. + +- Pointer types + - To declare a pointer type (including shared pointers), simply add an asterisk (i.e. `*`) to the class name. + - E.g. `gtsam::noiseModel::Base*` to define the wrapping for the `Base` noise model shared pointer. + +- Comments can use either C++ or C style, with multiple lines. + +- Namespace definitions + - Names of namespaces must start with a lowercase letter. + - Start a namespace with `namespace example_ns {`, where `example_ns` is the namespace name. + - End a namespace with exactly `}` + - Namespaces can be nested. + +- Namespace usage + - Namespaces can be specified for classes in arguments and return values. + - In each case, the namespace must be fully specified, e.g., `namespace1::namespace2::ClassName`. + +- Includes in C++ wrappers + - All includes will be collected and added in a single file. + - All namespaces must have angle brackets, e.g. `#include `. + - No default includes will be added. + +- Global/Namespace functions + - Functions specified outside of a class are **global**. + - Can be overloaded with different arguments. + - Can have multiple functions of the same name in different namespaces. + +- Using classes defined in other modules + - If you are using a class `OtherClass` not wrapped in an interface file, add `class OtherClass;` as a forward declaration to avoid a dependency error. + +- Virtual inheritance + - Specify fully-qualified base classes, i.e. `virtual class Derived : ns::Base {` where `ns` is the namespace. + - Mark with `virtual` keyword, e.g. `virtual class Base {`, and also `virtual class Derived : ns::Base {`. + - Forward declarations must also be marked virtual, e.g. `virtual class ns::Base;` and + also `virtual class ns::Derived;`. + - Pure virtual (abstract) classes should list no constructors in the interface file. + - Virtual classes must have a `clone()` function in C++ (though it does not have to be included + in the interface file). `clone()` will be called whenever an object copy is needed, instead + of using the copy constructor (which is used for non-virtual objects). + - Signature of clone function - `clone()` will be called virtually, so must appear at least at the top of the inheritance tree + + ```cpp + virtual boost::shared_ptr clone() const; + ``` + +- Class Templates + - Basic templates are supported either with an explicit list of types to instantiate, + e.g. + + ```cpp + template class Class1 { ... }; + ``` + + or with typedefs, e.g. + + ```cpp + template class Class2 { ... }; + typedef Class2 MyInstantiatedClass; + ``` + + - In the class definition, appearances of the template argument(s) will be replaced with their + instantiated types, e.g. `void setValue(const T& value);`. + - To refer to the instantiation of the template class itself, use `This`, i.e. `static This Create();`. + - To create new instantiations in other modules, you must copy-and-paste the whole class definition + into the new module, but use only your new instantiation types. + - When forward-declaring template instantiations, use the generated/typedefed name, e.g. + + ```cpp + class gtsam::Class1Pose2; + class gtsam::MyInstantiatedClass; + ``` + +- `Boost.serialization` within the wrapper: + - You need to mark classes as being serializable in the markup file (see `gtsam.i` for examples). + - There are two options currently, depending on the class. To "mark" a class as serializable, + add a function with a particular signature so that `wrap` will catch it. + - Add `void serialize()` to a class to create serialization functions for a class. + Adding this flag subsumes the `serializable()` flag below. + + Requirements: + - A default constructor must be publicly accessible. + - Must not be an abstract base class. + - The class must have an actual boost.serialization `serialize()` function. + + - Add `void serializable()` to a class if you only want the class to be serialized as a + part of a container (such as `noiseModel`). This version does not require a publicly + accessible default constructor. + +- Forward declarations and class definitions for **Pybind**: + - Need to specify the base class (both this forward class and base class are declared in an external Pybind header) + This is so that Pybind can generate proper inheritance. + + Example when wrapping a gtsam-based project: + + ```cpp + // forward declarations + virtual class gtsam::NonlinearFactor + virtual class gtsam::NoiseModelFactor : gtsam::NonlinearFactor + // class definition + #include + virtual class MyFactor : gtsam::NoiseModelFactor {...}; + ``` + + - **DO NOT** re-define an overriden function already declared in the external (forward-declared) base class + - This will cause an ambiguity problem in Pybind header file. + + +### TODO +- Default values for arguments. + - WORKAROUND: make multiple versions of the same function for different configurations of default arguments. +- Handle `gtsam::Rot3M` conversions to quaternions. +- Parse return of const ref arguments. +- Parse `std::string` variants and convert directly to special string. +- Add enum support. +- Add generalized serialization support via `boost.serialization` with hooks to MATLAB save/load. diff --git a/README.md b/README.md index 1b2783a4c..2f5689db7 100644 --- a/README.md +++ b/README.md @@ -3,12 +3,14 @@ The wrap library wraps the GTSAM library into a Python library or MATLAB toolbox. It was designed to be more general than just wrapping GTSAM. For notes on creating a wrap interface, see `gtsam.h` for what features can be wrapped into a toolbox, as well as the current state of the toolbox for GTSAM. -## Prerequisites: Pybind11 and pyparsing +## Prerequisites + +`Pybind11` and `pyparsing` 1. This library uses `pybind11`, which is included as a subdirectory in GTSAM. 2. The `interface_parser.py` in this library uses `pyparsing` to parse the interface file `gtsam.h`. Please install it first in your current Python environment before attempting the build. -``` +```sh python3 -m pip install pyparsing ``` @@ -42,6 +44,10 @@ pybind_wrap(${PROJECT_NAME}_py # target For more information, please follow our [tutorial](https://github.com/borglab/gtsam-project-python). +## Documentation + +Documentation for wrapping C++ code can be found [here](https://github.com/borglab/wrap/blob/master/DOCS.md). + ## Python Wrapper **WARNING: On macOS, you have to statically build GTSAM to use the wrapper.** @@ -51,13 +57,13 @@ For more information, please follow our [tutorial](https://github.com/borglab/gt 1. Just run python then import GTSAM and play around: - ``` + ```python import gtsam gtsam.__dir__() ``` 1. Run the unittests: - ``` + ```sh python -m unittest discover ``` 1. Edit the unittests in `python/gtsam/*.py` and simply rerun the test. @@ -66,11 +72,11 @@ For more information, please follow our [tutorial](https://github.com/borglab/gt 1. Do `make install` and `cd /python`. Here, you can: 1. Run the unittests: - ``` + ```sh python setup.py test ``` 2. Install `gtsam` to your current Python environment. - ``` + ```sh python setup.py install ``` - NOTE: It's a good idea to create a virtual environment otherwise it will be installed in your system Python's site-packages. diff --git a/gtwrap/interface_parser.py b/gtwrap/interface_parser.py index 73f9ebaa1..c936cfe11 100644 --- a/gtwrap/interface_parser.py +++ b/gtwrap/interface_parser.py @@ -6,37 +6,23 @@ All Rights Reserved See LICENSE for the license information Parser to get the interface of a C++ source file -Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar and Frank Dellaert +Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -import os -import sys -from pyparsing import ( - alphas, - alphanums, - cppStyleComment, - delimitedList, - empty, - nums, - stringEnd, - CharsNotIn, - Forward, - Group, - Keyword, - Literal, - OneOrMore, - Optional, - Or, - ParseException, - ParserElement, - Suppress, - Word, - ZeroOrMore, -) +# pylint: disable=unnecessary-lambda, unused-import, expression-not-assigned, no-else-return, protected-access, too-few-public-methods, too-many-arguments + +import typing + +from pyparsing import (CharsNotIn, Forward, Group, Keyword, Literal, OneOrMore, + Optional, Or, ParseException, ParserElement, Suppress, + Word, ZeroOrMore, alphanums, alphas, cppStyleComment, + delimitedList, empty, nums, stringEnd) ParserElement.enablePackrat() +# rule for identifiers (e.g. variable names) IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums) + POINTER, REF = map(Literal, "*&") LPAREN, RPAREN, LBRACE, RBRACE, COLON, SEMI_COLON = map(Suppress, "(){}:;") LOPBRACK, ROPBRACK, COMMA, EQUAL = map(Suppress, "<>,=") @@ -70,9 +56,9 @@ BASIS_TYPES = map( ) -class Typename(object): +class Typename: """ - Type's name with full namespaces. + Type's name with full namespaces, used in Type class. """ namespaces_name_rule = delimitedList(IDENT, "::") @@ -86,34 +72,38 @@ class Typename(object): ) ).setParseAction(lambda t: Typename(t.namespaces_name, t.instantiations)) - def __init__(self, namespaces_name, instantiations=[]): + def __init__(self, namespaces_name, instantiations=()): self.namespaces = namespaces_name[:-1] self.name = namespaces_name[-1] if instantiations: - if not isinstance(instantiations, list): + if not isinstance(instantiations, typing.Iterable): self.instantiations = instantiations.asList() else: self.instantiations = instantiations else: self.instantiations = [] + if self.name in ["Matrix", "Vector"] and not self.namespaces: self.namespaces = ["gtsam"] @staticmethod def from_parse_result(parse_result): + """Return the typename from the parsed result.""" return parse_result[0] def __repr__(self): return self.to_cpp() def instantiated_name(self): + """Get the instantiated name of the type.""" res = self.name for instantiation in self.instantiations: res += instantiation.instantiated_name() return res def to_cpp(self): + """Generate the C++ code for wrapping.""" idx = 1 if self.namespaces and not self.namespaces[0] else 0 if self.instantiations: cpp_name = self.name + "<{}>".format( @@ -140,8 +130,11 @@ class Typename(object): return not res -class Type(object): - class _QualifiedType(object): +class Type: + """ + The type value that is parsed, e.g. void, string, size_t. + """ + class _QualifiedType: """ Type with qualifiers. """ @@ -165,7 +158,7 @@ class Type(object): self.is_ptr = is_ptr self.is_ref = is_ref - class _BasisType(object): + class _BasisType: """ Basis types don't have qualifiers and only allow copy-by-value. """ @@ -185,6 +178,7 @@ class Type(object): @staticmethod def from_parse_result(t): + """Return the resulting Type from parsing the source.""" if t.basis: return Type( typename=t.basis, @@ -211,6 +205,8 @@ class Type(object): def to_cpp(self, use_boost): """ + Generate the C++ code for wrapping. + Treat all pointers as "const shared_ptr&" Treat Matrix and Vector as "const Matrix&" and "const Vector&" resp. """ @@ -237,7 +233,15 @@ class Type(object): ) -class Argument(object): +class Argument: + """ + The type and name of a function/method argument. + + E.g. + ``` + void sayHello(/*s is the method argument with type `const string&`*/ const string& s); + ``` + """ rule = (Type.rule("ctype") + IDENT("name")).setParseAction( lambda t: Argument(t.ctype, t.name) ) @@ -250,7 +254,10 @@ class Argument(object): return '{} {}'.format(self.ctype.__repr__(), self.name) -class ArgumentList(object): +class ArgumentList: + """ + List of Argument objects for all arguments in a function. + """ rule = Optional(delimitedList(Argument.rule)("args_list")).setParseAction( lambda t: ArgumentList.from_parse_result(t.args_list) ) @@ -262,6 +269,7 @@ class ArgumentList(object): @staticmethod def from_parse_result(parse_result): + """Return the result of parsing.""" if parse_result: return ArgumentList(parse_result.asList()) else: @@ -271,13 +279,20 @@ class ArgumentList(object): return self.args_list.__repr__() def args_names(self): + """Return a list of the names of all the arguments.""" return [arg.name for arg in self.args_list] def to_cpp(self, use_boost): + """Generate the C++ code for wrapping.""" return [arg.ctype.to_cpp(use_boost) for arg in self.args_list] -class ReturnType(object): +class ReturnType: + """ + Rule to parse the return type. + + The return type can either be a single type or a pair such as . + """ _pair = ( PAIR.suppress() + LOPBRACK @@ -295,6 +310,9 @@ class ReturnType(object): self.type2 = type2 def is_void(self): + """ + Check if the return type is void. + """ return self.type1.typename.name == "void" and not self.type2 def __repr__(self): @@ -303,6 +321,7 @@ class ReturnType(object): ) def to_cpp(self): + """Generate the C++ code for wrapping.""" if self.type2: return "std::pair<{type1},{type2}>".format( type1=self.type1.to_cpp(), type2=self.type2.to_cpp() @@ -311,8 +330,20 @@ class ReturnType(object): return self.type1.to_cpp() -class Template(object): - class TypenameAndInstantiations(object): +class Template: + """ + Rule to parse templated values in the interface file. + + E.g. + template // this is the Template. + class Camera { ... }; + """ + class TypenameAndInstantiations: + """ + Rule to parse the template parameters. + + template // POSE is the Instantiation. + """ rule = ( IDENT("typename") + Optional( @@ -351,8 +382,21 @@ class Template(object): self.typenames = [ti.typename for ti in ti_list] self.instantiations = [ti.instantiations for ti in ti_list] + def __repr__(self): + return "<{0}>".format(", ".join(self.typenames)) -class Method(object): + +class Method: + """ + Rule to parse a method in a class. + + E.g. + ``` + class Hello { + void sayHello() const; + }; + ``` + """ rule = ( Optional(Template.rule("template")) + ReturnType.rule("return_type") @@ -387,7 +431,17 @@ class Method(object): ) -class StaticMethod(object): +class StaticMethod: + """ + Rule to parse all the static methods in a class. + + E.g. + ``` + class Hello { + static void changeGreeting(); + }; + ``` + """ rule = ( STATIC + ReturnType.rule("return_type") @@ -411,10 +465,15 @@ class StaticMethod(object): return "static {} {}{}".format(self.return_type, self.name, self.args) def to_cpp(self): + """Generate the C++ code for wrapping.""" return self.name -class Constructor(object): +class Constructor: + """ + Rule to parse the class constructor. + Can have 0 or more arguments. + """ rule = ( IDENT("name") + LPAREN @@ -433,7 +492,17 @@ class Constructor(object): return "Constructor: {}".format(self.name) -class Property(object): +class Property: + """ + Rule to parse the variable members of a class. + + E.g. + ``` + class Hello { + string name; // This is a property. + }; + ```` + """ rule = (Type.rule("ctype") + IDENT("name") + SEMI_COLON).setParseAction( lambda t: Property(t.ctype, t.name) ) @@ -441,10 +510,6 @@ class Property(object): def __init__(self, ctype, name, parent=''): self.ctype = ctype self.name = name - # Check type constraints: no pointer, no ref. - if self.ctype.is_ptr or self.ctype.is_ref: - raise ValueError("Can't deal with pointer/ref class properties.") - self.parent = parent def __repr__(self): @@ -452,6 +517,7 @@ class Property(object): def collect_namespaces(obj): + """Get the chain of namespaces from the lowest to highest for the given object.""" namespaces = [] ancestor = obj.parent while ancestor and ancestor.name: @@ -460,8 +526,21 @@ def collect_namespaces(obj): return [''] + namespaces -class Class(object): - class MethodsAndProperties(object): +class Class: + """ + Rule to parse a class defined in the interface file. + + E.g. + ``` + class Hello { + ... + }; + ``` + """ + class MethodsAndProperties: + """ + Rule for all the methods and properties within a class. + """ rule = ZeroOrMore( Constructor.rule ^ StaticMethod.rule ^ Method.rule ^ Property.rule ).setParseAction(lambda t: Class.MethodsAndProperties(t.asList())) @@ -549,10 +628,19 @@ class Class(object): _property.parent = self def namespaces(self): + """Get the namespaces which this class is nested under as a list.""" return collect_namespaces(self) -class TypedefTemplateInstantiation(object): +class TypedefTemplateInstantiation: + """ + Rule for parsing typedefs (with templates) within the interface file. + + E.g. + ``` + typedef SuperComplexName EasierName; + ``` + """ rule = ( TYPEDEF + Typename.rule("typename") + IDENT("new_name") + SEMI_COLON ).setParseAction( @@ -567,7 +655,10 @@ class TypedefTemplateInstantiation(object): self.parent = parent -class Include(object): +class Include: + """ + Rule to parse #include directives. + """ rule = ( INCLUDE + LOPBRACK + CharsNotIn('>')("header") + ROPBRACK ).setParseAction(lambda t: Include(t.header)) @@ -580,7 +671,10 @@ class Include(object): return "#include <{}>".format(self.header) -class ForwardDeclaration(object): +class ForwardDeclaration: + """ + Rule to parse forward declarations in the interface file. + """ rule = ( Optional(VIRTUAL("is_virtual")) + CLASS @@ -606,7 +700,10 @@ class ForwardDeclaration(object): ) -class GlobalFunction(object): +class GlobalFunction: + """ + Rule to parse functions defined in the global scope. + """ rule = ( ReturnType.rule("return_type") + IDENT("name") @@ -634,10 +731,18 @@ class GlobalFunction(object): ) def to_cpp(self): + """Generate the C++ code for wrapping.""" return self.name def find_sub_namespace(namespace, str_namespaces): + """ + Get the namespaces nested under `namespace`, filtered by a list of namespace strings. + + Args: + namespace: The top-level namespace under which to find sub-namespaces. + str_namespaces: The list of namespace strings to filter against. + """ if not str_namespaces: return [namespace] @@ -659,7 +764,8 @@ def find_sub_namespace(namespace, str_namespaces): return res -class Namespace(object): +class Namespace: + """Rule for parsing a namespace in the interface file.""" rule = Forward() rule << ( NAMESPACE @@ -687,6 +793,7 @@ class Namespace(object): @staticmethod def from_parse_result(t): + """Return the result of parsing.""" if t.content: content = t.content.asList() else: @@ -717,6 +824,7 @@ class Namespace(object): return res[0] def top_level(self): + """Return the top leve namespace.""" if self.name == '' or self.parent == '': return self else: @@ -726,15 +834,23 @@ class Namespace(object): return "Namespace: {}\n\t{}".format(self.name, self.content) def full_namespaces(self): + """Get the full namespace list.""" ancestors = collect_namespaces(self) if self.name: ancestors.append(self.name) return ancestors -class Module(object): +class Module: """ Module is just a global namespace. + + E.g. + ``` + namespace gtsam { + ... + } + ``` """ rule = ( @@ -752,5 +868,6 @@ class Module(object): rule.ignore(cppStyleComment) @staticmethod - def parseString(str): - return Module.rule.parseString(str)[0] + def parseString(s: str): + """Parse the source string and apply the rules.""" + return Module.rule.parseString(s)[0] diff --git a/gtwrap/pybind_wrapper.py b/gtwrap/pybind_wrapper.py index a045afcbd..986cdd8b1 100755 --- a/gtwrap/pybind_wrapper.py +++ b/gtwrap/pybind_wrapper.py @@ -7,8 +7,11 @@ All Rights Reserved See LICENSE for the license information Code generator for wrapping a C++ module with Pybind11 -Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar and Frank Dellaert +Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ + +# pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument + import re import textwrap @@ -16,13 +19,16 @@ import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator -class PybindWrapper(object): +class PybindWrapper: + """ + Class to generate binding code for Pybind11 specifically. + """ def __init__(self, module, module_name, top_module_namespaces='', use_boost=False, - ignore_classes=[], + ignore_classes=(), module_template=""): self.module = module self.module_name = module_name @@ -34,6 +40,7 @@ class PybindWrapper(object): self.python_keywords = ['print', 'lambda'] def _py_args_names(self, args_list): + """Set the argument names in Pybind11 format.""" names = args_list.args_names() if names: py_args = ['py::arg("{}")'.format(name) for name in names] @@ -42,6 +49,7 @@ class PybindWrapper(object): return '' def _method_args_signature_with_names(self, args_list): + """Define the method signature types with the argument names.""" cpp_types = args_list.to_cpp(self.use_boost) names = args_list.args_names() types_names = ["{} {}".format(ctype, name) for ctype, name in zip(cpp_types, names)] @@ -49,6 +57,7 @@ class PybindWrapper(object): return ','.join(types_names) def wrap_ctors(self, my_class): + """Wrap the constructors.""" res = "" for ctor in my_class.ctors: res += ('\n' + ' ' * 8 + '.def(py::init<{args_cpp_types}>()' @@ -115,8 +124,10 @@ class PybindWrapper(object): '{py_args_names}){suffix}'.format( prefix=prefix, cdef="def_static" if is_static else "def", - py_method=py_method if not py_method in self.python_keywords else py_method + "_", - opt_self="{cpp_class}* self".format(cpp_class=cpp_class) if is_method else "", + py_method=py_method if not py_method in self.python_keywords + else py_method + "_", + opt_self="{cpp_class}* self".format( + cpp_class=cpp_class) if is_method else "", cpp_class=cpp_class, cpp_method=cpp_method, opt_comma=',' if is_method and args_names else '', @@ -152,6 +163,7 @@ class PybindWrapper(object): return ret def wrap_methods(self, methods, cpp_class, prefix='\n' + ' ' * 8, suffix=''): + """Wrap all the methods in the `cpp_class`.""" res = "" for method in methods: @@ -176,6 +188,7 @@ class PybindWrapper(object): return res def wrap_properties(self, properties, cpp_class, prefix='\n' + ' ' * 8): + """Wrap all the properties in the `cpp_class`.""" res = "" for prop in properties: res += ('{prefix}.def_{property}("{property_name}", ' @@ -188,50 +201,61 @@ class PybindWrapper(object): return res def wrap_instantiated_class(self, instantiated_class): + """Wrap the class.""" module_var = self._gen_module_var(instantiated_class.namespaces()) cpp_class = instantiated_class.cpp_class() if cpp_class in self.ignore_classes: return "" - return ('\n py::class_<{cpp_class}, {class_parent}' - '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")' - '{wrapped_ctors}' - '{wrapped_methods}' - '{wrapped_static_methods}' - '{wrapped_properties};\n'.format( - shared_ptr_type=('boost' if self.use_boost else 'std'), - cpp_class=cpp_class, - class_name=instantiated_class.name, - class_parent=str(instantiated_class.parent_class) + - (', ' if instantiated_class.parent_class else ''), - module_var=module_var, - wrapped_ctors=self.wrap_ctors(instantiated_class), - wrapped_methods=self.wrap_methods(instantiated_class.methods, cpp_class), - wrapped_static_methods=self.wrap_methods(instantiated_class.static_methods, cpp_class), - wrapped_properties=self.wrap_properties(instantiated_class.properties, cpp_class), - )) + return ( + '\n py::class_<{cpp_class}, {class_parent}' + '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")' + '{wrapped_ctors}' + '{wrapped_methods}' + '{wrapped_static_methods}' + '{wrapped_properties};\n'.format( + shared_ptr_type=('boost' if self.use_boost else 'std'), + cpp_class=cpp_class, + class_name=instantiated_class.name, + class_parent=str(instantiated_class.parent_class) + + (', ' if instantiated_class.parent_class else ''), + module_var=module_var, + wrapped_ctors=self.wrap_ctors(instantiated_class), + wrapped_methods=self.wrap_methods(instantiated_class.methods, + cpp_class), + wrapped_static_methods=self.wrap_methods( + instantiated_class.static_methods, cpp_class), + wrapped_properties=self.wrap_properties( + instantiated_class.properties, cpp_class), + )) def wrap_stl_class(self, stl_class): + """Wrap STL containers.""" module_var = self._gen_module_var(stl_class.namespaces()) cpp_class = stl_class.cpp_class() if cpp_class in self.ignore_classes: return "" - return ('\n py::class_<{cpp_class}, {class_parent}' - '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")' - '{wrapped_ctors}' - '{wrapped_methods}' - '{wrapped_static_methods}' - '{wrapped_properties};\n'.format( - shared_ptr_type=('boost' if self.use_boost else 'std'), - cpp_class=cpp_class, - class_name=stl_class.name, - class_parent=str(stl_class.parent_class) + (', ' if stl_class.parent_class else ''), - module_var=module_var, - wrapped_ctors=self.wrap_ctors(stl_class), - wrapped_methods=self.wrap_methods(stl_class.methods, cpp_class), - wrapped_static_methods=self.wrap_methods(stl_class.static_methods, cpp_class), - wrapped_properties=self.wrap_properties(stl_class.properties, cpp_class), - )) + return ( + '\n py::class_<{cpp_class}, {class_parent}' + '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")' + '{wrapped_ctors}' + '{wrapped_methods}' + '{wrapped_static_methods}' + '{wrapped_properties};\n'.format( + shared_ptr_type=('boost' if self.use_boost else 'std'), + cpp_class=cpp_class, + class_name=stl_class.name, + class_parent=str(stl_class.parent_class) + + (', ' if stl_class.parent_class else ''), + module_var=module_var, + wrapped_ctors=self.wrap_ctors(stl_class), + wrapped_methods=self.wrap_methods(stl_class.methods, + cpp_class), + wrapped_static_methods=self.wrap_methods( + stl_class.static_methods, cpp_class), + wrapped_properties=self.wrap_properties( + stl_class.properties, cpp_class), + )) def _partial_match(self, namespaces1, namespaces2): for i in range(min(len(namespaces1), len(namespaces2))): @@ -252,6 +276,7 @@ class PybindWrapper(object): return name def wrap_namespace(self, namespace): + """Wrap the complete `namespace`.""" wrapped = "" includes = "" @@ -298,7 +323,10 @@ class PybindWrapper(object): wrapped += self.wrap_instantiated_class(element) # Global functions. - all_funcs = [func for func in namespace.content if isinstance(func, parser.GlobalFunction)] + all_funcs = [ + func for func in namespace.content + if isinstance(func, parser.GlobalFunction) + ] wrapped += self.wrap_methods( all_funcs, self._add_namespaces('', namespaces)[:-2], @@ -308,6 +336,7 @@ class PybindWrapper(object): return wrapped, includes def wrap(self): + """Wrap the code in the interface file.""" wrapped_namespace, includes = self.wrap_namespace(self.module) # Export classes for serialization. @@ -323,14 +352,16 @@ class PybindWrapper(object): ) boost_class_export += "BOOST_CLASS_EXPORT({new_name})\n".format(new_name=new_name, ) + holder_type = "PYBIND11_DECLARE_HOLDER_TYPE(TYPE_PLACEHOLDER_DONOTUSE, " \ + "{shared_ptr_type}::shared_ptr);" + include_boost = "#include " if self.use_boost else "" + return self.module_template.format( - include_boost="#include " if self.use_boost else "", + include_boost=include_boost, module_name=self.module_name, includes=includes, - hoder_type= - "PYBIND11_DECLARE_HOLDER_TYPE(TYPE_PLACEHOLDER_DONOTUSE, {shared_ptr_type}::shared_ptr);" - .format(shared_ptr_type=('boost' if self.use_boost else 'std')) if self.use_boost else "", + holder_type=holder_type.format(shared_ptr_type=('boost' if self.use_boost else 'std')) + if self.use_boost else "", wrapped_namespace=wrapped_namespace, boost_class_export=boost_class_export, ) - diff --git a/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py index 6032beac4..331b20d02 100644 --- a/gtwrap/template_instantiator.py +++ b/gtwrap/template_instantiator.py @@ -1,15 +1,45 @@ +"""Code to help instantiate templated classes, methods and functions.""" + +# pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, unused-variable + +import itertools +from copy import deepcopy +from typing import List + import gtwrap.interface_parser as parser -def instantiate_type(ctype, template_typenames, instantiations, cpp_typename, instantiated_class=None): +def instantiate_type(ctype: parser.Type, + template_typenames: List[str], + instantiations: List[parser.Typename], + cpp_typename: parser.Typename, + instantiated_class=None): """ Instantiate template typename for @p ctype. + + Args: + instiated_class (InstantiatedClass): + @return If ctype's name is in the @p template_typenames, return the corresponding type to replace in @p instantiations. If ctype name is `This`, return the new typename @p `cpp_typename`. Otherwise, return the original ctype. """ + # make a deep copy so that there is no overwriting of original template params + ctype = deepcopy(ctype) + + # Check if the return type has template parameters + if len(ctype.typename.instantiations) > 0: + for idx, instantiation in enumerate(ctype.typename.instantiations): + if instantiation.name in template_typenames: + template_idx = template_typenames.index(instantiation.name) + ctype.typename.instantiations[idx] = instantiations[ + template_idx] + + return ctype + str_arg_typename = str(ctype.typename) + if str_arg_typename in template_typenames: idx = template_typenames.index(str_arg_typename) return parser.Type( @@ -20,7 +50,6 @@ def instantiate_type(ctype, template_typenames, instantiations, cpp_typename, in is_basis=ctype.is_basis, ) elif str_arg_typename == 'This': - # import sys if instantiated_class: name = instantiated_class.original.name namespaces_name = instantiated_class.namespaces() @@ -29,8 +58,8 @@ def instantiate_type(ctype, template_typenames, instantiations, cpp_typename, in # ctype, instantiations, cpp_typename, instantiated_class.instantiations # ), file=sys.stderr) cpp_typename = parser.Typename( - namespaces_name, instantiations=[inst for inst in instantiated_class.instantiations] - ) + namespaces_name, + instantiations=instantiated_class.instantiations) return parser.Type( typename=cpp_typename, is_const=ctype.is_const, @@ -70,12 +99,18 @@ def instantiate_args_list(args_list, template_typenames, instantiations, def instantiate_return_type(return_type, template_typenames, instantiations, cpp_typename, instantiated_class=None): - new_type1 = instantiate_type( - return_type.type1, template_typenames, instantiations, cpp_typename, instantiated_class=instantiated_class) + """Instantiate the return type.""" + new_type1 = instantiate_type(return_type.type1, + template_typenames, + instantiations, + cpp_typename, + instantiated_class=instantiated_class) if return_type.type2: - new_type2 = instantiate_type( - return_type.type2, template_typenames, instantiations, - cpp_typename, instantiated_class=instantiated_class) + new_type2 = instantiate_type(return_type.type2, + template_typenames, + instantiations, + cpp_typename, + instantiated_class=instantiated_class) else: new_type2 = '' return parser.ReturnType(new_type1, new_type2) @@ -91,7 +126,7 @@ def instantiate_name(original_name, instantiations): inst_name = '' return "{}{}".format(original_name, "".join( - [inst.instantiated_name() for inst in instantiations])) + [inst.instantiated_name().capitalize().replace('_', '') for inst in instantiations])) class InstantiatedMethod(parser.Method): @@ -111,6 +146,7 @@ class InstantiatedMethod(parser.Method): self.return_type = original.return_type self.args = original.args else: + #TODO(Varun) enable multiple templates for methods if len(self.original.template.typenames) > 1: raise ValueError("Can only instantiate template method with " "single template parameter.") @@ -133,11 +169,20 @@ class InstantiatedMethod(parser.Method): ) self.args = parser.ArgumentList(instantiated_args) + super().__init__(self.template, + self.name, + self.return_type, + self.args, + self.is_const, + parent=self.parent) + def to_cpp(self): + """Generate the C++ code for wrapping.""" if self.original.template: - return "{}<{}>".format(self.original.name, self.instantiation) + ret = "{}<{}>".format(self.original.name, self.instantiation) else: - return self.original.name + ret = self.original.name + return ret def __repr__(self): return "Instantiated {}".format( @@ -146,7 +191,10 @@ class InstantiatedMethod(parser.Method): class InstantiatedClass(parser.Class): - def __init__(self, original, instantiations=[], new_name=''): + """ + Instantiate the class defined in the interface file. + """ + def __init__(self, original, instantiations=(), new_name=''): """ Template Instantiations: [T1, U1] @@ -190,6 +238,18 @@ class InstantiatedClass(parser.Class): for inst in method.template.instantiations[0]: self.methods.append(InstantiatedMethod(method, inst)) + super().__init__( + self.template, + self.is_virtual, + self.name, + [self.parent_class], + self.ctors, + self.methods, + self.static_methods, + self.properties, + parent=self.parent, + ) + def __repr__(self): return "{virtual} class {name} [{cpp_class}]: {parent_class}\n"\ "{ctors}\n{static_methods}\n{methods}".format( @@ -204,6 +264,7 @@ class InstantiatedClass(parser.Class): ) def instantiate_ctors(self): + """Instantiate the class constructors.""" instantiated_ctors = [] for ctor in self.original.ctors: instantiated_args = instantiate_args_list( @@ -220,6 +281,7 @@ class InstantiatedClass(parser.Class): return instantiated_ctors def instantiate_static_methods(self): + """Instantiate static methods in the class.""" instantiated_static_methods = [] for static_method in self.original.static_methods: instantiated_args = instantiate_args_list( @@ -274,6 +336,7 @@ class InstantiatedClass(parser.Class): return class_instantiated_methods def instantiate_properties(self): + """Instantiate the class properties.""" instantiated_properties = instantiate_args_list( self.original.properties, self.original.template.typenames, @@ -283,6 +346,7 @@ class InstantiatedClass(parser.Class): return instantiated_properties def cpp_class(self): + """Generate the C++ code for wrapping.""" return self.cpp_typename().to_cpp() def cpp_typename(self): @@ -303,7 +367,10 @@ class InstantiatedClass(parser.Class): def instantiate_namespace_inplace(namespace): """ - @param[in/out] namespace The namespace which content will be replaced with + Instantiate the classes and other elements in the `namespace` content and + assign it back to the namespace content attribute. + + @param[in/out] namespace The namespace whose content will be replaced with the instantiated content. """ instantiated_content = [] @@ -316,15 +383,14 @@ def instantiate_namespace_inplace(namespace): instantiated_content.append( InstantiatedClass(original_class, [])) else: - if (len(original_class.template.typenames) > 1 - and original_class.template.instantiations[0]): - raise ValueError( - "Can't instantiate multi-parameter templates here. " - "Please use typedef template instantiation." - ) - for inst in original_class.template.instantiations[0]: + # Use itertools to get all possible combinations of instantiations + # Works even if one template does not have an instantiation list + for instantiations in itertools.product( + *original_class.template.instantiations): instantiated_content.append( - InstantiatedClass(original_class, [inst])) + InstantiatedClass(original_class, + list(instantiations))) + elif isinstance(element, parser.TypedefTemplateInstantiation): typedef_inst = element original_class = namespace.top_level().find_class( diff --git a/pybind_wrapper.tpl.example b/pybind_wrapper.tpl.example index 1bdd55140..2260e5406 100644 --- a/pybind_wrapper.tpl.example +++ b/pybind_wrapper.tpl.example @@ -11,7 +11,7 @@ {boost_class_export} -{hoder_type} +{holder_type} #include "python/preamble.h" diff --git a/scripts/pybind_wrap.py b/scripts/pybind_wrap.py index e641cfaaf..26e63d51c 100644 --- a/scripts/pybind_wrap.py +++ b/scripts/pybind_wrap.py @@ -6,6 +6,8 @@ This script is installed via CMake to the user's binary directory and invoked during the wrapping by CMake. """ +# pylint: disable=import-error + import argparse import gtwrap.interface_parser as parser @@ -68,13 +70,16 @@ def main(): if top_module_namespaces[0]: top_module_namespaces = [''] + top_module_namespaces + # Read in the complete interface (.i) file with open(args.src, "r") as f: content = f.read() + module = parser.Module.parseString(content) instantiator.instantiate_namespace_inplace(module) with open(args.template, "r") as f: template_content = f.read() + wrapper = PybindWrapper( module=module, module_name=args.module_name, @@ -84,7 +89,10 @@ def main(): module_template=template_content, ) + # Wrap the code and get back the cpp/cc code. cc_content = wrapper.wrap() + + # Generate the C++ code which Pybind11 will use. with open(args.out, "w") as f: f.write(cc_content) diff --git a/tests/expected-matlab/MultipleTemplatesIntDouble.m b/tests/expected-matlab/MultipleTemplatesIntDouble.m new file mode 100644 index 000000000..d5a27b199 --- /dev/null +++ b/tests/expected-matlab/MultipleTemplatesIntDouble.m @@ -0,0 +1,31 @@ +%class MultipleTemplatesIntDouble, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +classdef MultipleTemplatesIntDouble < handle + properties + ptr_MultipleTemplatesIntDouble = 0 + end + methods + function obj = MultipleTemplatesIntDouble(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + geometry_wrapper(89, my_ptr); + else + error('Arguments do not match any overload of MultipleTemplatesIntDouble constructor'); + end + obj.ptr_MultipleTemplatesIntDouble = my_ptr; + end + + function delete(obj) + geometry_wrapper(90, obj.ptr_MultipleTemplatesIntDouble); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/tests/expected-matlab/MultipleTemplatesIntFloat.m b/tests/expected-matlab/MultipleTemplatesIntFloat.m new file mode 100644 index 000000000..0434c6c79 --- /dev/null +++ b/tests/expected-matlab/MultipleTemplatesIntFloat.m @@ -0,0 +1,31 @@ +%class MultipleTemplatesIntFloat, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +classdef MultipleTemplatesIntFloat < handle + properties + ptr_MultipleTemplatesIntFloat = 0 + end + methods + function obj = MultipleTemplatesIntFloat(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + geometry_wrapper(91, my_ptr); + else + error('Arguments do not match any overload of MultipleTemplatesIntFloat constructor'); + end + obj.ptr_MultipleTemplatesIntFloat = my_ptr; + end + + function delete(obj) + geometry_wrapper(92, obj.ptr_MultipleTemplatesIntFloat); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/tests/expected-matlab/MyFactorPosePoint2.m b/tests/expected-matlab/MyFactorPosePoint2.m index 04381f1dc..d711c5325 100644 --- a/tests/expected-matlab/MyFactorPosePoint2.m +++ b/tests/expected-matlab/MyFactorPosePoint2.m @@ -12,9 +12,9 @@ classdef MyFactorPosePoint2 < handle function obj = MyFactorPosePoint2(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - geometry_wrapper(89, my_ptr); + geometry_wrapper(93, my_ptr); elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base') - my_ptr = geometry_wrapper(90, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + my_ptr = geometry_wrapper(94, varargin{1}, varargin{2}, varargin{3}, varargin{4}); else error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); end @@ -22,7 +22,7 @@ classdef MyFactorPosePoint2 < handle end function delete(obj) - geometry_wrapper(91, obj.ptr_MyFactorPosePoint2); + geometry_wrapper(95, obj.ptr_MyFactorPosePoint2); end function display(obj), obj.print(''); end diff --git a/tests/expected-matlab/aGlobalFunction.m b/tests/expected-matlab/aGlobalFunction.m index 0f0b225fa..8f1c65821 100644 --- a/tests/expected-matlab/aGlobalFunction.m +++ b/tests/expected-matlab/aGlobalFunction.m @@ -1,6 +1,6 @@ function varargout = aGlobalFunction(varargin) if length(varargin) == 0 - varargout{1} = geometry_wrapper(95, varargin{:}); + varargout{1} = geometry_wrapper(99, varargin{:}); else error('Arguments do not match any overload of function aGlobalFunction'); end diff --git a/tests/expected-matlab/geometry_wrapper.cpp b/tests/expected-matlab/geometry_wrapper.cpp index 40cd184b2..98d723fab 100644 --- a/tests/expected-matlab/geometry_wrapper.cpp +++ b/tests/expected-matlab/geometry_wrapper.cpp @@ -11,9 +11,11 @@ typedef MyTemplate MyTemplatePoint2; typedef MyTemplate MyTemplateMatrix; -typedef PrimitiveRef PrimitiveRefdouble; +typedef PrimitiveRef PrimitiveRefDouble; typedef MyVector<3> MyVector3; typedef MyVector<12> MyVector12; +typedef MultipleTemplates MultipleTemplatesIntDouble; +typedef MultipleTemplates MultipleTemplatesIntFloat; typedef MyFactor MyFactorPosePoint2; BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); @@ -31,12 +33,16 @@ typedef std::set*> Collector_MyTemplatePoint static Collector_MyTemplatePoint2 collector_MyTemplatePoint2; typedef std::set*> Collector_MyTemplateMatrix; static Collector_MyTemplateMatrix collector_MyTemplateMatrix; -typedef std::set*> Collector_PrimitiveRefdouble; -static Collector_PrimitiveRefdouble collector_PrimitiveRefdouble; +typedef std::set*> Collector_PrimitiveRefDouble; +static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; typedef std::set*> Collector_MyVector3; static Collector_MyVector3 collector_MyVector3; typedef std::set*> Collector_MyVector12; static Collector_MyVector12 collector_MyVector12; +typedef std::set*> Collector_MultipleTemplatesIntDouble; +static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; +typedef std::set*> Collector_MultipleTemplatesIntFloat; +static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; @@ -82,10 +88,10 @@ void _deleteAllObjects() collector_MyTemplateMatrix.erase(iter++); anyDeleted = true; } } - { for(Collector_PrimitiveRefdouble::iterator iter = collector_PrimitiveRefdouble.begin(); - iter != collector_PrimitiveRefdouble.end(); ) { + { for(Collector_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); + iter != collector_PrimitiveRefDouble.end(); ) { delete *iter; - collector_PrimitiveRefdouble.erase(iter++); + collector_PrimitiveRefDouble.erase(iter++); anyDeleted = true; } } { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); @@ -100,6 +106,18 @@ void _deleteAllObjects() collector_MyVector12.erase(iter++); anyDeleted = true; } } + { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); + iter != collector_MultipleTemplatesIntDouble.end(); ) { + delete *iter; + collector_MultipleTemplatesIntDouble.erase(iter++); + anyDeleted = true; + } } + { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); + iter != collector_MultipleTemplatesIntFloat.end(); ) { + delete *iter; + collector_MultipleTemplatesIntFloat.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -912,42 +930,42 @@ void MyTemplateMatrix_Level_78(int nargout, mxArray *out[], int nargin, const mx out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplateMatrix", false); } -void PrimitiveRefdouble_collectorInsertAndMakeBase_79(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_collectorInsertAndMakeBase_79(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; Shared *self = *reinterpret_cast (mxGetData(in[0])); - collector_PrimitiveRefdouble.insert(self); + collector_PrimitiveRefDouble.insert(self); } -void PrimitiveRefdouble_constructor_80(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_constructor_80(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; Shared *self = new Shared(new PrimitiveRef()); - collector_PrimitiveRefdouble.insert(self); + collector_PrimitiveRefDouble.insert(self); out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast (mxGetData(out[0])) = self; } -void PrimitiveRefdouble_deconstructor_81(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_deconstructor_81(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; - checkArguments("delete_PrimitiveRefdouble",nargout,nargin,1); + checkArguments("delete_PrimitiveRefDouble",nargout,nargin,1); Shared *self = *reinterpret_cast(mxGetData(in[0])); - Collector_PrimitiveRefdouble::iterator item; - item = collector_PrimitiveRefdouble.find(self); - if(item != collector_PrimitiveRefdouble.end()) { + Collector_PrimitiveRefDouble::iterator item; + item = collector_PrimitiveRefDouble.find(self); + if(item != collector_PrimitiveRefDouble.end()) { delete self; - collector_PrimitiveRefdouble.erase(item); + collector_PrimitiveRefDouble.erase(item); } } -void PrimitiveRefdouble_Brutal_82(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_Brutal_82(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("PrimitiveRefdouble.Brutal",nargout,nargin,1); + checkArguments("PrimitiveRefDouble.Brutal",nargout,nargin,1); double t = unwrap< double >(in[0]); out[0] = wrap_shared_ptr(boost::make_shared>(PrimitiveRef::Brutal(t)),"PrimitiveRefdouble", false); } @@ -1018,7 +1036,51 @@ void MyVector12_deconstructor_88(int nargout, mxArray *out[], int nargin, const } } -void MyFactorPosePoint2_collectorInsertAndMakeBase_89(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_89(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MultipleTemplatesIntDouble.insert(self); +} + +void MultipleTemplatesIntDouble_deconstructor_90(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr> Shared; + checkArguments("delete_MultipleTemplatesIntDouble",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MultipleTemplatesIntDouble::iterator item; + item = collector_MultipleTemplatesIntDouble.find(self); + if(item != collector_MultipleTemplatesIntDouble.end()) { + delete self; + collector_MultipleTemplatesIntDouble.erase(item); + } +} + +void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_91(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MultipleTemplatesIntFloat.insert(self); +} + +void MultipleTemplatesIntFloat_deconstructor_92(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr> Shared; + checkArguments("delete_MultipleTemplatesIntFloat",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MultipleTemplatesIntFloat::iterator item; + item = collector_MultipleTemplatesIntFloat.find(self); + if(item != collector_MultipleTemplatesIntFloat.end()) { + delete self; + collector_MultipleTemplatesIntFloat.erase(item); + } +} + +void MyFactorPosePoint2_collectorInsertAndMakeBase_93(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -1027,7 +1089,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_89(int nargout, mxArray *out[ collector_MyFactorPosePoint2.insert(self); } -void MyFactorPosePoint2_constructor_90(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_constructor_94(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -1042,7 +1104,7 @@ void MyFactorPosePoint2_constructor_90(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void MyFactorPosePoint2_deconstructor_91(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_deconstructor_95(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -1055,7 +1117,7 @@ void MyFactorPosePoint2_deconstructor_91(int nargout, mxArray *out[], int nargin } } -void load2D_92(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void load2D_96(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("load2D",nargout,nargin,5); string filename = unwrap< string >(in[0]); @@ -1067,7 +1129,7 @@ void load2D_92(int nargout, mxArray *out[], int nargin, const mxArray *in[]) out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false); } -void load2D_93(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void load2D_97(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("load2D",nargout,nargin,5); string filename = unwrap< string >(in[0]); @@ -1079,7 +1141,7 @@ void load2D_93(int nargout, mxArray *out[], int nargin, const mxArray *in[]) out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false); } -void load2D_94(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void load2D_98(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("load2D",nargout,nargin,2); string filename = unwrap< string >(in[0]); @@ -1088,18 +1150,18 @@ void load2D_94(int nargout, mxArray *out[], int nargin, const mxArray *in[]) out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false); } -void aGlobalFunction_95(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void aGlobalFunction_99(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("aGlobalFunction",nargout,nargin,0); out[0] = wrap< Vector >(aGlobalFunction()); } -void overloadedGlobalFunction_96(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void overloadedGlobalFunction_100(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("overloadedGlobalFunction",nargout,nargin,1); int a = unwrap< int >(in[0]); out[0] = wrap< Vector >(overloadedGlobalFunction(a)); } -void overloadedGlobalFunction_97(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void overloadedGlobalFunction_101(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("overloadedGlobalFunction",nargout,nargin,2); int a = unwrap< int >(in[0]); @@ -1356,16 +1418,16 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) MyTemplateMatrix_Level_78(nargout, out, nargin-1, in+1); break; case 79: - PrimitiveRefdouble_collectorInsertAndMakeBase_79(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_collectorInsertAndMakeBase_79(nargout, out, nargin-1, in+1); break; case 80: - PrimitiveRefdouble_constructor_80(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_constructor_80(nargout, out, nargin-1, in+1); break; case 81: - PrimitiveRefdouble_deconstructor_81(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_deconstructor_81(nargout, out, nargin-1, in+1); break; case 82: - PrimitiveRefdouble_Brutal_82(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_Brutal_82(nargout, out, nargin-1, in+1); break; case 83: MyVector3_collectorInsertAndMakeBase_83(nargout, out, nargin-1, in+1); @@ -1386,31 +1448,43 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) MyVector12_deconstructor_88(nargout, out, nargin-1, in+1); break; case 89: - MyFactorPosePoint2_collectorInsertAndMakeBase_89(nargout, out, nargin-1, in+1); + MultipleTemplatesIntDouble_collectorInsertAndMakeBase_89(nargout, out, nargin-1, in+1); break; case 90: - MyFactorPosePoint2_constructor_90(nargout, out, nargin-1, in+1); + MultipleTemplatesIntDouble_deconstructor_90(nargout, out, nargin-1, in+1); break; case 91: - MyFactorPosePoint2_deconstructor_91(nargout, out, nargin-1, in+1); + MultipleTemplatesIntFloat_collectorInsertAndMakeBase_91(nargout, out, nargin-1, in+1); break; case 92: - load2D_92(nargout, out, nargin-1, in+1); + MultipleTemplatesIntFloat_deconstructor_92(nargout, out, nargin-1, in+1); break; case 93: - load2D_93(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_collectorInsertAndMakeBase_93(nargout, out, nargin-1, in+1); break; case 94: - load2D_94(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_constructor_94(nargout, out, nargin-1, in+1); break; case 95: - aGlobalFunction_95(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_deconstructor_95(nargout, out, nargin-1, in+1); break; case 96: - overloadedGlobalFunction_96(nargout, out, nargin-1, in+1); + load2D_96(nargout, out, nargin-1, in+1); break; case 97: - overloadedGlobalFunction_97(nargout, out, nargin-1, in+1); + load2D_97(nargout, out, nargin-1, in+1); + break; + case 98: + load2D_98(nargout, out, nargin-1, in+1); + break; + case 99: + aGlobalFunction_99(nargout, out, nargin-1, in+1); + break; + case 100: + overloadedGlobalFunction_100(nargout, out, nargin-1, in+1); + break; + case 101: + overloadedGlobalFunction_101(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/tests/expected-matlab/load2D.m b/tests/expected-matlab/load2D.m index fec762dc5..bcea4ed76 100644 --- a/tests/expected-matlab/load2D.m +++ b/tests/expected-matlab/load2D.m @@ -1,10 +1,10 @@ function varargout = load2D(varargin) if length(varargin) == 5 && isa(varargin{1},'char') && isa(varargin{2},'Test') && isa(varargin{3},'numeric') && isa(varargin{4},'logical') && isa(varargin{5},'logical') - [ varargout{1} varargout{2} ] = geometry_wrapper(92, varargin{:}); + [ varargout{1} varargout{2} ] = geometry_wrapper(96, varargin{:}); elseif length(varargin) == 5 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.noiseModel.Diagonal') && isa(varargin{3},'numeric') && isa(varargin{4},'logical') && isa(varargin{5},'logical') - [ varargout{1} varargout{2} ] = geometry_wrapper(93, varargin{:}); + [ varargout{1} varargout{2} ] = geometry_wrapper(97, varargin{:}); elseif length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.noiseModel.Diagonal') - [ varargout{1} varargout{2} ] = geometry_wrapper(94, varargin{:}); + [ varargout{1} varargout{2} ] = geometry_wrapper(98, varargin{:}); else error('Arguments do not match any overload of function load2D'); end diff --git a/tests/expected-matlab/overloadedGlobalFunction.m b/tests/expected-matlab/overloadedGlobalFunction.m index 168c83c67..2fbaa88dc 100644 --- a/tests/expected-matlab/overloadedGlobalFunction.m +++ b/tests/expected-matlab/overloadedGlobalFunction.m @@ -1,8 +1,8 @@ function varargout = overloadedGlobalFunction(varargin) if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = geometry_wrapper(96, varargin{:}); + varargout{1} = geometry_wrapper(100, varargin{:}); elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') - varargout{1} = geometry_wrapper(97, varargin{:}); + varargout{1} = geometry_wrapper(101, varargin{:}); else error('Arguments do not match any overload of function overloadedGlobalFunction'); end diff --git a/tests/expected-python/geometry_pybind.cpp b/tests/expected-python/geometry_pybind.cpp index be6482d89..348888f25 100644 --- a/tests/expected-python/geometry_pybind.cpp +++ b/tests/expected-python/geometry_pybind.cpp @@ -115,7 +115,8 @@ PYBIND11_MODULE(geometry_py, m_) { gtsam::RedirectCout redirect; a.print(); return redirect.str(); - }); + }) + .def_readwrite("model_ptr", &Test::model_ptr); py::class_>(m_, "MyBase"); @@ -149,7 +150,7 @@ PYBIND11_MODULE(geometry_py, m_) { .def("return_ptrs",[](MyTemplate* self,const std::shared_ptr& p1,const std::shared_ptr& p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2")) .def_static("Level",[](const gtsam::Matrix& K){return MyTemplate::Level(K);}, py::arg("K")); - py::class_, std::shared_ptr>>(m_, "PrimitiveRefdouble") + py::class_, std::shared_ptr>>(m_, "PrimitiveRefDouble") .def(py::init<>()) .def_static("Brutal",[](const double& t){return PrimitiveRef::Brutal(t);}, py::arg("t")); @@ -159,6 +160,10 @@ PYBIND11_MODULE(geometry_py, m_) { py::class_, std::shared_ptr>>(m_, "MyVector12") .def(py::init<>()); + py::class_, std::shared_ptr>>(m_, "MultipleTemplatesIntDouble"); + + py::class_, std::shared_ptr>>(m_, "MultipleTemplatesIntFloat"); + py::class_, std::shared_ptr>>(m_, "MyFactorPosePoint2") .def(py::init< size_t, size_t, double, const std::shared_ptr&>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel")); diff --git a/tests/geometry.h b/tests/geometry.h index ec5d3b277..716b4307d 100644 --- a/tests/geometry.h +++ b/tests/geometry.h @@ -61,6 +61,9 @@ class Test { // another comment Test(); + // Test a shared ptr property + gtsam::noiseModel::Base* model_ptr; + pair return_pair (Vector v, Matrix A) const; // intentionally the first method pair return_pair (Vector v) const; // overload @@ -160,3 +163,7 @@ class MyVector { // comments at the end! // even more comments at the end! + +// Class with multiple instantiated templates +template +class MultipleTemplates {}; diff --git a/tests/pybind_wrapper.tpl b/tests/pybind_wrapper.tpl index 2674824b8..836075edf 100644 --- a/tests/pybind_wrapper.tpl +++ b/tests/pybind_wrapper.tpl @@ -11,7 +11,7 @@ {boost_class_export} -{hoder_type} +{holder_type} using namespace std; diff --git a/tests/test_matlab_wrapper.py b/tests/test_matlab_wrapper.py index 258f2da8f..6e7699c86 100644 --- a/tests/test_matlab_wrapper.py +++ b/tests/test_matlab_wrapper.py @@ -1,21 +1,26 @@ """ -Unit test for Matlab wrap program -Author: Matthew Sklar +Unit tests for Matlab wrap program +Author: Matthew Sklar, Varun Agrawal Date: March 2019 """ +# pylint: disable=import-error, wrong-import-position, too-many-branches + +import filecmp import os import sys import unittest -import filecmp sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -import gtwrap.template_instantiator as instantiator import gtwrap.interface_parser as parser +import gtwrap.template_instantiator as instantiator from gtwrap.matlab_wrapper import MatlabWrapper class TestWrap(unittest.TestCase): + """ + Test the Matlab wrapper + """ TEST_DIR = os.path.dirname(os.path.realpath(__file__)) + "/" MATLAB_TEST_DIR = TEST_DIR + "expected-matlab/" MATLAB_ACTUAL_DIR = TEST_DIR + "actual-matlab/" @@ -31,11 +36,11 @@ class TestWrap(unittest.TestCase): """ if path == '': path = self.MATLAB_ACTUAL_DIR + for c in cc_content: - if type(c) == list: + if isinstance(c, list): if len(c) == 0: continue - import sys print("c object: {}".format(c[0][0]), file=sys.stderr) path_to_folder = path + '/' + c[0][0] @@ -46,13 +51,12 @@ class TestWrap(unittest.TestCase): pass for sub_content in c: - import sys print("sub object: {}".format(sub_content[1][0][0]), file=sys.stderr) self.generate_content(sub_content[1], path_to_folder) - elif type(c[1]) == list: + + elif isinstance(c[1], list): path_to_folder = path + '/' + c[0] - import sys print("[generate_content_global]: {}".format(path_to_folder), file=sys.stderr) if not os.path.isdir(path_to_folder): try: @@ -60,15 +64,14 @@ class TestWrap(unittest.TestCase): except OSError: pass for sub_content in c[1]: - import sys path_to_file = path_to_folder + '/' + sub_content[0] print("[generate_global_method]: {}".format(path_to_file), file=sys.stderr) with open(path_to_file, 'w') as f: f.write(sub_content[1]) + else: path_to_file = path + '/' + c[0] - import sys print("[generate_content]: {}".format(path_to_file), file=sys.stderr) if not os.path.isdir(path_to_file): try: @@ -80,7 +83,8 @@ class TestWrap(unittest.TestCase): f.write(c[1]) def test_geometry_matlab(self): - """ Check generation of matlab geometry wrapper. + """ + Check generation of matlab geometry wrapper. python3 wrap/matlab_wrapper.py --src wrap/tests/geometry.h --module_name geometry --out wrap/tests/actual-matlab """ diff --git a/tests/test_pybind_wrapper.py b/tests/test_pybind_wrapper.py index d859cc99f..61edc3e78 100644 --- a/tests/test_pybind_wrapper.py +++ b/tests/test_pybind_wrapper.py @@ -1,27 +1,29 @@ """ Unit test for Pybind wrap program -Author: Matthew Sklar +Author: Matthew Sklar, Varun Agrawal Date: February 2019 """ +# pylint: disable=import-error, wrong-import-position, too-many-branches + +import filecmp import os +import os.path as path import sys import unittest -import filecmp - -import os.path as path sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) sys.path.append(os.path.normpath(os.path.abspath(os.path.join(__file__, '../../../build/wrap')))) -from gtwrap.pybind_wrapper import PybindWrapper import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator +from gtwrap.pybind_wrapper import PybindWrapper sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) class TestWrap(unittest.TestCase): + """Tests for Python wrapper based on Pybind11.""" TEST_DIR = os.path.dirname(os.path.realpath(__file__)) + "/" def test_geometry_python(self): @@ -39,7 +41,7 @@ class TestWrap(unittest.TestCase): with open(self.TEST_DIR + "pybind_wrapper.tpl") as template_file: module_template = template_file.read() - + # Create Pybind wrapper instance wrapper = PybindWrapper( module=module, From 51be66ff1460bfbfa850f2d4c5721d98726d9bc3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 13 Mar 2021 12:01:56 -0500 Subject: [PATCH 095/106] templated return types are now supported --- gtsam/gtsam.i | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 4269ab188..29a8cb681 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2719,7 +2719,7 @@ virtual class BearingRangeFactor : gtsam::NoiseModelFactor { const BEARING& measuredBearing, const RANGE& measuredRange, const gtsam::noiseModel::Base* noiseModel); - BearingRange measured() const; + gtsam::BearingRange measured() const; // enabling serialization functionality void serialize() const; @@ -2859,8 +2859,8 @@ class SfmTrack { double r; double g; double b; - // TODO Need to close wrap#10 to allow this to work. - // std::vector> measurements; + + std::vector> measurements; size_t number_measurements() const; pair measurement(size_t idx) const; From 6016a2d764c3d04eafcaef063e62b99af6574b44 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 13 Mar 2021 12:03:17 -0500 Subject: [PATCH 096/106] Move wrapper documentation to the wrap project --- gtsam/gtsam.i | 115 +------------------------------------------------- 1 file changed, 1 insertion(+), 114 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 29a8cb681..5abd8dcaa 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -5,120 +5,7 @@ * These are the current classes available through the matlab and python wrappers, * add more functions/classes as they are available. * - * IMPORTANT: the python wrapper supports keyword arguments for functions/methods. Hence, the - * argument names matter. An implementation restriction is that in overloaded methods - * or functions, arguments of different types *have* to have different names. - * - * Requirements: - * Classes must start with an uppercase letter - * - Can wrap a typedef - * Only one Method/Constructor per line, though methods/constructors can extend across multiple lines - * Methods can return - * - Eigen types: Matrix, Vector - * - C/C++ basic types: string, bool, size_t, int, double, char, unsigned char - * - void - * - Any class with which be copied with boost::make_shared() - * - boost::shared_ptr of any object type - * Constructors - * - Overloads are supported, but arguments of different types *have* to have different names - * - A class with no constructors can be returned from other functions but not allocated directly in MATLAB - * Methods - * - Constness has no effect - * - Specify by-value (not reference) return types, even if C++ method returns reference - * - Must start with a letter (upper or lowercase) - * - Overloads are supported - * Static methods - * - Must start with a letter (upper or lowercase) and use the "static" keyword - * - The first letter will be made uppercase in the generated MATLAB interface - * - Overloads are supported, but arguments of different types *have* to have different names - * Arguments to functions any of - * - Eigen types: Matrix, Vector - * - Eigen types and classes as an optionally const reference - * - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char - * - Any class with which be copied with boost::make_shared() (except Eigen) - * - boost::shared_ptr of any object type (except Eigen) - * Comments can use either C++ or C style, with multiple lines - * Namespace definitions - * - Names of namespaces must start with a lowercase letter - * - start a namespace with "namespace {" - * - end a namespace with exactly "}" - * - Namespaces can be nested - * Namespace usage - * - Namespaces can be specified for classes in arguments and return values - * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" - * Includes in C++ wrappers - * - All includes will be collected and added in a single file - * - All namespaces must have angle brackets: - * - No default includes will be added - * Global/Namespace functions - * - Functions specified outside of a class are global - * - Can be overloaded with different arguments - * - Can have multiple functions of the same name in different namespaces - * Using classes defined in other modules - * - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error - * Virtual inheritance - * - Specify fully-qualified base classes, i.e. "virtual class Derived : ns::Base {" where "ns" is the namespace - * - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : ns::Base {" - * - Forward declarations must also be marked virtual, e.g. "virtual class ns::Base;" and - * also "virtual class ns::Derived;" - * - Pure virtual (abstract) classes should list no constructors in this interface file - * - Virtual classes must have a clone() function in C++ (though it does not have to be included - * in the MATLAB interface). clone() will be called whenever an object copy is needed, instead - * of using the copy constructor (which is used for non-virtual objects). - * - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree - * virtual boost::shared_ptr clone() const; - * Class Templates - * - Basic templates are supported either with an explicit list of types to instantiate, - * e.g. template class Class1 { ... }; - * or with typedefs, e.g. - * template class Class2 { ... }; - * typedef Class2 MyInstantiatedClass; - * - In the class definition, appearances of the template argument(s) will be replaced with their - * instantiated types, e.g. 'void setValue(const T& value);'. - * - To refer to the instantiation of the template class itself, use 'This', i.e. 'static This Create();' - * - To create new instantiations in other modules, you must copy-and-paste the whole class definition - * into the new module, but use only your new instantiation types. - * - When forward-declaring template instantiations, use the generated/typedefed name, e.g. - * class gtsam::Class1Pose2; - * class gtsam::MyInstantiatedClass; - * Boost.serialization within Matlab: - * - you need to mark classes as being serializable in the markup file (see this file for an example). - * - There are two options currently, depending on the class. To "mark" a class as serializable, - * add a function with a particular signature so that wrap will catch it. - * - Add "void serialize()" to a class to create serialization functions for a class. - * Adding this flag subsumes the serializable() flag below. Requirements: - * - A default constructor must be publicly accessible - * - Must not be an abstract base class - * - The class must have an actual boost.serialization serialize() function. - * - Add "void serializable()" to a class if you only want the class to be serialized as a - * part of a container (such as noisemodel). This version does not require a publicly - * accessible default constructor. - * Forward declarations and class definitions for Pybind: - * - Need to specify the base class (both this forward class and base class are declared in an external Pybind header) - * This is so Pybind can generate proper inheritance. - * Example when wrapping a gtsam-based project: - * // forward declarations - * virtual class gtsam::NonlinearFactor - * virtual class gtsam::NoiseModelFactor : gtsam::NonlinearFactor - * // class definition - * #include - * virtual class MyFactor : gtsam::NoiseModelFactor {...}; - * - *DO NOT* re-define overriden function already declared in the external (forward-declared) base class - * - This will cause an ambiguity problem in Pybind header file - * Pickle support in Python: - * - Add "void pickle()" to a class to enable pickling via gtwrap. In the current implementation, "void serialize()" - * and a public constructor with no-arguments in needed for successful build. - */ - -/** - * Status: - * - TODO: default values for arguments - * - WORKAROUND: make multiple versions of the same function for different configurations of default arguments - * - TODO: Handle gtsam::Rot3M conversions to quaternions - * - TODO: Parse return of const ref arguments - * - TODO: Parse std::string variants and convert directly to special string - * - TODO: Add enum support - * - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load + * Please refer to the wrapping docs: https://github.com/borglab/wrap/blob/master/README.md */ namespace gtsam { From 1618218cffca56e8fce9c6e5949f051e46315527 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 13 Mar 2021 18:30:12 -0500 Subject: [PATCH 097/106] fix holder_type typo --- python/gtsam/gtsam.tpl | 2 +- python/gtsam_unstable/gtsam_unstable.tpl | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index 634a81e90..60b3d1fd0 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -25,7 +25,7 @@ {boost_class_export} // Holder type for pybind11 -{hoder_type} +{holder_type} // Preamble for STL classes // TODO(fan): make this automatic diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl index 1d9dfaa40..111e46d5e 100644 --- a/python/gtsam_unstable/gtsam_unstable.tpl +++ b/python/gtsam_unstable/gtsam_unstable.tpl @@ -22,7 +22,7 @@ {boost_class_export} -{hoder_type} +{holder_type} #include "python/gtsam_unstable/preamble.h" From 4af355b0eb6a52a5d744a2a24447840bd43cd174 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 13 Mar 2021 18:30:31 -0500 Subject: [PATCH 098/106] make ignore list consistent across Python and Matlab --- matlab/CMakeLists.txt | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 5de7ebac0..29f648d28 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -58,7 +58,24 @@ if(NOT BUILD_SHARED_LIBS) list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) endif() -set(ignore gtsam::Point2 gtsam::Point3) +# ignoring the non-concrete types (type aliases) +set(ignore + gtsam::Point2 + gtsam::Point3 + gtsam::LieVector + gtsam::LieMatrix + gtsam::ISAM2ThresholdMapValue + gtsam::FactorIndices + gtsam::FactorIndexSet + gtsam::IndexPairSetMap + gtsam::IndexPairVector + gtsam::BetweenFactorPose2s + gtsam::BetweenFactorPose3s + gtsam::Point2Vector + gtsam::Pose3Vector + gtsam::KeyVector + gtsam::BinaryMeasurementsUnit3 + gtsam::KeyPairDoubleMap) # Wrap matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" From ec48b14d703283baf726ab76529594bd52b2708e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 13 Mar 2021 18:31:10 -0500 Subject: [PATCH 099/106] Squashed 'wrap/' changes from d19cda546..91f3835a8 91f3835a8 Merge pull request #39 from borglab/fix/deepcopy b64e01744 don't remove underscore for instantiations 80c060b52 monkey patch deepcopy issue in pyparsing git-subtree-dir: wrap git-subtree-split: 91f3835a8a69919ccb67435c567803f162faeac7 --- gtwrap/interface_parser.py | 20 ++++++++++++++++++++ gtwrap/template_instantiator.py | 2 +- 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/gtwrap/interface_parser.py b/gtwrap/interface_parser.py index c936cfe11..b4328327f 100644 --- a/gtwrap/interface_parser.py +++ b/gtwrap/interface_parser.py @@ -11,13 +11,33 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae # pylint: disable=unnecessary-lambda, unused-import, expression-not-assigned, no-else-return, protected-access, too-few-public-methods, too-many-arguments +import sys import typing +import pyparsing from pyparsing import (CharsNotIn, Forward, Group, Keyword, Literal, OneOrMore, Optional, Or, ParseException, ParserElement, Suppress, Word, ZeroOrMore, alphanums, alphas, cppStyleComment, delimitedList, empty, nums, stringEnd) +# Fix deepcopy issue with pyparsing +# Can remove once https://github.com/pyparsing/pyparsing/issues/208 is resolved. +if sys.version_info >= (3, 8): + def fixed_get_attr(self, item): + """ + Fix for monkey-patching issue with deepcopy in pyparsing.ParseResults + """ + if item == '__deepcopy__': + raise AttributeError(item) + try: + return self[item] + except KeyError: + return "" + + # apply the monkey-patch + pyparsing.ParseResults.__getattr__ = fixed_get_attr + + ParserElement.enablePackrat() # rule for identifiers (e.g. variable names) diff --git a/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py index 331b20d02..a55a868a3 100644 --- a/gtwrap/template_instantiator.py +++ b/gtwrap/template_instantiator.py @@ -126,7 +126,7 @@ def instantiate_name(original_name, instantiations): inst_name = '' return "{}{}".format(original_name, "".join( - [inst.instantiated_name().capitalize().replace('_', '') for inst in instantiations])) + [inst.instantiated_name().capitalize() for inst in instantiations])) class InstantiatedMethod(parser.Method): From a30574fdff95a10eea97c96cb4b62fb32d5f3386 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 13 Mar 2021 18:56:36 -0500 Subject: [PATCH 100/106] Squashed 'wrap/' changes from 91f3835a8..aae9b4605 aae9b4605 Merge pull request #40 from borglab/fix/instantiation-capital dc8c67843 fix template instantiation capitalization git-subtree-dir: wrap git-subtree-split: aae9b4605b1a95fd0165f8ec3fede1fb7ffcb9c1 --- gtwrap/template_instantiator.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py index a55a868a3..fe2caf025 100644 --- a/gtwrap/template_instantiator.py +++ b/gtwrap/template_instantiator.py @@ -124,9 +124,14 @@ def instantiate_name(original_name, instantiations): namespaces, but I find that too verbose. """ inst_name = '' + instantiated_names = [] + for inst in instantiations: + # Ensure the first character of the type is capitalized + name = inst.instantiated_name() + # Using `capitalize` on the complete causes other caps to be lower case + instantiated_names.append(name.replace(name[0], name[0].capitalize())) - return "{}{}".format(original_name, "".join( - [inst.instantiated_name().capitalize() for inst in instantiations])) + return "{}{}".format(original_name, "".join(instantiated_names)) class InstantiatedMethod(parser.Method): From 1f60a7ea84161ba350efa2f1def27ec15a6f288f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 13 Mar 2021 19:40:33 -0500 Subject: [PATCH 101/106] undo ignore updates, all tests now pass --- matlab/CMakeLists.txt | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 29f648d28..c8d6d197f 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -61,21 +61,7 @@ endif() # ignoring the non-concrete types (type aliases) set(ignore gtsam::Point2 - gtsam::Point3 - gtsam::LieVector - gtsam::LieMatrix - gtsam::ISAM2ThresholdMapValue - gtsam::FactorIndices - gtsam::FactorIndexSet - gtsam::IndexPairSetMap - gtsam::IndexPairVector - gtsam::BetweenFactorPose2s - gtsam::BetweenFactorPose3s - gtsam::Point2Vector - gtsam::Pose3Vector - gtsam::KeyVector - gtsam::BinaryMeasurementsUnit3 - gtsam::KeyPairDoubleMap) + gtsam::Point3) # Wrap matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" From 43078f7cb8981740d80f214419c14750dbfdd4ea Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Wed, 17 Mar 2021 04:29:54 +0100 Subject: [PATCH 102/106] fix missing verbosityLMTranslator case --- gtsam/nonlinear/LevenbergMarquardtParams.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.cpp b/gtsam/nonlinear/LevenbergMarquardtParams.cpp index 5d558a43a..caa2cc083 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtParams.cpp @@ -37,6 +37,8 @@ LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTrans return LevenbergMarquardtParams::SILENT; if (s == "SUMMARY") return LevenbergMarquardtParams::SUMMARY; + if (s == "TERMINATION") + return LevenbergMarquardtParams::TERMINATION; if (s == "LAMBDA") return LevenbergMarquardtParams::LAMBDA; if (s == "TRYLAMBDA") From 4bf353031f6f288f3a1a623e0b8a40b6f7da3191 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 17 Mar 2021 20:22:54 -0400 Subject: [PATCH 103/106] fix the Similarity3 export declarations and wrapper --- gtsam/geometry/Similarity3.h | 48 ++++++++++++++++++------------------ gtsam/gtsam.i | 4 +-- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index d4a478310..0ef787b05 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -23,7 +23,7 @@ #include #include #include -#include +#include namespace gtsam { @@ -53,54 +53,54 @@ public: /// @{ /// Default constructor - GTSAM_UNSTABLE_EXPORT Similarity3(); + GTSAM_EXPORT Similarity3(); /// Construct pure scaling - GTSAM_UNSTABLE_EXPORT Similarity3(double s); + GTSAM_EXPORT Similarity3(double s); /// Construct from GTSAM types - GTSAM_UNSTABLE_EXPORT Similarity3(const Rot3& R, const Point3& t, double s); + GTSAM_EXPORT Similarity3(const Rot3& R, const Point3& t, double s); /// Construct from Eigen types - GTSAM_UNSTABLE_EXPORT Similarity3(const Matrix3& R, const Vector3& t, double s); + GTSAM_EXPORT Similarity3(const Matrix3& R, const Vector3& t, double s); /// Construct from matrix [R t; 0 s^-1] - GTSAM_UNSTABLE_EXPORT Similarity3(const Matrix4& T); + GTSAM_EXPORT Similarity3(const Matrix4& T); /// @} /// @name Testable /// @{ /// Compare with tolerance - GTSAM_UNSTABLE_EXPORT bool equals(const Similarity3& sim, double tol) const; + GTSAM_EXPORT bool equals(const Similarity3& sim, double tol) const; /// Exact equality - GTSAM_UNSTABLE_EXPORT bool operator==(const Similarity3& other) const; + GTSAM_EXPORT bool operator==(const Similarity3& other) const; /// Print with optional string - GTSAM_UNSTABLE_EXPORT void print(const std::string& s) const; + GTSAM_EXPORT void print(const std::string& s) const; - GTSAM_UNSTABLE_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p); + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p); /// @} /// @name Group /// @{ /// Return an identity transform - GTSAM_UNSTABLE_EXPORT static Similarity3 identity(); + GTSAM_EXPORT static Similarity3 identity(); /// Composition - GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& S) const; + GTSAM_EXPORT Similarity3 operator*(const Similarity3& S) const; /// Return the inverse - GTSAM_UNSTABLE_EXPORT Similarity3 inverse() const; + GTSAM_EXPORT Similarity3 inverse() const; /// @} /// @name Group action on Point3 /// @{ /// Action on a point p is s*(R*p+t) - GTSAM_UNSTABLE_EXPORT Point3 transformFrom(const Point3& p, // + GTSAM_EXPORT Point3 transformFrom(const Point3& p, // OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const; @@ -115,15 +115,15 @@ public: * This group action satisfies the compatibility condition. * For more details, refer to: https://en.wikipedia.org/wiki/Group_action */ - GTSAM_UNSTABLE_EXPORT Pose3 transformFrom(const Pose3& T) const; + GTSAM_EXPORT Pose3 transformFrom(const Pose3& T) const; /** syntactic sugar for transformFrom */ - GTSAM_UNSTABLE_EXPORT Point3 operator*(const Point3& p) const; + GTSAM_EXPORT Point3 operator*(const Point3& p) const; /** * Create Similarity3 by aligning at least three point pairs */ - GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPointPairs); + GTSAM_EXPORT static Similarity3 Align(const std::vector& abPointPairs); /** * Create the Similarity3 object that aligns at least two pose pairs. @@ -135,7 +135,7 @@ public: * 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); + GTSAM_EXPORT static Similarity3 Align(const std::vector& abPosePairs); /// @} /// @name Lie Group @@ -144,12 +144,12 @@ public: /** Log map at the identity * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$ */ - GTSAM_UNSTABLE_EXPORT static Vector7 Logmap(const Similarity3& s, // + GTSAM_EXPORT static Vector7 Logmap(const Similarity3& s, // OptionalJacobian<7, 7> Hm = boost::none); /** Exponential map at the identity */ - GTSAM_UNSTABLE_EXPORT static Similarity3 Expmap(const Vector7& v, // + GTSAM_EXPORT static Similarity3 Expmap(const Vector7& v, // OptionalJacobian<7, 7> Hm = boost::none); /// Chart at the origin @@ -170,17 +170,17 @@ public: * @return 4*4 element of Lie algebra that can be exponentiated * TODO(frank): rename to Hat, make part of traits */ - GTSAM_UNSTABLE_EXPORT static Matrix4 wedge(const Vector7& xi); + GTSAM_EXPORT static Matrix4 wedge(const Vector7& xi); /// Project from one tangent space to another - GTSAM_UNSTABLE_EXPORT Matrix7 AdjointMap() const; + GTSAM_EXPORT Matrix7 AdjointMap() const; /// @} /// @name Standard interface /// @{ /// Calculate 4*4 matrix group equivalent - GTSAM_UNSTABLE_EXPORT const Matrix4 matrix() const; + GTSAM_EXPORT const Matrix4 matrix() const; /// Return a GTSAM rotation const Rot3& rotation() const { @@ -199,7 +199,7 @@ public: /// Convert to a rigid body pose (R, s*t) /// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast. - GTSAM_UNSTABLE_EXPORT operator Pose3() const; + GTSAM_EXPORT operator Pose3() const; /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes inline static size_t Dim() { diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 38653ea9c..ba06dcbf2 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1037,8 +1037,8 @@ class Similarity3 { Similarity3(const Matrix& T); gtsam::Pose3 transformFrom(const gtsam::Pose3& T); - static Similarity3 Align(const gtsam::Point3Pairs & abPointPairs); - static Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs); + static gtsam::Similarity3 Align(const gtsam::Point3Pairs & abPointPairs); + static gtsam::Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs); // Standard Interface const Matrix matrix() const; From 6a35fedd29ef9885db29ae8fb547403efcc6a878 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 17 Mar 2021 00:54:37 -0400 Subject: [PATCH 104/106] Fix CI with separate boost install Github Actions no longer supports boost, so we install it separately. Also turned off some checks since the wrapper causes an OOM exception. --- .github/scripts/boost.sh | 18 ++++++++++++++++++ .github/scripts/python.sh | 10 ++++++---- .github/workflows/build-linux.yml | 18 +++++++++--------- .github/workflows/build-macos.yml | 4 +++- .github/workflows/build-python.yml | 18 ++++++++++-------- .github/workflows/build-special.yml | 13 ++++++++----- .github/workflows/build-windows.yml | 15 ++++++++++++--- 7 files changed, 66 insertions(+), 30 deletions(-) create mode 100644 .github/scripts/boost.sh diff --git a/.github/scripts/boost.sh b/.github/scripts/boost.sh new file mode 100644 index 000000000..8b36a8f21 --- /dev/null +++ b/.github/scripts/boost.sh @@ -0,0 +1,18 @@ +### Script to install Boost +BOOST_FOLDER=boost_${BOOST_VERSION//./_} + +# Download Boost +wget https://dl.bintray.com/boostorg/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz + +# Unzip +tar -zxf ${BOOST_FOLDER}.tar.gz + +# Bootstrap +cd ${BOOST_FOLDER}/ +./bootstrap.sh + +# Build and install +sudo ./b2 install + +# Rebuild ld cache +sudo ldconfig diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index a71e14c97..b9c8a25f5 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -65,21 +65,23 @@ sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt mkdir $GITHUB_WORKSPACE/build cd $GITHUB_WORKSPACE/build -cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \ - -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_UNSTABLE=ON \ +cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \ + -DGTSAM_BUILD_TESTS=OFF \ + -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ -DGTSAM_USE_QUATERNIONS=OFF \ -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ -DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \ - -DGTSAM_TYPEDEF_POINTS_TO_VECTORS=${TYPEDEF_POINTS_TO_VECTORS} \ + -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \ -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \ -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=OFF \ -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install -make -j$(nproc) install +# Set to 2 cores so that Actions does not error out during resource provisioning. +make -j2 install cd $GITHUB_WORKSPACE/build/python $PYTHON setup.py install --user --prefix= diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index be1da35bb..9ca3a8fe5 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -12,6 +12,7 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }} + BOOST_VERSION: 1.67.0 strategy: fail-fast: false @@ -44,7 +45,8 @@ jobs: steps: - name: Checkout - uses: actions/checkout@master + uses: actions/checkout@v2 + - name: Install (Linux) if: runner.os == 'Linux' run: | @@ -61,11 +63,8 @@ jobs: fi sudo apt-get -y update - sudo apt install cmake build-essential pkg-config libpython-dev python-numpy - - echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV - echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV - + sudo apt-get install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev + if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV @@ -75,10 +74,11 @@ jobs: echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV fi - - name: Check Boost version - if: runner.os == 'Linux' + + - name: Install Boost run: | - echo "BOOST_ROOT = $BOOST_ROOT" + bash .github/scripts/boost.sh + - name: Build and Test (Linux) if: runner.os == 'Linux' run: | diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 435936bdf..ed8f8563b 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -12,6 +12,7 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }} + strategy: fail-fast: false matrix: @@ -31,7 +32,8 @@ jobs: steps: - name: Checkout - uses: actions/checkout@master + uses: actions/checkout@v2 + - name: Install (macOS) if: runner.os == 'macOS' run: | diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 1b5293b16..0d5a2120c 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -12,6 +12,7 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} PYTHON_VERSION: ${{ matrix.python_version }} + strategy: fail-fast: false matrix: @@ -22,10 +23,11 @@ jobs: ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9, macOS-10.15-xcode-11.3.1, - #ubuntu-18.04-gcc-5-tbb, + # ubuntu-18.04-gcc-5-tbb, ] - build_type: [Debug, Release] + # build_type: [Debug, Release] + build_type: [Release] python_version: [3] include: - name: ubuntu-18.04-gcc-5 @@ -48,15 +50,15 @@ jobs: compiler: xcode version: "11.3.1" -# - name: ubuntu-18.04-gcc-5-tbb -# os: ubuntu-18.04 -# compiler: gcc -# version: "5" -# flag: tbb + # - name: ubuntu-18.04-gcc-5-tbb + # os: ubuntu-18.04 + # compiler: gcc + # version: "5" + # flag: tbb steps: - name: Checkout - uses: actions/checkout@master + uses: actions/checkout@v2 - name: Install (Linux) if: runner.os == 'Linux' run: | diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 532f917c1..3ac88bdc8 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -12,6 +12,7 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} GTSAM_BUILD_UNSTABLE: ON + BOOST_VERSION: 1.67.0 strategy: fail-fast: false @@ -56,7 +57,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@master + uses: actions/checkout@v2 - name: Install (Linux) if: runner.os == 'Linux' @@ -69,10 +70,7 @@ jobs: fi sudo apt-get -y update - sudo apt install cmake build-essential pkg-config libpython-dev python-numpy - - echo "BOOST_ROOT=$(echo $BOOST_ROOT_1_72_0)" >> $GITHUB_ENV - echo "LD_LIBRARY_PATH=$(echo $BOOST_ROOT_1_72_0/lib)" >> $GITHUB_ENV + sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib @@ -84,6 +82,11 @@ jobs: echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV fi + - name: Install Boost + if: runner.os == 'Linux' + run: | + bash .github/scripts/boost.sh + - name: Install (macOS) if: runner.os == 'macOS' run: | diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 887d41972..a564dade9 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -38,7 +38,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@master + uses: actions/checkout@v2 - name: Install (Windows) if: runner.os == 'Windows' shell: powershell @@ -64,12 +64,21 @@ jobs: } # Scoop modifies the PATH so we make the modified PATH global. echo "$env:PATH" >> $GITHUB_PATH + + - name: Download and install Boost + uses: MarkusJx/install-boost@v1.0.1 + id: install-boost + with: + boost_version: 1.72.0 + toolset: msvc14.2 + - name: Build (Windows) if: runner.os == 'Windows' + env: + BOOST_ROOT: ${{ steps.install-boost.outputs.BOOST_ROOT }} run: | cmake -E remove_directory build - echo "BOOST_ROOT_1_72_0: ${env:BOOST_ROOT_1_72_0}" - cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT_1_72_0}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT_1_72_0}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT_1_72_0}\lib" + cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" cmake --build build --config ${{ matrix.build_type }} --target gtsam cmake --build build --config ${{ matrix.build_type }} --target gtsam_unstable cmake --build build --config ${{ matrix.build_type }} --target wrap From a04b064fb18b1ac08b295a071aa1ba39c03384fa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 18 Mar 2021 10:34:52 -0400 Subject: [PATCH 105/106] temporarily added required check --- .github/workflows/build-python.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 0d5a2120c..208fcd8c7 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -26,6 +26,7 @@ jobs: # ubuntu-18.04-gcc-5-tbb, ] + #TODO update wrapper to prevent OOM # build_type: [Debug, Release] build_type: [Release] python_version: [3] @@ -45,6 +46,13 @@ jobs: compiler: clang version: "9" + #NOTE temporarily added this as it is a required check. + - name: ubuntu-18.04-clang-9 + os: ubuntu-18.04 + compiler: clang + version: "9" + build_type: Debug + - name: macOS-10.15-xcode-11.3.1 os: macOS-10.15 compiler: xcode From 7c0434c6da3793a496bff52a28a5110fff277271 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 18 Mar 2021 10:45:46 -0400 Subject: [PATCH 106/106] explicitly add python version --- .github/workflows/build-python.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 208fcd8c7..089ee3246 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -52,6 +52,7 @@ jobs: compiler: clang version: "9" build_type: Debug + python_version: "3" - name: macOS-10.15-xcode-11.3.1 os: macOS-10.15