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

add unit test

release/4.3a0
jing 2014-03-09 01:18:06 -05:00
parent 1e76082888
commit e311e993ad
1 changed files with 91 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
gtsam/geometry/tests/testCal3Unify.cpp Normal file
View File

@ -0,0 +1,91 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testCal3Unify.cpp
* @brief Unit tests for transform derivatives
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Unify.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify)
/* ground truth data will from matlab, code :
X = [2 3 1]';
V = [0.01, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240];
[P, J] = spaceToImgPlane(X, V);
matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */
static Cal3Unify K(0.01, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
static Point2 p(2,3);
/* ************************************************************************* */
TEST( Cal3Unify, uncalibrate)
{
Point2 p_i(582.5228344366194, 649.6685266099726) ;
Point2 q = K.uncalibrate(p);
CHECK(assert_equal(q,p_i));
}
TEST( Cal3Unify, calibrate )
{
Point2 pi = K.uncalibrate(p);
Point2 pn_hat = K.calibrate(pi);
CHECK( p.equals(pn_hat, 1e-5));
}
Point2 uncalibrate_(const Cal3Unify& k, const Point2& pt) { return k.uncalibrate(pt); }
/* ************************************************************************* */
TEST( Cal3Unify, Duncalibrate1)
{
Matrix computed;
K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-5));
}
/* ************************************************************************* */
TEST( Cal3Unify, Duncalibrate2)
{
Matrix computed;
K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-5));
}
/* ************************************************************************* */
TEST( Cal3Unify, assert_equal)
{
CHECK(assert_equal(K,K,1e-5));
}
/* ************************************************************************* */
TEST( Cal3Unify, retract)
{
Cal3Unify expected(0.01 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10);
Vector d(10);
d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
Cal3Unify actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */