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

Sanitized Matrix typedefs and constants a la Eigen

release/4.3a0
dellaert 2014-12-04 10:41:09 +01:00
parent 354de17fd7
commit cc96529eb6
3 changed files with 32 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
gtsam/base/Matrix.h
View File

@ -37,6 +37,22 @@ namespace gtsam {
typedef Eigen::MatrixXd Matrix; typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor; typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
// Create handy typedefs and constants for square-size matrices
#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \
typedef Eigen::Matrix<double, SIZE, SIZE> Matrix##SUFFIX; \
static const Eigen::MatrixBase<Matrix##SUFFIX>::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \
static const Eigen::MatrixBase<Matrix##SUFFIX>::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<double,1,2> Matrix12; typedef Eigen::Matrix<double,1,2> Matrix12;
typedef Eigen::Matrix<double,1,3> Matrix13; typedef Eigen::Matrix<double,1,3> Matrix13;
typedef Eigen::Matrix<double,1,4> Matrix14; typedef Eigen::Matrix<double,1,4> Matrix14;
@ -49,13 +65,6 @@ typedef Eigen::Matrix<double,4,1> Matrix41;
typedef Eigen::Matrix<double,5,1> Matrix51; typedef Eigen::Matrix<double,5,1> Matrix51;
typedef Eigen::Matrix<double,6,1> Matrix61; typedef Eigen::Matrix<double,6,1> Matrix61;
typedef Eigen::Matrix<double,1,1> Matrix1;
typedef Eigen::Matrix2d Matrix2;
typedef Eigen::Matrix3d Matrix3;
typedef Eigen::Matrix4d Matrix4;
typedef Eigen::Matrix<double,5,5> Matrix5;
typedef Eigen::Matrix<double,6,6> Matrix6;
typedef Eigen::Matrix<double,2,3> Matrix23; typedef Eigen::Matrix<double,2,3> Matrix23;
typedef Eigen::Matrix<double,2,4> Matrix24; typedef Eigen::Matrix<double,2,4> Matrix24;
typedef Eigen::Matrix<double,2,5> Matrix25; typedef Eigen::Matrix<double,2,5> Matrix25;
@ -76,93 +85,6 @@ typedef Eigen::Matrix<double,3,9> Matrix39;
typedef Eigen::Block<Matrix> SubMatrix; typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix; typedef Eigen::Block<const Matrix> ConstSubMatrix;
// Identity Matrices
static const Eigen::MatrixBase<Matrix1>::IdentityReturnType I_1x1 = Matrix1::Identity();
static const Eigen::MatrixBase<Matrix2>::IdentityReturnType I_2x2 = Matrix2::Identity();
static const Eigen::MatrixBase<Matrix3>::IdentityReturnType I_3x3 = Matrix3::Identity();
static const Eigen::MatrixBase<Matrix4>::IdentityReturnType I_4x4 = Matrix4::Identity();
static const Eigen::MatrixBase<Matrix5>::IdentityReturnType I_5x5 = Matrix5::Identity();
static const Eigen::MatrixBase<Matrix6>::IdentityReturnType I_6x6 = Matrix6::Identity();
// Zero matrices
static const Eigen::DenseBase<Matrix1>::ConstantReturnType Z_1x1 = Matrix1::Zero();
static const Eigen::DenseBase<Matrix2>::ConstantReturnType Z_2x2 = Matrix2::Zero();
static const Eigen::DenseBase<Matrix3>::ConstantReturnType Z_3x3 = Matrix3::Zero();
static const Eigen::DenseBase<Matrix4>::ConstantReturnType Z_4x4 = Matrix4::Zero();
static const Eigen::DenseBase<Matrix5>::ConstantReturnType Z_5x5 = Matrix5::Zero();
static const Eigen::DenseBase<Matrix6>::ConstantReturnType Z_6x6 = Matrix6::Zero();
static const Eigen::DenseBase<Matrix12>::ConstantReturnType Z_1x2 = Matrix12::Zero();
static const Eigen::DenseBase<Matrix13>::ConstantReturnType Z_1x3 = Matrix13::Zero();
static const Eigen::DenseBase<Matrix14>::ConstantReturnType Z_1x4 = Matrix14::Zero();
static const Eigen::DenseBase<Matrix15>::ConstantReturnType Z_1x5 = Matrix15::Zero();
static const Eigen::DenseBase<Matrix16>::ConstantReturnType Z_1x6 = Matrix16::Zero();
static const Eigen::DenseBase<Matrix23>::ConstantReturnType Z_2x3 = Matrix23::Zero();
static const Eigen::DenseBase<Matrix24>::ConstantReturnType Z_2x4 = Matrix24::Zero();
static const Eigen::DenseBase<Matrix25>::ConstantReturnType Z_2x5 = Matrix25::Zero();
static const Eigen::DenseBase<Matrix26>::ConstantReturnType Z_2x6 = Matrix26::Zero();
static const Eigen::DenseBase<Matrix27>::ConstantReturnType Z_2x7 = Matrix27::Zero();
static const Eigen::DenseBase<Matrix28>::ConstantReturnType Z_2x8 = Matrix28::Zero();
static const Eigen::DenseBase<Matrix29>::ConstantReturnType Z_2x9 = Matrix29::Zero();
static const Eigen::DenseBase<Matrix32>::ConstantReturnType Z_3x2 = Matrix32::Zero();
static const Eigen::DenseBase<Matrix34>::ConstantReturnType Z_3x4 = Matrix34::Zero();
static const Eigen::DenseBase<Matrix35>::ConstantReturnType Z_3x5 = Matrix35::Zero();
static const Eigen::DenseBase<Matrix36>::ConstantReturnType Z_3x6 = Matrix36::Zero();
static const Eigen::DenseBase<Matrix37>::ConstantReturnType Z_3x7 = Matrix37::Zero();
static const Eigen::DenseBase<Matrix38>::ConstantReturnType Z_3x8 = Matrix38::Zero();
static const Eigen::DenseBase<Matrix39>::ConstantReturnType Z_3x9 = Matrix39::Zero();
static const Eigen::DenseBase<Matrix21>::ConstantReturnType Z_2x1 = Matrix21::Zero();
static const Eigen::DenseBase<Matrix31>::ConstantReturnType Z_3x1 = Matrix31::Zero();
static const Eigen::DenseBase<Matrix41>::ConstantReturnType Z_4x1 = Matrix41::Zero();
static const Eigen::DenseBase<Matrix51>::ConstantReturnType Z_5x1 = Matrix51::Zero();
static const Eigen::DenseBase<Matrix61>::ConstantReturnType Z_6x1 = Matrix61::Zero();
// Ones matrices
// TODO : Make these for rectangular sized matrices as well.
static const Eigen::DenseBase<Matrix1>::ConstantReturnType O_1x1 = Matrix1::Ones();
static const Eigen::DenseBase<Matrix2>::ConstantReturnType O_2x2 = Matrix2::Ones();
static const Eigen::DenseBase<Matrix3>::ConstantReturnType O_3x3 = Matrix3::Ones();
static const Eigen::DenseBase<Matrix4>::ConstantReturnType O_4x4 = Matrix4::Ones();
static const Eigen::DenseBase<Matrix5>::ConstantReturnType O_5x5 = Matrix5::Ones();
static const Eigen::DenseBase<Matrix6>::ConstantReturnType O_6x6 = Matrix6::Ones();
static const Eigen::DenseBase<Matrix12>::ConstantReturnType O_1x2 = Matrix12::Ones();
static const Eigen::DenseBase<Matrix13>::ConstantReturnType O_1x3 = Matrix13::Ones();
static const Eigen::DenseBase<Matrix14>::ConstantReturnType O_1x4 = Matrix14::Ones();
static const Eigen::DenseBase<Matrix15>::ConstantReturnType O_1x5 = Matrix15::Ones();
static const Eigen::DenseBase<Matrix16>::ConstantReturnType O_1x6 = Matrix16::Ones();
static const Eigen::DenseBase<Matrix23>::ConstantReturnType O_2x3 = Matrix23::Ones();
static const Eigen::DenseBase<Matrix24>::ConstantReturnType O_2x4 = Matrix24::Ones();
static const Eigen::DenseBase<Matrix25>::ConstantReturnType O_2x5 = Matrix25::Ones();
static const Eigen::DenseBase<Matrix26>::ConstantReturnType O_2x6 = Matrix26::Ones();
static const Eigen::DenseBase<Matrix27>::ConstantReturnType O_2x7 = Matrix27::Ones();
static const Eigen::DenseBase<Matrix28>::ConstantReturnType O_2x8 = Matrix28::Ones();
static const Eigen::DenseBase<Matrix29>::ConstantReturnType O_2x9 = Matrix29::Ones();
static const Eigen::DenseBase<Matrix32>::ConstantReturnType O_3x2 = Matrix32::Ones();
static const Eigen::DenseBase<Matrix34>::ConstantReturnType O_3x4 = Matrix34::Ones();
static const Eigen::DenseBase<Matrix35>::ConstantReturnType O_3x5 = Matrix35::Ones();
static const Eigen::DenseBase<Matrix36>::ConstantReturnType O_3x6 = Matrix36::Ones();
static const Eigen::DenseBase<Matrix37>::ConstantReturnType O_3x7 = Matrix37::Ones();
static const Eigen::DenseBase<Matrix38>::ConstantReturnType O_3x8 = Matrix38::Ones();
static const Eigen::DenseBase<Matrix39>::ConstantReturnType O_3x9 = Matrix39::Ones();
static const Eigen::DenseBase<Matrix21>::ConstantReturnType O_2x1 = Matrix21::Ones();
static const Eigen::DenseBase<Matrix31>::ConstantReturnType O_3x1 = Matrix31::Ones();
static const Eigen::DenseBase<Matrix41>::ConstantReturnType O_4x1 = Matrix41::Ones();
static const Eigen::DenseBase<Matrix51>::ConstantReturnType O_5x1 = Matrix51::Ones();
static const Eigen::DenseBase<Matrix61>::ConstantReturnType O_6x1 = Matrix61::Ones();
// Matlab-like syntax // Matlab-like syntax
/** /**

15
gtsam/geometry/EssentialMatrix.cpp
View File

@ -28,7 +28,7 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
Matrix23 D_direction_1T2; Matrix23 D_direction_1T2;
Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
*H << I_3x3, Z_3x3, // *H << I_3x3, Z_3x3, //
Z_2x3, D_direction_1T2 * _1R2_.matrix(); Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix();
return EssentialMatrix(_1R2_, direction); return EssentialMatrix(_1R2_, direction);
} }
} }
@ -52,13 +52,13 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
/* ************************************************************************* */ /* ************************************************************************* */
Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
return (Vector(5) << return (Vector(5) << aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(
aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_)).finished(); other.aTb_)).finished();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 EssentialMatrix::transform_to(const Point3& p, Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE,
OptionalJacobian<3,5> DE, OptionalJacobian<3,3> Dpoint) const { OptionalJacobian<3, 3> Dpoint) const {
Pose3 pose(aRb_, aTb_.point3()); Pose3 pose(aRb_, aTb_.point3());
Matrix36 DE_; Matrix36 DE_;
Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint);
@ -90,8 +90,9 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
Matrix23 D_c1Tc2_cRb; // 2*3 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); Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
if (HE) *HE << cRb.matrix(), Z_3x2, // if (HE)
Z_2x3, D_c1Tc2_aTb; *HE << cRb.matrix(), Matrix32::Zero(), //
Matrix23::Zero(), D_c1Tc2_aTb;
if (HR) { if (HR) {
throw runtime_error( throw runtime_error(
"EssentialMatrix::rotate: derivative HR not implemented yet"); "EssentialMatrix::rotate: derivative HR not implemented yet");

2
gtsam/geometry/Pose3.cpp
View File

@ -320,7 +320,7 @@ double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
double r = range(pose.translation(), H1, H2? &D2 : 0); double r = range(pose.translation(), H1, H2? &D2 : 0);
if (H2) { if (H2) {
Matrix13 H2_ = D2 * pose.rotation().matrix(); Matrix13 H2_ = D2 * pose.rotation().matrix();
*H2 << Z_1x3, H2_; *H2 << Matrix13::Zero(), H2_;
} }
return r; return r;
} }