From cd77ec8fd4e1c03fc207b7ef1fe092dbcb69804f Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 3 May 2015 18:54:17 -0700
Subject: [PATCH] Added triangulation wrapping, tested and works in MATLAB !

---
 gtsam.h                                | 10 ++++
 matlab/gtsam_tests/testTriangulation.m | 70 ++++++++++++++++++++++++++
 2 files changed, 80 insertions(+)
 create mode 100644 matlab/gtsam_tests/testTriangulation.m

diff --git a/gtsam.h b/gtsam.h
index c56071c8f..6acc493fd 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -901,6 +901,16 @@ class StereoCamera {
   void serialize() const;
 };
 
+#include <gtsam/geometry/triangulation.h>
+
+// Templates appear not yet supported for free functions
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+    gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
+    double rank_tol, bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+    gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
+    double rank_tol, bool optimize);
+
 //*************************************************************************
 // Symbolic
 //*************************************************************************
diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m
new file mode 100644
index 000000000..d46493328
--- /dev/null
+++ b/matlab/gtsam_tests/testTriangulation.m
@@ -0,0 +1,70 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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
+%
+% @brief Test triangulation
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+import gtsam.*
+
+%% Some common constants
+sharedCal = Cal3_S2(1500, 1200, 0, 640, 480);
+
+%% Looking along X-axis, 1 meter above ground plane (x-y)
+upright = Rot3.Ypr(-pi / 2, 0., -pi / 2);
+pose1 = Pose3(upright, Point3(0, 0, 1));
+camera1 = SimpleCamera(pose1, sharedCal);
+
+%% create second camera 1 meter to the right of first camera
+pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)));
+camera2 = SimpleCamera(pose2, sharedCal);
+
+%% landmark ~5 meters infront of camera
+landmark =Point3 (5, 0.5, 1.2);
+
+%% 1. Project two landmarks into two cameras and triangulate
+z1 = camera1.project(landmark);
+z2 = camera2.project(landmark);
+
+%% twoPoses
+poses = Pose3Vector;
+measurements = Point2Vector;
+
+poses.push_back(pose1);
+poses.push_back(pose2);
+measurements.push_back(z1);
+measurements.push_back(z2);
+
+optimize = true;
+rank_tol = 1e-9;
+
+triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize);
+CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));
+
+%% 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+measurements = Point2Vector;
+measurements.push_back(z1.retract([0.1;0.5]));
+measurements.push_back(z2.retract([-0.2;0.3]));
+
+triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize);
+CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-2));
+
+%% two Poses with Bundler Calibration
+bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480);
+camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal);
+camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal);
+
+z1 = camera1.project(landmark);
+z2 = camera2.project(landmark);
+
+measurements = Point2Vector;
+measurements.push_back(z1);
+measurements.push_back(z2);
+
+triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize);
+CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));