From c2fb82b935562b304e0c08eca5d1331684b0f282 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 15 Apr 2013 19:54:46 +0000 Subject: [PATCH] Added matlab wrapping for Fixed-Lag Smoothers and Concurrent Filters and Smoothers --- gtsam_unstable/gtsam_unstable.h | 126 ++++++++++++++++++ .../nonlinear/ConcurrentBatchFilter.h | 16 ++- .../nonlinear/ConcurrentBatchSmoother.h | 12 ++ gtsam_unstable/nonlinear/FixedLagSmoother.h | 1 + 4 files changed, 151 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index adb14f25d..24547b49e 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -20,8 +20,13 @@ class gtsam::GaussianFactorGraph; class gtsam::NonlinearFactorGraph; class gtsam::Ordering; class gtsam::Values; +class gtsam::Key; +class gtsam::VectorValues; +class gtsam::KeyList; class gtsam::KeySet; class gtsam::KeyVector; +class gtsam::LevenbergMarquardtParams; +class gtsam::ISAM2Params; namespace gtsam { @@ -391,6 +396,127 @@ pair partialCholeskySummarization(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, const gtsam::KeySet& overlap_keys); +#include +class FixedLagSmootherKeyTimestampMapValue { + FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& 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 gtsam::Key& 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& timestamps); + gtsam::Values calculateEstimate() const; +}; + +#include +virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { + BatchFixedLagSmoother(); + BatchFixedLagSmoother(double smootherLag); + BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); + + gtsam::LevenbergMarquardtParams params() const; +}; + +#include +virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { + IncrementalFixedLagSmoother(); + IncrementalFixedLagSmoother(double smootherLag); + IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); + + gtsam::ISAM2Params params() const; +}; + +#include +virtual class ConcurrentFilter { + void print(string s) const; + bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; +}; + +virtual class ConcurrentSmoother { + void print(string s) const; + bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; +}; + +// Synchronize function +void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); + +#include +class ConcurrentBatchFilterResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { + ConcurrentBatchFilter(); + ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchFilterResult update(); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); + gtsam::Values calculateEstimate() const; +}; + +#include +class ConcurrentBatchSmootherResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { + ConcurrentBatchSmoother(); + ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchSmootherResult update(); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::Values calculateEstimate() const; +}; + //************************************************************************* // slam //************************************************************************* diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 875eb7c79..90b3bc33e 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -41,7 +41,16 @@ 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 + + /// Constructor Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {}; + + /// Getter methods + size_t getIterations() const { return iterations; } + size_t getLambdas() const { return lambdas; } + size_t getNonlinearVariables() const { return nonlinearVariables; } + size_t getLinearVariables() const { return linearVariables; } + double getError() const { return error; } }; /** Default constructor */ @@ -210,10 +219,6 @@ private: class EliminationForest { public: typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class -// typedef GaussianFactor Factor; ///< The factor Type -// typedef Factor::shared_ptr sharedFactor; ///< Shared pointer to a factor -// typedef BayesNet BayesNet; ///< The BayesNet -// typedef GaussianFactorGraph::Eliminate Eliminate; ///< The eliminate subroutine private: typedef FastList Factors; @@ -262,4 +267,7 @@ private: }; // ConcurrentBatchFilter +/// Typedef for Matlab wrapping +typedef ConcurrentBatchFilter::Result ConcurrentBatchFilterResult; + }/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index 637a3099a..f54c1f60d 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -41,7 +41,16 @@ 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 + + /// Constructor Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {}; + + /// Getter methods + size_t getIterations() const { return iterations; } + size_t getLambdas() const { return lambdas; } + size_t getNonlinearVariables() const { return nonlinearVariables; } + size_t getLinearVariables() const { return linearVariables; } + double getError() const { return error; } }; /** Default constructor */ @@ -243,4 +252,7 @@ private: }; // ConcurrentBatchSmoother +/// Typedef for Matlab wrapping +typedef ConcurrentBatchSmoother::Result ConcurrentBatchSmootherResult; + }/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index 521bd8b3b..4af10b667 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -124,6 +124,7 @@ protected: /// Typedef for matlab wrapping typedef FixedLagSmoother::KeyTimestampMap FixedLagSmootherKeyTimestampMap; +typedef FixedLagSmootherKeyTimestampMap::value_type FixedLagSmootherKeyTimestampMapValue; typedef FixedLagSmoother::Result FixedLagSmootherResult; } /// namespace gtsam