From b1922464defcf2e58e076de716ecacbd6b67a6a3 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Mon, 4 Apr 2011 03:55:50 +0000 Subject: [PATCH] add member functions --- gtsam/geometry/Cal3_S2.h | 13 +++ gtsam/geometry/CalibratedCameraT.h | 124 +++++++++++++++++++---------- 2 files changed, 94 insertions(+), 43 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index dc654cbda..c70fe286a 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -104,6 +104,19 @@ namespace gtsam { return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); } + + /** + * return calibration matrix inv(K) + */ + Matrix matrix_inverse() const { + const double fxy = fx_*fy_, sv0 = s_*v0_, fyu0 = fy_*u0_; + return Matrix_(3, 3, + 1.0/fx_, -s_/fxy, (sv0-fyu0)/fxy, + 0.0, 1.0/fy_, -v0_/fy_, + 0.0, 0.0, 1.0); + } + + /** * convert intrinsic coordinates xy to image coordinates uv * with optional derivatives diff --git a/gtsam/geometry/CalibratedCameraT.h b/gtsam/geometry/CalibratedCameraT.h index 08e01b860..469a89aa7 100644 --- a/gtsam/geometry/CalibratedCameraT.h +++ b/gtsam/geometry/CalibratedCameraT.h @@ -34,7 +34,9 @@ namespace gtsam { CalibratedCameraT(const Vector &v, const Vector &k):pose_(Pose3::Expmap(v)),k_(k){} virtual ~CalibratedCameraT() {} + inline Pose3& pose() { return pose_; } inline const Pose3& pose() const { return pose_; } + inline Calibration& calibration() { return k_; } 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) ; @@ -55,12 +57,10 @@ namespace gtsam { 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()) ; -// } + void print(const std::string& s = "") const { + pose_.print("pose3"); + k_.print("calibration"); + } inline size_t dim() const { return 6 ; } inline static size_t Dim() { return 6 ; } @@ -83,58 +83,44 @@ namespace gtsam { * @param point a 3D point to be projected * @return the intrinsic coordinates of the projected point */ - Point2 project(const Point3& point, + + inline Point2 project(const Point3& point, boost::optional D_intrinsic_pose = boost::none, boost::optional D_intrinsic_point = boost::none) const { + std::pair result = projectSafe(point, D_intrinsic_pose, D_intrinsic_point) ; + return result.first ; + } + + std::pair projectSafe( + const Point3& pw, + 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) ; + Point3 pc = pose_.transform_to(pw) ; Point2 pn = project_to_camera(pc) ; - return k_.uncalibrate(pn) ; + return std::make_pair(k_.uncalibrate(pn), pc.z() > 0) ; } - // world to camera coordinate Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ; - Point3 pc = pose_.transform_to(point, Hc1, Hc2) ; - + Point3 pc = pose_.transform_to(pw, 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 ; + if (D_intrinsic_pose) *D_intrinsic_pose = tmp * Hc1 ; + if (D_intrinsic_point) *D_intrinsic_point = tmp * Hc2 ; + return std::make_pair(pi, pc.z()>0) ; } - 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 -// */ + /** + * 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) { @@ -147,7 +133,7 @@ namespace gtsam { /** * backproject a 2-dimensional point to a 3-dimension point */ - Point3 backproject_from_camera(const Point2& pi, const double scale) { + Point3 backproject_from_camera(const Point2& pi, const double scale) const { Point2 pn = k_.calibrate(pi); Point3 pc(pn.x()*scale, pn.y()*scale, scale); return pose_.transform_from(pc); @@ -161,8 +147,60 @@ private: ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(k_); } - }; } +// static CalibratedCameraT Expmap(const Vector& v) { +// return CalibratedCameraT(Pose3::Expmap(v), k_) ; +// } +// static Vector Logmap(const CalibratedCameraT& p) { +// return Pose3::Logmap(p.pose()) ; +// } + +// 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) ); +// }