diff --git a/cpp/Cal3_S2.h b/cpp/Cal3_S2.h index 956a90eb0..54aa51ab0 100644 --- a/cpp/Cal3_S2.h +++ b/cpp/Cal3_S2.h @@ -8,6 +8,7 @@ #include "Matrix.h" #include "Point2.h" +#include 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); } diff --git a/cpp/testCal3_S2.cpp b/cpp/testCal3_S2.cpp index 1cd1d14ce..169a28b9b 100644 --- a/cpp/testCal3_S2.cpp +++ b/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); - CHECK(!K.equals(K1,tol)); + Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2); + CHECK(assert_equal(K,K1,1e-9)); } /* ************************************************************************* */