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;
/** 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
*/
Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
// We just call 3-derivative version in Base
Matrix26 Dpose;
Eigen::Matrix<double, 2, DimK> Dcal;
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 {
template<class POINT>
Point2 project2(
const POINT& pw, //
OptionalJacobian<2, dimension> Dcamera = boost::none,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint = boost::none) const {
// We just call 3-derivative version in Base
Matrix26 Dpose;
Eigen::Matrix<double, 2, DimK> Dcal;

64
gtsam/geometry/PinholePose.h
View File

@ -104,41 +104,15 @@ public:
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 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 Point3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> 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;
}
/** 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,
template <class POINT>
Point2 project(const POINT& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
// project to normalized coordinates
@ -344,35 +318,15 @@ public:
return *K_;
}
/** 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,
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)
/** project a point (possibly at infinity) from world coordinate to the image, 2 derivatives only
* @param pw is a point in world coordinates
* @param Dpose is the Jacobian w.r.t. the whole camera (realy only the pose)
* @param Dpoint is the Jacobian w.r.t. point3
* TODO should use Unit3
*/
Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const {
template<class POINT>
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);
}