From fa743ae0ac861897bd8d63f7e7815314ec48d3c4 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 14 May 2022 00:18:04 -0700 Subject: [PATCH 01/10] intialize values that may have between factors --- gtsam/sfm/TranslationRecovery.cpp | 14 ++++++++++---- gtsam/sfm/TranslationRecovery.h | 6 ++++++ 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 810fe7de9..8f1108806 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -134,6 +134,7 @@ void TranslationRecovery::addPrior( Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, std::mt19937 *rng, const Values &initialValues) const { uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly @@ -155,14 +156,19 @@ Values TranslationRecovery::initializeRandomly( insert(edge.key1()); insert(edge.key2()); } + for (auto edge : betweenTranslations) { + insert(edge.key1()); + insert(edge.key2()); + } return initial; } Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, const Values &initialValues) const { - return initializeRandomly(relativeTranslations, &kRandomNumberGenerator, - initialValues); + return initializeRandomly(relativeTranslations, betweenTranslations, + &kRandomNumberGenerator, initialValues); } Values TranslationRecovery::run( @@ -187,8 +193,8 @@ Values TranslationRecovery::run( &graph); // Uses initial values from params if provided. - Values initial = - initializeRandomly(nonzeroRelativeTranslations, initialValues); + Values initial = initializeRandomly( + nonzeroRelativeTranslations, nonzeroBetweenTranslations, initialValues); // If there are no valid edges, but zero-distance edges exist, initialize one // of the nodes in a connected component of zero-distance edges. diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 7863f5133..6ccc39ddb 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -112,12 +112,15 @@ class TranslationRecovery { * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @param rng random number generator * @param intialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, std::mt19937 *rng, const Values &initialValues = Values()) const; /** @@ -125,11 +128,14 @@ class TranslationRecovery { * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @param initialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, const Values &initialValues = Values()) const; /** From 8482cee10b66f8bcc7c3674235c6fbbcb07373e0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 3 Jun 2022 13:53:25 -0400 Subject: [PATCH 02/10] improvements made during the old hybrid scheme implementation --- gtsam/discrete/DecisionTree-inl.h | 9 +++------ gtsam/discrete/DecisionTree.h | 2 +- gtsam/discrete/DiscreteFactorGraph.h | 4 ++++ gtsam/inference/FactorGraph.h | 2 +- gtsam/inference/Ordering.cpp | 11 +++++++++++ gtsam/inference/Ordering.h | 18 +++++++++++++++++- gtsam/inference/tests/testOrdering.cpp | 26 ++++++++++++++++++++++++++ gtsam/linear/GaussianBayesNet.h | 2 +- gtsam/linear/GaussianConditional.cpp | 4 ++-- gtsam/linear/GaussianConditional.h | 1 + 10 files changed, 67 insertions(+), 12 deletions(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 99f29b8e5..f72743206 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -39,10 +39,10 @@ #include #include -using boost::assign::operator+=; - namespace gtsam { + using boost::assign::operator+=; + /****************************************************************************/ // Node /****************************************************************************/ @@ -291,10 +291,7 @@ namespace gtsam { } os << "\"" << this->id() << "\" -> \"" << branch->id() << "\""; - if (B == 2) { - if (i == 0) os << " [style=dashed]"; - if (i > 1) os << " [style=bold]"; - } + if (B == 2 && i == 0) os << " [style=dashed]"; os << std::endl; branch->dot(os, labelFormatter, valueFormatter, showZero); } diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 1f45d320b..7449ae995 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -220,7 +220,7 @@ namespace gtsam { /// @{ /// Make virtual - virtual ~DecisionTree() {} + virtual ~DecisionTree() = default; /// Check if tree is empty. bool empty() const { return !root_; } diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index f962b1802..bfa57e7db 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -209,6 +209,10 @@ class GTSAM_EXPORT DiscreteFactorGraph /// @} }; // \ DiscreteFactorGraph +std::pair // +EliminateForMPE(const DiscreteFactorGraph& factors, + const Ordering& frontalKeys); + /// traits template <> struct traits : public Testable {}; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index afea63da8..101134c83 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -361,7 +361,7 @@ class FactorGraph { * less than the original, factors at the end will be removed. If the new * size is larger than the original, null factors will be appended. */ - void resize(size_t size) { factors_.resize(size); } + virtual void resize(size_t size) { factors_.resize(size); } /** delete factor without re-arranging indexes by inserting a nullptr pointer */ diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 2ac2c0dde..7b7ff1403 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -283,6 +283,17 @@ void Ordering::print(const std::string& str, cout.flush(); } +/* ************************************************************************* */ +Ordering::This& Ordering::operator+=(KeyVector& keys) { + this->insert(this->end(), keys.begin(), keys.end()); + return *this; +} + +/* ************************************************************************* */ +bool Ordering::contains(const Key& key) const { + return std::find(this->begin(), this->end(), key) != this->end(); +} + /* ************************************************************************* */ bool Ordering::equals(const Ordering& other, double tol) const { return (*this) == other; diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index a2d165792..b366b3b3a 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -70,7 +70,23 @@ public: boost::assign_detail::call_push_back(*this))(key); } - /// Invert (not reverse) the ordering - returns a map from key to order position + /** + * @brief Append new keys to the ordering as `ordering += keys`. + * + * @param key + * @return The ordering variable with appended keys. + */ + This& operator+=(KeyVector& keys); + + /// Check if key exists in ordering. + bool contains(const Key& key) const; + + /** + * @brief Invert (not reverse) the ordering - returns a map from key to order + * position. + * + * @return FastMap + */ FastMap invert() const; /// @name Fill-reducing Orderings @{ diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 6fdca0d89..1afa1cfde 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -199,6 +199,32 @@ TEST(Ordering, csr_format_3) { EXPECT(adjExpected == adjAcutal); } +/* ************************************************************************* */ +TEST(Ordering, AppendVector) { + using symbol_shorthand::X; + Ordering actual; + KeyVector keys = {X(0), X(1), X(2)}; + actual += keys; + + Ordering expected; + expected += X(0); + expected += X(1); + expected += X(2); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(Ordering, Contains) { + using symbol_shorthand::X; + Ordering ordering; + ordering += X(0); + ordering += X(1); + ordering += X(2); + + EXPECT(ordering.contains(X(1))); + EXPECT(!ordering.contains(X(4))); +} + /* ************************************************************************* */ #ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, csr_format_4) { diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 940ffd882..e4f2950ed 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -45,7 +45,7 @@ namespace gtsam { /// @name Standard Constructors /// @{ - /** Construct empty factor graph */ + /** Construct empty bayes net */ GaussianBayesNet() {} /** Construct from iterator over conditionals */ diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 6199f91a7..951577641 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -15,10 +15,10 @@ * @author Christian Potthast, Frank Dellaert */ -#include #include -#include #include +#include +#include #include #ifdef __GNUC__ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index b2b616dab..11fe1f318 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include // for std::mt19937_64 From f911ccf176570cd93be5e1dea290283ec37ff3ff Mon Sep 17 00:00:00 2001 From: HViktorTsoi Date: Wed, 8 Jun 2022 16:30:10 +0800 Subject: [PATCH 03/10] Move Rot3::quaternion to the deprecated block --- gtsam/geometry/Rot3.cpp | 2 ++ gtsam/geometry/Rot3.h | 7 ++++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 08213b065..6db5e1919 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -228,6 +228,7 @@ double Rot3::yaw(OptionalJacobian<1, 3> H) const { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 Vector Rot3::quaternion() const { gtsam::Quaternion q = toQuaternion(); Vector v(4); @@ -237,6 +238,7 @@ Vector Rot3::quaternion() const { v(3) = q.z(); return v; } +#endif /* ************************************************************************* */ pair Rot3::axisAngle() const { diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 18bd88b52..01ca7778c 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -515,11 +515,16 @@ class GTSAM_EXPORT Rot3 : public LieGroup { */ gtsam::Quaternion toQuaternion() const; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** * Converts to a generic matrix to allow for use with matlab * In format: w x y z + * @deprecated: use Rot3::toQuaternion() instead. + * If still using this API, remind that in the returned Vector `V`, + * `V.x()` denotes the actual `qw`, `V.y()` denotes 'qx', `V.z()` denotes `qy`, and `V.w()` denotes 'qz'. */ - Vector quaternion() const; + Vector GTSAM_DEPRECATED quaternion() const; +#endif /** * @brief Spherical Linear intERPolation between *this and other From 27ddedfc630206a87c980c6b64f1b90dc7f26747 Mon Sep 17 00:00:00 2001 From: HViktorTsoi Date: Wed, 8 Jun 2022 16:42:50 +0800 Subject: [PATCH 04/10] Replace the usage of Rot3::quaternion to Rot3::toQuaternion in 'timeShonanAveraging.cpp' --- gtsam_unstable/timing/timeShonanAveraging.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/timing/timeShonanAveraging.cpp b/gtsam_unstable/timing/timeShonanAveraging.cpp index e932b6400..74dd04a78 100644 --- a/gtsam_unstable/timing/timeShonanAveraging.cpp +++ b/gtsam_unstable/timing/timeShonanAveraging.cpp @@ -79,9 +79,9 @@ void saveG2oResult(string name, const Values& values, std::map poses myfile << "VERTEX_SE3:QUAT" << " "; myfile << i << " "; myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " "; - Vector quaternion = Rot3(R).quaternion(); - myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1) - << " " << quaternion(0); + Quaternion quaternion = Rot3(R).toQuaternion(); + myfile << quaternion.x() << " " << quaternion.y() << " " << quaternion.z() + << " " << quaternion.w(); myfile << "\n"; } myfile.close(); From 5ac71d20d56c594e6ef118d01e47706a90e78c6e Mon Sep 17 00:00:00 2001 From: HViktorTsoi Date: Thu, 9 Jun 2022 17:56:35 +0800 Subject: [PATCH 05/10] Remove 'Rot3::quaternion()' API from 'geometry.i' --- gtsam/geometry/geometry.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 8079e2c2a..517558265 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -355,7 +355,7 @@ class Rot3 { double yaw() const; pair axisAngle() const; gtsam::Quaternion toQuaternion() const; - Vector quaternion() const; + // Vector quaternion() const; // @deprecated, see https://github.com/borglab/gtsam/pull/1219 gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; // enabling serialization functionality From f6b86fb17eab1788edc25630ab6f78508d0aaec6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Jun 2022 04:05:18 -0400 Subject: [PATCH 06/10] address review comments --- gtsam/hybrid/tests/testGaussianMixture.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 8a91a86cc..420e22315 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -35,7 +35,10 @@ using symbol_shorthand::M; using symbol_shorthand::X; /* ************************************************************************* */ -TEST(GaussianConditional, Equals) { +/* Check construction of GaussianMixture P(x1 | x2, m1) as well as accessing a + * specific mode i.e. P(x1 | x2, m1=1). + */ +TEST(GaussianMixture, Equals) { // create a conditional gaussian node Matrix S1(2, 2); S1(0, 0) = 1; @@ -89,4 +92,4 @@ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ \ No newline at end of file +/* ************************************************************************* */ From 5ec6127c0b5bebe03149068f276e53870a0aa8bb Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 18 Jun 2022 11:35:46 -0700 Subject: [PATCH 07/10] add unit test, update doc --- gtsam/sfm/TranslationRecovery.cpp | 1 + tests/testTranslationRecovery.cpp | 30 ++++++++++++++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 8f1108806..28c2b2e8a 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -156,6 +156,7 @@ Values TranslationRecovery::initializeRandomly( insert(edge.key1()); insert(edge.key2()); } + // There may be nodes in betweenTranslations that do not have a measurement. for (auto edge : betweenTranslations) { insert(edge.key1()); insert(edge.key2()); diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 5dd319d30..15f1caa1b 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -323,6 +323,36 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); } + +TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurements) { + // Checks that valid results are obtained when a between translation edge is + // provided with a node that does not have any other relative translations. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + poses.insert(4, Pose3(Rot3(), Point3(1, 2, 1))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); + + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), + noiseModel::Constrained::All(3, 1e2)); + // Node 4 only has this between translation prior, no relative translations. + betweenTranslations.emplace_back(0, 4, Point3(1, 2, 1)); + + TranslationRecovery algorithm; + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); + EXPECT(assert_equal(Point3(1, 2, 1), result.at(4), 1e-4)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 6fae6f42313552ca900b9d8af26b1f6698d889e8 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 18 Jun 2022 11:55:55 -0700 Subject: [PATCH 08/10] update docstring --- gtsam/sfm/TranslationRecovery.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 28c2b2e8a..c7ef2e526 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -119,16 +119,18 @@ void TranslationRecovery::addPrior( graph->emplace_shared>(edge->key1(), Point3(0, 0, 0), priorNoiseModel); - // Add between factors for optional relative translations. - for (auto edge : betweenTranslations) { - graph->emplace_shared>( - edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); - } - // Add a scale prior only if no other between factors were added. if (betweenTranslations.empty()) { graph->emplace_shared>( edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); + return; + } + + // Add between factors for optional relative translations. + for (auto prior_edge : betweenTranslations) { + graph->emplace_shared>( + prior_edge.key1(), prior_edge.key2(), prior_edge.measured(), + prior_edge.noiseModel()); } } From e18840baffd3e02fb3c05e76ee2586cf2ae83e77 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 18 Jun 2022 11:56:21 -0700 Subject: [PATCH 09/10] update doc in header --- gtsam/sfm/TranslationRecovery.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 6ccc39ddb..44a5ef43e 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -143,11 +143,15 @@ class TranslationRecovery { * * @param relativeTranslations the relative translations, in world coordinate * frames, vector of BinaryMeasurements of Unit3, where each key of a - * measurement is a point in 3D. + * measurement is a point in 3D. If a relative translation magnitude is zero, + * it is treated as a hard same-point constraint (the result of all nodes + * connected by a zero-magnitude edge will be the same). * @param scale scale for first relative translation which fixes gauge. * The scale is only used if betweenTranslations is empty. * @param betweenTranslations relative translations (with scale) between 2 - * points in world coordinate frame known a priori. + * points in world coordinate frame known a priori. Unlike + * relativeTranslations, zero-magnitude betweenTranslations are not treated as + * hard constraints. * @param initialValues intial values for optimization. Initializes randomly * if not provided. * @return Values From 0984fea3f506fa42675e92edbeda4ac9d4df3f5c Mon Sep 17 00:00:00 2001 From: vik748 Date: Mon, 20 Jun 2022 01:18:07 -0700 Subject: [PATCH 10/10] Address matplotlib deprecation warnings for fig.gca() and window.set_title(). --- python/gtsam/examples/Pose3ISAM2Example.py | 5 ++++- python/gtsam/examples/VisualISAM2Example.py | 5 ++++- python/gtsam/utils/plot.py | 19 ++++++++++++++----- 3 files changed, 22 insertions(+), 7 deletions(-) diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index cb71813c5..986bd5b9c 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -34,7 +34,10 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa # Plot the newly updated iSAM2 inference. fig = plt.figure(0) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] plt.cla() i = 1 diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py index 4b480fab7..60d4fb376 100644 --- a/python/gtsam/examples/VisualISAM2Example.py +++ b/python/gtsam/examples/VisualISAM2Example.py @@ -33,7 +33,10 @@ def visual_ISAM2_plot(result): fignum = 0 fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] plt.cla() # Plot points diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index a4d19f72b..880c436e8 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -333,7 +333,10 @@ def plot_point3( """ fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] plot_point3_on_axes(axes, point, linespec, P) axes.set_xlabel(axis_labels[0]) @@ -388,7 +391,7 @@ def plot_3d_points(fignum, fig = plt.figure(fignum) fig.suptitle(title) - fig.canvas.set_window_title(title.lower()) + fig.canvas.manager.set_window_title(title.lower()) def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): @@ -490,7 +493,10 @@ def plot_trajectory( axis_labels (iterable[string]): List of axis labels to set. """ fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] axes.set_xlabel(axis_labels[0]) axes.set_ylabel(axis_labels[1]) @@ -522,7 +528,7 @@ def plot_trajectory( plot_pose3_on_axes(axes, pose, P=covariance, axis_length=scale) fig.suptitle(title) - fig.canvas.set_window_title(title.lower()) + fig.canvas.manager.set_window_title(title.lower()) def plot_incremental_trajectory(fignum: int, @@ -545,7 +551,10 @@ def plot_incremental_trajectory(fignum: int, Used to create animation effect. """ fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] poses = gtsam.utilities.allPose3s(values) keys = gtsam.KeyVector(poses.keys())