diff --git a/cpp/Cal3_S2.h b/cpp/Cal3_S2.h
index ba54f9470..9db7f46d5 100644
--- a/cpp/Cal3_S2.h
+++ b/cpp/Cal3_S2.h
@@ -91,6 +91,14 @@ namespace gtsam {
 			return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
 		}
 
+		/**
+		 * convert image coordinates uv to intrinsic coordinates xy
+		 */
+		Point2 calibrate(const Point2& p) const {
+			const double u = p.x(), v = p.y();
+			return Point2((1/fx_)*(u-u0_ - (s_/fy_)*(v-v0_)), (1/fy_)*(v-v0_));
+		}
+
 		/** friends */
 		friend Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p);
 		friend Cal3_S2 expmap(const Cal3_S2& cal, const Vector& d);
diff --git a/cpp/testCal3_S2.cpp b/cpp/testCal3_S2.cpp
index 169a28b9b..d27b03bd9 100644
--- a/cpp/testCal3_S2.cpp
+++ b/cpp/testCal3_S2.cpp
@@ -24,6 +24,17 @@ TEST( Cal3_S2, easy_constructor)
 	CHECK(assert_equal(expected,actual,1e-3));
 }
 
+/* ************************************************************************* */
+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);
+	CHECK(assert_equal(expectedimage,imagecoordinates));
+	CHECK(assert_equal(intrinsic,K1.calibrate(imagecoordinates)));
+}
+
 /* ************************************************************************* */
 TEST( Cal3_S2, Duncalibrate1)
 {