From f5581ec652567d7d196372296ebe94e6eed64157 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 09:13:12 +0100 Subject: [PATCH] CalibratedCamera now derived from PinholeBase --- gtsam/geometry/CalibratedCamera.cpp | 40 +++------ gtsam/geometry/CalibratedCamera.h | 127 +++++++++------------------- 2 files changed, 51 insertions(+), 116 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 1bba07d06..327948f3e 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -55,33 +55,13 @@ Point2 PinholeBase::project_to_camera(const Point3& P, } /* ************************************************************************* */ -CalibratedCamera::CalibratedCamera(const Pose3& pose) : - pose_(pose) { -} - -/* ************************************************************************* */ -CalibratedCamera::CalibratedCamera(const Vector &v) : - pose_(Pose3::Expmap(v)) { -} - -/* ************************************************************************* */ -Point2 CalibratedCamera::project_to_camera(const Point3& P, - OptionalJacobian<2, 3> H1) { - if (H1) { - double d = 1.0 / P.z(), d2 = d * d; - *H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2; - } - return Point2(P.x() / P.z(), P.y() / P.z()); -} - -/* ************************************************************************* */ -Point3 CalibratedCamera::backproject_from_camera(const Point2& p, +Point3 PinholeBase::backproject_from_camera(const Point2& p, const double scale) { return Point3(p.x() * scale, p.y() * scale, scale); } /* ************************************************************************* */ -Pose3 CalibratedCamera::LevelPose(const Pose2& pose2, double height) { +Pose3 PinholeBase::LevelPose(const Pose2& pose2, double height) { const double st = sin(pose2.theta()), ct = cos(pose2.theta()); const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); const Rot3 wRc(x, y, z); @@ -90,12 +70,7 @@ Pose3 CalibratedCamera::LevelPose(const Pose2& pose2, double height) { } /* ************************************************************************* */ -CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { - return CalibratedCamera(LevelPose(pose2, height)); -} - -/* ************************************************************************* */ -Pose3 CalibratedCamera::LookatPose(const Point3& eye, const Point3& target, +Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target, const Point3& upVector) { Point3 zc = target - eye; zc = zc / zc.norm(); @@ -105,6 +80,11 @@ Pose3 CalibratedCamera::LookatPose(const Point3& eye, const Point3& target, return Pose3(Rot3(xc, yc, zc), eye); } +/* ************************************************************************* */ +CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { + return CalibratedCamera(LevelPose(pose2, height)); +} + /* ************************************************************************* */ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, const Point3& target, const Point3& upVector) { @@ -120,7 +100,7 @@ Point2 CalibratedCamera::project(const Point3& point, Matrix3 Dpoint_; Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0); #else - Point3 q = pose_.transform_to(point); + Point3 q = pose().transform_to(point); #endif Point2 intrinsic = project_to_camera(q); @@ -145,7 +125,7 @@ Point2 CalibratedCamera::project(const Point3& point, *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()); + 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) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index a57c6f852..6185cb4f9 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -44,6 +44,30 @@ private: public: + /// @name Static functions + /// @{ + + /** + * Create a level pose at the given 2D pose and height + * @param K the calibration + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + * @param height camera height + */ + static Pose3 LevelPose(const Pose2& pose2, double height); + + /** + * Create a camera pose at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + */ + static Pose3 LookatPose(const Point3& eye, const Point3& target, + const Point3& upVector); + + /// @} /// @name Standard Constructors /// @{ @@ -102,6 +126,11 @@ public: static Point2 project_to_camera(const Point3& P, // OptionalJacobian<2, 3> Dpoint = boost::none); + /** + * backproject a 2-dimensional point to a 3-dimension point + */ + static Point3 backproject_from_camera(const Point2& p, const double scale); + /** * Calculate range to a landmark * @param point 3D location of landmark @@ -195,9 +224,7 @@ private: * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT CalibratedCamera { -private: - Pose3 pose_; // 6DOF pose +class GTSAM_EXPORT CalibratedCamera: public PinholeBase { public: @@ -213,21 +240,14 @@ public: } /// construct with pose - explicit CalibratedCamera(const Pose3& pose); + explicit CalibratedCamera(const Pose3& pose) : + PinholeBase(pose) { + } /// @} /// @name Named Constructors /// @{ - /** - * Create a level pose at the given 2D pose and height - * @param K the calibration - * @param pose2 specifies the location and viewing direction - * (theta 0 = looking in direction of positive X axis) - * @param height camera height - */ - static Pose3 LevelPose(const Pose2& pose2, double height); - /** * Create a level camera at the given 2D pose and height * @param pose2 specifies the location and viewing direction @@ -236,17 +256,6 @@ public: */ static CalibratedCamera Level(const Pose2& pose2, double height); - /** - * Create a camera pose at the given eye position looking at a target point in the scene - * with the specified up direction vector. - * @param eye specifies the camera position - * @param target the point to look at - * @param upVector specifies the camera up direction vector, - * doesn't need to be on the image plane nor orthogonal to the viewing axis - */ - static Pose3 LookatPose(const Point3& eye, const Point3& target, - const Point3& upVector); - /** * Create a camera at the given eye position looking at a target point in the scene * with the specified up direction vector. @@ -263,19 +272,8 @@ public: /// @{ /// construct from vector - explicit CalibratedCamera(const Vector &v); - - /// @} - /// @name Testable - /// @{ - - virtual void print(const std::string& s = "") const { - pose_.print(s); - } - - /// check equality to another camera - bool equals(const CalibratedCamera &camera, double tol = 1e-9) const { - return pose_.equals(camera.pose(), tol); + explicit CalibratedCamera(const Vector &v) : + PinholeBase(v) { } /// @} @@ -286,11 +284,6 @@ public: virtual ~CalibratedCamera() { } - /// return pose - inline const Pose3& pose() const { - return pose_; - } - /// @} /// @name Manifold /// @{ @@ -311,10 +304,6 @@ public: return 6; } - /* ************************************************************************* */ - // measurement functions and derivatives - /* ************************************************************************* */ - /// @} /// @name Transformations and mesaurement functions /// @{ @@ -330,43 +319,6 @@ public: OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; - /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * With optional 2by3 derivative - */ - static Point2 project_to_camera(const Point3& cameraPoint, - OptionalJacobian<2, 3> H1 = boost::none); - - /** - * backproject a 2-dimensional point to a 3-dimension point - */ - static Point3 backproject_from_camera(const Point2& p, const double scale); - - /** - * Calculate range to a landmark - * @param point 3D location of landmark - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point - * @return range (double) - */ - double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) const { - return pose_.range(point, H1, H2); - } - - /** - * Calculate range to another pose - * @param pose Other SO(3) pose - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point - * @return range (double) - */ - double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 6> H2 = boost::none) const { - return pose_.range(pose, H1, H2); - } - /** * Calculate range to another camera * @param camera Other camera @@ -376,12 +328,13 @@ public: */ double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 = boost::none, OptionalJacobian<1, 6> H2 = boost::none) const { - return pose_.range(camera.pose_, H1, H2); + return pose().range(camera.pose(), H1, H2); } + /// @} + private: - /// @} /// @name Advanced Interface /// @{ @@ -389,7 +342,9 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(pose_); + ar + & boost::serialization::make_nvp("PinholeBase", + boost::serialization::base_object(*this)); } /// @}