diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 16884f4c1..689f36baa 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -41,6 +41,10 @@ typedef Eigen::Matrix3d Matrix3; typedef Eigen::Matrix4d Matrix4; typedef Eigen::Matrix Matrix6; +typedef Eigen::Matrix Matrix23; +typedef Eigen::Matrix Matrix25; +typedef Eigen::Matrix Matrix36; + // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 4f81750a5..baf450365 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -270,17 +270,15 @@ public: * @param Dpoint is the 2*3 Jacobian w.r.t. P */ inline static Point2 project_to_camera(const Point3& P, - boost::optional Dpoint = boost::none) { + boost::optional Dpoint = boost::none) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (P.z() <= 0) throw CheiralityException(); #endif double d = 1.0 / P.z(); const double u = P.x() * d, v = P.y() * d; - if (Dpoint) { - Dpoint->resize(2,3); + if (Dpoint) *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; - } return Point2(u, v); } @@ -356,7 +354,7 @@ public: Dpc_pose.block(0, 0, 3, 3) = Dpc_rot; // camera to normalized image coordinate - Matrix Dpn_pc; // 2*3 + Matrix23 Dpn_pc; // 2*3 const Point2 pn = project_to_camera(pc, Dpn_pc); // uncalibration diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 9e6f3f8ba..557565a6e 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -174,12 +174,14 @@ public: } else { // Calculate derivatives. TODO if slow: optimize with Mathematica - // 3*2 3*3 3*3 2*3 - Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2; + // 3*2 3*3 3*3 + Matrix D_1T2_dir, DdP2_rot, DP2_point; Point3 _1T2 = E.direction().point3(D_1T2_dir); Point3 d1T2 = d * _1T2; Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); + + Matrix23 Dpn_dP2; pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); if (DE) {