12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add member functions

release/4.3a0
Yong-Dian Jian 2011-04-04 03:55:50 +00:00
parent 53bb7584b6
commit b1922464de
2 changed files with 94 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Cal3_S2.h
View File

@ -104,6 +104,19 @@ namespace gtsam {
return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); 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 * convert intrinsic coordinates xy to image coordinates uv
* with optional derivatives * with optional derivatives

124
gtsam/geometry/CalibratedCameraT.h
View File

@ -34,7 +34,9 @@ namespace gtsam {
CalibratedCameraT(const Vector &v, const Vector &k):pose_(Pose3::Expmap(v)),k_(k){} CalibratedCameraT(const Vector &v, const Vector &k):pose_(Pose3::Expmap(v)),k_(k){}
virtual ~CalibratedCameraT() {} virtual ~CalibratedCameraT() {}
inline Pose3& pose() { return pose_; }
inline const Pose3& pose() const { return pose_; } inline const Pose3& pose() const { return pose_; }
inline Calibration& calibration() { return k_; }
inline const Calibration& calibration() const { return k_; } inline const Calibration& calibration() const { return k_; }
bool equals (const CalibratedCameraT &camera, double tol = 1e-9) const { bool equals (const CalibratedCameraT &camera, double tol = 1e-9) const {
return pose_.equals(camera.pose(), tol) && k_.equals(camera.calibration(), tol) ; return pose_.equals(camera.pose(), tol) && k_.equals(camera.calibration(), tol) ;
@ -55,12 +57,10 @@ namespace gtsam {
return pose().logmap(T2.pose()) ; return pose().logmap(T2.pose()) ;
} }
// static CalibratedCameraT Expmap(const Vector& v) { void print(const std::string& s = "") const {
// return CalibratedCameraT(Pose3::Expmap(v), k_) ; pose_.print("pose3");
// } k_.print("calibration");
// static Vector Logmap(const CalibratedCameraT& p) { }
// return Pose3::Logmap(p.pose()) ;
// }
inline size_t dim() const { return 6 ; } inline size_t dim() const { return 6 ; }
inline static size_t Dim() { return 6 ; } inline static size_t Dim() { return 6 ; }
@ -83,58 +83,44 @@ namespace gtsam {
* @param point a 3D point to be projected * @param point a 3D point to be projected
* @return the intrinsic coordinates of the projected point * @return the intrinsic coordinates of the projected point
*/ */
Point2 project(const Point3& point,
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 ;
}
std::pair<Point2,bool> projectSafe(
const Point3& pw,
boost::optional<Matrix&> D_intrinsic_pose = boost::none, boost::optional<Matrix&> D_intrinsic_pose = boost::none,
boost::optional<Matrix&> D_intrinsic_point = boost::none) const { boost::optional<Matrix&> D_intrinsic_point = boost::none) const {
// no derivative is necessary
if ( !D_intrinsic_pose && !D_intrinsic_point ) { 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) ; Point2 pn = project_to_camera(pc) ;
return k_.uncalibrate(pn) ; return std::make_pair(k_.uncalibrate(pn), pc.z() > 0) ;
} }
// world to camera coordinate // world to camera coordinate
Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ; 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 // camera to normalized image coordinate
Matrix Hn; // 2*3 Matrix Hn; // 2*3
Point2 pn = project_to_camera(pc, Hn) ; Point2 pn = project_to_camera(pc, Hn) ;
// uncalibration // uncalibration
Matrix Hi; // 2*2 Matrix Hi; // 2*2
Point2 pi = k_.uncalibrate(pn,boost::none,Hi); Point2 pi = k_.uncalibrate(pn,boost::none,Hi);
Matrix tmp = Hi*Hn ; Matrix tmp = Hi*Hn ;
*D_intrinsic_pose = tmp * Hc1 ; if (D_intrinsic_pose) *D_intrinsic_pose = tmp * Hc1 ;
*D_intrinsic_point = tmp * Hc2 ; if (D_intrinsic_point) *D_intrinsic_point = tmp * Hc2 ;
return pi ; return std::make_pair(pi, pc.z()>0) ;
} }
std::pair<Point2,bool> projectSafe( /**
const Point3& pw, * projects a 3-dimensional point in camera coordinates into the
boost::optional<Matrix&> H1 = boost::none, * camera and returns a 2-dimensional point, no calibration applied
boost::optional<Matrix&> H2 = boost::none) const { * With optional 2by3 derivative
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) );
}
// /**
// * 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, static Point2 project_to_camera(const Point3& P,
boost::optional<Matrix&> H1 = boost::none){ boost::optional<Matrix&> H1 = boost::none){
if (H1) { if (H1) {
@ -147,7 +133,7 @@ namespace gtsam {
/** /**
* backproject a 2-dimensional point to a 3-dimension point * 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); Point2 pn = k_.calibrate(pi);
Point3 pc(pn.x()*scale, pn.y()*scale, scale); Point3 pc(pn.x()*scale, pn.y()*scale, scale);
return pose_.transform_from(pc); return pose_.transform_from(pc);
@ -161,8 +147,60 @@ private:
ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(pose_);
ar & BOOST_SERIALIZATION_NVP(k_); 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<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) );
// }