From ddcfc1b50b428d9c8a26370c91ff405367fac75f Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sun, 12 Feb 2023 17:55:19 -0800 Subject: [PATCH] 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;