clean redundant header files
							parent
							
								
									22ebe16a31
								
							
						
					
					
						commit
						43fa8faa07
					
				| 
						 | 
				
			
			@ -1,272 +0,0 @@
 | 
			
		|||
///**
 | 
			
		||||
// * @file CalibratedCameraT.h
 | 
			
		||||
// * @date Mar 5, 2011
 | 
			
		||||
// * @author Yong-Dian Jian
 | 
			
		||||
// * @brief calibrated camera template
 | 
			
		||||
// */
 | 
			
		||||
//
 | 
			
		||||
//#pragma once
 | 
			
		||||
//
 | 
			
		||||
//#include <boost/optional.hpp>
 | 
			
		||||
//#include <gtsam/geometry/Pose2.h>
 | 
			
		||||
//#include <gtsam/geometry/Pose3.h>
 | 
			
		||||
//
 | 
			
		||||
//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 <typename Calibration>
 | 
			
		||||
//  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<Matrix&> D_intrinsic_pose = boost::none,
 | 
			
		||||
//      boost::optional<Matrix&> D_intrinsic_point = boost::none) const {
 | 
			
		||||
//      std::pair<Point2,bool> result = projectSafe(point, D_intrinsic_pose, D_intrinsic_point) ;
 | 
			
		||||
//      return result.first ;
 | 
			
		||||
//    }
 | 
			
		||||
//
 | 
			
		||||
//    ///TODO: comment
 | 
			
		||||
//    std::pair<Point2,bool> projectSafe(
 | 
			
		||||
//        const Point3& pw,
 | 
			
		||||
//        boost::optional<Matrix&> D_intrinsic_pose = boost::none,
 | 
			
		||||
//        boost::optional<Matrix&> 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<Matrix&> 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<class Archive>
 | 
			
		||||
//      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<Matrix&> D_intrinsic_pose = boost::none,
 | 
			
		||||
////      boost::optional<Matrix&> 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<Point2,bool> projectSafe(
 | 
			
		||||
////        const Point3& pw,
 | 
			
		||||
////        boost::optional<Matrix&> H1 = boost::none,
 | 
			
		||||
////        boost::optional<Matrix&> H2 = boost::none) const {
 | 
			
		||||
////        Point3 pc = pose_.transform_to(pw);
 | 
			
		||||
////      return std::pair<Point2, bool>( project(pw,H1,H2), pc.z() > 0);
 | 
			
		||||
////    }
 | 
			
		||||
////
 | 
			
		||||
////    std::pair<Point2,bool> projectSafe(
 | 
			
		||||
////        const Point3& pw,
 | 
			
		||||
////        const Point3& pw_normal,
 | 
			
		||||
////        boost::optional<Matrix&> H1 = boost::none,
 | 
			
		||||
////        boost::optional<Matrix&> H2 = boost::none) const {
 | 
			
		||||
////      Point3 pc = pose_.transform_to(pw);
 | 
			
		||||
////      Point3 pc_normal = pose_.rotation().unrotate(pw_normal);
 | 
			
		||||
////      return std::pair<Point2, bool>( project(pw,H1,H2), (pc.z() > 0) && (pc_normal.z() < -0.5) );
 | 
			
		||||
////    }
 | 
			
		||||
//
 | 
			
		||||
| 
						 | 
				
			
			@ -1,261 +0,0 @@
 | 
			
		|||
///* ----------------------------------------------------------------------------
 | 
			
		||||
//
 | 
			
		||||
// * 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 <gtsam/geometry/CalibratedCamera.h>
 | 
			
		||||
//#include <gtsam/geometry/Cal3_S2.h>
 | 
			
		||||
//#include <gtsam/geometry/Cal3Bundler.h>
 | 
			
		||||
//#include <gtsam/geometry/Cal3DS2.h>
 | 
			
		||||
//
 | 
			
		||||
//namespace gtsam {
 | 
			
		||||
//
 | 
			
		||||
///**
 | 
			
		||||
// * General camera template
 | 
			
		||||
// * @ingroup geometry
 | 
			
		||||
// * \nosubgrouping
 | 
			
		||||
// */
 | 
			
		||||
//template <typename Camera, typename Calibration>
 | 
			
		||||
//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<Point2,bool> projectSafe(
 | 
			
		||||
//			const Point3& P,
 | 
			
		||||
//			boost::optional<Matrix&> H1 = boost::none,
 | 
			
		||||
//			boost::optional<Matrix&> H2 = boost::none) const {
 | 
			
		||||
//
 | 
			
		||||
//		Point3 cameraPoint = calibrated_.pose().transform_to(P);
 | 
			
		||||
//		return std::pair<Point2, bool>(
 | 
			
		||||
//				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<Matrix&> H1 = boost::none,
 | 
			
		||||
//			boost::optional<Matrix&> 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<class Archive>
 | 
			
		||||
//
 | 
			
		||||
//	/// Serialization function
 | 
			
		||||
//	void serialize(Archive & ar, const unsigned int version)
 | 
			
		||||
//	{
 | 
			
		||||
//		ar & BOOST_SERIALIZATION_NVP(calibrated_);
 | 
			
		||||
//		ar & BOOST_SERIALIZATION_NVP(calibration_);
 | 
			
		||||
//	}
 | 
			
		||||
//
 | 
			
		||||
//	/// @}
 | 
			
		||||
//
 | 
			
		||||
//};
 | 
			
		||||
//
 | 
			
		||||
//typedef GeneralCameraT<CalibratedCamera,Cal3Bundler> Cal3BundlerCamera;
 | 
			
		||||
//typedef GeneralCameraT<CalibratedCamera,Cal3DS2> Cal3DS2Camera; // NOTE: Typedef not referenced in gtsam
 | 
			
		||||
//typedef GeneralCameraT<CalibratedCamera,Cal3_S2> Cal3_S2Camera; // NOTE: Typedef not referenced in gtsam
 | 
			
		||||
//
 | 
			
		||||
//}
 | 
			
		||||
		Loading…
	
		Reference in New Issue