From 4b2eb2f7aaf4d23790a23778feaefece00b1fa16 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 27 Mar 2015 18:42:05 -0400 Subject: [PATCH] using overloading rather than templates to manage projection of Point3 and Unit3 (the templates worked on mac, but had issues compiling in ubuntu) --- gtsam/geometry/PinholeCamera.h | 26 +++++++++++------ gtsam/geometry/PinholePose.h | 52 +++++++++++++++++++++------------- 2 files changed, 51 insertions(+), 27 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index cc903e7db..6353ddcdc 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -145,7 +145,7 @@ public: const Pose3& getPose(OptionalJacobian<6, dimension> H) const { if (H) { H->setZero(); - H->template block<6,6>(0, 0) = I_6x6; + H->template block<6, 6>(0, 0) = I_6x6; } return Base::pose(); } @@ -199,14 +199,12 @@ public: typedef Eigen::Matrix Matrix2K; - /** project a point from world coordinate to the image (possibly a Unit3) - * @param pw is a point in the world coordinate + /** Templated projection of a 3D point or a point at infinity into the image + * @param pw either a Point3 or a Unit3, in world coordinates */ template - Point2 project2( - const POINT& pw, // - OptionalJacobian<2, dimension> Dcamera = boost::none, - OptionalJacobian<2, FixedDimension::value> Dpoint = boost::none) const { + Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera, + OptionalJacobian<2, FixedDimension::value> Dpoint) const { // We just call 3-derivative version in Base Matrix26 Dpose; Eigen::Matrix Dcal; @@ -217,6 +215,18 @@ public: return pi; } + /// project a 3D point from world coordinates into the image + Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { + return _project2(pw, Dcamera, Dpoint); + } + + /// project a point at infinity from world coordinates into the image + Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera = + boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const { + return _project2(pw, Dcamera, Dpoint); + } + /** * Calculate range to a landmark * @param point 3D location of landmark @@ -264,7 +274,7 @@ public: if (Dother) { Dother->resize(1, 6 + CalibrationB::dimension); Dother->setZero(); - Dother->block<1,6>(0, 0) = Dother_; + Dother->block<1, 6>(0, 0) = Dother_; } return result; } diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index d98f36b6e..247d11823 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -87,12 +87,8 @@ public: * @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); + const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates + return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates } /** project a point from world coordinate to the image @@ -100,20 +96,20 @@ public: */ Point2 project(const Unit3& pw) const { const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame - const Point2 pn = PinholeBase::Project(pc); - return calibration().uncalibrate(pn); + const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates + return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates } - /** project a point (possibly at infinity) from world coordinate to the image - * @param pw is a point in world coordinates + /** Templated projection of a point (possibly at infinity) from world coordinate to the image + * @param pw is a 3D point or aUnit3 (point at infinity) in world coordinates * @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 */ template - Point2 project(const POINT& pw, OptionalJacobian<2, 6> Dpose, - OptionalJacobian<2, FixedDimension::value> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { + Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, FixedDimension::value> Dpoint, + OptionalJacobian<2, DimK> Dcal) const { // project to normalized coordinates const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); @@ -132,6 +128,20 @@ public: return pi; } + /// project a 3D point from world coordinates into the image + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 3> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { + return _project(pw, Dpose, Dpoint, Dcal); + } + + /// project a point at infinity from world coordinates into the image + Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 2> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { + return _project(pw, Dpose, Dpoint, Dcal); + } + /// 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); @@ -318,15 +328,19 @@ public: return *K_; } - /** project a point (possibly at infinity) from world coordinate to the image, 2 derivatives only + /** project a point from world coordinate to the image, 2 derivatives only * @param pw is a point in world coordinates - * @param Dpose is the Jacobian w.r.t. the whole camera (realy only the pose) + * @param Dpose is the Jacobian w.r.t. the whole camera (really only the pose) * @param Dpoint is the Jacobian w.r.t. point3 - * TODO should use Unit3 */ - template - Point2 project2(const POINT& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, FixedDimension::value> Dpoint = boost::none) const { + Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { + return Base::project(pw, Dpose, Dpoint); + } + + /// project2 version for point at infinity + Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const { return Base::project(pw, Dpose, Dpoint); }