diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 2876da579..80f63f3fc 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -19,6 +19,8 @@ #include #include +using namespace std; + namespace gtsam { /* ************************************************************************* */ @@ -69,7 +71,7 @@ bool PinholeBase::equals(const PinholeBase &camera, double tol) const { } /* ************************************************************************* */ -void PinholeBase::print(const std::string& s) const { +void PinholeBase::print(const string& s) const { pose_.print(s + ".pose"); } @@ -83,29 +85,36 @@ const Pose3& PinholeBase::pose(OptionalJacobian<6, 6> H) const { } /* ************************************************************************* */ -Point2 PinholeBase::project_to_camera(const Point3& P, +Point2 PinholeBase::project_to_camera(const Point3& pc, OptionalJacobian<2, 3> Dpoint) { -#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; + double d = 1.0 / pc.z(); + const double u = pc.x() * d, v = pc.y() * d; if (Dpoint) *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; return Point2(u, v); } +/* ************************************************************************* */ +pair PinholeBase::projectSafe(const Point3& pw) const { + const Point3 pc = pose().transform_to(pw); + const Point2 pn = project_to_camera(pc); + return make_pair(pn, pc.z() > 0); +} + /* ************************************************************************* */ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint) const { 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); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (q.z() <= 0) + throw CheiralityException(); +#endif + const Point2 pn = project_to_camera(q); if (Dpose || Dpoint) { - const double z = q.z(), d = 1.0 / z; + const double d = 1.0 / q.z(); if (Dpose) *Dpose = PinholeBase::Dpose(pn, d); if (Dpoint) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 9f0ba5ea0..35e8a5086 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -138,21 +138,23 @@ public: /// @{ /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point - * @param P A point in camera coordinates - * @param Dpoint is the 2*3 Jacobian w.r.t. P + * Project from 3D point in camera coordinates into image + * Does *not* throw a CheiralityException, even if pc behind image plane + * @param pc point in camera coordinates + * @param Dpoint is the 2*3 Jacobian w.r.t. pc */ - static Point2 project_to_camera(const Point3& P, // + static Point2 project_to_camera(const Point3& pc, // OptionalJacobian<2, 3> Dpoint = boost::none); - /** - * backproject a 2-dimensional point to a 3-dimensional point at given depth - */ + /// Project a point into the image and check depth + std::pair projectSafe(const Point3& pw) const; + + /// backproject a 2-dimensional point to a 3-dimensional point at given depth static Point3 backproject_from_camera(const Point2& p, const double depth); /** * Project point into the image + * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION * @param point 3D point in world coordinates * @param Dpose the optionally computed Jacobian with respect to camera * @param Dpoint the optionally computed Jacobian with respect to the 3D point diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 8785ec3fc..2fc463d2b 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -72,9 +72,9 @@ public: /// Project a point into the image and check depth std::pair projectSafe(const Point3& pw) const { - const Point3 pc = pose().transform_to(pw); - const Point2 pn = project_to_camera(pc); - return std::make_pair(calibration().uncalibrate(pn), pc.z() > 0); + std::pair pn = PinholeBase::projectSafe(pw); + pn.first = calibration().uncalibrate(pn.first); + return pn; } /** project a point from world coordinate to the image, fixed Jacobians