From 7116661a2e12dab77557f43a1d62194a0c09d3e3 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Wed, 3 Dec 2014 10:57:42 -0500 Subject: [PATCH] changed naming convention of const matrices to _DxD. @dellaert --- gtsam/base/Matrix.h | 58 +++++++++++++++--------------- gtsam/geometry/Cal3_S2.h | 4 +-- gtsam/geometry/EssentialMatrix.cpp | 4 +-- gtsam/geometry/Point2.h | 8 ++--- gtsam/geometry/Point3.cpp | 8 ++--- gtsam/geometry/Point3.h | 4 +-- gtsam/geometry/Pose2.cpp | 4 +-- gtsam/geometry/Pose3.cpp | 14 ++++---- gtsam/geometry/Rot2.h | 8 ++--- gtsam/geometry/Rot3.cpp | 12 +++---- gtsam/geometry/Rot3M.cpp | 6 ++-- gtsam/geometry/tests/testRot3.cpp | 10 +++--- 12 files changed, 71 insertions(+), 69 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 37f446234..e48f641e3 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -78,44 +78,46 @@ typedef Eigen::Block ConstSubMatrix; // Identity Matrices -static const int I1 = 1; -static const Matrix2 I2 = Matrix2::Identity(); -static const Matrix3 I3 = Matrix3::Identity(); -static const Matrix4 I4 = Matrix4::Identity(); -static const Matrix5 I5 = Matrix5::Identity(); -static const Matrix6 I6 = Matrix6::Identity(); +static const int I_1x1 = 1; +static const Matrix2 I_2x2 = Matrix2::Identity(); +static const Matrix3 I_3x3 = Matrix3::Identity(); +static const Matrix4 I_4x4 = Matrix4::Identity(); +static const Matrix5 I_5x5 = Matrix5::Identity(); +static const Matrix6 I_6x6 = Matrix6::Identity(); // Negative Identity -static const Matrix2 _I2 = -I2; -static const Matrix3 _I3 = -I3; -static const Matrix4 _I4 = -I4; -static const Matrix5 _I5 = -I5; -static const Matrix6 _I6 = -I6; +static const int _I_1x1 = -1; +static const Matrix2 _I_2x2 = -I_2x2; +static const Matrix3 _I_3x3 = -I_3x3; +static const Matrix4 _I_4x4 = -I_4x4; +static const Matrix5 _I_5x5 = -I_5x5; +static const Matrix6 _I_6x6 = -I_6x6; // Zero matrices // TODO : Make these for rectangular sized matrices as well. -static const int Z1 = 0; -static const Matrix2 Z2 = Matrix2::Zero(); -static const Matrix3 Z3 = Matrix3::Zero(); -static const Matrix4 Z4 = Matrix4::Zero(); -static const Matrix5 Z5 = Matrix5::Zero(); -static const Matrix6 Z6 = Matrix6::Zero(); +static const int Z_1x1 = 0; +static const Matrix2 Z_2x2 = Matrix2::Zero(); +static const Matrix3 Z_3x3 = Matrix3::Zero(); +static const Matrix4 Z_4x4 = Matrix4::Zero(); +static const Matrix5 Z_5x5 = Matrix5::Zero(); +static const Matrix6 Z_6x6 = Matrix6::Zero(); // Ones matrices // TODO : Make these for rectangular sized matrices as well. -static const int O1 = 1; -static const Matrix2 O2 = Matrix2::Ones(); -static const Matrix3 O3 = Matrix3::Ones(); -static const Matrix4 O4 = Matrix4::Ones(); -static const Matrix5 O5 = Matrix5::Ones(); -static const Matrix6 O6 = Matrix6::Ones(); +static const int O_1x1 = 1; +static const Matrix2 O_2x2 = Matrix2::Ones(); +static const Matrix3 O_3x3 = Matrix3::Ones(); +static const Matrix4 O_4x4 = Matrix4::Ones(); +static const Matrix5 O_5x5 = Matrix5::Ones(); +static const Matrix6 O_6x6 = Matrix6::Ones(); // Negative Ones -static const Matrix2 _O2 = -O2; -static const Matrix3 _O3 = -O3; -static const Matrix4 _O4 = -O4; -static const Matrix5 _O5 = -O5; -static const Matrix6 _O6 = -O6; +static const int _O_1x1 = -1; +static const Matrix2 _O_2x2 = -O_2x2; +static const Matrix3 _O_3x3 = -O_3x3; +static const Matrix4 _O_4x4 = -O_4x4; +static const Matrix5 _O_5x5 = -O_5x5; +static const Matrix6 _O_6x6 = -O_6x6; // Matlab-like syntax diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index aa4a2e2e8..61eafa283 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -173,8 +173,8 @@ public: inline Cal3_S2 between(const Cal3_S2& q, OptionalJacobian<5,5> H1=boost::none, OptionalJacobian<5,5> H2=boost::none) const { - if(H1) *H1 = _I5; - if(H2) *H2 = I5; + if(H1) *H1 = _I_5x5; + if(H2) *H2 = I_5x5; return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); } diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 57367a494..be4bba33f 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -27,9 +27,9 @@ 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->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<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 return EssentialMatrix(_1R2_, direction); } diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index a98dd3e0e..2491d92d1 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -127,8 +127,8 @@ public: inline Point2 compose(const Point2& q, OptionalJacobian<2,2> H1=boost::none, OptionalJacobian<2,2> H2=boost::none) const { - if(H1) *H1 = I2; - if(H2) *H2 = I2; + if(H1) *H1 = I_2x2; + if(H2) *H2 = I_2x2; return *this + q; } @@ -139,8 +139,8 @@ public: inline Point2 between(const Point2& q, OptionalJacobian<2,2> H1=boost::none, OptionalJacobian<2,2> H2=boost::none) const { - if(H1) *H1 = _I2; - if(H2) *H2 = I2; + if(H1) *H1 = _I_2x2; + if(H2) *H2 = I_2x2; return q - (*this); } diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index bd686f142..d82faf971 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -66,9 +66,9 @@ Point3 Point3::operator/(double s) const { Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) - *H1= I3; + *H1= I_3x3; if (H2) - *H2= I3; + *H2= I_3x3; 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, OptionalJacobian<3,3> H2) const { if (H1) - (*H1) = I3; + (*H1) = I_3x3; if (H2) - (*H2) = I3; + (*H2) = I_3x3; return *this - q; } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index c8e8eb5b3..33d9e47a5 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -143,12 +143,12 @@ namespace gtsam { /// Left-trivialized derivative of the exponential map static Matrix3 dexpL(const Vector& v) { - return I3; + return I_3x3; } /// Left-trivialized derivative inverse of the exponential map static Matrix3 dexpInvL(const Vector& v) { - return I3; + return I_3x3; } /// @} diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index d8da104fa..83895ad7c 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -149,7 +149,7 @@ Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { // TODO: inline and reuse? if(H1) *H1 = p2.inverse().AdjointMap(); - if(H2) *H2 = I3; + if(H2) *H2 = I_3x3; return (*this)*p2; } @@ -200,7 +200,7 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1, s, -c, dt2, 0.0, 0.0,-1.0; } - if (H2) *H2 = I3; + if (H2) *H2 = I_3x3; return Pose2(R,t); } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0b47df058..2e241a1e1 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -47,7 +47,7 @@ Matrix6 Pose3::AdjointMap() const { const Vector3 t = t_.vector(); Matrix3 A = skewSymmetric(t) * R; Matrix6 adj; - adj << R, Z3, A, R; + adj << R, Z_3x3, A, R; return adj; } @@ -56,7 +56,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); Matrix6 adj; - adj << w_hat, Z3, v_hat, w_hat; + adj << w_hat, Z_3x3, v_hat, w_hat; 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(); static const int N = 5; // order of approximation Matrix6 res; - res = I6; + res = I_6x6; Matrix6 ad_i; - ad_i = I6; + ad_i = I_6x6; Matrix6 ad_xi = adjointMap(xi); double fac = 1.0; for (int i = 1; i < N; ++i) { @@ -277,7 +277,7 @@ Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1, if (H1) *H1 = p2.inverse().AdjointMap(); if (H2) - *H2 = I6; + *H2 = I_6x6; return (*this) * p2; } @@ -297,7 +297,7 @@ Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, if (H1) *H1 = -result.inverse().AdjointMap(); if (H2) - (*H2) = I6; + (*H2) = I_6x6; return result; } @@ -364,7 +364,7 @@ boost::optional align(const vector& pairs) { // Recover transform with correction from Eggert97machinevisionandapplications Matrix3 UVtranspose = U * V.transpose(); - Matrix3 detWeighting = I3; + Matrix3 detWeighting = I_3x3; detWeighting(2, 2) = UVtranspose.determinant(); Rot3 R(Matrix(V * detWeighting * U.transpose())); Point3 t = Point3(cq) - R * Point3(cp); diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 9da4724fb..324332c9e 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -118,8 +118,8 @@ namespace gtsam { /** Compose - make a new rotation by adding angles */ inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 = boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 << I1; // set to 1x1 identity matrix - if (H2) *H2 << I1; // set to 1x1 identity matrix + if (H1) *H1 << I_1x1; // 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_); } @@ -131,8 +131,8 @@ namespace gtsam { /** Between using the default implementation */ inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 = boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 << -I1; // set to 1x1 identity matrix - if (H2) *H2 << I1; // set to 1x1 identity matrix + if (H1) *H1 << _I_1x1; // 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_); } diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 52f2fdd7a..95e7f85ce 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -120,7 +120,7 @@ Matrix3 Rot3::dexpL(const Vector3& v) { double theta = v.norm(), vi = theta/2.0; double s1 = sin(vi)/vi; 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; } @@ -132,7 +132,7 @@ Matrix3 Rot3::dexpInvL(const Vector3& v) { Matrix3 x2 = x*x; double theta = v.norm(), vi = theta/2.0; 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; } @@ -184,11 +184,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { double normx = norm_2(x); // rotation angle Matrix3 Jr; if (normx < 10e-8){ - Jr = I3; + Jr = I_3x3; } else{ 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 } return Jr; @@ -201,11 +201,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) { Matrix3 Jrinv; if (normx < 10e-8){ - Jrinv = I3; + Jrinv = I_3x3; } else{ 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; } return Jrinv; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index a7512148d..efc5d6f8b 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -31,7 +31,7 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Rot3::Rot3() : rot_(I3) {} +Rot3::Rot3() : rot_(I_3x3) {} /* ************************************************************************* */ 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) *H1 = R2.transpose(); if (H2) - *H2 = I3; + *H2 = I_3x3; return *this * R2; } @@ -169,7 +169,7 @@ Rot3 Rot3::inverse(boost::optional H1) const { Rot3 Rot3::between (const Rot3& R2, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = -(R2.transpose()*rot_); - if (H2) *H2 = I3; + if (H2) *H2 = I_3x3; Matrix3 R12 = transpose()*R2.rot_; return Rot3(R12); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 4e23ab744..6c3f1a7e7 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -49,7 +49,7 @@ TEST( Rot3, chart) /* ************************************************************************* */ TEST( Rot3, constructor) { - Rot3 expected(I3); + Rot3 expected(I_3x3); Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1); Rot3 actual(r1, r2, r3); CHECK(assert_equal(actual,expected)); @@ -94,7 +94,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { double t = norm_2(w); Matrix J = skewSymmetric(w / t); 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; } @@ -481,7 +481,7 @@ TEST( Rot3, RQ) Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); 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)); // 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(1), w(0), 0.0 ).finished(); 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 ; Rot3 expectedR( Rmat ); CHECK(assert_equal(expectedR, actualR, 1e-10)); @@ -577,7 +577,7 @@ TEST(Rot3, quaternion) { TEST( Rot3, Cayley ) { Matrix A = skewSymmetric(1,2,-3); 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))); }