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

A small saving

release/4.3a0
dellaert 2014-06-19 11:58:08 -04:00
parent 0e64c9495e
commit 12897a2c29
1 changed files with 21 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
gtsam/geometry/Cal3DS2.cpp
View File

@ -66,32 +66,31 @@ static Matrix D2dcalibration(double x, double y, double xx, double yy,
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Matrix D2dintrinsic(double x, double y, double rr, double r4, double fx, static Matrix D2dintrinsic(double x, double y, double rr, double g, double fx,
double fy, double s, double k1, double k2, double p1, double p2) { double fy, double s, double k1, double k2, double p1, double p2) {
const double drdx = 2 * x; const double drdx = 2. * x;
const double drdy = 2 * y; const double drdy = 2. * y;
const double g = 1 + k1 * rr + k2 * r4; const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
const double dgdx = k1 * drdx + k2 * 2 * rr * drdx; const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
const double dgdy = k1 * drdy + k2 * 2 * rr * drdy;
// Dx = 2*p1*xy + p2*(rr+2*xx); // Dx = 2*p1*xy + p2*(rr+2*xx);
// Dy = 2*p2*xy + p1*(rr+2*yy); // Dy = 2*p2*xy + p1*(rr+2*yy);
const double dDxdx = 2 * p1 * y + p2 * (drdx + 4 * x); const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
const double dDxdy = 2 * p1 * x + p2 * drdy; const double dDxdy = 2. * p1 * x + p2 * drdy;
const double dDydx = 2 * p2 * y + p1 * drdx; const double dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2 * p2 * x + p1 * (drdy + 4 * y); const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Matrix DK = (Matrix(2, 2) << fx, s, 0.0, fy); Matrix DK = (Matrix(2, 2) << fx, s, 0.0, fy);
Matrix DR = (Matrix(2, 2) << g + x * dgdx + dDxdx, x * dgdy + dDxdy, y * dgdx Matrix DR = (Matrix(2, 2) << //
+ dDydx, g + y * dgdy + dDydy); g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy);
return DK * DR; return DK * DR;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3DS2::uncalibrate(const Point2& p, Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
boost::optional<Matrix&> H2) const {
// rr = x^2 + y^2; // rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2); // g = (1 + k(1)*rr + k(2)*rr^2);
@ -107,16 +106,16 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy); const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
// Radial and tangential distortion applied // Radial and tangential distortion applied
const double pnx = g*x + dx; const double pnx = g * x + dx;
const double pny = g*y + dy; const double pny = g * y + dy;
// Derivative for calibration // Derivative for calibration
if (H1) if (H1)
*H1 = D2dcalibration(x,y,xx,yy,xy,rr,r4,fx_,fy_,s_,pnx,pny); *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, fx_, fy_, s_, pnx, pny);
// Derivative for points // Derivative for points
if (H2) if (H2)
*H2 = D2dintrinsic(x, y, rr, r4, fx_, fy_, s_, k1_, k2_, p1_, p2_); *H2 = D2dintrinsic(x, y, rr, g, fx_, fy_, s_, k1_, k2_, p1_, p2_);
// Regular uncalibrate after distortion // Regular uncalibrate after distortion
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
@ -156,7 +155,9 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
Matrix Cal3DS2::D2d_intrinsic(const Point2& p) 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 rr = xx + yy;
return D2dintrinsic(x, y, rr, rr * rr, fx_, fy_, s_, k1_, k2_, p1_, p2_); const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
return D2dintrinsic(x, y, rr, g, fx_, fy_, s_, k1_, k2_, p1_, p2_);
} }
/* ************************************************************************* */ /* ************************************************************************* */