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

Moved project2 to PinholeBase

release/4.3a0
dellaert 2015-02-21 10:19:10 +01:00
parent 3a755cc4fb
commit 286a3ff412
2 changed files with 34 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
gtsam/geometry/CalibratedCamera.cpp
View File

@ -96,6 +96,24 @@ Point2 PinholeBase::project_to_camera(const Point3& P,
return Point2(u, v);
}
/* ************************************************************************* */
Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const {
Matrix3 Rt; // calculated by transform_to if needed
const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
Point2 pn = project_to_camera(q);
if (Dpose || Dpoint) {
const double z = q.z(), d = 1.0 / z;
if (Dpose)
*Dpose = PinholeBase::Dpose(pn, d);
if (Dpoint)
*Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose
}
return pn;
}
/* ************************************************************************* */
Point3 PinholeBase::backproject_from_camera(const Point2& p,
const double depth) {
@ -116,19 +134,7 @@ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
/* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point,
OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
Matrix3 Rt; // calculated by transform_to if needed
const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
Point2 pn = project_to_camera(q);
if (Dcamera || Dpoint) {
const double z = q.z(), d = 1.0 / z;
if (Dcamera)
*Dcamera = PinholeBase::Dpose(pn, d);
if (Dpoint)
*Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose
}
return pn;
return project2(point, Dcamera, Dpoint);
}
/* ************************************************************************* */

23
gtsam/geometry/CalibratedCamera.h
View File

@ -147,10 +147,20 @@ public:
OptionalJacobian<2, 3> Dpoint = boost::none);
/**
* backproject a 2-dimensional point to a 3-dimension point
* backproject a 2-dimensional point to a 3-dimensional point at given depth
*/
static Point3 backproject_from_camera(const Point2& p, const double depth);
/**
* Project point into the image
* @param point 3D point in world coordinates
* @param Dpose the optionally computed Jacobian with respect to camera
* @param Dpoint the optionally computed Jacobian with respect to the 3D point
* @return the intrinsic coordinates of the projected point
*/
Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
private:
/** Serialization function */
@ -253,16 +263,13 @@ public:
/// @}
/// @name Transformations and mesaurement functions
/// @{
/**
* This function receives the camera pose and the landmark location and
* returns the location the point is supposed to appear in the image
* @param point a 3D point to be projected
* @param Dpose the optionally computed Jacobian with respect to pose
* @param Dpoint the optionally computed Jacobian with respect to the 3D point
* @return the intrinsic coordinates of the projected point
* @deprecated
* Use project2, which is more consistently named across Pinhole cameras
*/
Point2 project(const Point3& point,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 6> Dcamera = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
/**