Sanitized Matrix typedefs and constants a la Eigen
dellaert
parent
354de17fd7
commit
cc96529eb6
|
|
@ -37,6 +37,22 @@ namespace gtsam {
|
|||
typedef Eigen::MatrixXd Matrix;
|
||||
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,3> Matrix13;
|
||||
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,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,4> Matrix24;
|
||||
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<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
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue