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

moved projectPointAtInfinity down

release/4.3a0
dellaert 2015-02-23 12:37:55 +01:00
parent 0fee8f37a6
commit fb47cf8961
2 changed files with 58 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam/geometry/PinholeCamera.h
View File

@ -31,14 +31,6 @@ namespace gtsam {
template<typename Calibration>
class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
public:
/**
* Some classes template on either PinholeCamera or StereoCamera,
* and this typedef informs those classes what "project" returns.
*/
typedef Point2 Measurement;
private:
typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
@ -235,47 +227,6 @@ public:
return pi;
}
/** project a point at infinity from world coordinate to the image
* @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf)
* @param Dpose is the Jacobian w.r.t. pose3
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
const Point2 pn = Base::project_to_camera(pc); // project the point to the camera
return K_.uncalibrate(pn);
}
// world to camera coordinate
Matrix3 Dpc_rot, Dpc_point;
const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
Matrix36 Dpc_pose;
Dpc_pose.setZero();
Dpc_pose.leftCols<3>() = Dpc_rot;
// camera to normalized image coordinate
Matrix23 Dpn_pc; // 2*3
const Point2 pn = Base::project_to_camera(pc, Dpn_pc);
// uncalibration
Matrix2 Dpi_pn; // 2*2
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices
const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
if (Dpose)
*Dpose = Dpi_pc * Dpc_pose;
if (Dpoint)
*Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only)
return pi;
}
/** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate
*/

60
gtsam/geometry/PinholePose.h
View File

@ -35,7 +35,10 @@ class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
public :
// Get dimensions of calibration type at compile time
static const int DimK = FixedDimension<Calibration>::value;
public :
/// @name Standard Constructors
/// @{
@ -78,7 +81,19 @@ public :
return pn;
}
/** project a point from world coordinate to the image, fixed Jacobians
/** project a point from world coordinate to the image
* @param pw is a point in the world coordinates
*/
Point2 project(const Point3& pw) const {
// project to normalized coordinates
const Point2 pn = PinholeBase::project2(pw);
// uncalibrate to pixel coordinates
return calibration().uncalibrate(pn);
}
/** project a point from world coordinate to the image, w 2 derivatives
* @param pw is a point in the world coordinates
*/
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
@ -99,6 +114,47 @@ public :
return pi;
}
/** project a point at infinity from world coordinate to the image
* @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf)
* @param Dpose is the Jacobian w.r.t. pose3
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
const Point2 pn = PinholeBase::project_to_camera(pc);// project the point to the camera
return calibration().uncalibrate(pn);
}
// world to camera coordinate
Matrix3 Dpc_rot, Dpc_point;
const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
Matrix36 Dpc_pose;
Dpc_pose.setZero();
Dpc_pose.leftCols<3>() = Dpc_rot;
// camera to normalized image coordinate
Matrix23 Dpn_pc;// 2*3
const Point2 pn = PinholeBase::project_to_camera(pc, Dpn_pc);
// uncalibration
Matrix2 Dpi_pn;// 2*2
const Point2 pi = calibration().uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices
const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
if (Dpose)
*Dpose = Dpi_pc * Dpc_pose;
if (Dpoint)
*Dpoint = (Dpi_pc * Dpc_point).leftCols<2>();// only 2dof are important for the point (direction-only)
return pi;
}
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& p, double depth) const {
const Point2 pn = calibration().calibrate(p);