Easy constructor

cpp/Cal3_S2.h

@@ -8,6 +8,7 @@
 
 #include "Matrix.h"
 #include "Point2.h"
+#include <math.h>
 
 namespace gtsam {
 
@@ -31,6 +32,19 @@ namespace gtsam {
 			fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
 		}
 
+		/**
+		 * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
+		 * @param fov field of view in degrees
+		 * @param w image width
+		 * @param h image height
+		 */
+		Cal3_S2(double fov, int w, int h) :
+			s_(0), u0_((double)w/2.0), v0_((double)h/2.0) {
+			double a = fov*M_PI/360.0; // fov/2 in radians
+			fx_=(double)w*tan(a);
+			fy_=fx_;
+		}
+
 		void print(const std::string& s = "") const {
 			gtsam::print(matrix(), s);
 		}

cpp/testCal3_S2.cpp

@@ -11,16 +11,25 @@ using namespace gtsam;
 
 Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
 Point2 p(1, -2);
-double tol = 1e-8;
 
-/** transform derivatives */
+/* ************************************************************************* */
+TEST( Cal3_S2, easy_constructor)
+{
+	Cal3_S2 expected(369.504, 369.504, 0, 640 / 2, 480 / 2);
+
+	double fov = 60; // degrees
+	size_t w=640,h=480;
+	Cal3_S2 actual(fov,w,h);
+
+	CHECK(assert_equal(expected,actual,1e-3));
+}
 
 /* ************************************************************************* */
 TEST( Cal3_S2, Duncalibrate1)
 {
 	Matrix computed = Duncalibrate1(K, p);
 	Matrix numerical = numericalDerivative21(uncalibrate, K, p);
-	CHECK(assert_equal(numerical,computed,tol));
+	CHECK(assert_equal(numerical,computed,1e-8));
 }
 
 /* ************************************************************************* */
@@ -28,16 +37,16 @@ TEST( Cal3_S2, Duncalibrate2)
 {
 	Matrix computed = Duncalibrate2(K, p);
 	Matrix numerical = numericalDerivative22(uncalibrate, K, p);
-	CHECK(assert_equal(numerical,computed,tol));
+	CHECK(assert_equal(numerical,computed,1e-9));
 }
 
 /* ************************************************************************* */
 TEST( Cal3_S2, assert_equal)
 {
-	CHECK(assert_equal(K,K,tol));
+	CHECK(assert_equal(K,K,1e-9));
 
-	Cal3_S2 K1(501, 500, 0.1, 640 / 2, 480 / 2);
+	Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2);
-	CHECK(!K.equals(K1,tol));
+	CHECK(assert_equal(K,K1,1e-9));
 }
 
 /* ************************************************************************* */