From 0df5740a3978aa0ff70609d9a1ccc913f5df0c5c Mon Sep 17 00:00:00 2001
From: Sungtae An <stan84@gatech.edu>
Date: Sun, 22 Feb 2015 18:29:56 -0500
Subject: [PATCH] Add a unit test using BAL data

---
 gtsam/geometry/tests/testPinholeCamera.cpp | 32 ++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp
index 0e610d8d6..9fa9e3468 100644
--- a/gtsam/geometry/tests/testPinholeCamera.cpp
+++ b/gtsam/geometry/tests/testPinholeCamera.cpp
@@ -21,6 +21,10 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -321,6 +325,34 @@ TEST( PinholeCamera, range3) {
   EXPECT(assert_equal(Hexpected2, D2, 1e-7));
 }
 
+/* ************************************************************************* */
+typedef GeneralSFMFactor<Camera2, Point3> sfmFactor;
+using symbol_shorthand::P;
+
+/* ************************************************************************* */
+TEST( PinholeCamera, BAL) {
+  string filename = findExampleDataFile("dubrovnik-3-7-pre");
+  SfM_data db;
+  bool success = readBAL(filename, db);
+  if (!success) throw runtime_error("Could not access file!");
+
+  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)
+              graph.push_back(sfmFactor(m.second, unit2, m.first, P(j)));
+  }
+
+  Values initial = initialCamerasAndPointsEstimate(db);
+
+  LevenbergMarquardtOptimizer lm(graph, initial);
+
+  Values actual = lm.optimize();
+  double actualError = graph.error(actual);
+  EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-7);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */