Merge branch 'develop' into feature/hybrid

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;
-  readBAL(filename, mydata);
+  SfmData mydata = SfmData::FromBalFile(filename);
   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;
-  readBAL(filename, mydata);
-  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
+  SfmData mydata = SfmData::FromBalFile(filename);
+  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras();
 
   // 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;
-  readBAL(filename, mydata);
+  SfmData mydata = SfmData::FromBalFile(filename);
   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/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(const gtsam::Ordering& ordering);
-  std::pair<gtsam::DiscreteBayesNet, gtsam::DiscreteFactorGraph>
+  gtsam::DiscreteBayesNet* eliminateSequential();
+  gtsam::DiscreteBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type);
+  gtsam::DiscreteBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
+  pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*>
       eliminatePartialSequential(const gtsam::Ordering& ordering);
-  gtsam::DiscreteBayesTree eliminateMultifrontal();
-  gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering);
-  std::pair<gtsam::DiscreteBayesTree, gtsam::DiscreteFactorGraph>
+
+  gtsam::DiscreteBayesTree* eliminateMultifrontal();
+  gtsam::DiscreteBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type);  
+  gtsam::DiscreteBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);  
+  pair<gtsam::DiscreteBayesTree*, gtsam::DiscreteFactorGraph*>
       eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
 
   string dot(

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/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()) {
-      if (!first) parent += ",";
+    for (Key key : clique->conditional_->frontals()) {
+      if (!first) parent += ", ";
       first = false;
-      parent += indexFormatter(index);
+      parent += keyFormatter(key);
     }
 
     if (clique->parent()) {
@@ -116,18 +116,18 @@ namespace gtsam {
     }
 
     first = true;
-    for (Key sep : clique->conditional_->parents()) {
-      if (!first) parent += ",";
+    for (Key parentKey : clique->conditional_->parents()) {
+      if (!first) parent += ", ";
       first = false;
-      parent += indexFormatter(sep);
+      parent += keyFormatter(parentKey);
     }
     parent += "\"];\n";
     s << parent;
     parentnum = num;
 
     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,
-                            gtsam::GaussianFactorGraph}>
+  template <
+      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/linear.i

@@ -407,8 +407,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);
@@ -527,6 +529,7 @@ virtual class GaussianBayesNet {
 
   gtsam::VectorValues optimize() const;
   gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
+  std::pair<Matrix, Vector> matrix() const; 
   gtsam::VectorValues optimizeGradientSearch() const;
   gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
   gtsam::VectorValues gradientAtZero() const;
@@ -556,7 +559,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/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 << " gyro = " << Point3(bias.gyroscope());
+  os << "acc = " << bias.accelerometer().transpose();
+  os << " gyro = " << bias.gyroscope().transpose();
   return os;
 }