Merge branch 'develop' into ta-add-methods

gtsam/discrete/tests/testDiscreteFactorGraph.cpp

@@ -415,16 +415,16 @@ TEST(DiscreteFactorGraph, DotWithNames) {
       "graph {\n"
       "  size=\"5,5\";\n"
       "\n"
-      "  varC[label=\"C\"];\n"
+      "  var0[label=\"C\"];\n"
-      "  varA[label=\"A\"];\n"
+      "  var1[label=\"A\"];\n"
-      "  varB[label=\"B\"];\n"
+      "  var2[label=\"B\"];\n"
       "\n"
       "  factor0[label=\"\", shape=point];\n"
-      "  varC--factor0;\n"
+      "  var0--factor0;\n"
-      "  varA--factor0;\n"
+      "  var1--factor0;\n"
       "  factor1[label=\"\", shape=point];\n"
-      "  varC--factor1;\n"
+      "  var0--factor1;\n"
-      "  varB--factor1;\n"
+      "  var2--factor1;\n"
       "}\n";
   EXPECT(actual == expected);
 }

gtsam/geometry/geometry.i

@@ -547,6 +547,12 @@ class EssentialMatrix {
   // Standard Constructors
   EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
 
+  // Constructors from Pose3
+  gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_);
+
+  gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
+                            Eigen::Ref<Eigen::MatrixXd> H);
+
   // Testable
   void print(string s = "") const;
   bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
@@ -904,6 +910,12 @@ class PinholeCamera {
                             Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
                             Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
                             Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
+
+  gtsam::Point2 reprojectionError(const gtsam::Point3& pw, const gtsam::Point2& measured,
+                                  Eigen::Ref<Eigen::MatrixXd> Dpose,
+                                  Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                                  Eigen::Ref<Eigen::MatrixXd> Dcal);
+
   double range(const gtsam::Point3& point);
   double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
                Eigen::Ref<Eigen::MatrixXd> Dpoint);
@@ -914,7 +926,74 @@ class PinholeCamera {
   // enabling serialization functionality
   void serialize() const;
 };
-  
+
+// Forward declaration of PinholeCameraCalX is defined here.
+#include <gtsam/geometry/SimpleCamera.h>
+// Some typedefs for common camera types
+// PinholeCameraCal3_S2 is the same as SimpleCamera above
+typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
+
+#include <gtsam/geometry/PinholePose.h>
+template <CALIBRATION>
+class PinholePose {
+  // Standard Constructors and Named Constructors
+  PinholePose();
+  PinholePose(const gtsam::PinholePose<CALIBRATION> other);
+  PinholePose(const gtsam::Pose3& pose);
+  PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K);
+  static This Level(const gtsam::Pose2& pose, double height);
+  static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
+                     const gtsam::Point3& upVector, const CALIBRATION* K);
+
+  // Testable
+  void print(string s = "PinholePose") const;
+  bool equals(const This& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  CALIBRATION calibration() const;
+
+  // Manifold
+  This retract(Vector d) const;
+  Vector localCoordinates(const This& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
+  // Transformations and measurement functions
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
+  pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
+  gtsam::Point2 project(const gtsam::Point3& point);
+  gtsam::Point2 project(const gtsam::Point3& point,
+                        Eigen::Ref<Eigen::MatrixXd> Dpose,
+                        Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                        Eigen::Ref<Eigen::MatrixXd> Dcal);
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
+  double range(const gtsam::Point3& point);
+  double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpoint);
+  double range(const gtsam::Pose3& pose);
+  double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpose);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
+typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
+typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
+typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
+typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
+
 #include <gtsam/geometry/Similarity2.h>
 class Similarity2 {
   // Standard Constructors
@@ -961,16 +1040,6 @@ class Similarity3 {
   double scale() const;
 };
 
-// Forward declaration of PinholeCameraCalX is defined here.
-#include <gtsam/geometry/SimpleCamera.h>
-// Some typedefs for common camera types
-// PinholeCameraCal3_S2 is the same as SimpleCamera above
-typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
-typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
-typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
-typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
-typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
-
 template <T>
 class CameraSet {
   CameraSet();

gtsam/inference/BayesNet-inst.h

@@ -53,8 +53,9 @@ void BayesNet<CONDITIONAL>::dot(std::ostream& os,
     auto frontals = conditional->frontals();
     const Key me = frontals.front();
     auto parents = conditional->parents();
-    for (const Key& p : parents)
+    for (const Key& p : parents) {
-      os << "  var" << keyFormatter(p) << "->var" << keyFormatter(me) << "\n";
+      os << "  var" << p << "->var" << me << "\n";
+    }
   }
 
   os << "}";

gtsam/inference/DotWriter.cpp

@@ -43,7 +43,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter,
                              const boost::optional<Vector2>& position,
                              ostream* os) const {
   // Label the node with the label from the KeyFormatter
-  *os << "  var" << keyFormatter(key) << "[label=\"" << keyFormatter(key)
+  *os << "  var" << key << "[label=\"" << keyFormatter(key)
       << "\"";
   if (position) {
     *os << ", pos=\"" << position->x() << "," << position->y() << "!\"";
@@ -65,13 +65,13 @@ void DotWriter::DrawFactor(size_t i, const boost::optional<Vector2>& position,
 
 static void ConnectVariables(Key key1, Key key2,
                              const KeyFormatter& keyFormatter, ostream* os) {
-  *os << "  var" << keyFormatter(key1) << "--"
+  *os << "  var" << key1 << "--"
-      << "var" << keyFormatter(key2) << ";\n";
+      << "var" << key2 << ";\n";
 }
 
 static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter,
                                   size_t i, ostream* os) {
-  *os << "  var" << keyFormatter(key) << "--"
+  *os << "  var" << key << "--"
       << "factor" << i << ";\n";
 }
 

gtsam/linear/linear.i

@@ -671,6 +671,10 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
   DummyPreconditionerParameters();
 };
 
+virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParameters {
+  BlockJacobiPreconditionerParameters();
+};
+
 #include <gtsam/linear/PCGSolver.h>
 virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
   PCGSolverParameters();

gtsam/nonlinear/Marginals.h

@@ -121,7 +121,7 @@ public:
 
   /** Optimize the bayes tree */
   VectorValues optimize() const;
-            
+
 protected:
   
   /** Compute the Bayes Tree as a helper function to the constructor */

gtsam/nonlinear/PriorFactor.h

@@ -94,7 +94,6 @@ namespace gtsam {
     Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
       if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
       // manifold equivalent of z-x -> Local(x,z)
-      // TODO(ASL) Add Jacobians.
       return -traits<T>::Local(x, prior_);
     }
 

gtsam/nonlinear/nonlinear.i

@@ -226,6 +226,10 @@ class Values {
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
   void insert(size_t j, double c);
@@ -269,6 +273,10 @@ class Values {
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void update(size_t j, const gtsam::NavState& nav_state);
   void update(size_t j, Vector vector);
@@ -310,6 +318,10 @@ class Values {
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert_or_assign(size_t j, const gtsam::NavState& nav_state);
   void insert_or_assign(size_t j, Vector vector);
@@ -351,6 +363,10 @@ class Values {
                  gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                  gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
                  gtsam::PinholeCamera<gtsam::Cal3Unified>,
+                 gtsam::PinholePose<gtsam::Cal3_S2>,
+                 gtsam::PinholePose<gtsam::Cal3Bundler>,
+                 gtsam::PinholePose<gtsam::Cal3Fisheye>,
+                 gtsam::PinholePose<gtsam::Cal3Unified>,
                  gtsam::imuBias::ConstantBias,
                  gtsam::NavState,
                  Vector,

gtsam/sfm/SfmData.h

@@ -77,10 +77,14 @@ struct GTSAM_EXPORT SfmData {
   size_t numberCameras() const { return cameras.size(); }
 
   /// The track formed by series of landmark measurements
-  SfmTrack track(size_t idx) const { return tracks[idx]; }
+  const 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]; }
+  const SfmCamera& camera(size_t idx) const { return cameras[idx]; }
+
+  /// Getters
+  const std::vector<SfmCamera>& cameraList() const { return cameras; }
+  const std::vector<SfmTrack>& trackList() const { return tracks; }
 
   /**
    * @brief Create projection factors using keys i and P(j)

gtsam/sfm/sfm.i

@@ -11,6 +11,8 @@ class SfmTrack {
   SfmTrack();
   SfmTrack(const gtsam::Point3& pt);
   const Point3& point3() const;
+  
+  Point3 p;
 
   double r;
   double g;
@@ -36,12 +38,15 @@ class SfmData {
   static gtsam::SfmData FromBundlerFile(string filename);
   static gtsam::SfmData FromBalFile(string filename);
 
+  std::vector<gtsam::SfmTrack>& trackList() const;
+  std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>>& cameraList() const;
+
   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::SfmTrack& track(size_t idx) const;
-  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
+  gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera(size_t idx) const;
 
   gtsam::NonlinearFactorGraph generalSfmFactors(
       const gtsam::SharedNoiseModel& model =
@@ -99,6 +104,12 @@ class BinaryMeasurementsPoint3 {
   size_t size() const;
   gtsam::BinaryMeasurement<gtsam::Point3> at(size_t idx) const;
   void push_back(const gtsam::BinaryMeasurement<gtsam::Point3>& measurement);
+
+class BinaryMeasurementsRot3 {
+  BinaryMeasurementsRot3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Rot3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Rot3>& measurement);
 };
 
 #include <gtsam/sfm/ShonanAveraging.h>
@@ -194,6 +205,10 @@ class ShonanAveraging2 {
 };
 
 class ShonanAveraging3 {
+  ShonanAveraging3(
+      const std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>& measurements,
+      const gtsam::ShonanAveragingParameters3& parameters =
+          gtsam::ShonanAveragingParameters3());
   ShonanAveraging3(string g2oFile);
   ShonanAveraging3(string g2oFile,
                    const gtsam::ShonanAveragingParameters3& parameters);

gtsam/slam/dataset.h

@@ -224,6 +224,7 @@ parse3DFactors(const std::string &filename,
 
 using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
 using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
+using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
 inline boost::optional<IndexedPose> GTSAM_DEPRECATED

gtsam/slam/slam.i

@@ -90,6 +90,22 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>,
                                 gtsam::Point3>
     GeneralSFMFactorCal3Unified;
 
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3_S2>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3_S2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3DS2>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3DS2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Bundler>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3Bundler;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Fisheye>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3Fisheye;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Unified>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3Unified;
+
 template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
                          gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
 virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {

gtsam/slam/tests/testPriorFactor.cpp

@@ -5,12 +5,16 @@
  * @date   Nov 4, 2014
  */
 
-#include <gtsam/base/Vector.h>
-#include <gtsam/nonlinear/PriorFactor.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/nonlinear/PriorFactor.h>
+#include <gtsam/nonlinear/factorTesting.h>
 
 using namespace std;
+using namespace std::placeholders;
 using namespace gtsam;
+using namespace imuBias;
 
 /* ************************************************************************* */
 
@@ -23,16 +27,44 @@ TEST(PriorFactor, ConstructorScalar) {
 // Constructor vector3
 TEST(PriorFactor, ConstructorVector3) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
-  PriorFactor<Vector3> factor(1, Vector3(1,2,3), model);
+  PriorFactor<Vector3> factor(1, Vector3(1, 2, 3), model);
 }
 
 // Constructor dynamic sized vector
 TEST(PriorFactor, ConstructorDynamicSizeVector) {
-  Vector v(5); v << 1, 2, 3, 4, 5;
+  Vector v(5);
+  v << 1, 2, 3, 4, 5;
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
   PriorFactor<Vector> factor(1, v, model);
 }
 
+Vector callEvaluateError(const PriorFactor<ConstantBias>& factor,
+                         const ConstantBias& bias) {
+  return factor.evaluateError(bias);
+}
+
+// Test for imuBias::ConstantBias
+TEST(PriorFactor, ConstantBias) {
+  Vector3 biasAcc(1, 2, 3);
+  Vector3 biasGyro(0.1, 0.2, 0.3);
+  ConstantBias bias(biasAcc, biasGyro);
+
+  PriorFactor<ConstantBias> factor(1, bias,
+                                   noiseModel::Isotropic::Sigma(6, 0.1));
+  Values values;
+  values.insert(1, bias);
+
+  EXPECT_DOUBLES_EQUAL(0.0, factor.error(values), 1e-8);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+
+  ConstantBias incorrectBias(
+      (Vector6() << 1.1, 2.1, 3.1, 0.2, 0.3, 0.4).finished());
+  values.clear();
+  values.insert(1, incorrectBias);
+  EXPECT_DOUBLES_EQUAL(3.0, factor.error(values), 1e-8);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/symbolic/tests/testSymbolicBayesNet.cpp

@@ -104,16 +104,16 @@ TEST(SymbolicBayesNet, Dot) {
          "digraph {\n"
          "  size=\"5,5\";\n"
          "\n"
-         "  vara1[label=\"a1\", pos=\"1,2!\", shape=box];\n"
+         "  var6989586621679009793[label=\"a1\", pos=\"1,2!\", shape=box];\n"
-         "  vara2[label=\"a2\", pos=\"2,2!\", shape=box];\n"
+         "  var6989586621679009794[label=\"a2\", pos=\"2,2!\", shape=box];\n"
-         "  varx1[label=\"x1\", pos=\"1,1!\"];\n"
+         "  var8646911284551352321[label=\"x1\", pos=\"1,1!\"];\n"
-         "  varx2[label=\"x2\", pos=\"2,1!\"];\n"
+         "  var8646911284551352322[label=\"x2\", pos=\"2,1!\"];\n"
-         "  varx3[label=\"x3\", pos=\"3,1!\"];\n"
+         "  var8646911284551352323[label=\"x3\", pos=\"3,1!\"];\n"
          "\n"
-         "  varx1->varx2\n"
+         "  var8646911284551352321->var8646911284551352322\n"
-         "  vara1->varx2\n"
+         "  var6989586621679009793->var8646911284551352322\n"
-         "  varx2->varx3\n"
+         "  var8646911284551352322->var8646911284551352323\n"
-         "  vara2->varx3\n"
+         "  var6989586621679009794->var8646911284551352323\n"
          "}");
 }
 

python/CMakeLists.txt

@@ -49,6 +49,7 @@ set(ignore
     gtsam::KeyVector
     gtsam::BinaryMeasurementsPoint3
     gtsam::BinaryMeasurementsUnit3
+    gtsam::BinaryMeasurementsRot3
     gtsam::DiscreteKey
     gtsam::KeyPairDoubleMap)
 
@@ -108,6 +109,14 @@ file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py"
 foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})
         configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY)
 endforeach()
+file(GLOB GTSAM_PYTHON_PREAMBLE_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/preamble/*.h")
+foreach(util_file ${GTSAM_PYTHON_PREAMBLE_FILES})
+        configure_file(${util_file} "${GTSAM_MODULE_PATH}/preamble/${test_file}" COPYONLY)
+endforeach()
+file(GLOB GTSAM_PYTHON_SPECIALIZATION_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/specializations/*.h")
+foreach(util_file ${GTSAM_PYTHON_SPECIALIZATION_FILES})
+        configure_file(${util_file} "${GTSAM_MODULE_PATH}/specializations/${test_file}" COPYONLY)
+endforeach()
 
 # Common directory for data/datasets stored with the package.
 # This will store the data in the Python site package directly.
@@ -131,6 +140,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
             gtsam::FixedLagSmootherKeyTimestampMapValue
             gtsam::BinaryMeasurementsPoint3
             gtsam::BinaryMeasurementsUnit3
+            gtsam::BinaryMeasurementsRot3
             gtsam::CameraSetCal3_S2
             gtsam::CameraSetCal3Bundler
             gtsam::CameraSetCal3Unified

python/gtsam/preamble/sfm.h

@@ -9,4 +9,18 @@
  * automatic STL binding, such that the raw objects can be accessed in Python.
  * Without this they will be automatically converted to a Python object, and all
  * mutations on Python side will not be reflected on C++.
- */
+ */
+
+// Including <stl.h> can cause some serious hard-to-debug bugs!!!
+// #include <pybind11/stl.h>
+#include <pybind11/stl_bind.h>
+
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::SfmTrack>);
+
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::SfmCamera>);
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>);
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>);

python/gtsam/specializations/sfm.h

@@ -15,4 +15,19 @@ py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Point3> > >(
     m_, "BinaryMeasurementsPoint3");
 py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
     m_, "BinaryMeasurementsUnit3");
+py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(
+    m_, "BinaryMeasurementsRot3");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
+
+py::bind_vector<
+    std::vector<gtsam::SfmTrack> >(
+    m_, "SfmTracks");
+
+py::bind_vector<
+    std::vector<gtsam::SfmCamera> >(
+    m_, "SfmCameras");
+
+py::bind_vector<
+    std::vector<std::pair<size_t, gtsam::Point2>>>(
+        m_, "SfmMeasurementVector"
+    );

python/gtsam/tests/test_DiscreteBayesNet.py

@@ -11,12 +11,12 @@ Author: Frank Dellaert
 
 # pylint: disable=no-name-in-module, invalid-name
 
-import unittest
 import textwrap
+import unittest
 
 import gtsam
-from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph,
+from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution,
-                   DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering)
+                   DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering)
 from gtsam.utils.test_case import GtsamTestCase
 
 # Some keys:
@@ -152,10 +152,10 @@ class TestDiscreteBayesNet(GtsamTestCase):
               var4[label="4"];
               var5[label="5"];
               var6[label="6"];
-              vara0[label="a0", pos="0,2!"];
+              var6989586621679009792[label="a0", pos="0,2!"];
 
               var4->var6
-              vara0->var3
+              var6989586621679009792->var3
               var3->var5
               var6->var5
             }"""

tests/testNonlinearFactorGraph.cpp

@@ -335,21 +335,21 @@ TEST(NonlinearFactorGraph, dot) {
       "graph {\n"
       "  size=\"5,5\";\n"
       "\n"
-      "  varl1[label=\"l1\"];\n"
+      "  var7782220156096217089[label=\"l1\"];\n"
-      "  varx1[label=\"x1\"];\n"
+      "  var8646911284551352321[label=\"x1\"];\n"
-      "  varx2[label=\"x2\"];\n"
+      "  var8646911284551352322[label=\"x2\"];\n"
       "\n"
       "  factor0[label=\"\", shape=point];\n"
-      "  varx1--factor0;\n"
+      "  var8646911284551352321--factor0;\n"
       "  factor1[label=\"\", shape=point];\n"
-      "  varx1--factor1;\n"
+      "  var8646911284551352321--factor1;\n"
-      "  varx2--factor1;\n"
+      "  var8646911284551352322--factor1;\n"
       "  factor2[label=\"\", shape=point];\n"
-      "  varx1--factor2;\n"
+      "  var8646911284551352321--factor2;\n"
-      "  varl1--factor2;\n"
+      "  var7782220156096217089--factor2;\n"
       "  factor3[label=\"\", shape=point];\n"
-      "  varx2--factor3;\n"
+      "  var8646911284551352322--factor3;\n"
-      "  varl1--factor3;\n"
+      "  var7782220156096217089--factor3;\n"
       "}\n";
 
   const NonlinearFactorGraph fg = createNonlinearFactorGraph();
@@ -363,21 +363,21 @@ TEST(NonlinearFactorGraph, dot_extra) {
       "graph {\n"
       "  size=\"5,5\";\n"
       "\n"
-      "  varl1[label=\"l1\", pos=\"0,0!\"];\n"
+      "  var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n"
-      "  varx1[label=\"x1\", pos=\"1,0!\"];\n"
+      "  var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n"
-      "  varx2[label=\"x2\", pos=\"1,1.5!\"];\n"
+      "  var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n"
       "\n"
       "  factor0[label=\"\", shape=point];\n"
-      "  varx1--factor0;\n"
+      "  var8646911284551352321--factor0;\n"
       "  factor1[label=\"\", shape=point];\n"
-      "  varx1--factor1;\n"
+      "  var8646911284551352321--factor1;\n"
-      "  varx2--factor1;\n"
+      "  var8646911284551352322--factor1;\n"
       "  factor2[label=\"\", shape=point];\n"
-      "  varx1--factor2;\n"
+      "  var8646911284551352321--factor2;\n"
-      "  varl1--factor2;\n"
+      "  var7782220156096217089--factor2;\n"
       "  factor3[label=\"\", shape=point];\n"
-      "  varx2--factor3;\n"
+      "  var8646911284551352322--factor3;\n"
-      "  varl1--factor3;\n"
+      "  var7782220156096217089--factor3;\n"
       "}\n";
 
   const NonlinearFactorGraph fg = createNonlinearFactorGraph();