For exact comparison with Ceres, use exact same AutoDiff model

timing/timeSFMBAL.cpp

@@ -16,11 +16,14 @@
  * @date    June 6, 2015
  */
 
+#include <gtsam/3rdparty/ceres/example.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/AdaptAutoDiff.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
@@ -39,6 +42,32 @@ using namespace gtsam;
 
 //#define TERNARY
 
+// Special version of Cal3Bundler so that default constructor = 0,0,0
+struct CeresCalibration: public Cal3Bundler {
+  CeresCalibration(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0,
+      double v0 = 0) :
+      Cal3Bundler(f, k1, k2, u0, v0) {
+  }
+  CeresCalibration(const Cal3Bundler& cal) :
+      Cal3Bundler(cal) {
+  }
+  CeresCalibration retract(const Vector& d) const {
+    return CeresCalibration(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0());
+  }
+  Vector3 localCoordinates(const CeresCalibration& T2) const {
+    return T2.vector() - vector();
+  }
+};
+
+namespace gtsam {
+template<>
+struct traits<CeresCalibration> : public internal::Manifold<CeresCalibration> {
+};
+}
+
+// With that, camera below behaves like Snavely's 9-dim vector
+typedef PinholeCamera<CeresCalibration> CeresCamera;
+
 int main(int argc, char* argv[]) {
   typedef GeneralSFMFactor<PinholeCamera<Cal3Bundler>, Point3> sfmFactor;
   using symbol_shorthand::P;
@@ -47,17 +76,41 @@ int main(int argc, char* argv[]) {
   string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre");
   SfM_data db;
   bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db);
-  if (!success) throw runtime_error("Could not access file!");
+  if (!success)
+    throw runtime_error("Could not access file!");
+
+  typedef AdaptAutoDiff<SnavelyProjection, Point2, CeresCamera, Point3> Adaptor;
 
   // Build graph
   SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
   NonlinearFactorGraph graph;
   for (size_t j = 0; j < db.number_tracks(); j++) {
-    BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements)
+    BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
-      graph.push_back(sfmFactor(m.second, unit2, m.first, P(j)));
+      size_t i = m.first;
+      Point2 measurement = m.second;
+#ifdef USE_GTSAM_FACTOR
+      graph.push_back(sfmFactor(measurement, unit2, i, P(j)));
+#else
+      Expression<CeresCamera> camera_(i);
+      Expression<Point3> point_(P(j));
+      graph.addExpressionFactor(unit2, measurement,
+          Expression<Point2>(Adaptor(), camera_, point_));
+#endif
+    }
   }
 
-  Values initial = initialCamerasAndPointsEstimate(db);
+  Values initial;
+  size_t i = 0, j = 0;
+  BOOST_FOREACH(const SfM_Camera& camera, db.cameras) {
+#ifdef USE_GTSAM_FACTOR
+    initial.insert((i++), camera);
+#else
+    CeresCamera ceresCamera(camera.pose(), camera.calibration());
+    initial.insert((i++), ceresCamera);
+#endif
+  }
+  BOOST_FOREACH(const SfM_Track& track, db.tracks)
+    initial.insert(P(j++), track.p);
 
 // Create Schur-complement ordering
 #ifdef CCOLAMD
@@ -66,8 +119,10 @@ int main(int argc, char* argv[]) {
   Ordering ordering = Ordering::colamdConstrainedFirst(graph, pointKeys, true);
 #else
   Ordering ordering;
-  for (size_t j = 0; j < db.number_tracks(); j++) ordering.push_back(P(j));
+  for (size_t j = 0; j < db.number_tracks(); j++)
-  for (size_t i = 0; i < db.number_cameras(); i++) ordering.push_back(i);
+    ordering.push_back(P(j));
+  for (size_t i = 0; i < db.number_cameras(); i++)
+    ordering.push_back(i);
 #endif
 
   // Optimize