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);
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 100;
const int maxIterations = 20;
int iteration;
for ( iteration = 0; iteration < maxIterations; ++iteration ) {
pn.print();
if ( pn.distance(pi) <= tol ) break;
if ( uncalibrate(pn).distance(pi) <= tol ) break;
// part1: 3D space -> normalized plane
Point2 pnpl = spaceToNPlane(pn);

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

@ -19,28 +19,26 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Unify.h>
#include <iostream>
using namespace gtsam;
using namespace std;
GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify)
/*
ground truth 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];
X = [0.5 0.7 1]';
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);
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);
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(0.5, 0.7);
/* ************************************************************************* */
TEST( Cal3Unify, uncalibrate)
{
Point2 p_i(582.5228344366194, 649.6685266099726) ;
Point2 p_i(364.7791831734982, 305.6677211952602) ;
Point2 q = K.uncalibrate(p);
CHECK(assert_equal(q,p_i));
}
@ -49,6 +47,7 @@ TEST( Cal3Unify, uncalibrate)
TEST( Cal3Unify, spaceNplane)
{
Point2 q = K.spaceToNPlane(p);
CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
CHECK(assert_equal(p, K.nPlaneToSpace(q)));
}
@ -56,7 +55,7 @@ TEST( Cal3Unify, spaceNplane)
TEST( Cal3Unify, calibrate)
{
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));
}
@ -68,7 +67,7 @@ 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-6));
CHECK(assert_equal(numerical,computed,1e-5));
}
/* ************************************************************************* */
@ -77,7 +76,7 @@ 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-6));
CHECK(assert_equal(numerical,computed,1e-5));
}
/* ************************************************************************* */
@ -89,7 +88,7 @@ TEST( Cal3Unify, assert_equal)
/* ************************************************************************* */
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);
Vector d(10);
d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;