Moves 'BatchFixedLagSmoother' to 'gtsam' from 'gtsam_unstable'

release/4.3a0
Ankur Roy Chowdhury 2023-02-12 17:55:19 -08:00 committed by Frank Dellaert
parent 0a524dd02e
commit ddcfc1b50b
10 changed files with 26 additions and 32 deletions

View File

@ -17,7 +17,7 @@
* @date Oct 14, 2012 * @date Oct 14, 2012
*/ */
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h> #include <gtsam/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam/nonlinear/LinearContainerFactor.h> #include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/linear/GaussianJunctionTree.h> #include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>

View File

@ -20,7 +20,7 @@
// \callgraph // \callgraph
#pragma once #pragma once
#include <gtsam_unstable/nonlinear/FixedLagSmoother.h> #include <gtsam/nonlinear/FixedLagSmoother.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <queue> #include <queue>
@ -35,10 +35,10 @@ public:
/** default constructor */ /** default constructor */
BatchFixedLagSmoother(double smootherLag = 0.0, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool enforceConsistency = true) : 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 */ /** destructor */
~BatchFixedLagSmoother() override { }; ~BatchFixedLagSmoother() override {}
/** Print the factor for debugging and testing (implementing Testable) */ /** Print the factor for debugging and testing (implementing Testable) */
void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;

View File

@ -17,7 +17,7 @@
* @date Feb 27, 2013 * @date Feb 27, 2013
*/ */
#include <gtsam_unstable/nonlinear/FixedLagSmoother.h> #include <gtsam/nonlinear/FixedLagSmoother.h>
namespace gtsam { namespace gtsam {

View File

@ -51,7 +51,7 @@ public:
size_t nonlinearVariables; ///< The number of variables that can be relinearized 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 size_t linearVariables; ///< The number of variables that must keep a constant linearization point
double error; ///< The final factor graph error 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 /// Getter methods
size_t getIterations() const { return iterations; } size_t getIterations() const { return iterations; }

View File

@ -17,7 +17,7 @@
*/ */
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h> #include <gtsam/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>

View File

@ -33,13 +33,10 @@ public:
} }
const char *what() const noexcept override { const char *what() const noexcept override {
if (description_.empty()) if (description_->empty())
description_ = description_ =
"An infeasible initial value was provided for the solver.\n"; "An infeasible initial value was provided for the solver.\n";
return description_.c_str(); return description_->c_str();
} }
private:
mutable std::string description_;
}; };
} }

View File

@ -29,12 +29,9 @@ public:
} }
const char* what() const noexcept override { const char* what() const noexcept override {
if (description_.empty()) if (description_->empty())
description_ = "The problem is either infeasible or unbounded.\n"; description_ = "The problem is either infeasible or unbounded.\n";
return description_.c_str(); return description_->c_str();
} }
private:
mutable std::string description_;
}; };
} }

View File

@ -50,7 +50,7 @@ public:
double error; ///< The final factor graph error double error; ///< The final factor graph error
/// Constructor /// 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 /// Getter methods
size_t getIterations() const { return iterations; } size_t getIterations() const { return iterations; }
@ -61,10 +61,10 @@ public:
}; };
/** Default constructor */ /** Default constructor */
ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {}; ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {}
/** Default destructor */ /** Default destructor */
~ConcurrentBatchFilter() override {}; ~ConcurrentBatchFilter() override = default;
/** Implement a GTSAM standard 'print' function */ /** Implement a GTSAM standard 'print' function */
void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;

View File

@ -41,10 +41,10 @@ public:
typedef std::shared_ptr<ConcurrentFilter> shared_ptr; typedef std::shared_ptr<ConcurrentFilter> shared_ptr;
/** Default constructor */ /** Default constructor */
ConcurrentFilter() {}; ConcurrentFilter() = default;
/** Default destructor */ /** Default destructor */
virtual ~ConcurrentFilter() {}; virtual ~ConcurrentFilter() = default;
/** Implement a standard 'print' function */ /** Implement a standard 'print' function */
virtual void print( virtual void print(
@ -58,7 +58,7 @@ public:
* Perform any required operations before the synchronization process starts. * Perform any required operations before the synchronization process starts.
* Called by 'synchronize' * Called by 'synchronize'
*/ */
virtual void presync() {}; virtual void presync() {}
/** /**
* Populate the provided containers with factors that constitute the filter branch summarization * 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. * Perform any required operations after the synchronization process finishes.
* Called by 'synchronize' * Called by 'synchronize'
*/ */
virtual void postsync() {}; virtual void postsync() {}
}; // ConcurrentFilter }; // ConcurrentFilter
@ -103,10 +103,10 @@ public:
typedef std::shared_ptr<ConcurrentSmoother> shared_ptr; typedef std::shared_ptr<ConcurrentSmoother> shared_ptr;
/** Default constructor */ /** Default constructor */
ConcurrentSmoother() {}; ConcurrentSmoother() {}
/** Default destructor */ /** Default destructor */
virtual ~ConcurrentSmoother() {}; virtual ~ConcurrentSmoother() = default;
/** Implement a standard 'print' function */ /** Implement a standard 'print' function */
virtual void print( virtual void print(
@ -120,7 +120,7 @@ public:
* Perform any required operations before the synchronization process starts. * Perform any required operations before the synchronization process starts.
* Called by 'synchronize' * Called by 'synchronize'
*/ */
virtual void presync() {}; virtual void presync() {}
/** /**
* Populate the provided containers with factors that constitute the smoother branch summarization * 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. * Perform any required operations after the synchronization process finishes.
* Called by 'synchronize' * Called by 'synchronize'
*/ */
virtual void postsync() {}; virtual void postsync() {}
}; // ConcurrentSmoother }; // ConcurrentSmoother

View File

@ -45,7 +45,7 @@ protected:
public: public:
/** default constructor for serialization */ /** default constructor for serialization */
LinearizedGaussianFactor() {}; LinearizedGaussianFactor() = default;
/** /**
* @param gaussian: A jacobian or hessian factor * @param gaussian: A jacobian or hessian factor
@ -53,7 +53,7 @@ public:
*/ */
LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Values& lin_points); LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Values& lin_points);
~LinearizedGaussianFactor() override {}; ~LinearizedGaussianFactor() override = default;
// access functions // access functions
const Values& linearizationPoint() const { return lin_points_; } const Values& linearizationPoint() const { return lin_points_; }
@ -128,11 +128,11 @@ public:
// access functions // access functions
const constBVector b() const { return Ab_(size()).col(0); } 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()); } 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) */ /** 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 */ /** Calculate the error of the factor */
double error(const Values& c) const override; double error(const Values& c) const override;