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

Use rr instead of r for squared radius in undistort

release/4.3a0
Frank Dellaert 2013-05-20 22:23:50 +00:00
parent d05560687b
commit 314b854ecb
2 changed files with 28 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam/geometry/Cal3DS2.cpp
View File

@ -49,15 +49,15 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
Point2 Cal3DS2::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// r = x^2 + y^2 ;
// g = (1 + k(1)*r + k(2)*r^2) ;
// dp = [2*k(3)*x*y + k(4)*(r + 2*x^2) ; 2*k(4)*x*y + k(3)*(r + 2*y^2)] ;
// rr = x^2 + y^2 ;
// g = (1 + k(1)*rr + k(2)*rr^2) ;
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2) ; 2*k(4)*x*y + k(3)*(rr + 2*y^2)] ;
// pi(:,i) = g * pn(:,i) + dp ;
const double x = p.x(), y = p.y(), xy = x*y, xx = x*x, yy = y*y ;
const double r = xx + yy ;
const double g = (1+k1_*r+k2_*r*r) ;
const double dx = 2*k3_*xy + k4_*(r+2*xx) ;
const double dy = 2*k4_*xy + k3_*(r+2*yy) ;
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) ;
const double x2 = g*x + dx ;
const double y2 = g*y + dy ;
@ -85,10 +85,10 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
for ( iteration = 0 ; iteration < maxIterations ; ++iteration ) {
if ( uncalibrate(pn).dist(pi) <= tol ) break;
const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y ;
const double r = xx + yy ;
const double g = (1+k1_*r+k2_*r*r) ;
const double dx = 2*k3_*xy + k4_*(r+2*xx) ;
const double dy = 2*k4_*xy + k3_*(r+2*yy) ;
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 = (invKPi - Point2(dx,dy))/g ;
}
@ -104,16 +104,16 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
//const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y ;
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ;
const double r = xx + yy ;
const double rr = xx + yy ;
const double dr_dx = 2*x ;
const double dr_dy = 2*y ;
const double r2 = r*r ;
const double g = 1 + k1*r + k2*r2 ;
const double dg_dx = k1*dr_dx + k2*2*r*dr_dx ;
const double dg_dy = k1*dr_dy + k2*2*r*dr_dy ;
const double r4 = rr*rr ;
const double g = 1 + k1*rr + k2*r4 ;
const double dg_dx = k1*dr_dx + k2*2*rr*dr_dx ;
const double dg_dy = k1*dr_dy + k2*2*rr*dr_dy ;
// Dx = 2*k3*xy + k4*(r+2*xx) ;
// Dy = 2*k4*xy + k3*(r+2*yy) ;
// Dx = 2*k3*xy + k4*(rr+2*xx) ;
// Dy = 2*k4*xy + k3*(rr+2*yy) ;
const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x) ;
const double dDx_dy = 2*k3*x + k4*dr_dy ;
const double dDy_dx = 2*k4*y + k3*dr_dx ;
@ -129,18 +129,18 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
/* ************************************************************************* */
Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y ;
const double r = xx + yy ;
const double r2 = r*r ;
const double rr = xx + yy ;
const double r4 = rr*rr ;
const double fx = fx_, fy = fy_, s = s_ ;
const double g = (1+k1_*r+k2_*r2) ;
const double dx = 2*k3_*xy + k4_*(r+2*xx) ;
const double dy = 2*k4_*xy + k3_*(r+2*yy) ;
const double g = (1+k1_*rr+k2_*r4) ;
const double dx = 2*k3_*xy + k4_*(rr+2*xx) ;
const double dy = 2*k4_*xy + k3_*(rr+2*yy) ;
const double pnx = g*x + dx;
const double pny = g*y + dy;
return Matrix_(2, 9,
pnx, 0.0, pny, 1.0, 0.0, fx*x*r + s*y*r, fx*x*r2 + s*y*r2, fx*2*xy + s*(r+2*yy), fx*(r+2*xx) + s*(2*xy),
0.0, pny, 0.0, 0.0, 1.0, fy*y*r , fy*y*r2 , fy*(r+2*yy) , fy*(2*xy) );
pnx, 0.0, pny, 1.0, 0.0, fx*x*rr + s*y*rr, fx*x*r4 + s*y*r4, fx*2*xy + s*(rr+2*yy), fx*(rr+2*xx) + s*(2*xy),
0.0, pny, 0.0, 0.0, 1.0, fy*y*rr , fy*y*r4 , fy*(rr+2*yy) , fy*(2*xy) );
}
/* ************************************************************************* */

6
gtsam/geometry/Cal3DS2.h
View File

@ -37,9 +37,9 @@ private:
double k3_, k4_ ; // tangential distortion
// K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
// r = Pn.x^2 + Pn.y^2
// \hat{pn} = (1 + k1*r + k2*r^2 ) pn + [ 2*k3 pn.x pn.y + k4 (r + 2 Pn.x^2) ;
// k3 (r + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
// rr = Pn.x^2 + Pn.y^2
// \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
// k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
// pi = K*pn
public: