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

changed naming convention of const matrices to _DxD. @dellaert

release/4.3a0
nsrinivasan7 2014-12-03 10:57:42 -05:00
parent 4e557d38e6
commit 7116661a2e
12 changed files with 71 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
gtsam/base/Matrix.h
View File

@ -78,44 +78,46 @@ typedef Eigen::Block<const Matrix> ConstSubMatrix;
// Identity Matrices // Identity Matrices
static const int I1 = 1; static const int I_1x1 = 1;
static const Matrix2 I2 = Matrix2::Identity(); static const Matrix2 I_2x2 = Matrix2::Identity();
static const Matrix3 I3 = Matrix3::Identity(); static const Matrix3 I_3x3 = Matrix3::Identity();
static const Matrix4 I4 = Matrix4::Identity(); static const Matrix4 I_4x4 = Matrix4::Identity();
static const Matrix5 I5 = Matrix5::Identity(); static const Matrix5 I_5x5 = Matrix5::Identity();
static const Matrix6 I6 = Matrix6::Identity(); static const Matrix6 I_6x6 = Matrix6::Identity();
// Negative Identity // Negative Identity
static const Matrix2 _I2 = -I2; static const int _I_1x1 = -1;
static const Matrix3 _I3 = -I3; static const Matrix2 _I_2x2 = -I_2x2;
static const Matrix4 _I4 = -I4; static const Matrix3 _I_3x3 = -I_3x3;
static const Matrix5 _I5 = -I5; static const Matrix4 _I_4x4 = -I_4x4;
static const Matrix6 _I6 = -I6; static const Matrix5 _I_5x5 = -I_5x5;
static const Matrix6 _I_6x6 = -I_6x6;
// Zero matrices // Zero matrices
// TODO : Make these for rectangular sized matrices as well. // TODO : Make these for rectangular sized matrices as well.
static const int Z1 = 0; static const int Z_1x1 = 0;
static const Matrix2 Z2 = Matrix2::Zero(); static const Matrix2 Z_2x2 = Matrix2::Zero();
static const Matrix3 Z3 = Matrix3::Zero(); static const Matrix3 Z_3x3 = Matrix3::Zero();
static const Matrix4 Z4 = Matrix4::Zero(); static const Matrix4 Z_4x4 = Matrix4::Zero();
static const Matrix5 Z5 = Matrix5::Zero(); static const Matrix5 Z_5x5 = Matrix5::Zero();
static const Matrix6 Z6 = Matrix6::Zero(); static const Matrix6 Z_6x6 = Matrix6::Zero();
// Ones matrices // Ones matrices
// TODO : Make these for rectangular sized matrices as well. // TODO : Make these for rectangular sized matrices as well.
static const int O1 = 1; static const int O_1x1 = 1;
static const Matrix2 O2 = Matrix2::Ones(); static const Matrix2 O_2x2 = Matrix2::Ones();
static const Matrix3 O3 = Matrix3::Ones(); static const Matrix3 O_3x3 = Matrix3::Ones();
static const Matrix4 O4 = Matrix4::Ones(); static const Matrix4 O_4x4 = Matrix4::Ones();
static const Matrix5 O5 = Matrix5::Ones(); static const Matrix5 O_5x5 = Matrix5::Ones();
static const Matrix6 O6 = Matrix6::Ones(); static const Matrix6 O_6x6 = Matrix6::Ones();
// Negative Ones // Negative Ones
static const Matrix2 _O2 = -O2; static const int _O_1x1 = -1;
static const Matrix3 _O3 = -O3; static const Matrix2 _O_2x2 = -O_2x2;
static const Matrix4 _O4 = -O4; static const Matrix3 _O_3x3 = -O_3x3;
static const Matrix5 _O5 = -O5; static const Matrix4 _O_4x4 = -O_4x4;
static const Matrix6 _O6 = -O6; static const Matrix5 _O_5x5 = -O_5x5;
static const Matrix6 _O_6x6 = -O_6x6;
// Matlab-like syntax // Matlab-like syntax

4
gtsam/geometry/Cal3_S2.h
View File

@ -173,8 +173,8 @@ public:
inline Cal3_S2 between(const Cal3_S2& q, inline Cal3_S2 between(const Cal3_S2& q,
OptionalJacobian<5,5> H1=boost::none, OptionalJacobian<5,5> H1=boost::none,
OptionalJacobian<5,5> H2=boost::none) const { OptionalJacobian<5,5> H2=boost::none) const {
if(H1) *H1 = _I5; if(H1) *H1 = _I_5x5;
if(H2) *H2 = I5; if(H2) *H2 = I_5x5;
return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
} }

4
gtsam/geometry/EssentialMatrix.cpp
View File

@ -27,9 +27,9 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
// First get 2*3 derivative from Unit3::FromPoint3 // First get 2*3 derivative from Unit3::FromPoint3
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->block<3, 3>(0, 0) << I3; // upper left H->block<3, 3>(0, 0) << I_3x3; // upper left
H->block<2, 3>(3, 0).setZero(); // lower left H->block<2, 3>(3, 0).setZero(); // lower left
H->block<3, 3>(0, 3) << Z3; // upper right H->block<3, 3>(0, 3) << Z_3x3; // upper right
H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right
return EssentialMatrix(_1R2_, direction); return EssentialMatrix(_1R2_, direction);
} }

8
gtsam/geometry/Point2.h
View File

@ -127,8 +127,8 @@ public:
inline Point2 compose(const Point2& q, inline Point2 compose(const Point2& q,
OptionalJacobian<2,2> H1=boost::none, OptionalJacobian<2,2> H1=boost::none,
OptionalJacobian<2,2> H2=boost::none) const { OptionalJacobian<2,2> H2=boost::none) const {
if(H1) *H1 = I2; if(H1) *H1 = I_2x2;
if(H2) *H2 = I2; if(H2) *H2 = I_2x2;
return *this + q; return *this + q;
} }
@ -139,8 +139,8 @@ public:
inline Point2 between(const Point2& q, inline Point2 between(const Point2& q,
OptionalJacobian<2,2> H1=boost::none, OptionalJacobian<2,2> H1=boost::none,
OptionalJacobian<2,2> H2=boost::none) const { OptionalJacobian<2,2> H2=boost::none) const {
if(H1) *H1 = _I2; if(H1) *H1 = _I_2x2;
if(H2) *H2 = I2; if(H2) *H2 = I_2x2;
return q - (*this); return q - (*this);
} }

8
gtsam/geometry/Point3.cpp
View File

@ -66,9 +66,9 @@ Point3 Point3::operator/(double s) const {
Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const { OptionalJacobian<3,3> H2) const {
if (H1) if (H1)
*H1= I3; *H1= I_3x3;
if (H2) if (H2)
*H2= I3; *H2= I_3x3;
return *this + q; return *this + q;
} }
@ -76,9 +76,9 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const { OptionalJacobian<3,3> H2) const {
if (H1) if (H1)
(*H1) = I3; (*H1) = I_3x3;
if (H2) if (H2)
(*H2) = I3; (*H2) = I_3x3;
return *this - q; return *this - q;
} }

4
gtsam/geometry/Point3.h
View File

@ -143,12 +143,12 @@ namespace gtsam {
/// Left-trivialized derivative of the exponential map /// Left-trivialized derivative of the exponential map
static Matrix3 dexpL(const Vector& v) { static Matrix3 dexpL(const Vector& v) {
return I3; return I_3x3;
} }
/// Left-trivialized derivative inverse of the exponential map /// Left-trivialized derivative inverse of the exponential map
static Matrix3 dexpInvL(const Vector& v) { static Matrix3 dexpInvL(const Vector& v) {
return I3; return I_3x3;
} }
/// @} /// @}

4
gtsam/geometry/Pose2.cpp
View File

@ -149,7 +149,7 @@ Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const { OptionalJacobian<3,3> H2) const {
// TODO: inline and reuse? // TODO: inline and reuse?
if(H1) *H1 = p2.inverse().AdjointMap(); if(H1) *H1 = p2.inverse().AdjointMap();
if(H2) *H2 = I3; if(H2) *H2 = I_3x3;
return (*this)*p2; return (*this)*p2;
} }
@ -200,7 +200,7 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
s, -c, dt2, s, -c, dt2,
0.0, 0.0,-1.0; 0.0, 0.0,-1.0;
} }
if (H2) *H2 = I3; if (H2) *H2 = I_3x3;
return Pose2(R,t); return Pose2(R,t);
} }

14
gtsam/geometry/Pose3.cpp
View File

@ -47,7 +47,7 @@ Matrix6 Pose3::AdjointMap() const {
const Vector3 t = t_.vector(); const Vector3 t = t_.vector();
Matrix3 A = skewSymmetric(t) * R; Matrix3 A = skewSymmetric(t) * R;
Matrix6 adj; Matrix6 adj;
adj << R, Z3, A, R; adj << R, Z_3x3, A, R;
return adj; return adj;
} }
@ -56,7 +56,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
Matrix6 adj; Matrix6 adj;
adj << w_hat, Z3, v_hat, w_hat; adj << w_hat, Z_3x3, v_hat, w_hat;
return adj; return adj;
} }
@ -100,9 +100,9 @@ Matrix6 Pose3::dExpInv_exp(const Vector6& xi) {
0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished();
static const int N = 5; // order of approximation static const int N = 5; // order of approximation
Matrix6 res; Matrix6 res;
res = I6; res = I_6x6;
Matrix6 ad_i; Matrix6 ad_i;
ad_i = I6; ad_i = I_6x6;
Matrix6 ad_xi = adjointMap(xi); Matrix6 ad_xi = adjointMap(xi);
double fac = 1.0; double fac = 1.0;
for (int i = 1; i < N; ++i) { for (int i = 1; i < N; ++i) {
@ -277,7 +277,7 @@ Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1,
if (H1) if (H1)
*H1 = p2.inverse().AdjointMap(); *H1 = p2.inverse().AdjointMap();
if (H2) if (H2)
*H2 = I6; *H2 = I_6x6;
return (*this) * p2; return (*this) * p2;
} }
@ -297,7 +297,7 @@ Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1,
if (H1) if (H1)
*H1 = -result.inverse().AdjointMap(); *H1 = -result.inverse().AdjointMap();
if (H2) if (H2)
(*H2) = I6; (*H2) = I_6x6;
return result; return result;
} }
@ -364,7 +364,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
// Recover transform with correction from Eggert97machinevisionandapplications // Recover transform with correction from Eggert97machinevisionandapplications
Matrix3 UVtranspose = U * V.transpose(); Matrix3 UVtranspose = U * V.transpose();
Matrix3 detWeighting = I3; Matrix3 detWeighting = I_3x3;
detWeighting(2, 2) = UVtranspose.determinant(); detWeighting(2, 2) = UVtranspose.determinant();
Rot3 R(Matrix(V * detWeighting * U.transpose())); Rot3 R(Matrix(V * detWeighting * U.transpose()));
Point3 t = Point3(cq) - R * Point3(cp); Point3 t = Point3(cq) - R * Point3(cp);

8
gtsam/geometry/Rot2.h
View File

@ -118,8 +118,8 @@ namespace gtsam {
/** Compose - make a new rotation by adding angles */ /** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 = inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const { boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 << I1; // set to 1x1 identity matrix if (H1) *H1 << I_1x1; // set to 1x1 identity matrix
if (H2) *H2 << I1; // set to 1x1 identity matrix if (H2) *H2 << I_1x1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
} }
@ -131,8 +131,8 @@ namespace gtsam {
/** Between using the default implementation */ /** Between using the default implementation */
inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 = inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const { boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 << -I1; // set to 1x1 identity matrix if (H1) *H1 << _I_1x1; // set to 1x1 identity matrix
if (H2) *H2 << I1; // set to 1x1 identity matrix if (H2) *H2 << I_1x1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
} }

12
gtsam/geometry/Rot3.cpp
View File

@ -120,7 +120,7 @@ Matrix3 Rot3::dexpL(const Vector3& v) {
double theta = v.norm(), vi = theta/2.0; double theta = v.norm(), vi = theta/2.0;
double s1 = sin(vi)/vi; double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta); double s2 = (theta - sin(theta))/(theta*theta*theta);
Matrix3 res = I3 - 0.5*s1*s1*x + s2*x2; Matrix3 res = I_3x3 - 0.5*s1*s1*x + s2*x2;
return res; return res;
} }
@ -132,7 +132,7 @@ Matrix3 Rot3::dexpInvL(const Vector3& v) {
Matrix3 x2 = x*x; Matrix3 x2 = x*x;
double theta = v.norm(), vi = theta/2.0; double theta = v.norm(), vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
Matrix3 res = I3 + 0.5*x - s2*x2; Matrix3 res = I_3x3 + 0.5*x - s2*x2;
return res; return res;
} }
@ -184,11 +184,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) {
double normx = norm_2(x); // rotation angle double normx = norm_2(x); // rotation angle
Matrix3 Jr; Matrix3 Jr;
if (normx < 10e-8){ if (normx < 10e-8){
Jr = I3; Jr = I_3x3;
} }
else{ else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jr = I3 - ((1-cos(normx))/(normx*normx)) * X + Jr = I_3x3 - ((1-cos(normx))/(normx*normx)) * X +
((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
} }
return Jr; return Jr;
@ -201,11 +201,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) {
Matrix3 Jrinv; Matrix3 Jrinv;
if (normx < 10e-8){ if (normx < 10e-8){
Jrinv = I3; Jrinv = I_3x3;
} }
else{ else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jrinv = I3 + Jrinv = I_3x3 +
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
} }
return Jrinv; return Jrinv;

6
gtsam/geometry/Rot3M.cpp
View File

@ -31,7 +31,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3() : rot_(I3) {} Rot3::Rot3() : rot_(I_3x3) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
@ -144,7 +144,7 @@ Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3
if (H1) if (H1)
*H1 = R2.transpose(); *H1 = R2.transpose();
if (H2) if (H2)
*H2 = I3; *H2 = I_3x3;
return *this * R2; return *this * R2;
} }
@ -169,7 +169,7 @@ Rot3 Rot3::inverse(boost::optional<Matrix3 &> H1) const {
Rot3 Rot3::between (const Rot3& R2, Rot3 Rot3::between (const Rot3& R2,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*rot_); if (H1) *H1 = -(R2.transpose()*rot_);
if (H2) *H2 = I3; if (H2) *H2 = I_3x3;
Matrix3 R12 = transpose()*R2.rot_; Matrix3 R12 = transpose()*R2.rot_;
return Rot3(R12); return Rot3(R12);
} }

10
gtsam/geometry/tests/testRot3.cpp
View File

@ -49,7 +49,7 @@ TEST( Rot3, chart)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, constructor) TEST( Rot3, constructor)
{ {
Rot3 expected(I3); Rot3 expected(I_3x3);
Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1); Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1);
Rot3 actual(r1, r2, r3); Rot3 actual(r1, r2, r3);
CHECK(assert_equal(actual,expected)); CHECK(assert_equal(actual,expected));
@ -94,7 +94,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
double t = norm_2(w); double t = norm_2(w);
Matrix J = skewSymmetric(w / t); Matrix J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3(); if (t < 1e-5) return Rot3();
Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J); Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R; return R;
} }
@ -481,7 +481,7 @@ TEST( Rot3, RQ)
Vector actual; Vector actual;
boost::tie(actualK, actual) = RQ(R.matrix()); boost::tie(actualK, actual) = RQ(R.matrix());
Vector expected = Vector3(0.14715, 0.385821, 0.231671); Vector expected = Vector3(0.14715, 0.385821, 0.231671);
CHECK(assert_equal(I3,actualK)); CHECK(assert_equal(I_3x3,actualK));
CHECK(assert_equal(expected,actual,1e-6)); CHECK(assert_equal(expected,actual,1e-6));
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
@ -515,7 +515,7 @@ TEST( Rot3, expmapStability ) {
w(2), 0.0, -w(0), w(2), 0.0, -w(0),
-w(1), w(0), 0.0 ).finished(); -w(1), w(0), 0.0 ).finished();
Matrix W2 = W*W; Matrix W2 = W*W;
Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0 Matrix Rmat = I_3x3 + (1.0-theta2/6.0 + theta2*theta2/120.0
- theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ; - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
Rot3 expectedR( Rmat ); Rot3 expectedR( Rmat );
CHECK(assert_equal(expectedR, actualR, 1e-10)); CHECK(assert_equal(expectedR, actualR, 1e-10));
@ -577,7 +577,7 @@ TEST(Rot3, quaternion) {
TEST( Rot3, Cayley ) { TEST( Rot3, Cayley ) {
Matrix A = skewSymmetric(1,2,-3); Matrix A = skewSymmetric(1,2,-3);
Matrix Q = Cayley(A); Matrix Q = Cayley(A);
EXPECT(assert_equal((Matrix)I3, trans(Q)*Q)); EXPECT(assert_equal((Matrix)I_3x3, trans(Q)*Q));
EXPECT(assert_equal(A, Cayley(Q))); EXPECT(assert_equal(A, Cayley(Q)));
} }