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
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

4
gtsam/geometry/Cal3_S2.h
View File

@ -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_);
}

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
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);
}

8
gtsam/geometry/Point2.h
View File

@ -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);
}

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,
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;
}

4
gtsam/geometry/Point3.h
View File

@ -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;
}
/// @}

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 {
// 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);
}

14
gtsam/geometry/Pose3.cpp
View File

@ -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<Pose3> align(const vector<Point3Pair>& 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);

8
gtsam/geometry/Rot2.h
View File

@ -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_);
}

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 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;

6
gtsam/geometry/Rot3M.cpp
View File

@ -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<Matrix3 &> 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);
}

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

@ -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)));
}