Merge branch 'develop' into release/4.2a5

dellaert · dellaert · commit 4a1ec24de53a · 2022-02-05T17:36:01.000-05:00
diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
@@ -70,7 +70,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
   string dot(
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
       bool showZero = true) const;
-  std::vector<std::pair<DiscreteValues, double>> enumerate() const;
+  std::vector<std::pair<gtsam::DiscreteValues, double>> enumerate() const;
   string markdown(const gtsam::KeyFormatter& keyFormatter =
                       gtsam::DefaultKeyFormatter) const;
   string markdown(const gtsam::KeyFormatter& keyFormatter,
@@ -97,7 +97,7 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor {
                       const gtsam::Ordering& orderedKeys);
   gtsam::DiscreteConditional operator*(
       const gtsam::DiscreteConditional& other) const;
-  DiscreteConditional marginal(gtsam::Key key) const;
+  gtsam::DiscreteConditional marginal(gtsam::Key key) const;
   void print(string s = "Discrete Conditional\n",
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
@@ -269,16 +269,16 @@ class DiscreteFactorGraph {
   gtsam::DiscreteLookupDAG maxProduct(gtsam::Ordering::OrderingType type);
   gtsam::DiscreteLookupDAG maxProduct(const gtsam::Ordering& ordering);
 
-  gtsam::DiscreteBayesNet eliminateSequential();
-  gtsam::DiscreteBayesNet eliminateSequential(gtsam::Ordering::OrderingType type);
-  gtsam::DiscreteBayesNet eliminateSequential(const gtsam::Ordering& ordering);
-  std::pair<gtsam::DiscreteBayesNet, gtsam::DiscreteFactorGraph>
+  gtsam::DiscreteBayesNet* eliminateSequential();
+  gtsam::DiscreteBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type);
+  gtsam::DiscreteBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
+  pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*>
       eliminatePartialSequential(const gtsam::Ordering& ordering);
 
-  gtsam::DiscreteBayesTree eliminateMultifrontal();
-  gtsam::DiscreteBayesTree eliminateMultifrontal(gtsam::Ordering::OrderingType type);  
-  gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering);  
-  std::pair<gtsam::DiscreteBayesTree, gtsam::DiscreteFactorGraph>
+  gtsam::DiscreteBayesTree* eliminateMultifrontal();
+  gtsam::DiscreteBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type);  
+  gtsam::DiscreteBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);  
+  pair<gtsam::DiscreteBayesTree*, gtsam::DiscreteFactorGraph*>
       eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
 
   string dot(
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
@@ -41,6 +41,9 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
  public:
   enum { dimension = 3 };
 
+  ///< shared pointer to stereo calibration object
+  using shared_ptr = boost::shared_ptr<Cal3Bundler>;
+
   /// @name Standard Constructors
   /// @{
 
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
@@ -37,6 +37,9 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
  public:
   enum { dimension = 9 };
 
+  ///< shared pointer to stereo calibration object
+  using shared_ptr = boost::shared_ptr<Cal3DS2>;
+
   /// @name Standard Constructors
   /// @{
 
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
@@ -47,6 +47,9 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
  public:
   enum { dimension = 9 };
 
+  ///< shared pointer to stereo calibration object
+  using shared_ptr = boost::shared_ptr<Cal3DS2_Base>;
+
   /// @name Standard Constructors
   /// @{
 
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
@@ -52,6 +52,9 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
  public:
   enum { dimension = 10 };
 
+  ///< shared pointer to stereo calibration object
+  using shared_ptr = boost::shared_ptr<Cal3Unified>;
+
   /// @name Standard Constructors
   /// @{
 
diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h
@@ -300,50 +300,23 @@ struct GTSAM_EXPORT ISAM2Params {
   RelinearizationThreshold getRelinearizeThreshold() const {
     return relinearizeThreshold;
   }
-  int getRelinearizeSkip() const { return relinearizeSkip; }
-  bool isEnableRelinearization() const { return enableRelinearization; }
-  bool isEvaluateNonlinearError() const { return evaluateNonlinearError; }
   std::string getFactorization() const {
     return factorizationTranslator(factorization);
   }
-  bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; }
   KeyFormatter getKeyFormatter() const { return keyFormatter; }
-  bool isEnableDetailedResults() const { return enableDetailedResults; }
-  bool isEnablePartialRelinearizationCheck() const {
-    return enablePartialRelinearizationCheck;
-  }
 
   void setOptimizationParams(OptimizationParams optimizationParams) {
     this->optimizationParams = optimizationParams;
   }
   void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) {
     this->relinearizeThreshold = relinearizeThreshold;
   }
-  void setRelinearizeSkip(int relinearizeSkip) {
-    this->relinearizeSkip = relinearizeSkip;
-  }
-  void setEnableRelinearization(bool enableRelinearization) {
-    this->enableRelinearization = enableRelinearization;
-  }
-  void setEvaluateNonlinearError(bool evaluateNonlinearError) {
-    this->evaluateNonlinearError = evaluateNonlinearError;
-  }
   void setFactorization(const std::string& factorization) {
     this->factorization = factorizationTranslator(factorization);
   }
-  void setCacheLinearizedFactors(bool cacheLinearizedFactors) {
-    this->cacheLinearizedFactors = cacheLinearizedFactors;
-  }
   void setKeyFormatter(KeyFormatter keyFormatter) {
     this->keyFormatter = keyFormatter;
   }
-  void setEnableDetailedResults(bool enableDetailedResults) {
-    this->enableDetailedResults = enableDetailedResults;
-  }
-  void setEnablePartialRelinearizationCheck(
-      bool enablePartialRelinearizationCheck) {
-    this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck;
-  }
 
   GaussianFactorGraph::Eliminate getEliminationFunction() const {
     return factorization == CHOLESKY
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
@@ -588,21 +588,19 @@ class ISAM2Params {
   void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params);
   void setRelinearizeThreshold(double threshold);
   void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map);
-  int getRelinearizeSkip() const;
-  void setRelinearizeSkip(int relinearizeSkip);
-  bool isEnableRelinearization() const;
-  void setEnableRelinearization(bool enableRelinearization);
-  bool isEvaluateNonlinearError() const;
-  void setEvaluateNonlinearError(bool evaluateNonlinearError);
   string getFactorization() const;
   void setFactorization(string factorization);
-  bool isCacheLinearizedFactors() const;
-  void setCacheLinearizedFactors(bool cacheLinearizedFactors);
-  bool isEnableDetailedResults() const;
-  void setEnableDetailedResults(bool enableDetailedResults);
-  bool isEnablePartialRelinearizationCheck() const;
-  void setEnablePartialRelinearizationCheck(
-      bool enablePartialRelinearizationCheck);
+
+  int relinearizeSkip;
+  bool enableRelinearization;
+  bool evaluateNonlinearError;
+  bool cacheLinearizedFactors;
+  bool enableDetailedResults;
+  bool enablePartialRelinearizationCheck;
+  bool findUnusedFactorSlots;
+
+  enum Factorization { CHOLESKY, QR };
+  Factorization factorization;
 };
 
 class ISAM2Clique {
diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h
@@ -39,6 +39,9 @@ class PoseRotationPrior : public NoiseModelFactor1<POSE> {
 
 public:
 
+  /** default constructor - only use for serialization */
+  PoseRotationPrior() {}
+
   /** standard constructor */
   PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model)
   : Base(model, key), measured_(rot_z) {}
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
@@ -223,12 +223,12 @@ enum KernelFunctionType {
   KernelFunctionTypeTUKEY
 };
 
-std::pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
     string filename, gtsam::noiseModel::Diagonal* model = nullptr,
     size_t maxIndex = 0, bool addNoise = false, bool smart = true,
-    gtsam::NoiseFormat noiseFormat = gtsam::NoiseFormatAUTO,
+    gtsam::NoiseFormat noiseFormat = gtsam::NoiseFormat::NoiseFormatAUTO,
     gtsam::KernelFunctionType kernelFunctionType =
-        gtsam::KernelFunctionTypeNONE);
+        gtsam::KernelFunctionType::KernelFunctionTypeNONE);
 
 void save2D(const gtsam::NonlinearFactorGraph& graph,
             const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
@@ -259,7 +259,7 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(
     string filename, const bool is3D = false,
     gtsam::KernelFunctionType kernelFunctionType =
-        gtsam::KernelFunctionTypeNONE);
+        gtsam::KernelFunctionType::KernelFunctionTypeNONE);
 void writeG2o(const gtsam::NonlinearFactorGraph& graph,
               const gtsam::Values& estimate, string filename);
 
diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i
@@ -566,7 +566,13 @@ virtual class FixedLagSmoother {
   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);
+  gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
+                                       const gtsam::Values &newTheta,
+                                       const gtsam::FixedLagSmootherKeyTimestampMap &timestamps,
+                                       const gtsam::FactorIndices &factorsToRemove);
   gtsam::Values calculateEstimate() const;
 };
 
@@ -576,6 +582,8 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
   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,
@@ -589,7 +597,12 @@ virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
   IncrementalFixedLagSmoother(double smootherLag);
   IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
 
+  void print(string s = "IncrementalFixedLagSmoother:\n") const;
+
   gtsam::ISAM2Params params() const;
+
+  gtsam::NonlinearFactorGraph getFactors() const;
+  gtsam::ISAM2 getISAM2() const;
 };
 
 #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
@@ -113,6 +113,9 @@ class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother
   /// Get results of latest isam2 update
   const ISAM2Result& getISAM2Result() const{ return isamResult_; }
 
+  /// Get the iSAM2 object which is used for the inference internally
+  const ISAM2& getISAM2() const { return isam_; }
+
 protected:
 
   /** Create default parameters */
diff --git a/matlab/+gtsam/VisualISAMInitialize.m b/matlab/+gtsam/VisualISAMInitialize.m
@@ -7,7 +7,7 @@
 %% Initialize iSAM
 params = gtsam.ISAM2Params;
 if options.alwaysRelinearize
-    params.setRelinearizeSkip(1);
+    params.relinearizeSkip = 1;
 end
 isam = ISAM2(params);
 
diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt
@@ -68,6 +68,8 @@ set(interface_files
     ${GTSAM_SOURCE_DIR}/gtsam/gtsam.i
     ${GTSAM_SOURCE_DIR}/gtsam/base/base.i
     ${GTSAM_SOURCE_DIR}/gtsam/basis/basis.i
+    ${PROJECT_SOURCE_DIR}/gtsam/inference/inference.i
+    ${PROJECT_SOURCE_DIR}/gtsam/discrete/discrete.i
     ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i
     ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i
     ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m
@@ -52,7 +52,7 @@
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
-isamParams.setRelinearizeSkip(10);
+isamParams.relinearizeSkip = 10;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;
diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison.m b/matlab/unstable_examples/+imuSimulator/IMUComparison.m
@@ -46,7 +46,7 @@
 
 %% Solver object
 isamParams = ISAM2Params;
-isamParams.setRelinearizeSkip(1);
+isamParams.relinearizeSkip = 1;
 isam = gtsam.ISAM2(isamParams);
 
 initialValues = Values;
diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m
@@ -34,7 +34,7 @@
 
 %% Solver object
 isamParams = ISAM2Params;
-isamParams.setRelinearizeSkip(1);
+isamParams.relinearizeSkip = 1;
 isam = gtsam.ISAM2(isamParams);
 
 sigma_init_x = 1.0;
diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m
@@ -119,7 +119,7 @@
 % Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
-isamParams.setRelinearizeSkip(10);
+isamParams.relinearizeSkip = 10;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;
diff --git a/matlab/unstable_examples/IMUKittiExampleAdvanced.m b/matlab/unstable_examples/IMUKittiExampleAdvanced.m
@@ -82,7 +82,7 @@
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('QR');
-isamParams.setRelinearizeSkip(1);
+isamParams.relinearizeSkip = 1;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;
diff --git a/matlab/unstable_examples/IMUKittiExampleVO.m b/matlab/unstable_examples/IMUKittiExampleVO.m
@@ -58,7 +58,7 @@
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
-isamParams.setRelinearizeSkip(10);
+isamParams.relinearizeSkip = 10;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;
diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py
@@ -203,7 +203,7 @@ def optimize(gps_measurements: List[GpsMeasurement],
     # Set ISAM2 parameters and create ISAM2 solver object
     isam_params = gtsam.ISAM2Params()
     isam_params.setFactorization("CHOLESKY")
-    isam_params.setRelinearizeSkip(10)
+    isam_params.relinearizeSkip = 10
 
     isam = gtsam.ISAM2(isam_params)
 
diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py
@@ -111,7 +111,7 @@ def Pose2SLAM_ISAM2_example():
     # update calls are required to perform the relinearization.
     parameters = gtsam.ISAM2Params()
     parameters.setRelinearizeThreshold(0.1)
-    parameters.setRelinearizeSkip(1)
+    parameters.relinearizeSkip = 1
     isam = gtsam.ISAM2(parameters)
 
     # Create the ground truth odometry measurements of the robot during the trajectory.
diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py
@@ -140,7 +140,7 @@ def Pose3_ISAM2_example():
     # update calls are required to perform the relinearization.
     parameters = gtsam.ISAM2Params()
     parameters.setRelinearizeThreshold(0.1)
-    parameters.setRelinearizeSkip(1)
+    parameters.relinearizeSkip = 1
     isam = gtsam.ISAM2(parameters)
 
     # Create the ground truth poses of the robot trajectory.
diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py
@@ -81,7 +81,7 @@ def visual_ISAM2_example():
     # will approach the batch result.
     parameters = gtsam.ISAM2Params()
     parameters.setRelinearizeThreshold(0.01)
-    parameters.setRelinearizeSkip(1)
+    parameters.relinearizeSkip = 1
     isam = gtsam.ISAM2(parameters)
 
     # Create a Factor Graph and Values to hold the new data
diff --git a/python/gtsam/preamble/inference.h b/python/gtsam/preamble/inference.h
@@ -10,6 +10,3 @@
  * Without this they will be automatically converted to a Python object, and all
  * mutations on Python side will not be reflected on C++.
  */
-
-#include <pybind11/stl.h>
-
diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py
diff --git a/python/gtsam/utils/visual_data_generator.py b/python/gtsam/utils/visual_data_generator.py
diff --git a/python/gtsam/utils/visual_isam.py b/python/gtsam/utils/visual_isam.py
diff --git a/wrap/gtwrap/matlab_wrapper/mixins.py b/wrap/gtwrap/matlab_wrapper/mixins.py
diff --git a/wrap/gtwrap/matlab_wrapper/wrapper.py b/wrap/gtwrap/matlab_wrapper/wrapper.py
diff --git a/wrap/matlab.h b/wrap/matlab.h
diff --git a/wrap/tests/expected/matlab/ForwardKinematicsFactor.m b/wrap/tests/expected/matlab/ForwardKinematicsFactor.m
diff --git a/wrap/tests/expected/matlab/functions_wrapper.cpp b/wrap/tests/expected/matlab/functions_wrapper.cpp
diff --git a/wrap/tests/expected/matlab/geometry_wrapper.cpp b/wrap/tests/expected/matlab/geometry_wrapper.cpp
diff --git a/wrap/tests/expected/matlab/inheritance_wrapper.cpp b/wrap/tests/expected/matlab/inheritance_wrapper.cpp
diff --git a/wrap/tests/expected/python/inheritance_pybind.cpp b/wrap/tests/expected/python/inheritance_pybind.cpp
diff --git a/wrap/tests/fixtures/inheritance.i b/wrap/tests/fixtures/inheritance.i
