diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index c87705088..defdbe494 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -20,14 +20,13 @@
 
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
-#include <boost/foreach.hpp>
-
 #include <vector>
 
 namespace gtsam {
 
 /**
  * @brief A set of cameras, all with their own calibration
+ * Assumes that a camera is laid out as 6 Pose3 parameters then calibration
  */
 template<class CAMERA>
 class CameraSet {
@@ -91,13 +90,13 @@ public:
    *  project, with derivatives in this, point, and calibration
    * throws CheiralityException
    */
-  std::vector<Z> project(const Point3& point, boost::optional<Matrix&> F,
-      boost::optional<Matrix&> E, boost::optional<Matrix&> H) const {
+  std::vector<Z> project(const Point3& point, boost::optional<Matrix&> F=boost::none,
+      boost::optional<Matrix&> E=boost::none, boost::optional<Matrix&> H=boost::none) const {
 
     size_t nrCameras = cameras_.size();
     if (F) F->resize(ZDim * nrCameras, 6);
     if (E) E->resize(ZDim * nrCameras, 3);
-    if (H) H->resize(ZDim * nrCameras, Dim - 6);
+    if (H && Dim>6) H->resize(ZDim * nrCameras, Dim - 6);
     std::vector<Z> z(nrCameras);
 
     for (size_t i = 0; i < cameras_.size(); i++) {
diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp
index 8c9d11532..c1ff5476f 100644
--- a/gtsam/geometry/tests/testCameraSet.cpp
+++ b/gtsam/geometry/tests/testCameraSet.cpp
@@ -26,6 +26,7 @@ using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
+// Cal3Bundler test
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 TEST(CameraSet, Pinhole) {
@@ -36,7 +37,14 @@ TEST(CameraSet, Pinhole) {
   set.add(camera);
   set.add(camera);
   Point3 p(0, 0, 1);
-  // Calculate actual
+
+  // Check measurements
+  Point2 expected;
+  ZZ z = set.project(p);
+  CHECK(assert_equal(expected, z[0]));
+  CHECK(assert_equal(expected, z[1]));
+
+  // Calculate expected derivatives using Pinhole
   Matrix46 actualF;
   Matrix43 actualE;
   Matrix43 actualH;
@@ -49,11 +57,10 @@ TEST(CameraSet, Pinhole) {
     actualF << F1, F1;
     actualH << H1, H1;
   }
-  Point2 expected;
+
+  // Check computed derivatives
   Matrix F, E, H;
-  ZZ z = set.project(p, F, E, H);
-  CHECK(assert_equal(expected, z[0]));
-  CHECK(assert_equal(expected, z[1]));
+  set.project(p, F, E, H);
   CHECK(assert_equal(actualF, F));
   CHECK(assert_equal(actualE, E));
   CHECK(assert_equal(actualH, H));
@@ -62,7 +69,36 @@ TEST(CameraSet, Pinhole) {
 /* ************************************************************************* */
 #include <gtsam/geometry/StereoCamera.h>
 TEST(CameraSet, Stereo) {
-  CameraSet<StereoCamera> f;
+  typedef vector<StereoPoint2> ZZ;
+  CameraSet<StereoCamera> set;
+  StereoCamera camera;
+  set.add(camera);
+  set.add(camera);
+  Point3 p(0, 0, 1);
+  EXPECT_LONGS_EQUAL(6,traits<StereoCamera>::dimension);
+
+  // Check measurements
+  StereoPoint2 expected;
+  ZZ z = set.project(p);
+  CHECK(assert_equal(expected, z[0]));
+  CHECK(assert_equal(expected, z[1]));
+
+  // Calculate expected derivatives using Pinhole
+  Matrix46 actualF;
+  Matrix43 actualE;
+  {
+    Matrix36 F1;
+    Matrix33 E1;
+    camera.project(p, F1, E1);
+    actualE << E1, E1;
+    actualF << F1, F1;
+  }
+
+  // Check computed derivatives
+  Matrix F, E;
+  set.project(p, F, E);
+  CHECK(assert_equal(actualF, F));
+  CHECK(assert_equal(actualE, E));
 }
 
 /* ************************************************************************* */