diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index d1c343fcd..c6798859b 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -37,6 +37,22 @@ namespace gtsam { typedef Eigen::MatrixXd Matrix; typedef Eigen::Matrix MatrixRowMajor; +// Create handy typedefs and constants for square-size matrices +#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \ +typedef Eigen::Matrix Matrix##SUFFIX; \ +static const Eigen::MatrixBase::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \ +static const Eigen::MatrixBase::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero(); + +GTSAM_MAKE_TYPEDEFS(1,1); +GTSAM_MAKE_TYPEDEFS(2,2); +GTSAM_MAKE_TYPEDEFS(3,3); +GTSAM_MAKE_TYPEDEFS(4,4); +GTSAM_MAKE_TYPEDEFS(5,5); +GTSAM_MAKE_TYPEDEFS(6,6); +GTSAM_MAKE_TYPEDEFS(7,7); +GTSAM_MAKE_TYPEDEFS(8,8); +GTSAM_MAKE_TYPEDEFS(9,9); + typedef Eigen::Matrix Matrix12; typedef Eigen::Matrix Matrix13; typedef Eigen::Matrix Matrix14; @@ -49,13 +65,6 @@ typedef Eigen::Matrix Matrix41; typedef Eigen::Matrix Matrix51; typedef Eigen::Matrix Matrix61; -typedef Eigen::Matrix Matrix1; -typedef Eigen::Matrix2d Matrix2; -typedef Eigen::Matrix3d Matrix3; -typedef Eigen::Matrix4d Matrix4; -typedef Eigen::Matrix Matrix5; -typedef Eigen::Matrix Matrix6; - typedef Eigen::Matrix Matrix23; typedef Eigen::Matrix Matrix24; typedef Eigen::Matrix Matrix25; @@ -76,93 +85,6 @@ typedef Eigen::Matrix Matrix39; typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; - -// Identity Matrices -static const Eigen::MatrixBase::IdentityReturnType I_1x1 = Matrix1::Identity(); -static const Eigen::MatrixBase::IdentityReturnType I_2x2 = Matrix2::Identity(); -static const Eigen::MatrixBase::IdentityReturnType I_3x3 = Matrix3::Identity(); -static const Eigen::MatrixBase::IdentityReturnType I_4x4 = Matrix4::Identity(); -static const Eigen::MatrixBase::IdentityReturnType I_5x5 = Matrix5::Identity(); -static const Eigen::MatrixBase::IdentityReturnType I_6x6 = Matrix6::Identity(); - -// Zero matrices -static const Eigen::DenseBase::ConstantReturnType Z_1x1 = Matrix1::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x2 = Matrix2::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x3 = Matrix3::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_4x4 = Matrix4::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_5x5 = Matrix5::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_6x6 = Matrix6::Zero(); - -static const Eigen::DenseBase::ConstantReturnType Z_1x2 = Matrix12::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_1x3 = Matrix13::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_1x4 = Matrix14::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_1x5 = Matrix15::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_1x6 = Matrix16::Zero(); - - -static const Eigen::DenseBase::ConstantReturnType Z_2x3 = Matrix23::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x4 = Matrix24::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x5 = Matrix25::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x6 = Matrix26::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x7 = Matrix27::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x8 = Matrix28::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x9 = Matrix29::Zero(); - -static const Eigen::DenseBase::ConstantReturnType Z_3x2 = Matrix32::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x4 = Matrix34::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x5 = Matrix35::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x6 = Matrix36::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x7 = Matrix37::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x8 = Matrix38::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x9 = Matrix39::Zero(); - -static const Eigen::DenseBase::ConstantReturnType Z_2x1 = Matrix21::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x1 = Matrix31::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_4x1 = Matrix41::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_5x1 = Matrix51::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_6x1 = Matrix61::Zero(); - - - - -// Ones matrices -// TODO : Make these for rectangular sized matrices as well. -static const Eigen::DenseBase::ConstantReturnType O_1x1 = Matrix1::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x2 = Matrix2::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x3 = Matrix3::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_4x4 = Matrix4::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_5x5 = Matrix5::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_6x6 = Matrix6::Ones(); - -static const Eigen::DenseBase::ConstantReturnType O_1x2 = Matrix12::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_1x3 = Matrix13::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_1x4 = Matrix14::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_1x5 = Matrix15::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_1x6 = Matrix16::Ones(); - - -static const Eigen::DenseBase::ConstantReturnType O_2x3 = Matrix23::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x4 = Matrix24::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x5 = Matrix25::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x6 = Matrix26::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x7 = Matrix27::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x8 = Matrix28::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x9 = Matrix29::Ones(); - -static const Eigen::DenseBase::ConstantReturnType O_3x2 = Matrix32::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x4 = Matrix34::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x5 = Matrix35::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x6 = Matrix36::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x7 = Matrix37::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x8 = Matrix38::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x9 = Matrix39::Ones(); - -static const Eigen::DenseBase::ConstantReturnType O_2x1 = Matrix21::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x1 = Matrix31::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_4x1 = Matrix41::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_5x1 = Matrix51::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_6x1 = Matrix61::Ones(); - // Matlab-like syntax /** diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 5ea05e10d..c2b690496 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -14,7 +14,7 @@ namespace gtsam { /* ************************************************************************* */ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, - OptionalJacobian<5,6> H) { + OptionalJacobian<5, 6> H) { const Rot3& _1R2_ = _1P2_.rotation(); const Point3& _1T2_ = _1P2_.translation(); if (!H) { @@ -27,8 +27,8 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, // First get 2*3 derivative from Unit3::FromPoint3 Matrix23 D_direction_1T2; Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); - *H << I_3x3, Z_3x3, // - Z_2x3, D_direction_1T2 * _1R2_.matrix(); + *H << I_3x3, Z_3x3, // + Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix(); return EssentialMatrix(_1R2_, direction); } } @@ -52,13 +52,13 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { /* ************************************************************************* */ Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { - return (Vector(5) << - aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_)).finished(); + return (Vector(5) << aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates( + other.aTb_)).finished(); } /* ************************************************************************* */ -Point3 EssentialMatrix::transform_to(const Point3& p, - OptionalJacobian<3,5> DE, OptionalJacobian<3,3> Dpoint) const { +Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE, + OptionalJacobian<3, 3> Dpoint) const { Pose3 pose(aRb_, aTb_.point3()); Matrix36 DE_; Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); @@ -67,8 +67,8 @@ Point3 EssentialMatrix::transform_to(const Point3& p, // The last 3 columns are derivative with respect to change in translation // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() // Duy made an educated guess that this needs to be rotated to the local frame - Matrix35 H; - H << DE_.block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); + Matrix35 H; + H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis(); *DE = H; } return q; @@ -76,7 +76,7 @@ Point3 EssentialMatrix::transform_to(const Point3& p, /* ************************************************************************* */ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, - OptionalJacobian<5,5> HE, OptionalJacobian<5,3> HR) const { + OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const { // The rotation must be conjugated to act in the camera frame Rot3 c1Rc2 = aRb_.conjugate(cRb); @@ -88,10 +88,11 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, } else { // Calculate derivatives Matrix23 D_c1Tc2_cRb; // 2*3 - Matrix2 D_c1Tc2_aTb; // 2*2 + Matrix2 D_c1Tc2_aTb; // 2*2 Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); - if (HE) *HE << cRb.matrix(), Z_3x2, // - Z_2x3, D_c1Tc2_aTb; + if (HE) + *HE << cRb.matrix(), Matrix32::Zero(), // + Matrix23::Zero(), D_c1Tc2_aTb; if (HR) { throw runtime_error( "EssentialMatrix::rotate: derivative HR not implemented yet"); @@ -106,7 +107,7 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, /* ************************************************************************* */ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // - OptionalJacobian<1,5> H) const { + OptionalJacobian<1, 5> H) const { if (H) { // See math.lyx Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 09ff633d2..f85ad9641 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -320,7 +320,7 @@ double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1, double r = range(pose.translation(), H1, H2? &D2 : 0); if (H2) { Matrix13 H2_ = D2 * pose.rotation().matrix(); - *H2 << Z_1x3, H2_; + *H2 << Matrix13::Zero(), H2_; } return r; }