Merged in feature/ExpressionsBALExample (pull request #91)

New SFM Example, combines BAL/Expressions

examples/SFMExampleExpressions_bal.cpp

@@ -0,0 +1,121 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    SFMExampleExpressions_bal.cpp
+ * @brief   A structure-from-motion example done with Expressions
+ * @author  Frank Dellaert
+ * @date    January 2015
+ */
+
+/**
+ * This is the Expression version of SFMExample
+ * See detailed description of headers there, this focuses on explaining the AD part
+ */
+
+// The two new headers that allow using our Automatic Differentiation Expression framework
+#include <gtsam/slam/expressions.h>
+#include <gtsam/nonlinear/ExpressionFactorGraph.h>
+
+// 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 <vector>
+
+using namespace std;
+using namespace gtsam;
+using namespace noiseModel;
+using symbol_shorthand::C;
+using symbol_shorthand::P;
+
+// An SfM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
+// and has a total of 9 free parameters
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+
+  // Find default file, but if an argument is given, try loading a file
+  string filename = findExampleDataFile("dubrovnik-3-7-pre");
+  if (argc > 1)
+    filename = string(argv[1]);
+
+  // Load the SfM data from file
+  SfM_data mydata;
+  readBAL(filename, mydata);
+  cout
+      << boost::format("read %1% tracks on %2% cameras\n")
+          % mydata.number_tracks() % mydata.number_cameras();
+
+  // Create a factor graph
+  ExpressionFactorGraph graph;
+
+  // Here we don't use a PriorFactor but directly the ExpressionFactor class
+  // First, we create an expression to the pose from the first camera
+  Expression<SfM_Camera> camera0_(C(0));
+  // Then, to get its pose:
+  Pose3_ pose0_(&SfM_Camera::getPose, camera0_);
+  // Finally, we say it should be equal to first guess
+  graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
+      noiseModel::Isotropic::Sigma(6, 0.1));
+
+  // similarly, we create a prior on the first point
+  Point3_ point0_(P(0));
+  graph.addExpressionFactor(point0_, mydata.tracks[0].p,
+      noiseModel::Isotropic::Sigma(3, 0.1));
+
+  // We share *one* noiseModel between all projection factors
+  noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2,
+      1.0); // one pixel in u and v
+
+  // Simulated measurements from each camera pose, adding them to the factor graph
+  size_t j = 0;
+  BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
+    // Leaf expression for j^th point
+    Point3_ point_('p', j);
+    BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
+      size_t i = m.first;
+      Point2 uv = m.second;
+      // Leaf expression for i^th camera
+      Expression<SfM_Camera> camera_(C(i));
+      // Below an expression for the prediction of the measurement:
+      Point2_ predict_ = project2<SfM_Camera>(camera_, point_);
+      // Again, here we use an ExpressionFactor
+      graph.addExpressionFactor(predict_, uv, noise);
+    }
+    j += 1;
+  }
+
+  // Create initial estimate
+  Values initial;
+  size_t i = 0;
+  j = 0;
+  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras)
+    initial.insert(C(i++), camera);
+  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)
+    initial.insert(P(j++), track.p);
+
+  /* Optimize the graph and print results */
+  Values result;
+  try {
+    LevenbergMarquardtParams params;
+    params.setVerbosity("ERROR");
+    LevenbergMarquardtOptimizer lm(graph, initial, params);
+    result = lm.optimize();
+  } catch (exception& e) {
+    cout << e.what();
+  }
+  cout << "final error: " << graph.error(result) << endl;
+
+  return 0;
+}
+/* ************************************************************************* */
+

examples/SFMExample_bal.cpp

@@ -43,7 +43,7 @@ int main (int argc, char* argv[]) {
 
   // Load the SfM data from file
   SfM_data mydata;
-  assert(readBAL(filename, mydata));
+  readBAL(filename, mydata);
   cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
 
   // Create a factor graph

gtsam/geometry/PinholeCamera.h

@@ -151,11 +151,20 @@ public:
     return pose_;
   }
 
-  /// return pose
+  /// return pose, constant version
   inline const Pose3& pose() const {
     return pose_;
   }
 
+  /// return pose, with derivative
+  inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const {
+    if (H) {
+      H->setZero();
+      H->block(0, 0, 6, 6) = I_6x6;
+    }
+    return pose_;
+  }
+
   /// return calibration
   inline Calibration& calibration() {
     return K_;

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -51,8 +51,21 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0);
 /* ************************************************************************* */
 TEST( PinholeCamera, constructor)
 {
-  EXPECT(assert_equal( camera.calibration(), K));
-  EXPECT(assert_equal( camera.pose(), pose));
+  EXPECT(assert_equal( K, camera.calibration()));
+  EXPECT(assert_equal( pose, camera.pose()));
+}
+
+//******************************************************************************
+TEST(PinholeCamera, Pose) {
+
+  Matrix actualH;
+  EXPECT(assert_equal(pose, camera.getPose(actualH)));
+
+  // Check derivative
+  boost::function<Pose3(Camera)> f = //
+      boost::bind(&Camera::getPose,_1,boost::none);
+  Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
+  EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }
 
 /* ************************************************************************* */

gtsam/slam/expressions.h

@@ -9,6 +9,7 @@
 
 #include <gtsam/nonlinear/expressions.h>
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
 
 namespace gtsam {
@@ -19,7 +20,7 @@ typedef Expression<Point2> Point2_;
 typedef Expression<Rot2> Rot2_;
 typedef Expression<Pose2> Pose2_;
 
-Point2_ transform_to(const Pose2_& x, const Point2_& p) {
+inline Point2_ transform_to(const Pose2_& x, const Point2_& p) {
   return Point2_(x, &Pose2::transform_to, p);
 }
 
@@ -29,24 +30,36 @@ typedef Expression<Point3> Point3_;
 typedef Expression<Rot3> Rot3_;
 typedef Expression<Pose3> Pose3_;
 
-Point3_ transform_to(const Pose3_& x, const Point3_& p) {
+inline Point3_ transform_to(const Pose3_& x, const Point3_& p) {
   return Point3_(x, &Pose3::transform_to, p);
 }
 
 // Projection
 
 typedef Expression<Cal3_S2> Cal3_S2_;
+typedef Expression<Cal3Bundler> Cal3Bundler_;
 
-Point2_ project(const Point3_& p_cam) {
+inline Point2_ project(const Point3_& p_cam) {
   return Point2_(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
 }
 
-Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
+template <class CAMERA>
+Point2 project4(const CAMERA& camera, const Point3& p,
+    OptionalJacobian<2, CAMERA::dimension> Dcam, OptionalJacobian<2, 3> Dpoint) {
+  return camera.project2(p, Dcam, Dpoint);
+}
+
+template <class CAMERA>
+Point2_ project2(const Expression<CAMERA>& camera_, const Point3_& p_) {
+  return Point2_(project4<CAMERA>, camera_, p_);
+}
+
+inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
     OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) {
   return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
 }
 
-Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) {
+inline Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) {
   return Point2_(project6, x, p, K);
 }