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

46
gtsam/geometry/PinholePose.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
* \nosubgrouping * \nosubgrouping
*/ */
template<typename Calibration> template<typename Calibration>
class GTSAM_EXPORT PinholeBaseK : public PinholeBase { class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
public: public:
@ -45,7 +45,7 @@ public:
/** constructor with pose */ /** constructor with pose */
explicit PinholeBaseK(const Pose3& pose) : explicit PinholeBaseK(const Pose3& pose) :
PinholeBase(pose) { PinholeBase(pose) {
} }
/// @} /// @}
@ -53,7 +53,7 @@ public:
/// @{ /// @{
explicit PinholeBaseK(const Vector &v) : explicit PinholeBaseK(const Vector &v) :
PinholeBase(v) { PinholeBase(v) {
} }
/// @} /// @}
@ -78,40 +78,34 @@ 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); Matrix2 Dpi_pn;
} else { const Point2 pi = calibration().uncalibrate(pn, boost::none,
const double z = pc.z(), d = 1.0 / z; Dpose || Dpoint ? &Dpi_pn : 0);
// uncalibration // If needed, apply chain rule
Matrix2 Dpi_pn; if (Dpose)
const Point2 pi = calibration().uncalibrate(pn, boost::none, Dpi_pn); *Dpose = Dpi_pn * *Dpose;
if (Dpoint)
*Dpoint = Dpi_pn * *Dpoint;
if (Dcamera) return pi;
*Dcamera = Dpi_pn * PinholeBase::Dpose(pn, d);
if (Dpoint)
*Dpoint = Dpi_pn * PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose
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 {
const Point2 pn = calibration().calibrate(p); const Point2 pn = calibration().calibrate(p);
return pose().transform_from(backproject_from_camera(pn,depth)); return pose().transform_from(backproject_from_camera(pn, depth));
} }
/// backproject a 2-dimensional point to a 3-dimensional point at infinity /// backproject a 2-dimensional point to a 3-dimensional point at infinity