From e4edaf359cab35d5603937cc73b9f3118d5725ff Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Mar 2014 18:04:16 -0500 Subject: [PATCH] Avoiding mallocs and working with fixed blocks where possible, makes a pretty noticeable difference in the inner linearize loop --- gtsam/geometry/Cal3Bundler.cpp | 11 +- gtsam/geometry/PinholeCamera.h | 117 +++++++++++++-------- gtsam/geometry/Rot3.cpp | 5 +- gtsam/geometry/tests/testPinholeCamera.cpp | 47 ++++++--- 4 files changed, 115 insertions(+), 65 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 03071a4f2..587d0ea63 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -78,17 +78,16 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // // Derivatives make use of intermediate variables above if (Dcal) { double rx = r * x, ry = r * y; - Eigen::Matrix D; - D << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry; - *Dcal = D; + Dcal->resize(2, 3); + *Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry; } if (Dp) { const double a = 2. * (k1_ + 2. * k2_ * r); const double axx = a * x * x, axy = a * x * y, ayy = a * y * y; - Eigen::Matrix D; - D << g + axx, axy, axy, g + ayy; - *Dp = f_ * D; + Dp->resize(2,2); + *Dp << g + axx, axy, axy, g + ayy; + *Dp *= f_; } return Point2(u0_ + f_ * u, v0_ + f_ * v); diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index c162171c0..19eb87b5f 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -271,6 +271,10 @@ public: */ inline static Point2 project_to_camera(const Point3& P, boost::optional Dpoint = boost::none) { +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (P.z() <= 0) + throw CheiralityException(); +#endif if (Dpoint) { double d = 1.0 / P.z(), d2 = d * d; *Dpoint = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); @@ -300,40 +304,25 @@ public: // Transform to camera coordinates and check cheirality const Point3 pc = pose_.transform_to(pw); -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - if (pc.z() <= 0) - throw CheiralityException(); -#endif - // Project to normalized image coordinates const Point2 pn = project_to_camera(pc); 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; - } // uncalibration - Matrix Dpi_pn; // 2*2 + 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; + if (Dpose) { + Dpose->resize(2, 6); + calculateDpose(pn, d, Dpi_pn, *Dpose); + } + if (Dpoint) { + Dpoint->resize(2, 3); + calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); + } return pi; } else return K_.uncalibrate(pn, Dcal); @@ -391,27 +380,29 @@ public: boost::optional Dcamera = boost::none, boost::optional Dpoint = boost::none) const { + const Point3 pc = pose_.transform_to(pw); + const Point2 pn = project_to_camera(pc); + if (!Dcamera && !Dpoint) { - const Point3 pc = pose_.transform_to(pw); - -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - if (pc.z() <= 0) - throw CheiralityException(); -#endif - const Point2 pn = project_to_camera(pc); return K_.uncalibrate(pn); - } + } else { + const double z = pc.z(), d = 1.0 / z; - Matrix Dpose, Dp, Dcal; - const Point2 pi = this->project(pw, Dpose, Dp, Dcal); - if (Dcamera) { - *Dcamera = Matrix(2, this->dim()); - Dcamera->leftCols(pose().dim()) = Dpose; // Jacobian wrt pose3 - Dcamera->rightCols(calibration().dim()) = Dcal; // Jacobian wrt calib + // uncalibration + Matrix Dcal, Dpi_pn(2, 2); + const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + + if (Dcamera) { + Dcamera->resize(2, this->dim()); + calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>()); + Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib + } + if (Dpoint) { + Dpoint->resize(2, 3); + calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); + } + return pi; } - if (Dpoint) - *Dpoint = Dp; - return pi; } /// backproject a 2-dimensional point to a 3-dimensional point at given depth @@ -516,6 +507,50 @@ public: private: + /** + * Calculate Jacobian with respect to pose + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + * @param Dpi_pn derivative of uncalibrate with respect to pn + * @param Dpose Output argument, can be matrix or block, assumed right size ! + * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html + */ + template + static void calculateDpose(const Point2& pn, double d, const Matrix& Dpi_pn, + Eigen::MatrixBase const & Dpose) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + double uv = u * v, uu = u * u, vv = v * v; + Eigen::Matrix Dpn_pose; + Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; + assert(Dpose.rows()==2 && Dpose.cols()==6); + const_cast&>(Dpose) = // + Dpi_pn.block<2, 2>(0, 0) * Dpn_pose; + } + + /** + * Calculate Jacobian with respect to point + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + * @param Dpi_pn derivative of uncalibrate with respect to pn + * @param Dpoint Output argument, can be matrix or block, assumed right size ! + * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html + */ + template + static void calculateDpoint(const Point2& pn, double d, const Matrix& R, + const Matrix& Dpi_pn, Eigen::MatrixBase const & Dpoint) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + Eigen::Matrix Dpn_point; + 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; + assert(Dpoint.rows()==2 && Dpoint.cols()==3); + const_cast&>(Dpoint) = // + Dpi_pn.block<2, 2>(0, 0) * Dpn_point; + } + /// @} /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index e970b99f8..13cd802bc 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -101,10 +101,9 @@ Sphere2 Rot3::operator*(const Sphere2& p) const { // see doc/math.lyx, SO(3) section Point3 Rot3::unrotate(const Point3& p, boost::optional H1, boost::optional H2) const { - const Matrix Rt(transpose()); - Point3 q(Rt*p.vector()); // q = Rt*p + Point3 q(transpose()*p.vector()); // q = Rt*p if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); - if (H2) *H2 = Rt; + if (H2) *H2 = transpose(); return q; } diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 594db159b..b0a4cef24 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -169,27 +169,27 @@ static Point2 project3(const Pose3& pose, const Point3& point, const Cal3_S2& ca } /* ************************************************************************* */ -static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { - Point3 point(point2D.x(), point2D.y(), 1.0); - return Camera(pose,cal).projectPointAtInfinity(point); -} - -/* ************************************************************************* */ -TEST( PinholeCamera, Dproject_point_pose) +TEST( PinholeCamera, Dproject) { Matrix Dpose, Dpoint, Dcal; Point2 result = camera.project(point1, Dpose, Dpoint, Dcal); Matrix numerical_pose = numericalDerivative31(project3, pose1, point1, K); Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K); Matrix numerical_cal = numericalDerivative33(project3, pose1, point1, K); - CHECK(assert_equal(result, Point2(-100, 100) )); - CHECK(assert_equal(Dpose, numerical_pose, 1e-7)); - CHECK(assert_equal(Dpoint, numerical_point,1e-7)); - CHECK(assert_equal(Dcal, numerical_cal,1e-7)); + CHECK(assert_equal(Point2(-100, 100), result)); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); + CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); } /* ************************************************************************* */ -TEST( PinholeCamera, Dproject_point_pose_Infinity) +static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { + Point3 point(point2D.x(), point2D.y(), 1.0); + return Camera(pose,cal).projectPointAtInfinity(point); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, Dproject_Infinity) { Matrix Dpose, Dpoint, Dcal; Point2 point2D(-0.08,-0.08); @@ -198,9 +198,26 @@ TEST( PinholeCamera, Dproject_point_pose_Infinity) Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point2D, K); Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K); Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point2D, K); - CHECK(assert_equal(Dpose, numerical_pose, 1e-7)); - CHECK(assert_equal(Dpoint, numerical_point,1e-7)); - CHECK(assert_equal(Dcal, numerical_cal,1e-7)); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); + CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); +} + +/* ************************************************************************* */ +static Point2 project4(const Camera& camera, const Point3& point) { + return camera.project2(point); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, Dproject2) +{ + Matrix Dcamera, Dpoint; + Point2 result = camera.project2(point1, Dcamera, Dpoint); + Matrix numerical_camera = numericalDerivative21(project4, camera, point1); + Matrix numerical_point = numericalDerivative22(project4, camera, point1); + CHECK(assert_equal(result, Point2(-100, 100) )); + CHECK(assert_equal(numerical_camera, Dcamera, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } /* ************************************************************************* */