From 2d98056497aac3fa467a7cc41f8b8a7745ff6deb Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sun, 7 Jun 2015 20:24:45 -0700
Subject: [PATCH 1/3] organize headers

---
 gtsam/slam/GeneralSFMFactor.h | 23 +++++++++++++++++++++--
 gtsam/slam/dataset.cpp        | 29 ++++++++++++++++++++++-------
 gtsam/slam/dataset.h          | 17 +++++++++++------
 3 files changed, 54 insertions(+), 15 deletions(-)

diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index 31c8270a4..2516b2bcc 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -20,12 +20,31 @@
 
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/base/concepts.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/types.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/Vector.h>
+
+#include <boost/none.hpp>
+#include <boost/optional/optional.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/smart_ptr/shared_ptr.hpp>
 #include <iostream>
+#include <string>
+
+namespace boost {
+namespace serialization {
+class access;
+} /* namespace serialization */
+} /* namespace boost */
 
 namespace gtsam {
 
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 851adacf5..5ad1ff2c0 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -15,19 +15,34 @@
  * @brief utility functions for loading datasets
  */
 
-#include <gtsam/slam/dataset.h>
-#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/linear/Sampler.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/Values-inl.h>
+#include <gtsam/linear/Sampler.h>
+#include <gtsam/base/GenericValue.h>
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/types.h>
+#include <gtsam/base/Value.h>
+#include <gtsam/base/Vector.h>
 
-#include <boost/filesystem.hpp>
+#include <boost/assign/list_inserter.hpp>
+#include <boost/filesystem/operations.hpp>
+#include <boost/filesystem/path.hpp>
+#include <boost/foreach.hpp>
 
+#include <cmath>
 #include <fstream>
-#include <sstream>
-#include <cstdlib>
+#include <iostream>
+#include <stdexcept>
 
 using namespace std;
 namespace fs = boost::filesystem;
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 21c23f0e0..54e27229c 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -18,16 +18,21 @@
 
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/linear/NoiseModel.h>
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/base/types.h>
 
-#include <vector>
-#include <utility> // for pair
+#include <boost/smart_ptr/shared_ptr.hpp>
 #include <string>
+#include <utility> // for pair
+#include <vector>
 
 namespace gtsam {
 

From 1ae0f256a161b88732f70a876090b8c5fef0ab99 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sun, 7 Jun 2015 20:25:35 -0700
Subject: [PATCH 2/3] Moved test to right place (geometry tests should not be
 relying on optimization code, and this was a test of the factor, not
 pinholecamera)

---
 gtsam/geometry/tests/testPinholeCamera.cpp | 32 ----------
 tests/testGeneralSFMFactorB.cpp            | 72 ++++++++++++++++++++++
 2 files changed, 72 insertions(+), 32 deletions(-)
 create mode 100644 tests/testGeneralSFMFactorB.cpp

diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp
index 9fa9e3468..0e610d8d6 100644
--- a/gtsam/geometry/tests/testPinholeCamera.cpp
+++ b/gtsam/geometry/tests/testPinholeCamera.cpp
@@ -21,10 +21,6 @@
 #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>
 
@@ -325,34 +321,6 @@ 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); }
 /* ************************************************************************* */
diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp
new file mode 100644
index 000000000..d43e7e31a
--- /dev/null
+++ b/tests/testGeneralSFMFactorB.cpp
@@ -0,0 +1,72 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testGeneralSFMFactorB.cpp
+ * @author Frank Dellaert
+ * @brief test general SFM class, with nonlinear optimization and BAL files
+ */
+
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/NoiseModel.h>
+
+#include <CppUnitLite/Failure.h>
+#include <CppUnitLite/Test.h>
+#include <CppUnitLite/TestRegistry.h>
+#include <CppUnitLite/TestResult.h>
+#include <boost/foreach.hpp>
+#include <stddef.h>
+#include <stdexcept>
+#include <string>
+
+using namespace std;
+using namespace gtsam;
+
+typedef GeneralSFMFactor<PinholeCamera<Cal3Bundler>, 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);
+}
+/* ************************************************************************* */

From e93c346e024844b7c8ca0226d779ff2336e5b3f3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sun, 7 Jun 2015 21:18:37 -0700
Subject: [PATCH 3/3] Fixed minor warning triggering in tests

---
 gtsam/base/Manifold.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index b20c60822..d7ea9ea4c 100644
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@@ -138,7 +138,7 @@ class IsManifold {
 public:
 
   typedef typename traits<T>::structure_category structure_category_tag;
-  static const size_t dim = traits<T>::dimension;
+  static const int dim = traits<T>::dimension;
   typedef typename traits<T>::ManifoldType ManifoldType;
   typedef typename traits<T>::TangentVector TangentVector;