Merge pull request #1475 from borglab/feature/move_smoother

.github/workflows/build-linux.yml

@@ -26,7 +26,7 @@ jobs:
           ubuntu-22.04-clang-14,
         ]
 
-        build_type: [Debug, Release]
+        build_type: [Release]
         build_unstable: [ON]
         include:
           - name: ubuntu-20.04-gcc-9

cmake/GtsamBuildTypes.cmake

@@ -129,7 +129,6 @@ else()
     -fPIC                                          # ensure proper code generation for shared libraries
     $<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr>            # Error: return local address
     $<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address   -Werror=return-stack-address>  # Error: return local address
-    $<$<CXX_COMPILER_ID:Clang>:-Wno-misleading-indentation>  # Eigen triggers a ton!
     $<$<CXX_COMPILER_ID:Clang>:-Wno-weak-template-vtables>  # TODO(dellaert): don't know how to resolve
     $<$<CXX_COMPILER_ID:Clang>:-Wno-weak-vtables>  # TODO(dellaert): don't know how to resolve
     -Wreturn-type  -Werror=return-type             # Error on missing return()

examples/FixedLagSmootherExample.cpp

@@ -23,7 +23,7 @@
  */
 
 // This example demonstrates the use of the Fixed-Lag Smoothers in GTSAM unstable
-#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
+#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
 #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
 
 // In GTSAM, measurement functions are represented as 'factors'. Several common factors

gtsam/base/VectorSpace.h

@@ -185,7 +185,7 @@ template<class Class>
 struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
 
   // Check that Class has the necessary machinery
-  GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs<Class>);
+GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs<Class>);
 
   typedef vector_space_tag structure_category;
 

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
 

gtsam/nonlinear/BatchFixedLagSmoother.cpp

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

gtsam/nonlinear/BatchFixedLagSmoother.h

@@ -0,0 +1,182 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    BatchFixedLagSmoother.h
+ * @brief   An LM-based fixed-lag smoother.
+ *
+ * @author  Michael Kaess, Stephen Williams
+ * @date    Oct 14, 2012
+ */
+
+// \callgraph
+#pragma once
+
+#include <gtsam/nonlinear/FixedLagSmoother.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <queue>
+
+namespace gtsam {
+
+class GTSAM_EXPORT BatchFixedLagSmoother : public FixedLagSmoother {
+
+public:
+
+  /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
+  typedef std::shared_ptr<BatchFixedLagSmoother> shared_ptr;
+
+  /** default constructor */
+  BatchFixedLagSmoother(double smootherLag = 0.0, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool enforceConsistency = true) :
+    FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) { }
+
+  /** destructor */
+  ~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;
+
+  /** Check if two IncrementalFixedLagSmoother Objects are equal */
+  bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override;
+
+  /** Add new factors, updating the solution and relinearizing as needed. */
+  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
+                const Values& newTheta = Values(),
+                const KeyTimestampMap& timestamps = KeyTimestampMap(),
+                const FactorIndices& factorsToRemove = FactorIndices()) override;
+
+  /** Compute an estimate from the incomplete linear delta computed during the last update.
+   * This delta is incomplete because it was not updated below wildfire_threshold.  If only
+   * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
+   */
+  Values calculateEstimate() const override {
+    return theta_.retract(delta_);
+  }
+
+  /** Compute an estimate for a single variable using its incomplete linear delta computed
+   * during the last update.  This is faster than calling the no-argument version of
+   * calculateEstimate, which operates on all variables.
+   * @param key
+   * @return
+   */
+  template<class VALUE>
+  VALUE calculateEstimate(Key key) const {
+    const Vector delta = delta_.at(key);
+    return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
+  }
+
+  /** read the current set of optimizer parameters */
+  const LevenbergMarquardtParams& params() const {
+    return parameters_;
+  }
+
+  /** update the current set of optimizer parameters */
+  LevenbergMarquardtParams& params() {
+    return parameters_;
+  }
+
+  /** Access the current set of factors */
+  const NonlinearFactorGraph& getFactors() const {
+    return factors_;
+  }
+
+  /** Access the current linearization point */
+  const Values& getLinearizationPoint() const {
+    return theta_;
+  }
+
+  /** Access the current ordering */
+  const Ordering& getOrdering() const {
+    return ordering_;
+  }
+
+  /** Access the current set of deltas to the linearization point */
+  const VectorValues& getDelta() const {
+    return delta_;
+  }
+
+  /// Calculate marginal covariance on given variable
+  Matrix marginalCovariance(Key key) const;
+
+  /// Marginalize specific keys from a linear graph.
+  /// Does not check whether keys actually exist in graph.
+  /// In that case will fail somewhere deep within elimination
+  static GaussianFactorGraph CalculateMarginalFactors(
+      const GaussianFactorGraph& graph, const KeyVector& keys,
+      const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
+
+  /// Marginalize specific keys from a nonlinear graph, wrap in LinearContainers
+  static NonlinearFactorGraph CalculateMarginalFactors(
+      const NonlinearFactorGraph& graph, const Values& theta, const KeyVector& keys,
+      const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
+
+protected:
+
+  /** A typedef defining an Key-Factor mapping **/
+  typedef std::map<Key, std::set<Key> > FactorIndex;
+
+  /** The L-M optimization parameters **/
+  LevenbergMarquardtParams parameters_;
+
+  /** A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the
+   * linearization point of all variables involved in linearized/marginal factors at the edge of the
+   * smoothing window. This idea is from ??? TODO: Look up paper reference **/
+  bool enforceConsistency_;
+
+  /** The nonlinear factors **/
+  NonlinearFactorGraph factors_;
+
+  /** The current linearization point **/
+  Values theta_;
+
+  /** The set of values involved in current linear factors. **/
+  Values linearValues_;
+
+  /** The current ordering */
+  Ordering ordering_;
+
+  /** The current set of linear deltas */
+  VectorValues delta_;
+
+  /** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/
+  std::queue<size_t> availableSlots_;
+
+  /** A cross-reference structure to allow efficient factor lookups by key **/
+  FactorIndex factorIndex_;
+
+  /** Augment the list of factors with a set of new factors */
+  void insertFactors(const NonlinearFactorGraph& newFactors);
+
+  /** Remove factors from the list of factors by slot index */
+  void removeFactors(const std::set<size_t>& deleteFactors);
+
+  /** Erase any keys associated with timestamps before the provided time */
+  void eraseKeys(const KeyVector& keys);
+
+  /** Use colamd to update into an efficient ordering */
+  void reorder(const KeyVector& marginalizeKeys = KeyVector());
+
+  /** Optimize the current graph using a modified version of L-M */
+  Result optimize();
+
+  /** Marginalize out selected variables */
+  void marginalize(const KeyVector& marginalizableKeys);
+
+private:
+  /** Private methods for printing debug information */
+  static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
+  static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label);
+  static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
+  static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
+  static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);
+  static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label);
+}; // BatchFixedLagSmoother
+
+} /// namespace gtsam

gtsam/nonlinear/Expression.h

@@ -218,7 +218,7 @@ protected:
 template <typename T>
 class ScalarMultiplyExpression : public Expression<T> {
   // Check that T is a vector space
-  GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace<T>);
+  GTSAM_CONCEPT_ASSERT(IsVectorSpace<T>);
 
  public:
   explicit ScalarMultiplyExpression(double s, const Expression<T>& e);
@@ -231,7 +231,7 @@ class ScalarMultiplyExpression : public Expression<T> {
 template <typename T>
 class BinarySumExpression : public Expression<T> {
   // Check that T is a vector space
-  GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace<T>);
+  GTSAM_CONCEPT_ASSERT(IsVectorSpace<T>);
 
  public:
   explicit BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2);

gtsam/nonlinear/FixedLagSmoother.cpp

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

gtsam/nonlinear/FixedLagSmoother.h

@@ -0,0 +1,138 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    FixedLagSmoother.h
+ * @brief   Base class for a fixed-lag smoother. This mimics the basic interface to iSAM2.
+ *
+ * @author  Stephen Williams
+ * @date    Feb 27, 2013
+ */
+
+// \callgraph
+#pragma once
+
+#include <gtsam_unstable/dllexport.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+
+#include <map>
+#include <vector>
+
+namespace gtsam {
+
+class GTSAM_EXPORT FixedLagSmoother {
+
+public:
+
+  /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
+  typedef std::shared_ptr<FixedLagSmoother> shared_ptr;
+
+  /// Typedef for a Key-Timestamp map/database
+  typedef std::map<Key, double> KeyTimestampMap;
+  typedef std::multimap<double, Key> TimestampKeyMap;
+
+  /**
+   * Meta information returned about the update
+   */
+  // TODO: Think of some more things to put here
+  struct Result {
+    size_t iterations; ///< The number of optimizer iterations performed
+    size_t intermediateSteps; ///< The number of intermediate steps performed within the optimization. For L-M, this is the number of lambdas tried.
+    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) {}
+
+    /// Getter methods
+    size_t getIterations() const { return iterations; }
+    size_t getIntermediateSteps() const { return intermediateSteps; }
+    size_t getNonlinearVariables() const { return nonlinearVariables; }
+    size_t getLinearVariables() const { return linearVariables; }
+    double getError() const { return error; }
+    void print() const;
+  };
+
+  /** default constructor */
+  FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { }
+
+  /** destructor */
+  virtual ~FixedLagSmoother() { }
+
+  /** Print the factor for debugging and testing (implementing Testable) */
+  virtual void print(
+      const std::string& s = "FixedLagSmoother:\n",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+
+  /** Check if two IncrementalFixedLagSmoother Objects are equal */
+  virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const;
+
+  /** read the current smoother lag */
+  double smootherLag() const {
+    return smootherLag_;
+  }
+
+  /** write to the current smoother lag */
+  double& smootherLag() {
+    return smootherLag_;
+  }
+
+  /** Access the current set of timestamps associated with each variable */
+  const KeyTimestampMap& timestamps() const {
+    return keyTimestampMap_;
+  }
+
+  /** Add new factors, updating the solution and relinearizing as needed. */
+  virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
+                        const Values& newTheta = Values(),
+                        const KeyTimestampMap& timestamps = KeyTimestampMap(),
+                        const FactorIndices& factorsToRemove = FactorIndices()) = 0;
+
+  /** Compute an estimate from the incomplete linear delta computed during the last update.
+   * This delta is incomplete because it was not updated below wildfire_threshold.  If only
+   * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
+   */
+  virtual Values calculateEstimate() const  = 0;
+
+
+protected:
+
+  /** The length of the smoother lag. Any variable older than this amount will be marginalized out. */
+  double smootherLag_;
+
+  /** The current timestamp associated with each tracked key */
+  TimestampKeyMap timestampKeyMap_;
+  KeyTimestampMap keyTimestampMap_;
+
+  /** Update the Timestamps associated with the keys */
+  void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps);
+
+  /** Erase keys from the Key-Timestamps database */
+  void eraseKeyTimestampMap(const KeyVector& keys);
+
+  /** Find the most recent timestamp of the system */
+  double getCurrentTimestamp() const;
+
+  /** Find all of the keys associated with timestamps before the provided time */
+  KeyVector findKeysBefore(double timestamp) const;
+
+  /** Find all of the keys associated with timestamps before the provided time */
+  KeyVector findKeysAfter(double timestamp) const;
+
+}; // FixedLagSmoother
+
+/// Typedef for matlab wrapping
+typedef FixedLagSmoother::KeyTimestampMap FixedLagSmootherKeyTimestampMap;
+typedef FixedLagSmootherKeyTimestampMap::value_type FixedLagSmootherKeyTimestampMapValue;
+typedef FixedLagSmoother::Result FixedLagSmootherResult;
+
+} /// namespace gtsam

gtsam/nonlinear/internal/ExpressionNode.h

@@ -561,7 +561,7 @@ public:
 template <class T>
 class ScalarMultiplyNode : public ExpressionNode<T> {
   // Check that T is a vector space
-  GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace<T>);
+  GTSAM_CONCEPT_ASSERT(IsVectorSpace<T>);
 
   double scalar_;
   std::shared_ptr<ExpressionNode<T> > expression_;

gtsam/nonlinear/nonlinear.i

@@ -650,4 +650,65 @@ virtual class NonlinearEquality2 : gtsam::NoiseModelFactor {
   gtsam::Vector evaluateError(const T& x1, const T& x2);
 };
 
+#include <gtsam/nonlinear/FixedLagSmoother.h>
+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 &timestamps);
+  gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
+                                       const gtsam::Values &newTheta,
+                                       const gtsam::FixedLagSmootherKeyTimestampMap &timestamps,
+                                       const gtsam::FactorIndices &factorsToRemove);
+  gtsam::Values calculateEstimate() const;
+};
+
+#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
+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 = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
+                     gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
+                     Vector, Matrix}>
+  VALUE calculateEstimate(size_t key) const;
+};
+
+
 }  // namespace gtsam

gtsam/nonlinear/tests/testBatchFixedLagSmoother.cpp

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

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<Pose2>(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<Point2>(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;

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<size_t>(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.

gtsam/sfm/ShonanAveraging.h

@@ -366,7 +366,7 @@ class GTSAM_EXPORT ShonanAveraging {
   template <class T>
   static Values LiftTo(size_t p, const Values &values) {
     Values result;
-    for (const auto it : values.extract<T>()) {
+    for (const auto& it : values.extract<T>()) {
       result.insert(it.first, SOn::Lift(p, it.second.matrix()));
     }
     return result;

gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp

@@ -28,7 +28,7 @@
 #include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
 
 // We will compare the results to a similar Fixed-Lag Smoother
-#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
+#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
 
 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.

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 <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+#include <gtsam/nonlinear/FixedLagSmoother.h>
-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 <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::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
-                                       const gtsam::Values &newTheta,
-                                       const gtsam::FixedLagSmootherKeyTimestampMap &timestamps,
-                                       const gtsam::FactorIndices &factorsToRemove);
-  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);
-
-  void print(string s = "BatchFixedLagSmoother:\n") const;
-
-  gtsam::LevenbergMarquardtParams params() const;
-  template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
-                     gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
-                     Vector, Matrix}>
-  VALUE calculateEstimate(size_t key) const;
-};
-
 #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
 virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
   IncrementalFixedLagSmoother();

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_;
 };
 }

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_;
 };
 }

gtsam_unstable/nonlinear/BatchFixedLagSmoother.h

@@ -9,174 +9,13 @@
 
  * -------------------------------------------------------------------------- */
 
-/**
- * @file    BatchFixedLagSmoother.h
- * @brief   An LM-based fixed-lag smoother.
- *
- * @author  Michael Kaess, Stephen Williams
- * @date    Oct 14, 2012
- */
-
-// \callgraph
 #pragma once
 
-#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+#ifdef _MSC_VER
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#pragma message("BatchFixedLagSmoother was moved to the gtsam/nonlinear directory")
-#include <queue>
+#else
+#warning "BatchFixedLagSmoother was moved to the gtsam/nonlinear directory"
+#endif
 
-namespace gtsam {
 
-class GTSAM_UNSTABLE_EXPORT BatchFixedLagSmoother : public FixedLagSmoother {
+#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
-
-public:
-
-  /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
-  typedef std::shared_ptr<BatchFixedLagSmoother> shared_ptr;
-
-  /** default constructor */
-  BatchFixedLagSmoother(double smootherLag = 0.0, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool enforceConsistency = true) :
-    FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) { };
-
-  /** destructor */
-  ~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;
-
-  /** Check if two IncrementalFixedLagSmoother Objects are equal */
-  bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override;
-
-  /** Add new factors, updating the solution and relinearizing as needed. */
-  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
-                const Values& newTheta = Values(),
-                const KeyTimestampMap& timestamps = KeyTimestampMap(),
-                const FactorIndices& factorsToRemove = FactorIndices()) override;
-
-  /** Compute an estimate from the incomplete linear delta computed during the last update.
-   * This delta is incomplete because it was not updated below wildfire_threshold.  If only
-   * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
-   */
-  Values calculateEstimate() const override {
-    return theta_.retract(delta_);
-  }
-
-  /** Compute an estimate for a single variable using its incomplete linear delta computed
-   * during the last update.  This is faster than calling the no-argument version of
-   * calculateEstimate, which operates on all variables.
-   * @param key
-   * @return
-   */
-  template<class VALUE>
-  VALUE calculateEstimate(Key key) const {
-    const Vector delta = delta_.at(key);
-    return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
-  }
-
-  /** read the current set of optimizer parameters */
-  const LevenbergMarquardtParams& params() const {
-    return parameters_;
-  }
-
-  /** update the current set of optimizer parameters */
-  LevenbergMarquardtParams& params() {
-    return parameters_;
-  }
-
-  /** Access the current set of factors */
-  const NonlinearFactorGraph& getFactors() const {
-    return factors_;
-  }
-
-  /** Access the current linearization point */
-  const Values& getLinearizationPoint() const {
-    return theta_;
-  }
-
-  /** Access the current ordering */
-  const Ordering& getOrdering() const {
-    return ordering_;
-  }
-
-  /** Access the current set of deltas to the linearization point */
-  const VectorValues& getDelta() const {
-    return delta_;
-  }
-
-  /// Calculate marginal covariance on given variable
-  Matrix marginalCovariance(Key key) const;
-
-  /// Marginalize specific keys from a linear graph.
-  /// Does not check whether keys actually exist in graph.
-  /// In that case will fail somewhere deep within elimination
-  static GaussianFactorGraph CalculateMarginalFactors(
-      const GaussianFactorGraph& graph, const KeyVector& keys,
-      const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
-
-  /// Marginalize specific keys from a nonlinear graph, wrap in LinearContainers
-  static NonlinearFactorGraph CalculateMarginalFactors(
-      const NonlinearFactorGraph& graph, const Values& theta, const KeyVector& keys,
-      const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
-
-protected:
-
-  /** A typedef defining an Key-Factor mapping **/
-  typedef std::map<Key, std::set<Key> > FactorIndex;
-
-  /** The L-M optimization parameters **/
-  LevenbergMarquardtParams parameters_;
-
-  /** A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the
-   * linearization point of all variables involved in linearized/marginal factors at the edge of the
-   * smoothing window. This idea is from ??? TODO: Look up paper reference **/
-  bool enforceConsistency_;
-
-  /** The nonlinear factors **/
-  NonlinearFactorGraph factors_;
-
-  /** The current linearization point **/
-  Values theta_;
-
-  /** The set of values involved in current linear factors. **/
-  Values linearValues_;
-
-  /** The current ordering */
-  Ordering ordering_;
-
-  /** The current set of linear deltas */
-  VectorValues delta_;
-
-  /** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/
-  std::queue<size_t> availableSlots_;
-
-  /** A cross-reference structure to allow efficient factor lookups by key **/
-  FactorIndex factorIndex_;
-
-  /** Augment the list of factors with a set of new factors */
-  void insertFactors(const NonlinearFactorGraph& newFactors);
-
-  /** Remove factors from the list of factors by slot index */
-  void removeFactors(const std::set<size_t>& deleteFactors);
-
-  /** Erase any keys associated with timestamps before the provided time */
-  void eraseKeys(const KeyVector& keys);
-
-  /** Use colamd to update into an efficient ordering */
-  void reorder(const KeyVector& marginalizeKeys = KeyVector());
-
-  /** Optimize the current graph using a modified version of L-M */
-  Result optimize();
-
-  /** Marginalize out selected variables */
-  void marginalize(const KeyVector& marginalizableKeys);
-
-private:
-  /** Private methods for printing debug information */
-  static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
-  static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label);
-  static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
-  static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
-  static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);
-  static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label);
-}; // BatchFixedLagSmoother
-
-} /// namespace gtsam

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;

gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h

@@ -41,10 +41,10 @@ public:
   typedef std::shared_ptr<ConcurrentFilter> 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<ConcurrentSmoother> 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
 

gtsam_unstable/nonlinear/FixedLagSmoother.h

@@ -9,130 +9,13 @@
 
  * -------------------------------------------------------------------------- */
 
-/**
- * @file    FixedLagSmoother.h
- * @brief   Base class for a fixed-lag smoother. This mimics the basic interface to iSAM2.
- *
- * @author  Stephen Williams
- * @date    Feb 27, 2013
- */
-
-// \callgraph
 #pragma once
 
-#include <gtsam_unstable/dllexport.h>
+#ifdef _MSC_VER
-#include <gtsam/inference/Key.h>
+#pragma message("FixedLagSmoother was moved to the gtsam/nonlinear directory")
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#else
-#include <gtsam/nonlinear/Values.h>
+#warning "FixedLagSmoother was moved to the gtsam/nonlinear directory"
-
+#endif
-#include <map>
-#include <vector>
-
-namespace gtsam {
-
-class GTSAM_UNSTABLE_EXPORT FixedLagSmoother {
-
-public:
-
-  /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
-  typedef std::shared_ptr<FixedLagSmoother> shared_ptr;
-
-  /// Typedef for a Key-Timestamp map/database
-  typedef std::map<Key, double> KeyTimestampMap;
-  typedef std::multimap<double, Key> TimestampKeyMap;
-
-  /**
-   * Meta information returned about the update
-   */
-  // TODO: Think of some more things to put here
-  struct Result {
-    size_t iterations; ///< The number of optimizer iterations performed
-    size_t intermediateSteps; ///< The number of intermediate steps performed within the optimization. For L-M, this is the number of lambdas tried.
-    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) {};
-
-    /// Getter methods
-    size_t getIterations() const { return iterations; }
-    size_t getIntermediateSteps() const { return intermediateSteps; }
-    size_t getNonlinearVariables() const { return nonlinearVariables; }
-    size_t getLinearVariables() const { return linearVariables; }
-    double getError() const { return error; }
-    void print() const;
-  };
-
-  /** default constructor */
-  FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { }
-
-  /** destructor */
-  virtual ~FixedLagSmoother() { }
-
-  /** Print the factor for debugging and testing (implementing Testable) */
-  virtual void print(
-      const std::string& s = "FixedLagSmoother:\n",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
-
-  /** Check if two IncrementalFixedLagSmoother Objects are equal */
-  virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const;
-
-  /** read the current smoother lag */
-  double smootherLag() const {
-    return smootherLag_;
-  }
-
-  /** write to the current smoother lag */
-  double& smootherLag() {
-    return smootherLag_;
-  }
-
-  /** Access the current set of timestamps associated with each variable */
-  const KeyTimestampMap& timestamps() const {
-    return keyTimestampMap_;
-  }
-
-  /** Add new factors, updating the solution and relinearizing as needed. */
-  virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
-                        const Values& newTheta = Values(),
-                        const KeyTimestampMap& timestamps = KeyTimestampMap(),
-                        const FactorIndices& factorsToRemove = FactorIndices()) = 0;
-
-  /** Compute an estimate from the incomplete linear delta computed during the last update.
-   * This delta is incomplete because it was not updated below wildfire_threshold.  If only
-   * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
-   */
-  virtual Values calculateEstimate() const  = 0;
 
 
-protected:
+#include <gtsam/nonlinear/FixedLagSmoother.h>
-
-  /** The length of the smoother lag. Any variable older than this amount will be marginalized out. */
-  double smootherLag_;
-
-  /** The current timestamp associated with each tracked key */
-  TimestampKeyMap timestampKeyMap_;
-  KeyTimestampMap keyTimestampMap_;
-
-  /** Update the Timestamps associated with the keys */
-  void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps);
-
-  /** Erase keys from the Key-Timestamps database */
-  void eraseKeyTimestampMap(const KeyVector& keys);
-
-  /** Find the most recent timestamp of the system */
-  double getCurrentTimestamp() const;
-
-  /** Find all of the keys associated with timestamps before the provided time */
-  KeyVector findKeysBefore(double timestamp) const;
-
-  /** Find all of the keys associated with timestamps before the provided time */
-  KeyVector findKeysAfter(double timestamp) const;
-
-}; // FixedLagSmoother
-
-/// Typedef for matlab wrapping
-typedef FixedLagSmoother::KeyTimestampMap FixedLagSmootherKeyTimestampMap;
-typedef FixedLagSmootherKeyTimestampMap::value_type FixedLagSmootherKeyTimestampMapValue;
-typedef FixedLagSmoother::Result FixedLagSmootherResult;
-
-} /// namespace gtsam

gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h

@@ -20,7 +20,7 @@
 // \callgraph
 #pragma once
 
-#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+#include <gtsam/nonlinear/FixedLagSmoother.h>
 #include <gtsam/nonlinear/ISAM2.h>
 
 namespace gtsam {

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;

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

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)

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)