diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index f2bed032f..cd2277965 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -55,6 +55,7 @@ public:
    *  and this typedef informs those classes what "project" returns.
    */
   typedef Point2 Measurement;
+  typedef Point2Vector MeasurementVector;
 
 private:
 
diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index 18f311a23..709172c5b 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -31,7 +31,7 @@ namespace gtsam {
  * @brief A set of cameras, all with their own calibration
  */
 template<class CAMERA>
-class CameraSet: public std::vector<CAMERA> {
+class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> > {
 
 protected:
 
@@ -40,13 +40,14 @@ protected:
    * The order is kept the same as the keys that we use to create the factor.
    */
   typedef typename CAMERA::Measurement Z;
+  typedef typename CAMERA::MeasurementVector ZVector;
 
   static const int D = traits<CAMERA>::dimension; ///< Camera dimension
   static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
 
   /// Make a vector of re-projection errors
-  static Vector ErrorVector(const std::vector<Z>& predicted,
-      const std::vector<Z>& measured) {
+  static Vector ErrorVector(const ZVector& predicted,
+      const ZVector& measured) {
 
     // Check size
     size_t m = predicted.size();
@@ -69,7 +70,7 @@ public:
 
   /// Definitions for blocks of F
   typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
-  typedef std::vector<MatrixZD> FBlocks;
+  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
 
   /**
    * print
@@ -102,7 +103,7 @@ public:
    * throws CheiralityException
    */
   template<class POINT>
-  std::vector<Z> project2(const POINT& point, //
+  ZVector project2(const POINT& point, //
       boost::optional<FBlocks&> Fs = boost::none, //
       boost::optional<Matrix&> E = boost::none) const {
 
@@ -110,7 +111,7 @@ public:
 
     // Allocate result
     size_t m = this->size();
-    std::vector<Z> z;
+    ZVector z;
     z.reserve(m);
 
     // Allocate derivatives
@@ -131,7 +132,7 @@ public:
 
   /// Calculate vector [project2(point)-z] of re-projection errors
   template<class POINT>
-  Vector reprojectionError(const POINT& point, const std::vector<Z>& measured,
+  Vector reprojectionError(const POINT& point, const ZVector& measured,
       boost::optional<FBlocks&> Fs = boost::none, //
       boost::optional<Matrix&> E = boost::none) const {
     return ErrorVector(project2(point, Fs, E), measured);
@@ -315,6 +316,9 @@ private:
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & (*this);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 template<class CAMERA>
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index a5707ce89..4c7fdf94d 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -39,6 +39,7 @@ public:
    *  and this typedef informs those classes what "project" returns.
    */
   typedef Point2 Measurement;
+  typedef Point2Vector MeasurementVector;
 
 private:
 
diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h
index 65047df41..7869704ca 100644
--- a/gtsam/geometry/PinholeSet.h
+++ b/gtsam/geometry/PinholeSet.h
@@ -58,7 +58,7 @@ public:
 
   /// triangulateSafe
   TriangulationResult triangulateSafe(
-      const std::vector<Point2>& measured,
+      const typename CAMERA::MeasurementVector& measured,
       const TriangulationParameters& params) const {
     return gtsam::triangulateSafe(*this, measured, params);
   }
diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index fb250df6d..3657ebf05 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -164,7 +164,7 @@ typedef std::pair<Point2, Point2> Point2Pair;
 std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
 
 // For MATLAB wrapper
-typedef std::vector<Point2> Point2Vector;
+typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
 
 /// multiply with scalar
 inline Point2 operator*(double s, const Point2& p) {
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 6df62485b..3229cbbbe 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -327,6 +327,9 @@ public:
   }
   /// @}
 
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
 };
 // Pose3 class
 
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 58c150670..263301122 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -508,6 +508,9 @@ namespace gtsam {
 #endif
     }
 
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   };
 
   /**
diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h
index ac32be7ae..db4f74026 100644
--- a/gtsam/geometry/StereoCamera.h
+++ b/gtsam/geometry/StereoCamera.h
@@ -43,6 +43,7 @@ public:
    *  and this typedef informs those classes what "project" returns.
    */
   typedef StereoPoint2 Measurement;
+  typedef StereoPoint2Vector MeasurementVector;
 
 private:
   Pose3 leftCamPose_;
diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h
index 961cc041b..dd9bc9dbd 100644
--- a/gtsam/geometry/StereoPoint2.h
+++ b/gtsam/geometry/StereoPoint2.h
@@ -159,6 +159,8 @@ private:
 
 };
 
+typedef std::vector<StereoPoint2> StereoPoint2Vector;
+
 template<>
 struct traits<StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
 
diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp
index 01f784ae0..560206b9f 100644
--- a/gtsam/geometry/tests/testCameraSet.cpp
+++ b/gtsam/geometry/tests/testCameraSet.cpp
@@ -32,7 +32,7 @@ using namespace gtsam;
 TEST(CameraSet, Pinhole) {
   typedef PinholeCamera<Cal3Bundler> Camera;
   typedef CameraSet<Camera> Set;
-  typedef vector<Point2> ZZ;
+  typedef Point2Vector ZZ;
   Set set;
   Camera camera;
   set.push_back(camera);
@@ -135,8 +135,8 @@ TEST(CameraSet, Pinhole) {
 /* ************************************************************************* */
 #include <gtsam/geometry/StereoCamera.h>
 TEST(CameraSet, Stereo) {
-  typedef vector<StereoPoint2> ZZ;
   CameraSet<StereoCamera> set;
+  typedef StereoCamera::MeasurementVector ZZ;
   StereoCamera camera;
   set.push_back(camera);
   set.push_back(camera);
diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp
index 28b7ddac6..f32fe60ed 100644
--- a/gtsam/geometry/tests/testPinholeSet.cpp
+++ b/gtsam/geometry/tests/testPinholeSet.cpp
@@ -28,7 +28,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 #include <gtsam/geometry/CalibratedCamera.h>
 TEST(PinholeSet, Stereo) {
-  typedef vector<Point2> ZZ;
+  typedef Point2Vector ZZ;
   PinholeSet<CalibratedCamera> set;
   CalibratedCamera camera;
   set.push_back(camera);
@@ -72,7 +72,7 @@ TEST(PinholeSet, Stereo) {
 #include <gtsam/geometry/Cal3Bundler.h>
 TEST(PinholeSet, Pinhole) {
   typedef PinholeCamera<Cal3Bundler> Camera;
-  typedef vector<Point2> ZZ;
+  typedef Point2Vector ZZ;
   PinholeSet<Camera> set;
   Camera camera;
   set.push_back(camera);
diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
index 10fd431bc..ca550cdc7 100644
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@@ -743,7 +743,7 @@ namespace {
   /* ************************************************************************* */
   struct Triangle { size_t i_,j_,k_;};
 
-  boost::optional<Pose2> align2(const vector<Point2>& ps, const vector<Point2>& qs,
+  boost::optional<Pose2> align2(const Point2Vector& ps, const Point2Vector& qs,
     const pair<Triangle, Triangle>& trianglePair) {
       const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
       vector<Point2Pair> correspondences;
@@ -755,7 +755,7 @@ namespace {
 TEST(Pose2, align_4) {
   using namespace align_3;
 
-  vector<Point2> ps,qs;
+  Point2Vector ps,qs;
   ps += p1, p2, p3;
   qs += q3, q1, q2; // note in 3,1,2 order !
 
diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp
index 9e599f3f5..97412f94d 100644
--- a/gtsam/geometry/tests/testTriangulation.cpp
+++ b/gtsam/geometry/tests/testTriangulation.cpp
@@ -60,7 +60,7 @@ Point2 z2 = camera2.project(landmark);
 TEST( triangulation, twoPoses) {
 
   vector<Pose3> poses;
-  vector<Point2> measurements;
+  Point2Vector measurements;
 
   poses += pose1, pose2;
   measurements += z1, z2;
@@ -108,7 +108,7 @@ TEST( triangulation, twoPosesBundler) {
   Point2 z2 = camera2.project(landmark);
 
   vector<Pose3> poses;
-  vector<Point2> measurements;
+  Point2Vector measurements;
 
   poses += pose1, pose2;
   measurements += z1, z2;
@@ -132,7 +132,7 @@ TEST( triangulation, twoPosesBundler) {
 //******************************************************************************
 TEST( triangulation, fourPoses) {
   vector<Pose3> poses;
-  vector<Point2> measurements;
+  Point2Vector measurements;
 
   poses += pose1, pose2;
   measurements += z1, z2;
@@ -195,8 +195,8 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   Point2 z1 = camera1.project(landmark);
   Point2 z2 = camera2.project(landmark);
 
-  vector<SimpleCamera> cameras;
-  vector<Point2> measurements;
+  CameraSet<SimpleCamera> cameras;
+  Point2Vector measurements;
 
   cameras += camera1, camera2;
   measurements += z1, z2;
@@ -260,8 +260,8 @@ TEST( triangulation, outliersAndFarLandmarks) {
   Point2 z1 = camera1.project(landmark);
   Point2 z2 = camera2.project(landmark);
 
-  vector<SimpleCamera> cameras;
-  vector<Point2> measurements;
+  CameraSet<SimpleCamera> cameras;
+  Point2Vector measurements;
 
   cameras += camera1, camera2;
   measurements += z1, z2;
@@ -308,7 +308,7 @@ TEST( triangulation, twoIdenticalPoses) {
   Point2 z1 = camera1.project(landmark);
 
   vector<Pose3> poses;
-  vector<Point2> measurements;
+  Point2Vector measurements;
 
   poses += pose1, pose1;
   measurements += z1, z1;
@@ -323,7 +323,7 @@ TEST( triangulation, onePose) {
   // because there's only one camera observation
 
   vector<Pose3> poses;
-  vector<Point2> measurements;
+  Point2Vector measurements;
 
   poses += Pose3();
   measurements += Point2(0,0);
@@ -354,7 +354,7 @@ TEST( triangulation, StereotriangulateNonlinear ) {
   cameras.push_back(StereoCamera(Pose3(m1), stereoK));
   cameras.push_back(StereoCamera(Pose3(m2), stereoK));
 
-  vector<StereoPoint2> measurements;
+  StereoPoint2Vector measurements;
   measurements += StereoPoint2(226.936, 175.212, 424.469);
   measurements += StereoPoint2(339.571, 285.547, 669.973);
 
diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index a1dc3269a..8e9c140c2 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -25,7 +25,7 @@ namespace gtsam {
 
 Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34>& projection_matrices,
-    const std::vector<Point2>& measurements, double rank_tol) {
+    const Point2Vector& measurements, double rank_tol) {
 
   // number of cameras
   size_t m = projection_matrices.size();
@@ -54,7 +54,7 @@ Vector4 triangulateHomogeneousDLT(
 }
 
 Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
-    const std::vector<Point2>& measurements, double rank_tol) {
+    const Point2Vector& measurements, double rank_tol) {
 
   Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
 
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index fdfe32e8b..0f53705ad 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -19,6 +19,7 @@
 #pragma once
 
 #include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/slam/TriangulationFactor.h>
 #include <gtsam/slam/PriorFactor.h>
@@ -53,7 +54,7 @@ public:
  */
 GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34>& projection_matrices,
-    const std::vector<Point2>& measurements, double rank_tol = 1e-9);
+    const Point2Vector& measurements, double rank_tol = 1e-9);
 
 /**
  * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
@@ -64,7 +65,8 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
  */
 GTSAM_EXPORT Point3 triangulateDLT(
     const std::vector<Matrix34>& projection_matrices,
-    const std::vector<Point2>& measurements, double rank_tol = 1e-9);
+    const Point2Vector& measurements, 
+    double rank_tol = 1e-9);
 
 /**
  * Create a factor graph with projection factors from poses and one calibration
@@ -78,7 +80,7 @@ GTSAM_EXPORT Point3 triangulateDLT(
 template<class CALIBRATION>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
     const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
-    const std::vector<Point2>& measurements, Key landmarkKey,
+    const Point2Vector& measurements, Key landmarkKey,
     const Point3& initialEstimate) {
   Values values;
   values.insert(landmarkKey, initialEstimate); // Initial landmark value
@@ -106,8 +108,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
  */
 template<class CAMERA>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
-    const std::vector<CAMERA>& cameras,
-    const std::vector<typename CAMERA::Measurement>& measurements, Key landmarkKey,
+    const CameraSet<CAMERA>& cameras,
+    const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
     const Point3& initialEstimate) {
   Values values;
   values.insert(landmarkKey, initialEstimate); // Initial landmark value
@@ -125,8 +127,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
 /// PinholeCamera specific version // TODO: (chris) why does this exist?
 template<class CALIBRATION>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
-    const std::vector<PinholeCamera<CALIBRATION> >& cameras,
-    const std::vector<Point2>& measurements, Key landmarkKey,
+    const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
+    const Point2Vector& measurements, Key landmarkKey,
     const Point3& initialEstimate) {
   return triangulationGraph<PinholeCamera<CALIBRATION> > //
   (cameras, measurements, landmarkKey, initialEstimate);
@@ -153,7 +155,7 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
 template<class CALIBRATION>
 Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
     boost::shared_ptr<CALIBRATION> sharedCal,
-    const std::vector<Point2>& measurements, const Point3& initialEstimate) {
+    const Point2Vector& measurements, const Point3& initialEstimate) {
 
   // Create a factor graph and initial values
   Values values;
@@ -173,8 +175,8 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
  */
 template<class CAMERA>
 Point3 triangulateNonlinear(
-    const std::vector<CAMERA>& cameras,
-    const std::vector<typename CAMERA::Measurement>& measurements, const Point3& initialEstimate) {
+    const CameraSet<CAMERA>& cameras,
+    const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) {
 
   // Create a factor graph and initial values
   Values values;
@@ -188,8 +190,8 @@ Point3 triangulateNonlinear(
 /// PinholeCamera specific version  // TODO: (chris) why does this exist?
 template<class CALIBRATION>
 Point3 triangulateNonlinear(
-    const std::vector<PinholeCamera<CALIBRATION> >& cameras,
-    const std::vector<Point2>& measurements, const Point3& initialEstimate) {
+    const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
+    const Point2Vector& measurements, const Point3& initialEstimate) {
   return triangulateNonlinear<PinholeCamera<CALIBRATION> > //
   (cameras, measurements, initialEstimate);
 }
@@ -228,7 +230,7 @@ private:
 template<class CALIBRATION>
 Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     boost::shared_ptr<CALIBRATION> sharedCal,
-    const std::vector<Point2>& measurements, double rank_tol = 1e-9,
+    const Point2Vector& measurements, double rank_tol = 1e-9,
     bool optimize = false) {
 
   assert(poses.size() == measurements.size());
@@ -275,8 +277,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
  */
 template<class CAMERA>
 Point3 triangulatePoint3(
-    const std::vector<CAMERA>& cameras,
-    const std::vector<Point2>& measurements, double rank_tol = 1e-9,
+    const CameraSet<CAMERA>& cameras,
+    const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
     bool optimize = false) {
 
   size_t m = cameras.size();
@@ -312,8 +314,8 @@ Point3 triangulatePoint3(
 /// Pinhole-specific version
 template<class CALIBRATION>
 Point3 triangulatePoint3(
-    const std::vector<PinholeCamera<CALIBRATION> >& cameras,
-    const std::vector<Point2>& measurements, double rank_tol = 1e-9,
+    const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
+    const Point2Vector& measurements, double rank_tol = 1e-9,
     bool optimize = false) {
   return triangulatePoint3<PinholeCamera<CALIBRATION> > //
   (cameras, measurements, rank_tol, optimize);
@@ -453,8 +455,8 @@ private:
 
 /// triangulateSafe: extensive checking of the outcome
 template<class CAMERA>
-TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
-    const std::vector<Point2>& measured,
+TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
+    const typename CAMERA::MeasurementVector& measured,
     const TriangulationParameters& params) {
 
   size_t m = cameras.size();
diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h
index 99c45ff34..6fd98bfcb 100644
--- a/gtsam/navigation/CombinedImuFactor.h
+++ b/gtsam/navigation/CombinedImuFactor.h
@@ -83,12 +83,12 @@ public:
 
     // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
     static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
-      return boost::make_shared<Params>(Vector3(0, 0, g));
+      return boost::shared_ptr<Params>(new Params(Vector3(0, 0, g)));
     }
 
     // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
     static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
-      return boost::make_shared<Params>(Vector3(0, 0, -g));
+      return boost::shared_ptr<Params>(new Params(Vector3(0, 0, -g)));
     }
 
    private:
@@ -104,6 +104,9 @@ public:
       ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
       ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
     }
+
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   };
 
  protected:
@@ -195,6 +198,9 @@ public:
     ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
     ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /**
diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h
index 2ad3c17dd..5fc82f0a3 100644
--- a/gtsam/navigation/ImuBias.h
+++ b/gtsam/navigation/ImuBias.h
@@ -169,6 +169,9 @@ private:
     ar & BOOST_SERIALIZATION_NVP(biasGyro_);
   }
 
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   /// @}
 
 }; // ConstantBias class
diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h
index cb40774f4..025278c81 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/gtsam/navigation/PreintegratedRotation.h
@@ -58,6 +58,9 @@ struct PreintegratedRotationParams {
     ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
     ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /**
@@ -165,6 +168,9 @@ class PreintegratedRotation {
     ar& BOOST_SERIALIZATION_NVP(deltaRij_);
     ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
   }
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 template <>
diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h
index f2b2c0fef..1c673bff5 100644
--- a/gtsam/navigation/PreintegrationParams.h
+++ b/gtsam/navigation/PreintegrationParams.h
@@ -39,12 +39,12 @@ struct PreintegrationParams: PreintegratedRotationParams {
 
   // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
   static boost::shared_ptr<PreintegrationParams> MakeSharedD(double g = 9.81) {
-    return boost::make_shared<PreintegrationParams>(Vector3(0, 0, g));
+    return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, g)));
   }
 
   // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
   static boost::shared_ptr<PreintegrationParams> MakeSharedU(double g = 9.81) {
-    return boost::make_shared<PreintegrationParams>(Vector3(0, 0, -g));
+    return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g)));
   }
 
   void print(const std::string& s) const;
@@ -74,6 +74,9 @@ protected:
     ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
     ar & BOOST_SERIALIZATION_NVP(n_gravity);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } // namespace gtsam
diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h
index d09627b77..3f5290c58 100644
--- a/gtsam/slam/JacobianFactorQ.h
+++ b/gtsam/slam/JacobianFactorQ.h
@@ -51,7 +51,7 @@ public:
 
   /// Constructor
   JacobianFactorQ(const FastVector<Key>& keys,
-      const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P,
+      const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
       const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
       Base() {
     size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h
index 77102c24b..9e61f5324 100644
--- a/gtsam/slam/JacobianFactorQR.h
+++ b/gtsam/slam/JacobianFactorQR.h
@@ -29,7 +29,7 @@ public:
    * Constructor
    */
   JacobianFactorQR(const FastVector<Key>& keys,
-      const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P,
+      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/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h
index 8afe0bcbf..e7bc5864d 100644
--- a/gtsam/slam/JacobianFactorSVD.h
+++ b/gtsam/slam/JacobianFactorSVD.h
@@ -59,7 +59,7 @@ public:
    * @Fblocks:
    */
   JacobianFactorSVD(const FastVector<Key>& keys,
-      const std::vector<MatrixZD>& Fblocks, const Matrix& Enull,
+      const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
       const Vector& b, //
       const SharedDiagonal& model = SharedDiagonal()) :
       Base() {
diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h
index 7c481d0c8..2c0614cd6 100644
--- a/gtsam/slam/RegularImplicitSchurFactor.h
+++ b/gtsam/slam/RegularImplicitSchurFactor.h
@@ -36,7 +36,7 @@ protected:
   typedef Eigen::Matrix<double, ZDim, D> MatrixZD; ///< type of an F block
   typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
 
-  const std::vector<MatrixZD> FBlocks_; ///< All ZDim*D F blocks (one for each camera)
+  const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks_; ///< All ZDim*D F blocks (one for each camera)
   const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
   const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
   const Vector b_; ///< 2m-dimensional RHS vector
@@ -49,7 +49,7 @@ public:
 
   /// Construct from blocks of F, E, inv(E'*E), and RHS vector b
   RegularImplicitSchurFactor(const FastVector<Key>& keys,
-      const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix& P,
+      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) {
   }
@@ -58,7 +58,7 @@ public:
   virtual ~RegularImplicitSchurFactor() {
   }
 
-  std::vector<MatrixZD>& FBlocks() const {
+  std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks() const {
     return FBlocks_;
   }
 
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 1453d80a7..497ebbc5b 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -50,12 +50,14 @@ private:
   typedef NonlinearFactor Base;
   typedef SmartFactorBase<CAMERA> This;
   typedef typename CAMERA::Measurement Z;
+  typedef typename CAMERA::MeasurementVector ZVector;
 
 public:
 
   static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
   static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
   typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrpt camera)
+  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
 
 protected:
   /**
@@ -71,14 +73,14 @@ protected:
    * We keep a copy of measurements for I/O and computing the error.
    * The order is kept the same as the keys that we use to create the factor.
    */
-  std::vector<Z> measured_;
+  ZVector measured_;
 
   /// @name Pose of the camera in the body frame
   boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
   /// @}
 
   // Cache for Fblocks, to avoid a malloc ever time we re-linearize
-  mutable std::vector<MatrixZD> Fblocks;
+  mutable FBlocks Fs;
 
 public:
 
@@ -97,7 +99,7 @@ public:
   SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
                   boost::optional<Pose3> body_P_sensor = boost::none,
                   size_t expectedNumberCameras = 10)
-      : body_P_sensor_(body_P_sensor), Fblocks(expectedNumberCameras) {
+      : body_P_sensor_(body_P_sensor), Fs(expectedNumberCameras) {
 
     if (!sharedNoiseModel)
       throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
@@ -129,7 +131,7 @@ public:
   /**
    * Add a bunch of measurements, together with the camera keys
    */
-  void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys) {
+  void add(ZVector& measurements, std::vector<Key>& cameraKeys) {
     for (size_t i = 0; i < measurements.size(); i++) {
       this->measured_.push_back(measurements.at(i));
       this->keys_.push_back(cameraKeys.at(i));
@@ -154,7 +156,7 @@ public:
   }
 
   /** return the measurements */
-  const std::vector<Z>& measured() const {
+  const ZVector& measured() const {
     return measured_;
   }
 
@@ -258,22 +260,22 @@ public:
    *  TODO: Kill this obsolete method
    */
   template<class POINT>
-  void computeJacobians(std::vector<MatrixZD>& Fblocks, Matrix& E, Vector& b,
+  void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b,
       const Cameras& cameras, const POINT& point) const {
     // Project into Camera set and calculate derivatives
     // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar)
     // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z|
     //                                         = |A*dx - (z-h(x_bar))|
-    b = -unwhitenedError(cameras, point, Fblocks, E);
+    b = -unwhitenedError(cameras, point, Fs, E);
   }
 
   /// SVD version
   template<class POINT>
-  void computeJacobiansSVD(std::vector<MatrixZD>& Fblocks, Matrix& Enull,
+  void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull,
       Vector& b, const Cameras& cameras, const POINT& point) const {
 
     Matrix E;
-    computeJacobians(Fblocks, E, b, cameras, point);
+    computeJacobians(Fs, E, b, cameras, point);
 
     static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
 
@@ -291,10 +293,10 @@ public:
 
     Matrix E;
     Vector b;
-    computeJacobians(Fblocks, E, b, cameras, point);
+    computeJacobians(Fs, E, b, cameras, point);
 
     // build augmented hessian
-    SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, b);
+    SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fs, E, b);
 
     return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
         augmentedHessian);
@@ -311,12 +313,12 @@ public:
       const FastVector<Key> allKeys) const {
     Matrix E;
     Vector b;
-    computeJacobians(Fblocks, E, b, cameras, point);
-    Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, augmentedHessian);
+    computeJacobians(Fs, E, b, cameras, point);
+    Cameras::UpdateSchurComplement(Fs, E, b, allKeys, keys_, augmentedHessian);
   }
 
   /// Whiten the Jacobians computed by computeJacobians using noiseModel_
-  void whitenJacobians(std::vector<MatrixZD>& F, Matrix& E, Vector& b) const {
+  void whitenJacobians(FBlocks& F, Matrix& E, Vector& b) const {
     noiseModel_->WhitenSystem(E, b);
     // TODO make WhitenInPlace work with any dense matrix type
     for (size_t i = 0; i < F.size(); i++)
@@ -329,7 +331,7 @@ public:
       double lambda = 0.0, bool diagonalDamping = false) const {
     Matrix E;
     Vector b;
-    std::vector<MatrixZD> F;
+    FBlocks F;
     computeJacobians(F, E, b, cameras, point);
     whitenJacobians(F, E, b);
     Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
@@ -345,7 +347,7 @@ public:
       bool diagonalDamping = false) const {
     Matrix E;
     Vector b;
-    std::vector<MatrixZD> F;
+    FBlocks F;
     computeJacobians(F, E, b, cameras, point);
     const size_t M = b.size();
     Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
@@ -360,7 +362,7 @@ public:
   boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
     size_t m = this->keys_.size();
-    std::vector<MatrixZD> F;
+    FBlocks F;
     Vector b;
     const size_t M = ZDim * m;
     Matrix E0(M, M - 3);
@@ -371,12 +373,12 @@ public:
   }
 
   /// Create BIG block-diagonal matrix F from Fblocks
-  static void FillDiagonalF(const std::vector<MatrixZD>& Fblocks, Matrix& F) {
-    size_t m = Fblocks.size();
+  static void FillDiagonalF(const FBlocks& Fs, Matrix& F) {
+    size_t m = Fs.size();
     F.resize(ZDim * m, Dim * m);
     F.setZero();
     for (size_t i = 0; i < m; ++i)
-      F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fblocks.at(i);
+      F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fs.at(i);
   }
 
 
diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h
index 840ecbc4d..0d9f90d22 100644
--- a/gtsam/slam/SmartProjectionFactor.h
+++ b/gtsam/slam/SmartProjectionFactor.h
@@ -61,7 +61,7 @@ protected:
   /// @name Caching triangulation
   /// @{
   mutable TriangulationResult result_; ///< result from triangulateSafe
-  mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses
+  mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> > cameraPosesTriangulation_; ///< current triangulation poses
   /// @}
 
 public:
@@ -203,7 +203,7 @@ public:
     }
 
     // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
-    std::vector<typename Base::MatrixZD> Fblocks;
+    std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> > Fblocks;
     Matrix E;
     Vector b;
     computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
@@ -337,7 +337,7 @@ public:
   /// Assumes the point has been computed
   /// Note E can be 2m*3 or 2m*2, in case point is degenerate
   void computeJacobiansWithTriangulatedPoint(
-      std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
+      std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& E, Vector& b,
       const Cameras& cameras) const {
 
     if (!result_) {
@@ -354,7 +354,7 @@ public:
 
   /// Version that takes values, and creates the point
   bool triangulateAndComputeJacobians(
-      std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
+      std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& E, Vector& b,
       const Values& values) const {
     Cameras cameras = this->cameras(values);
     bool nonDegenerate = triangulateForLinearize(cameras);
@@ -365,7 +365,7 @@ public:
 
   /// takes values
   bool triangulateAndComputeJacobiansSVD(
-      std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
+      std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& Enull, Vector& b,
       const Values& values) const {
     Cameras cameras = this->cameras(values);
     bool nonDegenerate = triangulateForLinearize(cameras);
diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h
index ccad83f01..4abc59305 100644
--- a/gtsam/slam/tests/smartFactorScenarios.h
+++ b/gtsam/slam/tests/smartFactorScenarios.h
@@ -132,7 +132,7 @@ CAMERA perturbCameraPose(const CAMERA& camera) {
 
 template<class CAMERA>
 void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2,
-    const CAMERA& cam3, Point3 landmark, vector<Point2>& measurements_cam) {
+    const CAMERA& cam3, Point3 landmark, typename CAMERA::MeasurementVector& measurements_cam) {
   Point2 cam1_uv1 = cam1.project(landmark);
   Point2 cam2_uv1 = cam2.project(landmark);
   Point2 cam3_uv1 = cam3.project(landmark);
diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
index 4184d6769..6f9371e68 100644
--- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
+++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
@@ -41,7 +41,7 @@ using namespace gtsam;
 const Matrix26 F0 = Matrix26::Ones();
 const Matrix26 F1 = 2 * Matrix26::Ones();
 const Matrix26 F3 = 3 * Matrix26::Ones();
-const vector<Matrix26> FBlocks = list_of<Matrix26>(F0)(F1)(F3);
+const vector<Matrix26, Eigen::aligned_allocator<Matrix26> > FBlocks = list_of<Matrix26>(F0)(F1)(F3);
 const FastVector<Key> keys = list_of<Key>(0)(1)(3);
 // RHS and sigmas
 const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
index 4feeadc86..17fec7b9f 100644
--- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
@@ -166,7 +166,7 @@ TEST( SmartProjectionCameraFactor, noisy ) {
   EXPECT(assert_equal(expected, actual, 1));
 
   SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
-  vector<Point2> measurements;
+  Point2Vector measurements;
   measurements.push_back(level_uv);
   measurements.push_back(level_uv_right);
 
@@ -186,7 +186,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
   using namespace vanilla;
 
   // Project three landmarks into three cameras
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
@@ -288,7 +288,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
   using namespace vanilla;
 
   // Project three landmarks into three cameras
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
@@ -360,7 +360,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
 
   using namespace vanilla;
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
       measurements_cam4, measurements_cam5;
 
   // 1. Project three landmarks into three cameras and triangulate
@@ -440,7 +440,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
 
   using namespace bundler;
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
       measurements_cam4, measurements_cam5;
 
   // 1. Project three landmarks into three cameras and triangulate
@@ -516,7 +516,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
 
   using namespace bundler;
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
       measurements_cam4, measurements_cam5;
 
   // 1. Project three landmarks into three cameras and triangulate
@@ -768,8 +768,8 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
   Point3 point(0,0,0);
   if (factor1->point())
     point = *(factor1->point());
-  vector<Matrix29> Fblocks;
-  factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point);
+  SmartFactor::FBlocks Fs;
+  factor1->computeJacobians(Fs, expectedE, expectedb, cameras, point);
 
   NonlinearFactorGraph generalGraph;
   SFMFactor sfm1(level_uv, unit2, c1, l1);
diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
index 71ee79d86..7d3d9c63c 100644
--- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
@@ -155,8 +155,8 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
 
   // Calculate using computeJacobians
   Vector b;
-  vector<SmartFactor::MatrixZD> Fblocks;
-  factor.computeJacobians(Fblocks, E, b, cameras, *point);
+  SmartFactor::FBlocks Fs;
+  factor.computeJacobians(Fs, E, b, cameras, *point);
   double actualError3 = b.squaredNorm();
   EXPECT(assert_equal(expectedE, E, 1e-7));
   EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8);
@@ -185,7 +185,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
   double actualError1 = factor->error(values);
 
   SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK));
-  vector<Point2> measurements;
+  Point2Vector measurements;
   measurements.push_back(level_uv);
   measurements.push_back(level_uv_right);
 
@@ -228,7 +228,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
   Point3 landmark2(5, -0.5, 1.2);
   Point3 landmark3(5, 0, 3.0);
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@@ -292,7 +292,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
 TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
 
   using namespace vanillaPose2;
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@@ -363,7 +363,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
   // one landmarks 1m in front of camera
   Point3 landmark1(0, 0, 10);
 
-  vector<Point2> measurements_cam1;
+  Point2Vector measurements_cam1;
 
   // Project 2 landmarks into 2 cameras
   measurements_cam1.push_back(cam1.project(landmark1));
@@ -454,7 +454,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     E(2, 0) = 10;
     E(2, 2) = 1;
     E(3, 1) = 10;
-    vector<Matrix26> Fblocks = list_of<Matrix>(F1)(F2);
+    SmartFactor::FBlocks Fs = list_of<Matrix>(F1)(F2);
     Vector b(4);
     b.setZero();
 
@@ -466,7 +466,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     // createJacobianQFactor
     SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
     Matrix3 P = (E.transpose() * E).inverse();
-    JacobianFactorQ<6, 2> expectedQ(keys, Fblocks, E, P, b, n);
+    JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n);
     EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8));
 
     boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
@@ -480,11 +480,11 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     // Whiten for RegularImplicitSchurFactor (does not have noise model)
     model->WhitenSystem(E, b);
     Matrix3 whiteP = (E.transpose() * E).inverse();
-    Fblocks[0] = model->Whiten(Fblocks[0]);
-    Fblocks[1] = model->Whiten(Fblocks[1]);
+    Fs[0] = model->Whiten(Fs[0]);
+    Fs[1] = model->Whiten(Fs[1]);
 
     // createRegularImplicitSchurFactor
-    RegularImplicitSchurFactor<Camera> expected(keys, Fblocks, E, whiteP, b);
+    RegularImplicitSchurFactor<Camera> expected(keys, Fs, E, whiteP, b);
 
     boost::shared_ptr<RegularImplicitSchurFactor<Camera> > actual =
         smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
@@ -525,7 +525,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
   views.push_back(x2);
   views.push_back(x3);
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@@ -582,7 +582,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
   views.push_back(x2);
   views.push_back(x3);
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@@ -643,7 +643,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
   views.push_back(x2);
   views.push_back(x3);
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@@ -709,7 +709,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
   // add fourth landmark
   Point3 landmark4(5, -0.5, 1);
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
       measurements_cam4;
 
   // Project 4 landmarks into three cameras
@@ -772,7 +772,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
   views.push_back(x2);
   views.push_back(x3);
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@@ -883,7 +883,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
   Camera cam2(pose2, sharedK);
   Camera cam3(pose3, sharedK);
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@@ -966,7 +966,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
   Camera cam2(pose2, sharedK2);
   Camera cam3(pose3, sharedK2);
 
-  vector<Point2> measurements_cam1, measurements_cam2;
+  Point2Vector measurements_cam1, measurements_cam2;
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@@ -1026,7 +1026,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
   Camera cam2(pose2, sharedK);
   Camera cam3(pose3, sharedK);
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@@ -1106,7 +1106,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
   // Project three landmarks into 2 cameras
   Point2 cam1_uv1 = cam1.project(landmark1);
   Point2 cam2_uv1 = cam2.project(landmark1);
-  vector<Point2> measurements_cam1;
+  Point2Vector measurements_cam1;
   measurements_cam1.push_back(cam1_uv1);
   measurements_cam1.push_back(cam2_uv1);
 
@@ -1138,7 +1138,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
   views.push_back(x2);
   views.push_back(x3);
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
 
@@ -1195,7 +1195,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
   Camera cam2(level_pose, sharedK2);
   Camera cam3(level_pose, sharedK2);
 
-  vector<Point2> measurements_cam1;
+  Point2Vector measurements_cam1;
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
 
   SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2));
@@ -1253,7 +1253,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
   // three landmarks ~5 meters in front of camera
   Point3 landmark3(3, 0, 3.0);
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@@ -1324,7 +1324,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
   // landmark3 at 3 meters now
   Point3 landmark3(3, 0, 3.0);
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@@ -1422,7 +1422,7 @@ TEST(SmartProjectionPoseFactor, serialize2) {
 
   // insert some measurments
   vector<Key> key_view;
-  vector<Point2> meas_view;
+  Point2Vector meas_view;
   key_view.push_back(Symbol('x', 1));
   meas_view.push_back(Point2(10, 10));
   factor.add(meas_view, key_view);
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index 29ae6233b..b7d06b268 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -78,7 +78,7 @@ public:
   /// Vector of monocular cameras (stereo treated as 2 monocular)
   typedef PinholeCamera<Cal3_S2> MonoCamera;
   typedef CameraSet<MonoCamera> MonoCameras;
-  typedef std::vector<Point2> MonoMeasurements;
+  typedef MonoCamera::MeasurementVector MonoMeasurements;
 
   /**
    * Constructor
@@ -226,17 +226,17 @@ public:
     }
 
     // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
-    std::vector<Base::MatrixZD> Fblocks;
+    Base::FBlocks Fs;
     Matrix F, E;
     Vector b;
-    computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
+    computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
 
     // Whiten using noise model
-    Base::whitenJacobians(Fblocks, E, b);
+    Base::whitenJacobians(Fs, E, b);
 
     // build augmented hessian
     SymmetricBlockMatrix augmentedHessian = //
-        Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);
+        Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
 
     return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
         augmentedHessian);
@@ -360,7 +360,8 @@ public:
   /// Assumes the point has been computed
   /// Note E can be 2m*3 or 2m*2, in case point is degenerate
   void computeJacobiansWithTriangulatedPoint(
-      std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
+      FBlocks& Fs, 
+      Matrix& E, Vector& b,
       const Cameras& cameras) const {
 
     if (!result_) {
@@ -370,32 +371,32 @@ public:
 //      Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity(
 //          this->measured_.at(0)); */
 //
-//      Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
+//      Base::computeJacobians(Fs, E, b, cameras, backProjected);
     } else {
       // valid result: just return Base version
-      Base::computeJacobians(Fblocks, E, b, cameras, *result_);
+      Base::computeJacobians(Fs, E, b, cameras, *result_);
     }
   }
 
   /// Version that takes values, and creates the point
   bool triangulateAndComputeJacobians(
-      std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
+      FBlocks& Fs, Matrix& E, Vector& b,
       const Values& values) const {
     Cameras cameras = this->cameras(values);
     bool nonDegenerate = triangulateForLinearize(cameras);
     if (nonDegenerate)
-      computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
+      computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
     return nonDegenerate;
   }
 
   /// takes values
   bool triangulateAndComputeJacobiansSVD(
-      std::vector<Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
+      FBlocks& Fs, Matrix& Enull, Vector& b,
       const Values& values) const {
     Cameras cameras = this->cameras(values);
     bool nonDegenerate = triangulateForLinearize(cameras);
     if (nonDegenerate)
-      Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
+      Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
     return nonDegenerate;
   }
 
diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
index a018ec7ee..229a49e3f 100644
--- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
+++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
@@ -540,7 +540,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
   Point3 landmark2(5, -0.5, 1.2);
   Point3 landmark3(5, 0, 3.0);
 
-  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);