diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index bac9259a4..b67b1725e 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -313,35 +313,43 @@ public: boost::optional Dpoint = boost::none, boost::optional Dcal = boost::none) const { - if (!Dpose && !Dpoint && !Dcal) { - const Point3 pc = pose_.transform_to(pw); - if (pc.z() <= 0) - throw CheiralityException(); - const Point2 pn = project_to_camera(pc); - return K_.uncalibrate(pn); - } - - // world to camera coordinate - Matrix Dpc_pose /* 3*6 */, Dpc_point /* 3*3 */; - const Point3 pc = pose_.transform_to(pw, Dpc_pose, Dpc_point); + // Transform to camera coordinates and check cheirality + const Point3 pc = pose_.transform_to(pw); if (pc.z() <= 0) throw CheiralityException(); - // camera to normalized image coordinate - Matrix Dpn_pc; // 2*3 - const Point2 pn = project_to_camera(pc, Dpn_pc); + // Project to normalized image coordinates + const Point2 pn = project_to_camera(pc); - // uncalibration - Matrix Dpi_pn; // 2*2 - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + if (Dpose || Dpoint) { + // optimized version of derivatives, see CalibratedCamera.nb + const double z = pc.z(), d = 1.0 / z; + const double u = pn.x(), v = pn.y(); + Matrix Dpn_pose(2, 6), Dpn_point(2, 3); + if (Dpose) { + double uv = u * v, uu = u * u, vv = v * v; + Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; + } + if (Dpoint) { + const Matrix R(pose_.rotation().matrix()); + Dpn_point << // + R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // + /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); + Dpn_point *= d; + } - // chain the Jacobian matrices - const Matrix Dpi_pc = Dpi_pn * Dpn_pc; - if (Dpose) - *Dpose = Dpi_pc * Dpc_pose; - if (Dpoint) - *Dpoint = Dpi_pc * Dpc_point; - return pi; + // uncalibration + Matrix Dpi_pn; // 2*2 + const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + + // chain the Jacobian matrices + if (Dpose) + *Dpose = Dpi_pn * Dpn_pose; + if (Dpoint) + *Dpoint = Dpi_pn * Dpn_point; + return pi; + } else + return K_.uncalibrate(pn, Dcal); } /** project a point at infinity from world coordinate to the image @@ -531,4 +539,5 @@ private: ar & BOOST_SERIALIZATION_NVP(K_); } /// @} - };} + } + ;} diff --git a/gtsam/geometry/tests/timePinholeCamera.cpp b/gtsam/geometry/tests/timePinholeCamera.cpp index d81491f4e..b858d6175 100644 --- a/gtsam/geometry/tests/timePinholeCamera.cpp +++ b/gtsam/geometry/tests/timePinholeCamera.cpp @@ -45,8 +45,11 @@ int main() */ // Oct 12 2013, iMac 3.06GHz Core i3 - // 447577 calls/second - // 2.23425 musecs/call + // 6.78564e+06 calls/second + // 0.14737 musecs/call + // And after collapse: + // 8.71916e+06 calls/second + // 0.11469 musecs/call { long timeLog = clock(); for(int i = 0; i < n; i++) @@ -58,8 +61,11 @@ int main() } // Oct 12 2013, iMac 3.06GHz Core i3 - // 35367.5 calls/second - // 28.2746 musecs/call + // 258265 calls/second + // 3.87199 musecs/call + // And after collapse: + // 380686 calls/second + // 2.62684 musecs/call { Matrix Dpose, Dpoint; long timeLog = clock(); @@ -72,8 +78,11 @@ int main() } // Oct 12 2013, iMac 3.06GHz Core i3 - // 34325.7 calls/second - // 29.1327 musecs/call + // 249258 calls/second + // 4.0119 musecs/call + // And after collapse: + // 389135 calls/second + // 2.5698 musecs/call { Matrix Dpose, Dpoint, Dcal; long timeLog = clock();