From 35d6b9dc0e636fbb4f1d85b3c8f0cb2bf585cdf1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 08:44:57 +0100 Subject: [PATCH] Got rid of code duplication --- gtsam/geometry/PinholePose.h | 188 ++--------------------------------- 1 file changed, 8 insertions(+), 180 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index d7c099ab3..0cb189471 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -36,7 +36,7 @@ class PinholeBase { private: - Pose3 pose_; + Pose3 pose_; ///< 3D pose of camera public: @@ -229,7 +229,7 @@ public: return range(camera.pose(), Dcamera, Dother); } -private: +protected: /** * Calculate Jacobian with respect to pose @@ -275,6 +275,8 @@ private: Dpi_pn * Dpn_point; } +private: + /** Serialization function */ friend class boost::serialization::access; template @@ -297,14 +299,14 @@ class PinholePose: public PinholeBase { private: - typedef PinholeBase Base; - boost::shared_ptr K_; + typedef PinholeBase Base; ///< base class has 3D pose as private member + boost::shared_ptr K_; ///< shared pointer to fixed calibration public: enum { dimension = 6 - }; + }; ///< There are 6 DOF to optimize for /// @name Standard Constructors /// @{ @@ -395,7 +397,7 @@ public: } /// return calibration - const Calibration& calibration() const { + virtual const Calibration& calibration() const { return *K_; } @@ -431,183 +433,9 @@ public: } /// @} - /// @name Transformations and measurement functions - /// @{ - - /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * @param P A point in camera coordinates - * @param Dpoint is the 2*3 Jacobian w.r.t. P - */ - static Point2 project_to_camera(const Point3& P, // - OptionalJacobian<2, 3> Dpoint = boost::none) { -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - if (P.z() <= 0) - throw CheiralityException(); -#endif - double d = 1.0 / P.z(); - const double u = P.x() * d, v = P.y() * d; - if (Dpoint) - *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; - return Point2(u, v); - } - - /// Project a point into the image and check depth - std::pair projectSafe(const Point3& pw) const { - const Point3 pc = Base::pose().transform_to(pw); - const Point2 pn = project_to_camera(pc); - return std::make_pair(calibration().uncalibrate(pn), pc.z() > 0); - } - - /** project a point from world coordinate to the image, fixed Jacobians - * @param pw is a point in the world coordinate - * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] - * @param Dpoint is the Jacobian w.r.t. point3 - */ - Point2 project2(const Point3& pw, - OptionalJacobian<2, 6> Dcamera = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const { - - const Point3 pc = Base::pose().transform_to(pw); - const Point2 pn = project_to_camera(pc); - - if (!Dcamera && !Dpoint) { - return calibration().uncalibrate(pn); - } else { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix2 Dpi_pn; - const Point2 pi = calibration().uncalibrate(pn, boost::none, Dpi_pn); - - if (Dcamera) - calculateDpose(pn, d, Dpi_pn, *Dcamera); - if (Dpoint) - calculateDpoint(pn, d, Base::pose().rotation().matrix(), Dpi_pn, - *Dpoint); - - return pi; - } - } - - /// backproject a 2-dimensional point to a 3-dimensional point at given depth - Point3 backproject(const Point2& p, double depth) const { - const Point2 pn = calibration().calibrate(p); - const Point3 pc(pn.x() * depth, pn.y() * depth, depth); - return Base::pose().transform_from(pc); - } - - /// backproject a 2-dimensional point to a 3-dimensional point at infinity - Point3 backprojectPointAtInfinity(const Point2& p) const { - const Point2 pn = calibration().calibrate(p); - const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 - return Base::pose().rotation().rotate(pc); - } - - /** - * Calculate range to a landmark - * @param point 3D location of landmark - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dpoint the optionally computed Jacobian with respect to the landmark - * @return range (double) - */ - double range( - const Point3& point, // - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 3> Dpoint = boost::none) const { - return Base::pose().range(point, Dcamera, Dpoint); - } - - /** - * Calculate range to another pose - * @param pose Other SO(3) pose - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dpose2 the optionally computed Jacobian with respect to the other pose - * @return range (double) - */ - double range( - const Pose3& pose, // - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose = boost::none) const { - return Base::pose().range(pose, Dcamera, Dpose); - } - - /** - * Calculate range to another camera - * @param camera Other camera - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dother the optionally computed Jacobian with respect to the other camera - * @return range (double) - */ - template - double range( - const PinholePose& camera, // - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dother = boost::none) const { - return Base::pose().range(camera.pose(), Dcamera, Dother); - } - - /** - * Calculate range to another camera - * @param camera Other camera - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dother the optionally computed Jacobian with respect to the other camera - * @return range (double) - */ - double range( - const CalibratedCamera& camera, // - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dother = boost::none) const { - return range(camera.pose(), Dcamera, Dother); - } 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 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; - } - - /** - * 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 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; - } - /** Serialization function */ friend class boost::serialization::access; template