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

Templated instead of two identical functions

release/4.3a0
dellaert 2015-03-05 22:20:51 -08:00
parent 6f36bbf456
commit 4594c2dee5
2 changed files with 17 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/geometry/PinholeCamera.h
View File

@ -200,26 +200,14 @@ public:
typedef Eigen::Matrix<double, 2, DimK> Matrix2K; typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
/** project a point from world coordinate to the image, fixed Jacobians /** project a point from world coordinate to the image (possibly a Unit3)
* @param pw is a point in the world coordinate * @param pw is a point in the world coordinate
*/ */
Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera = template<class POINT>
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { Point2 project2(
// We just call 3-derivative version in Base const POINT& pw, //
Matrix26 Dpose; OptionalJacobian<2, dimension> Dcamera = boost::none,
Eigen::Matrix<double, 2, DimK> Dcal; OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint = boost::none) const {
Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint,
Dcamera ? &Dcal : 0);
if (Dcamera)
*Dcamera << Dpose, Dcal;
return pi;
}
/** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate
*/
Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
// We just call 3-derivative version in Base // We just call 3-derivative version in Base
Matrix26 Dpose; Matrix26 Dpose;
Eigen::Matrix<double, 2, DimK> Dcal; Eigen::Matrix<double, 2, DimK> Dcal;

68
gtsam/geometry/PinholePose.h
View File

@ -104,14 +104,15 @@ public:
return calibration().uncalibrate(pn); return calibration().uncalibrate(pn);
} }
/** project a point from world coordinate to the image /** project a point (possibly at infinity) from world coordinate to the image
* @param pw is a point in world coordinates * @param pw is a point in world coordinates
* @param Dpose is the Jacobian w.r.t. pose3 * @param Dpose is the Jacobian w.r.t. pose3
* @param Dpoint is the Jacobian w.r.t. point3 * @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration * @param Dcal is the Jacobian w.r.t. calibration
*/ */
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose, template <class POINT>
OptionalJacobian<2, 3> Dpoint = boost::none, Point2 project(const POINT& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const { OptionalJacobian<2, DimK> Dcal = boost::none) const {
// project to normalized coordinates // project to normalized coordinates
@ -124,36 +125,9 @@ public:
// If needed, apply chain rule // If needed, apply chain rule
if (Dpose) if (Dpose)
*Dpose = Dpi_pn * *Dpose; *Dpose = Dpi_pn * *Dpose;
if (Dpoint) if (Dpoint)
*Dpoint = Dpi_pn * *Dpoint; *Dpoint = Dpi_pn * *Dpoint;
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 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
// project to normalized coordinates
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
// uncalibrate to pixel coordinates
Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, Dcal,
Dpose || Dpoint ? &Dpi_pn : 0);
// If needed, apply chain rule
if (Dpose)
*Dpose = Dpi_pn * *Dpose;
if (Dpoint)
*Dpoint = Dpi_pn * *Dpoint;
return pi; return pi;
} }
@ -344,35 +318,15 @@ public:
return *K_; return *K_;
} }
/** project a point from world coordinate to the image, w 2 derivatives /** project a point (possibly at infinity) from world coordinate to the image, 2 derivatives only
* @param pw is a point in the world coordinates * @param pw is a point in world coordinates
*/
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
// project to normalized coordinates
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
// uncalibrate to pixel coordinates
Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, boost::none,
Dpose || Dpoint ? &Dpi_pn : 0);
// If needed, apply chain rule
if (Dpose) *Dpose = Dpi_pn * (*Dpose);
if (Dpoint) *Dpoint = Dpi_pn * (*Dpoint);
return pi;
}
/** project a point at infinity from world coordinate to the image, 2 derivatives only
* @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. the whole camera (realy only the pose) * @param Dpose is the Jacobian w.r.t. the whole camera (realy only the pose)
* @param Dpoint is the Jacobian w.r.t. point3 * @param Dpoint is the Jacobian w.r.t. point3
* TODO should use Unit3 * TODO should use Unit3
*/ */
Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, template<class POINT>
OptionalJacobian<2, 2> Dpoint = boost::none) const { Point2 project2(const POINT& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint = boost::none) const {
return Base::project(pw, Dpose, Dpoint); return Base::project(pw, Dpose, Dpoint);
} }