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

fix jacobians, calibration still buggy

release/4.3a0
jing 2014-03-10 16:27:41 -04:00
parent b43b84b59a
commit 4affb9a0d2
2 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/geometry/Cal3Unify.cpp
View File

@ -42,7 +42,7 @@ Vector Cal3Unify::vector() const {
void Cal3Unify::print(const std::string& s) const { void Cal3Unify::print(const std::string& s) const {
gtsam::print(K(), s + ".K"); gtsam::print(K(), s + ".K");
gtsam::print(Vector(k()), s + ".k"); gtsam::print(Vector(k()), s + ".k");
gtsam::print(Vector(xi_), s + ".xi"); gtsam::print((Vector)(Vector(1) << xi_), s + ".xi");
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -71,9 +71,9 @@ Point2 Cal3Unify::uncalibrate(const Point2& p,
// Part1: project 3D space to NPlane // Part1: project 3D space to NPlane
const double xs = p.x(), ys = p.y(); // normalized points in 3D space const double xs = p.x(), ys = p.y(); // normalized points in 3D space
const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0); const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0);
const double xi_sqrt_nx = 1 + xi * sqrt_nx; const double xi_sqrt_nx = 1.0 / (1 + xi * sqrt_nx);
const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx; const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx;
const double x = xs / xi_sqrt_nx, y = ys / xi_sqrt_nx; // points on NPlane const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane
// Part2: project NPlane point to pixel plane: same as Cal3DS2 // Part2: project NPlane point to pixel plane: same as Cal3DS2
const double xy = x * y, xx = x * x, yy = y * y; const double xy = x * y, xx = x * x, yy = y * y;
@ -110,8 +110,8 @@ Point2 Cal3Unify::uncalibrate(const Point2& p,
// Inlined derivative for calibration // Inlined derivative for calibration
if (H1) { if (H1) {
// part1 // part1
Matrix DU = (Matrix(2,1) << xs * sqrt_nx / xi_sqrt_nx2, Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2,
ys * sqrt_nx / xi_sqrt_nx2); -ys * sqrt_nx * xi_sqrt_nx2);
Matrix DDS2U = DDS2 * DU; Matrix DDS2U = DDS2 * DU;
// part2 // part2
Matrix DDS2V = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr, Matrix DDS2V = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
@ -124,10 +124,11 @@ Point2 Cal3Unify::uncalibrate(const Point2& p,
// Inlined derivative for points // Inlined derivative for points
if (H2) { if (H2) {
// part1 // part1
Matrix DU = (Matrix(2, 2) << (xi_sqrt_nx - xs * xs / sqrt_nx) / xi_sqrt_nx2, const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
-(ys * ys / (sqrt_nx * xi_sqrt_nx2)), const double mid = -(xi * xs*ys) * denom;
-(xs * xs / (sqrt_nx * xi_sqrt_nx2)), Matrix DU = (Matrix(2, 2) <<
(xi_sqrt_nx - ys * ys / sqrt_nx) / xi_sqrt_nx2); (sqrt_nx + xi*(ys*ys + 1)) * denom, mid,
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom);
*H2 = DDS2 * DU; *H2 = DDS2 * DU;
} }

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

@ -35,7 +35,7 @@ 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.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 Point2 p(2, 3);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Cal3Unify, uncalibrate) TEST( Cal3Unify, uncalibrate)
@ -68,7 +68,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-5)); CHECK(assert_equal(numerical,computed,1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -77,7 +77,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-5)); CHECK(assert_equal(numerical,computed,1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */