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

feature: add test to Cal3_S2 calibrate jacobian

release/4.3a0
lvzhaoyang 2015-06-23 01:59:32 -04:00
parent f7bf418c45
commit d8b9cae25d
1 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -26,6 +26,8 @@ GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2)
static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2); static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
static Point2 p(1, -2); static Point2 p(1, -2);
static Point2 p_uv(1320.3, 1740);
static Point2 p_xy(2, 3);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Cal3_S2, easy_constructor) TEST( Cal3_S2, easy_constructor)
@ -73,6 +75,33 @@ TEST( Cal3_S2, Duncalibrate2)
CHECK(assert_equal(numerical,computed,1e-9)); CHECK(assert_equal(numerical,computed,1e-9));
} }
Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {return k.calibrate(pt); }
/* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate1)
{
Matrix computed;
Point2 expected = K.calibrate(p_uv, computed, boost::none);
Matrix numerical = numericalDerivative21(calibrate_, K, p);
CHECK(assert_equal(numerical, computed, 1e-8));
CHECK(assert_equal(expected, p_xy, 1e-8));
}
/* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate1)
{
Matrix computed;
Point2 expected = K.calibrate(p_uv, boost::none, computed);
Matrix numerical = numericalDerivative22(calibrate_, K, p);
CHECK(assert_equal(numerical, computed, 1e-8));
CHECK(assert_equal(expected, p_xy, 1e-8));
}
/* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate2)
{
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Cal3_S2, assert_equal) TEST( Cal3_S2, assert_equal)
{ {