From 7d0674fe4b43f75a6cbf9941bce45462bde5e36c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 12 Oct 2013 07:07:08 +0000 Subject: [PATCH] Inlined derivatives in Cal3DS2, gets rid of a lot of duplicated calculations. --- .cproject | 8 ++ gtsam/geometry/Cal3DS2.cpp | 129 +++++++++++++-------- gtsam/geometry/tests/timePinholeCamera.cpp | 24 +++- 3 files changed, 108 insertions(+), 53 deletions(-) diff --git a/.cproject b/.cproject index ed6db2627..3de52ad94 100644 --- a/.cproject +++ b/.cproject @@ -1645,6 +1645,14 @@ true true + + make + -j5 + testCal3DS2.run + true + true + true + make -j2 diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 1d5ad695a..c47da2581 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -31,41 +31,70 @@ Cal3DS2::Cal3DS2(const Vector &v): Matrix Cal3DS2::K() const { return Matrix_(3,3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); } /* ************************************************************************* */ -Vector Cal3DS2::vector() const { return Vector_(9, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_) ; } +Vector Cal3DS2::vector() const { return Vector_(9, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); } /* ************************************************************************* */ -void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K") ; gtsam::print(Vector(k()), s + ".k") ; } +void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K"); gtsam::print(Vector(k()), s + ".k"); } /* ************************************************************************* */ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol) - return false ; - return true ; + return false; + return true; } /* ************************************************************************* */ Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, boost::optional H2) const { -// 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 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 ; + // parameters + const double fx = fx_, fy = fy_, s = s_; + const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; - if (H1) *H1 = D2d_calibration(p) ; - if (H2) *H2 = D2d_intrinsic(p) ; + // 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 rr = xx + yy; + const double r4 = rr * rr; + 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); - return Point2(fx_* x2 + s_ * y2 + u0_, fy_ * y2 + v0_) ; + 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; + + 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; + const double dDy_dy = 2. * k4 * x + k3 * (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; + } + + return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_); } /* ************************************************************************* */ @@ -82,14 +111,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 ) { + 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*k3_*xy + k4_*(rr+2*xx) ; - const double dy = 2*k4_*xy + k3_*(rr+2*yy) ; - pn = (invKPi - Point2(dx,dy))/g ; + 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*k3_*xy + k4_*(rr+2*xx); + const double dy = 2*k4_*xy + k3_*(rr+2*yy); + pn = (invKPi - Point2(dx,dy))/g; } if ( iteration >= maxIterations ) @@ -100,41 +129,41 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { /* ************************************************************************* */ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { - //const double fx = fx_, fy = fy_, s = s_ ; + //const double fx = fx_, fy = fy_, s = s_; 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 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 ; + //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 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*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 ; - const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y) ; + // 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; + const double dDy_dy = 2*k4*x + k3*(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) ; + y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy); return DK * DR; } /* ************************************************************************* */ 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 rr = xx + yy ; - const double r4 = rr*rr ; - const double fx = fx_, fy = fy_, s = s_ ; - 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 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 fx = fx_, fy = fy_, s = s_; + 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; @@ -144,7 +173,7 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { } /* ************************************************************************* */ -Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d) ; } +Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d); } /* ************************************************************************* */ Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { return vector() - T2.vector(); } diff --git a/gtsam/geometry/tests/timePinholeCamera.cpp b/gtsam/geometry/tests/timePinholeCamera.cpp index b858d6175..fbe458829 100644 --- a/gtsam/geometry/tests/timePinholeCamera.cpp +++ b/gtsam/geometry/tests/timePinholeCamera.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include using namespace std; using namespace gtsam; @@ -35,8 +35,8 @@ int main() ), Point3(0,0,0.5)); - static const Cal3_S2 K(625, 625, 0, 0, 0); - const PinholeCamera camera(pose1,K); + static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); + const PinholeCamera camera(pose1,K); const Point3 point1(-0.08,-0.08, 0.0); /** @@ -50,6 +50,12 @@ int main() // And after collapse: // 8.71916e+06 calls/second // 0.11469 musecs/call + // Cal3DS2: + // 7.04176e+06 calls/second + // 0.14201 musecs/call + // After Cal3DS2 fix: + // 8.17595e+06 calls/second + // 0.12231 musecs/call { long timeLog = clock(); for(int i = 0; i < n; i++) @@ -66,6 +72,12 @@ int main() // And after collapse: // 380686 calls/second // 2.62684 musecs/call + // Cal3DS2: + // 230789 calls/second + // 4.33297 musecs/call + // After Cal3DS2 fix: + // 304354 calls/second + // 3.28565 musecs/call { Matrix Dpose, Dpoint; long timeLog = clock(); @@ -83,6 +95,12 @@ int main() // And after collapse: // 389135 calls/second // 2.5698 musecs/call + // Cal3DS2: + // 206933 calls/second + // 4.83248 musecs/call + // After Cal3DS2 fix: + // 289996 calls/second + // 3.44832 musecs/call { Matrix Dpose, Dpoint, Dcal; long timeLog = clock();