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

using overloading rather than templates to manage projection of Point3 and Unit3 (the templates worked on mac, but had issues compiling in ubuntu)

release/4.3a0
Luca 2015-03-27 18:42:05 -04:00
parent 067f2ed39e
commit 4b2eb2f7aa
2 changed files with 51 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/geometry/PinholeCamera.h
View File

@ -145,7 +145,7 @@ public:
const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
if (H) {
H->setZero();
H->template block<6,6>(0, 0) = I_6x6;
H->template block<6, 6>(0, 0) = I_6x6;
}
return Base::pose();
}
@ -199,14 +199,12 @@ public:
typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
/** project a point from world coordinate to the image (possibly a Unit3)
* @param pw is a point in the world coordinate
/** Templated projection of a 3D point or a point at infinity into the image
* @param pw either a Point3 or a Unit3, in world coordinates
*/
template<class POINT>
Point2 project2(
const POINT& pw, //
OptionalJacobian<2, dimension> Dcamera = boost::none,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint = boost::none) const {
Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) const {
// We just call 3-derivative version in Base
Matrix26 Dpose;
Eigen::Matrix<double, 2, DimK> Dcal;
@ -217,6 +215,18 @@ public:
return pi;
}
/// project a 3D point from world coordinates into the image
Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
return _project2(pw, Dcamera, Dpoint);
}
/// project a point at infinity from world coordinates into the image
Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera =
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const {
return _project2(pw, Dcamera, Dpoint);
}
/**
* Calculate range to a landmark
* @param point 3D location of landmark
@ -264,7 +274,7 @@ public:
if (Dother) {
Dother->resize(1, 6 + CalibrationB::dimension);
Dother->setZero();
Dother->block<1,6>(0, 0) = Dother_;
Dother->block<1, 6>(0, 0) = Dother_;
}
return result;
}

52
gtsam/geometry/PinholePose.h
View File

@ -87,12 +87,8 @@ public:
* @param pw is a point in the world coordinates
*/
Point2 project(const Point3& pw) const {
// project to normalized coordinates
const Point2 pn = PinholeBase::project2(pw);
// uncalibrate to pixel coordinates
return calibration().uncalibrate(pn);
const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates
return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
}
/** project a point from world coordinate to the image
@ -100,20 +96,20 @@ public:
*/
Point2 project(const Unit3& pw) const {
const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame
const Point2 pn = PinholeBase::Project(pc);
return calibration().uncalibrate(pn);
const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates
return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
}
/** project a point (possibly at infinity) from world coordinate to the image
* @param pw is a point in world coordinates
/** Templated projection of a point (possibly at infinity) from world coordinate to the image
* @param pw is a 3D point or aUnit3 (point at infinity) 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
*/
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 {
Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint,
OptionalJacobian<2, DimK> Dcal) const {
// project to normalized coordinates
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
@ -132,6 +128,20 @@ public:
return pi;
}
/// project a 3D point from world coordinates into the image
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
return _project(pw, Dpose, Dpoint, Dcal);
}
/// project a point at infinity from world coordinates into the image
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
return _project(pw, Dpose, Dpoint, Dcal);
}
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& p, double depth) const {
const Point2 pn = calibration().calibrate(p);
@ -318,15 +328,19 @@ public:
return *K_;
}
/** project a point (possibly at infinity) from world coordinate to the image, 2 derivatives only
/** project a point 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 Dpose is the Jacobian w.r.t. the whole camera (really only the pose)
* @param Dpoint is the Jacobian w.r.t. point3
* TODO should use Unit3
*/
template<class POINT>
Point2 project2(const POINT& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint = boost::none) const {
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
return Base::project(pw, Dpose, Dpoint);
}
/// project2 version for point at infinity
Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const {
return Base::project(pw, Dpose, Dpoint);
}