diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py
index 1f6732f49..80f573d12 100644
--- a/cython/gtsam/examples/ImuFactorExample2.py
+++ b/cython/gtsam/examples/ImuFactorExample2.py
@@ -14,6 +14,11 @@ from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
 
 import gtsam
 import gtsam.utils.plot as gtsam_plot
+from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
+                   ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
+                   PinholeCameraCal3_S2, Point3, Pose3,
+                   PriorFactorConstantBias, PriorFactorPose3,
+                   PriorFactorVector, Rot3, Values)
 
 
 def X(key):
@@ -69,8 +74,8 @@ PARAMS.setUse2ndOrderCoriolis(False)
 PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
 
 BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
-DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0),
-                    gtsam.Point3(0.05, -0.10, 0.20))
+DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
+              Point3(0.05, -0.10, 0.20))
 
 
 def IMU_example():
@@ -78,10 +83,10 @@ def IMU_example():
 
     # Start with a camera on x-axis looking at origin
     radius = 30
-    up = gtsam.Point3(0, 0, 1)
-    target = gtsam.Point3(0, 0, 0)
-    position = gtsam.Point3(radius, 0, 0)
-    camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
+    up = Point3(0, 0, 1)
+    target = Point3(0, 0, 0)
+    position = Point3(radius, 0, 0)
+    camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
     pose_0 = camera.pose()
 
     # Create the set of ground-truth landmarks and poses
@@ -90,37 +95,37 @@ def IMU_example():
 
     angular_velocity_vector = vector3(0, -angular_velocity, 0)
     linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
-    scenario = gtsam.ConstantTwistScenario(
+    scenario = ConstantTwistScenario(
         angular_velocity_vector, linear_velocity_vector, pose_0)
 
     # Create a factor graph
-    newgraph = gtsam.NonlinearFactorGraph()
+    newgraph = NonlinearFactorGraph()
 
     # Create (incremental) ISAM2 solver
-    isam = gtsam.ISAM2()
+    isam = ISAM2()
 
     # Create the initial estimate to the solution
     # Intentionally initialize the variables off from the ground truth
-    initialEstimate = gtsam.Values()
+    initialEstimate = Values()
 
     # Add a prior on pose x0. This indirectly specifies where the origin is.
     # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
     noise = gtsam.noiseModel_Diagonal.Sigmas(
         np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
-    newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
+    newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise))
 
     # Add imu priors
     biasKey = gtsam.symbol(ord('b'), 0)
     biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
-    biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
-                                              biasnoise)
+    biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
+                                        biasnoise)
     newgraph.push_back(biasprior)
     initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
     velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
 
     # Calculate with correct initial velocity
     n_velocity = vector3(0, angular_velocity * radius, 0)
-    velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
+    velprior = PriorFactorVector(V(0), n_velocity, velnoise)
     newgraph.push_back(velprior)
     initialEstimate.insert(V(0), n_velocity)
 
@@ -141,7 +146,7 @@ def IMU_example():
             # Add Bias variables periodically
             if i % 5 == 0:
                 biasKey += 1
-                factor = gtsam.BetweenFactorConstantBias(
+                factor = BetweenFactorConstantBias(
                     biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
                 newgraph.add(factor)
                 initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
@@ -154,8 +159,7 @@ def IMU_example():
             accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
 
             # Add Imu Factor
-            imufac = gtsam.ImuFactor(
-                X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
+            imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
             newgraph.add(imufac)
 
             # insert new velocity, which is wrong
@@ -168,7 +172,7 @@ def IMU_example():
         ISAM2_plot(result)
 
         # reset
-        newgraph = gtsam.NonlinearFactorGraph()
+        newgraph = NonlinearFactorGraph()
         initialEstimate.clear()
 
 
diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py
index d4451d44e..7b664f2d0 100644
--- a/cython/gtsam/examples/SFMExample.py
+++ b/cython/gtsam/examples/SFMExample.py
@@ -79,7 +79,7 @@ def main():
 
     # Simulated measurements from each camera pose, adding them to the factor graph
     for i, pose in enumerate(poses):
-        camera = SimpleCamera(pose, K)
+        camera = PinholeCameraCal3_S2(pose, K)
         for j, point in enumerate(points):
             measurement = camera.project(point)
             factor = GenericProjectionFactorCal3_S2(
diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py
index f4d81eaf7..b7b83e2a1 100644
--- a/cython/gtsam/examples/VisualISAMExample.py
+++ b/cython/gtsam/examples/VisualISAMExample.py
@@ -18,7 +18,7 @@ from gtsam.examples import SFMdata
 from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
                          NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
                          PriorFactorPoint3, PriorFactorPose3, Rot3,
-                         SimpleCamera, Values)
+                         PinholeCameraCal3_S2, Values)
 
 
 def symbol(name: str, index: int) -> int:
@@ -54,7 +54,7 @@ def main():
 
     # Loop over the different poses, adding the observations to iSAM incrementally
     for i, pose in enumerate(poses):
-        camera = SimpleCamera(pose, K)
+        camera = PinholeCameraCal3_S2(pose, K)
         # Add factors for each landmark observation
         for j, point in enumerate(points):
             measurement = camera.project(point)
diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py
index efdfec561..a3654a5f1 100644
--- a/cython/gtsam/tests/test_SimpleCamera.py
+++ b/cython/gtsam/tests/test_SimpleCamera.py
@@ -5,7 +5,7 @@ All Rights Reserved
 
 See LICENSE for the license information
 
-SimpleCamera unit tests.
+PinholeCameraCal3_S2 unit tests.
 Author: Frank Dellaert & Duy Nguyen Ta (Python)
 """
 import math
@@ -14,7 +14,7 @@ import unittest
 import numpy as np
 
 import gtsam
-from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera
+from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2
 from gtsam.utils.test_case import GtsamTestCase
 
 K = Cal3_S2(625, 625, 0, 0, 0)
@@ -23,14 +23,14 @@ class TestSimpleCamera(GtsamTestCase):
 
     def test_constructor(self):
         pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
-        camera = SimpleCamera(pose1, K)
+        camera = PinholeCameraCal3_S2(pose1, K)
         self.gtsamAssertEquals(camera.calibration(), K, 1e-9)
         self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
 
     def test_level2(self):
         # Create a level camera, looking in Y-direction
         pose2 = Pose2(0.4,0.3,math.pi/2.0)
-        camera = SimpleCamera.Level(K, pose2, 0.1)
+        camera = PinholeCameraCal3_S2.Level(K, pose2, 0.1)
 
         # expected
         x = Point3(1,0,0)
diff --git a/cython/gtsam/utils/visual_data_generator.py b/cython/gtsam/utils/visual_data_generator.py
index 91194c565..f04588e70 100644
--- a/cython/gtsam/utils/visual_data_generator.py
+++ b/cython/gtsam/utils/visual_data_generator.py
@@ -1,8 +1,9 @@
 from __future__ import print_function
 
 import numpy as np
-from math import pi, cos, sin
+
 import gtsam
+from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3
 
 
 class Options:
@@ -10,7 +11,7 @@ class Options:
     Options to generate test scenario
     """
 
-    def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()):
+    def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()):
         """
         Options to generate test scenario
         @param triangle: generate a triangle scene with 3 points if True, otherwise
@@ -27,10 +28,10 @@ class GroundTruth:
     Object holding generated ground-truth data
     """
 
-    def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
+    def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
         self.K = K
-        self.cameras = [gtsam.Pose3()] * nrCameras
-        self.points = [gtsam.Point3()] * nrPoints
+        self.cameras = [Pose3()] * nrCameras
+        self.points = [Point3()] * nrPoints
 
     def print_(self, s=""):
         print(s)
@@ -52,11 +53,11 @@ class Data:
     class NoiseModels:
         pass
 
-    def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
+    def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
         self.K = K
-        self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras]
+        self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras]
         self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
-        self.odometry = [gtsam.Pose3()] * nrCameras
+        self.odometry = [Pose3()] * nrCameras
 
         # Set Noise parameters
         self.noiseModels = Data.NoiseModels()
@@ -73,7 +74,7 @@ class Data:
 def generate_data(options):
     """ Generate ground-truth and measurement data. """
 
-    K = gtsam.Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
+    K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
     nrPoints = 3 if options.triangle else 8
 
     truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
@@ -83,26 +84,26 @@ def generate_data(options):
     if options.triangle:  # Create a triangle target, just 3 points on a plane
         r = 10
         for j in range(len(truth.points)):
-            theta = j * 2 * pi / nrPoints
-            truth.points[j] = gtsam.Point3(r * cos(theta), r * sin(theta), 0)
+            theta = j * 2 * np.pi / nrPoints
+            truth.points[j] = Point3(r * np.cos(theta), r * np.sin(theta), 0)
     else:  # 3D landmarks as vertices of a cube
         truth.points = [
-            gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10),
-            gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10),
-            gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10),
-            gtsam.Point3(-10, -10, -10), gtsam.Point3(10, -10, -10)
+            Point3(10, 10, 10), Point3(-10, 10, 10),
+            Point3(-10, -10, 10), Point3(10, -10, 10),
+            Point3(10, 10, -10), Point3(-10, 10, -10),
+            Point3(-10, -10, -10), Point3(10, -10, -10)
         ]
 
     # Create camera cameras on a circle around the triangle
     height = 10
     r = 40
     for i in range(options.nrCameras):
-        theta = i * 2 * pi / options.nrCameras
-        t = gtsam.Point3(r * cos(theta), r * sin(theta), height)
-        truth.cameras[i] = gtsam.SimpleCamera.Lookat(t,
-                                                     gtsam.Point3(),
-                                                     gtsam.Point3(0, 0, 1),
-                                                     truth.K)
+        theta = i * 2 * np.pi / options.nrCameras
+        t = Point3(r * np.cos(theta), r * np.sin(theta), height)
+        truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
+                                                       Point3(),
+                                                       Point3(0, 0, 1),
+                                                       truth.K)
         # Create measurements
         for j in range(nrPoints):
             # All landmarks seen in every frame
diff --git a/cython/gtsam_unstable/examples/TimeOfArrivalExample.py b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py
new file mode 100644
index 000000000..6ba06f0f2
--- /dev/null
+++ b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py
@@ -0,0 +1,128 @@
+"""
+GTSAM Copyright 2010-2020, 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
+
+Track a moving object "Time of Arrival" measurements at 4 microphones.
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, no-name-in-module
+
+from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
+                   NonlinearFactorGraph, Point3, Values, noiseModel_Isotropic)
+from gtsam_unstable import Event, TimeOfArrival, TOAFactor
+
+# units
+MS = 1e-3
+CM = 1e-2
+
+# Instantiate functor with speed of sound value
+TIME_OF_ARRIVAL = TimeOfArrival(330)
+
+
+def define_microphones():
+    """Create microphones."""
+    height = 0.5
+    microphones = []
+    microphones.append(Point3(0, 0, height))
+    microphones.append(Point3(403 * CM, 0, height))
+    microphones.append(Point3(403 * CM, 403 * CM, height))
+    microphones.append(Point3(0, 403 * CM, 2 * height))
+    return microphones
+
+
+def create_trajectory(n):
+    """Create ground truth trajectory."""
+    trajectory = []
+    timeOfEvent = 10
+    # simulate emitting a sound every second while moving on straight line
+    for key in range(n):
+        trajectory.append(
+            Event(timeOfEvent, 245 * CM + key * 1.0, 201.5 * CM, (212 - 45) * CM))
+        timeOfEvent += 1
+
+    return trajectory
+
+
+def simulate_one_toa(microphones, event):
+    """Simulate time-of-arrival measurements for a single event."""
+    return [TIME_OF_ARRIVAL.measure(event, microphones[i])
+            for i in range(len(microphones))]
+
+
+def simulate_toa(microphones, trajectory):
+    """Simulate time-of-arrival measurements for an entire trajectory."""
+    return [simulate_one_toa(microphones, event)
+            for event in trajectory]
+
+
+def create_graph(microphones, simulatedTOA):
+    """Create factor graph."""
+    graph = NonlinearFactorGraph()
+
+    # Create a noise model for the TOA error
+    model = noiseModel_Isotropic.Sigma(1, 0.5 * MS)
+
+    K = len(microphones)
+    key = 0
+    for toa in simulatedTOA:
+        for i in range(K):
+            factor = TOAFactor(key, microphones[i], toa[i], model)
+            graph.push_back(factor)
+        key += 1
+
+    return graph
+
+
+def create_initial_estimate(n):
+    """Create initial estimate for n events."""
+    initial = Values()
+    zero = Event()
+    for key in range(n):
+        TOAFactor.InsertEvent(key, zero, initial)
+    return initial
+
+
+def toa_example():
+    """Run example with 4 microphones and 5 events in a straight line."""
+    # Create microphones
+    microphones = define_microphones()
+    K = len(microphones)
+    for i in range(K):
+        print("mic {} = {}".format(i, microphones[i]))
+
+    # Create a ground truth trajectory
+    n = 5
+    groundTruth = create_trajectory(n)
+    for event in groundTruth:
+        print(event)
+
+    # Simulate time-of-arrival measurements
+    simulatedTOA = simulate_toa(microphones, groundTruth)
+    for key in range(n):
+        for i in range(K):
+            print("z_{}{} = {} ms".format(key, i, simulatedTOA[key][i] / MS))
+
+    # create factor graph
+    graph = create_graph(microphones, simulatedTOA)
+    print(graph.at(0))
+
+    # Create initial estimate
+    initial_estimate = create_initial_estimate(n)
+    print(initial_estimate)
+
+    # Optimize using Levenberg-Marquardt optimization.
+    params = LevenbergMarquardtParams()
+    params.setAbsoluteErrorTol(1e-10)
+    params.setVerbosityLM("SUMMARY")
+    optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)
+    result = optimizer.optimize()
+    print("Final Result:\n", result)
+
+
+if __name__ == '__main__':
+    toa_example()
+    print("Example complete")
diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp
index e3408b67b..b12418098 100644
--- a/examples/CameraResectioning.cpp
+++ b/examples/CameraResectioning.cpp
@@ -18,7 +18,8 @@
 
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <boost/make_shared.hpp>
 
 using namespace gtsam;
@@ -47,7 +48,7 @@ public:
   /// evaluate the error
   virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
       boost::none) const {
-    SimpleCamera camera(pose, *K_);
+    PinholeCamera<Cal3_S2> camera(pose, *K_);
     return camera.project(P_, H, boost::none, boost::none) - p_;
   }
 };
diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp
index d8fee1516..21fe6e661 100644
--- a/examples/ISAM2Example_SmartFactor.cpp
+++ b/examples/ISAM2Example_SmartFactor.cpp
@@ -4,7 +4,8 @@
  * @author Alexander (pumaking on BitBucket)
  */
 
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/SmartProjectionPoseFactor.h>
diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp
index 25a6adf51..f83a539af 100644
--- a/examples/ImuFactorExample2.cpp
+++ b/examples/ImuFactorExample2.cpp
@@ -1,5 +1,6 @@
 
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/navigation/ImuFactor.h>
@@ -34,7 +35,7 @@ int main(int argc, char* argv[]) {
   double radius = 30;
   const Point3 up(0, 0, 1), target(0, 0, 0);
   const Point3 position(radius, 0, 0);
-  const SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
+  const auto camera = PinholeCamera<Cal3_S2>::Lookat(position, target, up);
   const auto pose_0 = camera.pose();
 
   // Now, create a constant-twist scenario that makes the camera orbit the
diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp
index 8c87fa315..16cd25dba 100644
--- a/examples/SFMExample.cpp
+++ b/examples/SFMExample.cpp
@@ -79,7 +79,7 @@ int main(int argc, char* argv[]) {
 
   // Simulated measurements from each camera pose, adding them to the factor graph
   for (size_t i = 0; i < poses.size(); ++i) {
-    SimpleCamera camera(poses[i], *K);
+    PinholeCamera<Cal3_S2> camera(poses[i], *K);
     for (size_t j = 0; j < points.size(); ++j) {
       Point2 measurement = camera.project(points[j]);
       graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp
index 191664ef6..73b6f27f7 100644
--- a/examples/SFMExampleExpressions.cpp
+++ b/examples/SFMExampleExpressions.cpp
@@ -27,7 +27,7 @@
 #include <gtsam/nonlinear/ExpressionFactorGraph.h>
 
 // Header order is close to far
-#include <examples/SFMdata.h>
+#include "SFMdata.h"
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/nonlinear/DoglegOptimizer.h>
 #include <gtsam/nonlinear/Values.h>
@@ -67,7 +67,7 @@ int main(int argc, char* argv[]) {
   // Simulated measurements from each camera pose, adding them to the factor graph
   for (size_t i = 0; i < poses.size(); ++i) {
     Pose3_ x('x', i);
-    SimpleCamera camera(poses[i], K);
+    PinholeCamera<Cal3_S2> camera(poses[i], K);
     for (size_t j = 0; j < points.size(); ++j) {
       Point2 measurement = camera.project(points[j]);
       // Below an expression for the prediction of the measurement:
diff --git a/examples/SFMdata.h b/examples/SFMdata.h
index 5462868c9..04d3c9e47 100644
--- a/examples/SFMdata.h
+++ b/examples/SFMdata.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    SFMMdata.h
+ * @file    SFMdata.h
  * @brief   Simple example for the structure-from-motion problems
  * @author  Duy-Nguyen Ta
  */
@@ -30,7 +30,8 @@
 #include <gtsam/geometry/Point3.h>
 
 // We will also need a camera object to hold calibration information and perform projections.
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 
 /* ************************************************************************* */
 std::vector<gtsam::Point3> createPoints() {
diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp
index ba097c074..1d26ea0aa 100644
--- a/examples/SelfCalibrationExample.cpp
+++ b/examples/SelfCalibrationExample.cpp
@@ -61,7 +61,7 @@ int main(int argc, char* argv[]) {
   noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
   for (size_t i = 0; i < poses.size(); ++i) {
     for (size_t j = 0; j < points.size(); ++j) {
-      SimpleCamera camera(poses[i], K);
+      PinholeCamera<Cal3_S2> camera(poses[i], K);
       Point2 measurement = camera.project(points[j]);
       // The only real difference with the Visual SLAM example is that here we use a
       // different factor type, that also calculates the Jacobian with respect to calibration
diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp
index 35bc9bcf6..db9090de6 100644
--- a/examples/VisualISAM2Example.cpp
+++ b/examples/VisualISAM2Example.cpp
@@ -89,7 +89,7 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < poses.size(); ++i) {
     // Add factors for each landmark observation
     for (size_t j = 0; j < points.size(); ++j) {
-      SimpleCamera camera(poses[i], *K);
+      PinholeCamera<Cal3_S2> camera(poses[i], *K);
       Point2 measurement = camera.project(points[j]);
       graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
           measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp
index 7a58deeb7..b4086910b 100644
--- a/examples/VisualISAMExample.cpp
+++ b/examples/VisualISAMExample.cpp
@@ -86,7 +86,7 @@ int main(int argc, char* argv[]) {
     // Add factors for each landmark observation
     for (size_t j = 0; j < points.size(); ++j) {
       // Create ground truth measurement
-      SimpleCamera camera(poses[i], *K);
+      PinholeCamera<Cal3_S2> camera(poses[i], *K);
       Point2 measurement = camera.project(points[j]);
       // Add measurement
       graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, noise,
diff --git a/gtsam.h b/gtsam.h
index 04a1b1ac4..8ee778f4c 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -281,7 +281,7 @@ virtual class Value {
 };
 
 #include <gtsam/base/GenericValue.h>
-template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
 virtual class GenericValue : gtsam::Value {
   void serializable() const;
 };
@@ -2138,7 +2138,7 @@ class Values {
   void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
   void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
   void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
-  void insert(size_t j, const gtsam::SimpleCamera& simpel_camera);
+  void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, Vector vector);
   void insert(size_t j, Matrix matrix);
@@ -2459,7 +2459,7 @@ class ISAM2 {
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
                      gtsam::Cal3Bundler, gtsam::EssentialMatrix,
-                     gtsam::SimpleCamera, Vector, Matrix}>
+                     gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}>
   VALUE calculateEstimate(size_t key) const;
   gtsam::Values calculateBestEstimate() const;
   Matrix marginalCovariance(size_t key) const;
@@ -2497,7 +2497,7 @@ class NonlinearISAM {
 #include <gtsam/geometry/StereoPoint2.h>
 
 #include <gtsam/slam/PriorFactor.h>
-template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
@@ -2520,7 +2520,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
 
 
 #include <gtsam/nonlinear/NonlinearEquality.h>
-template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
 virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   // Constructor - forces exact evaluation
   NonlinearEquality(size_t j, const T& feasible);
@@ -2546,9 +2546,9 @@ typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
 typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
-typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
+typedef gtsam::RangeFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> RangeFactorSimpleCameraPoint;
 typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
-typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
+typedef gtsam::RangeFactor<gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3_S2> RangeFactorSimpleCamera;
 
 
 #include <gtsam/sam/RangeFactor.h>
@@ -2639,7 +2639,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
   GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey);
   gtsam::Point2 measured() const;
 };
-typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
 // due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
 //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 
@@ -3153,7 +3153,7 @@ namespace utilities {
   void perturbPoint2(gtsam::Values& values, double sigma, int seed);
   void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
   void perturbPoint3(gtsam::Values& values, double sigma, int seed);
-  void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
+  void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCameraCal3_S2& c, Vector J, Matrix Z, double depth);
   void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
   void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
   Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index 8214b0727..7c73f3cbd 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -187,8 +187,8 @@ struct HasBearing {
 };
 
 // Similar helper class for to implement Range traits for classes with a range method
-// For classes with overloaded range methods, such as SimpleCamera, this can even be templated:
-//   template <typename T> struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
+// For classes with overloaded range methods, such as PinholeCamera, this can even be templated:
+//   template <typename T> struct Range<PinholeCamera, T> : HasRange<PinholeCamera, T, double> {};
 template <class A1, typename A2, class RT>
 struct HasRange {
   typedef RT result_type;
diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp
index 97412f94d..d2f0c91df 100644
--- a/gtsam/geometry/tests/testTriangulation.cpp
+++ b/gtsam/geometry/tests/testTriangulation.cpp
@@ -17,7 +17,7 @@
  */
 
 #include <gtsam/geometry/triangulation.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/Cal3Bundler.h>
@@ -151,7 +151,7 @@ TEST( triangulation, fourPoses) {
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
   Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
-  SimpleCamera camera3(pose3, *sharedCal);
+  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
   Point2 z3 = camera3.project(landmark);
 
   poses += pose3;
@@ -168,7 +168,7 @@ TEST( triangulation, fourPoses) {
 
   // 4. Test failure: Add a 4th camera facing the wrong way
   Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
-  SimpleCamera camera4(pose4, *sharedCal);
+  PinholeCamera<Cal3_S2> camera4(pose4, *sharedCal);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
@@ -185,17 +185,17 @@ TEST( triangulation, fourPoses) {
 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)
-  SimpleCamera camera1(pose1, K1);
+  PinholeCamera<Cal3_S2> camera1(pose1, K1);
 
   // create second camera 1 meter to the right of first camera
   Cal3_S2 K2(1600, 1300, 0, 650, 440);
-  SimpleCamera camera2(pose2, K2);
+  PinholeCamera<Cal3_S2> camera2(pose2, K2);
 
   // 1. Project two landmarks into two cameras and triangulate
   Point2 z1 = camera1.project(landmark);
   Point2 z2 = camera2.project(landmark);
 
-  CameraSet<SimpleCamera> cameras;
+  CameraSet<PinholeCamera<Cal3_S2> > cameras;
   Point2Vector measurements;
 
   cameras += camera1, camera2;
@@ -216,7 +216,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   // 3. Add a slightly rotated third camera above, again with measurement noise
   Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
   Cal3_S2 K3(700, 500, 0, 640, 480);
-  SimpleCamera camera3(pose3, K3);
+  PinholeCamera<Cal3_S2> camera3(pose3, K3);
   Point2 z3 = camera3.project(landmark);
 
   cameras += camera3;
@@ -234,7 +234,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   // 4. Test failure: Add a 4th camera facing the wrong way
   Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   Cal3_S2 K4(700, 500, 0, 640, 480);
-  SimpleCamera camera4(pose4, K4);
+  PinholeCamera<Cal3_S2> camera4(pose4, K4);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
@@ -250,17 +250,17 @@ TEST( triangulation, fourPoses_distinct_Ks) {
 TEST( triangulation, outliersAndFarLandmarks) {
   Cal3_S2 K1(1500, 1200, 0, 640, 480);
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  SimpleCamera camera1(pose1, K1);
+  PinholeCamera<Cal3_S2> camera1(pose1, K1);
 
   // create second camera 1 meter to the right of first camera
   Cal3_S2 K2(1600, 1300, 0, 650, 440);
-  SimpleCamera camera2(pose2, K2);
+  PinholeCamera<Cal3_S2> camera2(pose2, K2);
 
   // 1. Project two landmarks into two cameras and triangulate
   Point2 z1 = camera1.project(landmark);
   Point2 z2 = camera2.project(landmark);
 
-  CameraSet<SimpleCamera> cameras;
+  CameraSet<PinholeCamera<Cal3_S2> > cameras;
   Point2Vector measurements;
 
   cameras += camera1, camera2;
@@ -280,7 +280,7 @@ TEST( triangulation, outliersAndFarLandmarks) {
   // 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER)
   Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
   Cal3_S2 K3(700, 500, 0, 640, 480);
-  SimpleCamera camera3(pose3, K3);
+  PinholeCamera<Cal3_S2> camera3(pose3, K3);
   Point2 z3 = camera3.project(landmark);
 
   cameras += camera3;
@@ -302,7 +302,7 @@ TEST( triangulation, outliersAndFarLandmarks) {
 //******************************************************************************
 TEST( triangulation, twoIdenticalPoses) {
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  SimpleCamera camera1(pose1, *sharedCal);
+  PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
 
   // 1. Project two landmarks into two cameras and triangulate
   Point2 z1 = camera1.project(landmark);
diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h
index 7d02d479c..9e8aa3f29 100644
--- a/gtsam/nonlinear/Expression.h
+++ b/gtsam/nonlinear/Expression.h
@@ -66,7 +66,7 @@ public:
   // Expressions wrap trees of functions that can evaluate their own derivatives.
   // The meta-functions below are useful to specify the type of those functions.
   // Example, a function taking a camera and a 3D point and yielding a 2D point:
-  //   Expression<Point2>::BinaryFunction<SimpleCamera,Point3>::type
+  //   Expression<Point2>::BinaryFunction<PinholeCamera<Cal3_S2>,Point3>::type
   template<class A1>
   struct UnaryFunction {
     typedef boost::function<
diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h
index 867db70e0..cf9fb9482 100644
--- a/gtsam/nonlinear/utilities.h
+++ b/gtsam/nonlinear/utilities.h
@@ -28,7 +28,7 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 
 #include <exception>
 
@@ -169,7 +169,7 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
 }
 
 /// Insert a number of initial point values by backprojecting
-void insertBackprojections(Values& values, const SimpleCamera& camera,
+void insertBackprojections(Values& values, const PinholeCamera<Cal3_S2>& camera,
     const Vector& J, const Matrix& Z, double depth) {
   if (Z.rows() != 2)
     throw std::invalid_argument("insertBackProjections: Z must be 2*K");
diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp
index 5d57886f5..54d4a43c0 100644
--- a/gtsam/sam/tests/testRangeFactor.cpp
+++ b/gtsam/sam/tests/testRangeFactor.cpp
@@ -19,7 +19,8 @@
 #include <gtsam/sam/RangeFactor.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/base/TestableAssertions.h>
@@ -353,11 +354,13 @@ TEST(RangeFactor, Point3) {
 }
 
 /* ************************************************************************* */
-// Do tests with SimpleCamera
+// Do tests with PinholeCamera<Cal3_S2>
 TEST( RangeFactor, Camera) {
-  RangeFactor<SimpleCamera,Point3> factor1(poseKey, pointKey, measurement, model);
-  RangeFactor<SimpleCamera,Pose3> factor2(poseKey, pointKey, measurement, model);
-  RangeFactor<SimpleCamera,SimpleCamera> factor3(poseKey, pointKey, measurement, model);
+  using Camera = PinholeCamera<Cal3_S2>;
+
+  RangeFactor<Camera, Point3> factor1(poseKey, pointKey, measurement, model);
+  RangeFactor<Camera, Pose3>  factor2(poseKey, pointKey, measurement, model);
+  RangeFactor<Camera, Camera> factor3(poseKey, pointKey, measurement, model);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index 2e921bfef..8bd155a14 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -9,7 +9,7 @@
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/EssentialMatrix.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <iostream>
 
 namespace gtsam {
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index 4a8a6b138..856913aae 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -21,7 +21,8 @@
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <boost/optional.hpp>
 
 namespace gtsam {
diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
index 6d6daa33d..95db611d7 100644
--- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp
+++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
@@ -13,6 +13,7 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp
index 1d2baefee..4bbef6530 100644
--- a/gtsam/slam/tests/testTriangulationFactor.cpp
+++ b/gtsam/slam/tests/testTriangulationFactor.cpp
@@ -17,7 +17,7 @@
  */
 
 #include <gtsam/geometry/triangulation.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/nonlinear/Expression.h>
@@ -39,7 +39,7 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
 // Looking along X-axis, 1 meter above ground plane (x-y)
 static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
 static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
-SimpleCamera camera1(pose1, *sharedCal);
+PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
 
 // landmark ~5 meters infront of camera
 static const Point3 landmark(5, 0.5, 1.2);
@@ -52,7 +52,7 @@ TEST( triangulation, TriangulationFactor ) {
 
   Key pointKey(1);
   SharedNoiseModel model;
-  typedef TriangulationFactor<SimpleCamera> Factor;
+  typedef TriangulationFactor<PinholeCamera<Cal3_S2> > Factor;
   Factor factor(camera1, z1, model, pointKey);
 
   // Use the factor to calculate the Jacobians
diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp
new file mode 100644
index 000000000..9991e04b6
--- /dev/null
+++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp
@@ -0,0 +1,160 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, 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  TimeOfArrivalExample.cpp
+ *  @brief Track a moving object "Time of Arrival" measurements at 4
+ * microphones.
+ *  @author Frank Dellaert
+ *  @author Jay Chakravarty
+ *  @date March 2020
+ */
+
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam_unstable/slam/TOAFactor.h>
+
+#include <boost/bind.hpp>
+#include <boost/format.hpp>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+// units
+static const double ms = 1e-3;
+static const double cm = 1e-2;
+
+// Instantiate functor with speed of sound value
+static const TimeOfArrival kTimeOfArrival(330);
+
+/* ************************************************************************* */
+// Create microphones
+vector<Point3> defineMicrophones() {
+  const double height = 0.5;
+  vector<Point3> microphones;
+  microphones.push_back(Point3(0, 0, height));
+  microphones.push_back(Point3(403 * cm, 0, height));
+  microphones.push_back(Point3(403 * cm, 403 * cm, height));
+  microphones.push_back(Point3(0, 403 * cm, 2 * height));
+  return microphones;
+}
+
+/* ************************************************************************* */
+// Create ground truth trajectory
+vector<Event> createTrajectory(int n) {
+  vector<Event> trajectory;
+  double timeOfEvent = 10;
+  // simulate emitting a sound every second while moving on straight line
+  for (size_t key = 0; key < n; key++) {
+    trajectory.push_back(
+        Event(timeOfEvent, 245 * cm + key * 1.0, 201.5 * cm, (212 - 45) * cm));
+    timeOfEvent += 1;
+  }
+  return trajectory;
+}
+
+/* ************************************************************************* */
+// Simulate time-of-arrival measurements for a single event
+vector<double> simulateTOA(const vector<Point3>& microphones,
+                           const Event& event) {
+  size_t K = microphones.size();
+  vector<double> simulatedTOA(K);
+  for (size_t i = 0; i < K; i++) {
+    simulatedTOA[i] = kTimeOfArrival(event, microphones[i]);
+  }
+  return simulatedTOA;
+}
+
+/* ************************************************************************* */
+// Simulate time-of-arrival measurements for an entire trajectory
+vector<vector<double>> simulateTOA(const vector<Point3>& microphones,
+                                   const vector<Event>& trajectory) {
+  vector<vector<double>> simulatedTOA;
+  for (auto event : trajectory) {
+    simulatedTOA.push_back(simulateTOA(microphones, event));
+  }
+  return simulatedTOA;
+}
+
+/* ************************************************************************* */
+// create factor graph
+NonlinearFactorGraph createGraph(const vector<Point3>& microphones,
+                                 const vector<vector<double>>& simulatedTOA) {
+  NonlinearFactorGraph graph;
+
+  // Create a noise model for the TOA error
+  auto model = noiseModel::Isotropic::Sigma(1, 0.5 * ms);
+
+  size_t K = microphones.size();
+  size_t key = 0;
+  for (auto toa : simulatedTOA) {
+    for (size_t i = 0; i < K; i++) {
+      graph.emplace_shared<TOAFactor>(key, microphones[i], toa[i], model);
+    }
+    key += 1;
+  }
+  return graph;
+}
+
+/* ************************************************************************* */
+// create initial estimate for n events
+Values createInitialEstimate(int n) {
+  Values initial;
+
+  Event zero;
+  for (size_t key = 0; key < n; key++) {
+    initial.insert(key, zero);
+  }
+  return initial;
+}
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+  // Create microphones
+  auto microphones = defineMicrophones();
+  size_t K = microphones.size();
+  for (size_t i = 0; i < K; i++) {
+    cout << "mic" << i << " = " << microphones[i] << endl;
+  }
+
+  // Create a ground truth trajectory
+  const size_t n = 5;
+  auto groundTruth = createTrajectory(n);
+
+  // Simulate time-of-arrival measurements
+  auto simulatedTOA = simulateTOA(microphones, groundTruth);
+  for (size_t key = 0; key < n; key++) {
+    for (size_t i = 0; i < K; i++) {
+      cout << "z_" << key << i << " = " << simulatedTOA[key][i] / ms << " ms"
+           << endl;
+    }
+  }
+
+  // Create factor graph
+  auto graph = createGraph(microphones, simulatedTOA);
+
+  // Create initial estimate
+  auto initialEstimate = createInitialEstimate(n);
+  initialEstimate.print("Initial Estimate:\n");
+
+  // Optimize using Levenberg-Marquardt optimization.
+  LevenbergMarquardtParams params;
+  params.setAbsoluteErrorTol(1e-10);
+  params.setVerbosityLM("SUMMARY");
+  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
+  Values result = optimizer.optimize();
+  result.print("Final Result:\n");
+}
+/* ************************************************************************* */
diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp
index c503514a6..45a24c101 100644
--- a/gtsam_unstable/geometry/Event.cpp
+++ b/gtsam_unstable/geometry/Event.cpp
@@ -24,15 +24,16 @@ namespace gtsam {
 
 /* ************************************************************************* */
 void Event::print(const std::string& s) const {
-  std::cout << s << "time = " << time_ << "location = " << location_.transpose();
+  std::cout << s << "{'time':" << time_
+            << ", 'location': " << location_.transpose() << "}";
 }
 
 /* ************************************************************************* */
 bool Event::equals(const Event& other, double tol) const {
-  return std::abs(time_ - other.time_) < tol
-      && traits<Point3>::Equals(location_, other.location_, tol);
+  return std::abs(time_ - other.time_) < tol &&
+         traits<Point3>::Equals(location_, other.location_, tol);
 }
 
 /* ************************************************************************* */
 
-} //\ namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h
index fc186857f..383c34915 100644
--- a/gtsam_unstable/geometry/Event.h
+++ b/gtsam_unstable/geometry/Event.h
@@ -20,42 +20,43 @@
 #pragma once
 
 #include <gtsam/geometry/Point3.h>
+#include <gtsam_unstable/dllexport.h>
+
 #include <cmath>
 #include <iosfwd>
-#include <gtsam_unstable/dllexport.h>
+#include <string>
 
 namespace gtsam {
 
-/// A space-time event
+/**
+ * A space-time event models an event that happens at a certain 3D location, at
+ * a certain time. One use for it is in sound-based or UWB-ranging tracking or
+ * SLAM, where we have "time of arrival" measurements at a set of sensors. The
+ * TOA functor below provides a measurement function for those applications.
+ */
 class Event {
+  double time_;      ///< Time event was generated
+  Point3 location_;  ///< Location at time event was generated
 
-  double time_; ///< Time event was generated
-  Point3 location_; ///< Location at time event was generated
-
-public:
+ public:
   enum { dimension = 4 };
 
   /// Default Constructor
-  Event() :
-      time_(0), location_(0,0,0) {
-  }
+  Event() : time_(0), location_(0, 0, 0) {}
 
   /// Constructor from time and location
-  Event(double t, const Point3& p) :
-      time_(t), location_(p) {
-  }
+  Event(double t, const Point3& p) : time_(t), location_(p) {}
 
   /// Constructor with doubles
-  Event(double t, double x, double y, double z) :
-      time_(t), location_(x, y, z) {
-  }
+  Event(double t, double x, double y, double z)
+      : time_(t), location_(x, y, z) {}
 
-  double time() const { return time_;}
-  Point3 location() const { return location_;}
+  double time() const { return time_; }
+  Point3 location() const { return location_; }
 
-  // TODO we really have to think of a better way to do linear arguments
-  double height(OptionalJacobian<1,4> H = boost::none) const {
-    static const Matrix14 JacobianZ = (Matrix14() << 0,0,0,1).finished();
+  // TODO(frank) we really have to think of a better way to do linear arguments
+  double height(OptionalJacobian<1, 4> H = boost::none) const {
+    static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished();
     if (H) *H = JacobianZ;
     return location_.z();
   }
@@ -64,7 +65,8 @@ public:
   GTSAM_UNSTABLE_EXPORT void print(const std::string& s = "") const;
 
   /** equals with an tolerance */
-  GTSAM_UNSTABLE_EXPORT bool equals(const Event& other, double tol = 1e-9) const;
+  GTSAM_UNSTABLE_EXPORT bool equals(const Event& other,
+                                    double tol = 1e-9) const;
 
   /// Updates a with tangent space delta
   inline Event retract(const Vector4& v) const {
@@ -73,28 +75,44 @@ public:
 
   /// Returns inverse retraction
   inline Vector4 localCoordinates(const Event& q) const {
-    return Vector4::Zero(); // TODO
-  }
-
-  /// Time of arrival to given microphone
-  double toa(const Point3& microphone, //
-      OptionalJacobian<1, 4> H1 = boost::none, //
-      OptionalJacobian<1, 3> H2 = boost::none) const {
-    static const double Speed = 330;
-    Matrix13 D1, D2;
-    double distance = gtsam::distance3(location_, microphone, D1, D2);
-    if (H1)
-      // derivative of toa with respect to event
-      *H1 << 1.0, D1 / Speed;
-    if (H2)
-      // derivative of toa with respect to microphone location
-      *H2 << D2 / Speed;
-    return time_ + distance / Speed;
+    return Vector4::Zero();  // TODO(frank) implement!
   }
 };
 
 // Define GTSAM traits
-template<>
+template <>
 struct traits<Event> : internal::Manifold<Event> {};
 
-} //\ namespace gtsam
+/// Time of arrival to given sensor
+class TimeOfArrival {
+  const double speed_;  ///< signal speed
+
+ public:
+  typedef double result_type;
+
+  /// Constructor with optional speed of signal, in m/sec
+  explicit TimeOfArrival(double speed = 330) : speed_(speed) {}
+
+  /// Calculate time of arrival
+  double measure(const Event& event, const Point3& sensor) const {
+    double distance = gtsam::distance3(event.location(), sensor);
+    return event.time() + distance / speed_;
+  }
+
+  /// Calculate time of arrival, with derivatives
+  double operator()(const Event& event, const Point3& sensor,  //
+                    OptionalJacobian<1, 4> H1 = boost::none,   //
+                    OptionalJacobian<1, 3> H2 = boost::none) const {
+    Matrix13 D1, D2;
+    double distance = gtsam::distance3(event.location(), sensor, D1, D2);
+    if (H1)
+      // derivative of toa with respect to event
+      *H1 << 1.0, D1 / speed_;
+    if (H2)
+      // derivative of toa with respect to sensor location
+      *H2 << D2 / speed_;
+    return event.time() + distance / speed_;
+  }
+};
+
+}  // namespace gtsam
diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp
index ec8ca1f4b..0349f3293 100644
--- a/gtsam_unstable/geometry/tests/testEvent.cpp
+++ b/gtsam_unstable/geometry/tests/testEvent.cpp
@@ -17,10 +17,12 @@
  *  @date December 2014
  */
 
-#include <gtsam_unstable/geometry/Event.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/nonlinear/Expression.h>
+#include <gtsam_unstable/geometry/Event.h>
+
 #include <CppUnitLite/TestHarness.h>
+
 #include <boost/bind.hpp>
 
 using namespace std;
@@ -30,56 +32,59 @@ using namespace gtsam;
 static const double ms = 1e-3;
 static const double cm = 1e-2;
 typedef Eigen::Matrix<double, 1, 1> Vector1;
-static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms));
+static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
 
 static const double timeOfEvent = 25;
 static const Event exampleEvent(timeOfEvent, 1, 0, 0);
-static const Point3 microphoneAt0(0,0,0);
+static const Point3 microphoneAt0(0, 0, 0);
+
+static const double kSpeedOfSound = 340;
+static const TimeOfArrival kToa(kSpeedOfSound);
 
 //*****************************************************************************
-TEST( Event, Constructor ) {
+TEST(Event, Constructor) {
   const double t = 0;
   Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm);
 }
 
 //*****************************************************************************
-TEST( Event, Toa1 ) {
+TEST(Event, Toa1) {
   Event event(0, 1, 0, 0);
-  double expected = 1. / 330;
-  EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9);
+  double expected = 1. / kSpeedOfSound;
+  EXPECT_DOUBLES_EQUAL(expected, kToa(event, microphoneAt0), 1e-9);
 }
 
 //*****************************************************************************
-TEST( Event, Toa2 ) {
-  double expectedTOA = timeOfEvent + 1. / 330;
-  EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9);
+TEST(Event, Toa2) {
+  double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
+  EXPECT_DOUBLES_EQUAL(expectedTOA, kToa(exampleEvent, microphoneAt0), 1e-9);
 }
 
 //*************************************************************************
-TEST (Event, Derivatives) {
+TEST(Event, Derivatives) {
   Matrix14 actualH1;
   Matrix13 actualH2;
-  exampleEvent.toa(microphoneAt0, actualH1, actualH2);
+  kToa(exampleEvent, microphoneAt0, actualH1, actualH2);
   Matrix expectedH1 = numericalDerivative11<double, Event>(
-      boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none),
+      boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none),
       exampleEvent);
   EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
   Matrix expectedH2 = numericalDerivative11<double, Point3>(
-      boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none),
+      boost::bind(kToa, exampleEvent, _1, boost::none, boost::none),
       microphoneAt0);
   EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
 }
 
 //*****************************************************************************
-TEST( Event, Expression ) {
+TEST(Event, Expression) {
   Key key = 12;
   Expression<Event> event_(key);
-  Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
-  Expression<double> expression(&Event::toa, event_, knownMicrophone_);
+  Expression<Point3> knownMicrophone_(microphoneAt0);  // constant expression
+  Expression<double> expression(kToa, event_, knownMicrophone_);
 
   Values values;
   values.insert(key, exampleEvent);
-  double expectedTOA = timeOfEvent + 1. / 330;
+  double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
   EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9);
 }
 
@@ -97,4 +102,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 //*****************************************************************************
-
diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
index 6ac3389ed..4c6251831 100644
--- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
+++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
@@ -9,7 +9,8 @@
 
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 
 #include <gtsam_unstable/geometry/InvDepthCamera3.h>
 
@@ -18,7 +19,7 @@ using namespace gtsam;
 
 static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
 Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
-SimpleCamera level_camera(level_pose, *K);
+PinholeCamera<Cal3_S2> level_camera(level_pose, *K);
 
 /* ************************************************************************* */
 TEST( InvDepthFactor, Project1) {
diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h
index d77895d86..b46c5354b 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.h
@@ -377,6 +377,30 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
 
 typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> RangeFactorRTV;
 
+#include <gtsam_unstable/geometry/Event.h>
+class Event {
+  Event();
+  Event(double t, const gtsam::Point3& p);
+  Event(double t, double x, double y, double z);
+  double time() const;
+  gtsam::Point3 location() const;
+  double height() const;
+  void print(string s) const;
+};
+
+class TimeOfArrival {
+  TimeOfArrival();
+  TimeOfArrival(double speed);
+  double measure(const gtsam::Event& event, const gtsam::Point3& sensor) const;
+};
+
+#include <gtsam_unstable/slam/TOAFactor.h>
+virtual class TOAFactor : gtsam::NonlinearFactor {
+  // For now, because of overload issues, we only expose constructor with known sensor coordinates:
+  TOAFactor(size_t key1, gtsam::Point3 sensor, double measured,
+            const gtsam::noiseModel::Base* noiseModel);
+  static void InsertEvent(size_t key, const gtsam::Event& event, gtsam::Values* values);
+};
 
 #include <gtsam/nonlinear/NonlinearEquality.h>
 template<T = {gtsam::PoseRTV}>
diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h
index 3e1263bb9..5bef97bcf 100644
--- a/gtsam_unstable/slam/MultiProjectionFactor.h
+++ b/gtsam_unstable/slam/MultiProjectionFactor.h
@@ -21,7 +21,7 @@
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <boost/optional.hpp>
 
 namespace gtsam {
diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h
index 1df14a8de..8ad4a14a6 100644
--- a/gtsam_unstable/slam/TOAFactor.h
+++ b/gtsam_unstable/slam/TOAFactor.h
@@ -17,33 +17,51 @@
  *  @date December 2014
  */
 
+#pragma once
+
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam_unstable/geometry/Event.h>
 
 namespace gtsam {
 
 /// A "Time of Arrival" factor - so little code seems hardly worth it :-)
-class TOAFactor: public ExpressionFactor<double> {
-
+class TOAFactor : public ExpressionFactor<double> {
   typedef Expression<double> Double_;
 
-public:
-
+ public:
   /**
-   * Constructor
-   * @param some expression yielding an event
-   * @param microphone_ expression yielding a microphone location
-   * @param toaMeasurement time of arrival at microphone
+   * Most general constructor with two expressions
+   * @param eventExpression expression yielding an event
+   * @param sensorExpression expression yielding a sensor location
+   * @param toaMeasurement time of arrival at sensor
    * @param model noise model
+   * @param speed optional speed of signal, in m/sec
    */
   TOAFactor(const Expression<Event>& eventExpression,
-      const Expression<Point3>& microphone_, double toaMeasurement,
-      const SharedNoiseModel& model) :
-      ExpressionFactor<double>(model, toaMeasurement,
-          Double_(&Event::toa, eventExpression, microphone_)) {
-  }
+            const Expression<Point3>& sensorExpression, double toaMeasurement,
+            const SharedNoiseModel& model, double speed = 330)
+      : ExpressionFactor<double>(
+            model, toaMeasurement,
+            Double_(TimeOfArrival(speed), eventExpression, sensorExpression)) {}
 
+  /**
+   * Constructor with fixed sensor
+   * @param eventExpression expression yielding an event
+   * @param sensor a known sensor location
+   * @param toaMeasurement time of arrival at sensor
+   * @param model noise model
+   * @param toa optional time of arrival functor
+   */
+  TOAFactor(const Expression<Event>& eventExpression, const Point3& sensor,
+            double toaMeasurement, const SharedNoiseModel& model,
+            double speed = 330)
+      : TOAFactor(eventExpression, Expression<Point3>(sensor), toaMeasurement,
+                  model, speed) {}
+
+  static void InsertEvent(Key key, const Event& event,
+                          boost::shared_ptr<Values> values) {
+    values->insert(key, event);
+  }
 };
 
-} //\ namespace gtsam
-
+}  // namespace gtsam
diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp
index 17c95e614..5b4bd8929 100644
--- a/gtsam_unstable/slam/serialization.cpp
+++ b/gtsam_unstable/slam/serialization.cpp
@@ -31,20 +31,21 @@
 using namespace gtsam;
 
 // Creating as many permutations of factors as possible
-typedef PriorFactor<LieVector>         PriorFactorLieVector;
-typedef PriorFactor<LieMatrix>         PriorFactorLieMatrix;
-typedef PriorFactor<Point2>            PriorFactorPoint2;
-typedef PriorFactor<StereoPoint2>      PriorFactorStereoPoint2;
-typedef PriorFactor<Point3>            PriorFactorPoint3;
-typedef PriorFactor<Rot2>              PriorFactorRot2;
-typedef PriorFactor<Rot3>              PriorFactorRot3;
-typedef PriorFactor<Pose2>             PriorFactorPose2;
-typedef PriorFactor<Pose3>             PriorFactorPose3;
-typedef PriorFactor<Cal3_S2>           PriorFactorCal3_S2;
-typedef PriorFactor<Cal3DS2>           PriorFactorCal3DS2;
-typedef PriorFactor<CalibratedCamera>  PriorFactorCalibratedCamera;
-typedef PriorFactor<SimpleCamera>      PriorFactorSimpleCamera;
-typedef PriorFactor<StereoCamera>      PriorFactorStereoCamera;
+typedef PriorFactor<LieVector>            PriorFactorLieVector;
+typedef PriorFactor<LieMatrix>            PriorFactorLieMatrix;
+typedef PriorFactor<Point2>               PriorFactorPoint2;
+typedef PriorFactor<StereoPoint2>         PriorFactorStereoPoint2;
+typedef PriorFactor<Point3>               PriorFactorPoint3;
+typedef PriorFactor<Rot2>                 PriorFactorRot2;
+typedef PriorFactor<Rot3>                 PriorFactorRot3;
+typedef PriorFactor<Pose2>                PriorFactorPose2;
+typedef PriorFactor<Pose3>                PriorFactorPose3;
+typedef PriorFactor<Cal3_S2>              PriorFactorCal3_S2;
+typedef PriorFactor<Cal3DS2>              PriorFactorCal3DS2;
+typedef PriorFactor<CalibratedCamera>     PriorFactorCalibratedCamera;
+typedef PriorFactor<SimpleCamera>         PriorFactorSimpleCamera;
+typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
+typedef PriorFactor<StereoCamera>         PriorFactorStereoCamera;
 
 typedef BetweenFactor<LieVector>       BetweenFactorLieVector;
 typedef BetweenFactor<LieMatrix>       BetweenFactorLieMatrix;
@@ -55,29 +56,32 @@ typedef BetweenFactor<Rot3>            BetweenFactorRot3;
 typedef BetweenFactor<Pose2>           BetweenFactorPose2;
 typedef BetweenFactor<Pose3>           BetweenFactorPose3;
 
-typedef NonlinearEquality<LieVector>         NonlinearEqualityLieVector;
-typedef NonlinearEquality<LieMatrix>         NonlinearEqualityLieMatrix;
-typedef NonlinearEquality<Point2>            NonlinearEqualityPoint2;
-typedef NonlinearEquality<StereoPoint2>      NonlinearEqualityStereoPoint2;
-typedef NonlinearEquality<Point3>            NonlinearEqualityPoint3;
-typedef NonlinearEquality<Rot2>              NonlinearEqualityRot2;
-typedef NonlinearEquality<Rot3>              NonlinearEqualityRot3;
-typedef NonlinearEquality<Pose2>             NonlinearEqualityPose2;
-typedef NonlinearEquality<Pose3>             NonlinearEqualityPose3;
-typedef NonlinearEquality<Cal3_S2>           NonlinearEqualityCal3_S2;
-typedef NonlinearEquality<Cal3DS2>           NonlinearEqualityCal3DS2;
-typedef NonlinearEquality<CalibratedCamera>  NonlinearEqualityCalibratedCamera;
-typedef NonlinearEquality<SimpleCamera>      NonlinearEqualitySimpleCamera;
-typedef NonlinearEquality<StereoCamera>      NonlinearEqualityStereoCamera;
+typedef NonlinearEquality<LieVector>              NonlinearEqualityLieVector;
+typedef NonlinearEquality<LieMatrix>              NonlinearEqualityLieMatrix;
+typedef NonlinearEquality<Point2>                 NonlinearEqualityPoint2;
+typedef NonlinearEquality<StereoPoint2>           NonlinearEqualityStereoPoint2;
+typedef NonlinearEquality<Point3>                 NonlinearEqualityPoint3;
+typedef NonlinearEquality<Rot2>                   NonlinearEqualityRot2;
+typedef NonlinearEquality<Rot3>                   NonlinearEqualityRot3;
+typedef NonlinearEquality<Pose2>                  NonlinearEqualityPose2;
+typedef NonlinearEquality<Pose3>                  NonlinearEqualityPose3;
+typedef NonlinearEquality<Cal3_S2>                NonlinearEqualityCal3_S2;
+typedef NonlinearEquality<Cal3DS2>                NonlinearEqualityCal3DS2;
+typedef NonlinearEquality<CalibratedCamera>       NonlinearEqualityCalibratedCamera;
+typedef NonlinearEquality<SimpleCamera>           NonlinearEqualitySimpleCamera;
+typedef NonlinearEquality<PinholeCameraCal3_S2>   NonlinearEqualityPinholeCameraCal3_S2;
+typedef NonlinearEquality<StereoCamera>           NonlinearEqualityStereoCamera;
 
-typedef RangeFactor<Pose2, Point2>                      RangeFactor2D;
-typedef RangeFactor<Pose3, Point3>                      RangeFactor3D;
-typedef RangeFactor<Pose2, Pose2>                       RangeFactorPose2;
-typedef RangeFactor<Pose3, Pose3>                       RangeFactorPose3;
-typedef RangeFactor<CalibratedCamera, Point3>           RangeFactorCalibratedCameraPoint;
-typedef RangeFactor<SimpleCamera, Point3>               RangeFactorSimpleCameraPoint;
-typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
-typedef RangeFactor<SimpleCamera, SimpleCamera>         RangeFactorSimpleCamera;
+typedef RangeFactor<Pose2, Point2>                              RangeFactor2D;
+typedef RangeFactor<Pose3, Point3>                              RangeFactor3D;
+typedef RangeFactor<Pose2, Pose2>                               RangeFactorPose2;
+typedef RangeFactor<Pose3, Pose3>                               RangeFactorPose3;
+typedef RangeFactor<CalibratedCamera, Point3>                   RangeFactorCalibratedCameraPoint;
+typedef RangeFactor<SimpleCamera, Point3>                       RangeFactorSimpleCameraPoint;
+typedef RangeFactor<PinholeCameraCal3_S2, Point3>               RangeFactorPinholeCameraCal3_S2Point;
+typedef RangeFactor<CalibratedCamera, CalibratedCamera>         RangeFactorCalibratedCamera;
+typedef RangeFactor<SimpleCamera, SimpleCamera>                 RangeFactorSimpleCamera;
+typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
 
 typedef BearingRangeFactor<Pose2, Point2>  BearingRangeFactor2D;
 typedef BearingRangeFactor<Pose3, Point3>  BearingRangeFactor3D;
@@ -85,7 +89,7 @@ typedef BearingRangeFactor<Pose3, Point3>  BearingRangeFactor3D;
 typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;
 typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
 
-typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
 //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 
 typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
@@ -126,6 +130,7 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3DS2);
 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo);
 GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera);
 GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
+GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);
 GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
 
 /* Create GUIDs for factors */
diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
index a74bfc3c6..14ad43ae2 100644
--- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
+++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
@@ -7,7 +7,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -23,7 +23,7 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
 
 // camera pose at (0,0,1) looking straight along the x-axis.
 Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
-SimpleCamera level_camera(level_pose, *K);
+PinholeCamera<Cal3_S2> level_camera(level_pose, *K);
 
 typedef InvDepthFactor3<Pose3, Vector5, double> InverseDepthFactor;
 typedef NonlinearEquality<Pose3> PoseConstraint;
@@ -64,7 +64,7 @@ TEST( InvDepthFactor, optimize) {
 
   // add a camera 2 meters to the right
   Pose3 right_pose = level_pose * Pose3(Rot3(), Point3(2,0,0));
-  SimpleCamera right_camera(right_pose, *K);
+  PinholeCamera<Cal3_S2> right_camera(right_pose, *K);
 
   // projection measurement of landmark into right camera
   // this measurement disagrees with the depth initialization
diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
index 0ac2c24ee..14ac52c45 100644
--- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
+++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
@@ -523,9 +523,9 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
   Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
   Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
 
-  SimpleCamera cam1(cameraPose1, *K); // with camera poses
-  SimpleCamera cam2(cameraPose2, *K);
-  SimpleCamera cam3(cameraPose3, *K);
+  PinholeCamera<Cal3_S2> cam1(cameraPose1, *K); // with camera poses
+  PinholeCamera<Cal3_S2> cam2(cameraPose2, *K);
+  PinholeCamera<Cal3_S2> cam3(cameraPose3, *K);
 
   // create arbitrary body_Pose_sensor (transforms from sensor to body)
   Pose3 sensor_to_body =  Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp
index 4b10d5c0b..042130a24 100644
--- a/gtsam_unstable/slam/tests/testTOAFactor.cpp
+++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp
@@ -17,15 +17,16 @@
  *  @date December 2014
  */
 
-#include <gtsam_unstable/geometry/Event.h>
-#include <gtsam/nonlinear/ExpressionFactorGraph.h>
-#include <gtsam/nonlinear/expressions.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam_unstable/slam/TOAFactor.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/format.hpp>
 #include <boost/bind.hpp>
+#include <boost/format.hpp>
 
 using namespace std;
 using namespace gtsam;
@@ -43,83 +44,59 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
 
 static const double timeOfEvent = 25;
 static const Event exampleEvent(timeOfEvent, 1, 0, 0);
-static const Point3 microphoneAt0(0,0,0);
+static const Point3 sensorAt0(0, 0, 0);
 
 //*****************************************************************************
-TEST( TOAFactor, NewWay ) {
+TEST(TOAFactor, NewWay) {
   Key key = 12;
-  Event_ eventExpression(key);
-  Point3_ microphoneConstant(microphoneAt0); // constant expression
   double measurement = 7;
-  Double_ expression(&Event::toa, eventExpression, microphoneConstant);
-  ExpressionFactor<double> factor(model, measurement, expression);
+  TOAFactor factor(key, sensorAt0, measurement, model);
 }
 
 //*****************************************************************************
-TEST( TOAFactor, WholeEnchilada ) {
-
-  static const bool verbose = false;
-
-  // Create microphones
+TEST(TOAFactor, WholeEnchilada) {
+  // Create sensors
   const double height = 0.5;
-  vector<Point3> microphones;
-  microphones.push_back(Point3(0, 0, height));
-  microphones.push_back(Point3(403 * cm, 0, height));
-  microphones.push_back(Point3(403 * cm, 403 * cm, height));
-  microphones.push_back(Point3(0, 403 * cm, 2 * height));
-  EXPECT_LONGS_EQUAL(4, microphones.size());
-//  microphones.push_back(Point3(200 * cm, 200 * cm, height));
+  vector<Point3> sensors;
+  sensors.push_back(Point3(0, 0, height));
+  sensors.push_back(Point3(403 * cm, 0, height));
+  sensors.push_back(Point3(403 * cm, 403 * cm, height));
+  sensors.push_back(Point3(0, 403 * cm, 2 * height));
+  EXPECT_LONGS_EQUAL(4, sensors.size());
+  //  sensors.push_back(Point3(200 * cm, 200 * cm, height));
 
   // Create a ground truth point
   const double timeOfEvent = 0;
   Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);
 
   // Simulate simulatedTOA
-  size_t K = microphones.size();
+  size_t K = sensors.size();
   vector<double> simulatedTOA(K);
+  TimeOfArrival toa;
   for (size_t i = 0; i < K; i++) {
-    simulatedTOA[i] = groundTruthEvent.toa(microphones[i]);
-    if (verbose) {
-      cout << "mic" << i << " = " << microphones[i] << endl;
-      cout << "z" << i << " = " << simulatedTOA[i] / ms << endl;
-    }
+    simulatedTOA[i] = toa(groundTruthEvent, sensors[i]);
   }
 
   // Now, estimate using non-linear optimization
-  ExpressionFactorGraph graph;
+  NonlinearFactorGraph graph;
   Key key = 12;
-  Event_ eventExpression(key);
   for (size_t i = 0; i < K; i++) {
-    Point3_ microphone_i(microphones[i]); // constant expression
-    Double_ predictTOA(&Event::toa, eventExpression, microphone_i);
-    graph.addExpressionFactor(predictTOA, simulatedTOA[i], model);
+    graph.emplace_shared<TOAFactor>(key, sensors[i], simulatedTOA[i], model);
   }
 
-  /// Print the graph
-  if (verbose)
-    GTSAM_PRINT(graph);
-
   // Create initial estimate
   Values initialEstimate;
-  //Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
+  // Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
   Vector4 delta;
   delta << 0.1, 0.1, -0.1, 0.1;
   Event estimatedEvent = groundTruthEvent.retract(delta);
   initialEstimate.insert(key, estimatedEvent);
 
-  // Print
-  if (verbose)
-    initialEstimate.print("Initial Estimate:\n");
-
   // Optimize using Levenberg-Marquardt optimization.
   LevenbergMarquardtParams params;
   params.setAbsoluteErrorTol(1e-10);
-  if (verbose)
-    params.setVerbosity("ERROR");
   LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
   Values result = optimizer.optimize();
-  if (verbose)
-    result.print("Final Result:\n");
 
   EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
 }
@@ -129,4 +106,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 //*****************************************************************************
-
diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m
index 10fd5e142..035e7e509 100644
--- a/matlab/+gtsam/Contents.m
+++ b/matlab/+gtsam/Contents.m
@@ -94,6 +94,7 @@
 %   Rot2                              - class Rot2, see Doxygen page for details
 %   Rot3                              - class Rot3, see Doxygen page for details
 %   SimpleCamera                      - class SimpleCamera, see Doxygen page for details
+%   PinholeCameraCal3_S2              - class PinholeCameraCal3_S2, see Doxygen page for details
 %   StereoPoint2                      - class StereoPoint2, see Doxygen page for details
 % 
 %% SLAM and SFM
diff --git a/matlab/+gtsam/VisualISAMGenerateData.m b/matlab/+gtsam/VisualISAMGenerateData.m
index ab47e92db..57f9439a4 100644
--- a/matlab/+gtsam/VisualISAMGenerateData.m
+++ b/matlab/+gtsam/VisualISAMGenerateData.m
@@ -31,7 +31,7 @@ data.K = truth.K;
 for i=1:options.nrCameras
     theta = (i-1)*2*pi/options.nrCameras;
     t = Point3([r*cos(theta), r*sin(theta), height]');
-    truth.cameras{i} = SimpleCamera.Lookat(t, Point3, Point3([0,0,1]'), truth.K);
+    truth.cameras{i} = PinholeCameraCal3_S2.Lookat(t, Point3, Point3([0,0,1]'), truth.K);
     % Create measurements
     for j=1:nrPoints
         % All landmarks seen in every frame
diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m
index 71f72f6b9..2b913b52d 100644
--- a/matlab/+gtsam/cylinderSampleProjection.m
+++ b/matlab/+gtsam/cylinderSampleProjection.m
@@ -13,7 +13,7 @@ function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinder
 
 import gtsam.*
 
-camera = SimpleCamera(pose, K);
+camera = PinholeCameraCal3_S2(pose, K);
 
 %% memory allocation
 cylinderNum = length(cylinders);
diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m
index 36b7635e2..add2bc75a 100644
--- a/matlab/gtsam_examples/CameraFlyingExample.m
+++ b/matlab/gtsam_examples/CameraFlyingExample.m
@@ -92,7 +92,7 @@ if options.enableTests
         cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
     end
 
-    camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... 
+    camera = PinholeCameraCal3_S2.Lookat(Point3(10, 50, 10), ... 
         Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
         Point3([0,0,1]'), options.monoK); 
 
@@ -154,7 +154,7 @@ while 1
     
     %t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
     %    15, 10]');
-    camera = SimpleCamera.Lookat(t, ... 
+    camera = PinholeCameraCal3_S2.Lookat(t, ... 
         Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
         Point3([0,0,1]'), options.camera.monoK);    
     cameraPoses{end+1} = camera.pose;
diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m
index 7f50f2db8..584ace09a 100644
--- a/matlab/gtsam_examples/SBAExample.m
+++ b/matlab/gtsam_examples/SBAExample.m
@@ -46,7 +46,7 @@ end
 
 %% Add Gaussian priors for a pose and a landmark to constrain the system
 cameraPriorNoise  = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas);
-firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K);
+firstCamera = PinholeCameraCal3_S2(truth.cameras{1}.pose, truth.K);
 graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise));
 
 pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
@@ -60,7 +60,7 @@ graph.print(sprintf('\nFactor graph:\n'));
 initialEstimate = Values;
 for i=1:size(truth.cameras,2)
     pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
-    camera_i = SimpleCamera(pose_i, truth.K);
+    camera_i = PinholeCameraCal3_S2(pose_i, truth.K);
     initialEstimate.insert(symbol('c',i), camera_i);
 end
 for j=1:size(truth.points,2)
diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m
index d46493328..7116d3838 100644
--- a/matlab/gtsam_tests/testTriangulation.m
+++ b/matlab/gtsam_tests/testTriangulation.m
@@ -18,11 +18,11 @@ 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);
+camera1 = PinholeCameraCal3_S2(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);
+camera2 = PinholeCameraCal3_S2(pose2, sharedCal);
 
 %% landmark ~5 meters infront of camera
 landmark =Point3 (5, 0.5, 1.2);
diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
index 2cceea753..195b7ff69 100644
--- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
+++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
@@ -97,8 +97,8 @@ if options.useRealData == 1
       % Create the camera based on the current pose and the pose of the
       % camera in the body
       cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera);
-      camera = SimpleCamera(cameraPose, metadata.camera.calibration);
-      %camera = SimpleCamera(currentPose, metadata.camera.calibration);
+      camera = PinholeCameraCal3_S2(cameraPose, metadata.camera.calibration);
+      %camera = PinholeCameraCal3_S2(currentPose, metadata.camera.calibration);
       
       % Record measurements if the landmark is within visual range
       for j=1:length(metadata.camera.gtLandmarkPoints)
diff --git a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m
index c9e912ea4..129b8c176 100644
--- a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m
+++ b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m
@@ -108,7 +108,7 @@ for i=1:20
     % generate some camera measurements
     cam_pose = initial.atPose3(i).compose(actual_transform);
 %     gtsam.plotPose3(cam_pose);
-    cam = SimpleCamera(cam_pose,K);
+    cam = PinholeCameraCal3_S2(cam_pose,K);
     i
 %     result
     for j=1:nrPoints
diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
index fd4a17469..4557d711f 100644
--- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
+++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
@@ -182,7 +182,7 @@ for i=1:steps
     % generate some camera measurements
     cam_pose = currentIMUPoseGlobal.compose(actual_transform);
 %     gtsam.plotPose3(cam_pose);
-    cam = SimpleCamera(cam_pose,K);
+    cam = PinholeCameraCal3_S2(cam_pose,K);
     i
 %     result
     for j=1:nrPoints
diff --git a/matlab/unstable_examples/TransformProjectionFactorExample.m b/matlab/unstable_examples/TransformProjectionFactorExample.m
index 669073e56..79a96209d 100644
--- a/matlab/unstable_examples/TransformProjectionFactorExample.m
+++ b/matlab/unstable_examples/TransformProjectionFactorExample.m
@@ -73,7 +73,7 @@ for i=1:20
     % generate some camera measurements
     cam_pose = initial.atPose3(i).compose(actual_transform);
     gtsam.plotPose3(cam_pose);
-    cam = SimpleCamera(cam_pose,K);
+    cam = PinholeCameraCal3_S2(cam_pose,K);
     i
     for j=1:nrPoints
         % All landmarks seen in every frame
diff --git a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m
index 8edcfade7..ca5b70c62 100644
--- a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m
+++ b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m
@@ -98,7 +98,7 @@ for i=1:20
     % generate some camera measurements
     cam_pose = initial.atPose3(i).compose(actual_transform);
 %     gtsam.plotPose3(cam_pose);
-    cam = SimpleCamera(cam_pose,K);
+    cam = PinholeCameraCal3_S2(cam_pose,K);
     i
 %     result
     for j=1:nrPoints
diff --git a/matlab/unstable_examples/project_landmarks.m b/matlab/unstable_examples/project_landmarks.m
index 629c6d994..aaccc9248 100644
--- a/matlab/unstable_examples/project_landmarks.m
+++ b/matlab/unstable_examples/project_landmarks.m
@@ -4,7 +4,7 @@ function [ measurements ] = project_landmarks( pose, landmarks, K )
 
     import gtsam.*;
 
-    camera = SimpleCamera(pose,K);    
+    camera = PinholeCameraCal3_S2(pose,K);    
     measurements = Values;
     
     for i=1:size(landmarks)-1
diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp
index 95843e5ab..d59666655 100644
--- a/tests/testNonlinearEquality.cpp
+++ b/tests/testNonlinearEquality.cpp
@@ -28,7 +28,7 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -527,10 +527,10 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
   Rot3 faceTowardsY(Point3(1, 0, 0), Point3(0, 0, -1), Point3(0, 1, 0));
 
   Pose3 poseLeft(faceTowardsY, Point3(0, 0, 0));  // origin, left camera
-  SimpleCamera leftCamera(poseLeft, K);
+  PinholeCamera<Cal3_S2> leftCamera(poseLeft, K);
 
   Pose3 poseRight(faceTowardsY, Point3(2, 0, 0));  // 2 units to the right
-  SimpleCamera rightCamera(poseRight, K);
+  PinholeCamera<Cal3_S2> rightCamera(poseRight, K);
 
   Point3 landmark(1, 5, 0); //centered between the cameras, 5 units away
 
diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp
index 64e111e94..9222894a4 100644
--- a/tests/testSerializationSLAM.cpp
+++ b/tests/testSerializationSLAM.cpp
@@ -47,7 +47,7 @@
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/geometry/CalibratedCamera.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/StereoCamera.h>
 
 #include <gtsam/base/serializationTestHelpers.h>
@@ -57,20 +57,21 @@ using namespace gtsam;
 using namespace gtsam::serializationTestHelpers;
 
 // Creating as many permutations of factors as possible
-typedef PriorFactor<LieVector>         PriorFactorLieVector;
-typedef PriorFactor<LieMatrix>         PriorFactorLieMatrix;
-typedef PriorFactor<Point2>            PriorFactorPoint2;
-typedef PriorFactor<StereoPoint2>      PriorFactorStereoPoint2;
-typedef PriorFactor<Point3>            PriorFactorPoint3;
-typedef PriorFactor<Rot2>              PriorFactorRot2;
-typedef PriorFactor<Rot3>              PriorFactorRot3;
-typedef PriorFactor<Pose2>             PriorFactorPose2;
-typedef PriorFactor<Pose3>             PriorFactorPose3;
-typedef PriorFactor<Cal3_S2>           PriorFactorCal3_S2;
-typedef PriorFactor<Cal3DS2>           PriorFactorCal3DS2;
-typedef PriorFactor<CalibratedCamera>  PriorFactorCalibratedCamera;
-typedef PriorFactor<SimpleCamera>      PriorFactorSimpleCamera;
-typedef PriorFactor<StereoCamera>      PriorFactorStereoCamera;
+typedef PriorFactor<LieVector>              PriorFactorLieVector;
+typedef PriorFactor<LieMatrix>              PriorFactorLieMatrix;
+typedef PriorFactor<Point2>                 PriorFactorPoint2;
+typedef PriorFactor<StereoPoint2>           PriorFactorStereoPoint2;
+typedef PriorFactor<Point3>                 PriorFactorPoint3;
+typedef PriorFactor<Rot2>                   PriorFactorRot2;
+typedef PriorFactor<Rot3>                   PriorFactorRot3;
+typedef PriorFactor<Pose2>                  PriorFactorPose2;
+typedef PriorFactor<Pose3>                  PriorFactorPose3;
+typedef PriorFactor<Cal3_S2>                PriorFactorCal3_S2;
+typedef PriorFactor<Cal3DS2>                PriorFactorCal3DS2;
+typedef PriorFactor<CalibratedCamera>       PriorFactorCalibratedCamera;
+typedef PriorFactor<SimpleCamera>           PriorFactorSimpleCamera;
+typedef PriorFactor<PinholeCameraCal3_S2>   PriorFactorPinholeCameraCal3_S2;
+typedef PriorFactor<StereoCamera>           PriorFactorStereoCamera;
 
 typedef BetweenFactor<LieVector>       BetweenFactorLieVector;
 typedef BetweenFactor<LieMatrix>       BetweenFactorLieMatrix;
@@ -81,29 +82,32 @@ typedef BetweenFactor<Rot3>            BetweenFactorRot3;
 typedef BetweenFactor<Pose2>           BetweenFactorPose2;
 typedef BetweenFactor<Pose3>           BetweenFactorPose3;
 
-typedef NonlinearEquality<LieVector>         NonlinearEqualityLieVector;
-typedef NonlinearEquality<LieMatrix>         NonlinearEqualityLieMatrix;
-typedef NonlinearEquality<Point2>            NonlinearEqualityPoint2;
-typedef NonlinearEquality<StereoPoint2>      NonlinearEqualityStereoPoint2;
-typedef NonlinearEquality<Point3>            NonlinearEqualityPoint3;
-typedef NonlinearEquality<Rot2>              NonlinearEqualityRot2;
-typedef NonlinearEquality<Rot3>              NonlinearEqualityRot3;
-typedef NonlinearEquality<Pose2>             NonlinearEqualityPose2;
-typedef NonlinearEquality<Pose3>             NonlinearEqualityPose3;
-typedef NonlinearEquality<Cal3_S2>           NonlinearEqualityCal3_S2;
-typedef NonlinearEquality<Cal3DS2>           NonlinearEqualityCal3DS2;
-typedef NonlinearEquality<CalibratedCamera>  NonlinearEqualityCalibratedCamera;
-typedef NonlinearEquality<SimpleCamera>      NonlinearEqualitySimpleCamera;
-typedef NonlinearEquality<StereoCamera>      NonlinearEqualityStereoCamera;
+typedef NonlinearEquality<LieVector>              NonlinearEqualityLieVector;
+typedef NonlinearEquality<LieMatrix>              NonlinearEqualityLieMatrix;
+typedef NonlinearEquality<Point2>                 NonlinearEqualityPoint2;
+typedef NonlinearEquality<StereoPoint2>           NonlinearEqualityStereoPoint2;
+typedef NonlinearEquality<Point3>                 NonlinearEqualityPoint3;
+typedef NonlinearEquality<Rot2>                   NonlinearEqualityRot2;
+typedef NonlinearEquality<Rot3>                   NonlinearEqualityRot3;
+typedef NonlinearEquality<Pose2>                  NonlinearEqualityPose2;
+typedef NonlinearEquality<Pose3>                  NonlinearEqualityPose3;
+typedef NonlinearEquality<Cal3_S2>                NonlinearEqualityCal3_S2;
+typedef NonlinearEquality<Cal3DS2>                NonlinearEqualityCal3DS2;
+typedef NonlinearEquality<CalibratedCamera>       NonlinearEqualityCalibratedCamera;
+typedef NonlinearEquality<SimpleCamera>           NonlinearEqualitySimpleCamera;
+typedef NonlinearEquality<PinholeCameraCal3_S2>   NonlinearEqualityPinholeCameraCal3_S2;
+typedef NonlinearEquality<StereoCamera>           NonlinearEqualityStereoCamera;
 
-typedef RangeFactor<Pose2, Point2>                      RangeFactor2D;
-typedef RangeFactor<Pose3, Point3>                      RangeFactor3D;
-typedef RangeFactor<Pose2, Pose2>                       RangeFactorPose2;
-typedef RangeFactor<Pose3, Pose3>                       RangeFactorPose3;
-typedef RangeFactor<CalibratedCamera, Point3>           RangeFactorCalibratedCameraPoint;
-typedef RangeFactor<SimpleCamera, Point3>               RangeFactorSimpleCameraPoint;
-typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
-typedef RangeFactor<SimpleCamera, SimpleCamera>         RangeFactorSimpleCamera;
+typedef RangeFactor<Pose2, Point2>                              RangeFactor2D;
+typedef RangeFactor<Pose3, Point3>                              RangeFactor3D;
+typedef RangeFactor<Pose2, Pose2>                               RangeFactorPose2;
+typedef RangeFactor<Pose3, Pose3>                               RangeFactorPose3;
+typedef RangeFactor<CalibratedCamera, Point3>                   RangeFactorCalibratedCameraPoint;
+typedef RangeFactor<SimpleCamera, Point3>                       RangeFactorSimpleCameraPoint;
+typedef RangeFactor<PinholeCameraCal3_S2, Point3>               RangeFactorPinholeCameraCal3_S2Point;
+typedef RangeFactor<CalibratedCamera, CalibratedCamera>         RangeFactorCalibratedCamera;
+typedef RangeFactor<SimpleCamera, SimpleCamera>                 RangeFactorSimpleCamera;
+typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
 
 typedef BearingRangeFactor<Pose2, Point2>  BearingRangeFactor2D;
 typedef BearingRangeFactor<Pose3, Point3>  BearingRangeFactor3D;
@@ -111,7 +115,7 @@ typedef BearingRangeFactor<Pose3, Point3>  BearingRangeFactor3D;
 typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;
 typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
 
-typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
 //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 
 typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
@@ -158,6 +162,7 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3DS2);
 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo);
 GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera);
 GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
+GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);
 GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
 
 /* Create GUIDs for factors */
@@ -294,7 +299,7 @@ TEST (testSerializationSLAM, factors) {
   Cal3DS2 cal3ds2(1.0, 2.0, 3.0, 4.0, 5.0,6.0, 7.0, 8.0, 9.0);
   Cal3_S2Stereo cal3_s2stereo(1.0, 2.0, 3.0, 4.0, 5.0, 1.0);
   CalibratedCamera calibratedCamera(pose3);
-  SimpleCamera simpleCamera(pose3, cal3_s2);
+  PinholeCamera<Cal3_S2> simpleCamera(pose3, cal3_s2);
   StereoCamera stereoCamera(pose3, boost::make_shared<Cal3_S2Stereo>(cal3_s2stereo));
 
 
diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp
index 9c63e1aa8..182408004 100644
--- a/tests/testVisualISAM2.cpp
+++ b/tests/testVisualISAM2.cpp
@@ -59,7 +59,7 @@ TEST(testVisualISAM2, all)
         // Add factors for each landmark observation
         for (size_t j = 0; j < points.size(); ++j)
         {
-            SimpleCamera camera(poses[i], *K);
+            PinholeCamera<Cal3_S2> camera(poses[i], *K);
             Point2 measurement = camera.project(points[j]);
             graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
                 measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
diff --git a/wrap/python/pybind11 b/wrap/python/pybind11
new file mode 160000
index 000000000..b3bf248ee
--- /dev/null
+++ b/wrap/python/pybind11
@@ -0,0 +1 @@
+Subproject commit b3bf248eec9cad8260753c982e1ae6cb72fff470