diff --git a/cython/gtsam/examples/PlanarManipulatorExample.py b/cython/gtsam/examples/PlanarManipulatorExample.py
new file mode 100644
index 000000000..af21e6ca5
--- /dev/null
+++ b/cython/gtsam/examples/PlanarManipulatorExample.py
@@ -0,0 +1,325 @@
+"""
+GTSAM Copyright 2010-2018, 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
+
+Kinematics of three-link manipulator with GTSAM poses and product of exponential maps.
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import math
+import unittest
+from functools import reduce
+
+import matplotlib.pyplot as plt
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
+
+import gtsam
+import gtsam.utils.plot as gtsam_plot
+from gtsam import Pose2
+
+
+def vector3(x, y, z):
+    """Create 3D double numpy array."""
+    return np.array([x, y, z], dtype=np.float)
+
+
+def compose(*poses):
+    """Compose all Pose2 transforms given as arguments from left to right."""
+    return reduce((lambda x, y: x.compose(y)), poses)
+
+
+def vee(M):
+    """Pose2 vee operator."""
+    return vector3(M[0, 2], M[1, 2], M[1, 0])
+
+
+def delta(g0, g1):
+    """Difference between x,y,,theta components of SE(2) poses."""
+    return vector3(g1.x() - g0.x(), g1.y() - g0.y(), g1.theta() - g0.theta())
+
+
+def trajectory(g0, g1, N=20):
+    """ Create an interpolated trajectory in SE(2), treating x,y, and theta separately.
+        g0 and g1 are the initial and final pose, respectively.
+        N is the number of *intervals*
+        Returns N+1 poses
+    """
+    e = delta(g0, g1)
+    return [Pose2(g0.x()+e[0]*t, g0.y()+e[1]*t, g0.theta()+e[2]*t) for t in np.linspace(0, 1, N)]
+
+
+class ThreeLinkArm(object):
+    """Three-link arm class."""
+
+    def __init__(self):
+        self.L1 = 3.5
+        self.L2 = 3.5
+        self.L3 = 2.5
+        self.xi1 = vector3(0, 0, 1)
+        self.xi2 = vector3(self.L1, 0, 1)
+        self.xi3 = vector3(self.L1+self.L2, 0, 1)
+        self.sXt0 = Pose2(0, self.L1+self.L2 + self.L3, math.radians(90))
+
+    def fk(self, q):
+        """ Forward kinematics.
+            Takes numpy array of joint angles, in radians.
+        """
+        sXl1 = Pose2(0, 0, math.radians(90))
+        l1Zl1 = Pose2(0, 0, q[0])
+        l1Xl2 = Pose2(self.L1, 0, 0)
+        l2Zl2 = Pose2(0, 0, q[1])
+        l2Xl3 = Pose2(self.L2, 0, 0)
+        l3Zl3 = Pose2(0, 0, q[2])
+        l3Xt = Pose2(self.L3, 0, 0)
+        return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)
+
+    def jacobian(self, q):
+        """ Calculate manipulator Jacobian.
+            Takes numpy array of joint angles, in radians.
+        """
+        a = q[0]+q[1]
+        b = a+q[2]
+        return np.array([[-self.L1*math.cos(q[0]) - self.L2*math.cos(a)-self.L3*math.cos(b),
+                          -self.L1*math.cos(a)-self.L3*math.cos(b),
+                          - self.L3*math.cos(b)],
+                         [-self.L1*math.sin(q[0]) - self.L2*math.sin(a)-self.L3*math.sin(b),
+                          -self.L1*math.sin(a)-self.L3*math.sin(b),
+                          - self.L3*math.sin(b)],
+                         [1, 1, 1]], np.float)
+
+    def poe(self, q):
+        """ Forward kinematics.
+            Takes numpy array of joint angles, in radians.
+        """
+        l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
+        l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
+        l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
+        return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
+
+    def con(self, q):
+        """ Forward kinematics, conjugation form.
+            Takes numpy array of joint angles, in radians.
+        """
+        def expmap(x, y, theta):
+            """Implement exponential map via conjugation with axis (x,y)."""
+            return compose(Pose2(x, y, 0), Pose2(0, 0, theta), Pose2(-x, -y, 0))
+
+        l1Zl1 = expmap(0.0, 0.0, q[0])
+        l2Zl2 = expmap(0.0, self.L1, q[1])
+        l3Zl3 = expmap(0.0, 7.0, q[2])
+        return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
+
+    def ik(self, sTt_desired, e=1e-9):
+        """ Inverse kinematics.
+            Takes desired Pose2 of tool T with respect to base S.
+            Optional: mu, gradient descent rate; e: error norm threshold
+        """
+        q = np.radians(vector3(30, -30, 45))  # well within workspace
+        error = vector3(100, 100, 100)
+
+        while np.linalg.norm(error) > e:
+            error = delta(sTt_desired, self.fk(q))
+            J = self.jacobian(q)
+            q -= np.dot(np.linalg.pinv(J), error)
+
+        # return result in interval [-pi,pi)
+        return np.remainder(q+math.pi, 2*math.pi)-math.pi
+
+    def manipulator_jacobian(self, q):
+        """ Calculate manipulator Jacobian.
+            Takes numpy array of joint angles, in radians.
+            Returns the manipulator Jacobian of differential twists. When multiplied with
+            a vector of joint velocities, will yield a single differential twist which is
+            the spatial velocity d(sTt)/dt * inv(sTt) of the end-effector pose.
+            Just like always, differential twists can be hatted and multiplied with spatial
+            coordinates of a point to give the spatial velocity of the point.
+        """
+        l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
+        l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
+        # l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
+
+        p1 = self.xi1
+        # p1 = Pose2().Adjoint(self.xi1)
+
+        sTl1 = l1Zl1
+        p2 = sTl1.Adjoint(self.xi2)
+
+        sTl2 = compose(l1Zl1, l2Zl2)
+        p3 = sTl2.Adjoint(self.xi3)
+
+        differential_twists = [p1, p2, p3]
+        return np.stack(differential_twists, axis=1)
+
+    def plot(self, fignum, q):
+        """ Plot arm.
+            Takes figure number, and numpy array of joint angles, in radians.
+        """
+        fig = plt.figure(fignum)
+        axes = fig.gca()
+
+        sXl1 = Pose2(0, 0, math.radians(90))
+        t = sXl1.translation()
+        p1 = np.array([t.x(), t.y()])
+        gtsam_plot.plot_pose2_on_axes(axes, sXl1)
+
+        def plot_line(p, g, color):
+            t = g.translation()
+            q = np.array([t.x(), t.y()])
+            line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
+            axes.plot(line[:, 0], line[:, 1], color)
+            return q
+
+        l1Zl1 = Pose2(0, 0, q[0])
+        l1Xl2 = Pose2(self.L1, 0, 0)
+        sTl2 = compose(sXl1, l1Zl1, l1Xl2)
+        p2 = plot_line(p1, sTl2, 'r-')
+        gtsam_plot.plot_pose2_on_axes(axes, sTl2)
+
+        l2Zl2 = Pose2(0, 0, q[1])
+        l2Xl3 = Pose2(self.L2, 0, 0)
+        sTl3 = compose(sTl2, l2Zl2, l2Xl3)
+        p3 = plot_line(p2, sTl3, 'g-')
+        gtsam_plot.plot_pose2_on_axes(axes, sTl3)
+
+        l3Zl3 = Pose2(0, 0, q[2])
+        l3Xt = Pose2(self.L3, 0, 0)
+        sTt = compose(sTl3, l3Zl3, l3Xt)
+        plot_line(p3, sTt, 'b-')
+        gtsam_plot.plot_pose2_on_axes(axes, sTt)
+
+
+# Create common example configurations.
+Q0 = vector3(0, 0, 0)
+Q1 = np.radians(vector3(-30, -45, -90))
+Q2 = np.radians(vector3(-90, 90, 0))
+
+
+class TestPose2SLAMExample(unittest.TestCase):
+    """Unit tests for functions used below."""
+
+    def setUp(self):
+        self.arm = ThreeLinkArm()
+
+    def assertPose2Equals(self, actual, expected, tol=1e-2):
+        """Helper function that prints out actual and expected if not equal."""
+        equal = actual.equals(expected, tol)
+        if not equal:
+            raise self.failureException(
+                "Poses are not equal:\n{}!={}".format(actual, expected))
+
+    def test_fk_arm(self):
+        """Make sure forward kinematics is correct for some known test configurations."""
+        # at rest
+        expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
+        sTt = self.arm.fk(Q0)
+        self.assertIsInstance(sTt, Pose2)
+        self.assertPose2Equals(sTt, expected)
+
+        # -30, -45, -90
+        expected = Pose2(5.78, 1.52, math.radians(-75))
+        sTt = self.arm.fk(Q1)
+        self.assertPose2Equals(sTt, expected)
+
+    def test_jacobian(self):
+        """Test Jacobian calculation."""
+        # at rest
+        expected = np.array([[-9.5, -6, -2.5], [0, 0, 0], [1, 1, 1]], np.float)
+        J = self.arm.jacobian(Q0)
+        np.testing.assert_array_almost_equal(J, expected)
+
+        # at -90, 90, 0
+        expected = np.array([[-6, -6, -2.5], [3.5, 0, 0], [1, 1, 1]], np.float)
+        J = self.arm.jacobian(Q2)
+        np.testing.assert_array_almost_equal(J, expected)
+
+    def test_con_arm(self):
+        """Make sure POE is correct for some known test configurations."""
+        # at rest
+        expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
+        sTt = self.arm.con(Q0)
+        self.assertIsInstance(sTt, Pose2)
+        self.assertPose2Equals(sTt, expected)
+
+        # -30, -45, -90
+        expected = Pose2(5.78, 1.52, math.radians(-75))
+        sTt = self.arm.con(Q1)
+        self.assertPose2Equals(sTt, expected)
+
+    def test_poe_arm(self):
+        """Make sure POE is correct for some known test configurations."""
+        # at rest
+        expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
+        sTt = self.arm.poe(Q0)
+        self.assertIsInstance(sTt, Pose2)
+        self.assertPose2Equals(sTt, expected)
+
+        # -30, -45, -90
+        expected = Pose2(5.78, 1.52, math.radians(-75))
+        sTt = self.arm.poe(Q1)
+        self.assertPose2Equals(sTt, expected)
+
+    def test_ik(self):
+        """Check iterative inverse kinematics function."""
+        # at rest
+        actual = self.arm.ik(Pose2(0, 2*3.5 + 2.5, math.radians(90)))
+        np.testing.assert_array_almost_equal(actual, Q0, decimal=2)
+
+        # -30, -45, -90
+        sTt_desired = Pose2(5.78, 1.52, math.radians(-75))
+        actual = self.arm.ik(sTt_desired)
+        self.assertPose2Equals(self.arm.fk(actual), sTt_desired)
+        np.testing.assert_array_almost_equal(actual, Q1, decimal=2)
+
+    def test_manipulator_jacobian(self):
+        """Test Jacobian calculation."""
+        # at rest
+        expected = np.array([[0, 3.5, 7], [0, 0, 0], [1, 1, 1]], np.float)
+        J = self.arm.manipulator_jacobian(Q0)
+        np.testing.assert_array_almost_equal(J, expected)
+
+        # at -90, 90, 0
+        expected = np.array(
+            [[0, 0, 3.5], [0, -3.5, -3.5], [1, 1, 1]], np.float)
+        J = self.arm.manipulator_jacobian(Q2)
+        np.testing.assert_array_almost_equal(J, expected)
+
+
+def run_example():
+    """ Use trajectory interpolation and then trajectory tracking a la Murray
+        to move a 3-link arm on a straight line.
+    """
+    arm = ThreeLinkArm()
+    q = np.radians(vector3(30, -30, 45))
+    sTt_initial = arm.fk(q)
+    sTt_goal = Pose2(2.4, 4.3, math.radians(0))
+    poses = trajectory(sTt_initial, sTt_goal, 50)
+
+    fignum = 0
+    fig = plt.figure(fignum)
+    axes = fig.gca()
+    axes.set_xlim(-5, 5)
+    axes.set_ylim(0, 10)
+    gtsam_plot.plot_pose2(fignum, arm.fk(q))
+
+    for pose in poses:
+        sTt = arm.fk(q)
+        error = delta(sTt, pose)
+        J = arm.jacobian(q)
+        q += np.dot(np.linalg.inv(J), error)
+        arm.plot(fignum, q)
+        plt.pause(0.01)
+
+    plt.pause(10)
+
+
+if __name__ == "__main__":
+    run_example()
+    unittest.main()
diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp
index 63c512edb..f3915ce22 100644
--- a/examples/SolverComparer.cpp
+++ b/examples/SolverComparer.cpp
@@ -251,10 +251,10 @@ void runIncremental()
   Key firstPose;
   while(nextMeasurement < datasetMeasurements.size())
   {
-    if(BetweenFactor<Pose>::shared_ptr measurement =
+    if(BetweenFactor<Pose>::shared_ptr factor =
       boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
     {
-      Key key1 = measurement->key1(), key2 = measurement->key2();
+      Key key1 = factor->key1(), key2 = factor->key2();
       if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
         // We found an odometry starting at firstStep
         firstPose = std::min(key1, key2);
@@ -302,52 +302,53 @@ void runIncremental()
 
       NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
 
-      if(BetweenFactor<Pose>::shared_ptr measurement =
+      if(BetweenFactor<Pose>::shared_ptr factor =
         boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
       {
         // Stop collecting measurements that are for future steps
-        if(measurement->key1() > step || measurement->key2() > step)
+        if(factor->key1() > step || factor->key2() > step)
           break;
 
         // Require that one of the nodes is the current one
-        if(measurement->key1() != step && measurement->key2() != step)
+        if(factor->key1() != step && factor->key2() != step)
           throw runtime_error("Problem in data file, out-of-sequence measurements");
 
         // Add a new factor
-        newFactors.push_back(measurement);
+        newFactors.push_back(factor);
+        const auto& measured = factor->measured();
 
         // Initialize the new variable
-        if(measurement->key1() > measurement->key2()) {
-          if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
+        if(factor->key1() > factor->key2()) {
+          if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
             if(step == 1)
-              newVariables.insert(measurement->key1(), measurement->measured().inverse());
+              newVariables.insert(factor->key1(), measured.inverse());
             else {
-              Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
-              newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
+              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2());
+              newVariables.insert(factor->key1(), prevPose * measured.inverse());
             }
           }
         } else {
-          if(!newVariables.exists(measurement->key2())) { // Only need to check newVariables since loop closures come after odometry
+          if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry
             if(step == 1)
-              newVariables.insert(measurement->key2(), measurement->measured());
+              newVariables.insert(factor->key2(), measured);
             else {
-              Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
-              newVariables.insert(measurement->key2(), prevPose * measurement->measured());
+              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1());
+              newVariables.insert(factor->key2(), prevPose * measured);
             }
           }
         }
       }
-      else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
+      else if(BearingRangeFactor<Pose, Point2>::shared_ptr factor =
         boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
       {
-        Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1];
+        Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];
 
         // Stop collecting measurements that are for future steps
         if(poseKey > step)
           throw runtime_error("Problem in data file, out-of-sequence measurements");
 
         // Add new factor
-        newFactors.push_back(measurement);
+        newFactors.push_back(factor);
 
         // Initialize new landmark
         if(!isam2.getLinearizationPoint().exists(lmKey))
@@ -357,8 +358,9 @@ void runIncremental()
             pose = isam2.calculateEstimate<Pose>(poseKey);
           else
             pose = newVariables.at<Pose>(poseKey);
-          Rot2 measuredBearing = measurement->measured().first;
-          double measuredRange = measurement->measured().second;
+          const auto& measured = factor->measured();
+          Rot2 measuredBearing = measured.bearing();
+          double measuredRange = measured.range();
           newVariables.insert(lmKey, 
             pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
         }
@@ -427,7 +429,7 @@ void runIncremental()
   //    for (Key key12: boost::adaptors::reverse(values.keys())) {
   //      if(i != j) {
   //        gttic_(jointMarginalInformation);
-  //        std::vector<Key> keys(2);
+  //        KeyVector keys(2);
   //        keys[0] = key1;
   //        keys[1] = key2;
   //        JointMarginal info = marginals.jointMarginalInformation(keys);
@@ -522,7 +524,7 @@ void runCompare()
 
   // Check solution for equality
   cout << "Comparing solutions..." << endl;
-  vector<Key> missingKeys;
+  KeyVector missingKeys;
   br::set_symmetric_difference(soln1.keys(), soln2.keys(), std::back_inserter(missingKeys));
   if(!missingKeys.empty()) {
     cout << "  Keys unique to one solution file: ";
@@ -533,7 +535,7 @@ void runCompare()
     }
     cout << endl;
   }
-  vector<Key> commonKeys;
+  KeyVector commonKeys;
   br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
   double maxDiff = 0.0;
   for(Key j: commonKeys)
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h
index 41e18ff07..b91a50340 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h
@@ -52,7 +52,7 @@ struct general_matrix_matrix_triangular_product<Index,Scalar,LhsStorageOrder,Con
   static EIGEN_STRONG_INLINE void run(Index size, Index depth,const Scalar* lhs, Index lhsStride, \
                           const Scalar* rhs, Index rhsStride, Scalar* res, Index resStride, Scalar alpha, level3_blocking<Scalar, Scalar>& blocking) \
   { \
-    if ( lhs==rhs && ((UpLo&(Lower|Upper)==UpLo)) ) { \
+    if ( lhs==rhs && ((UpLo&(Lower|Upper))==UpLo) ) { \
       general_matrix_matrix_rankupdate<Index,Scalar,LhsStorageOrder,ConjugateLhs,ColMajor,UpLo> \
       ::run(size,depth,lhs,lhsStride,rhs,rhsStride,res,resStride,alpha,blocking); \
     } else { \
diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h
index 7d1cb970c..e54b91126 100644
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@@ -11,88 +11,26 @@
 
 /**
  * @file    FastVector.h
- * @brief   A thin wrapper around std::vector that uses boost's pool_allocator.
+ * @brief   A thin wrapper around std::vector that uses a custom allocator.
  * @author  Richard Roberts
+ * @author  Frank Dellaert
  * @date    Feb 9, 2011
  */
 
 #pragma once
 
 #include <gtsam/base/FastDefaultAllocator.h>
-#include <boost/serialization/nvp.hpp>
-#include <boost/serialization/vector.hpp>
 #include <vector>
 
 namespace gtsam {
 
 /**
- * FastVector is a thin wrapper around std::vector that uses the boost
- * pool_allocator instead of the default STL allocator.  This is just a
- * convenience to avoid having lengthy types in the code.  Through timing,
- * we've seen that the pool_allocator can lead to speedups of several %
+ * FastVector is a type alias to a std::vector with a custom memory allocator.
+ * The particular allocator depends on GTSAM's cmake configuration.
  * @addtogroup base
  */
-template<typename VALUE>
-class FastVector: public std::vector<VALUE,
-    typename internal::FastDefaultVectorAllocator<VALUE>::type> {
+template <typename T>
+using FastVector =
+    std::vector<T, typename internal::FastDefaultVectorAllocator<T>::type>;
 
-public:
-
-  typedef std::vector<VALUE,
-      typename internal::FastDefaultVectorAllocator<VALUE>::type> Base;
-
-  /** Default constructor */
-  FastVector() {
-  }
-
-  /** Constructor from size */
-  explicit FastVector(size_t size) :
-      Base(size) {
-  }
-
-  /** Constructor from size and initial values */
-  explicit FastVector(size_t size, const VALUE& initial) :
-      Base(size, initial) {
-  }
-
-  /** Constructor from a range, passes through to base class */
-  template<typename INPUTITERATOR>
-  explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) {
-    // This if statement works around a bug in boost pool allocator and/or
-    // STL vector where if the size is zero, the pool allocator will allocate
-    // huge amounts of memory.
-    if (first != last)
-      Base::assign(first, last);
-  }
-
-  /** Copy constructor from the base class */
-  FastVector(const Base& x) :
-      Base(x) {
-  }
-
-  /** Copy constructor from a standard STL container */
-  template<typename ALLOCATOR>
-  FastVector(const std::vector<VALUE, ALLOCATOR>& x) {
-    // This if statement works around a bug in boost pool allocator and/or
-    // STL vector where if the size is zero, the pool allocator will allocate
-    // huge amounts of memory.
-    if (x.size() > 0)
-      Base::assign(x.begin(), x.end());
-  }
-
-  /** Conversion to a standard STL container */
-  operator std::vector<VALUE>() const {
-    return std::vector<VALUE>(this->begin(), this->end());
-  }
-
-private:
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-  }
-
-};
-
-}
+}  // namespace gtsam
diff --git a/gtsam/base/tests/testFastContainers.cpp b/gtsam/base/tests/testFastContainers.cpp
index 4909f4ecb..12dee036e 100644
--- a/gtsam/base/tests/testFastContainers.cpp
+++ b/gtsam/base/tests/testFastContainers.cpp
@@ -22,8 +22,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 TEST( testFastContainers, KeySet ) {
 
-  KeyVector init_vector;
-  init_vector += 2, 3, 4, 5;
+  KeyVector init_vector {2, 3, 4, 5};
 
   KeySet actSet(init_vector);
   KeySet expSet; expSet += 2, 3, 4, 5;
diff --git a/gtsam/base/tests/testTreeTraversal.cpp b/gtsam/base/tests/testTreeTraversal.cpp
index 8fe5e53ef..88e476cb9 100644
--- a/gtsam/base/tests/testTreeTraversal.cpp
+++ b/gtsam/base/tests/testTreeTraversal.cpp
@@ -40,8 +40,8 @@ struct TestNode {
 struct TestForest {
   typedef TestNode Node;
   typedef Node::shared_ptr sharedNode;
-  vector<sharedNode> roots_;
-  const vector<sharedNode>& roots() const { return roots_; }
+  FastVector<sharedNode> roots_;
+  const FastVector<sharedNode>& roots() const { return roots_; }
 };
 
 TestForest makeTestForest() {
diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index cdfd7d522..4c2607f1f 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -72,7 +72,7 @@ public:
   typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
 
   /** A map from keys to values */
-  typedef std::vector<Key> Indices;
+  typedef KeyVector Indices;
   typedef Assignment<Key> Values;
   typedef boost::shared_ptr<Values> sharedValues;
 
diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp
index b4fd2e5a1..5ddad22b0 100644
--- a/gtsam/discrete/DiscreteKey.cpp
+++ b/gtsam/discrete/DiscreteKey.cpp
@@ -31,8 +31,8 @@ namespace gtsam {
     }
   }
 
-  vector<Key> DiscreteKeys::indices() const {
-    vector < Key > js;
+  KeyVector DiscreteKeys::indices() const {
+    KeyVector js;
     for(const DiscreteKey& key: *this)
       js.push_back(key.first);
     return js;
diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h
index bc2bd862d..c041c7e8e 100644
--- a/gtsam/discrete/DiscreteKey.h
+++ b/gtsam/discrete/DiscreteKey.h
@@ -19,6 +19,7 @@
 #pragma once
 
 #include <gtsam/global_includes.h>
+#include <gtsam/inference/Key.h>
 
 #include <map>
 #include <string>
@@ -53,7 +54,7 @@ namespace gtsam {
     GTSAM_EXPORT DiscreteKeys(const std::vector<int>& cs);
 
     /// Return a vector of indices
-    GTSAM_EXPORT std::vector<Key> indices() const;
+    GTSAM_EXPORT KeyVector indices() const;
 
     /// Return a map from index to cardinality
     GTSAM_EXPORT std::map<Key,size_t> cardinalities() const;
diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp
index 0ee5c63b8..89e763703 100644
--- a/gtsam/discrete/Signature.cpp
+++ b/gtsam/discrete/Signature.cpp
@@ -130,8 +130,8 @@ namespace gtsam {
     return keys;
   }
 
-  vector<Key> Signature::indices() const {
-    vector<Key> js;
+  KeyVector Signature::indices() const {
+    KeyVector js;
     js.push_back(key_.first);
     for(const DiscreteKey& key: parents_)
       js.push_back(key.first);
diff --git a/gtsam/discrete/Signature.h b/gtsam/discrete/Signature.h
index 6d49c7e4c..587cd6b30 100644
--- a/gtsam/discrete/Signature.h
+++ b/gtsam/discrete/Signature.h
@@ -90,7 +90,7 @@ namespace gtsam {
     DiscreteKeys discreteKeysParentsFirst() const;
 
     /** All key indices, with variable key first */
-    std::vector<Key> indices() const;
+    KeyVector indices() const;
 
     // the CPT as parsed, if successful
     const boost::optional<Table>& table() const {
diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index 9de5a07d8..ffa373027 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -67,6 +67,12 @@ public:
   /// @name Standard Interface
   /// @{
 
+  /// Return bearing measurement
+  const B& bearing() const { return bearing_; }
+
+  /// Return range measurement
+  const R& range() const { return range_; }
+
   /// Prediction function that stacks measurements
   static BearingRange Measure(
     const A1& a1, const A2& a2,
diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index 8e87e5a07..ecf9a572d 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -19,10 +19,11 @@
 #pragma once
 
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
+#include <gtsam/geometry/CalibratedCamera.h>  // for Cheirality exception
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/SymmetricBlockMatrix.h>
 #include <gtsam/base/FastMap.h>
+#include <gtsam/inference/Key.h>
 #include <vector>
 
 namespace gtsam {
@@ -244,7 +245,7 @@ public:
   template<int N> // N = 2 or 3
   static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
       const Eigen::Matrix<double, N, N>& P, const Vector& b,
-      const FastVector<Key>& allKeys, const FastVector<Key>& keys,
+      const KeyVector& allKeys, const KeyVector& keys,
       /*output ->*/SymmetricBlockMatrix& augmentedHessian) {
 
     assert(keys.size()==Fs.size());
diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp
index c93af783a..60cee59ee 100644
--- a/gtsam/geometry/tests/testCameraSet.cpp
+++ b/gtsam/geometry/tests/testCameraSet.cpp
@@ -93,18 +93,12 @@ TEST(CameraSet, Pinhole) {
   EXPECT(assert_equal(schur, actualReduced.selfadjointView()));
 
   // Check Schur complement update, same order, should just double
-  FastVector<Key> allKeys, keys;
-  allKeys.push_back(1);
-  allKeys.push_back(2);
-  keys.push_back(1);
-  keys.push_back(2);
+  KeyVector allKeys {1, 2}, keys {1, 2};
   Set::UpdateSchurComplement<3>(Fs, E, P, b, allKeys, keys, actualReduced);
   EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.selfadjointView()));
 
   // Check Schur complement update, keys reversed
-  FastVector<Key> keys2;
-  keys2.push_back(2);
-  keys2.push_back(1);
+  KeyVector keys2 {2, 1};
   Set::UpdateSchurComplement<3>(Fs, E, P, b, allKeys, keys2, actualReduced);
   Vector4 reverse_b;
   reverse_b << b.tail<2>(), b.head<2>();
diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h
index 96d191419..33a23056b 100644
--- a/gtsam/inference/BayesTree-inst.h
+++ b/gtsam/inference/BayesTree-inst.h
@@ -344,7 +344,7 @@ namespace gtsam {
       // Get the set of variables to eliminate, which is C1\B.
       gttic(Full_root_factoring);
       boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
-        FastVector<Key> C1_minus_B; {
+        KeyVector C1_minus_B; {
           KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
           for(const Key j: *B->conditional()) {
             C1_minus_B_set.erase(j); }
@@ -356,7 +356,7 @@ namespace gtsam {
           FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
       }
       boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
-        FastVector<Key> C2_minus_B; {
+        KeyVector C2_minus_B; {
           KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
           for(const Key j: *B->conditional()) {
             C2_minus_B_set.erase(j); }
@@ -460,7 +460,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class CLIQUE>
-  void BayesTree<CLIQUE>::removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans)
+  void BayesTree<CLIQUE>::removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans)
   {
     // process each key of the new factor
     for(const Key& j: keys)
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index c22a5e257..ee0f3c7b5 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -214,7 +214,7 @@ namespace gtsam {
      * Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
      * Factors and orphans are added to the in/out arguments.
      */
-    void removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans);
+    void removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans);
 
     /**
      * Remove the requested subtree. */
diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h
index 31a8c55b6..6bcfb434d 100644
--- a/gtsam/inference/BayesTreeCliqueBase-inst.h
+++ b/gtsam/inference/BayesTreeCliqueBase-inst.h
@@ -40,12 +40,12 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class DERIVED, class FACTORGRAPH>
-  FastVector<Key>
+  KeyVector
     BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
   {
     KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
     KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
-    FastVector<Key> S_setminus_B;
+    KeyVector S_setminus_B;
     std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
       indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
     return S_setminus_B;
@@ -53,14 +53,14 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class DERIVED, class FACTORGRAPH>
-  FastVector<Key> BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
+  KeyVector BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
     const derived_ptr& B, const FactorGraphType& p_Cp_B) const
   {
     gttic(shortcut_indices);
     KeySet allKeys = p_Cp_B.keys();
     KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
-    FastVector<Key> S_setminus_B = separator_setminus_B(B);
-    FastVector<Key> keep;
+    KeyVector S_setminus_B = separator_setminus_B(B);
+    KeyVector keep;
     // keep = S\B intersect allKeys (S_setminus_B is already sorted)
     std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
       allKeys.begin(), allKeys.end(), back_inserter(keep));
@@ -113,7 +113,7 @@ namespace gtsam {
     gttic(BayesTreeCliqueBase_shortcut);
     // We only calculate the shortcut when this clique is not B
     // and when the S\B is not empty
-    FastVector<Key> S_setminus_B = separator_setminus_B(B);
+    KeyVector S_setminus_B = separator_setminus_B(B);
     if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty())
     {
       // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
@@ -124,7 +124,7 @@ namespace gtsam {
       p_Cp_B += parent->conditional_; // P(Fp|Sp)
 
       // Determine the variables we want to keepSet, S union B
-      FastVector<Key> keep = shortcut_indices(B, p_Cp_B);
+      KeyVector keep = shortcut_indices(B, p_Cp_B);
 
       // Marginalize out everything except S union B
       boost::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function);
@@ -170,7 +170,7 @@ namespace gtsam {
         p_Cp += parent->conditional_; // P(Fp|Sp)
 
         // The variables we want to keepSet are exactly the ones in S
-        FastVector<Key> indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
+        KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
         cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function);
       }
     }
diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h
index 055a03939..317ba1c44 100644
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@@ -149,12 +149,12 @@ namespace gtsam {
   protected:
 
     /// Calculate set \f$ S \setminus B \f$ for shortcut calculations
-    FastVector<Key> separator_setminus_B(const derived_ptr& B) const;
+    KeyVector separator_setminus_B(const derived_ptr& B) const;
 
     /** Determine variable indices to keep in recursive separator shortcut calculation The factor
      *  graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables
      *  not in S union B. */
-    FastVector<Key> shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
+    KeyVector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
 
     /** Non-recursive delete cached shortcuts and marginals - internal only. */
     void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h
index 43bcffb09..af2a91257 100644
--- a/gtsam/inference/EliminateableFactorGraph-inst.h
+++ b/gtsam/inference/EliminateableFactorGraph-inst.h
@@ -129,7 +129,7 @@ namespace gtsam {
   template<class FACTORGRAPH>
   std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
     EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
-    const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
+    const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
     if(variableIndex) {
       gttic(eliminatePartialSequential);
@@ -169,7 +169,7 @@ namespace gtsam {
   template<class FACTORGRAPH>
   std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
     EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
-    const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
+    const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
     if(variableIndex) {
       gttic(eliminatePartialMultifrontal);
@@ -190,7 +190,7 @@ namespace gtsam {
   template<class FACTORGRAPH>
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
-    boost::variant<const Ordering&, const std::vector<Key>&> variables,
+    boost::variant<const Ordering&, const KeyVector&> variables,
     OptionalOrdering marginalizedVariableOrdering,
     const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
@@ -223,9 +223,9 @@ namespace gtsam {
         // No ordering was provided for the marginalized variables, so order them using constrained
         // COLAMD.
         bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
-        const std::vector<Key>* variablesOrOrdering =
+        const KeyVector* variablesOrOrdering =
           unmarginalizedAreOrdered ?
-          boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
+          boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
 
         Ordering totalOrdering =
           Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
@@ -249,7 +249,7 @@ namespace gtsam {
   template<class FACTORGRAPH>
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
-    boost::variant<const Ordering&, const std::vector<Key>&> variables,
+    boost::variant<const Ordering&, const KeyVector&> variables,
     OptionalOrdering marginalizedVariableOrdering,
     const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
@@ -282,9 +282,9 @@ namespace gtsam {
         // No ordering was provided for the marginalized variables, so order them using constrained
         // COLAMD.
         bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
-        const std::vector<Key>* variablesOrOrdering =
+        const KeyVector* variablesOrOrdering =
           unmarginalizedAreOrdered ?
-          boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
+          boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
 
         Ordering totalOrdering =
           Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
@@ -308,7 +308,7 @@ namespace gtsam {
   template<class FACTORGRAPH>
   boost::shared_ptr<FACTORGRAPH>
     EliminateableFactorGraph<FACTORGRAPH>::marginal(
-    const std::vector<Key>& variables,
+    const KeyVector& variables,
     const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
     if(variableIndex)
diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h
index 891f22e61..a8f68ca2e 100644
--- a/gtsam/inference/EliminateableFactorGraph.h
+++ b/gtsam/inference/EliminateableFactorGraph.h
@@ -170,7 +170,7 @@ namespace gtsam {
      *  factor graph, and \f$ B = X\backslash A \f$. */
     std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
       eliminatePartialSequential(
-      const std::vector<Key>& variables,
+      const KeyVector& variables,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
 
@@ -190,14 +190,14 @@ namespace gtsam {
      *  factor graph, and \f$ B = X\backslash A \f$. */
     std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
       eliminatePartialMultifrontal(
-      const std::vector<Key>& variables,
+      const KeyVector& variables,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
 
     /** Compute the marginal of the requested variables and return the result as a Bayes net.
      *  @param variables Determines the variables whose marginal to compute, if provided as an
      *         Ordering they will be ordered in the returned BayesNet as specified, and if provided
-     *         as a vector<Key> they will be ordered using constrained COLAMD.
+     *         as a KeyVector they will be ordered using constrained COLAMD.
      *  @param marginalizedVariableOrdering Optional ordering for the variables being marginalized
      *         out, i.e. all variables not in \c variables.  If this is boost::none, the ordering
      *         will be computed with COLAMD.
@@ -206,7 +206,7 @@ namespace gtsam {
      *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
      *         provided one will be computed. */
     boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
-      boost::variant<const Ordering&, const std::vector<Key>&> variables,
+      boost::variant<const Ordering&, const KeyVector&> variables,
       OptionalOrdering marginalizedVariableOrdering = boost::none,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
@@ -214,7 +214,7 @@ namespace gtsam {
     /** Compute the marginal of the requested variables and return the result as a Bayes tree.
      *  @param variables Determines the variables whose marginal to compute, if provided as an
      *         Ordering they will be ordered in the returned BayesNet as specified, and if provided
-     *         as a vector<Key> they will be ordered using constrained COLAMD.
+     *         as a KeyVector they will be ordered using constrained COLAMD.
      *  @param marginalizedVariableOrdering Optional ordering for the variables being marginalized
      *         out, i.e. all variables not in \c variables.  If this is boost::none, the ordering
      *         will be computed with COLAMD.
@@ -223,14 +223,14 @@ namespace gtsam {
      *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
      *         provided one will be computed. */
     boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
-      boost::variant<const Ordering&, const std::vector<Key>&> variables,
+      boost::variant<const Ordering&, const KeyVector&> variables,
       OptionalOrdering marginalizedVariableOrdering = boost::none,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
 
     /** Compute the marginal factor graph of the requested variables. */
     boost::shared_ptr<FactorGraphType> marginal(
-      const std::vector<Key>& variables,
+      const KeyVector& variables,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
 
diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h
index 90540aaa6..333f898b8 100644
--- a/gtsam/inference/EliminationTree-inst.h
+++ b/gtsam/inference/EliminationTree-inst.h
@@ -48,7 +48,7 @@ namespace gtsam {
     gatheredFactors.push_back(childrenResults.begin(), childrenResults.end());
 
     // Do dense elimination step
-    FastVector<Key> keyAsVector(1); keyAsVector[0] = key;
+    KeyVector keyAsVector(1); keyAsVector[0] = key;
     std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
       function(gatheredFactors, Ordering(keyAsVector));
 
diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h
index 3e41d7692..d44d82954 100644
--- a/gtsam/inference/Factor.h
+++ b/gtsam/inference/Factor.h
@@ -58,15 +58,15 @@ namespace gtsam {
 
   public:
     /// Iterator over keys
-    typedef FastVector<Key>::iterator iterator;
+    typedef KeyVector::iterator iterator;
 
     /// Const iterator over keys
-    typedef FastVector<Key>::const_iterator const_iterator;
+    typedef KeyVector::const_iterator const_iterator;
 
   protected:
 
     /// The keys involved in this factor
-    FastVector<Key> keys_;
+    KeyVector keys_;
 
     /// @name Standard Constructors
     /// @{
@@ -112,7 +112,7 @@ namespace gtsam {
     const_iterator find(Key key) const { return std::find(begin(), end(), key); }
 
     /// Access the factor's involved variable keys
-    const FastVector<Key>& keys() const { return keys_; }
+    const KeyVector& keys() const { return keys_; }
 
     /** Iterator at beginning of involved variable keys */
     const_iterator begin() const { return keys_.begin(); }
@@ -148,7 +148,7 @@ namespace gtsam {
     /// @{
 
     /** @return keys involved in this factor */
-    FastVector<Key>& keys() { return keys_; }
+    KeyVector& keys() { return keys_; }
 
     /** Iterator at beginning of involved variable keys */
     iterator begin() { return keys_.begin(); }
diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h
index 4471567fc..785b2507d 100644
--- a/gtsam/inference/ISAM-inst.h
+++ b/gtsam/inference/ISAM-inst.h
@@ -30,7 +30,7 @@ void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
   BayesNetType bn;
   const KeySet newFactorKeys = newFactors.keys();
   if (!this->empty()) {
-    std::vector<Key> keyVector(newFactorKeys.begin(), newFactorKeys.end());
+    KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
     this->removeTop(keyVector, bn, orphans);
   }
 
@@ -46,7 +46,7 @@ void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
   // Get an ordering where the new keys are eliminated last
   const VariableIndex index(factors);
   const Ordering ordering = Ordering::ColamdConstrainedLast(index,
-      std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
+      KeyVector(newFactorKeys.begin(), newFactorKeys.end()));
 
   // eliminate all factors (top, added, orphans) into a new Bayes tree
   auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h
index 2c3eb84c6..d400a33c0 100644
--- a/gtsam/inference/Key.h
+++ b/gtsam/inference/Key.h
@@ -52,7 +52,7 @@ GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
 static const gtsam::KeyFormatter MultiRobotKeyFormatter =
     &_multirobotKeyFormatter;
 
-/// Useful typedef for operations with Values - allows for matlab interface
+/// Define collection type once and for all - also used in wrappers
 typedef FastVector<Key> KeyVector;
 
 // TODO(frank): Nothing fast about these :-(
diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h
index 22b03ee5d..7ec435caa 100644
--- a/gtsam/inference/MetisIndex.h
+++ b/gtsam/inference/MetisIndex.h
@@ -19,7 +19,6 @@
 
 #include <gtsam/inference/Key.h>
 #include <gtsam/inference/FactorGraph.h>
-#include <gtsam/base/FastVector.h>
 #include <gtsam/base/types.h>
 #include <gtsam/base/timing.h>
 
@@ -49,9 +48,8 @@ public:
   typedef boost::bimap<Key, int32_t> bm_type;
 
 private:
-  FastVector<int32_t> xadj_; // Index of node's adjacency list in adj
-  FastVector<int32_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector
-  FastVector<int32_t> iadj_; // Integer keys for passing into metis. One to one mapping with adj_;
+  std::vector<int32_t> xadj_; // Index of node's adjacency list in adj
+  std::vector<int32_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector
   boost::bimap<Key, int32_t> intKeyBMap_; // Stores Key <-> integer value relationship
   size_t nKeys_;
 
@@ -83,10 +81,10 @@ public:
   template<class FACTOR>
   void augment(const FactorGraph<FACTOR>& factors);
 
-  std::vector<int32_t> xadj() const {
+  const std::vector<int32_t>& xadj() const {
     return xadj_;
   }
-  std::vector<int32_t> adj() const {
+  const std::vector<int32_t>& adj() const {
     return adj_;
   }
   size_t nValues() const {
diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp
index b94f01689..1165b4a0f 100644
--- a/gtsam/inference/Ordering.cpp
+++ b/gtsam/inference/Ordering.cpp
@@ -61,7 +61,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
 
   if (nVars == 1)
   {
-    return Ordering(std::vector<Key>(1, variableIndex.begin()->first));
+    return Ordering(KeyVector(1, variableIndex.begin()->first));
   }
 
   const size_t nEntries = variableIndex.nEntries(), nFactors =
@@ -75,7 +75,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
   // Fill in input data for COLAMD
   p[0] = 0;
   int count = 0;
-  vector<Key> keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
+  KeyVector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
   size_t index = 0;
   for (auto key_factors: variableIndex) {
     // Arrange factor indices into COLAMD format
@@ -127,7 +127,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
 
 /* ************************************************************************* */
 Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
-    const std::vector<Key>& constrainLast, bool forceOrder) {
+    const KeyVector& constrainLast, bool forceOrder) {
   gttic(Ordering_COLAMDConstrainedLast);
 
   size_t n = variableIndex.size();
@@ -154,7 +154,7 @@ Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
 
 /* ************************************************************************* */
 Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex,
-    const std::vector<Key>& constrainFirst, bool forceOrder) {
+    const KeyVector& constrainFirst, bool forceOrder) {
   gttic(Ordering_COLAMDConstrainedFirst);
 
   const int none = -1;
diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h
index 30cc54c06..f770c7da1 100644
--- a/gtsam/inference/Ordering.h
+++ b/gtsam/inference/Ordering.h
@@ -31,9 +31,9 @@
 
 namespace gtsam {
 
-class Ordering: public std::vector<Key> {
+class Ordering: public KeyVector {
 protected:
-  typedef std::vector<Key> Base;
+  typedef KeyVector Base;
 
 public:
 
@@ -94,12 +94,12 @@ public:
   /// VariableIndex, it is faster to use COLAMD(const VariableIndex&).  This function constrains
   /// the variables in \c constrainLast to the end of the ordering, and orders all other variables
   /// before in a fill-reducing ordering.  If \c forceOrder is true, the variables in \c
-  /// constrainLast will be ordered in the same order specified in the vector<Key> \c
+  /// constrainLast will be ordered in the same order specified in the KeyVector \c
   /// constrainLast.   If \c forceOrder is false, the variables in \c constrainLast will be
   /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
   template<class FACTOR_GRAPH>
   static Ordering ColamdConstrainedLast(const FACTOR_GRAPH& graph,
-      const std::vector<Key>& constrainLast, bool forceOrder = false) {
+      const KeyVector& constrainLast, bool forceOrder = false) {
     if (graph.empty())
       return Ordering();
     else
@@ -109,11 +109,11 @@ public:
   /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex.  This
   /// function constrains the variables in \c constrainLast to the end of the ordering, and orders
   /// all other variables before in a fill-reducing ordering.  If \c forceOrder is true, the
-  /// variables in \c constrainLast will be ordered in the same order specified in the vector<Key>
+  /// variables in \c constrainLast will be ordered in the same order specified in the KeyVector
   /// \c constrainLast.   If \c forceOrder is false, the variables in \c constrainLast will be
   /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
   static GTSAM_EXPORT Ordering ColamdConstrainedLast(
-      const VariableIndex& variableIndex, const std::vector<Key>& constrainLast,
+      const VariableIndex& variableIndex, const KeyVector& constrainLast,
       bool forceOrder = false);
 
   /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
@@ -121,12 +121,12 @@ public:
   /// VariableIndex, it is faster to use COLAMD(const VariableIndex&).  This function constrains
   /// the variables in \c constrainLast to the end of the ordering, and orders all other variables
   /// before in a fill-reducing ordering.  If \c forceOrder is true, the variables in \c
-  /// constrainFirst will be ordered in the same order specified in the vector<Key> \c
+  /// constrainFirst will be ordered in the same order specified in the KeyVector \c
   /// constrainFirst.   If \c forceOrder is false, the variables in \c constrainFirst will be
   /// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
   template<class FACTOR_GRAPH>
   static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH& graph,
-      const std::vector<Key>& constrainFirst, bool forceOrder = false) {
+      const KeyVector& constrainFirst, bool forceOrder = false) {
     if (graph.empty())
       return Ordering();
     else
@@ -137,12 +137,12 @@ public:
   /// function constrains the variables in \c constrainFirst to the front of the ordering, and
   /// orders all other variables after in a fill-reducing ordering.  If \c forceOrder is true, the
   /// variables in \c constrainFirst will be ordered in the same order specified in the
-  /// vector<Key> \c constrainFirst.   If \c forceOrder is false, the variables in \c
+  /// KeyVector \c constrainFirst.   If \c forceOrder is false, the variables in \c
   /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to
   /// reduce fill-in as well.
   static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
       const VariableIndex& variableIndex,
-      const std::vector<Key>& constrainFirst, bool forceOrder = false);
+      const KeyVector& constrainFirst, bool forceOrder = false);
 
   /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
   /// for note on performance).  This internally builds a VariableIndex so if you already have a
@@ -176,7 +176,7 @@ public:
   template<class FACTOR_GRAPH>
   static Ordering Natural(const FACTOR_GRAPH &fg) {
     KeySet src = fg.keys();
-    std::vector<Key> keys(src.begin(), src.end());
+    KeyVector keys(src.begin(), src.end());
     std::stable_sort(keys.begin(), keys.end());
     return Ordering(keys);
   }
diff --git a/gtsam/linear/BinaryJacobianFactor.h b/gtsam/linear/BinaryJacobianFactor.h
index 462258762..343548613 100644
--- a/gtsam/linear/BinaryJacobianFactor.h
+++ b/gtsam/linear/BinaryJacobianFactor.h
@@ -48,7 +48,7 @@ struct BinaryJacobianFactor: JacobianFactor {
   }
 
   // Fixed-size matrix update
-  void updateHessian(const FastVector<Key>& infoKeys,
+  void updateHessian(const KeyVector& infoKeys,
       SymmetricBlockMatrix* info) const {
     gttic(updateHessian_BinaryJacobianFactor);
     // Whiten the factor if it has a noise model
diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index 60187d129..93217c027 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -117,7 +117,7 @@ namespace gtsam {
   /* ************************************************************************* */
   VectorValues GaussianConditional::solve(const VectorValues& x) const {
     // Concatenate all vector values that correspond to parent variables
-    const Vector xS = x.vector(FastVector<Key>(beginParents(), endParents()));
+    const Vector xS = x.vector(KeyVector(beginParents(), endParents()));
 
     // Update right-hand-side
     const Vector rhs = get_d() - get_S() * xS;
@@ -145,10 +145,10 @@ namespace gtsam {
   VectorValues GaussianConditional::solveOtherRHS(
     const VectorValues& parents, const VectorValues& rhs) const {
     // Concatenate all vector values that correspond to parent variables
-    Vector xS = parents.vector(FastVector<Key>(beginParents(), endParents()));
+    Vector xS = parents.vector(KeyVector(beginParents(), endParents()));
 
     // Instead of updating getb(), update the right-hand-side from the given rhs
-    const Vector rhsR = rhs.vector(FastVector<Key>(beginFrontals(), endFrontals()));
+    const Vector rhsR = rhs.vector(KeyVector(beginFrontals(), endFrontals()));
     xS = rhsR - get_S() * xS;
 
     // Solve Matrix
@@ -171,7 +171,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
-    Vector frontalVec = gy.vector(FastVector<Key>(beginFrontals(), endFrontals()));
+    Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
     frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
 
     // Check for indeterminant solution
diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h
index 7f9c5ea3c..8c72ee734 100644
--- a/gtsam/linear/GaussianFactor.h
+++ b/gtsam/linear/GaussianFactor.h
@@ -126,7 +126,7 @@ namespace gtsam {
      * @param scatter A mapping from variable index to slot index in this HessianFactor
      * @param info The information matrix to be updated
      */
-    virtual void updateHessian(const FastVector<Key>& keys,
+    virtual void updateHessian(const KeyVector& keys,
                            SymmetricBlockMatrix* info) const = 0;
 
     /// y += alpha * A'*A*x
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 428fb6cde..cded6d2ee 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -155,7 +155,7 @@ DenseIndex _getSizeHF(const Vector& m) {
 }
 
 /* ************************************************************************* */
-HessianFactor::HessianFactor(const std::vector<Key>& js,
+HessianFactor::HessianFactor(const KeyVector& js,
     const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
     GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) {
   // Get the number of variables
@@ -356,7 +356,7 @@ double HessianFactor::error(const VectorValues& c) const {
 }
 
 /* ************************************************************************* */
-void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
+void HessianFactor::updateHessian(const KeyVector& infoKeys,
                                   SymmetricBlockMatrix* info) const {
   gttic(updateHessian_HessianFactor);
   assert(info);
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index e28bcdd81..cb9da0f1a 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -159,7 +159,7 @@ namespace gtsam {
      * quadratic term (the Hessian matrix) provided in row-order, gs the pieces
      * of the linear vector term, and f the constant term.
      */
-    HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
+    HessianFactor(const KeyVector& js, const std::vector<Matrix>& Gs,
         const std::vector<Vector>& gs, double f);
 
     /** Constructor with an arbitrary number of keys and with the augmented information matrix
@@ -310,7 +310,7 @@ namespace gtsam {
      * @param keys THe ordered vector of keys for the information matrix to be updated
      * @param info The information matrix to be updated
      */
-    void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const;
+    void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const;
 
     /** Update another Hessian factor
      * @param other the HessianFactor to be updated
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 56a5dc085..1f5e5557c 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -501,7 +501,7 @@ map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
 }
 
 /* ************************************************************************* */
-void JacobianFactor::updateHessian(const FastVector<Key>& infoKeys,
+void JacobianFactor::updateHessian(const KeyVector& infoKeys,
                                    SymmetricBlockMatrix* info) const {
   gttic(updateHessian_JacobianFactor);
 
diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index d7814f541..36d1e12da 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/gtsam/linear/JacobianFactor.h
@@ -283,7 +283,7 @@ namespace gtsam {
      * @param scatter A mapping from variable index to slot index in this HessianFactor
      * @param info The information matrix to be updated
      */
-    void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const;
+    void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const;
 
     /** Return A*x */
     Vector operator*(const VectorValues& x) const;
diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h
index f4df9d96b..1fe7009bc 100644
--- a/gtsam/linear/RegularHessianFactor.h
+++ b/gtsam/linear/RegularHessianFactor.h
@@ -36,7 +36,7 @@ public:
    * quadratic term (the Hessian matrix) provided in row-order, gs the pieces
    * of the linear vector term, and f the constant term.
    */
-  RegularHessianFactor(const std::vector<Key>& js,
+  RegularHessianFactor(const KeyVector& js,
       const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
       HessianFactor(js, Gs, gs, f) {
     checkInvariants();
diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index d51f365be..d796e28b7 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -571,14 +571,14 @@ void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
   /* in place back substitute */
   for (auto cg: boost::adaptors::reverse(*Rc1_)) {
     /* collect a subvector of x that consists of the parents of cg (S) */
-    const Vector xParent = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginParents(), cg->endParents()));
-    const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
+    const Vector xParent = getSubvector(x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents()));
+    const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
 
     /* compute the solution for the current pivot */
     const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);
 
     /* assign subvector of sol to the frontal variables */
-    setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
+    setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
   }
 }
 
@@ -590,7 +590,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
 
   /* in place back substitute */
   for(const boost::shared_ptr<GaussianConditional> & cg: *Rc1_) {
-    const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
+    const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
 //    const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
     const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
 
@@ -598,7 +598,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
     if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front());
 
     /* assign subvector of sol to the frontal variables */
-    setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
+    setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
 
     /* substract from parent variables */
     for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) {
@@ -626,7 +626,7 @@ void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo
 }
 
 /*****************************************************************************/
-Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys) {
+Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) {
 
   /* a cache of starting index and dim */
   typedef vector<pair<size_t, size_t> > Cache;
@@ -652,7 +652,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<
 }
 
 /*****************************************************************************/
-void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst) {
+void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) {
   /* use the cache  */
   size_t idx = 0;
   for ( const Key &key: keys ) {
diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h
index e74b92df1..e440f32e4 100644
--- a/gtsam/linear/SubgraphPreconditioner.h
+++ b/gtsam/linear/SubgraphPreconditioner.h
@@ -298,10 +298,10 @@ namespace gtsam {
   };
 
   /* get subvectors */
-  Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys);
+  Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys);
 
   /* set subvectors */
-  void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst);
+  void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst);
 
 
   /* build a factor subgraph, which is defined as a set of weighted edges (factors) */
diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 89b44f1f8..13601844c 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -229,7 +229,7 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
   VectorValues actual = gbn.optimizeGradientSearch();
 
   // Check that points agree
-  FastVector<Key> keys = list_of(0)(1)(2)(3)(4);
+  KeyVector keys {0, 1, 2, 3, 4};
   Vector actualAsVector = actual.vector(keys);
   EXPECT(assert_equal(expected, actualAsVector, 1e-5));
 
diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp
index e5634c357..2e8b60ac5 100644
--- a/gtsam/linear/tests/testGaussianBayesTree.cpp
+++ b/gtsam/linear/tests/testGaussianBayesTree.cpp
@@ -287,7 +287,7 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
   VectorValues actual = bt.optimizeGradientSearch();
 
   // Check that points agree
-  FastVector<Key> keys = list_of(0)(1)(2)(3)(4);
+  KeyVector keys {0, 1, 2, 3, 4};
   EXPECT(assert_equal(expected, actual.vector(keys), 1e-5));
   EXPECT(assert_equal(expectedFromBN, actual, 1e-5));
 
diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp
index 61fa8bd2c..3e13ecf10 100644
--- a/gtsam/linear/tests/testHessianFactor.cpp
+++ b/gtsam/linear/tests/testHessianFactor.cpp
@@ -41,7 +41,7 @@ const double tol = 1e-5;
 /* ************************************************************************* */
 TEST(HessianFactor, Slot)
 {
-  FastVector<Key> keys = list_of(2)(4)(1);
+  KeyVector keys {2, 4, 1};
   EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2));
   EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4));
   EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1));
@@ -252,8 +252,7 @@ TEST(HessianFactor, ConstructorNWay)
     (1, dx1)
     (2, dx2);
 
-  std::vector<Key> js;
-  js.push_back(0); js.push_back(1); js.push_back(2);
+  KeyVector js {0, 1, 2};
   std::vector<Matrix> Gs;
   Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
   std::vector<Vector> gs;
@@ -517,7 +516,7 @@ TEST(HessianFactor, gradientAtZero)
   // test gradient at zero
   VectorValues expectedG = pair_list_of<Key, Vector>(0, -g1) (1, -g2);
   Matrix A; Vector b; boost::tie(A,b) = factor.jacobian();
-  FastVector<Key> keys; keys += 0,1;
+  KeyVector keys {0, 1};
   EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys)));
   VectorValues actualG = factor.gradientAtZero();
   EXPECT(assert_equal(expectedG, actualG));
diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp
index 26f02923b..2ea1b15bd 100644
--- a/gtsam/linear/tests/testJacobianFactor.cpp
+++ b/gtsam/linear/tests/testJacobianFactor.cpp
@@ -350,7 +350,7 @@ TEST(JacobianFactor, operators )
   VectorValues expectedG;
   expectedG.insert(1, Vector2(20,-10));
   expectedG.insert(2, Vector2(-20, 10));
-  FastVector<Key> keys; keys += 1,2;
+  KeyVector keys {1, 2};
   EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys)));
   VectorValues actualG = lf.gradientAtZero();
   EXPECT(assert_equal(expectedG, actualG));
diff --git a/gtsam/linear/tests/testRegularHessianFactor.cpp b/gtsam/linear/tests/testRegularHessianFactor.cpp
index 1618451f3..3441c6cc2 100644
--- a/gtsam/linear/tests/testRegularHessianFactor.cpp
+++ b/gtsam/linear/tests/testRegularHessianFactor.cpp
@@ -64,7 +64,7 @@ TEST(RegularHessianFactor, Constructors)
   EXPECT(assert_equal(factor,factor2));
 
   // Test n-way constructor
-  vector<Key> keys; keys += 0, 1, 3;
+  KeyVector keys {0, 1, 3};
   vector<Matrix> Gs; Gs += G11, G12, G13, G22, G23, G33;
   vector<Vector> gs; gs += g1, g2, g3;
   RegularHessianFactor<2> factor3(keys, Gs, gs, f);
diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp
index 7e972903a..d1d9990b0 100644
--- a/gtsam/linear/tests/testVectorValues.cpp
+++ b/gtsam/linear/tests/testVectorValues.cpp
@@ -62,7 +62,7 @@ TEST(VectorValues, basics)
   EXPECT(assert_equal(Vector2(2, 3), actual[1]));
   EXPECT(assert_equal(Vector2(4, 5), actual[2]));
   EXPECT(assert_equal(Vector2(6, 7), actual[5]));
-  FastVector<Key> keys = list_of(0)(1)(2)(5);
+  KeyVector keys {0, 1, 2, 5};
   EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), actual.vector(keys)));
 
   // Check exceptions
@@ -101,8 +101,7 @@ TEST(VectorValues, subvector)
   init.insert(12, Vector2(4, 5));
   init.insert(13, Vector2(6, 7));
 
-  std::vector<Key> keys;
-  keys += 10, 12, 13;
+  KeyVector keys {10, 12, 13};
   Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished();
   EXPECT(assert_equal(expSubVector, init.vector(keys)));
 }
@@ -197,7 +196,7 @@ TEST(VectorValues, convert)
   EXPECT(assert_equal(expected, actual2));
 
   // Test other direction, note vector() is not guaranteed to give right result
-  FastVector<Key> keys = list_of(0)(1)(2)(5);
+  KeyVector keys {0, 1, 2, 5};
   EXPECT(assert_equal(x, actual.vector(keys)));
 
   // Test version with dims argument
@@ -222,7 +221,7 @@ TEST(VectorValues, vector_sub)
   expected << 1, 6, 7;
 
   // Test FastVector version
-  FastVector<Key> keys = list_of(0)(5);
+  KeyVector keys {0, 5};
   EXPECT(assert_equal(expected, vv.vector(keys)));
 
   // Test version with dims argument
diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp
index 293bffa00..9457f501d 100644
--- a/gtsam/navigation/tests/testGPSFactor.cpp
+++ b/gtsam/navigation/tests/testGPSFactor.cpp
@@ -22,19 +22,26 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <GeographicLib/Config.h>
 #include <GeographicLib/LocalCartesian.hpp>
 
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
 
+#if GEOGRAPHICLIB_VERSION_MINOR<37
+static const auto& kWGS84 = Geocentric::WGS84;
+#else
+static const auto& kWGS84 = Geocentric::WGS84();
+#endif
+
 // *************************************************************************
 namespace example {
 // ENU Origin is where the plane was in hold next to runway
 const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
 
 // Convert from GPS to ENU
-LocalCartesian origin_ENU(lat0, lon0, h0, Geocentric::WGS84);
+LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);
 
 // Dekalb-Peachtree Airport runway 2L
 const double lat = 33.87071, lon = -84.30482, h = 274;
@@ -107,8 +114,7 @@ TEST(GPSData, init) {
   // GPS Reading 1 will be ENU origin
   double t1 = 84831;
   Point3 NED1(0, 0, 0);
-  LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54,
-      Geocentric::WGS84);
+  LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54, kWGS84);
 
   // GPS Readin 2
   double t2 = 84831.5;
diff --git a/gtsam/navigation/tests/testGeographicLib.cpp b/gtsam/navigation/tests/testGeographicLib.cpp
index aaa01b54d..c568c7445 100644
--- a/gtsam/navigation/tests/testGeographicLib.cpp
+++ b/gtsam/navigation/tests/testGeographicLib.cpp
@@ -15,6 +15,7 @@
  * @author  Frank Dellaert
  */
 
+#include <GeographicLib/Config.h>
 #include <GeographicLib/Geocentric.hpp>
 #include <GeographicLib/UTMUPS.hpp>
 #include <GeographicLib/LocalCartesian.hpp>
@@ -22,6 +23,8 @@
 #include <gtsam/base/types.h>
 #include <CppUnitLite/TestHarness.h>
 
+#include <boost/algorithm/string.hpp>
+#include <string>
 #include <iostream>
 
 using namespace std;
@@ -29,21 +32,27 @@ using namespace std;
 using namespace GeographicLib;
 
 // Dekalb-Peachtree Airport runway 2L
-const double lat = 33.87071, lon = -84.30482, h = 274;
+static const double lat = 33.87071, lon = -84.30482, h = 274;
+
+#if GEOGRAPHICLIB_VERSION_MINOR<37
+static const auto& kWGS84 = Geocentric::WGS84;
+#else
+static const auto& kWGS84 = Geocentric::WGS84();
+#endif
 
 //**************************************************************************
 TEST( GeographicLib, Geocentric) {
 
   // From lat-lon to geocentric
   double X, Y, Z;
-  Geocentric::WGS84.Forward(lat, lon, h, X, Y, Z);
+  kWGS84.Forward(lat, lon, h, X, Y, Z);
   EXPECT_DOUBLES_EQUAL(526, X/1000, 1);
   EXPECT_DOUBLES_EQUAL(-5275, Y/1000, 1);
   EXPECT_DOUBLES_EQUAL(3535, Z/1000, 1);
 
   // From geocentric to lat-lon
   double lat_, lon_, h_;
-  Geocentric::WGS84.Reverse(X, Y, Z, lat_, lon_, h_);
+  kWGS84.Reverse(X, Y, Z, lat_, lon_, h_);
   EXPECT_DOUBLES_EQUAL(lat, lat_, 1e-5);
   EXPECT_DOUBLES_EQUAL(lon, lon_, 1e-5);
   EXPECT_DOUBLES_EQUAL(h, h_, 1e-5);
@@ -61,7 +70,9 @@ TEST( GeographicLib, UTM) {
   // UTM is 16N 749305.58 3751090.08
   // Obtained by
   // http://geographiclib.sourceforge.net/cgi-bin/GeoConvert?input=33.87071+-84.30482000000001&zone=-3&prec=2&option=Submit
-  EXPECT(UTMUPS::EncodeZone(zone, northp)=="16N");
+  auto actual = UTMUPS::EncodeZone(zone, northp);
+  boost::to_upper(actual);
+  EXPECT(actual=="16N");
   EXPECT_DOUBLES_EQUAL(749305.58, x, 1e-2);
   EXPECT_DOUBLES_EQUAL(3751090.08, y, 1e-2);
 }
@@ -69,11 +80,9 @@ TEST( GeographicLib, UTM) {
 //**************************************************************************
 TEST( GeographicLib, ENU) {
 
-  const Geocentric& earth = Geocentric::WGS84;
-
   // ENU Origin is where the plane was in hold next to runway
   const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
-  LocalCartesian enu(lat0, lon0, h0, earth);
+  LocalCartesian enu(lat0, lon0, h0, kWGS84);
 
   // From lat-lon to geocentric
   double E, N, U;
diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp
index 8a8813af5..2cedeea9f 100644
--- a/gtsam/nonlinear/ISAM2-impl.cpp
+++ b/gtsam/nonlinear/ISAM2-impl.cpp
@@ -239,10 +239,10 @@ void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys,
     // Update the current variable
     // Get VectorValues slice corresponding to current variables
     Vector gR =
-        grad.vector(FastVector<Key>(clique->conditional()->beginFrontals(),
+        grad.vector(KeyVector(clique->conditional()->beginFrontals(),
                                     clique->conditional()->endFrontals()));
     Vector gS =
-        grad.vector(FastVector<Key>(clique->conditional()->beginParents(),
+        grad.vector(KeyVector(clique->conditional()->beginParents(),
                                     clique->conditional()->endParents()));
 
     // Compute R*g and S*g for this clique
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index df07050de..32a24b51d 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -195,7 +195,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) {
 /* ************************************************************************* */
 boost::shared_ptr<KeySet> ISAM2::recalculate(
     const KeySet& markedKeys, const KeySet& relinKeys,
-    const vector<Key>& observedKeys, const KeySet& unusedIndices,
+    const KeyVector& observedKeys, const KeySet& unusedIndices,
     const boost::optional<FastMap<Key, int> >& constrainKeys,
     ISAM2Result* result) {
   // TODO(dellaert):  new factors are linearized twice,
@@ -243,7 +243,7 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
   gttic(removetop);
   Cliques orphans;
   GaussianBayesNet affectedBayesNet;
-  this->removeTop(FastVector<Key>(markedKeys.begin(), markedKeys.end()),
+  this->removeTop(KeyVector(markedKeys.begin(), markedKeys.end()),
                   affectedBayesNet, orphans);
   gttoc(removetop);
 
@@ -667,7 +667,7 @@ ISAM2Result ISAM2::update(
   // NOTE: we use assign instead of the iterator constructor here because this
   // is a vector of size_t, so the constructor unintentionally resolves to
   // vector(size_t count, Key value) instead of the iterator constructor.
-  FastVector<Key> observedKeys;
+  KeyVector observedKeys;
   observedKeys.reserve(markedKeys.size());
   for (Key index : markedKeys) {
     if (unusedIndices.find(index) ==
@@ -945,7 +945,7 @@ void ISAM2::marginalizeLeaves(
         // conditional
         auto cg = clique->conditional();
         const KeySet cliqueFrontals(cg->beginFrontals(), cg->endFrontals());
-        FastVector<Key> cliqueFrontalsToEliminate;
+        KeyVector cliqueFrontalsToEliminate;
         std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(),
                               leafKeys.begin(), leafKeys.end(),
                               std::back_inserter(cliqueFrontalsToEliminate));
@@ -967,7 +967,7 @@ void ISAM2::marginalizeLeaves(
         cg->matrixObject().rowStart() = dimToRemove;
 
         // Change the keys in the clique
-        FastVector<Key> originalKeys;
+        KeyVector originalKeys;
         originalKeys.swap(cg->keys());
         cg->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end());
         cg->nrFrontals() -= nToRemove;
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 5d448d786..04bd3d3eb 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -299,7 +299,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
 
   virtual boost::shared_ptr<KeySet> recalculate(
       const KeySet& markedKeys, const KeySet& relinKeys,
-      const std::vector<Key>& observedKeys, const KeySet& unusedIndices,
+      const KeyVector& observedKeys, const KeySet& unusedIndices,
       const boost::optional<FastMap<Key, int> >& constrainKeys,
       ISAM2Result* result);
 
diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp
index 68da1250e..fcbbf2f44 100644
--- a/gtsam/nonlinear/Marginals.cpp
+++ b/gtsam/nonlinear/Marginals.cpp
@@ -80,14 +80,14 @@ Matrix Marginals::marginalCovariance(Key variable) const {
 }
 
 /* ************************************************************************* */
-JointMarginal Marginals::jointMarginalCovariance(const std::vector<Key>& variables) const {
+JointMarginal Marginals::jointMarginalCovariance(const KeyVector& variables) const {
   JointMarginal info = jointMarginalInformation(variables);
   info.blockMatrix_.invertInPlace();
   return info;
 }
 
 /* ************************************************************************* */
-JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variables) const {
+JointMarginal Marginals::jointMarginalInformation(const KeyVector& variables) const {
 
   // If 2 variables, we can use the BayesTree::joint function, otherwise we
   // have to use sequential elimination.
@@ -119,7 +119,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
     Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1);
 
     // Information matrix will be returned with sorted keys
-    std::vector<Key> variablesSorted = variables;
+    KeyVector variablesSorted = variables;
     std::sort(variablesSorted.begin(), variablesSorted.end());
 
     // Get dimensions from factor graph
diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h
index 35b0770c2..e0a78042d 100644
--- a/gtsam/nonlinear/Marginals.h
+++ b/gtsam/nonlinear/Marginals.h
@@ -74,10 +74,10 @@ public:
   Matrix marginalCovariance(Key variable) const;
 
   /** Compute the joint marginal covariance of several variables */
-  JointMarginal jointMarginalCovariance(const std::vector<Key>& variables) const;
+  JointMarginal jointMarginalCovariance(const KeyVector& variables) const;
 
   /** Compute the joint marginal information of several variables */
-  JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const;
+  JointMarginal jointMarginalInformation(const KeyVector& variables) const;
 
   /** Optimize the bayes tree */
   VectorValues optimize() const;
@@ -130,7 +130,7 @@ public:
   void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
 
 protected:
-  JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const std::vector<Key>& keys) :
+  JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const KeyVector& keys) :
     blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {}
 
   friend class Marginals;
diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp
index 5ff022023..c705e3c8f 100644
--- a/gtsam/nonlinear/NonlinearFactor.cpp
+++ b/gtsam/nonlinear/NonlinearFactor.cpp
@@ -52,7 +52,7 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
 
 /* ************************************************************************* */
 NonlinearFactor::shared_ptr NonlinearFactor::rekey(
-    const std::vector<Key>& new_keys) const {
+    const KeyVector& new_keys) const {
   assert(new_keys.size() == keys().size());
   shared_ptr new_factor = clone();
   new_factor->keys() = new_keys;
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index dd4c9123a..4cddc7dda 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -141,7 +141,7 @@ public:
    * Clones a factor and fully replaces its keys
    * @param new_keys is the full replacement set of keys
    */
-  shared_ptr rekey(const std::vector<Key>& new_keys) const;
+  shared_ptr rekey(const KeyVector& new_keys) const;
 
 }; // \class NonlinearFactor
 
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index 56234c13c..a4bdd83e3 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -165,10 +165,10 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
 
   if (formatting.mergeSimilarFactors) {
     // Remove duplicate factors
-    std::set<vector<Key> > structure;
+    std::set<KeyVector > structure;
     for (const sharedFactor& factor : factors_) {
       if (factor) {
-        vector<Key> factorKeys = factor->keys();
+        KeyVector factorKeys = factor->keys();
         std::sort(factorKeys.begin(), factorKeys.end());
         structure.insert(factorKeys);
       }
@@ -176,7 +176,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
 
     // Create factors and variable connections
     size_t i = 0;
-    for(const vector<Key>& factorKeys: structure){
+    for(const KeyVector& factorKeys: structure){
       // Make each factor a dot
       stm << "  factor" << i << "[label=\"\", shape=point";
       {
@@ -199,7 +199,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
     for(size_t i = 0; i < size(); ++i) {
       const NonlinearFactor::shared_ptr& factor = at(i);
       if(formatting.plotFactorPoints) {
-        const FastVector<Key>& keys = factor->keys();
+        const KeyVector& keys = factor->keys();
         if (formatting.binaryEdges && keys.size()==2) {
           stm << "  var" << keys[0] << "--" << "var" << keys[1] << ";\n";
         } else {
diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp
index df1df0d20..9e5e08e92 100644
--- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp
+++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp
@@ -47,8 +47,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
   EXPECT(!actFactor.isHessian());
 
   // check keys
-  FastVector<Key> expKeys; expKeys += l1, l2;
-  EXPECT(assert_container_equality(expKeys, actFactor.keys()));
+  EXPECT(assert_container_equality({l1, l2}, actFactor.keys()));
 
   Values values;
   values.insert(l1, landmark1);
@@ -246,9 +245,7 @@ TEST( testLinearContainerFactor, creation ) {
   LinearContainerFactor actual(linear_factor, full_values);
 
   // Verify the keys
-  FastVector<Key> expKeys;
-  expKeys += l3, l5;
-  EXPECT(assert_container_equality(expKeys, actual.keys()));
+  EXPECT(assert_container_equality({l3, l5}, actual.keys()));
 
   // Verify subset of linearization points
   EXPECT(assert_equal(exp_values, actual.linearizationPoint(), tol));
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index c82ba3391..bcf01eff5 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -279,9 +279,8 @@ TEST(Values, extract_keys)
   config.insert(key3, Pose2());
   config.insert(key4, Pose2());
 
-  KeyVector expected, actual;
-  expected += key1, key2, key3, key4;
-  actual = config.keys();
+  KeyVector expected {key1, key2, key3, key4};
+  KeyVector actual = config.keys();
 
   CHECK(actual.size() == expected.size());
   KeyVector::const_iterator itAct = actual.begin(), itExp = expected.begin();
diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h
index 3816f26f8..58e7e78af 100644
--- a/gtsam/nonlinear/utilities.h
+++ b/gtsam/nonlinear/utilities.h
@@ -54,16 +54,16 @@ FastList<Key> createKeyList(std::string s, const Vector& I) {
 }
 
 // Create a KeyVector from indices
-FastVector<Key> createKeyVector(const Vector& I) {
-  FastVector<Key> set;
+KeyVector createKeyVector(const Vector& I) {
+  KeyVector set;
   for (int i = 0; i < I.size(); i++)
     set.push_back(I[i]);
   return set;
 }
 
 // Create a KeyVector from indices using symbol
-FastVector<Key> createKeyVector(std::string s, const Vector& I) {
-  FastVector<Key> set;
+KeyVector createKeyVector(std::string s, const Vector& I) {
+  KeyVector set;
   char c = s[0];
   for (int i = 0; i < I.size(); i++)
     set.push_back(Symbol(c, I[i]));
@@ -222,12 +222,12 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
 
 /// Convert from local to world coordinates
 Values localToWorld(const Values& local, const Pose2& base,
-    const FastVector<Key> user_keys = FastVector<Key>()) {
+    const KeyVector user_keys = KeyVector()) {
 
   Values world;
 
   // if no keys given, get all keys from local values
-  FastVector<Key> keys(user_keys);
+  KeyVector keys(user_keys);
   if (keys.size()==0)
     keys = local.keys();
 
diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp
index c7309786d..5d57886f5 100644
--- a/gtsam/sam/tests/testRangeFactor.cpp
+++ b/gtsam/sam/tests/testRangeFactor.cpp
@@ -102,9 +102,7 @@ TEST( RangeFactor, ConstructorWithTransform) {
 
   RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model,
       body_P_sensor_2D);
-  KeyVector expected;
-  expected.push_back(2);
-  expected.push_back(1);
+  KeyVector expected {2, 1};
   CHECK(factor2D.keys() == expected);
   RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model,
       body_P_sensor_3D);
diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index 58408e7e3..3979996da 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -53,7 +53,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
     else
       std::cout << "Error in buildLinearOrientationGraph" << std::endl;
 
-    const FastVector<Key>& keys = factor->keys();
+    const KeyVector& keys = factor->keys();
     Key key1 = keys[0], key2 = keys[1];
     Matrix M9 = Z_9x9;
     M9.block(0,0,3,3) = Rij;
diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h
index 3f5290c58..23cadbee9 100644
--- a/gtsam/slam/JacobianFactorQ.h
+++ b/gtsam/slam/JacobianFactorQ.h
@@ -37,7 +37,7 @@ public:
   }
 
   /// Empty constructor with keys
-  JacobianFactorQ(const FastVector<Key>& keys, //
+  JacobianFactorQ(const KeyVector& keys, //
       const SharedDiagonal& model = SharedDiagonal()) :
       Base() {
     Matrix zeroMatrix = Matrix::Zero(0, D);
@@ -50,7 +50,7 @@ public:
   }
 
   /// Constructor
-  JacobianFactorQ(const FastVector<Key>& keys,
+  JacobianFactorQ(const KeyVector& keys,
       const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
       const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
       Base() {
diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h
index 9e61f5324..aef10eb86 100644
--- a/gtsam/slam/JacobianFactorQR.h
+++ b/gtsam/slam/JacobianFactorQR.h
@@ -28,7 +28,7 @@ public:
   /**
    * Constructor
    */
-  JacobianFactorQR(const FastVector<Key>& keys,
+  JacobianFactorQR(const KeyVector& keys,
       const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
       const Vector& b, //
       const SharedDiagonal& model = SharedDiagonal()) :
@@ -46,7 +46,7 @@ public:
     // eliminate the point
     boost::shared_ptr<GaussianBayesNet> bn;
     GaussianFactorGraph::shared_ptr fg;
-    std::vector<Key> variables;
+    KeyVector variables;
     variables.push_back(pointKey);
     boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
     //fg->print("fg");
diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h
index e7bc5864d..b94714dd6 100644
--- a/gtsam/slam/JacobianFactorSVD.h
+++ b/gtsam/slam/JacobianFactorSVD.h
@@ -38,7 +38,7 @@ public:
   }
 
   /// Empty constructor with keys
-  JacobianFactorSVD(const FastVector<Key>& keys, //
+  JacobianFactorSVD(const KeyVector& keys, //
       const SharedDiagonal& model = SharedDiagonal()) :
       Base() {
     Matrix zeroMatrix = Matrix::Zero(0, D);
@@ -58,7 +58,7 @@ public:
    *
    * @Fblocks:
    */
-  JacobianFactorSVD(const FastVector<Key>& keys,
+  JacobianFactorSVD(const KeyVector& keys,
       const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
       const Vector& b, //
       const SharedDiagonal& model = SharedDiagonal()) :
diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h
index 47c9a4c7b..07b749811 100644
--- a/gtsam/slam/RegularImplicitSchurFactor.h
+++ b/gtsam/slam/RegularImplicitSchurFactor.h
@@ -48,7 +48,7 @@ public:
   }
 
   /// Construct from blocks of F, E, inv(E'*E), and RHS vector b
-  RegularImplicitSchurFactor(const FastVector<Key>& keys,
+  RegularImplicitSchurFactor(const KeyVector& keys,
       const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix& P,
       const Vector& b) :
       GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
@@ -108,7 +108,7 @@ public:
     return D;
   }
 
-  virtual void updateHessian(const FastVector<Key>& keys,
+  virtual void updateHessian(const KeyVector& keys,
                          SymmetricBlockMatrix* info) const {
     throw std::runtime_error(
         "RegularImplicitSchurFactor::updateHessian non implemented");
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 497ebbc5b..489a4adf4 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -131,7 +131,7 @@ public:
   /**
    * Add a bunch of measurements, together with the camera keys
    */
-  void add(ZVector& measurements, std::vector<Key>& cameraKeys) {
+  void add(ZVector& measurements, KeyVector& cameraKeys) {
     for (size_t i = 0; i < measurements.size(); i++) {
       this->measured_.push_back(measurements.at(i));
       this->keys_.push_back(cameraKeys.at(i));
@@ -310,7 +310,7 @@ public:
   void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
       const double lambda, bool diagonalDamping,
       SymmetricBlockMatrix& augmentedHessian,
-      const FastVector<Key> allKeys) const {
+      const KeyVector allKeys) const {
     Matrix E;
     Vector b;
     computeJacobians(Fs, E, b, cameras, point);
diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h
index 0d9f90d22..e554e0c85 100644
--- a/gtsam/slam/SmartProjectionFactor.h
+++ b/gtsam/slam/SmartProjectionFactor.h
@@ -179,7 +179,7 @@ public:
 
     size_t numKeys = this->keys_.size();
     // Create structures for Hessian Factors
-    std::vector<Key> js;
+    KeyVector js;
     std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
     std::vector<Vector> gs(numKeys);
 
diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp
index e1cf9cea2..34c8385e8 100644
--- a/gtsam/slam/lago.cpp
+++ b/gtsam/slam/lago.cpp
@@ -168,14 +168,14 @@ GaussianFactorGraph buildLinearOrientationGraph(
 
   // put original measurements in the spanning tree
   for(const size_t& factorId: spanningTreeIds) {
-    const FastVector<Key>& keys = g[factorId]->keys();
+    const KeyVector& keys = g[factorId]->keys();
     Key key1 = keys[0], key2 = keys[1];
     getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
     lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
   }
   // put regularized measurements in the chordsIds
   for(const size_t& factorId: chordsIds) {
-    const FastVector<Key>& keys = g[factorId]->keys();
+    const KeyVector& keys = g[factorId]->keys();
     Key key1 = keys[0], key2 = keys[1];
     getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
     double key1_DeltaTheta_key2 = deltaTheta(0);
diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
index 6f9371e68..b85dd891a 100644
--- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
+++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
@@ -42,7 +42,7 @@ const Matrix26 F0 = Matrix26::Ones();
 const Matrix26 F1 = 2 * Matrix26::Ones();
 const Matrix26 F3 = 3 * Matrix26::Ones();
 const vector<Matrix26, Eigen::aligned_allocator<Matrix26> > FBlocks = list_of<Matrix26>(F0)(F1)(F3);
-const FastVector<Key> keys = list_of<Key>(0)(1)(3);
+const KeyVector keys {0, 1, 3};
 // RHS and sigmas
 const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
 
@@ -86,8 +86,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
   F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3;
 
   // Calculate expected result F'*alpha*(I - E*P*E')*F*x
-  FastVector<Key> keys2;
-  keys2 += 0,1,2,3;
+  KeyVector keys2{0,1,2,3};
   Vector x = xvalues.vector(keys2);
   Vector expected = Vector::Zero(24);
   RegularImplicitSchurFactor<CalibratedCamera>::multiplyHessianAdd(F, E, P, alpha, x, expected);
diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
index 17fec7b9f..aaffbf0e6 100644
--- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
@@ -170,9 +170,7 @@ TEST( SmartProjectionCameraFactor, noisy ) {
   measurements.push_back(level_uv);
   measurements.push_back(level_uv_right);
 
-  vector<Key> views;
-  views.push_back(c1);
-  views.push_back(c2);
+  KeyVector views {c1, c2};
 
   factor2->add(measurements, views);
 
@@ -195,10 +193,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
   SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
   SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
   SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
-  vector<Key> views;
-  views.push_back(c1);
-  views.push_back(c2);
-  views.push_back(c3);
+  KeyVector views {c1, c2, c3};
   smartFactor1->add(measurements_cam1, views);
   smartFactor2->add(measurements_cam2, views);
   smartFactor3->add(measurements_cam3, views);
@@ -293,10 +288,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
-  vector<Key> views;
-  views.push_back(c1);
-  views.push_back(c2);
-  views.push_back(c3);
+  KeyVector views {c1, c2, c3};
 
   SfM_Track track1;
   for (size_t i = 0; i < 3; ++i) {
@@ -370,10 +362,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
   projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
 
-  vector<Key> views;
-  views.push_back(c1);
-  views.push_back(c2);
-  views.push_back(c3);
+  KeyVector views {c1, c2, c3};
 
   SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
   smartFactor1->add(measurements_cam1, views);
@@ -450,10 +439,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
   projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
 
-  vector<Key> views;
-  views.push_back(c1);
-  views.push_back(c2);
-  views.push_back(c3);
+  KeyVector views {c1, c2, c3};
 
   SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
   smartFactor1->add(measurements_cam1, views);
@@ -526,10 +512,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
   projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
 
-  vector<Key> views;
-  views.push_back(c1);
-  views.push_back(c2);
-  views.push_back(c3);
+  KeyVector views {c1, c2, c3};
 
   SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
   smartFactor1->add(measurements_cam1, views);
diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
index 7d3d9c63c..080046dd4 100644
--- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
@@ -189,7 +189,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
   measurements.push_back(level_uv);
   measurements.push_back(level_uv_right);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
 
@@ -236,7 +236,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
   // Create smart factors
-  std::vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -299,7 +299,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -370,7 +370,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
   measurements_cam1.push_back(cam2.project(landmark1));
 
   // Create smart factors
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
 
@@ -459,7 +459,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     b.setZero();
 
     // Create smart factors
-    FastVector<Key> keys;
+    KeyVector keys;
     keys.push_back(x1);
     keys.push_back(x2);
 
@@ -520,7 +520,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
 
   using namespace vanillaPose;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -577,7 +577,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
 
   using namespace vanillaPose;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -638,7 +638,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
 
   double excludeLandmarksFutherThanDist = 2;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -701,7 +701,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
   double excludeLandmarksFutherThanDist = 1e10;
   double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -767,7 +767,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
 
   using namespace vanillaPose;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -821,7 +821,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
 
   using namespace vanillaPose2;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -869,7 +869,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
 /* *************************************************************************/
 TEST( SmartProjectionPoseFactor, CheckHessian) {
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -955,7 +955,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
 TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) {
   using namespace vanillaPose2;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1014,7 +1014,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
   // this test considers a condition in which the cheirality constraint is triggered
   using namespace vanillaPose;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1099,7 +1099,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
 
   using namespace vanillaPose2;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
 
@@ -1133,7 +1133,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
 
   using namespace vanillaPose;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1186,7 +1186,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
 
   using namespace vanillaPose2;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1260,7 +1260,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1309,7 +1309,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
 
   using namespace bundlerPose;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1421,7 +1421,7 @@ TEST(SmartProjectionPoseFactor, serialize2) {
   SmartFactor factor(model, sharedK, bts, params);
 
   // insert some measurments
-  vector<Key> key_view;
+  KeyVector key_view;
   Point2Vector meas_view;
   key_view.push_back(Symbol('x', 1));
   meas_view.push_back(Point2(10, 10));
diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h
index 47bb06515..e3b4c3297 100644
--- a/gtsam/symbolic/SymbolicFactor-inst.h
+++ b/gtsam/symbolic/SymbolicFactor-inst.h
@@ -57,7 +57,7 @@ namespace gtsam
       const size_t nFrontals = keys.size();
 
       // Build a key vector with the frontals followed by the separator
-      FastVector<Key> orderedKeys(allKeys.size());
+      KeyVector orderedKeys(allKeys.size());
       std::copy(keys.begin(), keys.end(), orderedKeys.begin());
       std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);
 
diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
index 99a09adc9..309b8f22e 100644
--- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
+++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
@@ -81,10 +81,7 @@ TEST( SymbolicBayesNet, combine )
 TEST(SymbolicBayesNet, saveGraph) {
   SymbolicBayesNet bn;
   bn += SymbolicConditional(_A_, _B_);
-  std::vector<Key> keys;
-  keys.push_back(_B_);
-  keys.push_back(_C_);
-  keys.push_back(_D_);
+  KeyVector keys {_B_, _C_, _D_};
   bn += SymbolicConditional::FromKeys(keys,2);
   bn += SymbolicConditional(_D_);
 
diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp
index 57c3a2d5f..4e2a7b8c9 100644
--- a/gtsam/symbolic/tests/testSymbolicFactor.cpp
+++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp
@@ -33,7 +33,7 @@ using namespace boost::assign;
 /* ************************************************************************* */
 #ifdef TRACK_ELIMINATE
 TEST(SymbolicFactor, eliminate) {
-  vector<Key> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
+  KeyVector keys {2, 3, 4, 6, 7, 9, 10, 11};
   IndexFactor actual(keys.begin(), keys.end());
   BayesNet<IndexConditional> fragment = *actual.eliminate(3);
 
diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
index a26d2f581..3fd318456 100644
--- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
+++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
@@ -85,7 +85,7 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential)
   SymbolicBayesNet::shared_ptr actualBayesNet2;
   SymbolicFactorGraph::shared_ptr actualSfg2;
   boost::tie(actualBayesNet2, actualSfg2) =
-    simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container<vector<Key> >());
+    simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container<KeyVector >());
 
   EXPECT(assert_equal(expectedSfg, *actualSfg2));
   EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2));
@@ -137,7 +137,7 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal)
   SymbolicBayesTree::shared_ptr actualBayesTree2;
   SymbolicFactorGraph::shared_ptr actualFactorGraph2;
   boost::tie(actualBayesTree2, actualFactorGraph2) =
-    simpleTestGraph2.eliminatePartialMultifrontal(list_of<Key>(4)(5).convert_to_container<vector<Key> >());
+    simpleTestGraph2.eliminatePartialMultifrontal(list_of<Key>(4)(5).convert_to_container<KeyVector >());
 
   EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2));
   EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2));
diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h
index 93f5c5d7d..bbdadd3dc 100644
--- a/gtsam_unstable/discrete/CSP.h
+++ b/gtsam_unstable/discrete/CSP.h
@@ -22,7 +22,7 @@ namespace gtsam {
   public:
 
     /** A map from keys to values */
-    typedef std::vector<Key> Indices;
+    typedef KeyVector Indices;
     typedef Assignment<Key> Values;
     typedef boost::shared_ptr<Values> sharedValues;
 
diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h
index d8f4689a8..32fb6f1ce 100644
--- a/gtsam_unstable/discrete/Constraint.h
+++ b/gtsam_unstable/discrete/Constraint.h
@@ -38,7 +38,7 @@ namespace gtsam {
   protected:
 
     /// Construct n-way factor
-    Constraint(const std::vector<Key>& js) :
+    Constraint(const KeyVector& js) :
       DiscreteFactor(js) {
     }
 
diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp
index 72d424baf..740ef067c 100644
--- a/gtsam_unstable/discrete/Domain.cpp
+++ b/gtsam_unstable/discrete/Domain.cpp
@@ -57,7 +57,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  bool Domain::checkAllDiff(const vector<Key> keys, vector<Domain>& domains) {
+  bool Domain::checkAllDiff(const KeyVector keys, vector<Domain>& domains) {
     Key j = keys_[0];
     // for all values in this domain
     for(size_t value: values_) {
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index 812da9932..5dd597e20 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -104,7 +104,7 @@ namespace gtsam {
      *  If found, we make this a singleton... Called in AllDiff::ensureArcConsistency
      *  @param keys connected domains through alldiff
      */
-    bool checkAllDiff(const std::vector<Key> keys, std::vector<Domain>& domains);
+    bool checkAllDiff(const KeyVector keys, std::vector<Domain>& domains);
 
     /// Partially apply known values
     virtual Constraint::shared_ptr partiallyApply(
diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp
index ec71cae5b..47672a947 100644
--- a/gtsam_unstable/linear/RawQP.cpp
+++ b/gtsam_unstable/linear/RawQP.cpp
@@ -203,9 +203,9 @@ void RawQP::addQuadTerm(
 }
 
 QP RawQP::makeQP() {
-  std::vector < Key > keys;
-  std::vector < Matrix > Gs;
-  std::vector < Vector > gs;
+  KeyVector keys;
+  std::vector<Matrix> Gs;
+  std::vector<Vector> gs;
   for (auto kv : varname_to_key) {
     keys.push_back(kv.second);
   }
diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/RawQP.h
index aadf11e50..70b2a9d27 100644
--- a/gtsam_unstable/linear/RawQP.h
+++ b/gtsam_unstable/linear/RawQP.h
@@ -49,7 +49,7 @@ private:
   std::string name_;
   std::unordered_map<Key, double> up;
   std::unordered_map<Key, double> lo;
-  std::vector<Key> Free;
+  KeyVector Free;
   const bool debug = false;
 
 public:
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
index 884bba9a3..51a959075 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
@@ -362,7 +362,7 @@ void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysT
 
   // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
   if(keysToMove && keysToMove->size() > 0) {
-    ordering_ = Ordering::ColamdConstrainedFirst(factors_, std::vector<Key>(keysToMove->begin(), keysToMove->end()));
+    ordering_ = Ordering::ColamdConstrainedFirst(factors_, KeyVector(keysToMove->begin(), keysToMove->end()));
   }else{
     ordering_ = Ordering::Colamd(factors_);
   }
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
index d64b697b8..0fc975958 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
@@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() {
   variableIndex_ = VariableIndex(factors_);
 
   KeyVector separatorKeys = separatorValues_.keys();
-  ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, std::vector<Key>(separatorKeys.begin(), separatorKeys.end()));
+  ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, KeyVector(separatorKeys.begin(), separatorKeys.end()));
 
 }
 
diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
index 61f1c889f..70cb3e268 100644
--- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
@@ -72,7 +72,7 @@ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph,
     GaussianFactorGraph linearFactorGraph = *graph.linearize(theta);
     // .first is the eliminated Bayes tree, while .second is the remaining factor graph
     GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal(
-        std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end()), eliminateFunction).second;
+        KeyVector(marginalizeKeys.begin(), marginalizeKeys.end()), eliminateFunction).second;
 
     // Wrap in nonlinear container factors
     NonlinearFactorGraph marginalFactors;
diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
index 3913ba95c..e7c8229ef 100644
--- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
@@ -353,7 +353,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
   }
 
   // Create the set of clique keys LC:
-  std::vector<Key> cliqueKeys;
+  KeyVector cliqueKeys;
   for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
     for(Key key: clique->conditional()->frontals()) {
       cliqueKeys.push_back(key);
diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
index e95c1c81d..41a94b876 100644
--- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
@@ -194,7 +194,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
   }
 
   // Create the set of clique keys LC:
-  std::vector<Key> cliqueKeys;
+  KeyVector cliqueKeys;
   for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
     for(Key key: clique->conditional()->frontals()) {
       cliqueKeys.push_back(key);
diff --git a/gtsam_unstable/nonlinear/NonlinearClusterTree.h b/gtsam_unstable/nonlinear/NonlinearClusterTree.h
index b1f463c26..a483c9521 100644
--- a/gtsam_unstable/nonlinear/NonlinearClusterTree.h
+++ b/gtsam_unstable/nonlinear/NonlinearClusterTree.h
@@ -19,7 +19,7 @@ class NonlinearClusterTree : public ClusterTree<NonlinearFactorGraph> {
     // Given graph, index, add factors with specified keys into
     // Factors are erased in the graph
     // TODO(frank): fairly hacky and inefficient. Think about iterating the graph once instead
-    NonlinearCluster(const VariableIndex& variableIndex, const std::vector<Key>& keys,
+    NonlinearCluster(const VariableIndex& variableIndex, const KeyVector& keys,
                      NonlinearFactorGraph* graph) {
       for (const Key key : keys) {
         std::vector<NonlinearFactor::shared_ptr> factors;
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
index 4aff1b465..54ad22bcb 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
@@ -81,7 +81,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
 
   GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
 
-  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
 
   NonlinearFactorGraph LinearContainerForGaussianMarginals;
   for(const GaussianFactor::shared_ptr& factor: marginal) {
@@ -419,9 +419,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
   ordering.push_back(3);
 
   // Create the set of marginalizable variables
-  std::vector<Key> linearIndices;
-  linearIndices.push_back(1);
-  linearIndices.push_back(2);
+  KeyVector linearIndices {1, 2};
 
   GaussianFactorGraph linearPartialGraph = *partialGraph.linearize(partialValues);
   GaussianFactorGraph result = *linearPartialGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
@@ -1008,8 +1006,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_1 )
   GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues);
 
   // Create the set of marginalizable variables
-  std::vector<Key> linearIndices;
-  linearIndices.push_back(1);
+  KeyVector linearIndices {1};
 
   GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
 
@@ -1062,9 +1059,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 )
   GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues);
 
   // Create the set of marginalizable variables
-  std::vector<Key> linearIndices;
-  linearIndices.push_back(1);
-  linearIndices.push_back(2);
+  KeyVector linearIndices {1, 2};
 
   GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
 
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
index a0b6e2c1b..8ecd087c5 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
@@ -563,7 +563,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
   for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
     eliminateKeys.erase(key_value.key);
   }
-  std::vector<Key> variables(eliminateKeys.begin(), eliminateKeys.end());
+  KeyVector variables(eliminateKeys.begin(), eliminateKeys.end());
   GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second;
 
   expectedSmootherSummarization.resize(0);
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
index d6b7ab851..d6f6446e8 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
@@ -98,7 +98,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
 
   GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
 
-  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
 
   NonlinearFactorGraph LinearContainerForGaussianMarginals;
   for(const GaussianFactor::shared_ptr& factor: marginal) {
@@ -429,7 +429,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
 
   GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues);
 
-  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
 
   NonlinearFactorGraph expectedGraph;
 
@@ -513,7 +513,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
 
   GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues);
 
-    GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
+    GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
 
     NonlinearFactorGraph expectedGraph;
 
@@ -1108,17 +1108,15 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
   newValues.insert(3, value3);
 
   // Create the set of marginalizable variables
-  std::vector<Key> linearIndices;
-  linearIndices.push_back(1);
-
   GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
 
-    GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
+  KeyVector linearIndices {1};
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
 
-    NonlinearFactorGraph expectedMarginals;
-    for(const GaussianFactor::shared_ptr& factor: marginal) {
-      expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
-    }
+  NonlinearFactorGraph expectedMarginals;
+  for(const GaussianFactor::shared_ptr& factor: marginal) {
+    expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
+  }
 
   FastList<Key> keysToMarginalize;
   keysToMarginalize.push_back(1);
@@ -1156,15 +1154,10 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
   newValues.insert(3, value3);
 
 
-    // Create the set of marginalizable variables
-  std::vector<Key> linearIndices;
-  linearIndices.push_back(1);
-  linearIndices.push_back(2);
-
-
+  // Create the set of marginalizable variables
+  KeyVector linearIndices {1, 2};
   GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
-
-  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
 
   NonlinearFactorGraph expectedMarginals;
   for(const GaussianFactor::shared_ptr& factor: marginal) {
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
index c265bf5d1..d7bddece2 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
@@ -583,7 +583,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
   for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
     allkeys.erase(key_value.key);
   }
-  std::vector<Key> variables(allkeys.begin(), allkeys.end());
+  KeyVector variables(allkeys.begin(), allkeys.end());
   std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
 
   expectedSmootherSummarization.resize(0);
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
index fdf9f08b5..82d8f2153 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
@@ -584,7 +584,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
   KeySet allkeys = LinFactorGraph->keys();
   for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues)
     allkeys.erase(key_value.key);
-  std::vector<Key> variables(allkeys.begin(), allkeys.end());
+  KeyVector variables(allkeys.begin(), allkeys.end());
   std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
 
   expectedSmootherSummarization.resize(0);
diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h
index 56b1269f0..b009927d6 100644
--- a/gtsam_unstable/slam/BetweenFactorEM.h
+++ b/gtsam_unstable/slam/BetweenFactorEM.h
@@ -40,10 +40,10 @@ public:
 private:
 
   typedef BetweenFactorEM<VALUE> This;
-  typedef gtsam::NonlinearFactor Base;
+  typedef NonlinearFactor Base;
 
-  gtsam::Key key1_;
-  gtsam::Key key2_;
+  Key key1_;
+  Key key2_;
 
   VALUE measured_; /** The measurement */
 
@@ -114,38 +114,38 @@ public:
   /** implement functions needed to derive from Factor */
 
   /* ************************************************************************* */
-  virtual double error(const gtsam::Values& x) const {
+  virtual double error(const Values& x) const {
     return whitenedError(x).squaredNorm();
   }
 
   /* ************************************************************************* */
   /**
-   * Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
+   * Linearize a non-linearFactorN to get a GaussianFactor,
    * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
    * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
    */
   /* This version of linearize recalculates the noise model each time */
-  virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(
-      const gtsam::Values& x) const {
+  virtual boost::shared_ptr<GaussianFactor> linearize(
+      const Values& x) const {
     // Only linearize if the factor is active
     if (!this->active(x))
-      return boost::shared_ptr<gtsam::JacobianFactor>();
+      return boost::shared_ptr<JacobianFactor>();
 
     //std::cout<<"About to linearize"<<std::endl;
-    gtsam::Matrix A1, A2;
-    std::vector<gtsam::Matrix> A(this->size());
-    gtsam::Vector b = -whitenedError(x, A);
+    Matrix A1, A2;
+    std::vector<Matrix> A(this->size());
+    Vector b = -whitenedError(x, A);
     A1 = A[0];
     A2 = A[1];
 
-    return gtsam::GaussianFactor::shared_ptr(
-        new gtsam::JacobianFactor(key1_, A1, key2_, A2, b,
-            gtsam::noiseModel::Unit::Create(b.size())));
+    return GaussianFactor::shared_ptr(
+        new JacobianFactor(key1_, A1, key2_, A2, b,
+            noiseModel::Unit::Create(b.size())));
   }
 
   /* ************************************************************************* */
-  gtsam::Vector whitenedError(const gtsam::Values& x,
-      boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
+  Vector whitenedError(const Values& x,
+      boost::optional<std::vector<Matrix>&> H = boost::none) const {
 
     bool debug = true;
 
@@ -181,11 +181,11 @@ public:
 
       Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1);
       Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1);
-      Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier);
+      Matrix H1_aug = stack(2, &H1_inlier, &H1_outlier);
 
       Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2);
       Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2);
-      Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier);
+      Matrix H2_aug = stack(2, &H2_inlier, &H2_outlier);
 
       (*H)[0].resize(H1_aug.rows(), H1_aug.cols());
       (*H)[1].resize(H2_aug.rows(), H2_aug.cols());
@@ -229,7 +229,7 @@ public:
   }
 
   /* ************************************************************************* */
-  gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const {
+  Vector calcIndicatorProb(const Values& x) const {
 
     bool debug = false;
 
@@ -285,7 +285,7 @@ public:
   }
 
   /* ************************************************************************* */
-  gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
+  Vector unwhitenedError(const Values& x) const {
 
     const T& p1 = x.at<T>(key1_);
     const T& p2 = x.at<T>(key2_);
@@ -328,8 +328,8 @@ public:
   }
 
   /* ************************************************************************* */
-  void updateNoiseModels(const gtsam::Values& values,
-      const gtsam::NonlinearFactorGraph& graph) {
+  void updateNoiseModels(const Values& values,
+      const NonlinearFactorGraph& graph) {
     /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
      * (note these are given in the E step, where indicator probabilities are calculated).
      *
@@ -340,7 +340,7 @@ public:
      */
 
     // get joint covariance of the involved states
-    std::vector<gtsam::Key> Keys;
+    KeyVector Keys;
     Keys.push_back(key1_);
     Keys.push_back(key2_);
     Marginals marginals(graph, values, Marginals::QR);
@@ -353,7 +353,7 @@ public:
   }
 
   /* ************************************************************************* */
-  void updateNoiseModels_givenCovs(const gtsam::Values& values,
+  void updateNoiseModels_givenCovs(const Values& values,
       const Matrix& cov1, const Matrix& cov2, const Matrix& cov12) {
     /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
      * (note these are given in the E step, where indicator probabilities are calculated).
@@ -385,12 +385,12 @@ public:
     // update inlier and outlier noise models
     Matrix covRinlier =
         (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
-    model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(
+    model_inlier_ = noiseModel::Gaussian::Covariance(
         covRinlier + cov_state);
 
     Matrix covRoutlier =
         (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
-    model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(
+    model_outlier_ = noiseModel::Gaussian::Covariance(
         covRoutlier + cov_state);
 
     //       model_inlier_->print("after:");
@@ -426,4 +426,4 @@ private:
 };
 // \class BetweenFactorEM
 
-}/// namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index b7d06b268..f32dd3b3e 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -202,7 +202,7 @@ public:
 
     size_t numKeys = this->keys_.size();
     // Create structures for Hessian Factors
-    std::vector<Key> js;
+    KeyVector js;
     std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
     std::vector<Vector> gs(numKeys);
 
diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
index 3b4c5e0db..7ea5a4c2f 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
@@ -92,7 +92,7 @@ public:
    * @param poseKeys vector of keys corresponding to the camera observing the same landmark
    * @param Ks vector of calibration objects
    */
-  void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
+  void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
       std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
     Base::add(measurements, poseKeys);
     for (size_t i = 0; i < measurements.size(); i++) {
@@ -106,7 +106,7 @@ public:
    * @param poseKeys vector of keys corresponding to the camera observing the same landmark
    * @param K the (known) camera calibration (same for all measurements)
    */
-  void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
+  void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
       const boost::shared_ptr<Cal3_S2Stereo> K) {
     for (size_t i = 0; i < measurements.size(); i++) {
       Base::add(measurements.at(i), poseKeys.at(i));
diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
index bf10ae531..b6d906fd4 100644
--- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
+++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
@@ -43,16 +43,16 @@ namespace gtsam {
   private:
 
     typedef TransformBtwRobotsUnaryFactorEM<VALUE> This;
-    typedef gtsam::NonlinearFactor Base;
+    typedef NonlinearFactor Base;
 
-    gtsam::Key key_;
+    Key key_;
 
     VALUE measured_; /** The measurement */
 
-    gtsam::Values valA_; // given values for robot A map\trajectory
-    gtsam::Values valB_; // given values for robot B map\trajectory
-    gtsam::Key keyA_;    // key of robot A to which the measurement refers
-    gtsam::Key keyB_;    // key of robot B to which the measurement refers
+    Values valA_; // given values for robot A map\trajectory
+    Values valB_; // given values for robot B map\trajectory
+    Key keyA_;    // key of robot A to which the measurement refers
+    Key keyB_;    // key of robot B to which the measurement refers
 
     // TODO: create function to update valA_ and valB_
 
@@ -79,7 +79,7 @@ namespace gtsam {
 
     /** Constructor */
     TransformBtwRobotsUnaryFactorEM(Key key, const VALUE& measured, Key keyA, Key keyB,
-        const gtsam::Values& valA, const gtsam::Values& valB,
+        const Values& valA, const Values& valB,
         const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
         const double prior_inlier, const double prior_outlier,
         const bool flag_bump_up_near_zero_probs = false,
@@ -97,7 +97,7 @@ namespace gtsam {
 
 
     /** Clone */
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
+    virtual NonlinearFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
 
 
     /** implement functions needed for Testable */
@@ -134,7 +134,7 @@ namespace gtsam {
     /** implement functions needed to derive from Factor */
 
     /* ************************************************************************* */
-    void setValAValB(const gtsam::Values& valA, const gtsam::Values& valB){
+    void setValAValB(const Values& valA, const Values& valB){
       if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) )
         throw("something is wrong!");
 
@@ -151,36 +151,36 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    virtual double error(const gtsam::Values& x) const {
+    virtual double error(const Values& x) const {
       return whitenedError(x).squaredNorm();
     }
 
     /* ************************************************************************* */
     /**
-     * Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
+     * Linearize a non-linearFactorN to get a GaussianFactor,
      * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
      * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
      */
     /* This version of linearize recalculates the noise model each time */
-    virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const {
+    virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
       // Only linearize if the factor is active
       if (!this->active(x))
-        return boost::shared_ptr<gtsam::JacobianFactor>();
+        return boost::shared_ptr<JacobianFactor>();
 
       //std::cout<<"About to linearize"<<std::endl;
-      gtsam::Matrix A1;
-      std::vector<gtsam::Matrix> A(this->size());
-      gtsam::Vector b = -whitenedError(x, A);
+      Matrix A1;
+      std::vector<Matrix> A(this->size());
+      Vector b = -whitenedError(x, A);
       A1 = A[0];
 
-      return gtsam::GaussianFactor::shared_ptr(
-          new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(b.size())));
+      return GaussianFactor::shared_ptr(
+          new JacobianFactor(key_, A1, b, noiseModel::Unit::Create(b.size())));
     }
 
 
     /* ************************************************************************* */
-    gtsam::Vector whitenedError(const gtsam::Values& x,
-        boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
+    Vector whitenedError(const Values& x,
+        boost::optional<std::vector<Matrix>&> H = boost::none) const {
 
       bool debug = true;
 
@@ -227,7 +227,7 @@ namespace gtsam {
 
         Matrix H_inlier  = sqrt(p_inlier)*model_inlier_->Whiten(H_unwh);
         Matrix H_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H_unwh);
-        Matrix H_aug = gtsam::stack(2, &H_inlier, &H_outlier);
+        Matrix H_aug = stack(2, &H_inlier, &H_outlier);
 
         (*H)[0].resize(H_aug.rows(),H_aug.cols());
         (*H)[0] = H_aug;
@@ -246,7 +246,7 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const {
+    Vector calcIndicatorProb(const Values& x) const {
 
       Vector err =  unwhitenedError(x);
 
@@ -254,7 +254,7 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    gtsam::Vector calcIndicatorProb(const gtsam::Values& x, const gtsam::Vector& err) const {
+    Vector calcIndicatorProb(const Values& x, const Vector& err) const {
 
       // Calculate indicator probabilities (inlier and outlier)
       Vector err_wh_inlier  = model_inlier_->whiten(err);
@@ -288,7 +288,7 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
+    Vector unwhitenedError(const Values& x) const {
 
       T orgA_T_currA = valA_.at<T>(keyA_);
       T orgB_T_currB = valB_.at<T>(keyB_);
@@ -325,10 +325,10 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    void updateNoiseModels(const gtsam::Values& values, const gtsam::Marginals& marginals) {
+    void updateNoiseModels(const Values& values, const Marginals& marginals) {
       /* given marginals version, don't need to marginal multiple times if update a lot */
 
-      std::vector<gtsam::Key> Keys;
+      KeyVector Keys;
       Keys.push_back(keyA_);
       Keys.push_back(keyB_);
       JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
@@ -340,7 +340,7 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
+    void updateNoiseModels(const Values& values, const NonlinearFactorGraph& graph){
       /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
        * (note these are given in the E step, where indicator probabilities are calculated).
        *
@@ -358,7 +358,7 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
+    void updateNoiseModels_givenCovs(const Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
       /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
        * (note these are given in the E step, where indicator probabilities are calculated).
        *
@@ -389,10 +389,10 @@ namespace gtsam {
 
       // update inlier and outlier noise models
       Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
-      model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
+      model_inlier_ = noiseModel::Gaussian::Covariance(covRinlier + cov_state);
 
       Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
-      model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
+      model_outlier_ = noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
 
       //       model_inlier_->print("after:");
       //       std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
index 229a49e3f..0ac2c24ee 100644
--- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
+++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
@@ -274,7 +274,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
   Ks.push_back(K);
   Ks.push_back(K);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
 
@@ -313,7 +313,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
   vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
       cam2, cam3, landmark3);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -455,7 +455,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) {
   vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
       cam2, cam3, landmark3);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -548,7 +548,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
   // Create smart factors
-  std::vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -614,7 +614,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -680,7 +680,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) {
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -754,7 +754,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
 
 //  double excludeLandmarksFutherThanDist = 2;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -822,7 +822,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -919,7 +919,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, jacobianQ ){
 //
-//  vector<Key> views;
+//  KeyVector views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //  views.push_back(x3);
@@ -980,7 +980,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){
 //
-//  vector<Key> views;
+//  KeyVector views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //  views.push_back(x3);
@@ -1040,7 +1040,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1128,7 +1128,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
 //
-//  vector<Key> views;
+//  KeyVector views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //  views.push_back(x3);
@@ -1194,7 +1194,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
 //
-//  vector<Key> views;
+//  KeyVector views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //  views.push_back(x3);
@@ -1268,7 +1268,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, Hessian ){
 //
-//  vector<Key> views;
+//  KeyVector views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //
@@ -1309,7 +1309,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1379,7 +1379,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) {
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp
index b6b196acc..769b458e4 100644
--- a/tests/testExpressionFactor.cpp
+++ b/tests/testExpressionFactor.cpp
@@ -231,7 +231,7 @@ TEST(ExpressionFactor, Shallow) {
   Point2_ expression = project(transform_to(x_, p_));
 
   // Get and check keys and dims
-  FastVector<Key> keys;
+  KeyVector keys;
   FastVector<int> dims;
   boost::tie(keys, dims) = expression.keysAndDims();
   LONGS_EQUAL(2,keys.size());
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index d6ca896dd..2b6809ada 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -648,7 +648,7 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check)
 namespace {
   bool checkMarginalizeLeaves(ISAM2& isam, const FastList<Key>& leafKeys) {
     Matrix expectedAugmentedHessian, expected3AugmentedHessian;
-    vector<Key> toKeep;
+    KeyVector toKeep;
     for(Key j: isam.getDelta() | br::map_keys)
       if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end())
         toKeep.push_back(j);
diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp
index 392b84deb..f44496e27 100644
--- a/tests/testMarginals.cpp
+++ b/tests/testMarginals.cpp
@@ -139,10 +139,7 @@ TEST(Marginals, planarSLAMmarginals) {
       0.151935669757191,   0.007741936219615,   0.090000180000270,  -0.000000000000000,   0.000000000000000,   0.160967924878730,   0.007741936219615,   0.004516127560770,
      -0.104516127560770,   0.351935664055174,   0.000000000000000,   0.090000180000270,   0.040000000000000,   0.007741936219615,   0.351935664055174,   0.056129031890193,
      -0.050967744878460,   0.056129031890193,   0.000000000000000,   0.000000000000000,   0.010000000000000,   0.004516127560770,   0.056129031890193,   0.027741936219615;
-  vector<Key> variables(3);
-  variables[0] = x1;
-  variables[1] = l2;
-  variables[2] = x3;
+  KeyVector variables {x1, l2, x3};
   JointMarginal joint_l2x1x3 = marginals.jointMarginalCovariance(variables);
   EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,0,2,2)), Matrix(joint_l2x1x3(l2,l2)), 1e-6));
   EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,0,3,2)), Matrix(joint_l2x1x3(x1,l2)), 1e-6));
@@ -227,7 +224,7 @@ TEST(Marginals, order) {
 
   Marginals marginals(fg, vals);
   KeySet set = fg.keys();
-  FastVector<Key> keys(set.begin(), set.end());
+  KeyVector keys(set.begin(), set.end());
   JointMarginal joint = marginals.jointMarginalCovariance(keys);
 
   LONGS_EQUAL(3, (long)joint(0,0).rows());
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index fb85c3742..6e49fa749 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -401,11 +401,7 @@ TEST( NonlinearFactor, clone_rekey )
   EXPECT(assert_equal(*init, *actClone));
 
   // Re-key factor - clones with different keys
-  std::vector<Key> new_keys(4);
-  new_keys[0] = X(5);
-  new_keys[1] = X(6);
-  new_keys[2] = X(7);
-  new_keys[3] = X(8);
+  KeyVector new_keys {X(5),X(6),X(7),X(8)};
   shared_nlf actRekey = init->rekey(new_keys);
   EXPECT(actRekey.get() != init.get()); // Ensure different pointers
 
diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp
index 61e129699..e09b83232 100644
--- a/timing/timeIncremental.cpp
+++ b/timing/timeIncremental.cpp
@@ -153,8 +153,8 @@ int main(int argc, char *argv[]) {
         if(!isam2.getLinearizationPoint().exists(lmKey))
         {
           Pose pose = isam2.calculateEstimate<Pose>(poseKey);
-          Rot2 measuredBearing = measurement->measured().first;
-          double measuredRange = measurement->measured().second;
+          Rot2 measuredBearing = measurement->measured().bearing();
+          double measuredRange = measurement->measured().range();
           newVariables.insert(lmKey, 
             pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
         }
@@ -231,7 +231,7 @@ int main(int argc, char *argv[]) {
       for (Key key2: boost::adaptors::reverse(values.keys())) {
         if(i != j) {
           gttic_(jointMarginalInformation);
-          std::vector<Key> keys(2);
+          KeyVector keys(2);
           keys[0] = key1;
           keys[1] = key2;
           JointMarginal info = marginals.jointMarginalInformation(keys);
diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp
index e64c34340..9e057f830 100644
--- a/timing/timeSchurFactors.cpp
+++ b/timing/timeSchurFactors.cpp
@@ -39,8 +39,8 @@ void timeAll(size_t m, size_t N) {
   // create F
   static const int D = CAMERA::dimension;
   typedef Eigen::Matrix<double, 2, D> Matrix2D;
-  FastVector<Key> keys;
-  vector <Matrix2D> Fblocks;
+  KeyVector keys;
+  vector <Matrix2D, Eigen::aligned_allocator<Matrix2D>> Fblocks;
   for (size_t i = 0; i < m; i++) {
     keys.push_back(i);
     Fblocks.push_back((i + 1) * Matrix::Ones(2, D));