Merge pull request #1099 from borglab/release/4.2a5

CMakeLists.txt

@@ -11,7 +11,7 @@ endif()
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 2)
 set (GTSAM_VERSION_PATCH 0)
-set (GTSAM_PRERELEASE_VERSION "a4")
+set (GTSAM_PRERELEASE_VERSION "a5")
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 
 if (${GTSAM_VERSION_PATCH} EQUAL 0)

examples/SFMExampleExpressions_bal.cpp

@@ -28,7 +28,8 @@
 // Header order is close to far
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/dataset.h>  // for loading BAL datasets !
+#include <gtsam/sfm/SfmData.h>  // for loading BAL datasets !
+#include <gtsam/slam/dataset.h>
 #include <vector>
 
 using namespace std;
@@ -46,10 +47,9 @@ int main(int argc, char* argv[]) {
   if (argc > 1) filename = string(argv[1]);
 
   // Load the SfM data from file
-  SfmData mydata;
+  SfmData mydata = SfmData::FromBalFile(filename);
-  readBAL(filename, mydata);
   cout << boost::format("read %1% tracks on %2% cameras\n") %
-              mydata.number_tracks() % mydata.number_cameras();
+              mydata.numberTracks() % mydata.numberCameras();
 
   // Create a factor graph
   ExpressionFactorGraph graph;

examples/SFMExample_bal.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    SFMExample.cpp
+ * @file    SFMExample_bal.cpp
  * @brief   Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
  * @author  Frank Dellaert
  */
@@ -20,7 +20,8 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
-#include <gtsam/slam/dataset.h> // for loading BAL datasets !
+#include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
+#include <gtsam/slam/dataset.h>
 #include <vector>
 
 using namespace std;
@@ -41,9 +42,8 @@ int main (int argc, char* argv[]) {
   if (argc>1) filename = string(argv[1]);
 
   // Load the SfM data from file
-  SfmData mydata;
+  SfmData mydata = SfmData::FromBalFile(filename);
-  readBAL(filename, mydata);
+  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras();
-  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
 
   // Create a factor graph
   NonlinearFactorGraph graph;

examples/SFMExample_bal_COLAMD_METIS.cpp

@@ -22,7 +22,8 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
-#include <gtsam/slam/dataset.h>  // for loading BAL datasets !
+#include <gtsam/sfm/SfmData.h>  // for loading BAL datasets !
+#include <gtsam/slam/dataset.h>
 
 #include <gtsam/base/timing.h>
 
@@ -45,10 +46,9 @@ int main(int argc, char* argv[]) {
   if (argc > 1) filename = string(argv[1]);
 
   // Load the SfM data from file
-  SfmData mydata;
+  SfmData mydata = SfmData::FromBalFile(filename);
-  readBAL(filename, mydata);
   cout << boost::format("read %1% tracks on %2% cameras\n") %
-              mydata.number_tracks() % mydata.number_cameras();
+              mydata.numberTracks() % mydata.numberCameras();
 
   // Create a factor graph
   NonlinearFactorGraph graph;
@@ -131,7 +131,7 @@ int main(int argc, char* argv[]) {
 
     cout << "Time comparison by solving " << filename << " results:" << endl;
     cout << boost::format("%1% point tracks and %2% cameras\n") %
-                mydata.number_tracks() % mydata.number_cameras()
+                mydata.numberTracks() % mydata.numberCameras()
          << endl;
 
     tictoc_print_();

gtsam/base/FastSet.h

@@ -18,6 +18,9 @@
 
 #pragma once
 
+#if BOOST_VERSION >= 107400
+#include <boost/serialization/library_version_type.hpp>
+#endif
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/set.hpp>
 #include <gtsam/base/FastDefaultAllocator.h>

gtsam/base/serialization.h

@@ -25,6 +25,7 @@
 #include <string>
 
 // includes for standard serialization types
+#include <boost/serialization/version.hpp>
 #include <boost/serialization/optional.hpp>
 #include <boost/serialization/shared_ptr.hpp>
 #include <boost/serialization/vector.hpp>

gtsam/discrete/DiscreteConditional.cpp

@@ -225,13 +225,13 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
 
 /* ****************************************************************************/
 DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
-    size_t parent_value) const {
+    size_t frontal) const {
   if (nrFrontals() != 1)
     throw std::invalid_argument(
         "Single value likelihood can only be invoked on single-variable "
         "conditional");
   DiscreteValues values;
-  values.emplace(keys_[0], parent_value);
+  values.emplace(keys_[0], frontal);
   return likelihood(values);
 }
 

gtsam/discrete/DiscreteConditional.h

@@ -177,7 +177,7 @@ class GTSAM_EXPORT DiscreteConditional
       const DiscreteValues& frontalValues) const;
 
   /** Single variable version of likelihood. */
-  DecisionTreeFactor::shared_ptr likelihood(size_t parent_value) const;
+  DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const;
 
   /**
    * sample

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,13 +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::DiscreteBayesNet eliminateSequential(const gtsam::Ordering& ordering);
+  gtsam::DiscreteBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type);
-  std::pair<gtsam::DiscreteBayesNet, gtsam::DiscreteFactorGraph>
+  gtsam::DiscreteBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
+  pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*>
       eliminatePartialSequential(const gtsam::Ordering& ordering);
-  gtsam::DiscreteBayesTree eliminateMultifrontal();
+
-  gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering);
+  gtsam::DiscreteBayesTree* eliminateMultifrontal();
-  std::pair<gtsam::DiscreteBayesTree, gtsam::DiscreteFactorGraph>
+  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(

gtsam/discrete/tests/testDiscreteBayesNet.cpp

@@ -150,7 +150,6 @@ TEST(DiscreteBayesNet, Dot) {
   fragment.add((Either | Tuberculosis, LungCancer) = "F T T T");
 
   string actual = fragment.dot();
-  cout << actual << endl;
   EXPECT(actual ==
          "digraph {\n"
          "  size=\"5,5\";\n"

gtsam/discrete/tests/testDiscreteBayesTree.cpp

@@ -243,27 +243,27 @@ TEST(DiscreteBayesTree, Dot) {
   string actual = self.bayesTree->dot();
   EXPECT(actual ==
          "digraph G{\n"
-         "0[label=\"13,11,6,7\"];\n"
+         "0[label=\"13, 11, 6, 7\"];\n"
          "0->1\n"
-         "1[label=\"14 : 11,13\"];\n"
+         "1[label=\"14 : 11, 13\"];\n"
          "1->2\n"
-         "2[label=\"9,12 : 14\"];\n"
+         "2[label=\"9, 12 : 14\"];\n"
          "2->3\n"
-         "3[label=\"3 : 9,12\"];\n"
+         "3[label=\"3 : 9, 12\"];\n"
          "2->4\n"
-         "4[label=\"2 : 9,12\"];\n"
+         "4[label=\"2 : 9, 12\"];\n"
          "2->5\n"
-         "5[label=\"8 : 12,14\"];\n"
+         "5[label=\"8 : 12, 14\"];\n"
          "5->6\n"
-         "6[label=\"1 : 8,12\"];\n"
+         "6[label=\"1 : 8, 12\"];\n"
          "5->7\n"
-         "7[label=\"0 : 8,12\"];\n"
+         "7[label=\"0 : 8, 12\"];\n"
          "1->8\n"
-         "8[label=\"10 : 13,14\"];\n"
+         "8[label=\"10 : 13, 14\"];\n"
          "8->9\n"
-         "9[label=\"5 : 10,13\"];\n"
+         "9[label=\"5 : 10, 13\"];\n"
          "8->10\n"
-         "10[label=\"4 : 10,13\"];\n"
+         "10[label=\"4 : 10, 13\"];\n"
          "}");
 }
 

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
   /// @{
 

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
   /// @{
 

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
   /// @{
 

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
   /// @{
 

gtsam/geometry/Rot3.h

@@ -49,16 +49,14 @@
 
 namespace gtsam {
 
-  /**
+/**
-   * @brief A 3D rotation represented as a rotation matrix if the preprocessor
+ * @brief Rot3 is a 3D rotation represented as a rotation matrix if the
-   * symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it
+ * preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion
-   * is defined.
+ * if it is defined.
-   * @addtogroup geometry
+ * @addtogroup geometry
-   * \nosubgrouping
+ */
-   */
+class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
-  class GTSAM_EXPORT Rot3 : public LieGroup<Rot3,3> {
+ private:
-
-  private:
 
 #ifdef GTSAM_USE_QUATERNIONS
     /** Internal Eigen Quaternion */
@@ -67,8 +65,7 @@ namespace gtsam {
     SO3 rot_;
 #endif
 
-  public:
+ public:
-
     /// @name Constructors and named constructors
     /// @{
 
@@ -83,7 +80,7 @@ namespace gtsam {
      */
     Rot3(const Point3& col1, const Point3& col2, const Point3& col3);
 
-    /** constructor from a rotation matrix, as doubles in *row-major* order !!! */
+    /// Construct from a rotation matrix, as doubles in *row-major* order !!!
     Rot3(double R11, double R12, double R13,
         double R21, double R22, double R23,
         double R31, double R32, double R33);
@@ -567,6 +564,9 @@ namespace gtsam {
 #endif
   };
 
+  /// std::vector of Rot3s, mainly for wrapper
+  using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3> >;
+
   /**
    * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
    * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'
@@ -585,5 +585,6 @@ namespace gtsam {
 
   template<>
   struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
-}
+  
+}  // namespace gtsam
 

gtsam/inference/BayesTree-inst.h

@@ -95,7 +95,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template <class CLIQUE>
   void BayesTree<CLIQUE>::dot(std::ostream& s, sharedClique clique,
-                              const KeyFormatter& indexFormatter,
+                              const KeyFormatter& keyFormatter,
                               int parentnum) const {
     static int num = 0;
     bool first = true;
@@ -104,10 +104,10 @@ namespace gtsam {
     std::string parent = out.str();
     parent += "[label=\"";
 
-    for (Key index : clique->conditional_->frontals()) {
+    for (Key key : clique->conditional_->frontals()) {
-      if (!first) parent += ",";
+      if (!first) parent += ", ";
       first = false;
-      parent += indexFormatter(index);
+      parent += keyFormatter(key);
     }
 
     if (clique->parent()) {
@@ -116,10 +116,10 @@ namespace gtsam {
     }
 
     first = true;
-    for (Key sep : clique->conditional_->parents()) {
+    for (Key parentKey : clique->conditional_->parents()) {
-      if (!first) parent += ",";
+      if (!first) parent += ", ";
       first = false;
-      parent += indexFormatter(sep);
+      parent += keyFormatter(parentKey);
     }
     parent += "\"];\n";
     s << parent;
@@ -127,7 +127,7 @@ namespace gtsam {
 
     for (sharedClique c : clique->children) {
       num++;
-      dot(s, c, indexFormatter, parentnum);
+      dot(s, c, keyFormatter, parentnum);
     }
   }
 

gtsam/inference/inference.i

@@ -4,6 +4,12 @@
 
 namespace gtsam {
 
+// Headers for overloaded methods below, break hierarchy :-/
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/symbolic/SymbolicFactorGraph.h>
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+
 #include <gtsam/inference/Key.h>
 
 // Default keyformatter
@@ -98,10 +104,41 @@ class Ordering {
   Ordering();
   Ordering(const gtsam::Ordering& other);
 
-  template <FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
+  template <
-                            gtsam::GaussianFactorGraph}>
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
   static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
 
+  template <
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+  static gtsam::Ordering ColamdConstrainedLast(
+      const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast,
+      bool forceOrder = false);
+
+  template <
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+  static gtsam::Ordering ColamdConstrainedFirst(
+      const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst,
+      bool forceOrder = false);
+
+  template <
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+  static gtsam::Ordering Natural(const FACTOR_GRAPH& graph);
+
+  template <
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+  static gtsam::Ordering Metis(const FACTOR_GRAPH& graph);
+
+  template <
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+  static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType,
+                                const FACTOR_GRAPH& graph);
+
   // Testable
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                 gtsam::DefaultKeyFormatter) const;
@@ -135,12 +172,6 @@ class DotWriter {
 };
 
 #include <gtsam/inference/VariableIndex.h>
-
-// Headers for overloaded methods below, break hierarchy :-/
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/symbolic/SymbolicFactorGraph.h>
-
 class VariableIndex {
   // Standard Constructors and Named Constructors
   VariableIndex();

gtsam/linear/GaussianBayesNet.cpp

@@ -26,6 +26,9 @@
 using namespace std;
 using namespace gtsam;
 
+// In Wrappers we have no access to this so have a default ready
+static std::mt19937_64 kRandomNumberGenerator(42);
+
 namespace gtsam {
 
   // Instantiate base class
@@ -37,28 +40,50 @@ namespace gtsam {
     return Base::equals(bn, tol);
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
-  VectorValues GaussianBayesNet::optimize() const
+  VectorValues GaussianBayesNet::optimize() const {
-  {
+    VectorValues solution;  // no missing variables -> create an empty vector
-    VectorValues soln; // no missing variables -> just create an empty vector
+    return optimize(solution);
-    return optimize(soln);
   }
 
-  /* ************************************************************************* */
+  VectorValues GaussianBayesNet::optimize(VectorValues solution) const {
-  VectorValues GaussianBayesNet::optimize(
-      const VectorValues& solutionForMissing) const {
-    VectorValues soln(solutionForMissing); // possibly empty
     // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
-    /** solve each node in turn in topological sort order (parents first)*/
+    // solve each node in reverse topological sort order (parents first)
-    for (auto cg: boost::adaptors::reverse(*this)) {
+    for (auto cg : boost::adaptors::reverse(*this)) {
       // i^th part of R*x=y, x=inv(R)*y
-      // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
+      // (Rii*xi + R_i*x(i+1:))./si = yi =>
-      soln.insert(cg->solve(soln));
+      // xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
+      solution.insert(cg->solve(solution));
     }
-    return soln;
+    return solution;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
+  VectorValues GaussianBayesNet::sample(std::mt19937_64* rng) const {
+    VectorValues result;  // no missing variables -> create an empty vector
+    return sample(result, rng);
+  }
+
+  VectorValues GaussianBayesNet::sample(VectorValues result,
+                                        std::mt19937_64* rng) const {
+    // sample each node in reverse topological sort order (parents first)
+    for (auto cg : boost::adaptors::reverse(*this)) {
+      const VectorValues sampled = cg->sample(result, rng);
+      result.insert(sampled);
+    }
+    return result;
+  }
+
+  /* ************************************************************************ */
+  VectorValues GaussianBayesNet::sample() const {
+    return sample(&kRandomNumberGenerator);
+  }
+
+  VectorValues GaussianBayesNet::sample(VectorValues given) const {
+    return sample(given, &kRandomNumberGenerator);
+  }
+
+  /* ************************************************************************ */
   VectorValues GaussianBayesNet::optimizeGradientSearch() const
   {
     gttic(GaussianBayesTree_optimizeGradientSearch);

gtsam/linear/GaussianBayesNet.h

@@ -88,11 +88,35 @@ namespace gtsam {
     /// @name Standard Interface
     /// @{
 
-    /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by back-substitution
+    /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by
+    /// back-substitution
     VectorValues optimize() const;
 
-    /// Version of optimize for incomplete BayesNet, needs solution for missing variables
+    /// Version of optimize for incomplete BayesNet, given missing variables
-    VectorValues optimize(const VectorValues& solutionForMissing) const;
+    VectorValues optimize(const VectorValues given) const;
+
+    /**
+     * Sample using ancestral sampling
+     * Example:
+     *   std::mt19937_64 rng(42);
+     *   auto sample = gbn.sample(&rng);
+     */
+    VectorValues sample(std::mt19937_64* rng) const;
+
+    /**
+     * Sample from an incomplete BayesNet, given missing variables
+     * Example:
+     *   std::mt19937_64 rng(42);
+     *   VectorValues given = ...;
+     *   auto sample = gbn.sample(given, &rng);
+     */
+    VectorValues sample(VectorValues given, std::mt19937_64* rng) const;
+
+    /// Sample using ancestral sampling, use default rng
+    VectorValues sample() const;
+
+    /// Sample from an incomplete BayesNet, use default rng
+    VectorValues sample(VectorValues given) const;
 
     /**
      * Return ordering corresponding to a topological sort.

gtsam/linear/GaussianConditional.cpp

@@ -18,6 +18,7 @@
 #include <gtsam/linear/linearExceptions.h>
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/VectorValues.h>
+#include <gtsam/linear/Sampler.h>
 
 #include <boost/format.hpp>
 #ifdef __GNUC__
@@ -34,6 +35,9 @@
 #include <list>
 #include <string>
 
+// In Wrappers we have no access to this so have a default ready
+static std::mt19937_64 kRandomNumberGenerator(42);
+
 using namespace std;
 
 namespace gtsam {
@@ -43,19 +47,47 @@ namespace gtsam {
     Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) :
   BaseFactor(key, R, d, sigmas), BaseConditional(1) {}
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
-  GaussianConditional::GaussianConditional(
+  GaussianConditional::GaussianConditional(Key key, const Vector& d,
-    Key key, const Vector& d, const Matrix& R,
+                                           const Matrix& R, Key parent1,
-    Key name1, const Matrix& S, const SharedDiagonal& sigmas) :
+                                           const Matrix& S,
-  BaseFactor(key, R, name1, S, d, sigmas), BaseConditional(1) {}
+                                           const SharedDiagonal& sigmas)
+      : BaseFactor(key, R, parent1, S, d, sigmas), BaseConditional(1) {}
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
-  GaussianConditional::GaussianConditional(
+  GaussianConditional::GaussianConditional(Key key, const Vector& d,
-    Key key, const Vector& d, const Matrix& R,
+                                           const Matrix& R, Key parent1,
-    Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) :
+                                           const Matrix& S, Key parent2,
-  BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {}
+                                           const Matrix& T,
+                                           const SharedDiagonal& sigmas)
+      : BaseFactor(key, R, parent1, S, parent2, T, d, sigmas),
+        BaseConditional(1) {}
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
+  GaussianConditional GaussianConditional::FromMeanAndStddev(
+      Key key, const Matrix& A, Key parent, const Vector& b, double sigma) {
+    // |Rx + Sy - d| = |x-(Ay + b)|/sigma
+    const Matrix R = Matrix::Identity(b.size(), b.size());
+    const Matrix S = -A;
+    const Vector d = b;
+    return GaussianConditional(key, d, R, parent, S,
+                               noiseModel::Isotropic::Sigma(b.size(), sigma));
+  }
+
+  /* ************************************************************************ */
+  GaussianConditional GaussianConditional::FromMeanAndStddev(
+      Key key, const Matrix& A1, Key parent1, const Matrix& A2, Key parent2,
+      const Vector& b, double sigma) {
+    // |Rx + Sy + Tz - d| = |x-(A1 y + A2 z + b)|/sigma
+    const Matrix R = Matrix::Identity(b.size(), b.size());
+    const Matrix S = -A1;
+    const Matrix T = -A2;
+    const Vector d = b;
+    return GaussianConditional(key, d, R, parent1, S, parent2, T,
+                               noiseModel::Isotropic::Sigma(b.size(), sigma));
+  }
+
+  /* ************************************************************************ */
   void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
     cout << s << "  Conditional density ";
     for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
@@ -192,7 +224,88 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
+  JacobianFactor::shared_ptr GaussianConditional::likelihood(
+      const VectorValues& frontalValues) const {
+    // Error is |Rx - (d - Sy - Tz - ...)|^2
+    // so when we instantiate x (which has to be completely known) we beget:
+    // |Sy + Tz + ... - (d - Rx)|^2
+    // The noise model just transfers over!
+
+    // Get frontalValues as vector
+    const Vector x =
+        frontalValues.vector(KeyVector(beginFrontals(), endFrontals()));
+
+    // Copy the augmented Jacobian matrix:
+    auto newAb = Ab_;
+
+    // Restrict view to parent blocks
+    newAb.firstBlock() += nrFrontals_;
+
+    // Update right-hand-side (last column)
+    auto last = newAb.matrix().cols() - 1;
+    const auto RR = R().triangularView<Eigen::Upper>();
+    newAb.matrix().col(last) -= RR * x;
+
+    // The keys now do not include the frontal keys:
+    KeyVector newKeys;
+    newKeys.reserve(nrParents());
+    for (auto&& key : parents()) newKeys.push_back(key);
+
+    // Hopefully second newAb copy below is optimized out...
+    return boost::make_shared<JacobianFactor>(newKeys, newAb, model_);
+  }
+
+  /* **************************************************************************/
+  JacobianFactor::shared_ptr GaussianConditional::likelihood(
+      const Vector& frontal) const {
+    if (nrFrontals() != 1)
+      throw std::invalid_argument(
+          "GaussianConditional Single value likelihood can only be invoked on "
+          "single-variable conditional");
+    VectorValues values;
+    values.insert(keys_[0], frontal);
+    return likelihood(values);
+  }
+
+  /* ************************************************************************ */
+  VectorValues GaussianConditional::sample(const VectorValues& parentsValues,
+                                           std::mt19937_64* rng) const {
+    if (nrFrontals() != 1) {
+      throw std::invalid_argument(
+          "GaussianConditional::sample can only be called on single variable "
+          "conditionals");
+    }
+    if (!model_) {
+      throw std::invalid_argument(
+          "GaussianConditional::sample can only be called if a diagonal noise "
+          "model was specified at construction.");
+    }
+    VectorValues solution = solve(parentsValues);
+    Key key = firstFrontalKey();
+    const Vector& sigmas = model_->sigmas();
+    solution[key] += Sampler::sampleDiagonal(sigmas, rng);
+    return solution;
+  }
+
+  VectorValues GaussianConditional::sample(std::mt19937_64* rng) const {
+    if (nrParents() != 0)
+      throw std::invalid_argument(
+          "sample() can only be invoked on no-parent prior");
+    VectorValues values;
+    return sample(values);
+  }
+
+  /* ************************************************************************ */
+  VectorValues GaussianConditional::sample() const {
+    return sample(&kRandomNumberGenerator);
+  }
+
+  VectorValues GaussianConditional::sample(const VectorValues& given) const {
+    return sample(given, &kRandomNumberGenerator);
+  }
+
+  /* ************************************************************************ */
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
   void GTSAM_DEPRECATED
   GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {

gtsam/linear/GaussianConditional.h

@@ -26,12 +26,15 @@
 #include <gtsam/inference/Conditional.h>
 #include <gtsam/linear/VectorValues.h>
 
+#include <random> // for std::mt19937_64 
+
 namespace gtsam {
 
   /**
-  * A conditional Gaussian functions as the node in a Bayes network
+  * A GaussianConditional functions as the node in a Bayes network.
   * It has a set of parents y,z, etc. and implements a probability density on x.
   * The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$
+  * @addtogroup linear
   */
   class GTSAM_EXPORT GaussianConditional :
     public JacobianFactor,
@@ -43,6 +46,9 @@ namespace gtsam {
     typedef JacobianFactor BaseFactor; ///< Typedef to our factor base class
     typedef Conditional<BaseFactor, This> BaseConditional; ///< Typedef to our conditional base class
 
+    /// @name Constructors
+    /// @{
+
     /** default constructor needed for serialization */
     GaussianConditional() {}
 
@@ -51,13 +57,14 @@ namespace gtsam {
       const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** constructor with only one parent |Rx+Sy-d| */
-    GaussianConditional(Key key, const Vector& d, const Matrix& R,
+    GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
-      Key name1, const Matrix& S, const SharedDiagonal& sigmas = SharedDiagonal());
+                        const Matrix& S,
+                        const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** constructor with two parents |Rx+Sy+Tz-d| */
-    GaussianConditional(Key key, const Vector& d, const Matrix& R,
+    GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
-      Key name1, const Matrix& S, Key name2, const Matrix& T,
+                        const Matrix& S, Key parent2, const Matrix& T,
-      const SharedDiagonal& sigmas = SharedDiagonal());
+                        const SharedDiagonal& sigmas = SharedDiagonal());
 
     /** Constructor with arbitrary number of frontals and parents.
     *   @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
@@ -76,6 +83,17 @@ namespace gtsam {
       const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix,
       const SharedDiagonal& sigmas = SharedDiagonal());
 
+    /// Construct from mean A1 p1 + b and standard deviation.
+    static GaussianConditional FromMeanAndStddev(Key key, const Matrix& A,
+                                                 Key parent, const Vector& b,
+                                                 double sigma);
+
+    /// Construct from mean A1 p1 + A2 p2 + b and standard deviation.
+    static GaussianConditional FromMeanAndStddev(Key key,  //
+                                                 const Matrix& A1, Key parent1,
+                                                 const Matrix& A2, Key parent2,
+                                                 const Vector& b, double sigma);
+
     /** Combine several GaussianConditional into a single dense GC.  The conditionals enumerated by
     *   \c first and \c last must be in increasing order, meaning that the parents of any
     *   conditional may not include a conditional coming before it.
@@ -86,6 +104,10 @@ namespace gtsam {
     template<typename ITERATOR>
     static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional);
 
+    /// @}
+    /// @name Testable
+    /// @{
+
     /** print */
     void print(const std::string& = "GaussianConditional",
       const KeyFormatter& formatter = DefaultKeyFormatter) const override;
@@ -93,6 +115,10 @@ namespace gtsam {
     /** equals function */
     bool equals(const GaussianFactor&cg, double tol = 1e-9) const override;
 
+    /// @}
+    /// @name Standard Interface
+    /// @{
+
     /** Return a view of the upper-triangular R block of the conditional */
     constABlock R() const { return Ab_.range(0, nrFrontals()); }
 
@@ -125,10 +151,46 @@ namespace gtsam {
     /** Performs transpose backsubstition in place on values */
     void solveTransposeInPlace(VectorValues& gy) const;
 
+    /** Convert to a likelihood factor by providing value before bar. */
+    JacobianFactor::shared_ptr likelihood(
+        const VectorValues& frontalValues) const;
+
+    /** Single variable version of likelihood. */
+    JacobianFactor::shared_ptr likelihood(const Vector& frontal) const;
+
+    /**
+     * Sample from conditional, zero parent version
+     * Example:
+     *   std::mt19937_64 rng(42);
+     *   auto sample = gbn.sample(&rng);
+     */
+    VectorValues sample(std::mt19937_64* rng) const;
+
+    /**
+     * Sample from conditional, given missing variables
+     * Example:
+     *   std::mt19937_64 rng(42);
+     *   VectorValues given = ...;
+     *   auto sample = gbn.sample(given, &rng);
+     */
+    VectorValues sample(const VectorValues& parentsValues,
+                        std::mt19937_64* rng) const;
+
+    /// Sample, use default rng
+    VectorValues sample() const;
+
+    /// Sample with given values, use default rng
+    VectorValues sample(const VectorValues& parentsValues) const;
+
+    /// @}
+
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
+    /// @name Deprecated
+    /// @{
     /** Scale the values in \c gy according to the sigmas for the frontal variables in this
      *  conditional. */
     void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const;
+    /// @}
 #endif
 
    private:

gtsam/linear/GaussianDensity.cpp

@@ -23,9 +23,12 @@ using namespace std;
 namespace gtsam {
 
   /* ************************************************************************* */
-  GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean, const double& sigma)
+  GaussianDensity GaussianDensity::FromMeanAndStddev(Key key,
-  {
+                                                     const Vector& mean,
-    return GaussianDensity(key, mean / sigma, Matrix::Identity(mean.size(), mean.size()) / sigma);
+                                                     double sigma) {
+    return GaussianDensity(key, mean,
+                           Matrix::Identity(mean.size(), mean.size()),
+                           noiseModel::Isotropic::Sigma(mean.size(), sigma));
   }
 
   /* ************************************************************************* */
@@ -35,8 +38,8 @@ namespace gtsam {
     for(const_iterator it = beginFrontals(); it != endFrontals(); ++it)
       cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
     cout << endl;
-    gtsam::print(Matrix(R()), "R: ");
+    gtsam::print(mean(), "mean: ");
-    gtsam::print(Vector(d()), "d: ");
+    gtsam::print(covariance(), "covariance: ");
     if(model_)
       model_->print("Noise model: ");
   }

gtsam/linear/GaussianDensity.h

@@ -24,11 +24,10 @@
 namespace gtsam {
 
   /**
-  * A Gaussian density.
+  * A GaussianDensity is a GaussianConditional without parents.
-  *
-  * It is implemented as a GaussianConditional without parents.
   * The negative log-probability is given by \f$ |Rx - d|^2 \f$
   * with \f$ \Lambda = \Sigma^{-1} = R^T R \f$ and \f$ \mu = R^{-1} d \f$
+  * @addtogroup linear
   */
   class GTSAM_EXPORT GaussianDensity : public GaussianConditional {
 
@@ -52,8 +51,9 @@ namespace gtsam {
     GaussianDensity(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& noiseModel = SharedDiagonal()) :
       GaussianConditional(key, d, R, noiseModel) {}
 
-    /// Construct using a mean and variance
+    /// Construct using a mean and standard deviation
-    static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean, const double& sigma);
+    static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean,
+                                             double sigma);
 
     /// print
     void print(const std::string& = "GaussianDensity",

gtsam/linear/Sampler.cpp

@@ -22,14 +22,18 @@ namespace gtsam {
 /* ************************************************************************* */
 Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model,
                  uint_fast64_t seed)
-    : model_(model), generator_(seed) {}
+    : model_(model), generator_(seed) {
+  if (!model) {
+    throw std::invalid_argument("Sampler::Sampler needs a non-null model.");
+  }
+}
 
 /* ************************************************************************* */
 Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed)
     : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {}
 
 /* ************************************************************************* */
-Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
+Vector Sampler::sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng) {
   size_t d = sigmas.size();
   Vector result(d);
   for (size_t i = 0; i < d; i++) {
@@ -39,14 +43,18 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
     if (sigma == 0.0) {
       result(i) = 0.0;
     } else {
-      typedef std::normal_distribution<double> Normal;
+      std::normal_distribution<double> dist(0.0, sigma);
-      Normal dist(0.0, sigma);
+      result(i) = dist(*rng);
-      result(i) = dist(generator_);
     }
   }
   return result;
 }
 
+/* ************************************************************************* */
+Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
+  return sampleDiagonal(sigmas, &generator_);
+}
+
 /* ************************************************************************* */
 Vector Sampler::sample() const {
   assert(model_.get());

gtsam/linear/Sampler.h

@@ -63,15 +63,9 @@ class GTSAM_EXPORT Sampler {
   /// @name access functions
   /// @{
 
-  size_t dim() const {
+  size_t dim() const { return model_->dim(); }
-    assert(model_.get());
-    return model_->dim();
-  }
 
-  Vector sigmas() const {
+  Vector sigmas() const { return model_->sigmas(); }
-    assert(model_.get());
-    return model_->sigmas();
-  }
 
   const noiseModel::Diagonal::shared_ptr& model() const { return model_; }
 
@@ -82,6 +76,8 @@ class GTSAM_EXPORT Sampler {
   /// sample from distribution
   Vector sample() const;
 
+  /// sample with given random number generator
+  static Vector sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng);
   /// @}
 
  protected:

gtsam/linear/VectorValues.cpp

@@ -33,7 +33,7 @@ namespace gtsam {
   using boost::adaptors::map_values;
   using boost::accumulate;
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues::VectorValues(const VectorValues& first, const VectorValues& second)
   {
     // Merge using predicate for comparing first of pair
@@ -44,7 +44,7 @@ namespace gtsam {
       throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues::VectorValues(const Vector& x, const Dims& dims) {
     using Pair = pair<const Key, size_t>;
     size_t j = 0;
@@ -61,7 +61,7 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues::VectorValues(const Vector& x, const Scatter& scatter) {
     size_t j = 0;
     for (const SlotEntry& v : scatter) {
@@ -74,7 +74,7 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues VectorValues::Zero(const VectorValues& other)
   {
     VectorValues result;
@@ -87,7 +87,7 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues::iterator VectorValues::insert(const std::pair<Key, Vector>& key_value) {
     std::pair<iterator, bool> result = values_.insert(key_value);
     if(!result.second)
@@ -97,7 +97,7 @@ namespace gtsam {
     return result.first;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void VectorValues::update(const VectorValues& values)
   {
     iterator hint = begin();
@@ -115,7 +115,7 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void VectorValues::insert(const VectorValues& values)
   {
     size_t originalSize = size();
@@ -124,14 +124,14 @@ namespace gtsam {
       throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys.");
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void VectorValues::setZero()
   {
     for(Vector& v: values_ | map_values)
       v.setZero();
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   GTSAM_EXPORT ostream& operator<<(ostream& os, const VectorValues& v) {
     // Change print depending on whether we are using TBB
 #ifdef GTSAM_USE_TBB
@@ -150,7 +150,7 @@ namespace gtsam {
     return os;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void VectorValues::print(const string& str,
                            const KeyFormatter& formatter) const {
     cout << str << ": " << size() << " elements\n";
@@ -158,7 +158,7 @@ namespace gtsam {
     cout.flush();
 }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   bool VectorValues::equals(const VectorValues& x, double tol) const {
     if(this->size() != x.size())
       return false;
@@ -170,7 +170,7 @@ namespace gtsam {
     return true;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   Vector VectorValues::vector() const {
     // Count dimensions
     DenseIndex totalDim = 0;
@@ -187,7 +187,7 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   Vector VectorValues::vector(const Dims& keys) const
   {
     // Count dimensions
@@ -203,12 +203,12 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   void VectorValues::swap(VectorValues& other) {
     this->values_.swap(other.values_);
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   namespace internal
   {
     bool structureCompareOp(const boost::tuple<VectorValues::value_type,
@@ -219,14 +219,14 @@ namespace gtsam {
     }
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   bool VectorValues::hasSameStructure(const VectorValues other) const
   {
     return accumulate(combine(*this, other)
       | transformed(internal::structureCompareOp), true, logical_and<bool>());
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   double VectorValues::dot(const VectorValues& v) const
   {
     if(this->size() != v.size())
@@ -244,12 +244,12 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   double VectorValues::norm() const {
     return std::sqrt(this->squaredNorm());
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   double VectorValues::squaredNorm() const {
     double sumSquares = 0.0;
     using boost::adaptors::map_values;
@@ -258,7 +258,7 @@ namespace gtsam {
     return sumSquares;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues VectorValues::operator+(const VectorValues& c) const
   {
     if(this->size() != c.size())
@@ -278,13 +278,13 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues VectorValues::add(const VectorValues& c) const
   {
     return *this + c;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues& VectorValues::operator+=(const VectorValues& c)
   {
     if(this->size() != c.size())
@@ -301,13 +301,13 @@ namespace gtsam {
     return *this;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues& VectorValues::addInPlace(const VectorValues& c)
   {
     return *this += c;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues& VectorValues::addInPlace_(const VectorValues& c)
   {
     for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) {
@@ -320,7 +320,7 @@ namespace gtsam {
     return *this;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues VectorValues::operator-(const VectorValues& c) const
   {
     if(this->size() != c.size())
@@ -340,13 +340,13 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues VectorValues::subtract(const VectorValues& c) const
   {
     return *this - c;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues operator*(const double a, const VectorValues &v)
   {
     VectorValues result;
@@ -359,13 +359,13 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues VectorValues::scale(const double a) const
   {
     return a * *this;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues& VectorValues::operator*=(double alpha)
   {
     for(Vector& v: *this | map_values)
@@ -373,12 +373,43 @@ namespace gtsam {
     return *this;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues& VectorValues::scaleInPlace(double alpha)
   {
     return *this *= alpha;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
+  string VectorValues::html(const KeyFormatter& keyFormatter) const {
+    stringstream ss;
+
+    // Print out preamble.
+    ss << "<div>\n<table class='VectorValues'>\n  <thead>\n";
+
+    // Print out header row.
+    ss << "    <tr><th>Variable</th><th>value</th></tr>\n";
+
+    // Finish header and start body.
+    ss << "  </thead>\n  <tbody>\n";
+
+    // Print out all rows.
+#ifdef GTSAM_USE_TBB
+    // TBB uses un-ordered map, so inefficiently order them:
+    std::map<Key, Vector> ordered;
+    for (const auto& kv : *this) ordered.emplace(kv);
+    for (const auto& kv : ordered) {
+#else
+    for (const auto& kv : *this) {
+#endif
+      ss << "    <tr>";
+      ss << "<th>" << keyFormatter(kv.first) << "</th><td>"
+         << kv.second.transpose() << "</td>";
+      ss << "</tr>\n";
+    }
+    ss << "  </tbody>\n</table>\n</div>";
+    return ss.str();
+  }
+
+  /* ************************************************************************ */
 
 } // \namespace gtsam

gtsam/linear/VectorValues.h

@@ -34,7 +34,7 @@
 namespace gtsam {
 
   /**
-   * This class represents a collection of vector-valued variables associated
+   * VectorValues represents a collection of vector-valued variables associated
    * each with a unique integer index.  It is typically used to store the variables
    * of a GaussianFactorGraph.  Optimizing a GaussianFactorGraph or GaussianBayesNet
    * returns this class.
@@ -69,7 +69,7 @@ namespace gtsam {
    * which is a view on the underlying data structure.
    *
    * This class is additionally used in gradient descent and dog leg to store the gradient.
-   * \nosubgrouping
+   * @addtogroup linear
    */
   class GTSAM_EXPORT VectorValues {
    protected:
@@ -344,11 +344,16 @@ namespace gtsam {
 
     /// @}
 
-    /// @}
+    /// @name Wrapper support
-    /// @name Matlab syntactic sugar for linear algebra operations
     /// @{
 
-    //inline VectorValues scale(const double a, const VectorValues& c) const { return a * (*this); }
+    /**
+     * @brief Output as a html table.
+     *
+     * @param keyFormatter function that formats keys.
+     */
+    std::string html(
+        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
     /// @}
 

gtsam/linear/linear.i

@@ -255,6 +255,7 @@ class VectorValues {
 
   // enabling serialization functionality
   void serialize() const;
+  string html() const;
 };
 
 #include <gtsam/linear/GaussianFactor.h>
@@ -407,8 +408,10 @@ class GaussianFactorGraph {
 
   // Elimination and marginals
   gtsam::GaussianBayesNet* eliminateSequential();
+  gtsam::GaussianBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type);
   gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
   gtsam::GaussianBayesTree* eliminateMultifrontal();
+  gtsam::GaussianBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type);
   gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);
   pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
     const gtsam::Ordering& ordering);
@@ -466,15 +469,36 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
   GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
                       size_t name2, Matrix T);
 
-  // Standard Interface
+  // Named constructors
+  static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, 
+                                                      const Matrix& A,
+                                                      gtsam::Key parent,
+                                                      const Vector& b,
+                                                      double sigma);
+
+  static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
+                                                      const Matrix& A1,
+                                                      gtsam::Key parent1, 
+                                                      const Matrix& A2,
+                                                      gtsam::Key parent2, 
+                                                      const Vector& b,
+                                                      double sigma);
+  // Testable
   void print(string s = "GaussianConditional",
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::GaussianConditional& cg, double tol) const;
+  
+  // Standard Interface
   gtsam::Key firstFrontalKey() const;
+  gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
+  gtsam::JacobianFactor* likelihood(
+      const gtsam::VectorValues& frontalValues) const;
+  gtsam::JacobianFactor* likelihood(Vector frontal) const;
+  gtsam::VectorValues sample(const gtsam::VectorValues& parents) const;
+  gtsam::VectorValues sample() const;
   
   // Advanced Interface
-  gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
   gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents,
                                     const gtsam::VectorValues& rhs) const;
   void solveTransposeInPlace(gtsam::VectorValues& gy) const;
@@ -488,14 +512,21 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
 
 #include <gtsam/linear/GaussianDensity.h>
 virtual class GaussianDensity : gtsam::GaussianConditional {
-    //Constructors
+  // Constructors
-  GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
+  GaussianDensity(gtsam::Key key, Vector d, Matrix R,
+                  const gtsam::noiseModel::Diagonal* sigmas);
 
-  //Standard Interface
+  static gtsam::GaussianDensity FromMeanAndStddev(gtsam::Key key,
+                                                  const Vector& mean,
+                                                  double sigma);
+
+  // Testable
   void print(string s = "GaussianDensity",
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::GaussianDensity &cg, double tol) const;
+  bool equals(const gtsam::GaussianDensity& cg, double tol) const;
+
+  // Standard Interface
   Vector mean() const;
   Matrix covariance() const;
 };
@@ -512,6 +543,21 @@ virtual class GaussianBayesNet {
   bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
   size_t size() const;
 
+  // Standard interface
+  void push_back(gtsam::GaussianConditional* conditional);
+  void push_back(const gtsam::GaussianBayesNet& bayesNet);
+  gtsam::GaussianConditional* front() const;
+  gtsam::GaussianConditional* back() const;
+
+  gtsam::VectorValues optimize() const;
+  gtsam::VectorValues optimize(gtsam::VectorValues given) const;
+  gtsam::VectorValues optimizeGradientSearch() const;
+  
+  gtsam::VectorValues sample(gtsam::VectorValues given) const;
+  gtsam::VectorValues sample() const;
+  gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
+  gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const;
+
   // FactorGraph derived interface
   gtsam::GaussianConditional* at(size_t idx) const;
   gtsam::KeySet keys() const;
@@ -520,21 +566,12 @@ virtual class GaussianBayesNet {
 
   void saveGraph(const string& s) const;
 
-  gtsam::GaussianConditional* front() const;
+  std::pair<Matrix, Vector> matrix() const; 
-  gtsam::GaussianConditional* back() const;
-  void push_back(gtsam::GaussianConditional* conditional);
-  void push_back(const gtsam::GaussianBayesNet& bayesNet);
-
-  gtsam::VectorValues optimize() const;
-  gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
-  gtsam::VectorValues optimizeGradientSearch() const;
   gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
   gtsam::VectorValues gradientAtZero() const;
   double error(const gtsam::VectorValues& x) const;
   double determinant() const;
   double logDeterminant() const;
-  gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
-  gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const;
 
   string dot(
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
@@ -556,7 +593,12 @@ virtual class GaussianBayesTree {
   size_t size() const;
   bool empty() const;
   size_t numCachedSeparatorMarginals() const;
-  void saveGraph(string s) const;
+
+  string dot(const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+  void saveGraph(string s,
+                const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
 
   gtsam::VectorValues optimize() const;
   gtsam::VectorValues optimizeGradientSearch() const;

gtsam/linear/tests/testGaussianBayesNet.cpp

@@ -16,10 +16,12 @@
  */
 
 #include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianDensity.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/inference/Symbol.h>
 
 #include <CppUnitLite/TestHarness.h>
 #include <boost/tuple/tuple.hpp>
@@ -35,6 +37,7 @@ using namespace boost::assign;
 using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
+using symbol_shorthand::X;
 
 static const Key _x_ = 11, _y_ = 22, _z_ = 33;
 
@@ -138,6 +141,30 @@ TEST( GaussianBayesNet, optimize3 )
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST(GaussianBayesNet, sample) {
+  GaussianBayesNet gbn;
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  const Vector2 mean(20, 40), b(10, 10);
+  const double sigma = 0.01;
+
+  gbn.add(GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma));
+  gbn.add(GaussianDensity::FromMeanAndStddev(X(1), mean, sigma));
+
+  auto actual = gbn.sample();
+  EXPECT_LONGS_EQUAL(2, actual.size());
+  EXPECT(assert_equal(mean, actual[X(1)], 50 * sigma));
+  EXPECT(assert_equal(A1 * mean + b, actual[X(0)], 50 * sigma));
+
+  // Use a specific random generator
+  std::mt19937_64 rng(4242);
+  auto actual3 = gbn.sample(&rng);
+  EXPECT_LONGS_EQUAL(2, actual.size());
+  // regression is not repeatable across platforms/versions :-(
+  // EXPECT(assert_equal(Vector2(20.0129382, 40.0039798), actual[X(1)], 1e-5));
+  // EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5));
+}
+
 /* ************************************************************************* */
 TEST(GaussianBayesNet, ordering)
 {

gtsam/linear/tests/testGaussianConditional.cpp

@@ -20,9 +20,10 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/VerticalBlockMatrix.h>
-#include <gtsam/inference/Key.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/GaussianDensity.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 
 #include <boost/assign/std/list.hpp>
@@ -38,6 +39,7 @@
 using namespace gtsam;
 using namespace std;
 using namespace boost::assign;
+using symbol_shorthand::X;
 
 static const double tol = 1e-5;
 
@@ -316,5 +318,87 @@ TEST( GaussianConditional, isGaussianFactor ) {
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+// Test FromMeanAndStddev named constructors
+TEST(GaussianConditional, FromMeanAndStddev) {
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.).finished();
+  const Vector2 b(20, 40), x0(1, 2), x1(3, 4), x2(5, 6);
+  const double sigma = 3;
+
+  VectorValues values = map_list_of(X(0), x0)(X(1), x1)(X(2), x2);
+
+  auto conditional1 =
+      GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
+  Vector2 e1 = (x0 - (A1 * x1 + b)) / sigma;
+  double expected1 = 0.5 * e1.dot(e1);
+  EXPECT_DOUBLES_EQUAL(expected1, conditional1.error(values), 1e-9);
+
+  auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), A2,
+                                                             X(2), b, sigma);
+  Vector2 e2 = (x0 - (A1 * x1 + A2 * x2 + b)) / sigma;
+  double expected2 = 0.5 * e2.dot(e2);
+  EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9);
+}
+
+/* ************************************************************************* */
+// Test likelihood method (conversion to JacobianFactor)
+TEST(GaussianConditional, likelihood) {
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  const Vector2 b(20, 40), x0(1, 2);
+  const double sigma = 0.01;
+
+  // |x0 - A1 x1 - b|^2
+  auto conditional =
+      GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
+
+  VectorValues frontalValues;
+  frontalValues.insert(X(0), x0);
+  auto actual1 = conditional.likelihood(frontalValues);
+  CHECK(actual1);
+
+  // |(-A1) x1 - (b - x0)|^2
+  JacobianFactor expected(X(1), -A1, b - x0,
+                          noiseModel::Isotropic::Sigma(2, sigma));
+  EXPECT(assert_equal(expected, *actual1, tol));
+
+  // Check single vector version
+  auto actual2 = conditional.likelihood(x0);
+  CHECK(actual2);
+  EXPECT(assert_equal(expected, *actual2, tol));
+}
+
+/* ************************************************************************* */
+// Test sampling
+TEST(GaussianConditional, sample) {
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  const Vector2 b(20, 40), x1(3, 4);
+  const double sigma = 0.01;
+
+  auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma);
+  auto actual1 = density.sample();
+  EXPECT_LONGS_EQUAL(1, actual1.size());
+  EXPECT(assert_equal(b, actual1[X(0)], 50 * sigma));
+
+  VectorValues given;
+  given.insert(X(1), x1);
+
+  auto conditional =
+      GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
+  auto actual2 = conditional.sample(given);
+  EXPECT_LONGS_EQUAL(1, actual2.size());
+  EXPECT(assert_equal(A1 * x1 + b, actual2[X(0)], 50 * sigma));
+
+  // Use a specific random generator
+  std::mt19937_64 rng(4242);
+  auto actual3 = conditional.sample(given, &rng);
+  EXPECT_LONGS_EQUAL(1, actual2.size());
+  // regression is not repeatable across platforms/versions :-(
+  // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam/linear/tests/testGaussianDensity.cpp

@@ -17,10 +17,13 @@
  **/
 
 #include <gtsam/linear/GaussianDensity.h>
+#include <gtsam/inference/Symbol.h>
+
 #include <CppUnitLite/TestHarness.h>
 
 using namespace gtsam;
 using namespace std;
+using symbol_shorthand::X;
 
 /* ************************************************************************* */
 TEST(GaussianDensity, constructor)
@@ -37,6 +40,22 @@ TEST(GaussianDensity, constructor)
   EXPECT(assert_equal(s, copied.get_model()->sigmas()));
 }
 
+/* ************************************************************************* */
+// Test FromMeanAndStddev named constructor
+TEST(GaussianDensity, FromMeanAndStddev) {
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  const Vector2 b(20, 40), x0(1, 2);
+  const double sigma = 3;
+
+  VectorValues values;
+  values.insert(X(0), x0);
+
+  auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma);
+  Vector2 e = (x0 - b) / sigma;
+  double expected = 0.5 * e.dot(e);
+  EXPECT_DOUBLES_EQUAL(expected, density.error(values), 1e-9);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

gtsam/linear/tests/testVectorValues.cpp

@@ -17,7 +17,7 @@
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/inference/LabeledSymbol.h>
+#include <gtsam/inference/Symbol.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -248,6 +248,33 @@ TEST(VectorValues, print)
   EXPECT(expected == actual.str());
 }
 
+/* ************************************************************************* */
+// Check html representation.
+TEST(VectorValues, html) {
+  VectorValues vv;
+  using symbol_shorthand::X;
+  vv.insert(X(1), Vector2(2, 3.1));
+  vv.insert(X(2), Vector2(4, 5.2));
+  vv.insert(X(5), Vector2(6, 7.3));
+  vv.insert(X(7), Vector2(8, 9.4));
+  string expected =
+      "<div>\n"
+      "<table class='VectorValues'>\n"
+      "  <thead>\n"
+      "    <tr><th>Variable</th><th>value</th></tr>\n"
+      "  </thead>\n"
+      "  <tbody>\n"
+      "    <tr><th>x1</th><td>  2 3.1</td></tr>\n"
+      "    <tr><th>x2</th><td>  4 5.2</td></tr>\n"
+      "    <tr><th>x5</th><td>  6 7.3</td></tr>\n"
+      "    <tr><th>x7</th><td>  8 9.4</td></tr>\n"
+      "  </tbody>\n"
+      "</table>\n"
+      "</div>";
+  string actual = vv.html();
+  EXPECT(actual == expected);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/navigation/ImuBias.cpp

@@ -66,8 +66,8 @@ namespace imuBias {
 //    }
 /// ostream operator
 std::ostream& operator<<(std::ostream& os, const ConstantBias& bias) {
-  os << "acc = " << Point3(bias.accelerometer());
+  os << "acc = " << bias.accelerometer().transpose();
-  os << " gyro = " << Point3(bias.gyroscope());
+  os << " gyro = " << bias.gyroscope().transpose();
   return os;
 }
 

gtsam/nonlinear/ISAM2Params.h

@@ -300,18 +300,10 @@ 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;
@@ -319,31 +311,12 @@ struct GTSAM_EXPORT ISAM2Params {
   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

gtsam/nonlinear/nonlinear.i

@@ -98,11 +98,11 @@ class NonlinearFactorGraph {
   string dot(
       const gtsam::Values& values,
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
-      const GraphvizFormatting& formatting = GraphvizFormatting());
+      const GraphvizFormatting& writer = GraphvizFormatting());
   void saveGraph(
       const string& s, const gtsam::Values& values,
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
-      const GraphvizFormatting& formatting = GraphvizFormatting()) const;
+      const GraphvizFormatting& writer = GraphvizFormatting()) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -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);
+  int relinearizeSkip;
-  bool isEnableDetailedResults() const;
+  bool enableRelinearization;
-  void setEnableDetailedResults(bool enableDetailedResults);
+  bool evaluateNonlinearError;
-  bool isEnablePartialRelinearizationCheck() const;
+  bool cacheLinearizedFactors;
-  void setEnablePartialRelinearizationCheck(
+  bool enableDetailedResults;
-      bool enablePartialRelinearizationCheck);
+  bool enablePartialRelinearizationCheck;
+  bool findUnusedFactorSlots;
+
+  enum Factorization { CHOLESKY, QR };
+  Factorization factorization;
 };
 
 class ISAM2Clique {

gtsam/sam/sam.i

@@ -22,10 +22,19 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
+// between points:
+typedef gtsam::RangeFactor<gtsam::Point2, gtsam::Point2> RangeFactor2;
+typedef gtsam::RangeFactor<gtsam::Point3, gtsam::Point3> RangeFactor3;
+
+// between pose and point:
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
-typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
+
+// between poses:
+typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
+
+// more specialized types:
 typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3>
     RangeFactorCalibratedCameraPoint;
 typedef gtsam::RangeFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>, gtsam::Point3>

gtsam/sfm/SfmData.cpp

@@ -0,0 +1,460 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 SfmData.cpp
+ * @date January 2022
+ * @author Frank dellaert
+ * @brief Data structure for dealing with Structure from Motion data
+ */
+
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/sfm/SfmData.h>
+#include <gtsam/slam/GeneralSFMFactor.h>
+
+#include <fstream>
+#include <iostream>
+
+namespace gtsam {
+
+using std::cout;
+using std::endl;
+
+using gtsam::symbol_shorthand::P;
+
+/* ************************************************************************** */
+void SfmData::print(const std::string &s) const {
+  std::cout << "Number of cameras = " << cameras.size() << std::endl;
+  std::cout << "Number of tracks = " << tracks.size() << std::endl;
+}
+
+/* ************************************************************************** */
+bool SfmData::equals(const SfmData &sfmData, double tol) const {
+  // check number of cameras and tracks
+  if (cameras.size() != sfmData.cameras.size() ||
+      tracks.size() != sfmData.tracks.size()) {
+    return false;
+  }
+
+  // check each camera
+  for (size_t i = 0; i < cameras.size(); ++i) {
+    if (!camera(i).equals(sfmData.camera(i), tol)) {
+      return false;
+    }
+  }
+
+  // check each track
+  for (size_t j = 0; j < tracks.size(); ++j) {
+    if (!track(j).equals(sfmData.track(j), tol)) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+/* ************************************************************************* */
+Rot3 openGLFixedRotation() {  // this is due to different convention for
+                              // cameras in gtsam and openGL
+  /* R = [ 1   0   0
+   *       0  -1   0
+   *       0   0  -1]
+   */
+  Matrix3 R_mat = Matrix3::Zero(3, 3);
+  R_mat(0, 0) = 1.0;
+  R_mat(1, 1) = -1.0;
+  R_mat(2, 2) = -1.0;
+  return Rot3(R_mat);
+}
+
+/* ************************************************************************* */
+Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) {
+  Rot3 R90 = openGLFixedRotation();
+  Rot3 wRc = (R.inverse()).compose(R90);
+
+  // Our camera-to-world translation wTc = -R'*t
+  return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz)));
+}
+
+/* ************************************************************************* */
+Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) {
+  Rot3 R90 = openGLFixedRotation();
+  Rot3 cRw_openGL = R90.compose(R.inverse());
+  Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
+  return Pose3(cRw_openGL, t_openGL);
+}
+
+/* ************************************************************************* */
+Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) {
+  return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
+                      PoseGTSAM.z());
+}
+
+/* ************************************************************************** */
+SfmData SfmData::FromBundlerFile(const std::string &filename) {
+  // Load the data file
+  std::ifstream is(filename.c_str(), std::ifstream::in);
+  if (!is) {
+    throw std::runtime_error(
+        "Error in FromBundlerFile: can not find the file!!");
+  }
+
+  SfmData sfmData;
+
+  // Ignore the first line
+  char aux[500];
+  is.getline(aux, 500);
+
+  // Get the number of camera poses and 3D points
+  size_t nrPoses, nrPoints;
+  is >> nrPoses >> nrPoints;
+
+  // Get the information for the camera poses
+  for (size_t i = 0; i < nrPoses; i++) {
+    // Get the focal length and the radial distortion parameters
+    float f, k1, k2;
+    is >> f >> k1 >> k2;
+    Cal3Bundler K(f, k1, k2);
+
+    // Get the rotation matrix
+    float r11, r12, r13;
+    float r21, r22, r23;
+    float r31, r32, r33;
+    is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
+
+    // Bundler-OpenGL rotation matrix
+    Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
+
+    // Check for all-zero R, in which case quit
+    if (r11 == 0 && r12 == 0 && r13 == 0) {
+      throw std::runtime_error(
+          "Error in FromBundlerFile: zero rotation matrix");
+    }
+
+    // Get the translation vector
+    float tx, ty, tz;
+    is >> tx >> ty >> tz;
+
+    Pose3 pose = openGL2gtsam(R, tx, ty, tz);
+
+    sfmData.cameras.emplace_back(pose, K);
+  }
+
+  // Get the information for the 3D points
+  sfmData.tracks.reserve(nrPoints);
+  for (size_t j = 0; j < nrPoints; j++) {
+    SfmTrack track;
+
+    // Get the 3D position
+    float x, y, z;
+    is >> x >> y >> z;
+    track.p = Point3(x, y, z);
+
+    // Get the color information
+    float r, g, b;
+    is >> r >> g >> b;
+    track.r = r / 255.f;
+    track.g = g / 255.f;
+    track.b = b / 255.f;
+
+    // Now get the visibility information
+    size_t nvisible = 0;
+    is >> nvisible;
+
+    track.measurements.reserve(nvisible);
+    track.siftIndices.reserve(nvisible);
+    for (size_t k = 0; k < nvisible; k++) {
+      size_t cam_idx = 0, point_idx = 0;
+      float u, v;
+      is >> cam_idx >> point_idx >> u >> v;
+      track.measurements.emplace_back(cam_idx, Point2(u, -v));
+      track.siftIndices.emplace_back(cam_idx, point_idx);
+    }
+
+    sfmData.tracks.push_back(track);
+  }
+
+  return sfmData;
+}
+
+/* ************************************************************************** */
+SfmData SfmData::FromBalFile(const std::string &filename) {
+  // Load the data file
+  std::ifstream is(filename.c_str(), std::ifstream::in);
+  if (!is) {
+    throw std::runtime_error("Error in FromBalFile: can not find the file!!");
+  }
+
+  SfmData sfmData;
+
+  // Get the number of camera poses and 3D points
+  size_t nrPoses, nrPoints, nrObservations;
+  is >> nrPoses >> nrPoints >> nrObservations;
+
+  sfmData.tracks.resize(nrPoints);
+
+  // Get the information for the observations
+  for (size_t k = 0; k < nrObservations; k++) {
+    size_t i = 0, j = 0;
+    float u, v;
+    is >> i >> j >> u >> v;
+    sfmData.tracks[j].measurements.emplace_back(i, Point2(u, -v));
+  }
+
+  // Get the information for the camera poses
+  for (size_t i = 0; i < nrPoses; i++) {
+    // Get the Rodrigues vector
+    float wx, wy, wz;
+    is >> wx >> wy >> wz;
+    Rot3 R = Rot3::Rodrigues(wx, wy, wz);  // BAL-OpenGL rotation matrix
+
+    // Get the translation vector
+    float tx, ty, tz;
+    is >> tx >> ty >> tz;
+
+    Pose3 pose = openGL2gtsam(R, tx, ty, tz);
+
+    // Get the focal length and the radial distortion parameters
+    float f, k1, k2;
+    is >> f >> k1 >> k2;
+    Cal3Bundler K(f, k1, k2);
+
+    sfmData.cameras.emplace_back(pose, K);
+  }
+
+  // Get the information for the 3D points
+  for (size_t j = 0; j < nrPoints; j++) {
+    // Get the 3D position
+    float x, y, z;
+    is >> x >> y >> z;
+    SfmTrack &track = sfmData.tracks[j];
+    track.p = Point3(x, y, z);
+    track.r = 0.4f;
+    track.g = 0.4f;
+    track.b = 0.4f;
+  }
+
+  return sfmData;
+}
+
+/* ************************************************************************** */
+bool writeBAL(const std::string &filename, const SfmData &data) {
+  // Open the output file
+  std::ofstream os;
+  os.open(filename.c_str());
+  os.precision(20);
+  if (!os.is_open()) {
+    cout << "Error in writeBAL: can not open the file!!" << endl;
+    return false;
+  }
+
+  // Write the number of camera poses and 3D points
+  size_t nrObservations = 0;
+  for (size_t j = 0; j < data.tracks.size(); j++) {
+    nrObservations += data.tracks[j].numberMeasurements();
+  }
+
+  // Write observations
+  os << data.cameras.size() << " " << data.tracks.size() << " "
+     << nrObservations << endl;
+  os << endl;
+
+  for (size_t j = 0; j < data.tracks.size(); j++) {  // for each 3D point j
+    const SfmTrack &track = data.tracks[j];
+
+    for (size_t k = 0; k < track.numberMeasurements();
+         k++) {  // for each observation of the 3D point j
+      size_t i = track.measurements[k].first;  // camera id
+      double u0 = data.cameras[i].calibration().px();
+      double v0 = data.cameras[i].calibration().py();
+
+      if (u0 != 0 || v0 != 0) {
+        cout << "writeBAL has not been tested for calibration with nonzero "
+                "(u0,v0)"
+             << endl;
+      }
+
+      double pixelBALx = track.measurements[k].second.x() -
+                         u0;  // center of image is the origin
+      double pixelBALy = -(track.measurements[k].second.y() -
+                           v0);  // center of image is the origin
+      Point2 pixelMeasurement(pixelBALx, pixelBALy);
+      os << i /*camera id*/ << " " << j /*point id*/ << " "
+         << pixelMeasurement.x() /*u of the pixel*/ << " "
+         << pixelMeasurement.y() /*v of the pixel*/ << endl;
+    }
+  }
+  os << endl;
+
+  // Write cameras
+  for (size_t i = 0; i < data.cameras.size(); i++) {  // for each camera
+    Pose3 poseGTSAM = data.cameras[i].pose();
+    Cal3Bundler cameraCalibration = data.cameras[i].calibration();
+    Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
+    os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
+    os << poseOpenGL.translation().x() << endl;
+    os << poseOpenGL.translation().y() << endl;
+    os << poseOpenGL.translation().z() << endl;
+    os << cameraCalibration.fx() << endl;
+    os << cameraCalibration.k1() << endl;
+    os << cameraCalibration.k2() << endl;
+    os << endl;
+  }
+
+  // Write the points
+  for (size_t j = 0; j < data.tracks.size(); j++) {  // for each 3D point j
+    Point3 point = data.tracks[j].p;
+    os << point.x() << endl;
+    os << point.y() << endl;
+    os << point.z() << endl;
+    os << endl;
+  }
+
+  os.close();
+  return true;
+}
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
+bool readBundler(const std::string &filename, SfmData &data) {
+  try {
+    data = SfmData::FromBundlerFile(filename);
+    return true;
+  } catch (const std::exception & /* e */) {
+    return false;
+  }
+}
+bool readBAL(const std::string &filename, SfmData &data) {
+  try {
+    data = SfmData::FromBalFile(filename);
+    return true;
+  } catch (const std::exception & /* e */) {
+    return false;
+  }
+}
+#endif
+
+SfmData readBal(const std::string &filename) {
+  return SfmData::FromBalFile(filename);
+}
+
+/* ************************************************************************** */
+bool writeBALfromValues(const std::string &filename, const SfmData &data,
+                        const Values &values) {
+  using Camera = PinholeCamera<Cal3Bundler>;
+  SfmData dataValues = data;
+
+  // Store poses or cameras in SfmData
+  size_t nrPoses = values.count<Pose3>();
+  if (nrPoses == dataValues.cameras.size()) {  // we only estimated camera poses
+    for (size_t i = 0; i < dataValues.cameras.size(); i++) {  // for each camera
+      Pose3 pose = values.at<Pose3>(i);
+      Cal3Bundler K = dataValues.cameras[i].calibration();
+      Camera camera(pose, K);
+      dataValues.cameras[i] = camera;
+    }
+  } else {
+    size_t nrCameras = values.count<Camera>();
+    if (nrCameras == dataValues.cameras.size()) {  // we only estimated camera
+                                                   // poses and calibration
+      for (size_t i = 0; i < nrCameras; i++) {     // for each camera
+        Key cameraKey = i;                         // symbol('c',i);
+        Camera camera = values.at<Camera>(cameraKey);
+        dataValues.cameras[i] = camera;
+      }
+    } else {
+      cout << "writeBALfromValues: different number of cameras in "
+              "SfM_dataValues (#cameras "
+           << dataValues.cameras.size() << ") and values (#cameras " << nrPoses
+           << ", #poses " << nrCameras << ")!!" << endl;
+      return false;
+    }
+  }
+
+  // Store 3D points in SfmData
+  size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.tracks.size();
+  if (nrPoints != nrTracks) {
+    cout << "writeBALfromValues: different number of points in "
+            "SfM_dataValues (#points= "
+         << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl;
+  }
+
+  for (size_t j = 0; j < nrTracks; j++) {  // for each point
+    Key pointKey = P(j);
+    if (values.exists(pointKey)) {
+      Point3 point = values.at<Point3>(pointKey);
+      dataValues.tracks[j].p = point;
+    } else {
+      dataValues.tracks[j].r = 1.0;
+      dataValues.tracks[j].g = 0.0;
+      dataValues.tracks[j].b = 0.0;
+      dataValues.tracks[j].p = Point3(0, 0, 0);
+    }
+  }
+
+  // Write SfmData to file
+  return writeBAL(filename, dataValues);
+}
+
+/* ************************************************************************** */
+NonlinearFactorGraph SfmData::generalSfmFactors(
+    const SharedNoiseModel &model) const {
+  using ProjectionFactor = GeneralSFMFactor<SfmCamera, Point3>;
+  NonlinearFactorGraph factors;
+
+  size_t j = 0;
+  for (const SfmTrack &track : tracks) {
+    for (const SfmMeasurement &m : track.measurements) {
+      size_t i = m.first;
+      Point2 uv = m.second;
+      factors.emplace_shared<ProjectionFactor>(uv, model, i, P(j));
+    }
+    j += 1;
+  }
+
+  return factors;
+}
+
+/* ************************************************************************** */
+NonlinearFactorGraph SfmData::sfmFactorGraph(
+    const SharedNoiseModel &model, boost::optional<size_t> fixedCamera,
+    boost::optional<size_t> fixedPoint) const {
+  using ProjectionFactor = GeneralSFMFactor<SfmCamera, Point3>;
+  NonlinearFactorGraph graph = generalSfmFactors(model);
+  using noiseModel::Constrained;
+  if (fixedCamera) {
+    graph.addPrior(*fixedCamera, cameras[0], Constrained::All(9));
+  }
+  if (fixedPoint) {
+    graph.addPrior(P(*fixedPoint), tracks[0].p, Constrained::All(3));
+  }
+  return graph;
+}
+
+/* ************************************************************************** */
+Values initialCamerasEstimate(const SfmData &db) {
+  Values initial;
+  size_t i = 0;  // NO POINTS:  j = 0;
+  for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera);
+  return initial;
+}
+
+/* ************************************************************************** */
+Values initialCamerasAndPointsEstimate(const SfmData &db) {
+  Values initial;
+  size_t i = 0, j = 0;
+  for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera);
+  for (const SfmTrack &track : db.tracks) initial.insert(P(j++), track.p);
+  return initial;
+}
+
+/* ************************************************************************** */
+
+}  // namespace gtsam

gtsam/sfm/SfmData.h

@@ -0,0 +1,236 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 SfmData.h
+ * @date January 2022
+ * @author Frank dellaert
+ * @brief Data structure for dealing with Structure from Motion data
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/sfm/SfmTrack.h>
+
+#include <string>
+#include <vector>
+
+namespace gtsam {
+
+/// Define the structure for the camera poses
+typedef PinholeCamera<Cal3Bundler> SfmCamera;
+
+/**
+ * @brief SfmData stores a bunch of SfmTracks
+ * @addtogroup sfm
+ */
+struct GTSAM_EXPORT SfmData {
+  std::vector<SfmCamera> cameras;  ///< Set of cameras
+
+  std::vector<SfmTrack> tracks;  ///< Sparse set of points
+
+  /// @name Create from file
+  /// @{
+
+  /**
+   * @brief Parses a bundler output file and return result as SfmData instance.
+   * @param filename The name of the bundler file
+   * @param data SfM structure where the data is stored
+   * @return true if the parsing was successful, false otherwise
+   */
+  static SfmData FromBundlerFile(const std::string& filename);
+
+  /**
+   * @brief Parse a "Bundle Adjustment in the Large" (BAL) file and return
+   * result as SfmData instance.
+   * @param filename The name of the BAL file.
+   * @return SfM structure where the data is stored.
+   */
+  static SfmData FromBalFile(const std::string& filename);
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// Add a track to SfmData
+  void addTrack(const SfmTrack& t) { tracks.push_back(t); }
+
+  /// Add a camera to SfmData
+  void addCamera(const SfmCamera& cam) { cameras.push_back(cam); }
+
+  /// The number of reconstructed 3D points
+  size_t numberTracks() const { return tracks.size(); }
+
+  /// The number of cameras
+  size_t numberCameras() const { return cameras.size(); }
+
+  /// The track formed by series of landmark measurements
+  SfmTrack track(size_t idx) const { return tracks[idx]; }
+
+  /// The camera pose at frame index `idx`
+  SfmCamera camera(size_t idx) const { return cameras[idx]; }
+
+  /**
+   * @brief Create projection factors using keys i and P(j)
+   *
+   * @param model a noise model for projection errors
+   * @return NonlinearFactorGraph
+   */
+  NonlinearFactorGraph generalSfmFactors(
+      const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2,
+                                                                   1.0)) const;
+
+  /**
+   * @brief Create factor graph with projection factors and gauge fix.
+   *
+   * Note: pose keys are simply integer indices, points use Symbol('p', j).
+   *
+   * @param model a noise model for projection errors
+   * @param fixedCamera which camera to fix, if any (use boost::none if none)
+   * @param fixedPoint which point to fix, if any (use boost::none if none)
+   * @return NonlinearFactorGraph
+   */
+  NonlinearFactorGraph sfmFactorGraph(
+      const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0),
+      boost::optional<size_t> fixedCamera = 0,
+      boost::optional<size_t> fixedPoint = 0) const;
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// print
+  void print(const std::string& s = "") const;
+
+  /// assert equality up to a tolerance
+  bool equals(const SfmData& sfmData, double tol = 1e-9) const;
+
+  /// @}
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
+  /// @name Deprecated
+  /// @{
+  void GTSAM_DEPRECATED add_track(const SfmTrack& t) { tracks.push_back(t); }
+  void GTSAM_DEPRECATED add_camera(const SfmCamera& cam) {
+    cameras.push_back(cam);
+  }
+  size_t GTSAM_DEPRECATED number_tracks() const { return tracks.size(); }
+  size_t GTSAM_DEPRECATED number_cameras() const { return cameras.size(); }
+  /// @}
+#endif
+  /// @name Serialization
+  /// @{
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_NVP(cameras);
+    ar& BOOST_SERIALIZATION_NVP(tracks);
+  }
+
+  /// @}
+};
+
+/// traits
+template <>
+struct traits<SfmData> : public Testable<SfmData> {};
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
+GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename,
+                                               SfmData& data);
+GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename,
+                                           SfmData& data);
+#endif
+
+/**
+ * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and
+ * returns the data as a SfmData structure. Mainly used by wrapped code.
+ * @param filename The name of the BAL file.
+ * @return SfM structure where the data is stored.
+ */
+GTSAM_EXPORT SfmData readBal(const std::string& filename);
+
+/**
+ * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file
+ * from a SfmData structure
+ * @param filename The name of the BAL file to write
+ * @param data SfM structure where the data is stored
+ * @return true if the parsing was successful, false otherwise
+ */
+GTSAM_EXPORT bool writeBAL(const std::string& filename, const SfmData& data);
+
+/**
+ * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file
+ * from a SfmData structure and a value structure (measurements are the same as
+ * the SfM input data, while camera poses and values are read from Values)
+ * @param filename The name of the BAL file to write
+ * @param data SfM structure where the data is stored
+ * @param values structure where the graph values are stored (values can be
+ * either Pose3 or PinholeCamera<Cal3Bundler> for the cameras, and should be
+ * Point3 for the 3D points). Note: assumes that the keys are "i" for pose i
+ * and "Symbol::('p',j)" for landmark j.
+ * @return true if the parsing was successful, false otherwise
+ */
+GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
+                                     const SfmData& data, const Values& values);
+
+/**
+ * @brief This function converts an openGL camera pose to an GTSAM camera pose
+ * @param R rotation in openGL
+ * @param tx x component of the translation in openGL
+ * @param ty y component of the translation in openGL
+ * @param tz z component of the translation in openGL
+ * @return Pose3 in GTSAM format
+ */
+GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);
+
+/**
+ * @brief This function converts a GTSAM camera pose to an openGL camera pose
+ * @param R rotation in GTSAM
+ * @param tx x component of the translation in GTSAM
+ * @param ty y component of the translation in GTSAM
+ * @param tz z component of the translation in GTSAM
+ * @return Pose3 in openGL format
+ */
+GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
+
+/**
+ * @brief This function converts a GTSAM camera pose to an openGL camera pose
+ * @param PoseGTSAM pose in GTSAM format
+ * @return Pose3 in openGL format
+ */
+GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
+
+/**
+ * @brief This function creates initial values for cameras from db.
+ *
+ * No symbol is used, so camera keys are simply integer indices.
+ *
+ * @param SfmData
+ * @return Values
+ */
+GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
+
+/**
+ * @brief This function creates initial values for cameras and points from db
+ *
+ * Note: Pose keys are simply integer indices, points use Symbol('p', j).
+ *
+ * @param SfmData
+ * @return Values
+ */
+GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
+
+}  // namespace gtsam

gtsam/sfm/SfmTrack.cpp

@@ -0,0 +1,71 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 SfmTrack.cpp
+ * @date January 2022
+ * @author Frank Dellaert
+ * @brief A simple data structure for a track in Structure from Motion
+ */
+
+#include <gtsam/sfm/SfmTrack.h>
+
+#include <iostream>
+
+namespace gtsam {
+
+void SfmTrack::print(const std::string& s) const {
+  std::cout << "Track with " << measurements.size();
+  std::cout << " measurements of point " << p << std::endl;
+}
+
+bool SfmTrack::equals(const SfmTrack& sfmTrack, double tol) const {
+  // check the 3D point
+  if (!p.isApprox(sfmTrack.p)) {
+    return false;
+  }
+
+  // check the RGB values
+  if (r != sfmTrack.r || g != sfmTrack.g || b != sfmTrack.b) {
+    return false;
+  }
+
+  // compare size of vectors for measurements and siftIndices
+  if (numberMeasurements() != sfmTrack.numberMeasurements() ||
+      siftIndices.size() != sfmTrack.siftIndices.size()) {
+    return false;
+  }
+
+  // compare measurements (order sensitive)
+  for (size_t idx = 0; idx < numberMeasurements(); ++idx) {
+    SfmMeasurement measurement = measurements[idx];
+    SfmMeasurement otherMeasurement = sfmTrack.measurements[idx];
+
+    if (measurement.first != otherMeasurement.first ||
+        !measurement.second.isApprox(otherMeasurement.second)) {
+      return false;
+    }
+  }
+
+  // compare sift indices (order sensitive)
+  for (size_t idx = 0; idx < siftIndices.size(); ++idx) {
+    SiftIndex index = siftIndices[idx];
+    SiftIndex otherIndex = sfmTrack.siftIndices[idx];
+
+    if (index.first != otherIndex.first || index.second != otherIndex.second) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+}  // namespace gtsam

gtsam/sfm/SfmTrack.h

@@ -0,0 +1,133 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 SfmTrack.h
+ * @date January 2022
+ * @author Frank Dellaert
+ * @brief A simple data structure for a track in Structure from Motion
+ */
+
+#pragma once
+
+#include <gtsam/base/serialization.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace gtsam {
+
+/// A measurement with its camera index
+typedef std::pair<size_t, Point2> SfmMeasurement;
+
+/// Sift index for SfmTrack
+typedef std::pair<size_t, size_t> SiftIndex;
+
+/**
+ * @brief An SfmTrack stores SfM measurements grouped in a track
+ * @addtogroup sfm
+ */
+struct GTSAM_EXPORT SfmTrack {
+  Point3 p;       ///< 3D position of the point
+  float r, g, b;  ///< RGB color of the 3D point
+
+  /// The 2D image projections (id,(u,v))
+  std::vector<SfmMeasurement> measurements;
+
+  /// The feature descriptors
+  std::vector<SiftIndex> siftIndices;
+
+  /// @name Constructors
+  /// @{
+
+  explicit SfmTrack(float r = 0, float g = 0, float b = 0)
+      : p(0, 0, 0), r(r), g(g), b(b) {}
+
+  explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0,
+                    float b = 0)
+      : p(pt), r(r), g(g), b(b) {}
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// Add measurement (camera_idx, Point2) to track
+  void addMeasurement(size_t idx, const gtsam::Point2& m) {
+    measurements.emplace_back(idx, m);
+  }
+
+  /// Total number of measurements in this track
+  size_t numberMeasurements() const { return measurements.size(); }
+
+  /// Get the measurement (camera index, Point2) at pose index `idx`
+  const SfmMeasurement& measurement(size_t idx) const {
+    return measurements[idx];
+  }
+
+  /// Get the SIFT feature index corresponding to the measurement at `idx`
+  const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; }
+
+  /// Get 3D point
+  const Point3& point3() const { return p; }
+
+  /// Get RGB values describing 3d point
+  Point3 rgb() const { return Point3(r, g, b); }
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// print
+  void print(const std::string& s = "") const;
+
+  /// assert equality up to a tolerance
+  bool equals(const SfmTrack& sfmTrack, double tol = 1e-9) const;
+
+  /// @}
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
+  /// @name Deprecated
+  /// @{
+  void GTSAM_DEPRECATED add_measurement(size_t idx, const gtsam::Point2& m) {
+    measurements.emplace_back(idx, m);
+  }
+
+  size_t GTSAM_DEPRECATED number_measurements() const {
+    return measurements.size();
+  }
+  /// @}
+#endif
+  /// @name Serialization
+  /// @{
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_NVP(p);
+    ar& BOOST_SERIALIZATION_NVP(r);
+    ar& BOOST_SERIALIZATION_NVP(g);
+    ar& BOOST_SERIALIZATION_NVP(b);
+    ar& BOOST_SERIALIZATION_NVP(measurements);
+    ar& BOOST_SERIALIZATION_NVP(siftIndices);
+  }
+  /// @}
+};
+
+template <typename T>
+struct traits;
+
+template <>
+struct traits<SfmTrack> : public Testable<SfmTrack> {};
+
+}  // namespace gtsam

gtsam/sfm/ShonanAveraging.h

@@ -165,7 +165,7 @@ class GTSAM_EXPORT ShonanAveraging {
   size_t nrUnknowns() const { return nrUnknowns_; }
 
   /// Return number of measurements
-  size_t nrMeasurements() const { return measurements_.size(); }
+  size_t numberMeasurements() const { return measurements_.size(); }
 
   /// k^th binary measurement
   const BinaryMeasurement<Rot> &measurement(size_t k) const {

gtsam/sfm/sfm.i

@@ -4,7 +4,62 @@
 
 namespace gtsam {
 
-// #####
+#include <gtsam/sfm/SfmTrack.h>
+class SfmTrack {
+  SfmTrack();
+  SfmTrack(const gtsam::Point3& pt);
+  const Point3& point3() const;
+
+  double r;
+  double g;
+  double b;
+
+  std::vector<pair<size_t, gtsam::Point2>> measurements;
+
+  size_t numberMeasurements() const;
+  pair<size_t, gtsam::Point2> measurement(size_t idx) const;
+  pair<size_t, size_t> siftIndex(size_t idx) const;
+  void addMeasurement(size_t idx, const gtsam::Point2& m);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enabling function to compare objects
+  bool equals(const gtsam::SfmTrack& expected, double tol) const;
+};
+
+#include <gtsam/sfm/SfmData.h>
+class SfmData {
+  SfmData();
+  static gtsam::SfmData FromBundlerFile(string filename);
+  static gtsam::SfmData FromBalFile(string filename);
+
+  void addTrack(const gtsam::SfmTrack& t);
+  void addCamera(const gtsam::SfmCamera& cam);
+  size_t numberTracks() const;
+  size_t numberCameras() const;
+  gtsam::SfmTrack track(size_t idx) const;
+  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
+
+  gtsam::NonlinearFactorGraph generalSfmFactors(
+      const gtsam::SharedNoiseModel& model =
+          gtsam::noiseModel::Isotropic::Sigma(2, 1.0)) const;
+  gtsam::NonlinearFactorGraph sfmFactorGraph(
+      const gtsam::SharedNoiseModel& model =
+          gtsam::noiseModel::Isotropic::Sigma(2, 1.0),
+      size_t fixedCamera = 0, size_t fixedPoint = 0) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+
+  // enabling function to compare objects
+  bool equals(const gtsam::SfmData& expected, double tol) const;
+};
+
+gtsam::SfmData readBal(string filename);
+bool writeBAL(string filename, gtsam::SfmData& data);
+gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
+gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
 
 #include <gtsam/sfm/ShonanFactor.h>
 
@@ -92,7 +147,7 @@ class ShonanAveraging2 {
 
   // Query properties
   size_t nrUnknowns() const;
-  size_t nrMeasurements() const;
+  size_t numberMeasurements() const;
   gtsam::Rot2 measured(size_t i);
   gtsam::KeyVector keys(size_t i);
 
@@ -140,7 +195,7 @@ class ShonanAveraging3 {
 
   // Query properties
   size_t nrUnknowns() const;
-  size_t nrMeasurements() const;
+  size_t numberMeasurements() const;
   gtsam::Rot3 measured(size_t i);
   gtsam::KeyVector keys(size_t i);
 

gtsam/sfm/tests/testSfmData.cpp

@@ -0,0 +1,214 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 TestSfmData.cpp
+ * @date January 2022
+ * @author Frank dellaert
+ * @brief tests for SfmData class and associated utilites
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/sfm/SfmData.h>
+
+using namespace std;
+using namespace gtsam;
+
+using gtsam::symbol_shorthand::P;
+
+namespace gtsam {
+GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
+GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
+}  // namespace gtsam
+
+/* ************************************************************************* */
+TEST(dataSet, Balbianello) {
+  // The structure where we will save the SfM data
+  const string filename = findExampleDataFile("Balbianello");
+  SfmData sfmData = SfmData::FromBundlerFile(filename);
+
+  // Check number of things
+  EXPECT_LONGS_EQUAL(5, sfmData.numberCameras());
+  EXPECT_LONGS_EQUAL(544, sfmData.numberTracks());
+  const SfmTrack& track0 = sfmData.tracks[0];
+  EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
+
+  // Check projection of a given point
+  EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
+  const SfmCamera& camera0 = sfmData.cameras[0];
+  Point2 expected = camera0.project(track0.p),
+         actual = track0.measurements[0].second;
+  EXPECT(assert_equal(expected, actual, 1));
+
+  // We share *one* noiseModel between all projection factors
+  auto model = noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
+
+  // Convert to NonlinearFactorGraph
+  NonlinearFactorGraph graph = sfmData.sfmFactorGraph(model);
+  EXPECT_LONGS_EQUAL(1419, graph.size());  // regression
+
+  // Get initial estimate
+  Values values = initialCamerasAndPointsEstimate(sfmData);
+  EXPECT_LONGS_EQUAL(549, values.size());  // regression
+}
+
+/* ************************************************************************* */
+TEST(dataSet, readBAL_Dubrovnik) {
+  // The structure where we will save the SfM data
+  const string filename = findExampleDataFile("dubrovnik-3-7-pre");
+  SfmData sfmData = SfmData::FromBalFile(filename);
+
+  // Check number of things
+  EXPECT_LONGS_EQUAL(3, sfmData.numberCameras());
+  EXPECT_LONGS_EQUAL(7, sfmData.numberTracks());
+  const SfmTrack& track0 = sfmData.tracks[0];
+  EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
+
+  // Check projection of a given point
+  EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
+  const SfmCamera& camera0 = sfmData.cameras[0];
+  Point2 expected = camera0.project(track0.p),
+         actual = track0.measurements[0].second;
+  EXPECT(assert_equal(expected, actual, 12));
+}
+
+/* ************************************************************************* */
+TEST(dataSet, openGL2gtsam) {
+  Vector3 rotVec(0.2, 0.7, 1.1);
+  Rot3 R = Rot3::Expmap(rotVec);
+  Point3 t = Point3(0.0, 0.0, 0.0);
+  Pose3 poseGTSAM = Pose3(R, t);
+
+  Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z());
+
+  Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3();  // columns!
+  Rot3 cRw(r1.x(), r2.x(), r3.x(), -r1.y(), -r2.y(), -r3.y(), -r1.z(), -r2.z(),
+           -r3.z());
+  Rot3 wRc = cRw.inverse();
+  Pose3 actual = Pose3(wRc, t);
+
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(dataSet, gtsam2openGL) {
+  Vector3 rotVec(0.2, 0.7, 1.1);
+  Rot3 R = Rot3::Expmap(rotVec);
+  Point3 t = Point3(1.0, 20.0, 10.0);
+  Pose3 actual = Pose3(R, t);
+  Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z());
+
+  Pose3 expected = gtsam2openGL(poseGTSAM);
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(dataSet, writeBAL_Dubrovnik) {
+  const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
+  SfmData readData = SfmData::FromBalFile(filenameToRead);
+
+  // Write readData to file filenameToWrite
+  const string filenameToWrite = createRewrittenFileName(filenameToRead);
+  CHECK(writeBAL(filenameToWrite, readData));
+
+  // Read what we wrote
+  SfmData writtenData = SfmData::FromBalFile(filenameToWrite);
+
+  // Check that what we read is the same as what we wrote
+  EXPECT_LONGS_EQUAL(readData.numberCameras(), writtenData.numberCameras());
+  EXPECT_LONGS_EQUAL(readData.numberTracks(), writtenData.numberTracks());
+
+  for (size_t i = 0; i < readData.numberCameras(); i++) {
+    PinholeCamera<Cal3Bundler> expectedCamera = writtenData.cameras[i];
+    PinholeCamera<Cal3Bundler> actualCamera = readData.cameras[i];
+    EXPECT(assert_equal(expectedCamera, actualCamera));
+  }
+
+  for (size_t j = 0; j < readData.numberTracks(); j++) {
+    // check point
+    SfmTrack expectedTrack = writtenData.tracks[j];
+    SfmTrack actualTrack = readData.tracks[j];
+    Point3 expectedPoint = expectedTrack.p;
+    Point3 actualPoint = actualTrack.p;
+    EXPECT(assert_equal(expectedPoint, actualPoint));
+
+    // check rgb
+    Point3 expectedRGB =
+        Point3(expectedTrack.r, expectedTrack.g, expectedTrack.b);
+    Point3 actualRGB = Point3(actualTrack.r, actualTrack.g, actualTrack.b);
+    EXPECT(assert_equal(expectedRGB, actualRGB));
+
+    // check measurements
+    for (size_t k = 0; k < actualTrack.numberMeasurements(); k++) {
+      EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,
+                         actualTrack.measurements[k].first);
+      EXPECT(assert_equal(expectedTrack.measurements[k].second,
+                          actualTrack.measurements[k].second));
+    }
+  }
+}
+
+/* ************************************************************************* */
+TEST(dataSet, writeBALfromValues_Dubrovnik) {
+  const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
+  SfmData readData = SfmData::FromBalFile(filenameToRead);
+
+  Pose3 poseChange =
+      Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3));
+
+  Values values;
+  for (size_t i = 0; i < readData.numberCameras(); i++) {  // for each camera
+    Pose3 pose = poseChange.compose(readData.cameras[i].pose());
+    values.insert(i, pose);
+  }
+  for (size_t j = 0; j < readData.numberTracks(); j++) {  // for each point
+    Point3 point = poseChange.transformFrom(readData.tracks[j].p);
+    values.insert(P(j), point);
+  }
+
+  // Write values and readData to a file
+  const string filenameToWrite = createRewrittenFileName(filenameToRead);
+  writeBALfromValues(filenameToWrite, readData, values);
+
+  // Read the file we wrote
+  SfmData writtenData = SfmData::FromBalFile(filenameToWrite);
+
+  // Check that the reprojection errors are the same and the poses are correct
+  // Check number of things
+  EXPECT_LONGS_EQUAL(3, writtenData.numberCameras());
+  EXPECT_LONGS_EQUAL(7, writtenData.numberTracks());
+  const SfmTrack& track0 = writtenData.tracks[0];
+  EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
+
+  // Check projection of a given point
+  EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
+  const SfmCamera& camera0 = writtenData.cameras[0];
+  Point2 expected = camera0.project(track0.p),
+         actual = track0.measurements[0].second;
+  EXPECT(assert_equal(expected, actual, 12));
+
+  Pose3 expectedPose = camera0.pose();
+  Pose3 actualPose = values.at<Pose3>(0);
+  EXPECT(assert_equal(expectedPose, actualPose, 1e-7));
+
+  Point3 expectedPoint = track0.p;
+  Point3 actualPoint = values.at<Point3>(P(0));
+  EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/slam/FrobeniusFactor.h

@@ -54,6 +54,8 @@ class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1<Rot> {
   Eigen::Matrix<double, Dim, 1> vecM_;  ///< vectorized matrix to approximate
 
  public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   /// Constructor
   FrobeniusPrior(Key j, const MatrixNN& M,
                  const SharedNoiseModel& model = nullptr)
@@ -106,6 +108,8 @@ class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
   enum { Dim = Rot::VectorN2::RowsAtCompileTime };
 
  public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   /// @name Constructor
   /// @{
 

gtsam/slam/PoseRotationPrior.h

@@ -39,6 +39,9 @@ protected:
 
 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) {}

gtsam/slam/SmartFactorBase.h

@@ -146,7 +146,7 @@ protected:
    */
   template<class SFM_TRACK>
   void add(const SFM_TRACK& trackToAdd) {
-    for (size_t k = 0; k < trackToAdd.number_measurements(); k++) {
+    for (size_t k = 0; k < trackToAdd.numberMeasurements(); k++) {
       this->measured_.push_back(trackToAdd.measurements[k].second);
       this->keys_.push_back(trackToAdd.measurements[k].first);
     }

gtsam/slam/dataset.cpp

@@ -54,8 +54,6 @@
 using namespace std;
 namespace fs = boost::filesystem;
 using gtsam::symbol_shorthand::L;
-using gtsam::symbol_shorthand::P;
-using gtsam::symbol_shorthand::X;
 
 #define LINESIZE 81920
 
@@ -208,15 +206,15 @@ std::map<size_t, Point2> parseVariables<Point2>(const std::string &filename,
 
 /* ************************************************************************* */
 // Interpret noise parameters according to flags
-static SharedNoiseModel
+static SharedNoiseModel createNoiseModel(
-createNoiseModel(const Vector6 v, bool smart, NoiseFormat noiseFormat,
+    const Vector6 &v, bool smart, NoiseFormat noiseFormat,
-                 KernelFunctionType kernelFunctionType) {
+    KernelFunctionType kernelFunctionType) {
   if (noiseFormat == NoiseFormatAUTO) {
     // Try to guess covariance matrix layout
-    if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && //
+    if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 &&  //
         v(3) != 0.0 && v(4) == 0.0 && v(5) == 0.0) {
       noiseFormat = NoiseFormatGRAPH;
-    } else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && //
+    } else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 &&  //
                v(3) != 0.0 && v(4) == 0.0 && v(5) != 0.0) {
       noiseFormat = NoiseFormatCOV;
     } else {
@@ -945,352 +943,6 @@ GraphAndValues load3D(const string &filename) {
   return make_pair(graph, initial);
 }
 
-/* ************************************************************************* */
-Rot3 openGLFixedRotation() { // this is due to different convention for
-                             // cameras in gtsam and openGL
-  /* R = [ 1   0   0
-   *       0  -1   0
-   *       0   0  -1]
-   */
-  Matrix3 R_mat = Matrix3::Zero(3, 3);
-  R_mat(0, 0) = 1.0;
-  R_mat(1, 1) = -1.0;
-  R_mat(2, 2) = -1.0;
-  return Rot3(R_mat);
-}
-
-/* ************************************************************************* */
-Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) {
-  Rot3 R90 = openGLFixedRotation();
-  Rot3 wRc = (R.inverse()).compose(R90);
-
-  // Our camera-to-world translation wTc = -R'*t
-  return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz)));
-}
-
-/* ************************************************************************* */
-Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) {
-  Rot3 R90 = openGLFixedRotation();
-  Rot3 cRw_openGL = R90.compose(R.inverse());
-  Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
-  return Pose3(cRw_openGL, t_openGL);
-}
-
-/* ************************************************************************* */
-Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) {
-  return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
-                      PoseGTSAM.z());
-}
-
-/* ************************************************************************* */
-bool readBundler(const string &filename, SfmData &data) {
-  // Load the data file
-  ifstream is(filename.c_str(), ifstream::in);
-  if (!is) {
-    cout << "Error in readBundler: can not find the file!!" << endl;
-    return false;
-  }
-
-  // Ignore the first line
-  char aux[500];
-  is.getline(aux, 500);
-
-  // Get the number of camera poses and 3D points
-  size_t nrPoses, nrPoints;
-  is >> nrPoses >> nrPoints;
-
-  // Get the information for the camera poses
-  for (size_t i = 0; i < nrPoses; i++) {
-    // Get the focal length and the radial distortion parameters
-    float f, k1, k2;
-    is >> f >> k1 >> k2;
-    Cal3Bundler K(f, k1, k2);
-
-    // Get the rotation matrix
-    float r11, r12, r13;
-    float r21, r22, r23;
-    float r31, r32, r33;
-    is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
-
-    // Bundler-OpenGL rotation matrix
-    Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
-
-    // Check for all-zero R, in which case quit
-    if (r11 == 0 && r12 == 0 && r13 == 0) {
-      cout << "Error in readBundler: zero rotation matrix for pose " << i
-           << endl;
-      return false;
-    }
-
-    // Get the translation vector
-    float tx, ty, tz;
-    is >> tx >> ty >> tz;
-
-    Pose3 pose = openGL2gtsam(R, tx, ty, tz);
-
-    data.cameras.emplace_back(pose, K);
-  }
-
-  // Get the information for the 3D points
-  data.tracks.reserve(nrPoints);
-  for (size_t j = 0; j < nrPoints; j++) {
-    SfmTrack track;
-
-    // Get the 3D position
-    float x, y, z;
-    is >> x >> y >> z;
-    track.p = Point3(x, y, z);
-
-    // Get the color information
-    float r, g, b;
-    is >> r >> g >> b;
-    track.r = r / 255.f;
-    track.g = g / 255.f;
-    track.b = b / 255.f;
-
-    // Now get the visibility information
-    size_t nvisible = 0;
-    is >> nvisible;
-
-    track.measurements.reserve(nvisible);
-    track.siftIndices.reserve(nvisible);
-    for (size_t k = 0; k < nvisible; k++) {
-      size_t cam_idx = 0, point_idx = 0;
-      float u, v;
-      is >> cam_idx >> point_idx >> u >> v;
-      track.measurements.emplace_back(cam_idx, Point2(u, -v));
-      track.siftIndices.emplace_back(cam_idx, point_idx);
-    }
-
-    data.tracks.push_back(track);
-  }
-
-  is.close();
-  return true;
-}
-
-/* ************************************************************************* */
-bool readBAL(const string &filename, SfmData &data) {
-  // Load the data file
-  ifstream is(filename.c_str(), ifstream::in);
-  if (!is) {
-    cout << "Error in readBAL: can not find the file!!" << endl;
-    return false;
-  }
-
-  // Get the number of camera poses and 3D points
-  size_t nrPoses, nrPoints, nrObservations;
-  is >> nrPoses >> nrPoints >> nrObservations;
-
-  data.tracks.resize(nrPoints);
-
-  // Get the information for the observations
-  for (size_t k = 0; k < nrObservations; k++) {
-    size_t i = 0, j = 0;
-    float u, v;
-    is >> i >> j >> u >> v;
-    data.tracks[j].measurements.emplace_back(i, Point2(u, -v));
-  }
-
-  // Get the information for the camera poses
-  for (size_t i = 0; i < nrPoses; i++) {
-    // Get the Rodrigues vector
-    float wx, wy, wz;
-    is >> wx >> wy >> wz;
-    Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix
-
-    // Get the translation vector
-    float tx, ty, tz;
-    is >> tx >> ty >> tz;
-
-    Pose3 pose = openGL2gtsam(R, tx, ty, tz);
-
-    // Get the focal length and the radial distortion parameters
-    float f, k1, k2;
-    is >> f >> k1 >> k2;
-    Cal3Bundler K(f, k1, k2);
-
-    data.cameras.emplace_back(pose, K);
-  }
-
-  // Get the information for the 3D points
-  for (size_t j = 0; j < nrPoints; j++) {
-    // Get the 3D position
-    float x, y, z;
-    is >> x >> y >> z;
-    SfmTrack &track = data.tracks[j];
-    track.p = Point3(x, y, z);
-    track.r = 0.4f;
-    track.g = 0.4f;
-    track.b = 0.4f;
-  }
-
-  is.close();
-  return true;
-}
-
-/* ************************************************************************* */
-SfmData readBal(const string &filename) {
-  SfmData data;
-  readBAL(filename, data);
-  return data;
-}
-
-/* ************************************************************************* */
-bool writeBAL(const string &filename, SfmData &data) {
-  // Open the output file
-  ofstream os;
-  os.open(filename.c_str());
-  os.precision(20);
-  if (!os.is_open()) {
-    cout << "Error in writeBAL: can not open the file!!" << endl;
-    return false;
-  }
-
-  // Write the number of camera poses and 3D points
-  size_t nrObservations = 0;
-  for (size_t j = 0; j < data.number_tracks(); j++) {
-    nrObservations += data.tracks[j].number_measurements();
-  }
-
-  // Write observations
-  os << data.number_cameras() << " " << data.number_tracks() << " "
-     << nrObservations << endl;
-  os << endl;
-
-  for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
-    const SfmTrack &track = data.tracks[j];
-
-    for (size_t k = 0; k < track.number_measurements();
-         k++) { // for each observation of the 3D point j
-      size_t i = track.measurements[k].first; // camera id
-      double u0 = data.cameras[i].calibration().px();
-      double v0 = data.cameras[i].calibration().py();
-
-      if (u0 != 0 || v0 != 0) {
-        cout << "writeBAL has not been tested for calibration with nonzero "
-                "(u0,v0)"
-             << endl;
-      }
-
-      double pixelBALx = track.measurements[k].second.x() -
-                         u0; // center of image is the origin
-      double pixelBALy = -(track.measurements[k].second.y() -
-                           v0); // center of image is the origin
-      Point2 pixelMeasurement(pixelBALx, pixelBALy);
-      os << i /*camera id*/ << " " << j /*point id*/ << " "
-         << pixelMeasurement.x() /*u of the pixel*/ << " "
-         << pixelMeasurement.y() /*v of the pixel*/ << endl;
-    }
-  }
-  os << endl;
-
-  // Write cameras
-  for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera
-    Pose3 poseGTSAM = data.cameras[i].pose();
-    Cal3Bundler cameraCalibration = data.cameras[i].calibration();
-    Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
-    os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
-    os << poseOpenGL.translation().x() << endl;
-    os << poseOpenGL.translation().y() << endl;
-    os << poseOpenGL.translation().z() << endl;
-    os << cameraCalibration.fx() << endl;
-    os << cameraCalibration.k1() << endl;
-    os << cameraCalibration.k2() << endl;
-    os << endl;
-  }
-
-  // Write the points
-  for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
-    Point3 point = data.tracks[j].p;
-    os << point.x() << endl;
-    os << point.y() << endl;
-    os << point.z() << endl;
-    os << endl;
-  }
-
-  os.close();
-  return true;
-}
-
-bool writeBALfromValues(const string &filename, const SfmData &data,
-                        Values &values) {
-  using Camera = PinholeCamera<Cal3Bundler>;
-  SfmData dataValues = data;
-
-  // Store poses or cameras in SfmData
-  size_t nrPoses = values.count<Pose3>();
-  if (nrPoses ==
-      dataValues.number_cameras()) { // we only estimated camera poses
-    for (size_t i = 0; i < dataValues.number_cameras();
-         i++) { // for each camera
-      Pose3 pose = values.at<Pose3>(X(i));
-      Cal3Bundler K = dataValues.cameras[i].calibration();
-      Camera camera(pose, K);
-      dataValues.cameras[i] = camera;
-    }
-  } else {
-    size_t nrCameras = values.count<Camera>();
-    if (nrCameras == dataValues.number_cameras()) { // we only estimated camera
-                                                    // poses and calibration
-      for (size_t i = 0; i < nrCameras; i++) {      // for each camera
-        Key cameraKey = i;                          // symbol('c',i);
-        Camera camera = values.at<Camera>(cameraKey);
-        dataValues.cameras[i] = camera;
-      }
-    } else {
-      cout << "writeBALfromValues: different number of cameras in "
-              "SfM_dataValues (#cameras "
-           << dataValues.number_cameras() << ") and values (#cameras "
-           << nrPoses << ", #poses " << nrCameras << ")!!" << endl;
-      return false;
-    }
-  }
-
-  // Store 3D points in SfmData
-  size_t nrPoints = values.count<Point3>(),
-         nrTracks = dataValues.number_tracks();
-  if (nrPoints != nrTracks) {
-    cout << "writeBALfromValues: different number of points in "
-            "SfM_dataValues (#points= "
-         << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl;
-  }
-
-  for (size_t j = 0; j < nrTracks; j++) { // for each point
-    Key pointKey = P(j);
-    if (values.exists(pointKey)) {
-      Point3 point = values.at<Point3>(pointKey);
-      dataValues.tracks[j].p = point;
-    } else {
-      dataValues.tracks[j].r = 1.0;
-      dataValues.tracks[j].g = 0.0;
-      dataValues.tracks[j].b = 0.0;
-      dataValues.tracks[j].p = Point3(0, 0, 0);
-    }
-  }
-
-  // Write SfmData to file
-  return writeBAL(filename, dataValues);
-}
-
-Values initialCamerasEstimate(const SfmData &db) {
-  Values initial;
-  size_t i = 0; // NO POINTS:  j = 0;
-  for (const SfmCamera &camera : db.cameras)
-    initial.insert(i++, camera);
-  return initial;
-}
-
-Values initialCamerasAndPointsEstimate(const SfmData &db) {
-  Values initial;
-  size_t i = 0, j = 0;
-  for (const SfmCamera &camera : db.cameras)
-    initial.insert((i++), camera);
-  for (const SfmTrack &track : db.tracks)
-    initial.insert(P(j++), track.p);
-  return initial;
-}
-
 // Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
 BetweenFactorPose2s
 parse2DFactors(const std::string &filename,

gtsam/slam/dataset.h

@@ -22,6 +22,7 @@
 
 #include <gtsam/sfm/BinaryMeasurement.h>
 #include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/sfm/SfmData.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose2.h>
@@ -167,10 +168,11 @@ GTSAM_EXPORT GraphAndValues load2D(
  * @param kernelFunctionType whether to wrap the noise model in a robust kernel
  * @return graph and initial values
  */
-GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
+GTSAM_EXPORT GraphAndValues
-    SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise =
+load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
-        false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
+       size_t maxIndex = 0, bool addNoise = false, bool smart = true,
-    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
+       NoiseFormat noiseFormat = NoiseFormatAUTO,  //
+       KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
 
 /** save 2d graph */
 GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
@@ -185,8 +187,9 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
  * @param kernelFunctionType whether to wrap the noise model in a robust kernel
  * @return graph and initial values
  */
-GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D = false,
+GTSAM_EXPORT GraphAndValues
-    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
+readG2o(const std::string& g2oFile, const bool is3D = false,
+        KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
 
 /**
  * @brief This function writes a g2o file from
@@ -206,286 +209,6 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
 /// Load TORO 3D Graph
 GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
 
-/// A measurement with its camera index
-typedef std::pair<size_t, Point2> SfmMeasurement;
-
-/// Sift index for SfmTrack
-typedef std::pair<size_t, size_t> SiftIndex;
-
-/// Define the structure for the 3D points
-struct SfmTrack {
-  SfmTrack(float r = 0, float g = 0, float b = 0): p(0,0,0), r(r), g(g), b(b) {}
-  SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, float b = 0) : p(pt), r(r), g(g), b(b) {}
- 
-  Point3 p; ///< 3D position of the point
-  float r, g, b; ///< RGB color of the 3D point
-  std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
-  std::vector<SiftIndex> siftIndices;
-  
-  /// Get RGB values describing 3d point
-  const Point3 rgb() const { return Point3(r, g, b); }
-
-  /// Total number of measurements in this track
-  size_t number_measurements() const {
-    return measurements.size();
-  }
-  /// Get the measurement (camera index, Point2) at pose index `idx`
-  SfmMeasurement measurement(size_t idx) const {
-    return measurements[idx];
-  }
-  /// Get the SIFT feature index corresponding to the measurement at `idx`
-  SiftIndex siftIndex(size_t idx) const {
-    return siftIndices[idx];
-  }
-  /// Get 3D point
-  const Point3& point3() const {
-    return p;
-  }
-  /// Add measurement (camera_idx, Point2) to track
-  void add_measurement(size_t idx, const gtsam::Point2& m) {
-    measurements.emplace_back(idx, m);
-  }
-
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_NVP(p);
-    ar & BOOST_SERIALIZATION_NVP(r);
-    ar & BOOST_SERIALIZATION_NVP(g);
-    ar & BOOST_SERIALIZATION_NVP(b);
-    ar & BOOST_SERIALIZATION_NVP(measurements);
-    ar & BOOST_SERIALIZATION_NVP(siftIndices);
-  }
-
-  /// assert equality up to a tolerance
-  bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const {
-    // check the 3D point
-    if (!p.isApprox(sfmTrack.p)) {
-      return false;
-    }
-
-    // check the RGB values
-    if (r!=sfmTrack.r || g!=sfmTrack.g || b!=sfmTrack.b) {
-      return false;
-    }
-
-    // compare size of vectors for measurements and siftIndices
-    if (number_measurements() != sfmTrack.number_measurements() ||
-        siftIndices.size() != sfmTrack.siftIndices.size()) {
-      return false;
-    }
-
-    // compare measurements (order sensitive)
-    for (size_t idx = 0; idx < number_measurements(); ++idx) {
-      SfmMeasurement measurement = measurements[idx];
-      SfmMeasurement otherMeasurement = sfmTrack.measurements[idx];
-
-      if (measurement.first != otherMeasurement.first ||
-          !measurement.second.isApprox(otherMeasurement.second)) {
-        return false;
-      }
-    }
-
-    // compare sift indices (order sensitive)
-    for (size_t idx = 0; idx < siftIndices.size(); ++idx) {
-      SiftIndex index = siftIndices[idx];
-      SiftIndex otherIndex = sfmTrack.siftIndices[idx];
-
-      if (index.first != otherIndex.first ||
-          index.second != otherIndex.second) {
-        return false;
-      }
-    }
-
-    return true;
-  }
-
-  /// print
-  void print(const std::string& s = "") const {
-    std::cout << "Track with " << measurements.size();
-    std::cout << " measurements of point " << p << std::endl;
-  }
-};
-
-/* ************************************************************************* */
-/// traits
-template<>
-struct traits<SfmTrack> : public Testable<SfmTrack> {
-};
-
-
-/// Define the structure for the camera poses
-typedef PinholeCamera<Cal3Bundler> SfmCamera;
-
-/// Define the structure for SfM data
-struct SfmData {
-  std::vector<SfmCamera> cameras; ///< Set of cameras
-  std::vector<SfmTrack> tracks; ///< Sparse set of points
-  size_t number_cameras() const {
-    return cameras.size();
-  }
-  /// The number of reconstructed 3D points
-  size_t number_tracks() const {
-    return tracks.size();
-  }
-  /// The camera pose at frame index `idx`
-  SfmCamera camera(size_t idx) const {
-    return cameras[idx];
-  }
-  /// The track formed by series of landmark measurements
-  SfmTrack track(size_t idx) const {
-    return tracks[idx];
-  }
-  /// Add a track to SfmData
-  void add_track(const SfmTrack& t) {
-    tracks.push_back(t);
-  }
-  /// Add a camera to SfmData
-  void add_camera(const SfmCamera& cam) {
-    cameras.push_back(cam);
-  }
-
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_NVP(cameras);
-    ar & BOOST_SERIALIZATION_NVP(tracks);
-  }
-
-  /// @}
-  /// @name Testable
-  /// @{
-
-  /// assert equality up to a tolerance
-  bool equals(const SfmData &sfmData, double tol = 1e-9) const {
-    // check number of cameras and tracks
-    if (number_cameras() != sfmData.number_cameras() ||
-        number_tracks() != sfmData.number_tracks()) {
-      return false;
-    }
-
-    // check each camera
-    for (size_t i = 0; i < number_cameras(); ++i) {
-      if (!camera(i).equals(sfmData.camera(i), tol)) {
-        return false;
-      }
-    }
-
-    // check each track
-    for (size_t j = 0; j < number_tracks(); ++j) {
-      if (!track(j).equals(sfmData.track(j), tol)) {
-        return false;
-      }
-    }
-
-    return true;
-  }
-
-  /// print
-  void print(const std::string& s = "") const {
-    std::cout << "Number of cameras = " << number_cameras() << std::endl;
-    std::cout << "Number of tracks = " << number_tracks() << std::endl;
-  }
-};
-
-/* ************************************************************************* */
-/// traits
-template<>
-struct traits<SfmData> : public Testable<SfmData> {
-};
-
-/**
- * @brief This function parses a bundler output file and stores the data into a
- * SfmData structure
- * @param filename The name of the bundler file
- * @param data SfM structure where the data is stored
- * @return true if the parsing was successful, false otherwise
- */
-GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData &data);
-
-/**
- * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
- * SfmData structure
- * @param filename The name of the BAL file
- * @param data SfM structure where the data is stored
- * @return true if the parsing was successful, false otherwise
- */
-GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data);
-
-/**
- * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data
- * as a SfmData structure. Mainly used by wrapped code.
- * @param filename The name of the BAL file.
- * @return SfM structure where the data is stored.
- */
-GTSAM_EXPORT SfmData readBal(const std::string& filename);
-
-/**
- * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
- * SfmData structure
- * @param filename The name of the BAL file to write
- * @param data SfM structure where the data is stored
- * @return true if the parsing was successful, false otherwise
- */
-GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData &data);
-
-/**
- * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
- * SfmData structure and a value structure (measurements are the same as the SfM input data,
- * while camera poses and values are read from Values)
- * @param filename The name of the BAL file to write
- * @param data SfM structure where the data is stored
- * @param values structure where the graph values are stored (values can be either Pose3 or PinholeCamera<Cal3Bundler> for the
- * cameras, and should be Point3 for the 3D points). Note that the current version
- * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
- * @return true if the parsing was successful, false otherwise
- */
-GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
-    const SfmData &data, Values& values);
-
-/**
- * @brief This function converts an openGL camera pose to an GTSAM camera pose
- * @param R rotation in openGL
- * @param tx x component of the translation in openGL
- * @param ty y component of the translation in openGL
- * @param tz z component of the translation in openGL
- * @return Pose3 in GTSAM format
- */
-GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);
-
-/**
- * @brief This function converts a GTSAM camera pose to an openGL camera pose
- * @param R rotation in GTSAM
- * @param tx x component of the translation in GTSAM
- * @param ty y component of the translation in GTSAM
- * @param tz z component of the translation in GTSAM
- * @return Pose3 in openGL format
- */
-GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
-
-/**
- * @brief This function converts a GTSAM camera pose to an openGL camera pose
- * @param PoseGTSAM pose in GTSAM format
- * @return Pose3 in openGL format
- */
-GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
-
-/**
- * @brief This function creates initial values for cameras from db
- * @param SfmData
- * @return Values
- */
-GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
-
-/**
- * @brief This function creates initial values for cameras and points from db
- * @param SfmData
- * @return Values
- */
-GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
-
 // Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
 using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
 GTSAM_EXPORT BetweenFactorPose2s

gtsam/slam/slam.i

@@ -209,61 +209,27 @@ virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor {
 
 #include <gtsam/slam/dataset.h>
 
-class SfmTrack {
+enum NoiseFormat {
-  SfmTrack();
+  NoiseFormatG2O,
-  SfmTrack(const gtsam::Point3& pt);
+  NoiseFormatTORO,
-  const Point3& point3() const;
+  NoiseFormatGRAPH,
-
+  NoiseFormatCOV,
-  double r;
+  NoiseFormatAUTO
-  double g;
-  double b;
-
-  std::vector<pair<size_t, gtsam::Point2>> measurements;
-
-  size_t number_measurements() const;
-  pair<size_t, gtsam::Point2> measurement(size_t idx) const;
-  pair<size_t, size_t> siftIndex(size_t idx) const;
-  void add_measurement(size_t idx, const gtsam::Point2& m);
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enabling function to compare objects
-  bool equals(const gtsam::SfmTrack& expected, double tol) const;
 };
 
-class SfmData {
+enum KernelFunctionType {
-  SfmData();
+  KernelFunctionTypeNONE,
-  size_t number_cameras() const;
+  KernelFunctionTypeHUBER,
-  size_t number_tracks() const;
+  KernelFunctionTypeTUKEY
-  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
-  gtsam::SfmTrack track(size_t idx) const;
-  void add_track(const gtsam::SfmTrack& t);
-  void add_camera(const gtsam::SfmCamera& cam);
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // enabling function to compare objects
-  bool equals(const gtsam::SfmData& expected, double tol) const;
 };
 
-gtsam::SfmData readBal(string filename);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
-bool writeBAL(string filename, gtsam::SfmData& data);
+    string filename, gtsam::noiseModel::Diagonal* model = nullptr,
-gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
+    size_t maxIndex = 0, bool addNoise = false, bool smart = true,
-gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
+    gtsam::NoiseFormat noiseFormat = gtsam::NoiseFormat::NoiseFormatAUTO,
+    gtsam::KernelFunctionType kernelFunctionType =
+        gtsam::KernelFunctionType::KernelFunctionTypeNONE);
 
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
-    string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
-    bool addNoise, bool smart);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
-    string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
-    bool addNoise);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
-    string filename, gtsam::noiseModel::Diagonal* model, int maxIndex);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
-    string filename, gtsam::noiseModel::Diagonal* model);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
 void save2D(const gtsam::NonlinearFactorGraph& graph,
             const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
             string filename);
@@ -290,9 +256,10 @@ gtsam::BetweenFactorPose3s parse3DFactors(string filename);
 
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
 
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename,
+    string filename, const bool is3D = false,
-                                                           bool is3D);
+    gtsam::KernelFunctionType kernelFunctionType =
+        gtsam::KernelFunctionType::KernelFunctionTypeNONE);
 void writeG2o(const gtsam::NonlinearFactorGraph& graph,
               const gtsam::Values& estimate, string filename);
 
@@ -323,6 +290,8 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
   KarcherMeanFactor(const gtsam::KeyVector& keys);
 };
 
+gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations);
+
 #include <gtsam/slam/FrobeniusFactor.h>
 gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
                                                 size_t d);

gtsam/slam/tests/testDataset.cpp

@@ -151,27 +151,6 @@ TEST(dataSet, load2DVictoriaPark) {
   EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]);
 }
 
-/* ************************************************************************* */
-TEST( dataSet, Balbianello)
-{
-  ///< The structure where we will save the SfM data
-  const string filename = findExampleDataFile("Balbianello");
-  SfmData mydata;
-  CHECK(readBundler(filename, mydata));
-
-  // Check number of things
-  EXPECT_LONGS_EQUAL(5,mydata.number_cameras());
-  EXPECT_LONGS_EQUAL(544,mydata.number_tracks());
-  const SfmTrack& track0 = mydata.tracks[0];
-  EXPECT_LONGS_EQUAL(3,track0.number_measurements());
-
-  // Check projection of a given point
-  EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
-  const SfmCamera& camera0 = mydata.cameras[0];
-  Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
-  EXPECT(assert_equal(expected,actual,1));
-}
-
 /* ************************************************************************* */
 TEST(dataSet, readG2o3D) {
   const string g2oFile = findExampleDataFile("pose3example");
@@ -461,160 +440,6 @@ TEST( dataSet, writeG2o3DNonDiagonalNoise)
   EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
 }
 
-/* ************************************************************************* */
-TEST( dataSet, readBAL_Dubrovnik)
-{
-  ///< The structure where we will save the SfM data
-  const string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  SfmData mydata;
-  CHECK(readBAL(filename, mydata));
-
-  // Check number of things
-  EXPECT_LONGS_EQUAL(3,mydata.number_cameras());
-  EXPECT_LONGS_EQUAL(7,mydata.number_tracks());
-  const SfmTrack& track0 = mydata.tracks[0];
-  EXPECT_LONGS_EQUAL(3,track0.number_measurements());
-
-  // Check projection of a given point
-  EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
-  const SfmCamera& camera0 = mydata.cameras[0];
-  Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
-  EXPECT(assert_equal(expected,actual,12));
-}
-
-/* ************************************************************************* */
-TEST( dataSet, openGL2gtsam)
-{
-  Vector3 rotVec(0.2, 0.7, 1.1);
-  Rot3 R = Rot3::Expmap(rotVec);
-  Point3 t = Point3(0.0,0.0,0.0);
-  Pose3 poseGTSAM = Pose3(R,t);
-
-  Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z());
-
-  Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns!
-  Rot3 cRw(
-      r1.x(),  r2.x(),  r3.x(),
-     -r1.y(), -r2.y(), -r3.y(),
-      -r1.z(), -r2.z(), -r3.z());
-  Rot3 wRc = cRw.inverse();
-  Pose3 actual = Pose3(wRc,t);
-
-  EXPECT(assert_equal(expected,actual));
-}
-
-/* ************************************************************************* */
-TEST( dataSet, gtsam2openGL)
-{
-  Vector3 rotVec(0.2, 0.7, 1.1);
-  Rot3 R = Rot3::Expmap(rotVec);
-  Point3 t = Point3(1.0,20.0,10.0);
-  Pose3 actual = Pose3(R,t);
-  Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z());
-
-  Pose3 expected = gtsam2openGL(poseGTSAM);
-  EXPECT(assert_equal(expected,actual));
-}
-
-/* ************************************************************************* */
-TEST( dataSet, writeBAL_Dubrovnik)
-{
-  ///< Read a file using the unit tested readBAL
-  const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
-  SfmData readData;
-  readBAL(filenameToRead, readData);
-
-  // Write readData to file filenameToWrite
-  const string filenameToWrite = createRewrittenFileName(filenameToRead);
-  CHECK(writeBAL(filenameToWrite, readData));
-
-  // Read what we wrote
-  SfmData writtenData;
-  CHECK(readBAL(filenameToWrite, writtenData));
-
-  // Check that what we read is the same as what we wrote
-  EXPECT_LONGS_EQUAL(readData.number_cameras(),writtenData.number_cameras());
-  EXPECT_LONGS_EQUAL(readData.number_tracks(),writtenData.number_tracks());
-
-  for (size_t i = 0; i < readData.number_cameras(); i++){
-    PinholeCamera<Cal3Bundler> expectedCamera = writtenData.cameras[i];
-    PinholeCamera<Cal3Bundler> actualCamera = readData.cameras[i];
-    EXPECT(assert_equal(expectedCamera,actualCamera));
-  }
-
-  for (size_t j = 0; j < readData.number_tracks(); j++){
-    // check point
-    SfmTrack expectedTrack  = writtenData.tracks[j];
-    SfmTrack actualTrack = readData.tracks[j];
-    Point3 expectedPoint = expectedTrack.p;
-    Point3 actualPoint = actualTrack.p;
-    EXPECT(assert_equal(expectedPoint,actualPoint));
-
-    // check rgb
-    Point3 expectedRGB = Point3( expectedTrack.r,  expectedTrack.g, expectedTrack.b );
-    Point3 actualRGB = Point3(  actualTrack.r,  actualTrack.g, actualTrack.b);
-    EXPECT(assert_equal(expectedRGB,actualRGB));
-
-    // check measurements
-    for (size_t k = 0; k < actualTrack.number_measurements(); k++){
-      EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first);
-      EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second));
-    }
-  }
-}
-
-
-/* ************************************************************************* */
-TEST( dataSet, writeBALfromValues_Dubrovnik){
-
-  ///< Read a file using the unit tested readBAL
-  const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
-  SfmData readData;
-  readBAL(filenameToRead, readData);
-
-  Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.3,0.1,0.3));
-
-  Values value;
-  for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera
-    Pose3 pose = poseChange.compose(readData.cameras[i].pose());
-    value.insert(X(i), pose);
-  }
-  for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
-    Point3 point = poseChange.transformFrom( readData.tracks[j].p );
-    value.insert(P(j), point);
-  }
-
-  // Write values and readData to a file
-  const string filenameToWrite = createRewrittenFileName(filenameToRead);
-  writeBALfromValues(filenameToWrite, readData, value);
-
-  // Read the file we wrote
-  SfmData writtenData;
-  readBAL(filenameToWrite, writtenData);
-
-  // Check that the reprojection errors are the same and the poses are correct
-  // Check number of things
-  EXPECT_LONGS_EQUAL(3,writtenData.number_cameras());
-  EXPECT_LONGS_EQUAL(7,writtenData.number_tracks());
-  const SfmTrack& track0 = writtenData.tracks[0];
-  EXPECT_LONGS_EQUAL(3,track0.number_measurements());
-
-  // Check projection of a given point
-  EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
-  const SfmCamera& camera0 = writtenData.cameras[0];
-  Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
-  EXPECT(assert_equal(expected,actual,12));
-
-  Pose3 expectedPose = camera0.pose();
-  Pose3 actualPose = value.at<Pose3>(X(0));
-  EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
-
-  Point3 expectedPoint = track0.p;
-  Point3 actualPoint = value.at<Point3>(P(0));
-  EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
-}
-
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -15,6 +15,7 @@
 #include <gtsam/nonlinear/expressionTesting.h>
 #include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/slam/EssentialMatrixFactor.h>
+#include <gtsam/sfm/SfmData.h>
 #include <gtsam/slam/dataset.h>
 
 using namespace std::placeholders;
@@ -34,8 +35,7 @@ gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse();
 namespace example1 {
 
 const string filename = findExampleDataFile("18pointExample1.txt");
-SfmData data;
+SfmData data = SfmData::FromBalFile(filename);
-bool readOK = readBAL(filename, data);
 Rot3 c1Rc2 = data.cameras[1].pose().rotation();
 Point3 c1Tc2 = data.cameras[1].pose().translation();
 // TODO: maybe default value not good; assert with 0th
@@ -53,8 +53,6 @@ Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); }
 
 //*************************************************************************
 TEST(EssentialMatrixFactor, testData) {
-  CHECK(readOK);
-
   // Check E matrix
   Matrix expected(3, 3);
   expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
@@ -538,8 +536,7 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) {
 namespace example2 {
 
 const string filename = findExampleDataFile("5pointExample2.txt");
-SfmData data;
+SfmData data = SfmData::FromBalFile(filename);
-bool readOK = readBAL(filename, data);
 Rot3 aRb = data.cameras[1].pose().rotation();
 Point3 aTb = data.cameras[1].pose().translation();
 EssentialMatrix trueE(aRb, Unit3(aTb));
@@ -632,14 +629,14 @@ TEST(EssentialMatrixFactor2, extraMinimization) {
   // We start with a factor graph and add constraints to it
   // Noise sigma is 1, assuming pixel measurements
   NonlinearFactorGraph graph;
-  for (size_t i = 0; i < data.number_tracks(); i++)
+  for (size_t i = 0; i < data.numberTracks(); i++)
     graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2,
                                                  K);
 
   // Check error at ground truth
   Values truth;
   truth.insert(100, trueE);
-  for (size_t i = 0; i < data.number_tracks(); i++) {
+  for (size_t i = 0; i < data.numberTracks(); i++) {
     Point3 P1 = data.tracks[i].p;
     truth.insert(i, double(baseline / P1.z()));
   }
@@ -654,7 +651,7 @@ TEST(EssentialMatrixFactor2, extraMinimization) {
   // Check result
   EssentialMatrix actual = result.at<EssentialMatrix>(100);
   EXPECT(assert_equal(trueE, actual, 1e-1));
-  for (size_t i = 0; i < data.number_tracks(); i++)
+  for (size_t i = 0; i < data.numberTracks(); i++)
     EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
 
   // Check error at result

gtsam/slam/tests/testSerializationDataset.cpp

@@ -16,6 +16,7 @@
  * @date Jan 1, 2021
  */
 
+#include <gtsam/sfm/SfmData.h>
 #include <gtsam/slam/dataset.h>
 
 #include <gtsam/base/serializationTestHelpers.h>
@@ -29,8 +30,7 @@ using namespace gtsam::serializationTestHelpers;
 TEST(dataSet, sfmDataSerialization) {
   // Test the serialization of SfmData
   const string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  SfmData mydata;
+  SfmData mydata = SfmData::FromBalFile(filename);
-  CHECK(readBAL(filename, mydata));
 
   // round-trip equality check on serialization and subsequent deserialization
   EXPECT(equalsObj(mydata));
@@ -42,8 +42,7 @@ TEST(dataSet, sfmDataSerialization) {
 TEST(dataSet, sfmTrackSerialization) {
   // Test the serialization of SfmTrack
   const string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  SfmData mydata;
+  SfmData mydata = SfmData::FromBalFile(filename);
-  CHECK(readBAL(filename, mydata));
 
   SfmTrack track = mydata.track(0);
 

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>

gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h

@@ -113,6 +113,9 @@ public:
   /// 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 */

matlab/+gtsam/VisualISAMInitialize.m

@@ -7,7 +7,7 @@ import gtsam.*
 %% Initialize iSAM
 params = gtsam.ISAM2Params;
 if options.alwaysRelinearize
-    params.setRelinearizeSkip(1);
+    params.relinearizeSkip = 1;
 end
 isam = ISAM2(params);
 

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

matlab/gtsam_examples/IMUKittiExampleGPS.m

@@ -52,7 +52,7 @@ IMU_params.setOmegaCoriolis(w_coriolis);
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
-isamParams.setRelinearizeSkip(10);
+isamParams.relinearizeSkip = 10;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;

matlab/unstable_examples/+imuSimulator/IMUComparison.m

@@ -46,7 +46,7 @@ posesIMUbody(1).R = poses(1).R;
 
 %% Solver object
 isamParams = ISAM2Params;
-isamParams.setRelinearizeSkip(1);
+isamParams.relinearizeSkip = 1;
 isam = gtsam.ISAM2(isamParams);
 
 initialValues = Values;

matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m

@@ -34,7 +34,7 @@ poses(1).R = currentPoseGlobal.rotation.matrix;
 
 %% Solver object
 isamParams = ISAM2Params;
-isamParams.setRelinearizeSkip(1);
+isamParams.relinearizeSkip = 1;
 isam = gtsam.ISAM2(isamParams);
 
 sigma_init_x = 1.0;

matlab/unstable_examples/+imuSimulator/coriolisExample.m

@@ -119,7 +119,7 @@ h = figure;
 % Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
-isamParams.setRelinearizeSkip(10);
+isamParams.relinearizeSkip = 10;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;

matlab/unstable_examples/IMUKittiExampleAdvanced.m

@@ -82,7 +82,7 @@ w_coriolis = [0;0;0];
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('QR');
-isamParams.setRelinearizeSkip(1);
+isamParams.relinearizeSkip = 1;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;

matlab/unstable_examples/IMUKittiExampleVO.m

@@ -58,7 +58,7 @@ w_coriolis = [0;0;0];
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
-isamParams.setRelinearizeSkip(10);
+isamParams.relinearizeSkip = 10;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;

python/CMakeLists.txt

@@ -45,6 +45,7 @@ set(ignore
     gtsam::Point3Pairs
     gtsam::Pose3Pairs
     gtsam::Pose3Vector
+    gtsam::Rot3Vector
     gtsam::KeyVector
     gtsam::BinaryMeasurementsUnit3
     gtsam::DiscreteKey

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)
 

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.

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.

python/gtsam/examples/SFMExample.py

@@ -8,7 +8,6 @@ See LICENSE for the license information
 
 A structure-from-motion problem on a simulated dataset
 """
-from __future__ import print_function
 
 import gtsam
 import matplotlib.pyplot as plt
@@ -89,7 +88,7 @@ def main():
     point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
     factor = PriorFactorPoint3(L(0), points[0], point_noise)
     graph.push_back(factor)
-    graph.print_('Factor Graph:\n')
+    graph.print('Factor Graph:\n')
 
     # Create the data structure to hold the initial estimate to the solution
     # Intentionally initialize the variables off from the ground truth
@@ -100,7 +99,7 @@ def main():
     for j, point in enumerate(points):
         transformed_point = point + 0.1*np.random.randn(3)
         initial_estimate.insert(L(j), transformed_point)
-    initial_estimate.print_('Initial Estimates:\n')
+    initial_estimate.print('Initial Estimates:\n')
 
     # Optimize the graph and print results
     params = gtsam.DoglegParams()
@@ -108,7 +107,7 @@ def main():
     optimizer = DoglegOptimizer(graph, initial_estimate, params)
     print('Optimizing:')
     result = optimizer.optimize()
-    result.print_('Final results:\n')
+    result.print('Final results:\n')
     print('initial error = {}'.format(graph.error(initial_estimate)))
     print('final error = {}'.format(graph.error(result)))
 

python/gtsam/examples/SFMExample_bal.py

@@ -15,9 +15,9 @@ import logging
 import sys
 
 import gtsam
-from gtsam import (GeneralSFMFactorCal3Bundler,
+from gtsam import (GeneralSFMFactorCal3Bundler, SfmData,
                    PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3)
-from gtsam.symbol_shorthand import C, P  # type: ignore
+from gtsam.symbol_shorthand import P  # type: ignore
 from gtsam.utils import plot  # type: ignore
 from matplotlib import pyplot as plt
 
@@ -26,13 +26,12 @@ logging.basicConfig(stream=sys.stdout, level=logging.INFO)
 DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre"
 
 
-def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None:
+def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None:
     """Plot the SFM results."""
     plot_vals = gtsam.Values()
-    for cam_idx in range(scene_data.number_cameras()):
+    for i in range(scene_data.numberCameras()):
-        plot_vals.insert(C(cam_idx),
+        plot_vals.insert(i, result.atPinholeCameraCal3Bundler(i).pose())
-                         result.atPinholeCameraCal3Bundler(C(cam_idx)).pose())
+    for j in range(scene_data.numberTracks()):
-    for j in range(scene_data.number_tracks()):
         plot_vals.insert(P(j), result.atPoint3(P(j)))
 
     plot.plot_3d_points(0, plot_vals, linespec="g.")
@@ -46,9 +45,9 @@ def run(args: argparse.Namespace) -> None:
     input_file = args.input_file
 
     # Load the SfM data from file
-    scene_data = gtsam.readBal(input_file)
+    scene_data = SfmData.FromBalFile(input_file)
-    logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(),
+    logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(),
-                 scene_data.number_cameras())
+                 scene_data.numberCameras())
 
     # Create a factor graph
     graph = gtsam.NonlinearFactorGraph()
@@ -57,19 +56,19 @@ def run(args: argparse.Namespace) -> None:
     noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)  # one pixel in u and v
 
     # Add measurements to the factor graph
-    for j in range(scene_data.number_tracks()):
+    for j in range(scene_data.numberTracks()):
         track = scene_data.track(j)  # SfmTrack
         # retrieve the SfmMeasurement objects
-        for m_idx in range(track.number_measurements()):
+        for m_idx in range(track.numberMeasurements()):
             # i represents the camera index, and uv is the 2d measurement
             i, uv = track.measurement(m_idx)
             # note use of shorthand symbols C and P
-            graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
+            graph.add(GeneralSFMFactorCal3Bundler(uv, noise, i, P(j)))
 
     # Add a prior on pose x1. This indirectly specifies where the origin is.
     graph.push_back(
         PriorFactorPinholeCameraCal3Bundler(
-            C(0), scene_data.camera(0),
+            0, scene_data.camera(0),
             gtsam.noiseModel.Isotropic.Sigma(9, 0.1)))
     # Also add a prior on the position of the first landmark to fix the scale
     graph.push_back(
@@ -82,13 +81,13 @@ def run(args: argparse.Namespace) -> None:
 
     i = 0
     # add each PinholeCameraCal3Bundler
-    for cam_idx in range(scene_data.number_cameras()):
+    for i in range(scene_data.numberCameras()):
-        camera = scene_data.camera(cam_idx)
+        camera = scene_data.camera(i)
-        initial.insert(C(i), camera)
+        initial.insert(i, camera)
         i += 1
 
     # add each SfmTrack
-    for j in range(scene_data.number_tracks()):
+    for j in range(scene_data.numberTracks()):
         track = scene_data.track(j)
         initial.insert(P(j), track.point3())
 

python/gtsam/examples/SimpleRotation.py

@@ -31,7 +31,7 @@ def main():
     - A measurement model with the correct dimensionality for the factor
     """
     prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
-    prior.print_('goal angle')
+    prior.print('goal angle')
     model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
     key = X(1)
     factor = gtsam.PriorFactorRot2(key, prior, model)
@@ -48,7 +48,7 @@ def main():
     """
     graph = gtsam.NonlinearFactorGraph()
     graph.push_back(factor)
-    graph.print_('full graph')
+    graph.print('full graph')
 
     """
     Step 3: Create an initial estimate
@@ -65,7 +65,7 @@ def main():
     """
     initial = gtsam.Values()
     initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
-    initial.print_('initial estimate')
+    initial.print('initial estimate')
 
     """
     Step 4: Optimize
@@ -77,7 +77,7 @@ def main():
     with the final state of the optimization.
     """
     result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
-    result.print_('final result')
+    result.print('final result')
 
 
 if __name__ == '__main__':

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

python/gtsam/examples/VisualISAMExample.py

@@ -10,8 +10,6 @@ A visualSLAM example for the structure-from-motion problem on a simulated datase
 This version uses iSAM to solve the problem incrementally
 """
 
-from __future__ import print_function
-
 import numpy as np
 import gtsam
 from gtsam.examples import SFMdata
@@ -94,7 +92,7 @@ def main():
             current_estimate = isam.estimate()
             print('*' * 50)
             print('Frame {}:'.format(i))
-            current_estimate.print_('Current estimate: ')
+            current_estimate.print('Current estimate: ')
 
             # Clear the factor graph and values for the next iteration
             graph.resize(0)

python/gtsam/preamble/inference.h

@@ -11,5 +11,4 @@
  * mutations on Python side will not be reflected on C++.
  */
 
-#include <pybind11/stl.h>
+#include <pybind11/stl.h>
-

python/gtsam/preamble/slam.h

@@ -15,3 +15,4 @@ PYBIND11_MAKE_OPAQUE(
     std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
 PYBIND11_MAKE_OPAQUE(
     std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
+PYBIND11_MAKE_OPAQUE(gtsam::Rot3Vector);

python/gtsam/specializations/slam.h

@@ -12,8 +12,9 @@
  */
 
 py::bind_vector<
-    std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(
+    std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3>>>>(
     m_, "BetweenFactorPose3s");
 py::bind_vector<
-    std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(
+    std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>>>>(
     m_, "BetweenFactorPose2s");
+py::bind_vector<gtsam::Rot3Vector>(m_, "Rot3Vector");

python/gtsam/tests/test_DiscreteBayesNet.py

@@ -12,7 +12,9 @@ Author: Frank Dellaert
 # pylint: disable=no-name-in-module, invalid-name
 
 import unittest
+import textwrap
 
+import gtsam
 from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph,
                    DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering)
 from gtsam.utils.test_case import GtsamTestCase
@@ -126,6 +128,39 @@ class TestDiscreteBayesNet(GtsamTestCase):
         actual = fragment.sample(given)
         self.assertEqual(len(actual), 5)
 
+    def test_dot(self):
+        """Check that dot works with position hints."""
+        fragment = DiscreteBayesNet()
+        fragment.add(Either, [Tuberculosis, LungCancer], "F T T T")
+        MyAsia = gtsam.symbol('a', 0), 2  # use a symbol!
+        fragment.add(Tuberculosis, [MyAsia], "99/1 95/5")
+        fragment.add(LungCancer, [Smoking], "99/1 90/10")
+
+        # Make sure we can *update* position hints
+        writer = gtsam.DotWriter()
+        ph: dict = writer.positionHints
+        ph.update({'a': 2})  # hint at symbol position
+        writer.positionHints = ph
+
+        # Check the output of dot
+        actual = fragment.dot(writer=writer)
+        expected_result = """\
+            digraph {
+              size="5,5";
+
+              var3[label="3"];
+              var4[label="4"];
+              var5[label="5"];
+              var6[label="6"];
+              vara0[label="a0", pos="0,2!"];
+
+              var4->var6
+              vara0->var3
+              var3->var5
+              var6->var5
+            }"""
+        self.assertEqual(actual, textwrap.dedent(expected_result))
+
 
 if __name__ == "__main__":
     unittest.main()

python/gtsam/tests/test_GaussianBayesNet.py

@@ -0,0 +1,53 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for Gaussian Bayes Nets.
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam import GaussianBayesNet, GaussianConditional
+from gtsam.utils.test_case import GtsamTestCase
+
+# some keys
+_x_ = 11
+_y_ = 22
+_z_ = 33
+
+
+def smallBayesNet():
+    """Create a small Bayes Net for testing"""
+    bayesNet = GaussianBayesNet()
+    I_1x1 = np.eye(1, dtype=float)
+    bayesNet.push_back(GaussianConditional(
+        _x_, [9.0], I_1x1, _y_, I_1x1))
+    bayesNet.push_back(GaussianConditional(_y_, [5.0], I_1x1))
+    return bayesNet
+
+
+class TestGaussianBayesNet(GtsamTestCase):
+    """Tests for Gaussian Bayes nets."""
+
+    def test_matrix(self):
+        """Test matrix method"""
+        R, d = smallBayesNet().matrix()  # get matrix and RHS
+        R1 = np.array([
+            [1.0, 1.0],
+            [0.0, 1.0]])
+        d1 = np.array([9.0, 5.0])
+        np.testing.assert_equal(R, R1)
+        np.testing.assert_equal(d, d1)
+
+
+if __name__ == '__main__':
+    unittest.main()

python/gtsam/tests/test_GraphvizFormatting.py

@@ -78,7 +78,7 @@ class TestGraphvizFormatting(GtsamTestCase):
         graphviz_formatting.paperHorizontalAxis = gtsam.GraphvizFormatting.Axis.X
         graphviz_formatting.paperVerticalAxis = gtsam.GraphvizFormatting.Axis.Y
         self.assertEqual(self.graph.dot(self.values,
-                                        formatting=graphviz_formatting),
+                                        writer=graphviz_formatting),
                          textwrap.dedent(expected_result))
 
     def test_factor_points(self):
@@ -100,7 +100,7 @@ class TestGraphvizFormatting(GtsamTestCase):
         graphviz_formatting.plotFactorPoints = False
 
         self.assertEqual(self.graph.dot(self.values,
-                                        formatting=graphviz_formatting),
+                                        writer=graphviz_formatting),
                          textwrap.dedent(expected_result))
 
     def test_width_height(self):
@@ -127,7 +127,7 @@ class TestGraphvizFormatting(GtsamTestCase):
         graphviz_formatting.figureHeightInches = 10
 
         self.assertEqual(self.graph.dot(self.values,
-                                        formatting=graphviz_formatting),
+                                        writer=graphviz_formatting),
                          textwrap.dedent(expected_result))
 
 

python/gtsam/tests/test_KarcherMeanFactor.py

@@ -15,27 +15,15 @@ import unittest
 
 import gtsam
 import numpy as np
+from gtsam import Rot3
 from gtsam.utils.test_case import GtsamTestCase
 
 KEY = 0
 MODEL = gtsam.noiseModel.Unit.Create(3)
 
 
-def find_Karcher_mean_Rot3(rotations):
-    """Find the Karcher mean of given values."""
-    # Cost function C(R) = \sum PriorFactor(R_i)::error(R)
-    # No closed form solution.
-    graph = gtsam.NonlinearFactorGraph()
-    for R in rotations:
-        graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
-    initial = gtsam.Values()
-    initial.insert(KEY, gtsam.Rot3())
-    result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
-    return result.atRot3(KEY)
-
-
 # Rot3 version
-R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0]))
+R = Rot3.Expmap(np.array([0.1, 0, 0]))
 
 
 class TestKarcherMean(GtsamTestCase):
@@ -43,11 +31,23 @@ class TestKarcherMean(GtsamTestCase):
     def test_find(self):
         # Check that optimizing for Karcher mean (which minimizes Between distance)
         # gets correct result.
-        rotations = {R, R.inverse()}
+        rotations = gtsam.Rot3Vector([R, R.inverse()])
-        expected = gtsam.Rot3()
+        expected = Rot3()
-        actual = find_Karcher_mean_Rot3(rotations)
+        actual = gtsam.FindKarcherMean(rotations)
         self.gtsamAssertEquals(expected, actual)
 
+    def test_find_karcher_mean_identity(self):
+        """Averaging 3 identity rotations should yield the identity."""
+        a1Rb1 = Rot3()
+        a2Rb2 = Rot3()
+        a3Rb3 = Rot3()
+
+        aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3])
+        aRb_expected = Rot3()
+
+        aRb = gtsam.FindKarcherMean(aRb_list)
+        self.gtsamAssertEquals(aRb, aRb_expected)
+
     def test_factor(self):
         """Check that the InnerConstraint factor leaves the mean unchanged."""
         # Make a graph with two variables, one between, and one InnerConstraint
@@ -66,11 +66,11 @@ class TestKarcherMean(GtsamTestCase):
         initial = gtsam.Values()
         initial.insert(1, R.inverse())
         initial.insert(2, R)
-        expected = find_Karcher_mean_Rot3([R, R.inverse()])
+        expected = Rot3()
 
         result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
-        actual = find_Karcher_mean_Rot3(
+        actual = gtsam.FindKarcherMean(
-            [result.atRot3(1), result.atRot3(2)])
+            gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)]))
         self.gtsamAssertEquals(expected, actual)
         self.gtsamAssertEquals(
             R12, result.atRot3(1).between(result.atRot3(2)))

python/gtsam/tests/test_SfmData.py

@@ -14,9 +14,9 @@ from __future__ import print_function
 
 import unittest
 
-import numpy as np
-
 import gtsam
+import numpy as np
+from gtsam import Point2, Point3, SfmData, SfmTrack
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -25,55 +25,97 @@ class TestSfmData(GtsamTestCase):
 
     def setUp(self):
         """Initialize SfmData and SfmTrack"""
-        self.data = gtsam.SfmData()
+        self.data = SfmData()
         # initialize SfmTrack with 3D point
-        self.tracks = gtsam.SfmTrack()
+        self.tracks = SfmTrack()
 
     def test_tracks(self):
         """Test functions in SfmTrack"""
         # measurement is of format (camera_idx, imgPoint)
         # create arbitrary camera indices for two cameras
-        i1, i2 = 4,5
+        i1, i2 = 4, 5
+
         # create arbitrary image measurements for cameras i1 and i2
-        uv_i1 = gtsam.Point2(12.6, 82)
+        uv_i1 = Point2(12.6, 82)
+
         # translating point uv_i1 along X-axis
-        uv_i2 = gtsam.Point2(24.88, 82)
+        uv_i2 = Point2(24.88, 82)
+
         # add measurements to the track
-        self.tracks.add_measurement(i1, uv_i1)
+        self.tracks.addMeasurement(i1, uv_i1)
-        self.tracks.add_measurement(i2, uv_i2)
+        self.tracks.addMeasurement(i2, uv_i2)
+
         # Number of measurements in the track is 2
-        self.assertEqual(self.tracks.number_measurements(), 2)
+        self.assertEqual(self.tracks.numberMeasurements(), 2)
+
         # camera_idx in the first measurement of the track corresponds to i1
         cam_idx, img_measurement = self.tracks.measurement(0)
         self.assertEqual(cam_idx, i1)
         np.testing.assert_array_almost_equal(
-            gtsam.Point3(0.,0.,0.), 
+            Point3(0., 0., 0.),
             self.tracks.point3()
         )
 
-
     def test_data(self):
         """Test functions in SfmData"""
         # Create new track with 3 measurements
-        i1, i2, i3 = 3,5,6
+        i1, i2, i3 = 3, 5, 6
-        uv_i1 = gtsam.Point2(21.23, 45.64)
+        uv_i1 = Point2(21.23, 45.64)
+
         # translating along X-axis
-        uv_i2 = gtsam.Point2(45.7, 45.64)
+        uv_i2 = Point2(45.7, 45.64)
-        uv_i3 = gtsam.Point2(68.35, 45.64)
+        uv_i3 = Point2(68.35, 45.64)
+
         # add measurements and arbitrary point to the track
         measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
-        pt = gtsam.Point3(1.0, 6.0, 2.0)
+        pt = Point3(1.0, 6.0, 2.0)
-        track2 = gtsam.SfmTrack(pt)
+        track2 = SfmTrack(pt)
-        track2.add_measurement(i1, uv_i1)
+        track2.addMeasurement(i1, uv_i1)
-        track2.add_measurement(i2, uv_i2)
+        track2.addMeasurement(i2, uv_i2)
-        track2.add_measurement(i3, uv_i3)
+        track2.addMeasurement(i3, uv_i3)
-        self.data.add_track(self.tracks)
+        self.data.addTrack(self.tracks)
-        self.data.add_track(track2)
+        self.data.addTrack(track2)
+
         # Number of tracks in SfmData is 2
-        self.assertEqual(self.data.number_tracks(), 2)
+        self.assertEqual(self.data.numberTracks(), 2)
+
         # camera idx of first measurement of second track corresponds to i1
         cam_idx, img_measurement = self.data.track(1).measurement(0)
         self.assertEqual(cam_idx, i1)
 
+    def test_Balbianello(self):
+        """ Check that we can successfully read a bundler file and create a 
+            factor graph from it
+        """
+        # The structure where we will save the SfM data
+        filename = gtsam.findExampleDataFile("Balbianello.out")
+        sfm_data = SfmData.FromBundlerFile(filename)
+
+        # Check number of things
+        self.assertEqual(5, sfm_data.numberCameras())
+        self.assertEqual(544, sfm_data.numberTracks())
+        track0 = sfm_data.track(0)
+        self.assertEqual(3, track0.numberMeasurements())
+
+        # Check projection of a given point
+        self.assertEqual(0, track0.measurement(0)[0])
+        camera0 = sfm_data.camera(0)
+        expected = camera0.project(track0.point3())
+        actual = track0.measurement(0)[1]
+        self.gtsamAssertEquals(expected, actual, 1)
+
+        # We share *one* noiseModel between all projection factors
+        model = gtsam.noiseModel.Isotropic.Sigma(
+            2, 1.0)  # one pixel in u and v
+
+        # Convert to NonlinearFactorGraph
+        graph = sfm_data.sfmFactorGraph(model)
+        self.assertEqual(1419, graph.size())  # regression
+
+        # Get initial estimate
+        values = gtsam.initialCamerasAndPointsEstimate(sfm_data)
+        self.assertEqual(549, values.size())  # regression
+
+
 if __name__ == '__main__':
     unittest.main()

python/gtsam/tests/test_pickle.py

@@ -37,8 +37,8 @@ class TestPickle(GtsamTestCase):
 
     def test_sfmTrack_roundtrip(self):
         obj = SfmTrack(Point3(1, 1, 0))
-        obj.add_measurement(0, Point2(-1, 5))
+        obj.addMeasurement(0, Point2(-1, 5))
-        obj.add_measurement(1, Point2(6, 2))
+        obj.addMeasurement(1, Point2(6, 2))
         self.assertEqualityOnPickleRoundtrip(obj)
 
     def test_unit3_roundtrip(self):

python/gtsam/utils/visual_data_generator.py

@@ -1,12 +1,12 @@
 from __future__ import print_function
-from typing import Tuple
 
 import math
-import numpy as np
 from math import pi
+from typing import Tuple
 
 import gtsam
-from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2
+import numpy as np
+from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3
 
 
 class Options:
@@ -36,7 +36,7 @@ class GroundTruth:
         self.cameras = [Pose3()] * nrCameras
         self.points = [Point3(0, 0, 0)] * nrPoints
 
-    def print_(self, s="") -> None:
+    def print(self, s: str = "") -> None:
         print(s)
         print("K = ", self.K)
         print("Cameras: ", len(self.cameras))
@@ -88,7 +88,8 @@ def generate_data(options) -> Tuple[Data, GroundTruth]:
         r = 10
         for j in range(len(truth.points)):
             theta = j * 2 * pi / nrPoints
-            truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0)
+            truth.points[j] = Point3(
+                r * math.cos(theta), r * math.sin(theta), 0)
     else:  # 3D landmarks as vertices of a cube
         truth.points = [
             Point3(10, 10, 10), Point3(-10, 10, 10),

python/gtsam/utils/visual_isam.py

@@ -17,7 +17,7 @@ def initialize(data, truth, options):
     # Initialize iSAM
     params = gtsam.ISAM2Params()
     if options.alwaysRelinearize:
-        params.setRelinearizeSkip(1)
+        params.relinearizeSkip = 1
     isam = gtsam.ISAM2(params=params)
 
     # Add constraints/priors

tests/testGaussianBayesTreeB.cpp

@@ -112,54 +112,50 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
    C4      x7 : x6
 
 ************************************************************************* */
-TEST( GaussianBayesTree, balanced_smoother_marginals )
+TEST(GaussianBayesTree, balanced_smoother_marginals) {
-{
   // Create smoother with 7 nodes
   GaussianFactorGraph smoother = createSmoother(7);
 
   // Create the Bayes tree
   Ordering ordering;
-  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
+  ordering += X(1), X(3), X(5), X(7), X(2), X(6), X(4);
   GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
 
   VectorValues actualSolution = bayesTree.optimize();
   VectorValues expectedSolution = VectorValues::Zero(actualSolution);
-  EXPECT(assert_equal(expectedSolution,actualSolution,tol));
+  EXPECT(assert_equal(expectedSolution, actualSolution, tol));
 
-  LONGS_EQUAL(4, (long)bayesTree.size());
+  LONGS_EQUAL(4, bayesTree.size());
 
-  double tol=1e-5;
+  double tol = 1e-5;
 
   // Check marginal on x1
-  JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1);
   JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
-  Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1);
+  Matrix expectedCovX1 = I_2x2 * (sigmax1 * sigmax1);
-  Matrix actualCovarianceX1;
+  auto m = bayesTree.marginalFactor(X(1), EliminateCholesky);
-  GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky);
+  Matrix actualCovarianceX1 = m->information().inverse();
-  actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse();
+  EXPECT(assert_equal(expectedCovX1, actualCovarianceX1, tol));
-  EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
-  EXPECT(assert_equal(expected1,actual1,tol));
 
   // Check marginal on x2
-  double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
+  double sigmax2 = 0.68712938;  // FIXME: this should be corrected analytically
-  JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2);
   JacobianFactor actual2 = *bayesTree.marginalFactor(X(2));
-  EXPECT(assert_equal(expected2,actual2,tol));
+  Matrix expectedCovX2 = I_2x2 * (sigmax2 * sigmax2);
+  EXPECT(assert_equal(expectedCovX2, actual2.information().inverse(), tol));
 
   // Check marginal on x3
-  JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3);
   JacobianFactor actual3 = *bayesTree.marginalFactor(X(3));
-  EXPECT(assert_equal(expected3,actual3,tol));
+  Matrix expectedCovX3 = I_2x2 * (sigmax3 * sigmax3);
+  EXPECT(assert_equal(expectedCovX3, actual3.information().inverse(), tol));
 
   // Check marginal on x4
-  JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4);
   JacobianFactor actual4 = *bayesTree.marginalFactor(X(4));
-  EXPECT(assert_equal(expected4,actual4,tol));
+  Matrix expectedCovX4 = I_2x2 * (sigmax4 * sigmax4);
+  EXPECT(assert_equal(expectedCovX4, actual4.information().inverse(), tol));
 
   // Check marginal on x7 (should be equal to x1)
-  JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7);
   JacobianFactor actual7 = *bayesTree.marginalFactor(X(7));
-  EXPECT(assert_equal(expected7,actual7,tol));
+  Matrix expectedCovX7 = I_2x2 * (sigmax7 * sigmax7);
+  EXPECT(assert_equal(expectedCovX7, actual7.information().inverse(), tol));
 }
 
 /* ************************************************************************* */

tests/testGeneralSFMFactorB.cpp

@@ -15,6 +15,7 @@
  * @brief test general SFM class, with nonlinear optimization and BAL files
  */
 
+#include <gtsam/sfm/SfmData.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/geometry/Point3.h>
@@ -42,14 +43,12 @@ using symbol_shorthand::P;
 /* ************************************************************************* */
 TEST(PinholeCamera, BAL) {
   string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  SfmData db;
+  SfmData db = SfmData::FromBalFile(filename);
-  bool success = readBAL(filename, db);
-  if (!success) throw runtime_error("Could not access file!");
 
   SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
   NonlinearFactorGraph graph;
 
-  for (size_t j = 0; j < db.number_tracks(); j++) {
+  for (size_t j = 0; j < db.numberTracks(); j++) {
     for (const SfmMeasurement& m: db.tracks[j].measurements)
       graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j));
   }

tests/testTranslationRecovery.cpp

@@ -18,6 +18,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/sfm/TranslationRecovery.h>
+#include <gtsam/sfm/SfmData.h>
 #include <gtsam/slam/dataset.h>
 
 using namespace std;
@@ -42,9 +43,7 @@ Unit3 GetDirectionFromPoses(const Values& poses,
 // sets up an optimization problem for the three unknown translations.
 TEST(TranslationRecovery, BAL) {
   const string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  SfmData db;
+  SfmData db = SfmData::FromBalFile(filename);
-  bool success = readBAL(filename, db);
-  if (!success) throw runtime_error("Could not access file!");
 
   // Get camera poses, as Values
   size_t j = 0;

timing/timeSFMBAL.cpp

@@ -36,7 +36,7 @@ int main(int argc, char* argv[]) {
 
   // Build graph using conventional GeneralSFMFactor
   NonlinearFactorGraph graph;
-  for (size_t j = 0; j < db.number_tracks(); j++) {
+  for (size_t j = 0; j < db.numberTracks(); j++) {
     for (const SfmMeasurement& m: db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;

timing/timeSFMBAL.h

@@ -16,7 +16,7 @@
  * @date    July 5, 2015
  */
 
-#include <gtsam/slam/dataset.h>
+#include <gtsam/sfm/SfmData.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
@@ -54,9 +54,7 @@ SfmData preamble(int argc, char* argv[]) {
     filename = argv[argc - 1];
   else
     filename = findExampleDataFile("dubrovnik-16-22106-pre");
-  bool success = readBAL(filename, db);
+  return SfmData::FromBalFile(filename);
-  if (!success) throw runtime_error("Could not access file!");
-  return db;
 }
 
 // Create ordering and optimize
@@ -73,8 +71,8 @@ int optimize(const SfmData& db, const NonlinearFactorGraph& graph,
   if (gUseSchur) {
     // Create Schur-complement ordering
     Ordering ordering;
-    for (size_t j = 0; j < db.number_tracks(); j++) ordering.push_back(P(j));
+    for (size_t j = 0; j < db.numberTracks(); j++) ordering.push_back(P(j));
-    for (size_t i = 0; i < db.number_cameras(); i++) {
+    for (size_t i = 0; i < db.numberCameras(); i++) {
       ordering.push_back(C(i));
       if (separateCalibration) ordering.push_back(K(i));
     }

timing/timeSFMBALautodiff.cpp

@@ -44,7 +44,7 @@ int main(int argc, char* argv[]) {
 
   // Build graph
   NonlinearFactorGraph graph;
-  for (size_t j = 0; j < db.number_tracks(); j++) {
+  for (size_t j = 0; j < db.numberTracks(); j++) {
     for (const SfmMeasurement& m: db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;
@@ -59,7 +59,7 @@ int main(int argc, char* argv[]) {
   Values initial;
   size_t i = 0, j = 0;
   for (const SfmCamera& camera: db.cameras) {
-    // readBAL converts to GTSAM format, so we need to convert back !
+    // SfmData::FromBalFile converts to GTSAM format, so we need to convert back !
     Pose3 openGLpose = gtsam2openGL(camera.pose());
     Vector9 v9;
     v9 << Pose3::Logmap(openGLpose), camera.calibration();

timing/timeSFMBALcamTnav.cpp

@@ -33,7 +33,7 @@ int main(int argc, char* argv[]) {
 
   // Build graph using conventional GeneralSFMFactor
   NonlinearFactorGraph graph;
-  for (size_t j = 0; j < db.number_tracks(); j++) {
+  for (size_t j = 0; j < db.numberTracks(); j++) {
     for (const SfmMeasurement& m: db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;

timing/timeSFMBALnavTcam.cpp

@@ -33,7 +33,7 @@ int main(int argc, char* argv[]) {
 
   // Build graph using conventional GeneralSFMFactor
   NonlinearFactorGraph graph;
-  for (size_t j = 0; j < db.number_tracks(); j++) {
+  for (size_t j = 0; j < db.numberTracks(); j++) {
     Point3_ nav_point_(P(j));
     for (const SfmMeasurement& m: db.tracks[j].measurements) {
       size_t i = m.first;

timing/timeSFMBALsmart.cpp

@@ -35,7 +35,7 @@ int main(int argc, char* argv[]) {
 
   // Add smart factors to graph
   NonlinearFactorGraph graph;
-  for (size_t j = 0; j < db.number_tracks(); j++) {
+  for (size_t j = 0; j < db.numberTracks(); j++) {
     auto smartFactor = boost::make_shared<SfmFactor>(gNoiseModel);
     for (const SfmMeasurement& m : db.tracks[j].measurements) {
       size_t i = m.first;

wrap/gtwrap/matlab_wrapper/mixins.py

@@ -26,25 +26,30 @@ class CheckMixin:
                 return True
         return False
 
+    def can_be_pointer(self, arg_type: parser.Type):
+        """
+        Determine if the `arg_type` can have a pointer to it.
+
+        E.g. `Pose3` can have `Pose3*` but 
+        `Matrix` should not have `Matrix*`.
+        """
+        return (arg_type.typename.name not in self.not_ptr_type
+                and arg_type.typename.name not in self.ignore_namespace
+                and arg_type.typename.name != 'string')
+
     def is_shared_ptr(self, arg_type: parser.Type):
         """
         Determine if the `interface_parser.Type` should be treated as a
         shared pointer in the wrapper.
         """
-        return arg_type.is_shared_ptr or (
+        return arg_type.is_shared_ptr
-            arg_type.typename.name not in self.not_ptr_type
-            and arg_type.typename.name not in self.ignore_namespace
-            and arg_type.typename.name != 'string')
 
     def is_ptr(self, arg_type: parser.Type):
         """
         Determine if the `interface_parser.Type` should be treated as a
         raw pointer in the wrapper.
         """
-        return arg_type.is_ptr or (
+        return arg_type.is_ptr
-            arg_type.typename.name not in self.not_ptr_type
-            and arg_type.typename.name not in self.ignore_namespace
-            and arg_type.typename.name != 'string')
 
     def is_ref(self, arg_type: parser.Type):
         """

wrap/gtwrap/matlab_wrapper/wrapper.py

@@ -147,11 +147,13 @@ class MatlabWrapper(CheckMixin, FormatMixin):
         """
         def args_copy(args):
             return ArgumentList([copy.copy(arg) for arg in args.list()])
+
         def method_copy(method):
             method2 = copy.copy(method)
             method2.args = args_copy(method.args)
             method2.args.backup = method.args.backup
             return method2
+
         if save_backup:
             method.args.backup = args_copy(method.args)
         method = method_copy(method)
@@ -162,7 +164,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                 method.args.list().remove(arg)
                 return [
                     methodWithArg,
-                    *MatlabWrapper._expand_default_arguments(method, save_backup=False)
+                    *MatlabWrapper._expand_default_arguments(method,
+                                                             save_backup=False)
                 ]
             break
         assert all(arg.default is None for arg in method.args.list()), \
@@ -180,9 +183,12 @@ class MatlabWrapper(CheckMixin, FormatMixin):
 
             if method_index is None:
                 method_map[method.name] = len(method_out)
-                method_out.append(MatlabWrapper._expand_default_arguments(method))
+                method_out.append(
+                    MatlabWrapper._expand_default_arguments(method))
             else:
-                method_out[method_index] += MatlabWrapper._expand_default_arguments(method)
+                method_out[
+                    method_index] += MatlabWrapper._expand_default_arguments(
+                        method)
 
         return method_out
 
@@ -337,43 +343,42 @@ class MatlabWrapper(CheckMixin, FormatMixin):
         body_args = ''
 
         for arg in args.list():
+            ctype_camel = self._format_type_name(arg.ctype.typename,
+                                                 separator='')
+            ctype_sep = self._format_type_name(arg.ctype.typename)
+
             if self.is_ref(arg.ctype):  # and not constructor:
-                ctype_camel = self._format_type_name(arg.ctype.typename,
+                arg_type = "{ctype}&".format(ctype=ctype_sep)
-                                                     separator='')
+                unwrap = '*unwrap_shared_ptr< {ctype} >(in[{id}], "ptr_{ctype_camel}");'.format(
-                body_args += textwrap.indent(textwrap.dedent('''\
+                    ctype=ctype_sep, ctype_camel=ctype_camel, id=arg_id)
-                   {ctype}& {name} = *unwrap_shared_ptr< {ctype} >(in[{id}], "ptr_{ctype_camel}");
-                '''.format(ctype=self._format_type_name(arg.ctype.typename),
-                           ctype_camel=ctype_camel,
-                           name=arg.name,
-                           id=arg_id)),
-                                             prefix='  ')
 
-            elif (self.is_shared_ptr(arg.ctype) or self.is_ptr(arg.ctype)) and \
+            elif self.is_ptr(arg.ctype) and \
                     arg.ctype.typename.name not in self.ignore_namespace:
-                if arg.ctype.is_shared_ptr:
-                    call_type = arg.ctype.is_shared_ptr
-                else:
-                    call_type = arg.ctype.is_ptr
 
-                body_args += textwrap.indent(textwrap.dedent('''\
+                arg_type = "{ctype_sep}*".format(ctype_sep=ctype_sep)
-                    {std_boost}::shared_ptr<{ctype_sep}> {name} = unwrap_shared_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}");
+                unwrap = 'unwrap_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}");'.format(
-                '''.format(std_boost='boost' if constructor else 'boost',
+                    ctype_sep=ctype_sep, ctype=ctype_camel, id=arg_id)
-                           ctype_sep=self._format_type_name(
+
-                               arg.ctype.typename),
+            elif (self.is_shared_ptr(arg.ctype) or self.can_be_pointer(arg.ctype)) and \
-                           ctype=self._format_type_name(arg.ctype.typename,
+                    arg.ctype.typename.name not in self.ignore_namespace:
-                                                        separator=''),
+                call_type = arg.ctype.is_shared_ptr
-                           name=arg.name,
+
-                           id=arg_id)),
+                arg_type = "{std_boost}::shared_ptr<{ctype_sep}>".format(
-                                             prefix='  ')
+                    std_boost='boost' if constructor else 'boost',
+                    ctype_sep=ctype_sep)
+                unwrap = 'unwrap_shared_ptr< {ctype_sep} >(in[{id}], "ptr_{ctype}");'.format(
+                    ctype_sep=ctype_sep, ctype=ctype_camel, id=arg_id)
 
             else:
-                body_args += textwrap.indent(textwrap.dedent('''\
+                arg_type = "{ctype}".format(ctype=arg.ctype.typename.name)
-                    {ctype} {name} = unwrap< {ctype} >(in[{id}]);
+                unwrap = 'unwrap< {ctype} >(in[{id}]);'.format(
-                '''.format(ctype=arg.ctype.typename.name,
+                    ctype=arg.ctype.typename.name, id=arg_id)
-                           name=arg.name,
-                           id=arg_id)),
-                                             prefix='  ')
 
+            body_args += textwrap.indent(textwrap.dedent('''\
+                    {arg_type} {name} = {unwrap}
+                    '''.format(arg_type=arg_type, name=arg.name,
+                               unwrap=unwrap)),
+                                         prefix='  ')
             arg_id += 1
 
         params = ''
@@ -383,12 +388,14 @@ class MatlabWrapper(CheckMixin, FormatMixin):
             if params != '':
                 params += ','
 
-            if (arg.default is not None) and (arg.name not in explicit_arg_names):
+            if (arg.default is not None) and (arg.name
+                                              not in explicit_arg_names):
                 params += arg.default
                 continue
 
-            if (not self.is_ref(arg.ctype)) and (self.is_shared_ptr(arg.ctype)) and (self.is_ptr(
+            if not self.is_ref(arg.ctype) and (self.is_shared_ptr(arg.ctype) or \
-                    arg.ctype)) and (arg.ctype.typename.name not in self.ignore_namespace):
+                self.is_ptr(arg.ctype) or self.can_be_pointer(arg.ctype))and \
+                    arg.ctype.typename.name not in self.ignore_namespace:
                 if arg.ctype.is_shared_ptr:
                     call_type = arg.ctype.is_shared_ptr
                 else:
@@ -601,7 +608,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
         if not isinstance(ctors, Iterable):
             ctors = [ctors]
 
-        ctors = sum((MatlabWrapper._expand_default_arguments(ctor) for ctor in ctors), [])
+        ctors = sum((MatlabWrapper._expand_default_arguments(ctor)
+                     for ctor in ctors), [])
 
         methods_wrap = textwrap.indent(textwrap.dedent("""\
             methods
@@ -885,10 +893,10 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                     wrapper=self._wrapper_name(),
                     id=self._update_wrapper_id(
                         (namespace_name, instantiated_class,
-                        static_overload.name, static_overload)),
+                         static_overload.name, static_overload)),
                     class_name=instantiated_class.name,
                     end_statement=end_statement),
-                                            prefix='    ')
+                                               prefix='    ')
 
             # If the arguments don't match any of the checks above,
             # throw an error with the class and method name.
@@ -1079,7 +1087,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
         pair_value = 'first' if func_id == 0 else 'second'
         new_line = '\n' if func_id == 0 else ''
 
-        if self.is_shared_ptr(return_type) or self.is_ptr(return_type):
+        if self.is_shared_ptr(return_type) or self.is_ptr(return_type) or \
+            self.can_be_pointer(return_type):
             shared_obj = 'pairResult.' + pair_value
 
             if not (return_type.is_shared_ptr or return_type.is_ptr):
@@ -1145,7 +1154,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
 
         if return_1_name != 'void':
             if return_count == 1:
-                if self.is_shared_ptr(return_1) or self.is_ptr(return_1):
+                if self.is_shared_ptr(return_1) or self.is_ptr(return_1) or \
+                    self.can_be_pointer(return_1):
                     sep_method_name = partial(self._format_type_name,
                                               return_1.typename,
                                               include_namespace=True)

wrap/matlab.h

@@ -477,6 +477,14 @@ boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& pro
   return *spp;
 }
 
+template <typename Class>
+Class* unwrap_ptr(const mxArray* obj, const string& propertyName) {
+
+  mxArray* mxh = mxGetProperty(obj,0, propertyName.c_str());
+  Class* x = reinterpret_cast<Class*> (mxGetData(mxh));
+  return x;
+}
+
 //// throw an error if unwrap_shared_ptr is attempted for an Eigen Vector
 //template <>
 //Vector unwrap_shared_ptr<Vector>(const mxArray* obj, const string& propertyName) {

wrap/tests/expected/matlab/ForwardKinematicsFactor.m

@@ -11,9 +11,9 @@ classdef ForwardKinematicsFactor < gtsam.BetweenFactor<gtsam.Pose3>
         if nargin == 2
           my_ptr = varargin{2};
         else
-          my_ptr = inheritance_wrapper(36, varargin{2});
+          my_ptr = inheritance_wrapper(52, varargin{2});
         end
-        base_ptr = inheritance_wrapper(35, my_ptr);
+        base_ptr = inheritance_wrapper(51, my_ptr);
       else
         error('Arguments do not match any overload of ForwardKinematicsFactor constructor');
       end
@@ -22,7 +22,7 @@ classdef ForwardKinematicsFactor < gtsam.BetweenFactor<gtsam.Pose3>
     end
 
     function delete(obj)
-      inheritance_wrapper(37, obj.ptr_ForwardKinematicsFactor);
+      inheritance_wrapper(53, obj.ptr_ForwardKinematicsFactor);
     end
 
     function display(obj), obj.print(''); end

wrap/tests/expected/matlab/functions_wrapper.cpp

@@ -86,7 +86,7 @@ void load2D_2(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("load2D",nargout,nargin,2);
   string filename = unwrap< string >(in[0]);
-  boost::shared_ptr<gtsam::noiseModel::Diagonal> model = unwrap_shared_ptr< gtsam::noiseModel::Diagonal >(in[1], "ptr_gtsamnoiseModelDiagonal");
+  gtsam::noiseModel::Diagonal* model = unwrap_ptr< gtsam::noiseModel::Diagonal >(in[1], "ptr_gtsamnoiseModelDiagonal");
   auto pairResult = load2D(filename,model);
   out[0] = wrap_shared_ptr(pairResult.first,"gtsam.NonlinearFactorGraph", false);
   out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Values", false);

wrap/tests/expected/matlab/geometry_wrapper.cpp

@@ -151,7 +151,7 @@ void gtsamPoint2_argChar_7(int nargout, mxArray *out[], int nargin, const mxArra
 {
   checkArguments("argChar",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  boost::shared_ptr<char> a = unwrap_shared_ptr< char >(in[1], "ptr_char");
+  char* a = unwrap_ptr< char >(in[1], "ptr_char");
   obj->argChar(a);
 }
 
@@ -175,7 +175,7 @@ void gtsamPoint2_argChar_10(int nargout, mxArray *out[], int nargin, const mxArr
 {
   checkArguments("argChar",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  boost::shared_ptr<char> a = unwrap_shared_ptr< char >(in[1], "ptr_char");
+  char* a = unwrap_ptr< char >(in[1], "ptr_char");
   obj->argChar(a);
 }
 

wrap/tests/expected/matlab/inheritance_wrapper.cpp

@@ -9,6 +9,7 @@
 
 typedef MyTemplate<gtsam::Point2> MyTemplatePoint2;
 typedef MyTemplate<gtsam::Matrix> MyTemplateMatrix;
+typedef MyTemplate<A> MyTemplateA;
 
 typedef std::set<boost::shared_ptr<MyBase>*> Collector_MyBase;
 static Collector_MyBase collector_MyBase;
@@ -16,6 +17,8 @@ typedef std::set<boost::shared_ptr<MyTemplatePoint2>*> Collector_MyTemplatePoint
 static Collector_MyTemplatePoint2 collector_MyTemplatePoint2;
 typedef std::set<boost::shared_ptr<MyTemplateMatrix>*> Collector_MyTemplateMatrix;
 static Collector_MyTemplateMatrix collector_MyTemplateMatrix;
+typedef std::set<boost::shared_ptr<MyTemplateA>*> Collector_MyTemplateA;
+static Collector_MyTemplateA collector_MyTemplateA;
 typedef std::set<boost::shared_ptr<ForwardKinematicsFactor>*> Collector_ForwardKinematicsFactor;
 static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor;
 
@@ -44,6 +47,12 @@ void _deleteAllObjects()
     collector_MyTemplateMatrix.erase(iter++);
     anyDeleted = true;
   } }
+  { for(Collector_MyTemplateA::iterator iter = collector_MyTemplateA.begin();
+      iter != collector_MyTemplateA.end(); ) {
+    delete *iter;
+    collector_MyTemplateA.erase(iter++);
+    anyDeleted = true;
+  } }
   { for(Collector_ForwardKinematicsFactor::iterator iter = collector_ForwardKinematicsFactor.begin();
       iter != collector_ForwardKinematicsFactor.end(); ) {
     delete *iter;
@@ -67,6 +76,7 @@ void _inheritance_RTTIRegister() {
     types.insert(std::make_pair(typeid(MyBase).name(), "MyBase"));
     types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2"));
     types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix"));
+    types.insert(std::make_pair(typeid(MyTemplateA).name(), "MyTemplateA"));
     types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor"));
 
 
@@ -462,7 +472,157 @@ void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mx
   out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Matrix>>(MyTemplate<gtsam::Matrix>::Level(K)),"MyTemplateMatrix", false);
 }
 
-void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateA_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<MyTemplate<A>> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_MyTemplateA.insert(self);
+
+  typedef boost::shared_ptr<MyBase> SharedBase;
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
+}
+
+void MyTemplateA_upcastFromVoid_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<MyTemplate<A>> Shared;
+  boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  Shared *self = new Shared(boost::static_pointer_cast<MyTemplate<A>>(*asVoid));
+  *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
+}
+
+void MyTemplateA_constructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<MyTemplate<A>> Shared;
+
+  Shared *self = new Shared(new MyTemplate<A>());
+  collector_MyTemplateA.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+
+  typedef boost::shared_ptr<MyBase> SharedBase;
+  out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);
+}
+
+void MyTemplateA_deconstructor_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<MyTemplate<A>> Shared;
+  checkArguments("delete_MyTemplateA",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_MyTemplateA::iterator item;
+  item = collector_MyTemplateA.find(self);
+  if(item != collector_MyTemplateA.end()) {
+    collector_MyTemplateA.erase(item);
+  }
+  delete self;
+}
+
+void MyTemplateA_accept_T_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("accept_T",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  A& value = *unwrap_shared_ptr< A >(in[1], "ptr_A");
+  obj->accept_T(value);
+}
+
+void MyTemplateA_accept_Tptr_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("accept_Tptr",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  boost::shared_ptr<A> value = unwrap_shared_ptr< A >(in[1], "ptr_A");
+  obj->accept_Tptr(value);
+}
+
+void MyTemplateA_create_MixedPtrs_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("create_MixedPtrs",nargout,nargin-1,0);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  auto pairResult = obj->create_MixedPtrs();
+  out[0] = wrap_shared_ptr(boost::make_shared<A>(pairResult.first),"A", false);
+  out[1] = wrap_shared_ptr(pairResult.second,"A", false);
+}
+
+void MyTemplateA_create_ptrs_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("create_ptrs",nargout,nargin-1,0);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  auto pairResult = obj->create_ptrs();
+  out[0] = wrap_shared_ptr(pairResult.first,"A", false);
+  out[1] = wrap_shared_ptr(pairResult.second,"A", false);
+}
+
+void MyTemplateA_return_T_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("return_T",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  A* value = unwrap_ptr< A >(in[1], "ptr_A");
+  out[0] = wrap_shared_ptr(boost::make_shared<A>(obj->return_T(value)),"A", false);
+}
+
+void MyTemplateA_return_Tptr_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("return_Tptr",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  boost::shared_ptr<A> value = unwrap_shared_ptr< A >(in[1], "ptr_A");
+  out[0] = wrap_shared_ptr(obj->return_Tptr(value),"A", false);
+}
+
+void MyTemplateA_return_ptrs_45(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("return_ptrs",nargout,nargin-1,2);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  boost::shared_ptr<A> p1 = unwrap_shared_ptr< A >(in[1], "ptr_A");
+  boost::shared_ptr<A> p2 = unwrap_shared_ptr< A >(in[2], "ptr_A");
+  auto pairResult = obj->return_ptrs(p1,p2);
+  out[0] = wrap_shared_ptr(pairResult.first,"A", false);
+  out[1] = wrap_shared_ptr(pairResult.second,"A", false);
+}
+
+void MyTemplateA_templatedMethod_46(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  Matrix t = unwrap< Matrix >(in[1]);
+  out[0] = wrap< Matrix >(obj->templatedMethod<gtsam::Matrix>(t));
+}
+
+void MyTemplateA_templatedMethod_47(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  Point2 t = unwrap< Point2 >(in[1]);
+  out[0] = wrap< Point2 >(obj->templatedMethod<gtsam::Point2>(t));
+}
+
+void MyTemplateA_templatedMethod_48(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  Point3 t = unwrap< Point3 >(in[1]);
+  out[0] = wrap< Point3 >(obj->templatedMethod<gtsam::Point3>(t));
+}
+
+void MyTemplateA_templatedMethod_49(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("templatedMethodVector",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<MyTemplate<A>>(in[0], "ptr_MyTemplateA");
+  Vector t = unwrap< Vector >(in[1]);
+  out[0] = wrap< Vector >(obj->templatedMethod<gtsam::Vector>(t));
+}
+
+void MyTemplateA_Level_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("MyTemplate<A>.Level",nargout,nargin,1);
+  A& K = *unwrap_shared_ptr< A >(in[0], "ptr_A");
+  out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<A>>(MyTemplate<A>::Level(K)),"MyTemplateA", false);
+}
+
+void ForwardKinematicsFactor_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<ForwardKinematicsFactor> Shared;
@@ -475,7 +635,7 @@ void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray
   *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
 }
 
-void ForwardKinematicsFactor_upcastFromVoid_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+void ForwardKinematicsFactor_upcastFromVoid_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<ForwardKinematicsFactor> Shared;
   boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
@@ -484,7 +644,7 @@ void ForwardKinematicsFactor_upcastFromVoid_36(int nargout, mxArray *out[], int
   *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
 }
 
-void ForwardKinematicsFactor_deconstructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void ForwardKinematicsFactor_deconstructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<ForwardKinematicsFactor> Shared;
   checkArguments("delete_ForwardKinematicsFactor",nargout,nargin,1);
@@ -615,13 +775,61 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyTemplateMatrix_Level_34(nargout, out, nargin-1, in+1);
       break;
     case 35:
-      ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1);
+      MyTemplateA_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1);
       break;
     case 36:
-      ForwardKinematicsFactor_upcastFromVoid_36(nargout, out, nargin-1, in+1);
+      MyTemplateA_upcastFromVoid_36(nargout, out, nargin-1, in+1);
       break;
     case 37:
-      ForwardKinematicsFactor_deconstructor_37(nargout, out, nargin-1, in+1);
+      MyTemplateA_constructor_37(nargout, out, nargin-1, in+1);
+      break;
+    case 38:
+      MyTemplateA_deconstructor_38(nargout, out, nargin-1, in+1);
+      break;
+    case 39:
+      MyTemplateA_accept_T_39(nargout, out, nargin-1, in+1);
+      break;
+    case 40:
+      MyTemplateA_accept_Tptr_40(nargout, out, nargin-1, in+1);
+      break;
+    case 41:
+      MyTemplateA_create_MixedPtrs_41(nargout, out, nargin-1, in+1);
+      break;
+    case 42:
+      MyTemplateA_create_ptrs_42(nargout, out, nargin-1, in+1);
+      break;
+    case 43:
+      MyTemplateA_return_T_43(nargout, out, nargin-1, in+1);
+      break;
+    case 44:
+      MyTemplateA_return_Tptr_44(nargout, out, nargin-1, in+1);
+      break;
+    case 45:
+      MyTemplateA_return_ptrs_45(nargout, out, nargin-1, in+1);
+      break;
+    case 46:
+      MyTemplateA_templatedMethod_46(nargout, out, nargin-1, in+1);
+      break;
+    case 47:
+      MyTemplateA_templatedMethod_47(nargout, out, nargin-1, in+1);
+      break;
+    case 48:
+      MyTemplateA_templatedMethod_48(nargout, out, nargin-1, in+1);
+      break;
+    case 49:
+      MyTemplateA_templatedMethod_49(nargout, out, nargin-1, in+1);
+      break;
+    case 50:
+      MyTemplateA_Level_50(nargout, out, nargin-1, in+1);
+      break;
+    case 51:
+      ForwardKinematicsFactor_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1);
+      break;
+    case 52:
+      ForwardKinematicsFactor_upcastFromVoid_52(nargout, out, nargin-1, in+1);
+      break;
+    case 53:
+      ForwardKinematicsFactor_deconstructor_53(nargout, out, nargin-1, in+1);
       break;
     }
   } catch(const std::exception& e) {

wrap/tests/expected/python/inheritance_pybind.cpp

@@ -54,6 +54,21 @@ PYBIND11_MODULE(inheritance_py, m_) {
         .def("return_ptrs",[](MyTemplate<gtsam::Matrix>* self, const std::shared_ptr<gtsam::Matrix> p1, const std::shared_ptr<gtsam::Matrix> p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
         .def_static("Level",[](const gtsam::Matrix& K){return MyTemplate<gtsam::Matrix>::Level(K);}, py::arg("K"));
 
+    py::class_<MyTemplate<A>, MyBase, std::shared_ptr<MyTemplate<A>>>(m_, "MyTemplateA")
+        .def(py::init<>())
+        .def("templatedMethodPoint2",[](MyTemplate<A>* self, const gtsam::Point2& t){return self->templatedMethod<gtsam::Point2>(t);}, py::arg("t"))
+        .def("templatedMethodPoint3",[](MyTemplate<A>* self, const gtsam::Point3& t){return self->templatedMethod<gtsam::Point3>(t);}, py::arg("t"))
+        .def("templatedMethodVector",[](MyTemplate<A>* self, const gtsam::Vector& t){return self->templatedMethod<gtsam::Vector>(t);}, py::arg("t"))
+        .def("templatedMethodMatrix",[](MyTemplate<A>* self, const gtsam::Matrix& t){return self->templatedMethod<gtsam::Matrix>(t);}, py::arg("t"))
+        .def("accept_T",[](MyTemplate<A>* self, const A& value){ self->accept_T(value);}, py::arg("value"))
+        .def("accept_Tptr",[](MyTemplate<A>* self, std::shared_ptr<A> value){ self->accept_Tptr(value);}, py::arg("value"))
+        .def("return_Tptr",[](MyTemplate<A>* self, std::shared_ptr<A> value){return self->return_Tptr(value);}, py::arg("value"))
+        .def("return_T",[](MyTemplate<A>* self, A* value){return self->return_T(value);}, py::arg("value"))
+        .def("create_ptrs",[](MyTemplate<A>* self){return self->create_ptrs();})
+        .def("create_MixedPtrs",[](MyTemplate<A>* self){return self->create_MixedPtrs();})
+        .def("return_ptrs",[](MyTemplate<A>* self, std::shared_ptr<A> p1, std::shared_ptr<A> p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
+        .def_static("Level",[](const A& K){return MyTemplate<A>::Level(K);}, py::arg("K"));
+
     py::class_<ForwardKinematicsFactor, gtsam::BetweenFactor<gtsam::Pose3>, std::shared_ptr<ForwardKinematicsFactor>>(m_, "ForwardKinematicsFactor");