Merge pull request #941 from borglab/wrap/isam2

Wrap additional ISAM2 methods

gtsam/linear/NoiseModel.h

@@ -177,17 +177,16 @@ namespace gtsam {
         return *sqrt_information_;
       }
 
-    protected:
-
-      /** protected constructor takes square root information matrix */
-      Gaussian(size_t dim = 1, const boost::optional<Matrix>& sqrt_information = boost::none) :
-        Base(dim), sqrt_information_(sqrt_information) {
-      }
 
     public:
 
       typedef boost::shared_ptr<Gaussian> shared_ptr;
 
+      /** constructor takes square root information matrix */
+      Gaussian(size_t dim = 1,
+               const boost::optional<Matrix>& sqrt_information = boost::none)
+          : Base(dim), sqrt_information_(sqrt_information) {}
+
       ~Gaussian() override {}
 
       /**
@@ -290,13 +289,13 @@ namespace gtsam {
       Vector sigmas_, invsigmas_, precisions_;
 
     protected:
-      /** protected constructor - no initializations */
-      Diagonal();
 
       /** constructor to allow for disabling initialization of invsigmas */
       Diagonal(const Vector& sigmas);
 
     public:
+      /** constructor - no initializations, for serialization */
+      Diagonal();
 
       typedef boost::shared_ptr<Diagonal> shared_ptr;
 
@@ -387,14 +386,6 @@ namespace gtsam {
       // Sigmas are contained in the base class
       Vector mu_; ///< Penalty function weight - needs to be large enough to dominate soft constraints
 
-      /**
-       * protected constructor takes sigmas.
-       * prevents any inf values
-       * from appearing in invsigmas or precisions.
-       * mu set to large default value (1000.0)
-       */
-      Constrained(const Vector& sigmas = Z_1x1);
-
       /**
        * Constructor that prevents any inf values
        * from appearing in invsigmas or precisions.
@@ -406,6 +397,14 @@ namespace gtsam {
 
       typedef boost::shared_ptr<Constrained> shared_ptr;
 
+      /**
+       * protected constructor takes sigmas.
+       * prevents any inf values
+       * from appearing in invsigmas or precisions.
+       * mu set to large default value (1000.0)
+       */
+      Constrained(const Vector& sigmas = Z_1x1);
+
       ~Constrained() override {}
 
       /// true if a constrained noise mode, saves slow/clumsy dynamic casting
@@ -531,11 +530,11 @@ namespace gtsam {
       Isotropic(size_t dim, double sigma) :
         Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
 
+    public:
+
       /* dummy constructor to allow for serialization */
       Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
 
-    public:
-
       ~Isotropic() override {}
 
       typedef boost::shared_ptr<Isotropic> shared_ptr;
@@ -592,14 +591,13 @@ namespace gtsam {
      * Unit: i.i.d. unit-variance noise on all m dimensions.
      */
     class GTSAM_EXPORT Unit : public Isotropic {
-    protected:
-
-      Unit(size_t dim=1): Isotropic(dim,1.0) {}
-
     public:
 
       typedef boost::shared_ptr<Unit> shared_ptr;
 
+      /** constructor for serialization */
+      Unit(size_t dim=1): Isotropic(dim,1.0) {}
+
       ~Unit() override {}
 
       /**

gtsam/linear/linear.i

@@ -34,6 +34,9 @@ virtual class Gaussian : gtsam::noiseModel::Base {
 
   // enabling serialization functionality
   void serializable() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 virtual class Diagonal : gtsam::noiseModel::Gaussian {
@@ -49,6 +52,9 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
 
   // enabling serialization functionality
   void serializable() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 virtual class Constrained : gtsam::noiseModel::Diagonal {
@@ -66,6 +72,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal {
 
     // enabling serialization functionality
     void serializable() const;
+
+    // enable pickling in python
+    void pickle() const;
 };
 
 virtual class Isotropic : gtsam::noiseModel::Diagonal {
@@ -78,6 +87,9 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal {
 
   // enabling serialization functionality
   void serializable() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 virtual class Unit : gtsam::noiseModel::Isotropic {
@@ -85,6 +97,9 @@ virtual class Unit : gtsam::noiseModel::Isotropic {
 
   // enabling serialization functionality
   void serializable() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 namespace mEstimator {

gtsam/navigation/navigation.i

@@ -41,6 +41,12 @@ class ConstantBias {
   Vector gyroscope() const;
   Vector correctAccelerometer(Vector measurement) const;
   Vector correctGyroscope(Vector measurement) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 }///\namespace imuBias
@@ -64,6 +70,12 @@ class NavState {
 
   gtsam::NavState retract(const Vector& x) const;
   Vector localCoordinates(const gtsam::NavState& g) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 #include <gtsam/navigation/PreintegratedRotation.h>
@@ -106,6 +118,12 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
   Matrix getAccelerometerCovariance() const;
   Matrix getIntegrationCovariance()   const;
   bool   getUse2ndOrderCoriolis()     const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 #include <gtsam/navigation/ImuFactor.h>
@@ -135,6 +153,12 @@ class PreintegratedImuMeasurements {
   Vector biasHatVector() const;
   gtsam::NavState predict(const gtsam::NavState& state_i,
       const gtsam::imuBias::ConstantBias& bias) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enable pickling in python
+  void pickle() const;
 };
 
 virtual class ImuFactor: gtsam::NonlinearFactor {

gtsam/nonlinear/LinearContainerFactor.h

@@ -29,9 +29,6 @@ protected:
   GaussianFactor::shared_ptr factor_;
   boost::optional<Values> linearizationPoint_;
 
-  /** Default constructor - necessary for serialization */
-  LinearContainerFactor() {}
-
   /** direct copy constructor */
   GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional<Values>& linearizationPoint);
 
@@ -43,6 +40,9 @@ public:
 
   typedef boost::shared_ptr<This> shared_ptr;
 
+  /** Default constructor - necessary for serialization */
+  LinearContainerFactor() {}
+
   /** Primary constructor: store a linear factor with optional linearization point */
   GTSAM_EXPORT LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values());
 

gtsam/nonlinear/nonlinear.i

@@ -738,7 +738,12 @@ class ISAM2 {
                             const gtsam::KeyList& extraReelimKeys,
                             bool force_relinearize);
 
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
+                            const gtsam::Values& newTheta,
+                            const gtsam::ISAM2UpdateParams& updateParams);
+
   gtsam::Values getLinearizationPoint() const;
+  bool valueExists(gtsam::Key key) const;
   gtsam::Values calculateEstimate() const;
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
@@ -748,12 +753,17 @@ class ISAM2 {
                      gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
                      gtsam::PinholeCamera<gtsam::Cal3Unified>, Vector, Matrix}>
   VALUE calculateEstimate(size_t key) const;
-  gtsam::Values calculateBestEstimate() const;
   Matrix marginalCovariance(size_t key) const;
+  gtsam::Values calculateBestEstimate() const;
   gtsam::VectorValues getDelta() const;
+  double error(const gtsam::VectorValues& x) const;
   gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
   gtsam::VariableIndex getVariableIndex() const;
+  const gtsam::KeySet& getFixedVariables() const;
   gtsam::ISAM2Params params() const;
+
+  void printStats() const;
+  gtsam::VectorValues gradientAtZero() const;
 };
 
 #include <gtsam/nonlinear/NonlinearISAM.h>