Two static functions save on copy/paste
dellaert
parent
6ecc32a311
commit
0e64c9495e
|
|
@ -52,6 +52,42 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
|
|||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Matrix D2dcalibration(double x, double y, double xx, double yy,
|
||||
double xy, double rr, double r4, double fx, double fy, double s, double pnx,
|
||||
double pny) {
|
||||
return (Matrix(2, 9) << //
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Matrix D2dintrinsic(double x, double y, double rr, double r4, double fx,
|
||||
double fy, double s, double k1, double k2, double p1, double p2) {
|
||||
const double drdx = 2 * x;
|
||||
const double drdy = 2 * y;
|
||||
const double g = 1 + k1 * rr + k2 * r4;
|
||||
const double dgdx = k1 * drdx + k2 * 2 * rr * drdx;
|
||||
const double dgdy = k1 * drdy + k2 * 2 * rr * drdy;
|
||||
|
||||
// Dx = 2*p1*xy + p2*(rr+2*xx);
|
||||
// Dy = 2*p2*xy + p1*(rr+2*yy);
|
||||
const double dDxdx = 2 * p1 * y + p2 * (drdx + 4 * x);
|
||||
const double dDxdy = 2 * p1 * x + p2 * drdy;
|
||||
const double dDydx = 2 * p2 * y + p1 * drdx;
|
||||
const double dDydy = 2 * p2 * x + p1 * (drdy + 4 * y);
|
||||
|
||||
Matrix DK = (Matrix(2, 2) << fx, s, 0.0, fy);
|
||||
Matrix DR = (Matrix(2, 2) << g + x * dgdx + dDxdx, x * dgdy + dDxdy, y * dgdx
|
||||
+ dDydx, g + y * dgdy + dDydy);
|
||||
|
||||
return DK * DR;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3DS2::uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> H1,
|
||||
|
|
@ -74,31 +110,13 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
|
|||
const double pnx = g*x + dx;
|
||||
const double pny = g*y + dy;
|
||||
|
||||
// Inlined derivative for calibration
|
||||
if (H1) {
|
||||
*H1 = (Matrix(2, 9) << 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));
|
||||
}
|
||||
// Inlined derivative for points
|
||||
if (H2) {
|
||||
const double dr_dx = 2. * x;
|
||||
const double dr_dy = 2. * y;
|
||||
const double dg_dx = k1_ * dr_dx + k2_ * 2. * rr * dr_dx;
|
||||
const double dg_dy = k1_ * dr_dy + k2_ * 2. * rr * dr_dy;
|
||||
// Derivative for calibration
|
||||
if (H1)
|
||||
*H1 = D2dcalibration(x,y,xx,yy,xy,rr,r4,fx_,fy_,s_,pnx,pny);
|
||||
|
||||
const double dDx_dx = 2. * p1_ * y + p2_ * (dr_dx + 4. * x);
|
||||
const double dDx_dy = 2. * p1_ * x + p2_ * dr_dy;
|
||||
const double dDy_dx = 2. * p2_ * y + p1_ * dr_dx;
|
||||
const double dDy_dy = 2. * p2_ * x + p1_ * (dr_dy + 4. * y);
|
||||
|
||||
Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_);
|
||||
Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
|
||||
y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy);
|
||||
|
||||
*H2 = DK * DR;
|
||||
}
|
||||
// Derivative for points
|
||||
if (H2)
|
||||
*H2 = D2dintrinsic(x, y, rr, r4, fx_, fy_, s_, k1_, k2_, p1_, p2_);
|
||||
|
||||
// Regular uncalibrate after distortion
|
||||
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
|
||||
|
|
@ -118,14 +136,14 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
|
|||
// iterate until the uncalibrate is close to the actual pixel coordinate
|
||||
const int maxIterations = 10;
|
||||
int iteration;
|
||||
for ( iteration = 0; iteration < maxIterations; ++iteration ) {
|
||||
if ( uncalibrate(pn).distance(pi) <= tol ) break;
|
||||
const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y;
|
||||
for (iteration = 0; iteration < maxIterations; ++iteration) {
|
||||
if (uncalibrate(pn).distance(pi) <= tol) break;
|
||||
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
|
||||
const double rr = xx + yy;
|
||||
const double g = (1+k1_*rr+k2_*rr*rr);
|
||||
const double dx = 2*p1_*xy + p2_*(rr+2*xx);
|
||||
const double dy = 2*p2_*xy + p1_*(rr+2*yy);
|
||||
pn = (invKPi - Point2(dx,dy))/g;
|
||||
const double g = (1 + k1_ * rr + k2_ * rr * rr);
|
||||
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
|
||||
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
|
||||
pn = (invKPi - Point2(dx, dy)) / g;
|
||||
}
|
||||
|
||||
if ( iteration >= maxIterations )
|
||||
|
|
@ -136,43 +154,22 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
|
||||
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
|
||||
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
|
||||
const double rr = xx + yy;
|
||||
const double dr_dx = 2*x;
|
||||
const double dr_dy = 2*y;
|
||||
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*p1_*xy + p2_*(rr+2*xx);
|
||||
// Dy = 2*p2_*xy + p1_*(rr+2*yy);
|
||||
const double dDx_dx = 2*p1_*y + p2_*(dr_dx + 4*x);
|
||||
const double dDx_dy = 2*p1_*x + p2_*dr_dy;
|
||||
const double dDy_dx = 2*p2_*y + p1_*dr_dx;
|
||||
const double dDy_dy = 2*p2_*x + p1_*(dr_dy + 4*y);
|
||||
|
||||
Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_);
|
||||
Matrix DR = (Matrix(2, 2) << g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy,
|
||||
y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy);
|
||||
|
||||
return DK * DR;
|
||||
return D2dintrinsic(x, y, rr, rr * rr, fx_, fy_, s_, k1_, k2_, p1_, p2_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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 x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
|
||||
const double rr = xx + yy;
|
||||
const double r4 = rr*rr;
|
||||
const double g = (1+k1_*rr+k2_*r4);
|
||||
const double dx = 2*p1_*xy + p2_*(rr+2*xx);
|
||||
const double dy = 2*p2_*xy + p1_*(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*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) );
|
||||
const double r4 = rr * rr;
|
||||
const double g = (1 + k1_ * rr + k2_ * r4);
|
||||
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
|
||||
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
|
||||
const double pnx = g * x + dx;
|
||||
const double pny = g * y + dy;
|
||||
return D2dcalibration(x, y, xx, yy, xy, rr, r4, fx_, fy_, s_, pnx, pny);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -60,6 +60,8 @@ TEST( Cal3DS2, Duncalibrate1)
|
|||
K.uncalibrate(p, computed, boost::none);
|
||||
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
|
||||
CHECK(assert_equal(numerical,computed,1e-5));
|
||||
Matrix separate = K.D2d_calibration(p);
|
||||
CHECK(assert_equal(numerical,separate,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -68,6 +70,8 @@ TEST( Cal3DS2, Duncalibrate2)
|
|||
Matrix computed; K.uncalibrate(p, boost::none, computed);
|
||||
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
|
||||
CHECK(assert_equal(numerical,computed,1e-5));
|
||||
Matrix separate = K.D2d_intrinsic(p);
|
||||
CHECK(assert_equal(numerical,separate,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue