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

Fixed-size version of project2 (copy/paste!)

release/4.3a0
dellaert 2014-10-22 12:48:45 +02:00
parent 19f0e3fc46
commit e18a2164bb
1 changed files with 43 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
gtsam/geometry/PinholeCamera.h
View File

@ -42,6 +42,8 @@ private:
Pose3 pose_;
Calibration K_;
static const int DimK = traits::dimension<Calibration>::value;
public:
/// @name Standard Constructors
@ -303,7 +305,7 @@ public:
return K_.uncalibrate(pn);
}
typedef Eigen::Matrix<double,2,traits::dimension<Calibration>::value> Matrix2K;
typedef Eigen::Matrix<double,2,DimK> Matrix2K;
/** project a point from world coordinate to the image
* @param pw is a point in world coordinates
@ -421,12 +423,48 @@ public:
return pi;
}
typedef Eigen::Matrix<double,2,6+DimK> Matrix2K6;
/** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
* @param Dpoint is the Jacobian w.r.t. point3
*/
Point2 project2(
const Point3& pw, //
boost::optional<Matrix2K6&> Dcamera = boost::none,
boost::optional<Matrix23&> Dpoint = boost::none) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
if (!Dcamera && !Dpoint) {
return K_.uncalibrate(pn);
} else {
const double z = pc.z(), d = 1.0 / z;
// uncalibration
Matrix2K Dcal;
Matrix2 Dpi_pn;
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
if (Dcamera) { // TODO why does leftCols<6>() not compile ??
calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols(6));
Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
}
if (Dpoint) {
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
}
return pi;
}
}
/** project a point from world coordinate to the image
* @param pw is a point in the world coordinate
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
* @param Dpoint is the Jacobian w.r.t. point3
*/
inline Point2 project2(
Point2 project2(
const Point3& pw, //
boost::optional<Matrix&> Dcamera = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const {
@ -440,12 +478,13 @@ public:
const double z = pc.z(), d = 1.0 / z;
// uncalibration
Matrix Dcal, Dpi_pn(2,2);
Matrix2K Dcal;
Matrix2 Dpi_pn;
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
if (Dcamera) {
Dcamera->resize(2, this->dim());
calculateDpose(pn, d, Dpi_pn.block<2,2>(0,0), Dcamera->leftCols<6>());
calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>());
Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
}
if (Dpoint) {