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

Separated out DK component

release/4.3a0
dellaert 2014-06-19 12:19:02 -04:00
parent 12897a2c29
commit 8ce541f05f
1 changed files with 28 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/geometry/Cal3DS2.cpp
View File

@ -53,21 +53,23 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Matrix D2dcalibration(double x, double y, double xx, double yy, static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
double xy, double rr, double r4, double fx, double fy, double s, double pnx, double yy, double xy, double rr, double r4, double pnx, double pny,
double pny) { const Eigen::Matrix<double, 2, 2>& DK) {
return (Matrix(2, 9) << // Eigen::Matrix<double, 2, 5> DR1;
pnx, 0.0, pny, 1.0, 0.0, // DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
fx * x * rr + s * y * rr, fx * x * r4 + s * y * r4, // Eigen::Matrix<double, 2, 4> DR2;
fx * 2 * xy + s * (rr + 2 * yy), fx * (rr + 2 * xx) + s * (2 * xy), // DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
0.0, pny, 0.0, 0.0, 1.0, // y * rr, y * r4, rr + 2 * yy, 2 * xy;
fy * y * rr, fy * y * r4, // Eigen::Matrix<double, 2, 9> D;
fy * (rr + 2 * yy), fy * (2 * xy)); D << DR1, DK * DR2;
return D;
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Matrix D2dintrinsic(double x, double y, double rr, double g, double fx, static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
double fy, double s, double k1, double k2, double p1, double p2) { double g, double k1, double k2, double p1, double p2,
const Eigen::Matrix<double, 2, 2>& DK) {
const double drdx = 2. * x; const double drdx = 2. * x;
const double drdy = 2. * y; const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@ -80,10 +82,9 @@ static Matrix D2dintrinsic(double x, double y, double rr, double g, double fx,
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); Eigen::Matrix<double, 2, 2> DR;
Matrix DR = (Matrix(2, 2) << // DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
g + x * dgdx + dDxdx, x * dgdy + dDxdy, // y * dgdx + dDydx, g + y * dgdy + dDydy;
y * dgdx + dDydx, g + y * dgdy + dDydy);
return DK * DR; return DK * DR;
} }
@ -109,13 +110,16 @@ Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
const double pnx = g * x + dx; const double pnx = g * x + dx;
const double pny = g * y + dy; const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// 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, pnx, pny, DK);
// Derivative for points // Derivative for points
if (H2) if (H2)
*H2 = D2dintrinsic(x, y, rr, g, fx_, fy_, s_, k1_, k2_, p1_, p2_); *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
// 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_);
@ -157,7 +161,9 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
const double rr = xx + yy; const double rr = xx + yy;
const double r4 = rr * rr; const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4); const double g = (1 + k1_ * rr + k2_ * r4);
return D2dintrinsic(x, y, rr, g, fx_, fy_, s_, k1_, k2_, p1_, p2_); Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -170,7 +176,9 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx; const double pnx = g * x + dx;
const double pny = g * y + dy; const double pny = g * y + dy;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, fx_, fy_, s_, pnx, pny); Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
} }
/* ************************************************************************* */ /* ************************************************************************* */