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

New matrix definitions

release/4.3a0
dellaert 2014-10-07 00:31:13 +02:00
parent 399bf7c993
commit e289539312
1 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/geometry/Cal3DS2.cpp
View File

@ -53,23 +53,23 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx, static Matrix29 D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny, double yy, double xy, double rr, double r4, double pnx, double pny,
const Eigen::Matrix<double, 2, 2>& DK) { const Matrix2& DK) {
Eigen::Matrix<double, 2, 5> DR1; Matrix25 DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 4> DR2; Matrix24 DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy; y * rr, y * r4, rr + 2 * yy, 2 * xy;
Eigen::Matrix<double, 2, 9> D; Matrix29 D;
D << DR1, DK * DR2; D << DR1, DK * DR2;
return D; return D;
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr, static Matrix2 D2dintrinsic(double x, double y, double rr,
double g, 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 Matrix2& 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;
@ -82,7 +82,7 @@ static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
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);
Eigen::Matrix<double, 2, 2> DR; Matrix2 DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy; y * dgdx + dDydx, g + y * dgdy + dDydy;
@ -110,7 +110,7 @@ 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; Matrix2 DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_; if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration // Derivative for calibration
@ -161,7 +161,7 @@ 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);
Eigen::Matrix<double, 2, 2> DK; Matrix2 DK;
DK << fx_, s_, 0.0, fy_; DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
} }
@ -176,7 +176,7 @@ 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;
Eigen::Matrix<double, 2, 2> DK; Matrix2 DK;
DK << fx_, s_, 0.0, fy_; DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
} }