From 29e5faeef05a1157640f7d201abd8e19fd3a6223 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 10:00:56 +0100 Subject: [PATCH] Refactored derivatives --- gtsam/geometry/CalibratedCamera.cpp | 54 +++++++++++++++++------------ gtsam/geometry/CalibratedCamera.h | 38 +++----------------- gtsam/geometry/PinholePose.h | 8 +++-- 3 files changed, 42 insertions(+), 58 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 41ed3e331..620ba1cc8 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -80,6 +80,28 @@ Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target, return Pose3(Rot3(xc, yc, zc), eye); } +/* ************************************************************************* */ +Matrix26 PinholeBase::Dpose(const Point2& pn, double d) { + // 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; + Matrix26 Dpn_pose; + Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; + return Dpn_pose; +} + +/* ************************************************************************* */ +Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& R) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + Matrix23 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; + return Dpn_point; +} + /* ************************************************************************* */ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { return CalibratedCamera(LevelPose(pose2, height)); @@ -93,32 +115,20 @@ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, /* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, - OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint) const { + OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const { - Point3 q = pose().transform_to(point); - Point2 intrinsic = project_to_camera(q); + Matrix3 Rt; // calculated by transform_to if needed + const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); + Point2 pn = project_to_camera(q); - // Check if point is in front of camera - if (q.z() <= 0) - throw CheiralityException(); - - if (Dpose || Dpoint) { - // optimized version, see CalibratedCamera.nb + if (Dcamera || Dpoint) { const double z = q.z(), d = 1.0 / z; - const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v; - if (Dpose) - *Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), -uv, -u, 0., -d, d - * v; - if (Dpoint) { - const Matrix3 R(pose().rotation().matrix()); - Matrix23 Dpoint_; - Dpoint_ << 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); - *Dpoint = d * Dpoint_; - } + if (Dcamera) + *Dcamera = PinholeBase::Dpose(pn, d); + if (Dpoint) + *Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose } - return intrinsic; + return pn; } /* ************************************************************************* */ diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 13d607c80..fa02f706d 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -19,10 +19,11 @@ #pragma once #include -#include namespace gtsam { +class Point2; + class GTSAM_EXPORT CheiralityException: public ThreadsafeException< CheiralityException> { public: @@ -134,45 +135,16 @@ protected: * 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 Matrix2& 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; - Matrix26 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 * Dpn_pose; - } + static Matrix26 Dpose(const Point2& pn, double d); /** * 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 + * @param R rotation matrix */ - template - static void calculateDpoint(const Point2& pn, double d, const Matrix3& R, - const Matrix2& Dpi_pn, Eigen::MatrixBase const & Dpoint) { - // optimized version of derivatives, see CalibratedCamera.nb - const double u = pn.x(), v = pn.y(); - Matrix23 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 * Dpn_point; - } + static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& R); private: diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 40373c9fb..488e860bc 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include #include @@ -85,7 +86,8 @@ public: OptionalJacobian<2, 6> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { - const Point3 pc = pose().transform_to(pw); + Matrix3 Rt; // calculated by transform_to if needed + const Point3 pc = pose().transform_to(pw, boost::none, Dpoint ? &Rt : 0); const Point2 pn = project_to_camera(pc); if (!Dcamera && !Dpoint) { @@ -98,9 +100,9 @@ public: const Point2 pi = calibration().uncalibrate(pn, boost::none, Dpi_pn); if (Dcamera) - calculateDpose(pn, d, Dpi_pn, *Dcamera); + *Dcamera = Dpi_pn * PinholeBase::Dpose(pn, d); if (Dpoint) - calculateDpoint(pn, d, pose().rotation().matrix(), Dpi_pn, *Dpoint); + *Dpoint = Dpi_pn * PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose return pi; }