From fb47cf89614507172182826e6ca98400f9c7e46c Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 23 Feb 2015 12:37:55 +0100 Subject: [PATCH] moved projectPointAtInfinity down --- gtsam/geometry/PinholeCamera.h | 49 --------------------------- gtsam/geometry/PinholePose.h | 60 ++++++++++++++++++++++++++++++++-- 2 files changed, 58 insertions(+), 51 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index bfc2bbb93..632f6de86 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -31,14 +31,6 @@ namespace gtsam { template class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { -public: - - /** - * Some classes template on either PinholeCamera or StereoCamera, - * and this typedef informs those classes what "project" returns. - */ - typedef Point2 Measurement; - private: typedef PinholeBaseK Base; ///< base class has 3D pose as private member @@ -235,47 +227,6 @@ public: return pi; } - /** project a point at infinity from world coordinate to the image - * @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf) - * @param Dpose is the Jacobian w.r.t. pose3 - * @param Dpoint is the Jacobian w.r.t. point3 - * @param Dcal is the Jacobian w.r.t. calibration - */ - Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose = - boost::none, OptionalJacobian<2, 2> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { - - if (!Dpose && !Dpoint && !Dcal) { - const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter) - const Point2 pn = Base::project_to_camera(pc); // project the point to the camera - return K_.uncalibrate(pn); - } - - // world to camera coordinate - Matrix3 Dpc_rot, Dpc_point; - const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point); - - Matrix36 Dpc_pose; - Dpc_pose.setZero(); - Dpc_pose.leftCols<3>() = Dpc_rot; - - // camera to normalized image coordinate - Matrix23 Dpn_pc; // 2*3 - const Point2 pn = Base::project_to_camera(pc, Dpn_pc); - - // uncalibration - Matrix2 Dpi_pn; // 2*2 - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; - if (Dpose) - *Dpose = Dpi_pc * Dpc_pose; - if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only) - return pi; - } - /** project a point from world coordinate to the image, fixed Jacobians * @param pw is a point in the world coordinate */ diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 7588f517e..6f2f7dca0 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -35,7 +35,10 @@ class GTSAM_EXPORT PinholeBaseK: public PinholeBase { GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) -public : + // Get dimensions of calibration type at compile time +static const int DimK = FixedDimension::value; + +public : /// @name Standard Constructors /// @{ @@ -78,7 +81,19 @@ public : return pn; } - /** project a point from world coordinate to the image, fixed Jacobians + /** project a point from world coordinate to the image + * @param pw is a point in the world coordinates + */ + Point2 project(const Point3& pw) const { + + // project to normalized coordinates + const Point2 pn = PinholeBase::project2(pw); + + // uncalibrate to pixel coordinates + return calibration().uncalibrate(pn); + } + + /** project a point from world coordinate to the image, w 2 derivatives * @param pw is a point in the world coordinates */ Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, @@ -99,6 +114,47 @@ public : return pi; } + /** project a point at infinity from world coordinate to the image + * @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf) + * @param Dpose is the Jacobian w.r.t. pose3 + * @param Dpoint is the Jacobian w.r.t. point3 + * @param Dcal is the Jacobian w.r.t. calibration + */ + Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose = + boost::none, OptionalJacobian<2, 2> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { + + if (!Dpose && !Dpoint && !Dcal) { + const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter) + const Point2 pn = PinholeBase::project_to_camera(pc);// project the point to the camera + return calibration().uncalibrate(pn); + } + + // world to camera coordinate + Matrix3 Dpc_rot, Dpc_point; + const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point); + + Matrix36 Dpc_pose; + Dpc_pose.setZero(); + Dpc_pose.leftCols<3>() = Dpc_rot; + + // camera to normalized image coordinate + Matrix23 Dpn_pc;// 2*3 + const Point2 pn = PinholeBase::project_to_camera(pc, Dpn_pc); + + // uncalibration + Matrix2 Dpi_pn;// 2*2 + const Point2 pi = calibration().uncalibrate(pn, Dcal, Dpi_pn); + + // chain the Jacobian matrices + const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; + if (Dpose) + *Dpose = Dpi_pc * Dpc_pose; + if (Dpoint) + *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>();// only 2dof are important for the point (direction-only) + 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);