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

Fixed size matrix in project_to_camera

release/4.3a0
dellaert 2014-10-06 14:22:49 +02:00
parent e5c3f4228a
commit f9695f058e
3 changed files with 11 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/base/Matrix.h
View File

@ -41,6 +41,10 @@ typedef Eigen::Matrix3d Matrix3;
typedef Eigen::Matrix4d Matrix4; typedef Eigen::Matrix4d Matrix4;
typedef Eigen::Matrix<double,6,6> Matrix6; typedef Eigen::Matrix<double,6,6> Matrix6;
typedef Eigen::Matrix<double,2,3> Matrix23;
typedef Eigen::Matrix<double,2,5> Matrix25;
typedef Eigen::Matrix<double,3,6> Matrix36;
// Matrix expressions for accessing parts of matrices // Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix; typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix; typedef Eigen::Block<const Matrix> ConstSubMatrix;

8
gtsam/geometry/PinholeCamera.h
View File

@ -270,17 +270,15 @@ public:
* @param Dpoint is the 2*3 Jacobian w.r.t. P * @param Dpoint is the 2*3 Jacobian w.r.t. P
*/ */
inline static Point2 project_to_camera(const Point3& P, inline static Point2 project_to_camera(const Point3& P,
boost::optional<Matrix&> Dpoint = boost::none) { boost::optional<Matrix23&> Dpoint = boost::none) {
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (P.z() <= 0) if (P.z() <= 0)
throw CheiralityException(); throw CheiralityException();
#endif #endif
double d = 1.0 / P.z(); double d = 1.0 / P.z();
const double u = P.x() * d, v = P.y() * d; const double u = P.x() * d, v = P.y() * d;
if (Dpoint) { if (Dpoint)
Dpoint->resize(2,3);
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
}
return Point2(u, v); return Point2(u, v);
} }
@ -356,7 +354,7 @@ public:
Dpc_pose.block(0, 0, 3, 3) = Dpc_rot; Dpc_pose.block(0, 0, 3, 3) = Dpc_rot;
// camera to normalized image coordinate // camera to normalized image coordinate
Matrix Dpn_pc; // 2*3 Matrix23 Dpn_pc; // 2*3
const Point2 pn = project_to_camera(pc, Dpn_pc); const Point2 pn = project_to_camera(pc, Dpn_pc);
// uncalibration // uncalibration

6
gtsam/slam/EssentialMatrixFactor.h
View File

@ -174,12 +174,14 @@ public:
} else { } else {
// Calculate derivatives. TODO if slow: optimize with Mathematica // Calculate derivatives. TODO if slow: optimize with Mathematica
// 3*2 3*3 3*3 2*3 // 3*2 3*3 3*3
Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2; Matrix D_1T2_dir, DdP2_rot, DP2_point;
Point3 _1T2 = E.direction().point3(D_1T2_dir); Point3 _1T2 = E.direction().point3(D_1T2_dir);
Point3 d1T2 = d * _1T2; Point3 d1T2 = d * _1T2;
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
Matrix23 Dpn_dP2;
pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
if (DE) { if (DE) {