Added matlab wrapping for Fixed-Lag Smoothers and Concurrent Filters and Smoothers

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<gtsam::GaussianFactorGraph,gtsam::Ordering>
 partialCholeskySummarization(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
     const gtsam::KeySet& overlap_keys);
 
+#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+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 <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+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 <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
+virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
+  BatchFixedLagSmoother();
+  BatchFixedLagSmoother(double smootherLag);
+  BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
+
+  gtsam::LevenbergMarquardtParams params() const;
+};
+
+#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
+virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
+  IncrementalFixedLagSmoother();
+  IncrementalFixedLagSmoother(double smootherLag);
+  IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
+
+  gtsam::ISAM2Params params() const;
+};
+
+#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
+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 <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
+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 <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
+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
 //*************************************************************************

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<EliminationForest> 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<Factor::ConditionalType> BayesNet; ///< The BayesNet
-//    typedef GaussianFactorGraph::Eliminate Eliminate; ///< The eliminate subroutine
 
   private:
     typedef FastList<GaussianFactor::shared_ptr> Factors;
@@ -262,4 +267,7 @@ private:
 
 }; // ConcurrentBatchFilter
 
+/// Typedef for Matlab wrapping
+typedef ConcurrentBatchFilter::Result ConcurrentBatchFilterResult;
+
 }/// namespace gtsam

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

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