diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp
index 1fb7f4069..c8c0255ea 100644
--- a/gtsam/geometry/Cal3_S2.cpp
+++ b/gtsam/geometry/Cal3_S2.cpp
@@ -89,6 +89,11 @@ Point2 Cal3_S2::calibrate(const Point2& p) const {
       (1 / fy_) * (v - v0_));
 }
 
+/* ************************************************************************* */
+Vector3 Cal3_S2::calibrate(const Vector3& p) const {
+  return matrix_inverse() * p;
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index bb3327afc..a62266046 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -158,6 +158,13 @@ public:
    */
   Point2 calibrate(const Point2& p) const;
 
+  /**
+   * convert homogeneous image coordinates to intrinsic coordinates
+   * @param p point in image coordinates
+   * @return point in intrinsic coordinates
+   */
+  Vector3 calibrate(const Vector3& p) const;
+
   /// @}
   /// @name Manifold
   /// @{
diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp
index fe15afa0c..e1dba2272 100644
--- a/gtsam/geometry/tests/testCal3_S2.cpp
+++ b/gtsam/geometry/tests/testCal3_S2.cpp
@@ -42,12 +42,18 @@ TEST( Cal3_S2, easy_constructor)
 /* ************************************************************************* */
 TEST( Cal3_S2, calibrate)
 {
-  Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2);
   Point2 intrinsic(2,3);
   Point2 expectedimage(1320.3, 1740);
-  Point2 imagecoordinates = K1.uncalibrate(intrinsic);
+  Point2 imagecoordinates = K.uncalibrate(intrinsic);
   CHECK(assert_equal(expectedimage,imagecoordinates));
-  CHECK(assert_equal(intrinsic,K1.calibrate(imagecoordinates)));
+  CHECK(assert_equal(intrinsic,K.calibrate(imagecoordinates)));
+}
+
+/* ************************************************************************* */
+TEST( Cal3_S2, calibrate_homogeneous) {
+  Vector3 intrinsic(2, 3, 1);
+  Vector3 image(1320.3, 1740, 1);
+  CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image)));
 }
 
 /* ************************************************************************* */