From 0fd1ff7b26d52cea75e942454eafb32c7f851fe3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sat, 4 Jul 2015 19:14:56 -0700
Subject: [PATCH] Now use Vector unknowns -> exactly same result as ceres...

---
 timing/timeSFMBAL.cpp | 37 ++++++++++++++++++++++++-------------
 1 file changed, 24 insertions(+), 13 deletions(-)

diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp
index 763c22af0..fce535d14 100644
--- a/timing/timeSFMBAL.cpp
+++ b/timing/timeSFMBAL.cpp
@@ -45,6 +45,7 @@ typedef GeneralSFMFactor<Camera, Point3> SfmFactor;
 #include <gtsam/3rdparty/ceres/example.h>
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/AdaptAutoDiff.h>
+// See http://www.cs.cornell.edu/~snavely/bundler/bundler-v0.3-manual.html for conventions
 // 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,
@@ -76,14 +77,15 @@ int main(int argc, char* argv[]) {
   using symbol_shorthand::P;
 
   // Load BAL file (default is tiny)
-  string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre");
+  //string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre");
+  string defaultFilename = "/home/frank/problem-16-22106-pre.txt";
   SfM_data db;
   bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db);
   if (!success)
     throw runtime_error("Could not access file!");
 
 #ifndef USE_GTSAM_FACTOR
-  AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> snavely;
+  AdaptAutoDiff<SnavelyProjection, 2, 9, 3> snavely;
 #endif
 
   // Build graph
@@ -96,11 +98,11 @@ int main(int argc, char* argv[]) {
 #ifdef USE_GTSAM_FACTOR
       graph.push_back(SfmFactor(z, unit2, i, P(j)));
 #else
-      Expression<Camera> camera_(i);
-      Expression<Point3> point_(P(j));
+      Expression<Vector9> camera_(i);
+      Expression<Vector3> point_(P(j));
       // Snavely expects measurements in OpenGL format, with y increasing upwards
-      graph.addExpressionFactor(unit2, Point2(z.x(), -z.y()),
-                                Expression<Point2>(snavely, camera_, point_));
+      graph.addExpressionFactor(unit2, Vector2(z.x(), -z.y()),
+                                Expression<Vector2>(snavely, camera_, point_));
 #endif
     }
   }
@@ -113,21 +115,31 @@ int main(int argc, char* argv[]) {
 #else
     // readBAL converts to GTSAM format, so we need to convert back !
     Camera ceresCamera(gtsam2openGL(camera.pose()), camera.calibration());
-    initial.insert((i++), ceresCamera);
+    Vector9 v9 = Camera().localCoordinates(ceresCamera);
+    initial.insert((i++), v9);
 #endif
   }
-  BOOST_FOREACH(const SfM_Track& track, db.tracks)
+  BOOST_FOREACH(const SfM_Track& track, db.tracks) {
+#ifdef USE_GTSAM_FACTOR
     initial.insert(P(j++), track.p);
+#else
+    Vector3 v3 = track.p.vector();
+    initial.insert(P(j++), v3);
+#endif
+  }
 
   // Check projection of first point in first camera
   Point2 expected = db.tracks.front().measurements.front().second;
+#ifdef USE_GTSAM_FACTOR
   Camera camera = initial.at<Camera>(0);
   Point3 point = initial.at<Point3>(P(0));
-#ifdef USE_GTSAM_FACTOR
   Point2 actual = camera.project(point);
 #else
-  Point2 opengl = snavely(camera, point);
-  Point2 actual(opengl.x(), -opengl.y());
+  Vector9 camera = initial.at<Vector9>(0);
+  Vector3 point = initial.at<Vector3>(P(0));
+  Point2 z = snavely(camera, point);
+  // Again: fix y to increase upwards
+  Point2 actual(z.x(), -z.y());
 #endif
   assert_equal(expected,actual,10);
 
@@ -149,8 +161,7 @@ int main(int argc, char* argv[]) {
   LevenbergMarquardtParams params;
   LevenbergMarquardtParams::SetCeresDefaults(&params);
   params.setOrdering(ordering);
-  params.setVerbosity("ERROR");
-  params.setVerbosityLM("TRYLAMBDA");
+  params.setVerbosityLM("SUMMARY");
   LevenbergMarquardtOptimizer lm(graph, initial, params);
   Values result = lm.optimize();