12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Easy constructor

release/4.3a0
Frank Dellaert 2009-11-09 04:46:08 +00:00
parent dd1b023ca9
commit e340178de5
2 changed files with 30 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cpp/Cal3_S2.h
View File

@ -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);
}

23
cpp/testCal3_S2.cpp
View File

@ -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));
}
/* ************************************************************************* */