From ddcfc1b50b428d9c8a26370c91ff405367fac75f Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sun, 12 Feb 2023 17:55:19 -0800 Subject: [PATCH 01/14] Moves 'BatchFixedLagSmoother' to 'gtsam' from 'gtsam_unstable' --- .../nonlinear/BatchFixedLagSmoother.cpp | 2 +- .../nonlinear/BatchFixedLagSmoother.h | 6 +++--- .../nonlinear/FixedLagSmoother.cpp | 2 +- .../nonlinear/FixedLagSmoother.h | 2 +- .../tests/testBatchFixedLagSmoother.cpp | 2 +- gtsam_unstable/linear/InfeasibleInitialValues.h | 7 ++----- .../linear/InfeasibleOrUnboundedProblem.h | 7 ++----- gtsam_unstable/nonlinear/ConcurrentBatchFilter.h | 6 +++--- .../nonlinear/ConcurrentFilteringAndSmoothing.h | 16 ++++++++-------- gtsam_unstable/nonlinear/LinearizedFactor.h | 8 ++++---- 10 files changed, 26 insertions(+), 32 deletions(-) rename {gtsam_unstable => gtsam}/nonlinear/BatchFixedLagSmoother.cpp (99%) rename {gtsam_unstable => gtsam}/nonlinear/BatchFixedLagSmoother.h (97%) rename {gtsam_unstable => gtsam}/nonlinear/FixedLagSmoother.cpp (98%) rename {gtsam_unstable => gtsam}/nonlinear/FixedLagSmoother.h (99%) rename {gtsam_unstable => gtsam}/nonlinear/tests/testBatchFixedLagSmoother.cpp (99%) diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam/nonlinear/BatchFixedLagSmoother.cpp similarity index 99% rename from gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp rename to gtsam/nonlinear/BatchFixedLagSmoother.cpp index f5280ceff..4545c1540 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam/nonlinear/BatchFixedLagSmoother.cpp @@ -17,7 +17,7 @@ * @date Oct 14, 2012 */ -#include +#include #include #include #include diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam/nonlinear/BatchFixedLagSmoother.h similarity index 97% rename from gtsam_unstable/nonlinear/BatchFixedLagSmoother.h rename to gtsam/nonlinear/BatchFixedLagSmoother.h index e1206d942..e5464896e 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam/nonlinear/BatchFixedLagSmoother.h @@ -20,7 +20,7 @@ // \callgraph #pragma once -#include +#include #include #include @@ -35,10 +35,10 @@ public: /** default constructor */ BatchFixedLagSmoother(double smootherLag = 0.0, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool enforceConsistency = true) : - FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) { }; + FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) { } /** destructor */ - ~BatchFixedLagSmoother() override { }; + ~BatchFixedLagSmoother() override {} /** Print the factor for debugging and testing (implementing Testable) */ void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp b/gtsam/nonlinear/FixedLagSmoother.cpp similarity index 98% rename from gtsam_unstable/nonlinear/FixedLagSmoother.cpp rename to gtsam/nonlinear/FixedLagSmoother.cpp index 34a23982f..ce3242257 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp +++ b/gtsam/nonlinear/FixedLagSmoother.cpp @@ -17,7 +17,7 @@ * @date Feb 27, 2013 */ -#include +#include namespace gtsam { diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam/nonlinear/FixedLagSmoother.h similarity index 99% rename from gtsam_unstable/nonlinear/FixedLagSmoother.h rename to gtsam/nonlinear/FixedLagSmoother.h index 10e13271d..4272e011f 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam/nonlinear/FixedLagSmoother.h @@ -51,7 +51,7 @@ public: size_t nonlinearVariables; ///< The number of variables that can be relinearized size_t linearVariables; ///< The number of variables that must keep a constant linearization point double error; ///< The final factor graph error - Result() : iterations(0), intermediateSteps(0), nonlinearVariables(0), linearVariables(0), error(0) {}; + Result() : iterations(0), intermediateSteps(0), nonlinearVariables(0), linearVariables(0), error(0) {} /// Getter methods size_t getIterations() const { return iterations; } diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam/nonlinear/tests/testBatchFixedLagSmoother.cpp similarity index 99% rename from gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp rename to gtsam/nonlinear/tests/testBatchFixedLagSmoother.cpp index a708c57cc..23d569c28 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include diff --git a/gtsam_unstable/linear/InfeasibleInitialValues.h b/gtsam_unstable/linear/InfeasibleInitialValues.h index dbd1b3940..4e05c75d4 100644 --- a/gtsam_unstable/linear/InfeasibleInitialValues.h +++ b/gtsam_unstable/linear/InfeasibleInitialValues.h @@ -33,13 +33,10 @@ public: } const char *what() const noexcept override { - if (description_.empty()) + if (description_->empty()) description_ = "An infeasible initial value was provided for the solver.\n"; - return description_.c_str(); + return description_->c_str(); } - -private: - mutable std::string description_; }; } diff --git a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h index 5f9b9f5b3..0f4021019 100644 --- a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h +++ b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h @@ -29,12 +29,9 @@ public: } const char* what() const noexcept override { - if (description_.empty()) + if (description_->empty()) description_ = "The problem is either infeasible or unbounded.\n"; - return description_.c_str(); + return description_->c_str(); } - -private: - mutable std::string description_; }; } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index b9e0f59ff..6a69871e1 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -50,7 +50,7 @@ public: double error; ///< The final factor graph error /// Constructor - Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {}; + Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {} /// Getter methods size_t getIterations() const { return iterations; } @@ -61,10 +61,10 @@ public: }; /** Default constructor */ - ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {}; + ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {} /** Default destructor */ - ~ConcurrentBatchFilter() override {}; + ~ConcurrentBatchFilter() override = default; /** Implement a GTSAM standard 'print' function */ void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h index 80c10aa66..2e3c34361 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -41,10 +41,10 @@ public: typedef std::shared_ptr shared_ptr; /** Default constructor */ - ConcurrentFilter() {}; + ConcurrentFilter() = default; /** Default destructor */ - virtual ~ConcurrentFilter() {}; + virtual ~ConcurrentFilter() = default; /** Implement a standard 'print' function */ virtual void print( @@ -58,7 +58,7 @@ public: * Perform any required operations before the synchronization process starts. * Called by 'synchronize' */ - virtual void presync() {}; + virtual void presync() {} /** * Populate the provided containers with factors that constitute the filter branch summarization @@ -91,7 +91,7 @@ public: * Perform any required operations after the synchronization process finishes. * Called by 'synchronize' */ - virtual void postsync() {}; + virtual void postsync() {} }; // ConcurrentFilter @@ -103,10 +103,10 @@ public: typedef std::shared_ptr shared_ptr; /** Default constructor */ - ConcurrentSmoother() {}; + ConcurrentSmoother() {} /** Default destructor */ - virtual ~ConcurrentSmoother() {}; + virtual ~ConcurrentSmoother() = default; /** Implement a standard 'print' function */ virtual void print( @@ -120,7 +120,7 @@ public: * Perform any required operations before the synchronization process starts. * Called by 'synchronize' */ - virtual void presync() {}; + virtual void presync() {} /** * Populate the provided containers with factors that constitute the smoother branch summarization @@ -147,7 +147,7 @@ public: * Perform any required operations after the synchronization process finishes. * Called by 'synchronize' */ - virtual void postsync() {}; + virtual void postsync() {} }; // ConcurrentSmoother diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 689c97c3a..1ff45ef5f 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -45,7 +45,7 @@ protected: public: /** default constructor for serialization */ - LinearizedGaussianFactor() {}; + LinearizedGaussianFactor() = default; /** * @param gaussian: A jacobian or hessian factor @@ -53,7 +53,7 @@ public: */ LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Values& lin_points); - ~LinearizedGaussianFactor() override {}; + ~LinearizedGaussianFactor() override = default; // access functions const Values& linearizationPoint() const { return lin_points_; } @@ -128,11 +128,11 @@ public: // access functions const constBVector b() const { return Ab_(size()).col(0); } - const constABlock A() const { return Ab_.range(0, size()); }; + const constABlock A() const { return Ab_.range(0, size()); } const constABlock A(Key key) const { return Ab_(std::find(begin(), end(), key) - begin()); } /** get the dimension of the factor (number of rows on linearization) */ - size_t dim() const override { return Ab_.rows(); }; + size_t dim() const override { return Ab_.rows(); } /** Calculate the error of the factor */ double error(const Values& c) const override; From 9a50233426dd5cc0e10ff513d66c1c1b7c3a6009 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Tue, 14 Feb 2023 17:23:25 -0800 Subject: [PATCH 02/14] Make GTSAM_CONCEPT_ASSERTS work with '-Wextra-semi' --- gtsam/base/VectorSpace.h | 2 +- gtsam/base/concepts.h | 2 +- gtsam/nonlinear/Expression.h | 4 ++-- gtsam/nonlinear/internal/ExpressionNode.h | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index f4e3d3020..a9e9ca106 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -185,7 +185,7 @@ template struct VectorSpaceTraits: VectorSpaceImpl { // Check that Class has the necessary machinery - GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs); +GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs); typedef vector_space_tag structure_category; diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 5159d6f9b..f951329a7 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -19,7 +19,7 @@ // This does something sensible: #define BOOST_CONCEPT_USAGE(concept) void check##concept() // These just ignore the concept checking for now: -#define GTSAM_CONCEPT_ASSERT(concept) +#define GTSAM_CONCEPT_ASSERT(concept) void*(concept) #define GTSAM_CONCEPT_REQUIRES(concept, return_type) return_type #endif diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 1e977ade1..e5017390d 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -218,7 +218,7 @@ protected: template class ScalarMultiplyExpression : public Expression { // Check that T is a vector space - GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace); + GTSAM_CONCEPT_ASSERT(IsVectorSpace); public: explicit ScalarMultiplyExpression(double s, const Expression& e); @@ -231,7 +231,7 @@ class ScalarMultiplyExpression : public Expression { template class BinarySumExpression : public Expression { // Check that T is a vector space - GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace); + GTSAM_CONCEPT_ASSERT(IsVectorSpace); public: explicit BinarySumExpression(const Expression& e1, const Expression& e2); diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 1f45f7a87..de0c9721e 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -561,7 +561,7 @@ public: template class ScalarMultiplyNode : public ExpressionNode { // Check that T is a vector space - GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace); + GTSAM_CONCEPT_ASSERT(IsVectorSpace); double scalar_; std::shared_ptr > expression_; From 72b31221c171e39ef9db438f8c5107f067f58e02 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Tue, 14 Feb 2023 17:45:23 -0800 Subject: [PATCH 03/14] Deal with '-Wunused-exception-parameter' from 'nonliner/utilities.h' --- gtsam/nonlinear/utilities.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 2997ed7d7..0a0ceb849 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -328,12 +328,12 @@ Values localToWorld(const Values& local, const Pose2& base, // if value is a Pose2, compose it with base pose Pose2 pose = local.at(key); world.insert(key, base.compose(pose)); - } catch (const std::exception& e1) { + } catch ([[maybe_unused]] const std::exception& e1) { try { // if value is a Point2, transform it from base pose Point2 point = local.at(key); world.insert(key, base.transformFrom(point)); - } catch (const std::exception& e2) { + } catch ([[maybe_unused]] const std::exception& e2) { // if not Pose2 or Point2, do nothing #ifndef NDEBUG std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl; From ccb151d07b64473aed1b6031905aafee7ae56fc1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 20 Feb 2023 08:03:36 -0800 Subject: [PATCH 04/14] Added deprecated headers --- .../nonlinear/BatchFixedLagSmoother.h | 21 +++++++++++++++++++ gtsam_unstable/nonlinear/FixedLagSmoother.h | 21 +++++++++++++++++++ .../nonlinear/IncrementalFixedLagSmoother.h | 2 +- 3 files changed, 43 insertions(+), 1 deletion(-) create mode 100644 gtsam_unstable/nonlinear/BatchFixedLagSmoother.h create mode 100644 gtsam_unstable/nonlinear/FixedLagSmoother.h diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h new file mode 100644 index 000000000..52ffe7a70 --- /dev/null +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -0,0 +1,21 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +#pragma once + +#ifdef _MSC_VER +#pragma message("BatchFixedLagSmoother was moved to the gtsam/nonlinear directory") +#else +#warning "BatchFixedLagSmoother was moved to the gtsam/nonlinear directory" +#endif + + +#include diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h new file mode 100644 index 000000000..ae4fa874f --- /dev/null +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -0,0 +1,21 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +#pragma once + +#ifdef _MSC_VER +#pragma message("FixedLagSmoother was moved to the gtsam/nonlinear directory") +#else +#warning "FixedLagSmoother was moved to the gtsam/nonlinear directory" +#endif + + +#include diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index beef95c1b..bc1c48524 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -20,7 +20,7 @@ // \callgraph #pragma once -#include +#include #include namespace gtsam { From 6df2f3eeda30861bef177795b8ed1c671c97bf15 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 20 Feb 2023 08:05:07 -0800 Subject: [PATCH 05/14] Fixed examples --- .../examples => examples}/FixedLagSmootherExample.cpp | 2 +- .../examples/ConcurrentFilteringAndSmoothingExample.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) rename {gtsam_unstable/examples => examples}/FixedLagSmootherExample.cpp (99%) diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/examples/FixedLagSmootherExample.cpp similarity index 99% rename from gtsam_unstable/examples/FixedLagSmootherExample.cpp rename to examples/FixedLagSmootherExample.cpp index d0cd918f8..7b6a7d57f 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/examples/FixedLagSmootherExample.cpp @@ -23,7 +23,7 @@ */ // This example demonstrates the use of the Fixed-Lag Smoothers in GTSAM unstable -#include +#include #include // In GTSAM, measurement functions are represented as 'factors'. Several common factors diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 52a45b6d0..502c23765 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -28,7 +28,7 @@ #include // We will compare the results to a similar Fixed-Lag Smoother -#include +#include // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. From 2ce0b57f0525c5413cf2483a3316c9a9911d29c4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 20 Feb 2023 08:20:16 -0800 Subject: [PATCH 06/14] Fixed python wrapper --- gtsam/nonlinear/nonlinear.i | 61 ++++++++++++++++++ gtsam_unstable/gtsam_unstable.i | 63 +------------------ python/CMakeLists.txt | 2 +- .../examples/FixedLagSmootherExample.py | 4 +- .../tests/test_FixedLagSmootherExample.py | 4 +- 5 files changed, 68 insertions(+), 66 deletions(-) rename python/{gtsam_unstable => gtsam}/examples/FixedLagSmootherExample.py (95%) rename python/{gtsam_unstable => gtsam}/tests/test_FixedLagSmootherExample.py (96%) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index e7a05cf62..19f4ae588 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -650,4 +650,65 @@ virtual class NonlinearEquality2 : gtsam::NoiseModelFactor { gtsam::Vector evaluateError(const T& x1, const T& x2); }; +#include +class FixedLagSmootherKeyTimestampMapValue { + FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); + FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); +}; + +class FixedLagSmootherKeyTimestampMap { + FixedLagSmootherKeyTimestampMap(); + FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + double at(const size_t key) const; + void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); +}; + +class FixedLagSmootherResult { + size_t getIterations() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class FixedLagSmoother { + void print(string s) const; + bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; + + gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; + double smootherLag() const; + + gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors, + const gtsam::Values &newTheta, + const gtsam::FixedLagSmootherKeyTimestampMap ×tamps); + gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors, + const gtsam::Values &newTheta, + const gtsam::FixedLagSmootherKeyTimestampMap ×tamps, + const gtsam::FactorIndices &factorsToRemove); + gtsam::Values calculateEstimate() const; +}; + +#include +virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { + BatchFixedLagSmoother(); + BatchFixedLagSmoother(double smootherLag); + BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); + + void print(string s = "BatchFixedLagSmoother:\n") const; + + gtsam::LevenbergMarquardtParams params() const; + template + VALUE calculateEstimate(size_t key) const; +}; + + } // namespace gtsam diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 5cd7e0da2..f36118873 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -40,6 +40,7 @@ class gtsam::LevenbergMarquardtParams; class gtsam::ISAM2Params; class gtsam::GaussianDensity; class gtsam::LevenbergMarquardtOptimizer; +class gtsam::FixedLagSmoother; namespace gtsam { @@ -530,67 +531,7 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* // nonlinear //************************************************************************* -#include -class FixedLagSmootherKeyTimestampMapValue { - FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); - FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); -}; - -class FixedLagSmootherKeyTimestampMap { - FixedLagSmootherKeyTimestampMap(); - FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - double at(const size_t key) const; - void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); -}; - -class FixedLagSmootherResult { - size_t getIterations() const; - size_t getNonlinearVariables() const; - size_t getLinearVariables() const; - double getError() const; -}; - -#include -virtual class FixedLagSmoother { - void print(string s) const; - bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; - - gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; - double smootherLag() const; - - gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors, - const gtsam::Values &newTheta, - const gtsam::FixedLagSmootherKeyTimestampMap ×tamps); - gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors, - const gtsam::Values &newTheta, - const gtsam::FixedLagSmootherKeyTimestampMap ×tamps, - const gtsam::FactorIndices &factorsToRemove); - gtsam::Values calculateEstimate() const; -}; - -#include -virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { - BatchFixedLagSmoother(); - BatchFixedLagSmoother(double smootherLag); - BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); - - void print(string s = "BatchFixedLagSmoother:\n") const; - - gtsam::LevenbergMarquardtParams params() const; - template - VALUE calculateEstimate(size_t key) const; -}; - +#include #include virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { IncrementalFixedLagSmoother(); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 28bad4ad2..48e0329fb 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -40,6 +40,7 @@ set(ignore gtsam::IndexPairVector gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s + gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::Point2Vector gtsam::Point2Pairs gtsam::Point3Pairs @@ -144,7 +145,6 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::Point2Vector gtsam::Pose3Vector gtsam::KeyVector - gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsRot3 diff --git a/python/gtsam_unstable/examples/FixedLagSmootherExample.py b/python/gtsam/examples/FixedLagSmootherExample.py similarity index 95% rename from python/gtsam_unstable/examples/FixedLagSmootherExample.py rename to python/gtsam/examples/FixedLagSmootherExample.py index 7d2cea8ae..99af0edcf 100644 --- a/python/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/python/gtsam/examples/FixedLagSmootherExample.py @@ -25,13 +25,13 @@ def BatchFixedLagSmootherExample(): # Define a batch fixed lag smoother, which uses # Levenberg-Marquardt to perform the nonlinear optimization lag = 2.0 - smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) + smoother_batch = gtsam.BatchFixedLagSmoother(lag) # Create containers to store the factors and linearization points # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() - new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() + new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) diff --git a/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/python/gtsam/tests/test_FixedLagSmootherExample.py similarity index 96% rename from python/gtsam_unstable/tests/test_FixedLagSmootherExample.py rename to python/gtsam/tests/test_FixedLagSmootherExample.py index c1ccd1ea1..ddd2d49e4 100644 --- a/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ b/python/gtsam/tests/test_FixedLagSmootherExample.py @@ -30,13 +30,13 @@ class TestFixedLagSmootherExample(GtsamTestCase): # Define a batch fixed lag smoother, which uses # Levenberg-Marquardt to perform the nonlinear optimization lag = 2.0 - smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) + smoother_batch = gtsam.BatchFixedLagSmoother(lag) # Create containers to store the factors and linearization points # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() - new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() + new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) From ad2f598245e257f11682d734f8097286ba35103a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 20 Feb 2023 12:54:56 -0800 Subject: [PATCH 07/14] Remove disable of warning not consistent across different clang versions --- cmake/GtsamBuildTypes.cmake | 1 - 1 file changed, 1 deletion(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 3a2826ec6..3e8cf7192 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -129,7 +129,6 @@ else() -fPIC # ensure proper code generation for shared libraries $<$:-Wreturn-local-addr -Werror=return-local-addr> # Error: return local address $<$:-Wreturn-stack-address -Werror=return-stack-address> # Error: return local address - $<$:-Wno-misleading-indentation> # Eigen triggers a ton! $<$:-Wno-weak-template-vtables> # TODO(dellaert): don't know how to resolve $<$:-Wno-weak-vtables> # TODO(dellaert): don't know how to resolve -Wreturn-type -Werror=return-type # Error on missing return() From 7374a7a87dd1046a9178bbbf868dce0b392e5f22 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 22 Feb 2023 22:35:30 -0800 Subject: [PATCH 08/14] Try CI fix to save space --- .github/workflows/build-linux.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index c699db0d3..92b3e76fd 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -50,6 +50,9 @@ jobs: version: "14" steps: + - name: Maximize build disk space + uses: easimon/maximize-build-space@v6 + - name: Checkout uses: actions/checkout@v3 From fa676d285ca4c5426db2c7dc472b8502ca6b30ef Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 22 Feb 2023 22:36:35 -0800 Subject: [PATCH 09/14] Fixed export macros --- gtsam/nonlinear/BatchFixedLagSmoother.h | 2 +- gtsam/nonlinear/FixedLagSmoother.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/BatchFixedLagSmoother.h b/gtsam/nonlinear/BatchFixedLagSmoother.h index e5464896e..8dde520a7 100644 --- a/gtsam/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam/nonlinear/BatchFixedLagSmoother.h @@ -26,7 +26,7 @@ namespace gtsam { -class GTSAM_UNSTABLE_EXPORT BatchFixedLagSmoother : public FixedLagSmoother { +class GTSAM_EXPORT BatchFixedLagSmoother : public FixedLagSmoother { public: diff --git a/gtsam/nonlinear/FixedLagSmoother.h b/gtsam/nonlinear/FixedLagSmoother.h index 4272e011f..cb657f141 100644 --- a/gtsam/nonlinear/FixedLagSmoother.h +++ b/gtsam/nonlinear/FixedLagSmoother.h @@ -30,7 +30,7 @@ namespace gtsam { -class GTSAM_UNSTABLE_EXPORT FixedLagSmoother { +class GTSAM_EXPORT FixedLagSmoother { public: From d6cd6a1cf5eb920f041cb732b4375f4e534f91d6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 22 Feb 2023 23:06:53 -0800 Subject: [PATCH 10/14] Only most recent compilers --- .github/workflows/build-linux.yml | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 92b3e76fd..f51fd0db5 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -29,15 +29,15 @@ jobs: build_type: [Debug, Release] build_unstable: [ON] include: - - name: ubuntu-20.04-gcc-9 - os: ubuntu-20.04 - compiler: gcc - version: "9" + # - name: ubuntu-20.04-gcc-9 + # os: ubuntu-20.04 + # compiler: gcc + # version: "9" - - name: ubuntu-20.04-clang-9 - os: ubuntu-20.04 - compiler: clang - version: "9" + # - name: ubuntu-20.04-clang-9 + # os: ubuntu-20.04 + # compiler: clang + # version: "9" - name: ubuntu-22.04-gcc-11 os: ubuntu-22.04 @@ -50,9 +50,6 @@ jobs: version: "14" steps: - - name: Maximize build disk space - uses: easimon/maximize-build-space@v6 - - name: Checkout uses: actions/checkout@v3 From 86ee0166b02f2e9234a31e07cc1b2eda7709389c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 22 Feb 2023 23:08:54 -0800 Subject: [PATCH 11/14] Remove old compilers --- .github/workflows/build-linux.yml | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index f51fd0db5..db23231c5 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -29,16 +29,6 @@ jobs: build_type: [Debug, Release] build_unstable: [ON] include: - # - name: ubuntu-20.04-gcc-9 - # os: ubuntu-20.04 - # compiler: gcc - # version: "9" - - # - name: ubuntu-20.04-clang-9 - # os: ubuntu-20.04 - # compiler: clang - # version: "9" - - name: ubuntu-22.04-gcc-11 os: ubuntu-22.04 compiler: gcc From 6caea27638ff53e09c260aa3b5ea54da8882a9cd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 22 Feb 2023 23:10:16 -0800 Subject: [PATCH 12/14] Another CI fix --- .github/workflows/build-linux.yml | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index db23231c5..d141d267e 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -20,8 +20,8 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - ubuntu-20.04-gcc-9, - ubuntu-20.04-clang-9, + # ubuntu-20.04-gcc-9, + # ubuntu-20.04-clang-9, ubuntu-22.04-gcc-11, ubuntu-22.04-clang-14, ] @@ -29,6 +29,16 @@ jobs: build_type: [Debug, Release] build_unstable: [ON] include: + # - name: ubuntu-20.04-gcc-9 + # os: ubuntu-20.04 + # compiler: gcc + # version: "9" + + # - name: ubuntu-20.04-clang-9 + # os: ubuntu-20.04 + # compiler: clang + # version: "9" + - name: ubuntu-22.04-gcc-11 os: ubuntu-22.04 compiler: gcc From 38f0158890bd6aa7ea829a0fdef31db3491bffe0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 23 Feb 2023 06:49:36 -0800 Subject: [PATCH 13/14] Build Release only --- .github/workflows/build-linux.yml | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index d141d267e..b678a71db 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -20,24 +20,24 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - # ubuntu-20.04-gcc-9, - # ubuntu-20.04-clang-9, + ubuntu-20.04-gcc-9, + ubuntu-20.04-clang-9, ubuntu-22.04-gcc-11, ubuntu-22.04-clang-14, ] - build_type: [Debug, Release] + build_type: [Release] build_unstable: [ON] include: - # - name: ubuntu-20.04-gcc-9 - # os: ubuntu-20.04 - # compiler: gcc - # version: "9" + - name: ubuntu-20.04-gcc-9 + os: ubuntu-20.04 + compiler: gcc + version: "9" - # - name: ubuntu-20.04-clang-9 - # os: ubuntu-20.04 - # compiler: clang - # version: "9" + - name: ubuntu-20.04-clang-9 + os: ubuntu-20.04 + compiler: clang + version: "9" - name: ubuntu-22.04-gcc-11 os: ubuntu-22.04 From 36b376339872b84c8117bbd0cb9b8468063d4085 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 23 Feb 2023 06:46:15 -0800 Subject: [PATCH 14/14] Fix warnings on gcc --- gtsam/sfm/DsfTrackGenerator.cpp | 3 ++- gtsam/sfm/ShonanAveraging.h | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.cpp b/gtsam/sfm/DsfTrackGenerator.cpp index 6880138d9..cf989f282 100644 --- a/gtsam/sfm/DsfTrackGenerator.cpp +++ b/gtsam/sfm/DsfTrackGenerator.cpp @@ -39,7 +39,8 @@ static DSFMapIndexPair generateDSF(const MatchIndicesMap& matches) { // Image pair is (i1,i2). size_t i1 = pair_indices.first; size_t i2 = pair_indices.second; - for (size_t k = 0; k < corr_indices.rows(); k++) { + size_t m = static_cast(corr_indices.rows()); + for (size_t k = 0; k < m; k++) { // Measurement indices are found in a single matrix row, as (k1,k2). size_t k1 = corr_indices(k, 0), k2 = corr_indices(k, 1); // Unique key for DSF is (i,k), representing keypoint index in an image. diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index 989733cb5..4896b885d 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -366,7 +366,7 @@ class GTSAM_EXPORT ShonanAveraging { template static Values LiftTo(size_t p, const Values &values) { Values result; - for (const auto it : values.extract()) { + for (const auto& it : values.extract()) { result.insert(it.first, SOn::Lift(p, it.second.matrix())); } return result;