updated Matrix.h with commonly used matrices.

.gitignore

@@ -6,3 +6,4 @@
 /examples/Data/pose2example-rewritten.txt
 /examples/Data/pose3example-rewritten.txt
 *.txt.user
+*.txt.user.6d59f0c

gtsam/base/Matrix.h

@@ -76,6 +76,48 @@ typedef Eigen::Matrix<double,3,9> Matrix39;
 typedef Eigen::Block<Matrix> SubMatrix;
 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();
+
+// 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;
+
+// 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();
+
+// 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();
+
+// 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;
+
+
 // Matlab-like syntax
 
 /**

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 = -Matrix5::Identity();
+    if(H1) *H1 = _I5;
-    if(H2) *H2 = Matrix5::Identity();
+    if(H2) *H2 = I5;
     return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
   }
 

gtsam/geometry/CalibratedCamera.cpp

@@ -36,7 +36,7 @@ Point2 CalibratedCamera::project_to_camera(const Point3& P,
     OptionalJacobian<2,3> H1) {
   if (H1) {
     double d = 1.0 / P.z(), d2 = d * d;
-    (*H1) <<  d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
+    *H1 <<  d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
   }
   return Point2(P.x() / P.z(), P.y() / P.z());
 }

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) = Eigen::Matrix3d::Identity(); // upper left
+    H->block<3, 3>(0, 0) << I3; // upper left
     H->block<2, 3>(3, 0).setZero(); // lower left
-    H->block<3, 3>(0, 3).setZero(); // upper right
+    H->block<3, 3>(0, 3) << Z3; // upper right
     H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right
     return EssentialMatrix(_1R2_, direction);
   }

gtsam/geometry/Point2.cpp

@@ -50,7 +50,6 @@ double Point2::norm(OptionalJacobian<1,2> H) const {
 }
 
 /* ************************************************************************* */
-static const Matrix2 I2 = Eigen::Matrix2d::Identity();
 double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
     OptionalJacobian<1,2> H2) const {
   Point2 d = point - *this;

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 = Eigen::Matrix2d::Identity();
+    if(H1) *H1 = I2;
-    if(H2) *H2 = Eigen::Matrix2d::Identity();
+    if(H2) *H2 = I2;
     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 = -Eigen::Matrix2d::Identity();
+    if(H1) *H1 = _I2;
-    if(H2) *H2 = Eigen::Matrix2d::Identity();
+    if(H2) *H2 = I2;
     return q - (*this);
   }
 

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).setIdentity();
+    *H1= I3;
   if (H2)
-    (*H2).setIdentity();
+    *H2= I3;
   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).setIdentity();
+    (*H1) = I3;
   if (H2)
-    (*H2).setIdentity();
+    (*H2) = I3;
   return *this - q;
 }
 

gtsam/geometry/Point3.h

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

gtsam/geometry/Pose2.cpp

@@ -33,7 +33,6 @@ INSTANTIATE_LIE(Pose2);
 /** instantiate concept checks */
 GTSAM_CONCEPT_POSE_INST(Pose2);
 
-static const Matrix3 I3 = Eigen::Matrix3d::Identity();
 static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
 
 /* ************************************************************************* */

gtsam/geometry/Pose3.cpp

@@ -32,8 +32,6 @@ INSTANTIATE_LIE(Pose3);
 /** instantiate concept checks */
 GTSAM_CONCEPT_POSE_INST(Pose3);
 
-static const Matrix3 I3 = Eigen::Matrix3d::Identity(), Z3 = Eigen::Matrix3d::Zero(), _I3 = -I3;
-
 /* ************************************************************************* */
 Pose3::Pose3(const Pose2& pose2) :
     R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
@@ -102,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.setIdentity();
+  res = I6;
   Matrix6 ad_i;
-  ad_i.setIdentity();
+  ad_i = I6;
   Matrix6 ad_xi = adjointMap(xi);
   double fac = 1.0;
   for (int i = 1; i < N; ++i) {
@@ -279,7 +277,7 @@ Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1,
   if (H1)
     *H1 = p2.inverse().AdjointMap();
   if (H2)
-    (*H2).setIdentity();
+    *H2 = I6;
   return (*this) * p2;
 }
 
@@ -299,7 +297,7 @@ Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1,
   if (H1)
     *H1 = -result.inverse().AdjointMap();
   if (H2)
-    (*H2).setIdentity();
+    (*H2) = I6;
   return result;
 }
 
@@ -366,7 +364,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
 
   // Recover transform with correction from Eggert97machinevisionandapplications
   Matrix3 UVtranspose = U * V.transpose();
-  Matrix3 detWeighting = Eigen::Matrix3d::Identity();
+  Matrix3 detWeighting = I3;
   detWeighting(2, 2) = UVtranspose.determinant();
   Rot3 R(Matrix(V * detWeighting * U.transpose()));
   Point3 t = Point3(cq) - R * Point3(cp);

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)<< 1; // set to 1x1 identity matrix
+      if (H1) *H1 << I1; // set to 1x1 identity matrix
-      if (H2) (*H2)<< 1; // set to 1x1 identity matrix
+      if (H2) *H2 << I1; // 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 << -1; // set to 1x1 identity matrix
+      if (H1) *H1 << -I1; // set to 1x1 identity matrix
-      if (H2) *H2 <<  1; // set to 1x1 identity matrix
+      if (H2) *H2 <<  I1; // set to 1x1 identity matrix
       return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
     }
 

gtsam/geometry/Rot3.cpp

@@ -27,8 +27,6 @@ using namespace std;
 
 namespace gtsam {
 
-static const Matrix3 I3 = Matrix3::Identity();
-
 /* ************************************************************************* */
 void Rot3::print(const std::string& s) const {
   gtsam::print((Matrix)matrix(), s);
@@ -186,11 +184,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x)    {
   double normx = norm_2(x); // rotation angle
   Matrix3 Jr;
   if (normx < 10e-8){
-    Jr = Matrix3::Identity();
+    Jr = I3;
   }
   else{
     const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
-    Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
+    Jr = I3 - ((1-cos(normx))/(normx*normx)) * X +
         ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
   }
   return Jr;
@@ -203,11 +201,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x)    {
   Matrix3 Jrinv;
 
   if (normx < 10e-8){
-    Jrinv = Matrix3::Identity();
+    Jrinv = I3;
   }
   else{
     const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
-    Jrinv = Matrix3::Identity() +
+    Jrinv = I3 +
         0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx))   ) * X * X;
   }
   return Jrinv;

gtsam/geometry/Rot3M.cpp

@@ -30,10 +30,8 @@ using namespace std;
 
 namespace gtsam {
 
-static const Matrix3 I3 = Matrix3::Identity();
-
 /* ************************************************************************* */
-Rot3::Rot3() : rot_(Matrix3::Identity()) {}
+Rot3::Rot3() : rot_(I3) {}
 
 /* ************************************************************************* */
 Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {

gtsam/geometry/Unit3.cpp

@@ -43,7 +43,7 @@ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
   Unit3 direction(point);
   if (H) {
     // 3*3 Derivative of representation with respect to point is 3*3:
-    Matrix D_p_point;
+    Matrix3 D_p_point;
     point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
     // Calculate the 2*3 Jacobian
     *H << direction.basis().transpose() * D_p_point;
@@ -108,33 +108,34 @@ Matrix3 Unit3::skew() const {
 }
 
 /* ************************************************************************* */
-Vector Unit3::error(const Unit3& q, boost::optional<Matrix&> H) const {
+Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
   // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
-  Matrix Bt = basis().transpose();
+  Matrix23 Bt = basis().transpose();
-  Vector xi = Bt * q.p_.vector();
+  Vector2 xi = Bt * q.p_.vector();
   if (H)
     *H = Bt * q.basis();
   return xi;
 }
 
 /* ************************************************************************* */
-double Unit3::distance(const Unit3& q, boost::optional<Matrix&> H) const {
+double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
-  Vector xi = error(q, H);
+  Matrix2 H_;
+  Vector2 xi = error(q, H_);
   double theta = xi.norm();
   if (H)
-    *H = (xi.transpose() / theta) * (*H);
+    *H = (xi.transpose() / theta) * H_;
   return theta;
 }
 
 /* ************************************************************************* */
-Unit3 Unit3::retract(const Vector& v) const {
+Unit3 Unit3::retract(const Vector2& v) const {
 
   // Get the vector form of the point and the basis matrix
-  Vector p = Point3::Logmap(p_);
+  Vector3 p = Point3::Logmap(p_);
-  Matrix B = basis();
+  Matrix32 B = basis();
 
   // Compute the 3D xi_hat vector
-  Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
+  Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
 
   double xi_hat_norm = xi_hat.norm();
 
@@ -146,17 +147,17 @@ Unit3 Unit3::retract(const Vector& v) const {
       return Unit3(-point3());
   }
 
-  Vector exp_p_xi_hat = cos(xi_hat_norm) * p
+  Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p
       + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
   return Unit3(exp_p_xi_hat);
 
 }
 
 /* ************************************************************************* */
-Vector Unit3::localCoordinates(const Unit3& y) const {
+Vector2 Unit3::localCoordinates(const Unit3& y) const {
 
-  Vector p = Point3::Logmap(p_);
+  Vector3 p = Point3::Logmap(p_);
-  Vector q = Point3::Logmap(y.p_);
+  Vector3 q = Point3::Logmap(y.p_);
   double dot = p.dot(q);
 
   // Check for special cases
@@ -167,7 +168,7 @@ Vector Unit3::localCoordinates(const Unit3& y) const {
   else {
     // no special case
     double theta = acos(dot);
-    Vector result_hat = (theta / sin(theta)) * (q - p * dot);
+    Vector3 result_hat = (theta / sin(theta)) * (q - p * dot);
     return basis().transpose() * result_hat;
   }
 }

gtsam/geometry/Unit3.h

@@ -50,6 +50,11 @@ public:
       p_(p / p.norm()) {
   }
 
+  /// Construct from a vector3
+  explicit Unit3(const Vector3& p) :
+      p_(p / p.norm()) {
+  }
+
   /// Construct from x,y,z
   Unit3(double x, double y, double z) :
       p_(x, y, z) {
@@ -103,12 +108,12 @@ public:
   }
 
   /// Signed, vector-valued error between two directions
-  Vector error(const Unit3& q,
+  Vector2 error(const Unit3& q,
-      boost::optional<Matrix&> H = boost::none) const;
+      OptionalJacobian<2,2> H = boost::none) const;
 
   /// Distance between two directions
   double distance(const Unit3& q,
-      boost::optional<Matrix&> H = boost::none) const;
+      OptionalJacobian<1,2> H = boost::none) const;
 
   /// @}
 
@@ -131,10 +136,10 @@ public:
   };
 
   /// The retract function
-  Unit3 retract(const Vector& v) const;
+  Unit3 retract(const Vector2& v) const;
 
   /// The local coordinates function
-  Vector localCoordinates(const Unit3& s) const;
+  Vector2 localCoordinates(const Unit3& s) const;
 
   /// @}
 

gtsam/geometry/tests/testRot3.cpp

@@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3)
 static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
 static Point3 P(0.2, 0.7, -2.0);
 static double error = 1e-9, epsilon = 0.001;
-static const Matrix I3 = eye(3);
 
 /* ************************************************************************* */
 TEST( Rot3, chart)
@@ -578,7 +577,7 @@ TEST(Rot3, quaternion) {
 TEST( Rot3, Cayley ) {
   Matrix A = skewSymmetric(1,2,-3);
   Matrix Q = Cayley(A);
-  EXPECT(assert_equal(I3, trans(Q)*Q));
+  EXPECT(assert_equal((Matrix)I3, trans(Q)*Q));
   EXPECT(assert_equal(A, Cayley(Q)));
 }
 

gtsam/geometry/tests/testRot3M.cpp

@@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3)
 
 static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
 static Point3 P(0.2, 0.7, -2.0);
-static const Matrix I3 = eye(3);
 
 /* ************************************************************************* */
 TEST(Rot3, manifold_cayley)

gtsam/geometry/tests/testUnit3.cpp

@@ -108,9 +108,9 @@ TEST(Unit3, unrotate) {
 TEST(Unit3, error) {
   Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
   r = p.retract(Vector2(0.8, 0));
-  EXPECT(assert_equal(Vector2(0, 0), p.error(p), 1e-8));
+  EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-8));
-  EXPECT(assert_equal(Vector2(0.479426, 0), p.error(q), 1e-5));
+  EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5));
-  EXPECT(assert_equal(Vector2(0.717356, 0), p.error(r), 1e-5));
+  EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
 
   Matrix actual, expected;
   // Use numerical derivatives to calculate the expected Jacobian

gtsam_unstable/slam/AHRS.cpp

@@ -20,9 +20,6 @@ Matrix cov(const Matrix& m) {
   return DDt / (num_observations - 1);
 }
 
-Matrix I3 = eye(3);
-Matrix Z3 = zeros(3, 3);
-
 /* ************************************************************************* */
 AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
     bool flat) :