From 48da36acf839ecfb06f46335be8e3e1ff4906a74 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Tue, 8 Mar 2011 19:59:35 +0000 Subject: [PATCH] add a versatile calibrated camera template --- gtsam/geometry/Cal3_S2.h | 6 ++ gtsam/geometry/CalibratedCameraT.h | 168 +++++++++++++++++++++++++++++ 2 files changed, 174 insertions(+) create mode 100644 gtsam/geometry/CalibratedCameraT.h diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index a83552e93..f471acb44 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -70,6 +70,12 @@ namespace gtsam { */ Cal3_S2(const std::string &path); + inline double fx() const { return fx_; } + inline double fy() const { return fy_; } + inline double skew() const { return s_; } + inline double px() const { return u0_; } + inline double py() const { return v0_; } + /** * return the principal point */ diff --git a/gtsam/geometry/CalibratedCameraT.h b/gtsam/geometry/CalibratedCameraT.h new file mode 100644 index 000000000..08e01b860 --- /dev/null +++ b/gtsam/geometry/CalibratedCameraT.h @@ -0,0 +1,168 @@ +/* + * CalibratedCameraT.h + * + * Created on: Mar 5, 2011 + * Author: Yong-Dian Jian + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + class Point2; + /** + * A Calibrated camera class [R|-R't], calibration K. + * If calibration is known, it is more computationally efficient + * to calibrate the measurements rather than try to predict in pixels. + */ + + template + class CalibratedCameraT { + private: + Pose3 pose_; // 6DOF pose + Calibration k_; + + public: + CalibratedCameraT() {} + CalibratedCameraT(const Pose3& pose):pose_(pose){} + CalibratedCameraT(const Pose3& pose, const Calibration& k):pose_(pose),k_(k) {} + CalibratedCameraT(const Vector &v): pose_(Pose3::Expmap(v)) {} + CalibratedCameraT(const Vector &v, const Vector &k):pose_(Pose3::Expmap(v)),k_(k){} + virtual ~CalibratedCameraT() {} + + inline const Pose3& pose() const { return pose_; } + inline const Calibration& calibration() const { return k_; } + bool equals (const CalibratedCameraT &camera, double tol = 1e-9) const { + return pose_.equals(camera.pose(), tol) && k_.equals(camera.calibration(), tol) ; + } + + inline const CalibratedCameraT compose(const CalibratedCameraT &c) const { + return CalibratedCameraT( pose_ * c.pose(), k_ ) ; + } + + inline const CalibratedCameraT inverse() const { + return CalibratedCameraT( pose_.inverse(), k_ ) ; + } + + CalibratedCameraT expmap(const Vector& d) const { + return CalibratedCameraT(pose().expmap(d), k_) ; + } + Vector logmap(const CalibratedCameraT& T2) const { + return pose().logmap(T2.pose()) ; + } + +// static CalibratedCameraT Expmap(const Vector& v) { +// return CalibratedCameraT(Pose3::Expmap(v), k_) ; +// } +// static Vector Logmap(const CalibratedCameraT& p) { +// return Pose3::Logmap(p.pose()) ; +// } + + inline size_t dim() const { return 6 ; } + inline static size_t Dim() { return 6 ; } + + /** + * Create a level camera at the given 2D pose and height + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + */ + // static CalibratedCameraT level(const Pose2& pose2, double height); + + /* ************************************************************************* */ + // measurement functions and derivatives + /* ************************************************************************* */ + + /** + * This function receives the camera pose and the landmark location and + * returns the location the point is supposed to appear in the image + * @param camera the CalibratedCameraT + * @param point a 3D point to be projected + * @return the intrinsic coordinates of the projected point + */ + Point2 project(const Point3& point, + boost::optional D_intrinsic_pose = boost::none, + boost::optional D_intrinsic_point = boost::none) const { + + // no derivative is necessary + if ( !D_intrinsic_pose && !D_intrinsic_point ) { + Point3 pc = pose_.transform_to(point) ; + Point2 pn = project_to_camera(pc) ; + return k_.uncalibrate(pn) ; + } + + // world to camera coordinate + Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ; + Point3 pc = pose_.transform_to(point, Hc1, Hc2) ; + + // camera to normalized image coordinate + Matrix Hn; // 2*3 + Point2 pn = project_to_camera(pc, Hn) ; + + // uncalibration + Matrix Hi; // 2*2 + Point2 pi = k_.uncalibrate(pn,boost::none,Hi); + + Matrix tmp = Hi*Hn ; + *D_intrinsic_pose = tmp * Hc1 ; + *D_intrinsic_point = tmp * Hc2 ; + return pi ; + } + + std::pair projectSafe( + const Point3& pw, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + Point3 pc = pose_.transform_to(pw); + return std::pair( project(pw,H1,H2), pc.z() > 0); + } + + std::pair projectSafe( + const Point3& pw, + const Point3& pw_normal, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + Point3 pc = pose_.transform_to(pw); + Point3 pc_normal = pose_.rotation().unrotate(pw_normal); + return std::pair( project(pw,H1,H2), (pc.z() > 0) && (pc_normal.z() < -0.5) ); + } + + // /** +// * 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& P, + boost::optional H1 = boost::none){ + if (H1) { + double d = 1.0 / P.z(), d2 = d * d; + *H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); + } + return Point2(P.x() / P.z(), P.y() / P.z()); + } + + /** + * backproject a 2-dimensional point to a 3-dimension point + */ + Point3 backproject_from_camera(const Point2& pi, const double scale) { + Point2 pn = k_.calibrate(pi); + Point3 pc(pn.x()*scale, pn.y()*scale, scale); + return pose_.transform_from(pc); + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(pose_); + ar & BOOST_SERIALIZATION_NVP(k_); + } + + }; +} + +