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

Now just calls PinholeBase::project2

release/4.3a0
dellaert 2015-02-21 10:27:07 +01:00
parent 286a3ff412
commit f08e228173
1 changed files with 20 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
gtsam/geometry/PinholePose.h
View File

@ -78,35 +78,29 @@ public:
} }
/** project a point from world coordinate to the image, fixed Jacobians /** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate * @param pw is a point in the world coordinates
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration] * @param Dpose is the Jacobian w.r.t. pose
* @param Dpoint is the Jacobian w.r.t. point3 * @param Dpoint is the Jacobian w.r.t. pw
*/ */
Point2 project2(const Point3& pw, Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 6> Dcamera = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const { OptionalJacobian<2, 3> Dpoint = boost::none) const {
Matrix3 Rt; // calculated by transform_to if needed // project to normalized coordinates
const Point3 pc = pose().transform_to(pw, boost::none, Dpoint ? &Rt : 0); const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
const Point2 pn = project_to_camera(pc);
if (!Dcamera && !Dpoint) { // uncalibrate to pixel coordinates
return calibration().uncalibrate(pn);
} else {
const double z = pc.z(), d = 1.0 / z;
// uncalibration
Matrix2 Dpi_pn; Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, boost::none, Dpi_pn); const Point2 pi = calibration().uncalibrate(pn, boost::none,
Dpose || Dpoint ? &Dpi_pn : 0);
if (Dcamera) // If needed, apply chain rule
*Dcamera = Dpi_pn * PinholeBase::Dpose(pn, d); if (Dpose)
*Dpose = Dpi_pn * *Dpose;
if (Dpoint) if (Dpoint)
*Dpoint = Dpi_pn * PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose *Dpoint = Dpi_pn * *Dpoint;
return pi; return pi;
} }
}
/// backproject a 2-dimensional point to a 3-dimensional point at given depth /// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& p, double depth) const { Point3 backproject(const Point2& p, double depth) const {