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

all unit test failure fixed

release/4.3a0
jing 2014-03-10 17:09:56 -04:00
parent 4affb9a0d2
commit 7ff7482792
2 changed files with 13 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/Cal3Unify.cpp
View File

@ -152,12 +152,11 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const {
Point2 pn = nPlaneToSpace(invKPi); Point2 pn = nPlaneToSpace(invKPi);
// iterate until the uncalibrate is close to the actual pixel coordinate // iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 100; const int maxIterations = 20;
int iteration; int iteration;
for ( iteration = 0; iteration < maxIterations; ++iteration ) { for ( iteration = 0; iteration < maxIterations; ++iteration ) {
pn.print(); if ( uncalibrate(pn).distance(pi) <= tol ) break;
if ( pn.distance(pi) <= tol ) break;
// part1: 3D space -> normalized plane // part1: 3D space -> normalized plane
Point2 pnpl = spaceToNPlane(pn); Point2 pnpl = spaceToNPlane(pn);

23
gtsam/geometry/tests/testCal3Unify.cpp
View File

@ -19,28 +19,26 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Unify.h> #include <gtsam/geometry/Cal3Unify.h>
#include <iostream>
using namespace gtsam; using namespace gtsam;
using namespace std;
GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify) GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify) GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify)
/* /*
ground truth from matlab, code : ground truth from matlab, code :
X = [2 3 1]'; X = [0.5 0.7 1]';
V = [0.01, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; V = [0.1, 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); [P, J] = spaceToImgPlane(X, V);
matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox 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 Cal3Unify K(0.1, 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); static Point2 p(0.5, 0.7);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Cal3Unify, uncalibrate) TEST( Cal3Unify, uncalibrate)
{ {
Point2 p_i(582.5228344366194, 649.6685266099726) ; Point2 p_i(364.7791831734982, 305.6677211952602) ;
Point2 q = K.uncalibrate(p); Point2 q = K.uncalibrate(p);
CHECK(assert_equal(q,p_i)); CHECK(assert_equal(q,p_i));
} }
@ -49,6 +47,7 @@ TEST( Cal3Unify, uncalibrate)
TEST( Cal3Unify, spaceNplane) TEST( Cal3Unify, spaceNplane)
{ {
Point2 q = K.spaceToNPlane(p); Point2 q = K.spaceToNPlane(p);
CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
CHECK(assert_equal(p, K.nPlaneToSpace(q))); CHECK(assert_equal(p, K.nPlaneToSpace(q)));
} }
@ -56,7 +55,7 @@ TEST( Cal3Unify, spaceNplane)
TEST( Cal3Unify, calibrate) TEST( Cal3Unify, calibrate)
{ {
Point2 pi = K.uncalibrate(p); Point2 pi = K.uncalibrate(p);
Point2 pn_hat = K.calibrate(pi, 1); Point2 pn_hat = K.calibrate(pi);
CHECK( p.equals(pn_hat, 1e-5)); CHECK( p.equals(pn_hat, 1e-5));
} }
@ -68,7 +67,7 @@ TEST( Cal3Unify, Duncalibrate1)
Matrix computed; Matrix computed;
K.uncalibrate(p, computed, boost::none); K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-6)); CHECK(assert_equal(numerical,computed,1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -77,7 +76,7 @@ TEST( Cal3Unify, Duncalibrate2)
Matrix computed; Matrix computed;
K.uncalibrate(p, boost::none, computed); K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-6)); CHECK(assert_equal(numerical,computed,1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -89,7 +88,7 @@ TEST( Cal3Unify, assert_equal)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Cal3Unify, retract) TEST( Cal3Unify, retract)
{ {
Cal3Unify expected(0.01 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, Cal3Unify expected(0.1 + 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); 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10);
Vector d(10); Vector d(10);
d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;