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;
-  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/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(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/geometry/Rot3.h

@@ -49,16 +49,14 @@
 
 namespace gtsam {
 
-  /**
-   * @brief A 3D rotation represented as a rotation matrix if the preprocessor
-   * symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it
-   * is defined.
-   * @addtogroup geometry
-   * \nosubgrouping
-   */
-  class GTSAM_EXPORT Rot3 : public LieGroup<Rot3,3> {
-
-  private:
+/**
+ * @brief Rot3 is a 3D rotation represented as a rotation matrix if the
+ * preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion
+ * if it is defined.
+ * @addtogroup geometry
+ */
+class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
+ 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()) {
-      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);
     }
   }