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

calibrate() done by using base class

release/4.3a0
jing 2014-03-27 15:09:26 -04:00
parent 3f5bbe3fd2
commit 77f494b341
2 changed files with 8 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
gtsam/geometry/Cal3Unify.cpp
View File

@ -104,41 +104,12 @@ Point2 Cal3Unify::uncalibrate(const Point2& p,
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion.
// pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
// point on the normalized plane, input for DS2 // calibrate point to Nplane use base class::calibrate()
double px = pi.x(); Point2 pnplane = Base::calibrate(pi, tol);
double py = pi.y();
const Point2 invKPi ((1 / fx_) * (px - u0_ - (s_ / fy_) * (py - v0_)),
(1 / fy_) * (py - v0_));
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary // call nplane to space
Point2 pn = nPlaneToSpace(invKPi); return this->nPlaneToSpace(pnplane);
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 20;
int iteration;
for ( iteration = 0; iteration < maxIterations; ++iteration ) {
if ( uncalibrate(pn).distance(pi) <= tol ) break;
// part1: 3D space -> normalized plane
Point2 pnpl = spaceToNPlane(pn);
// part2: normalized plane -> 3D space
px = pnpl.x(), py = pnpl.y();
const double xy = px*py, xx = px*px, yy = py*py;
const double rr = xx + yy;
const double g = (1+k1_*rr+k2_*rr*rr);
const double dx = 2*k3_*xy + k4_*(rr+2*xx);
const double dy = 2*k4_*xy + k3_*(rr+2*yy);
pn = nPlaneToSpace((invKPi - Point2(dx,dy))/g);
}
if ( iteration >= maxIterations )
throw std::runtime_error("Cal3Unify::calibrate fails to converge. need a better initialization");
return pn;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const { Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const {

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

@ -56,7 +56,7 @@ TEST( Cal3Unify, calibrate)
{ {
Point2 pi = K.uncalibrate(p); Point2 pi = K.uncalibrate(p);
Point2 pn_hat = K.calibrate(pi); Point2 pn_hat = K.calibrate(pi);
CHECK( p.equals(pn_hat, 1e-5)); CHECK( p.equals(pn_hat, 1e-8));
} }
Point2 uncalibrate_(const Cal3Unify& k, const Point2& pt) { return k.uncalibrate(pt); } Point2 uncalibrate_(const Cal3Unify& k, const Point2& pt) { return k.uncalibrate(pt); }
@ -82,7 +82,7 @@ TEST( Cal3Unify, Duncalibrate2)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Cal3Unify, assert_equal) TEST( Cal3Unify, assert_equal)
{ {
CHECK(assert_equal(K,K,1e-5)); CHECK(assert_equal(K,K,1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -93,8 +93,8 @@ TEST( Cal3Unify, retract)
Vector d(10); Vector d(10);
d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1;
Cal3Unify actual = K.retract(d); Cal3Unify actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7)); CHECK(assert_equal(expected,actual,1e-9));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); CHECK(assert_equal(d,K.localCoordinates(actual),1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */