diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 120f8dc8b..d7640cc15 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -49,6 +49,9 @@ namespace gtsam { fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) { } + Cal3_S2(const Vector &d): fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)){} + + /** * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect * @param fov field of view in degrees diff --git a/gtsam/geometry/CalibratedCameraT.h b/gtsam/geometry/CalibratedCameraT.h index 6a70905a4..0e316af86 100644 --- a/gtsam/geometry/CalibratedCameraT.h +++ b/gtsam/geometry/CalibratedCameraT.h @@ -1,272 +1,272 @@ -/** - * @file CalibratedCameraT.h - * @date Mar 5, 2011 - * @author Yong-Dian Jian - * @brief calibrated camera template - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - - /** - * 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. - * AGC: Is this used or tested anywhere? - * AGC: If this is a "CalibratedCamera," why is there a calibration stored internally? - * @ingroup geometry - * \nosubgrouping - */ - template - class CalibratedCameraT { - private: - Pose3 pose_; // 6DOF pose - Calibration k_; - - public: - - /// @name Standard Constructors - /// @{ - - ///TODO: comment - CalibratedCameraT() {} - - ///TODO: comment - CalibratedCameraT(const Pose3& pose):pose_(pose){} - - ///TODO: comment - CalibratedCameraT(const Pose3& pose, const Calibration& k):pose_(pose),k_(k) {} - - /// @} - /// @name Advanced Constructors - /// @{ - - ///TODO: comment - CalibratedCameraT(const Vector &v): pose_(Pose3::Expmap(v)) {} - - ///TODO: comment - CalibratedCameraT(const Vector &v, const Vector &k):pose_(Pose3::Expmap(v)),k_(k){} - - /// @} - /// @name Standard Interface - /// @{ - - virtual ~CalibratedCameraT() {} - - ///TODO: comment - inline Pose3& pose() { return pose_; } - - ///TODO: comment - inline const Pose3& pose() const { return pose_; } - - ///TODO: comment - inline Calibration& calibration() { return k_; } - - ///TODO: comment - inline const Calibration& calibration() const { return k_; } - - ///TODO: comment - inline const CalibratedCameraT compose(const CalibratedCameraT &c) const { - return CalibratedCameraT( pose_ * c.pose(), k_ ) ; - } - - ///TODO: comment - inline const CalibratedCameraT inverse() const { - return CalibratedCameraT( pose_.inverse(), k_ ) ; - } - - /// @} - /// @name Testable - /// @{ - - /// assert equality up to a tolerance - bool equals (const CalibratedCameraT &camera, double tol = 1e-9) const { - return pose_.equals(camera.pose(), tol) && k_.equals(camera.calibration(), tol) ; - } - - /// print - void print(const std::string& s = "") const { - pose_.print("pose3"); - k_.print("calibration"); - } - - /// @} - /// @name Manifold - /// @{ - - ///TODO: comment - CalibratedCameraT retract(const Vector& d) const { - return CalibratedCameraT(pose().retract(d), k_) ; - } - - ///TODO: comment - Vector localCoordinates(const CalibratedCameraT& T2) const { - return pose().localCoordinates(T2.pose()) ; - } - - ///TODO: comment - inline size_t dim() const { return 6 ; } //TODO: add final dimension variable? - - ///TODO: comment - inline static size_t Dim() { return 6 ; } //TODO: add final dimension variable? - - //TODO: remove comment and method? - /** - * 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 - */ - - /// @} - /// @name Transformations - /// @{ - - ///TODO: comment - 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 ; - } - - ///TODO: comment - std::pair projectSafe( - const Point3& pw, - boost::optional D_intrinsic_pose = boost::none, - boost::optional D_intrinsic_point = boost::none) const { - - if ( !D_intrinsic_pose && !D_intrinsic_point ) { - Point3 pc = pose_.transform_to(pw) ; - Point2 pn = project_to_camera(pc) ; - 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(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 ; - 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) ; - } - - /** - * 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) const { - Point2 pn = k_.calibrate(pi); - Point3 pc(pn.x()*scale, pn.y()*scale, scale); - return pose_.transform_from(pc); - } - -private: - - /// @} - /// @name Advanced Interface - /// @{ - - /** 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_); - } - - /// @} - }; -} - -//TODO: remove? - -// static CalibratedCameraT Expmap(const Vector& v) { -// return CalibratedCameraT(Pose3::Expmap(v), k_) ; +///** +// * @file CalibratedCameraT.h +// * @date Mar 5, 2011 +// * @author Yong-Dian Jian +// * @brief calibrated camera template +// */ +// +//#pragma once +// +//#include +//#include +//#include +// +//namespace gtsam { +// +// /** +// * 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. +// * AGC: Is this used or tested anywhere? +// * AGC: If this is a "CalibratedCamera," why is there a calibration stored internally? +// * @ingroup geometry +// * \nosubgrouping +// */ +// template +// class CalibratedCameraT { +// private: +// Pose3 pose_; // 6DOF pose +// Calibration k_; +// +// public: +// +// /// @name Standard Constructors +// /// @{ +// +// ///TODO: comment +// CalibratedCameraT() {} +// +// ///TODO: comment +// CalibratedCameraT(const Pose3& pose):pose_(pose){} +// +// ///TODO: comment +// CalibratedCameraT(const Pose3& pose, const Calibration& k):pose_(pose),k_(k) {} +// +// /// @} +// /// @name Advanced Constructors +// /// @{ +// +// ///TODO: comment +// CalibratedCameraT(const Vector &v): pose_(Pose3::Expmap(v)) {} +// +// ///TODO: comment +// CalibratedCameraT(const Vector &v, const Vector &k):pose_(Pose3::Expmap(v)),k_(k){} +// +// /// @} +// /// @name Standard Interface +// /// @{ +// +// virtual ~CalibratedCameraT() {} +// +// ///TODO: comment +// inline Pose3& pose() { return pose_; } +// +// ///TODO: comment +// inline const Pose3& pose() const { return pose_; } +// +// ///TODO: comment +// inline Calibration& calibration() { return k_; } +// +// ///TODO: comment +// inline const Calibration& calibration() const { return k_; } +// +// ///TODO: comment +// inline const CalibratedCameraT compose(const CalibratedCameraT &c) const { +// return CalibratedCameraT( pose_ * c.pose(), k_ ) ; // } -// static Vector Logmap(const CalibratedCameraT& p) { -// return Pose3::Logmap(p.pose()) ; +// +// ///TODO: comment +// inline const CalibratedCameraT inverse() const { +// return CalibratedCameraT( pose_.inverse(), k_ ) ; // } - -// Point2 project(const Point3& point, +// +// /// @} +// /// @name Testable +// /// @{ +// +// /// assert equality up to a tolerance +// bool equals (const CalibratedCameraT &camera, double tol = 1e-9) const { +// return pose_.equals(camera.pose(), tol) && k_.equals(camera.calibration(), tol) ; +// } +// +// /// print +// void print(const std::string& s = "") const { +// pose_.print("pose3"); +// k_.print("calibration"); +// } +// +// /// @} +// /// @name Manifold +// /// @{ +// +// ///TODO: comment +// CalibratedCameraT retract(const Vector& d) const { +// return CalibratedCameraT(pose().retract(d), k_) ; +// } +// +// ///TODO: comment +// Vector localCoordinates(const CalibratedCameraT& T2) const { +// return pose().localCoordinates(T2.pose()) ; +// } +// +// ///TODO: comment +// inline size_t dim() const { return 6 ; } //TODO: add final dimension variable? +// +// ///TODO: comment +// inline static size_t Dim() { return 6 ; } //TODO: add final dimension variable? +// +// //TODO: remove comment and method? +// /** +// * 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 +// */ +// +// /// @} +// /// @name Transformations +// /// @{ +// +// ///TODO: comment +// 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 ; +// } +// +// ///TODO: comment +// 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); +// /** +// * 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()); // } // -// 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) ); +// /** +// * backproject a 2-dimensional point to a 3-dimension point +// */ +// 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); // } - +// +//private: +// +// /// @} +// /// @name Advanced Interface +// /// @{ +// +// /** 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_); +// } +// +// /// @} +// }; +//} +// +////TODO: remove? +// +//// 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) ); +//// } +// diff --git a/gtsam/geometry/GeneralCameraT.h b/gtsam/geometry/GeneralCameraT.h index d93c56472..dc2879eaa 100644 --- a/gtsam/geometry/GeneralCameraT.h +++ b/gtsam/geometry/GeneralCameraT.h @@ -1,261 +1,261 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file GeneralCameraT.h - * @brief General camera template - * @date Mar 1, 2010 - * @author Yong-Dian Jian - */ - -#pragma once - -#include -#include -#include -#include - -namespace gtsam { - -/** - * General camera template - * @ingroup geometry - * \nosubgrouping - */ -template -class GeneralCameraT { - -private: - Camera calibrated_; // Calibrated camera - Calibration calibration_; // Calibration - -public: - - /// @name Standard Constructors - /// @{ - - ///TODO: comment - GeneralCameraT(){} - - ///TODO: comment - GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {} - - ///TODO: comment - GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {} - - ///TODO: comment - GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {} - - ///TODO: comment - GeneralCameraT(const Pose3& pose) : calibrated_(pose) {} - - /// @} - /// @name Advanced Constructors - /// @{ - - ///TODO: comment - GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {} - - ///TODO: comment - GeneralCameraT(const Vector &v) : - calibrated_(sub(v, 0, Camera::Dim())), - calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {} - - /// @} - /// @name Standard Interface - /// @{ - - ///TODO: comment - inline const Pose3& pose() const { return calibrated_.pose(); } - - ///TODO: comment - inline const Camera& calibrated() const { return calibrated_; } - - ///TODO: comment - inline const Calibration& calibration() const { return calibration_; } - - ///TODO: comment - inline GeneralCameraT compose(const Pose3 &p) const { - return GeneralCameraT( pose().compose(p), calibration_ ) ; - } - - ///TODO: comment - Matrix D2d_camera(const Point3& point) const { - Point2 intrinsic = calibrated_.project(point); - Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); - Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); - Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; - Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); - - const int n1 = calibrated_.dim() ; - const int n2 = calibration_.dim() ; - Matrix D(2,n1+n2) ; - - sub(D,0,2,0,n1) = D_2d_pose ; - sub(D,0,2,n1,n1+n2) = D_2d_calibration ; - return D; - } - - ///TODO: comment - Matrix D2d_3d(const Point3& point) const { - Point2 intrinsic = calibrated_.project(point); - Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); - Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); - return D_2d_intrinsic * D_intrinsic_3d; - } - - ///TODO: comment - Matrix D2d_camera_3d(const Point3& point) const { - Point2 intrinsic = calibrated_.project(point); - Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); - Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); - Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; - Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); - - Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); - Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d; - - const int n1 = calibrated_.dim() ; - const int n2 = calibration_.dim() ; - - Matrix D(2,n1+n2+3) ; - - sub(D,0,2,0,n1) = D_2d_pose ; - sub(D,0,2,n1,n1+n2) = D_2d_calibration ; - sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ; - return D; - } - - /// @} - /// @name Transformations - /// @{ - - ///TODO: comment - std::pair projectSafe( - const Point3& P, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - - Point3 cameraPoint = calibrated_.pose().transform_to(P); - return std::pair( - project(P,H1,H2), - cameraPoint.z() > 0); - } - - ///TODO: comment - Point3 backproject(const Point2& projection, const double scale) const { - Point2 intrinsic = calibration_.calibrate(projection); - Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale); - return calibrated_.pose().transform_from(cameraPoint); - } - - /** - * project function that does not merge the Jacobians of calibration and pose - */ - Point2 project(const Point3& P, Matrix& H1_pose, Matrix& H1_k, Matrix& H2_pt) const { - Matrix tmp; - Point2 intrinsic = calibrated_.project(P, H1_pose, H2_pt); - Point2 projection = calibration_.uncalibrate(intrinsic, H1_k, tmp); - H1_pose = tmp * H1_pose; - H2_pt = tmp * H2_pt; - return projection; - } - - /** - * project a 3d point to the camera - * P is point in camera coordinate - * H1 is respect to pose + calibration - * H2 is respect to landmark - */ - Point2 project(const Point3& P, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - - if (!H1 && !H2) { - Point2 intrinsic = calibrated_.project(P); - return calibration_.uncalibrate(intrinsic) ; - } - - Matrix H1_k, H1_pose, H2_pt; - Point2 projection = project(P, H1_pose, H1_k, H2_pt); - if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k); - if ( H2 ) *H2 = H2_pt; - - return projection; - } - - /// @} - /// @name Testable - /// @{ - - /// checks equality up to a tolerance - bool equals (const GeneralCameraT &camera, double tol = 1e-9) const { - return calibrated_.equals(camera.calibrated_, tol) && - calibration_.equals(camera.calibration_, tol) ; - } - - /// print with optional string - void print(const std::string& s = "") const { - calibrated_.pose().print(s + ".camera.") ; - calibration_.print(s + ".calibration.") ; - } - - /// @} - /// @name Manifold - /// @{ - - ///TODO: comment - GeneralCameraT retract(const Vector &v) const { - Vector v1 = sub(v,0,Camera::Dim()); - Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim()); - return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2)); - } - - ///TODO: comment - Vector localCoordinates(const GeneralCameraT &C) const { - const Vector v1(calibrated().localCoordinates(C.calibrated())), - v2(calibration().localCoordinates(C.calibration())); - return concatVectors(2,&v1,&v2) ; - } - - //inline size_t dim() { return Camera::dim() + Calibration::dim() ; } - - ///TODO: comment - inline size_t dim() const { return calibrated().dim() + calibration().dim() ; } - - ///TODO: comment - static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; } - -private: - - /// @} - /// @name Advanced Interface - /// @{ - - friend class boost::serialization::access; - template - - /// Serialization function - void serialize(Archive & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(calibrated_); - ar & BOOST_SERIALIZATION_NVP(calibration_); - } - - /// @} - -}; - -typedef GeneralCameraT Cal3BundlerCamera; -typedef GeneralCameraT Cal3DS2Camera; // NOTE: Typedef not referenced in gtsam -typedef GeneralCameraT Cal3_S2Camera; // NOTE: Typedef not referenced in gtsam - -} +///* ---------------------------------------------------------------------------- +// +// * GTSAM Copyright 2010, Georgia Tech Research Corporation, +// * Atlanta, Georgia 30332-0415 +// * All Rights Reserved +// * Authors: Frank Dellaert, et al. (see THANKS for the full author list) +// +// * See LICENSE for the license information +// +// * -------------------------------------------------------------------------- */ +// +///** +// * @file GeneralCameraT.h +// * @brief General camera template +// * @date Mar 1, 2010 +// * @author Yong-Dian Jian +// */ +// +//#pragma once +// +//#include +//#include +//#include +//#include +// +//namespace gtsam { +// +///** +// * General camera template +// * @ingroup geometry +// * \nosubgrouping +// */ +//template +//class GeneralCameraT { +// +//private: +// Camera calibrated_; // Calibrated camera +// Calibration calibration_; // Calibration +// +//public: +// +// /// @name Standard Constructors +// /// @{ +// +// ///TODO: comment +// GeneralCameraT(){} +// +// ///TODO: comment +// GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {} +// +// ///TODO: comment +// GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {} +// +// ///TODO: comment +// GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {} +// +// ///TODO: comment +// GeneralCameraT(const Pose3& pose) : calibrated_(pose) {} +// +// /// @} +// /// @name Advanced Constructors +// /// @{ +// +// ///TODO: comment +// GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {} +// +// ///TODO: comment +// GeneralCameraT(const Vector &v) : +// calibrated_(sub(v, 0, Camera::Dim())), +// calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {} +// +// /// @} +// /// @name Standard Interface +// /// @{ +// +// ///TODO: comment +// inline const Pose3& pose() const { return calibrated_.pose(); } +// +// ///TODO: comment +// inline const Camera& calibrated() const { return calibrated_; } +// +// ///TODO: comment +// inline const Calibration& calibration() const { return calibration_; } +// +// ///TODO: comment +// inline GeneralCameraT compose(const Pose3 &p) const { +// return GeneralCameraT( pose().compose(p), calibration_ ) ; +// } +// +// ///TODO: comment +// Matrix D2d_camera(const Point3& point) const { +// Point2 intrinsic = calibrated_.project(point); +// Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); +// Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); +// Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; +// Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); +// +// const int n1 = calibrated_.dim() ; +// const int n2 = calibration_.dim() ; +// Matrix D(2,n1+n2) ; +// +// sub(D,0,2,0,n1) = D_2d_pose ; +// sub(D,0,2,n1,n1+n2) = D_2d_calibration ; +// return D; +// } +// +// ///TODO: comment +// Matrix D2d_3d(const Point3& point) const { +// Point2 intrinsic = calibrated_.project(point); +// Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); +// Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); +// return D_2d_intrinsic * D_intrinsic_3d; +// } +// +// ///TODO: comment +// Matrix D2d_camera_3d(const Point3& point) const { +// Point2 intrinsic = calibrated_.project(point); +// Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); +// Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); +// Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; +// Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); +// +// Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); +// Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d; +// +// const int n1 = calibrated_.dim() ; +// const int n2 = calibration_.dim() ; +// +// Matrix D(2,n1+n2+3) ; +// +// sub(D,0,2,0,n1) = D_2d_pose ; +// sub(D,0,2,n1,n1+n2) = D_2d_calibration ; +// sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ; +// return D; +// } +// +// /// @} +// /// @name Transformations +// /// @{ +// +// ///TODO: comment +// std::pair projectSafe( +// const Point3& P, +// boost::optional H1 = boost::none, +// boost::optional H2 = boost::none) const { +// +// Point3 cameraPoint = calibrated_.pose().transform_to(P); +// return std::pair( +// project(P,H1,H2), +// cameraPoint.z() > 0); +// } +// +// ///TODO: comment +// Point3 backproject(const Point2& projection, const double scale) const { +// Point2 intrinsic = calibration_.calibrate(projection); +// Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale); +// return calibrated_.pose().transform_from(cameraPoint); +// } +// +// /** +// * project function that does not merge the Jacobians of calibration and pose +// */ +// Point2 project(const Point3& P, Matrix& H1_pose, Matrix& H1_k, Matrix& H2_pt) const { +// Matrix tmp; +// Point2 intrinsic = calibrated_.project(P, H1_pose, H2_pt); +// Point2 projection = calibration_.uncalibrate(intrinsic, H1_k, tmp); +// H1_pose = tmp * H1_pose; +// H2_pt = tmp * H2_pt; +// return projection; +// } +// +// /** +// * project a 3d point to the camera +// * P is point in camera coordinate +// * H1 is respect to pose + calibration +// * H2 is respect to landmark +// */ +// Point2 project(const Point3& P, +// boost::optional H1 = boost::none, +// boost::optional H2 = boost::none) const { +// +// if (!H1 && !H2) { +// Point2 intrinsic = calibrated_.project(P); +// return calibration_.uncalibrate(intrinsic) ; +// } +// +// Matrix H1_k, H1_pose, H2_pt; +// Point2 projection = project(P, H1_pose, H1_k, H2_pt); +// if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k); +// if ( H2 ) *H2 = H2_pt; +// +// return projection; +// } +// +// /// @} +// /// @name Testable +// /// @{ +// +// /// checks equality up to a tolerance +// bool equals (const GeneralCameraT &camera, double tol = 1e-9) const { +// return calibrated_.equals(camera.calibrated_, tol) && +// calibration_.equals(camera.calibration_, tol) ; +// } +// +// /// print with optional string +// void print(const std::string& s = "") const { +// calibrated_.pose().print(s + ".camera.") ; +// calibration_.print(s + ".calibration.") ; +// } +// +// /// @} +// /// @name Manifold +// /// @{ +// +// ///TODO: comment +// GeneralCameraT retract(const Vector &v) const { +// Vector v1 = sub(v,0,Camera::Dim()); +// Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim()); +// return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2)); +// } +// +// ///TODO: comment +// Vector localCoordinates(const GeneralCameraT &C) const { +// const Vector v1(calibrated().localCoordinates(C.calibrated())), +// v2(calibration().localCoordinates(C.calibration())); +// return concatVectors(2,&v1,&v2) ; +// } +// +// //inline size_t dim() { return Camera::dim() + Calibration::dim() ; } +// +// ///TODO: comment +// inline size_t dim() const { return calibrated().dim() + calibration().dim() ; } +// +// ///TODO: comment +// static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; } +// +//private: +// +// /// @} +// /// @name Advanced Interface +// /// @{ +// +// friend class boost::serialization::access; +// template +// +// /// Serialization function +// void serialize(Archive & ar, const unsigned int version) +// { +// ar & BOOST_SERIALIZATION_NVP(calibrated_); +// ar & BOOST_SERIALIZATION_NVP(calibration_); +// } +// +// /// @} +// +//}; +// +//typedef GeneralCameraT Cal3BundlerCamera; +//typedef GeneralCameraT Cal3DS2Camera; // NOTE: Typedef not referenced in gtsam +//typedef GeneralCameraT Cal3_S2Camera; // NOTE: Typedef not referenced in gtsam +// +//} diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h new file mode 100644 index 000000000..bc0364970 --- /dev/null +++ b/gtsam/geometry/PinholeCamera.h @@ -0,0 +1,286 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PinholeCamera.h + * @brief Base class for all pinhole cameras + * @author Yong-Dian Jian + * @date Jan 27, 2012 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + + /** + * A pinhole camera class that has a Pose3 and a Calibration. + * @ingroup geometry + * \nosubgrouping + */ + template + class PinholeCamera { + private: + Pose3 pose_; + Calibration k_; + + public: + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + PinholeCamera() {} + + /** constructor with pose */ + PinholeCamera(const Pose3& pose):pose_(pose){} + + /** constructor with pose and calibration */ + PinholeCamera(const Pose3& pose, const Calibration& k):pose_(pose),k_(k) {} + + /** alternative constructor with pose and calibration */ + PinholeCamera(const Calibration& k, const Pose3& pose):pose_(pose),k_(k) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + PinholeCamera(const Vector &v){ + pose_ = Pose3::Expmap(v.head(Pose3::Dim())); + if ( v.size() > Pose3::Dim()) { + k_ = Calibration(v.tail(Calibration::Dim())); + } + } + + PinholeCamera(const Vector &v, const Vector &k) : pose_(Pose3::Expmap(v)),k_(k){} + + /// @} + /// @name Standard Interface + /// @{ + + virtual ~PinholeCamera() {} + + /// return pose + inline Pose3& pose() { return pose_; } + + /// return pose + inline const Pose3& pose() const { return pose_; } + + /// return calibration + inline Calibration& calibration() { return k_; } + + /// return calibration + inline const Calibration& calibration() const { return k_; } + + /// compose two cameras + inline const PinholeCamera compose(const Pose3 &c) const { + return PinholeCamera( pose_ * c, k_ ) ; + } + + /// inverse + inline const PinholeCamera inverse() const { + return PinholeCamera( pose_.inverse(), k_ ) ; + } + + /// @} + /// @name Testable + /// @{ + + /// assert equality up to a tolerance + bool equals (const PinholeCamera &camera, double tol = 1e-9) const { + return pose_.equals(camera.pose(), tol) && + k_.equals(camera.calibration(), tol) ; + } + + /// print + void print(const std::string& s = "") const { + pose_.print("pose3"); + k_.print("calibration"); + } + + /// @} + /// @name Manifold + /// @{ + + /// move a cameras according to d + PinholeCamera retract(const Vector& d) const { + if ( d.size() == pose_.dim() ) + return PinholeCamera(pose().retract(d), calibration()) ; + else + return PinholeCamera(pose().retract(d.head(pose().dim())), + calibration().retract(d.tail(calibration().dim()))) ; + } + + /// return canonical coordinate + Vector localCoordinates(const PinholeCamera& T2) const { + Vector d(dim()); + d.head(pose().dim()) = pose().localCoordinates(T2.pose()); + d.tail(calibration().dim()) = calibration().localCoordinates(T2.calibration()); + return d; + } + + /// Lie group dimensionality + inline size_t dim() const { return pose_.dim() + k_.dim(); } + + /// Lie group dimensionality + inline static size_t Dim() { return Pose3::Dim() + Calibration::Dim(); } + + /** + * 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 PinholeCamera level(const Pose2& pose2, double height) { + return PinholeCamera::level(Calibration(), pose2, height); + } + + static PinholeCamera level(const Calibration &K, 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); + const Point3 t(pose2.x(), pose2.y(), height); + const Pose3 pose3(wRc, t); + return PinholeCamera(pose3, K); + } + + /* ************************************************************************* */ + // measurement functions and derivatives + /* ************************************************************************* */ + + /** + * projects a 3-dimensional point in camera coordinates into the + * camera and returns a 2-dimensional point, no calibration applied + * With optional 2by3 derivative + */ + inline 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()); + } + + /// Project a point into the image and check depth + inline 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(k_.uncalibrate(pn), pc.z()>0); + } + + /// @} + /// @name Transformations + /// @{ + + /** project a point from world coordinate to the image + * @param pw is a point in the world coordinate + * @param H1 is the jacobian w.r.t. pose3 + * @param H2 is the jacobian w.r.t. point3 + * @param H3 is the jacobian w.r.t. calibration + */ + inline Point2 project(const Point3& pw, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const { + + if (!H1 && !H2 && !H3) { + const Point3 pc = pose_.transform_to(pw) ; + if ( pc.z() <= 0 ) throw CheiralityException(); + const Point2 pn = project_to_camera(pc) ; + return k_.uncalibrate(pn); + } + + // world to camera coordinate + Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ; + const Point3 pc = pose_.transform_to(pw, Hc1, Hc2) ; + if( pc.z() <= 0 ) throw CheiralityException(); + + // camera to normalized image coordinate + Matrix Hn; // 2*3 + const Point2 pn = project_to_camera(pc, Hn) ; + + // uncalibration + Matrix Hk, Hi; // 2*2 + const Point2 pi = k_.uncalibrate(pn, Hk, Hi); + + // chain the jacobian matrices + const Matrix tmp = Hi*Hn ; + if (H1) *H1 = tmp * Hc1 ; + if (H2) *H2 = tmp * Hc2 ; + if (H3) *H3 = Hk; + return pi; + } + + /** project a point from world coordinate to the image + * @param pw is a point in the world coordinate + * @param H1 is the jacobian w.r.t. [pose3 calibration] + * @param H2 is the jacobian w.r.t. point3 + */ + inline Point2 project2(const Point3& pw, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + + if (!H1 && !H2) { + const Point3 pc = pose_.transform_to(pw) ; + if ( pc.z() <= 0 ) throw CheiralityException(); + const Point2 pn = project_to_camera(pc) ; + return k_.uncalibrate(pn); + } + + Matrix Htmp1, Htmp2, Htmp3; + const Point2 pi = this->project(pw, Htmp1, Htmp2, Htmp3); + if (H1) { + *H1 = Matrix(2, this->dim()); + H1->leftCols(pose().dim()) = Htmp1 ; // jacobian wrt pose3 + H1->rightCols(calibration().dim()) = Htmp3 ; // jacobian wrt calib + } + if (H2) *H2 = Htmp2; + return pi; + } + + /** + * backproject a 2-dimensional point to a 3-dimension point + */ + + inline Point3 backproject(const Point2& pi, const double scale) const { + const Point2 pn = k_.calibrate(pi); + const Point3 pc(pn.x()*scale, pn.y()*scale, scale); + return pose_.transform_from(pc); + } + + inline Point3 backproject_from_camera(const Point2& pi, const double scale) const { + return backproject(pi, scale); + } + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + /** 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_); + } + /// @} + }; +} diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index 5c9859e60..31a18ab99 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -1,72 +1,72 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file SimpleCamera.cpp - * @date Aug 16, 2009 - * @author dellaert - */ - -#include -#include - -using namespace std; - -namespace gtsam { - - /* ************************************************************************* */ - - SimpleCamera::SimpleCamera(const Cal3_S2& K, - const CalibratedCamera& calibrated) : - calibrated_(calibrated), K_(K) { - } - - SimpleCamera::SimpleCamera(const Cal3_S2& K, const Pose3& pose) : - calibrated_(pose), K_(K) { - } - - SimpleCamera::~SimpleCamera() { - } - - pair SimpleCamera::projectSafe(const Point3& P) const { - Point3 cameraPoint = calibrated_.pose().transform_to(P); - Point2 intrinsic = CalibratedCamera::project_to_camera(cameraPoint); - Point2 projection = K_.uncalibrate(intrinsic); - return pair(projection, cameraPoint.z() > 0); - } - - Point2 SimpleCamera::project(const Point3& point, - boost::optional H1, boost::optional H2) const { - - Point2 intrinsic = calibrated_.project(point, H1, H2); - if (!H1 && !H2) - return K_.uncalibrate(intrinsic); - else { - Matrix D_projection_intrinsic; - Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic); - if (H1) *H1 = D_projection_intrinsic * (*H1); - if (H2) *H2 = D_projection_intrinsic * (*H2); - return projection; - } - } - - Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const { - Point2 intrinsic = K_.calibrate(projection); - Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale); - return calibrated_.pose().transform_from(cameraPoint); - } - - SimpleCamera SimpleCamera::level(const Cal3_S2& K, const Pose2& pose2, double height) { - return SimpleCamera(K, CalibratedCamera::level(pose2, height)); - } - -/* ************************************************************************* */ -} +///* ---------------------------------------------------------------------------- +// +// * GTSAM Copyright 2010, Georgia Tech Research Corporation, +// * Atlanta, Georgia 30332-0415 +// * All Rights Reserved +// * Authors: Frank Dellaert, et al. (see THANKS for the full author list) +// +// * See LICENSE for the license information +// +// * -------------------------------------------------------------------------- */ +// +///** +// * @file SimpleCamera.cpp +// * @date Aug 16, 2009 +// * @author dellaert +// */ +// +//#include +//#include +// +//using namespace std; +// +//namespace gtsam { +// +// /* ************************************************************************* */ +// +// SimpleCamera::SimpleCamera(const Cal3_S2& K, +// const CalibratedCamera& calibrated) : +// calibrated_(calibrated), K_(K) { +// } +// +// SimpleCamera::SimpleCamera(const Cal3_S2& K, const Pose3& pose) : +// calibrated_(pose), K_(K) { +// } +// +// SimpleCamera::~SimpleCamera() { +// } +// +// pair SimpleCamera::projectSafe(const Point3& P) const { +// Point3 cameraPoint = calibrated_.pose().transform_to(P); +// Point2 intrinsic = CalibratedCamera::project_to_camera(cameraPoint); +// Point2 projection = K_.uncalibrate(intrinsic); +// return pair(projection, cameraPoint.z() > 0); +// } +// +// Point2 SimpleCamera::project(const Point3& point, +// boost::optional H1, boost::optional H2) const { +// +// Point2 intrinsic = calibrated_.project(point, H1, H2); +// if (!H1 && !H2) +// return K_.uncalibrate(intrinsic); +// else { +// Matrix D_projection_intrinsic; +// Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic); +// if (H1) *H1 = D_projection_intrinsic * (*H1); +// if (H2) *H2 = D_projection_intrinsic * (*H2); +// return projection; +// } +// } +// +// Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const { +// Point2 intrinsic = K_.calibrate(projection); +// Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale); +// return calibrated_.pose().transform_from(cameraPoint); +// } +// +// SimpleCamera SimpleCamera::level(const Cal3_S2& K, const Pose2& pose2, double height) { +// return SimpleCamera(K, CalibratedCamera::level(pose2, height)); +// } +// +///* ************************************************************************* */ +//} diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 88eabb0f5..b6d060192 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -18,77 +18,84 @@ #pragma once -#include +#include #include namespace gtsam { - - /** - * A simple camera class with a Cal3_S2 calibration - * Basically takes a Calibrated camera and adds calibration information - * to produce measurements in pixels. - * Not a sublass as a SimpleCamera *is not* a CalibratedCamera. - */ - class SimpleCamera { - private: - CalibratedCamera calibrated_; // Calibrated camera - Cal3_S2 K_; // Calibration - - public: - SimpleCamera(const Cal3_S2& K, const CalibratedCamera& calibrated); - SimpleCamera(const Cal3_S2& K, const Pose3& pose); - - /** constructor for serialization */ - SimpleCamera(){} - - virtual ~SimpleCamera(); - - const Pose3& pose() const { - return calibrated_.pose(); - } - - const Cal3_S2& calibration() const { - return K_; - } - - /** - * project a 3d point to the camera and also check the depth - */ - std::pair projectSafe(const Point3& P) const; - - /** - * backproject a 2d point from the camera up to a given scale - */ - Point3 backproject(const Point2& projection, const double scale) const; - - /** - * 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 SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height); - - /** - * This function receives the camera pose and the landmark location and - * returns the location the point is supposed to appear in the image - */ - Point2 project(const Point3& point, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; - - bool equals(const SimpleCamera& X, double tol=1e-9) const { - return calibrated_.equals(X.calibrated_, tol) && K_.equals(X.K_, tol); - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(calibrated_); - ar & BOOST_SERIALIZATION_NVP(K_); - } - - }; - + typedef PinholeCamera SimpleCamera; } + +//#include +//#include +// +//namespace gtsam { +// +// /** +// * A simple camera class with a Cal3_S2 calibration +// * Basically takes a Calibrated camera and adds calibration information +// * to produce measurements in pixels. +// * Not a sublass as a SimpleCamera *is not* a CalibratedCamera. +// */ +// class SimpleCamera { +// private: +// CalibratedCamera calibrated_; // Calibrated camera +// Cal3_S2 K_; // Calibration +// +// public: +// SimpleCamera(const Cal3_S2& K, const CalibratedCamera& calibrated); +// SimpleCamera(const Cal3_S2& K, const Pose3& pose); +// +// /** constructor for serialization */ +// SimpleCamera(){} +// +// virtual ~SimpleCamera(); +// +// const Pose3& pose() const { +// return calibrated_.pose(); +// } +// +// const Cal3_S2& calibration() const { +// return K_; +// } +// +// /** +// * project a 3d point to the camera and also check the depth +// */ +// std::pair projectSafe(const Point3& P) const; +// +// /** +// * backproject a 2d point from the camera up to a given scale +// */ +// Point3 backproject(const Point2& projection, const double scale) const; +// +// /** +// * 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 SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height); +// +// /** +// * This function receives the camera pose and the landmark location and +// * returns the location the point is supposed to appear in the image +// */ +// Point2 project(const Point3& point, +// boost::optional H1 = boost::none, +// boost::optional H2 = boost::none) const; +// +// bool equals(const SimpleCamera& X, double tol=1e-9) const { +// return calibrated_.equals(X.calibrated_, tol) && K_.equals(X.K_, tol); +// } +// +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE & ar, const unsigned int version) { +// ar & BOOST_SERIALIZATION_NVP(calibrated_); +// ar & BOOST_SERIALIZATION_NVP(K_); +// } +// +// }; +// +//} diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 66ec820c4..ee3bdb38d 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -20,6 +20,7 @@ #pragma once +#include #include @@ -82,7 +83,7 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const { - Vector error = z_.localCoordinates(camera.project(point,H1,H2)); + Vector error = z_.localCoordinates(camera.project2(point,H1,H2)); // gtsam::print(error, "error"); return error; } diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index dc53c098c..7825e0b75 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -16,20 +16,19 @@ using namespace boost; #define GTSAM_MAGIC_KEY #include +#include +#include #include #include #include #include #include - -#include #include using namespace std; using namespace gtsam; -typedef Cal3_S2Camera GeneralCamera; -//typedef Cal3BundlerCamera GeneralCamera; +typedef PinholeCamera GeneralCamera; typedef TypedSymbol CameraKey; typedef TypedSymbol PointKey; typedef Values CameraConfig; @@ -341,6 +340,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer2 = optimizer.levenbergMarquardt(); + EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements); } diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index c75450cfa..40385c84f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -16,19 +16,19 @@ using namespace boost; #define GTSAM_MAGIC_KEY #include +#include +#include #include #include #include #include #include - -#include #include using namespace std; using namespace gtsam; -typedef Cal3BundlerCamera GeneralCamera; +typedef PinholeCamera GeneralCamera; typedef TypedSymbol CameraKey; typedef TypedSymbol PointKey; typedef Values CameraConfig;