From 9b5ac235e2d8b932714a93582c696c108efea13c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2012 05:09:11 +0000 Subject: [PATCH] Optimized StereoCamera.project derivative --- .cproject | 4 +- gtsam/geometry/Makefile.am | 2 +- gtsam/geometry/SimpleCamera.cpp | 16 ++- gtsam/geometry/StereoCamera.cpp | 124 ++++++++++++++-------- gtsam/geometry/tests/timeStereoCamera.cpp | 51 +++++++++ 5 files changed, 138 insertions(+), 59 deletions(-) create mode 100644 gtsam/geometry/tests/timeStereoCamera.cpp diff --git a/.cproject b/.cproject index 26815d331..4526f371e 100644 --- a/.cproject +++ b/.cproject @@ -1507,10 +1507,10 @@ true true - + make -j2 - tests/timeCalibratedCamera.run + tests/timeStereoCamera.run true true true diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am index 34b307717..c0cfb66fc 100644 --- a/gtsam/geometry/Makefile.am +++ b/gtsam/geometry/Makefile.am @@ -33,7 +33,7 @@ sources += projectiveGeometry.cpp tensorInterface.cpp check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental # Timing tests -noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera +noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera tests/timeStereoCamera # Rot3M and Rot3Q both use Rot3.h, they do not have individual header files allsources = $(sources) diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index c87f72e3e..5c9859e60 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -49,17 +49,13 @@ namespace gtsam { Point2 intrinsic = calibrated_.project(point, H1, H2); if (!H1 && !H2) return K_.uncalibrate(intrinsic); - - Matrix D_projection_intrinsic; - Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic); - - if (H1) { - *H1 = D_projection_intrinsic * (*H1); + else { + Matrix D_projection_intrinsic; + Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic); + if (H1) *H1 = D_projection_intrinsic * (*H1); + if (H2) *H2 = D_projection_intrinsic * (*H2); + return projection; } - if (H2) { - *H2 = D_projection_intrinsic * (*H2); - } - return projection; } Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const { diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index e652930bf..b1e5f0db4 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -1,18 +1,18 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) +*GTSAM Copyright 2010, Georgia Tech Research Corporation, +*Atlanta, Georgia 30332-0415 +*All Rights Reserved +*Authors: Frank Dellaert, et al. (see THANKS for the full author list) - * See LICENSE for the license information +*See LICENSE for the license information - * -------------------------------------------------------------------------- */ +*-------------------------------------------------------------------------- */ /** - * @file StereoCamera.h - * @brief A Stereo Camera based on two Simple Cameras - * @author Chris Beall +*@file StereoCamera.h +*@brief A Stereo Camera based on two Simple Cameras +*@author Chris Beall */ #include @@ -22,45 +22,77 @@ using namespace gtsam; namespace gtsam { -/* ************************************************************************* */ -StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K) : - leftCamPose_(leftCamPose), K_(K) { -} - -/* ************************************************************************* */ -StereoPoint2 StereoCamera::project(const Point3& point, - boost::optional H1, - boost::optional H2) const { - - Point3 cameraPoint = leftCamPose_.transform_to(point, H1, H2); - - if(H1 || H2) { - Matrix D_project_point = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian - - if (H1) - *H1 = D_project_point * *H1; - if (H2) - *H2 = D_project_point * *H2; + /* ************************************************************************* */ + StereoCamera::StereoCamera(const Pose3& leftCamPose, + const Cal3_S2Stereo::shared_ptr K) : + leftCamPose_(leftCamPose), K_(K) { } - const Cal3_S2Stereo& K = *K_; - double f_x = K.fx(), f_y = K.fy(), b=K.baseline(); - double d = 1.0 / cameraPoint.z(); - double uL = K.px() + d * f_x * cameraPoint.x(); - double uR = K.px() + d * f_x * (cameraPoint.x() - b); - double v = K.py() + d * f_y * cameraPoint.y(); - return StereoPoint2(uL, uR, v); -} + /* ************************************************************************* */ + StereoPoint2 StereoCamera::project(const Point3& point, + boost::optional H1, boost::optional H2) const { -/* ************************************************************************* */ -Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { - double d = 1.0 / P.z(), d2 = d * d; - const Cal3_S2Stereo& K = *K_; - double f_x = K.fx(), f_y = K.fy(), b=K.baseline(); - return Matrix_(3, 3, - f_x*d, 0.0, -f_x *P.x() * d2, - f_x*d, 0.0, -f_x *(P.x() - b) * d2, - 0.0, f_y*d, -f_y*P.y() * d2); -} +#ifdef STEREOCAMERA_CHAIN_RULE + const Point3 q = leftCamPose_.transform_to(point, H1, H2); +#else + // omit derivatives + const Point3 q = leftCamPose_.transform_to(point); +#endif + + // get calibration + const Cal3_S2Stereo& K = *K_; + const double fx = K.fx(), fy = K.fy(), b = K.baseline(); + + // calculate scaled but not translated image coordinates + const double d = 1.0 / q.z(); + const double x = q.x(), y = q.y(); + const double uL = d*fx*x; + const double uR = d*fx*(x - b); + const double v = d*fy*y; + + // check if derivatives need to be computed + if (H1 || H2) { +#ifdef STEREOCAMERA_CHAIN_RULE + // just implement chain rule + Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian + if (H1) *H1 = D_project_point*(*H1); + if (H2) *H2 = D_project_point*(*H2); +#else + // optimized version, see StereoCamera.nb + if (H1) { + const double z = q.z(), fxz = fx*z, v1 = v/fy, v2 = fx*v1; + const double dfx = d*fx, dx = d*x; + *H1 = Matrix_(3, 6, + uL*v1, -d*(uL*x + fxz), v2, -dfx, 0.0, d*uL, + uR*v1, -d*(uR*x + fxz), v2, -dfx, 0.0, d*uR, + fy + v*v1, -dx*v, -dx*fy, 0.0, -d*fy, d*v + ); + } + if (H2) { + const Matrix R(leftCamPose_.rotation().matrix()); + *H2 = d * Matrix_(3, 3, + fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, + fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, + fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v + ); + } +#endif + } + + // finally translate + return StereoPoint2(K.px() + uL, K.px() + uR, K.py() + v); + } + + /* ************************************************************************* */ + Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { + double d = 1.0 / P.z(), d2 = d*d; + const Cal3_S2Stereo& K = *K_; + double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); + return Matrix_(3, 3, + f_x*d, 0.0, -d2*f_x* P.x(), + f_x*d, 0.0, -d2*f_x*(P.x() - b), + 0.0, f_y*d, -d2*f_y* P.y() + ); + } } diff --git a/gtsam/geometry/tests/timeStereoCamera.cpp b/gtsam/geometry/tests/timeStereoCamera.cpp new file mode 100644 index 000000000..08da1b0e3 --- /dev/null +++ b/gtsam/geometry/tests/timeStereoCamera.cpp @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file timeStereoCamera.cpp + * @brief time StereoCamera derivatives + * @author Frank Dellaert + */ + +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +int main() +{ + int n = 100000; + + const Pose3 pose1(Matrix_(3,3, + 1., 0., 0., + 0.,-1., 0., + 0., 0.,-1. + ), + Point3(0,0,0.5)); + + const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + const StereoCamera camera(pose1, K); + const Point3 point1(-0.08,-0.08, 0.0); + + Matrix computed1, computed2; + long timeLog = clock(); + for(int i = 0; i < n; i++) + camera.project(point1, computed1, computed2); + long timeLog2 = clock(); + double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; + cout << ((double)n/seconds) << " calls/second" << endl; + cout << ((double)seconds*1000000/n) << " musecs/call" << endl; + + return 0; +}