Merge branch 'feature/LM/triangulation'

gtsam_unstable/geometry/TriangulationFactor.h

@@ -0,0 +1,159 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * testTriangulationFactor.h
+ * @date March 2, 2014
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <boost/optional.hpp>
+
+namespace gtsam {
+
+/**
+ * Non-linear factor for a constraint derived from a 2D measurement.
+ * The calibration and pose are assumed known.
+ * @addtogroup SLAM
+ */
+template<class CALIBRATION = Cal3_S2>
+class TriangulationFactor: public NoiseModelFactor1<Point3> {
+
+public:
+
+  /// Camera type
+  typedef PinholeCamera<CALIBRATION> Camera;
+
+protected:
+
+  // Keep a copy of measurement and calibration for I/O
+  const Camera camera_; ///< Camera in which this landmark was seen
+  const Point2 measured_; ///< 2D measurement
+
+  // verbosity handling for Cheirality Exceptions
+  const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
+  const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
+
+public:
+
+  /// shorthand for base class type
+  typedef NoiseModelFactor1<Point3> Base;
+
+  /// shorthand for this class
+  typedef TriangulationFactor<CALIBRATION> This;
+
+  /// shorthand for a smart pointer to a factor
+  typedef boost::shared_ptr<This> shared_ptr;
+
+  /// Default constructor
+  TriangulationFactor() :
+      throwCheirality_(false), verboseCheirality_(false) {
+  }
+
+  /**
+   * Constructor with exception-handling flags
+   * @param camera is the camera in which unknown landmark is seen
+   * @param measured is the 2 dimensional location of point in image (the measurement)
+   * @param model is the standard deviation
+   * @param pointKey is the index of the landmark
+   * @param throwCheirality determines whether Cheirality exceptions are rethrown
+   * @param verboseCheirality determines whether exceptions are printed for Cheirality
+   */
+  TriangulationFactor(const Camera& camera, const Point2& measured,
+      const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
+      bool verboseCheirality = false) :
+      Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
+          throwCheirality), verboseCheirality_(verboseCheirality) {
+  }
+
+  /** Virtual destructor */
+  virtual ~TriangulationFactor() {
+  }
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /**
+   * print
+   * @param s optional string naming the factor
+   * @param keyFormatter optional formatter useful for printing Symbols
+   */
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const {
+    std::cout << s << "TriangulationFactor,";
+    camera_.print("camera");
+    measured_.print("z");
+    Base::print("", keyFormatter);
+  }
+
+  /// equals
+  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+    const This *e = dynamic_cast<const This*>(&p);
+    return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol)
+        && this->measured_.equals(e->measured_, tol);
+  }
+
+  /// Evaluate error h(x)-z and optionally derivatives
+  Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
+      boost::none) const {
+    try {
+      Point2 error(camera_.project(point, boost::none, H2) - measured_);
+      return error.vector();
+    } catch (CheiralityException& e) {
+      if (H2)
+        *H2 = zeros(2, 3);
+      if (verboseCheirality_)
+        std::cout << e.what() << ": Landmark "
+            << DefaultKeyFormatter(this->key()) << " moved behind camera"
+            << std::endl;
+      if (throwCheirality_)
+        throw e;
+      return ones(2) * 2.0 * camera_.calibration().fx();
+    }
+  }
+
+  /** return the measurement */
+  const Point2& measured() const {
+    return measured_;
+  }
+
+  /** return verbosity */
+  inline bool verboseCheirality() const {
+    return verboseCheirality_;
+  }
+
+  /** return flag for throwing cheirality exceptions */
+  inline bool throwCheirality() const {
+    return throwCheirality_;
+  }
+
+private:
+
+  /// Serialization function
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar & BOOST_SERIALIZATION_NVP(camera_);
+    ar & BOOST_SERIALIZATION_NVP(measured_);
+    ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
+    ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
+  }
+};
+} // \ namespace gtsam
+
+

gtsam_unstable/geometry/tests/testTriangulation.cpp

@@ -16,50 +16,46 @@
  *      Author: cbeall3
  */
 
-#include <CppUnitLite/TestHarness.h>
-
-#include <gtsam/base/Testable.h>
-#include <gtsam/geometry/SimpleCamera.h>
-#include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/Cal3Bundler.h>
-
-#include <gtsam_unstable/geometry/InvDepthCamera3.h>
 #include <gtsam_unstable/geometry/triangulation.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <CppUnitLite/TestHarness.h>
 
 #include <boost/assign.hpp>
 #include <boost/assign/std/vector.hpp>
-#include <boost/make_shared.hpp>
 
 using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
-/* ************************************************************************* */
+// Some common constants
 
-TEST( triangulation, twoPosesBundler) {
+static const boost::shared_ptr<Cal3_S2> sharedCal = //
-  boost::shared_ptr<Cal3Bundler> sharedCal = //
+    boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
-      boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480);
-  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
-      gtsam::Point3(0, 0, 1));
-  PinholeCamera<Cal3Bundler> level_camera(level_pose, *sharedCal);
 
-  // create second camera 1 meter to the right of first camera
+// Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
+static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2);
-  PinholeCamera<Cal3Bundler> level_camera_right(level_pose_right, *sharedCal);
+static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
+PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
 
-  // landmark ~5 meters infront of camera
+// create second camera 1 meter to the right of first camera
-  Point3 landmark(5, 0.5, 1.2);
+static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
+PinholeCamera<Cal3_S2> camera2(pose2, *sharedCal);
 
-  // 1. Project two landmarks into two cameras and triangulate
+// landmark ~5 meters infront of camera
-  Point2 level_uv = level_camera.project(landmark);
+static const Point3 landmark(5, 0.5, 1.2);
-  Point2 level_uv_right = level_camera_right.project(landmark);
 
-  vector < Pose3 > poses;
+// 1. Project two landmarks into two cameras and triangulate
+Point2 z1 = camera1.project(landmark);
+Point2 z2 = camera2.project(landmark);
+
+//******************************************************************************
+TEST( triangulation, twoPoses) {
+
+  vector<Pose3> poses;
   vector<Point2> measurements;
 
-  poses += level_pose, level_pose_right;
+  poses += pose1, pose2;
-  measurements += level_uv, level_uv_right;
+  measurements += z1, z2;
 
   bool optimize = true;
   double rank_tol = 1e-9;
@@ -77,32 +73,48 @@ TEST( triangulation, twoPosesBundler) {
   EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 
-TEST( triangulation, fourPoses) {
+TEST( triangulation, twoPosesBundler) {
-  boost::shared_ptr<Cal3_S2> sharedCal = //
-      boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
-  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
-      gtsam::Point3(0, 0, 1));
-  SimpleCamera level_camera(level_pose, *sharedCal);
 
-  // create second camera 1 meter to the right of first camera
+  boost::shared_ptr<Cal3Bundler> bundlerCal = //
-  Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
+      boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480);
-  SimpleCamera level_camera_right(level_pose_right, *sharedCal);
+  PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal);
-
+  PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal);
-  // landmark ~5 meters infront of camera
-  Point3 landmark(5, 0.5, 1.2);
 
   // 1. Project two landmarks into two cameras and triangulate
-  Point2 level_uv = level_camera.project(landmark);
+  Point2 z1 = camera1.project(landmark);
-  Point2 level_uv_right = level_camera_right.project(landmark);
+  Point2 z2 = camera2.project(landmark);
 
-  vector < Pose3 > poses;
+  vector<Pose3> poses;
   vector<Point2> measurements;
 
-  poses += level_pose, level_pose_right;
+  poses += pose1, pose2;
-  measurements += level_uv, level_uv_right;
+  measurements += z1, z2;
+
+  bool optimize = true;
+  double rank_tol = 1e-9;
+
+  boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
+      bundlerCal, measurements, rank_tol, optimize);
+  EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
+
+  // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+  measurements.at(0) += Point2(0.1, 0.5);
+  measurements.at(1) += Point2(-0.2, 0.3);
+
+  boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
+      bundlerCal, measurements, rank_tol, optimize);
+  EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
+}
+
+//******************************************************************************
+TEST( triangulation, fourPoses) {
+  vector<Pose3> poses;
+  vector<Point2> measurements;
+
+  poses += pose1, pose2;
+  measurements += z1, z2;
 
   boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
       sharedCal, measurements);
@@ -112,21 +124,20 @@ TEST( triangulation, fourPoses) {
   measurements.at(0) += Point2(0.1, 0.5);
   measurements.at(1) += Point2(-0.2, 0.3);
 
-  boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
+  boost::optional<Point3> triangulated_landmark_noise = //
-      sharedCal, measurements);
+      triangulatePoint3(poses, sharedCal, measurements);
   EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
-  Pose3 pose_top = level_pose
+  Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
-      * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  SimpleCamera camera3(pose3, *sharedCal);
-  SimpleCamera camera_top(pose_top, *sharedCal);
+  Point2 z3 = camera3.project(landmark);
-  Point2 top_uv = camera_top.project(landmark);
 
-  poses += pose_top;
+  poses += pose3;
-  measurements += top_uv + Point2(0.1, -0.1);
+  measurements += z3 + Point2(0.1, -0.1);
 
-  boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses,
+  boost::optional<Point3> triangulated_3cameras = //
-      sharedCal, measurements);
+      triangulatePoint3(poses, sharedCal, measurements);
   EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
 
   // Again with nonlinear optimization
@@ -135,48 +146,42 @@ TEST( triangulation, fourPoses) {
   EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
 
   // 4. Test failure: Add a 4th camera facing the wrong way
-  Pose3 level_pose180 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2),
+  Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
-      Point3(0, 0, 1));
+  SimpleCamera camera4(pose4, *sharedCal);
-  SimpleCamera camera_180(level_pose180, *sharedCal);
 
-  CHECK_EXCEPTION(camera_180.project(landmark) ;, CheiralityException);
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+  CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException);
 
-  poses += level_pose180;
+  poses += pose4;
   measurements += Point2(400, 400);
 
   CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
       TriangulationCheiralityException);
+#endif
 }
 
-/* ************************************************************************* */
+//******************************************************************************
-
 TEST( triangulation, fourPoses_distinct_Ks) {
   Cal3_S2 K1(1500, 1200, 0, 640, 480);
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
+  SimpleCamera camera1(pose1, K1);
-      gtsam::Point3(0, 0, 1));
-  SimpleCamera level_camera(level_pose, K1);
 
   // create second camera 1 meter to the right of first camera
-  Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
   Cal3_S2 K2(1600, 1300, 0, 650, 440);
-  SimpleCamera level_camera_right(level_pose_right, K2);
+  SimpleCamera camera2(pose2, K2);
-
-  // landmark ~5 meters infront of camera
-  Point3 landmark(5, 0.5, 1.2);
 
   // 1. Project two landmarks into two cameras and triangulate
-  Point2 level_uv = level_camera.project(landmark);
+  Point2 z1 = camera1.project(landmark);
-  Point2 level_uv_right = level_camera_right.project(landmark);
+  Point2 z2 = camera2.project(landmark);
 
   vector<SimpleCamera> cameras;
   vector<Point2> measurements;
 
-  cameras += level_camera, level_camera_right;
+  cameras += camera1, camera2;
-  measurements += level_uv, level_uv_right;
+  measurements += z1, z2;
 
-  boost::optional<Point3> triangulated_landmark = triangulatePoint3(cameras,
+  boost::optional<Point3> triangulated_landmark = //
-      measurements);
+      triangulatePoint3(cameras, measurements);
   EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
 
   // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
@@ -188,17 +193,16 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
-  Pose3 pose_top = level_pose
+  Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
-      * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
   Cal3_S2 K3(700, 500, 0, 640, 480);
-  SimpleCamera camera_top(pose_top, K3);
+  SimpleCamera camera3(pose3, K3);
-  Point2 top_uv = camera_top.project(landmark);
+  Point2 z3 = camera3.project(landmark);
 
-  cameras += camera_top;
+  cameras += camera3;
-  measurements += top_uv + Point2(0.1, -0.1);
+  measurements += z3 + Point2(0.1, -0.1);
 
-  boost::optional<Point3> triangulated_3cameras = triangulatePoint3(cameras,
+  boost::optional<Point3> triangulated_3cameras = //
-      measurements);
+      triangulatePoint3(cameras, measurements);
   EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
 
   // Again with nonlinear optimization
@@ -207,47 +211,40 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
 
   // 4. Test failure: Add a 4th camera facing the wrong way
-  Pose3 level_pose180 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2),
+  Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
-      Point3(0, 0, 1));
   Cal3_S2 K4(700, 500, 0, 640, 480);
-  SimpleCamera camera_180(level_pose180, K4);
+  SimpleCamera camera4(pose4, K4);
 
-  CHECK_EXCEPTION(camera_180.project(landmark) ;, CheiralityException);
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+  CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException);
 
-  cameras += camera_180;
+  cameras += camera4;
   measurements += Point2(400, 400);
   CHECK_EXCEPTION(triangulatePoint3(cameras, measurements),
       TriangulationCheiralityException);
+#endif
 }
 
-/* ************************************************************************* */
+//******************************************************************************
-
 TEST( triangulation, twoIdenticalPoses) {
-  boost::shared_ptr<Cal3_S2> sharedCal = //
-      boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
+  SimpleCamera camera1(pose1, *sharedCal);
-      gtsam::Point3(0, 0, 1));
-  SimpleCamera level_camera(level_pose, *sharedCal);
-
-  // landmark ~5 meters infront of camera
-  Point3 landmark(5, 0.5, 1.2);
 
   // 1. Project two landmarks into two cameras and triangulate
-  Point2 level_uv = level_camera.project(landmark);
+  Point2 z1 = camera1.project(landmark);
 
-  vector < Pose3 > poses;
+  vector<Pose3> poses;
   vector<Point2> measurements;
 
-  poses += level_pose, level_pose;
+  poses += pose1, pose1;
-  measurements += level_uv, level_uv;
+  measurements += z1, z1;
 
   CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
       TriangulationUnderconstrainedException);
 }
 
-/* ************************************************************************* *
+//******************************************************************************
-
+/*
  TEST( triangulation, onePose) {
  // we expect this test to fail with a TriangulationUnderconstrainedException
  // because there's only one camera observation
@@ -263,10 +260,34 @@ TEST( triangulation, twoIdenticalPoses) {
  CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal),
  TriangulationUnderconstrainedException);
  }
+ */
 
- /* ************************************************************************* */
+//******************************************************************************
+TEST( triangulation, TriangulationFactor ) {
+  // Create the factor with a measurement that is 3 pixels off in x
+  Key pointKey(1);
+  SharedNoiseModel model;
+  typedef TriangulationFactor<> Factor;
+  Factor factor(camera1, z1, model, pointKey, sharedCal);
+
+  // Use the factor to calculate the Jacobians
+  Matrix HActual;
+  factor.evaluateError(landmark, HActual);
+
+//  Matrix expectedH1 = numericalDerivative11<Pose3>(
+//      boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
+//          boost::none, boost::none), pose1);
+  // The expected Jacobian
+  Matrix HExpected = numericalDerivative11<Point3>(
+      boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
+
+  // Verify the Jacobians are correct
+  CHECK(assert_equal(HExpected, HActual, 1e-3));
+}
+
+//******************************************************************************
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }
-/* ************************************************************************* */
+//******************************************************************************

gtsam_unstable/geometry/triangulation.cpp

@@ -18,6 +18,9 @@
 
 #include <gtsam_unstable/geometry/triangulation.h>
 
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+
 namespace gtsam {
 
 /**

gtsam_unstable/geometry/triangulation.h

@@ -18,19 +18,11 @@
 
 #pragma once
 
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/ProjectionFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam_unstable/base/dllexport.h>
-
+#include <gtsam_unstable/geometry/TriangulationFactor.h>
-#include <boost/foreach.hpp>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <boost/assign.hpp>
+#include <gtsam/inference/Symbol.h>
-#include <boost/assign/std/vector.hpp>
+#include <gtsam/slam/PriorFactor.h>
 
 #include <vector>
 
@@ -60,10 +52,9 @@ public:
  * @param rank_tol SVD rank tolerance
  * @return Triangulated Point3
  */
-GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices, const std::vector<Point2>& measurements, double rank_tol);
+GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT(
-
+    const std::vector<Matrix>& projection_matrices,
-// Frank says: putting priors on poses and then optimizing is a terrible idea: we turn a 3dof problem into a much more difficult problem
+    const std::vector<Point2>& measurements, double rank_tol);
-// We should have a projectionfactor that knows pose is fixed
 
 ///
 /**
@@ -87,10 +78,9 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
   static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
   for (size_t i = 0; i < measurements.size(); i++) {
     const Pose3& pose_i = poses[i];
-    graph.push_back(GenericProjectionFactor<Pose3, Point3, CALIBRATION> //
+    PinholeCamera<CALIBRATION> camera_i(pose_i, *sharedCal);
-        (measurements[i], unit2, i, landmarkKey, sharedCal));
+    graph.push_back(TriangulationFactor<CALIBRATION> //
-    graph.push_back(PriorFactor<Pose3>(i, pose_i, prior_model));
+        (camera_i, measurements[i], unit2, landmarkKey));
-    values.insert(i, pose_i);
   }
   return std::make_pair(graph, values);
 }
@@ -116,13 +106,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
   static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
   for (size_t i = 0; i < measurements.size(); i++) {
     const PinholeCamera<CALIBRATION>& camera_i = cameras[i];
-    boost::shared_ptr<CALIBRATION> // Seems wasteful to create new object
+    graph.push_back(TriangulationFactor<CALIBRATION> //
-    sharedCal(new CALIBRATION(camera_i.calibration()));
+        (camera_i, measurements[i], unit2, landmarkKey));
-    graph.push_back(GenericProjectionFactor<Pose3, Point3, CALIBRATION> //
-        (measurements[i], unit2, i, landmarkKey, sharedCal));
-    const Pose3& pose_i = camera_i.pose();
-    graph.push_back(PriorFactor<Pose3>(i, pose_i, prior_model));
-    values.insert(i, pose_i);
   }
   return std::make_pair(graph, values);
 }
@@ -135,7 +120,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
  * @param landmarkKey to refer to landmark
  * @return refined Point3
  */
-GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey);
+GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
+    const Values& values, Key landmarkKey);
 
 /**
  * Given an initial estimate , refine a point using measurements in several cameras
@@ -221,7 +207,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   BOOST_FOREACH(const Pose3& pose, poses) {
     const Point3& p_local = pose.transform_to(point);
     if (p_local.z() <= 0)
-      throw(TriangulationCheiralityException());
+    throw(TriangulationCheiralityException());
   }
 #endif
 
@@ -271,7 +257,7 @@ Point3 triangulatePoint3(
   BOOST_FOREACH(const Camera& camera, cameras) {
     const Point3& p_local = camera.pose().transform_to(point);
     if (p_local.z() <= 0)
-      throw(TriangulationCheiralityException());
+    throw(TriangulationCheiralityException());
   }
 #endif