Fix Matrix_(...) to Mat() <<... in gtsam/base & gtsam/geometry

gtsam/base/tests/testNumericalDerivative.cpp

@@ -31,7 +31,7 @@ double f(const LieVector& x) {
 TEST(testNumericalDerivative, numericalHessian) {
   LieVector center = ones(2);
 
-  Matrix expected = Matrix_(2,2,
+  Matrix expected = (Mat(2,2) <<
       -sin(center(0)), 0.0,
       0.0, -cos(center(1)));
 
@@ -50,7 +50,7 @@ double f2(const LieVector& x) {
 TEST(testNumericalDerivative, numericalHessian2) {
   LieVector center(2, 0.5, 1.0);
 
-  Matrix expected = Matrix_(2,2,
+  Matrix expected = (Mat(2,2) <<
       -cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)),
       -cos(center(0))*sin(center(1)), -sin(center(0))*cos(center(1)));
 
@@ -69,15 +69,15 @@ double f3(const LieVector& x1, const LieVector& x2) {
 TEST(testNumericalDerivative, numericalHessian211) {
   LieVector center1(1, 1.0), center2(1, 5.0);
 
-  Matrix expected11 = Matrix_(1,1,-sin(center1(0))*cos(center2(0)));
+  Matrix expected11 = (Mat(1, 1) << -sin(center1(0))*cos(center2(0)));
   Matrix actual11 = numericalHessian211(f3, center1, center2);
   EXPECT(assert_equal(expected11, actual11, 1e-5));
 
-  Matrix expected12 = Matrix_(1,1,-cos(center1(0))*sin(center2(0)));
+  Matrix expected12 = (Mat(1, 1) <<-cos(center1(0))*sin(center2(0)));
   Matrix actual12 = numericalHessian212(f3, center1, center2);
   EXPECT(assert_equal(expected12, actual12, 1e-5));
 
-  Matrix expected22 = Matrix_(1,1,-sin(center1(0))*cos(center2(0)));
+  Matrix expected22 = (Mat(1, 1) <<-sin(center1(0))*cos(center2(0)));
   Matrix actual22 = numericalHessian222(f3, center1, center2);
   EXPECT(assert_equal(expected22, actual22, 1e-5));
 }
@@ -92,27 +92,27 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) {
 TEST(testNumericalDerivative, numericalHessian311) {
   LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0);
   double x = center1(0), y = center2(0), z = center3(0);
-  Matrix expected11 = Matrix_(1,1,-sin(x)*cos(y)*z*z);
+  Matrix expected11 = (Mat(1, 1) << -sin(x)*cos(y)*z*z);
   Matrix actual11 = numericalHessian311(f4, center1, center2, center3);
   EXPECT(assert_equal(expected11, actual11, 1e-5));
 
-  Matrix expected12 = Matrix_(1,1,-cos(x)*sin(y)*z*z);
+  Matrix expected12 = (Mat(1, 1) << -cos(x)*sin(y)*z*z);
   Matrix actual12 = numericalHessian312(f4, center1, center2, center3);
   EXPECT(assert_equal(expected12, actual12, 1e-5));
 
-  Matrix expected13 = Matrix_(1,1,cos(x)*cos(y)*2*z);
+  Matrix expected13 = (Mat(1, 1) << cos(x)*cos(y)*2*z);
   Matrix actual13 = numericalHessian313(f4, center1, center2, center3);
   EXPECT(assert_equal(expected13, actual13, 1e-5));
 
-  Matrix expected22 = Matrix_(1,1,-sin(x)*cos(y)*z*z);
+  Matrix expected22 = (Mat(1, 1) << -sin(x)*cos(y)*z*z);
   Matrix actual22 = numericalHessian322(f4, center1, center2, center3);
   EXPECT(assert_equal(expected22, actual22, 1e-5));
 
-  Matrix expected23 = Matrix_(1,1,-sin(x)*sin(y)*2*z);
+  Matrix expected23 = (Mat(1, 1) << -sin(x)*sin(y)*2*z);
   Matrix actual23 = numericalHessian323(f4, center1, center2, center3);
   EXPECT(assert_equal(expected23, actual23, 1e-5));
 
-  Matrix expected33 = Matrix_(1,1,sin(x)*cos(y)*2);
+  Matrix expected33 = (Mat(1, 1) << sin(x)*cos(y)*2);
   Matrix actual33 = numericalHessian333(f4, center1, center2, center3);
   EXPECT(assert_equal(expected33, actual33, 1e-5));
 }

gtsam/base/tests/testSerializationBase.cpp

@@ -88,19 +88,19 @@ TEST (Serialization, matrix_vector) {
   EXPECT(equality<Vector2>(Vector2(1.0, 2.0)));
   EXPECT(equality<Vector3>(Vector3(1.0, 2.0, 3.0)));
   EXPECT(equality<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
-  EXPECT(equality<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
+  EXPECT(equality<Matrix>((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0)));
 
   EXPECT(equalityXML<Vector>((Vec(4) << 1.0, 2.0, 3.0, 4.0)));
   EXPECT(equalityXML<Vector2>(Vector2(1.0, 2.0)));
   EXPECT(equalityXML<Vector3>(Vector3(1.0, 2.0, 3.0)));
   EXPECT(equalityXML<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
-  EXPECT(equalityXML<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
+  EXPECT(equalityXML<Matrix>((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0)));
 
   EXPECT(equalityBinary<Vector>((Vec(4) << 1.0, 2.0, 3.0, 4.0)));
   EXPECT(equalityBinary<Vector2>(Vector2(1.0, 2.0)));
   EXPECT(equalityBinary<Vector3>(Vector3(1.0, 2.0, 3.0)));
   EXPECT(equalityBinary<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
-  EXPECT(equalityBinary<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
+  EXPECT(equalityBinary<Matrix>((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0)));
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3DS2.cpp

@@ -28,10 +28,10 @@ Cal3DS2::Cal3DS2(const Vector &v):
     fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){}
 
 /* ************************************************************************* */
-Matrix Cal3DS2::K() const { return Matrix_(3,3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); }
+Matrix Cal3DS2::K() const { return (Mat(3, 3) <<  fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); }
 
 /* ************************************************************************* */
-Vector Cal3DS2::vector() const { return Vector_(9, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); }
+Vector Cal3DS2::vector() const { return (Vec(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); }
 
 /* ************************************************************************* */
 void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K"); gtsam::print(Vector(k()), s + ".k"); }
@@ -70,7 +70,7 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
 
   // Inlined derivative for calibration
   if (H1) {
-    *H1 = Matrix_(2, 9, pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
+    *H1 = (Mat(2, 9) <<  pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
         fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy),
         fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0,
         fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy));
@@ -87,8 +87,8 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
     const double dDy_dx = 2. * k4 * y + k3 * dr_dx;
     const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y);
 
-    Matrix DK = Matrix_(2, 2, fx, s_, 0.0, fy);
+    Matrix DK = (Mat(2, 2) << fx, s_, 0.0, fy);
-    Matrix DR = Matrix_(2, 2, g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
+    Matrix DR = (Mat(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
         y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy);
 
     *H2 = DK * DR;
@@ -148,8 +148,8 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
   const double dDy_dx = 2*k4*y + k3*dr_dx;
   const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y);
 
-  Matrix DK = Matrix_(2, 2, fx_, s_, 0.0, fy_);
+  Matrix DK = (Mat(2, 2) << fx_, s_, 0.0, fy_);
-  Matrix DR = Matrix_(2, 2, g + x*dg_dx + dDx_dx,     x*dg_dy + dDx_dy,
+  Matrix DR = (Mat(2, 2) << g + x*dg_dx + dDx_dx,     x*dg_dy + dDx_dy,
              y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy);
 
   return DK * DR;
@@ -167,7 +167,7 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
   const double pnx = g*x + dx;
   const double pny = g*y + dy;
 
-  return Matrix_(2, 9,
+  return (Mat(2, 9) <<
   pnx, 0.0, pny, 1.0, 0.0, fx*x*rr + s*y*rr, fx*x*r4 + s*y*r4, fx*2*xy + s*(rr+2*yy), fx*(rr+2*xx) + s*(2*xy),
   0.0, pny, 0.0, 0.0, 1.0, fy*y*rr      , fy*y*r4         , fy*(rr+2*yy)         , fy*(2*xy)  );
 }

gtsam/geometry/Cal3_S2.cpp

@@ -76,9 +76,9 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
     boost::optional<Matrix&> Dp) const {
   const double x = p.x(), y = p.y();
   if (Dcal)
-    *Dcal = Matrix_(2, 5, x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0);
+    *Dcal = (Mat(2, 5) << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0);
   if (Dp)
-    *Dp = Matrix_(2, 2, fx_, s_, 0.000, fy_);
+    *Dp = (Mat(2, 2) << fx_, s_, 0.000, fy_);
   return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
 }
 

gtsam/geometry/Cal3_S2.h

@@ -126,7 +126,7 @@ public:
 
   /// return calibration matrix K
   Matrix K() const {
-    return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
+    return (Mat(3, 3) <<  fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
   }
 
   /** @deprecated The following function has been deprecated, use K above */
@@ -137,7 +137,7 @@ public:
   /// return inverted calibration matrix inv(K)
   Matrix matrix_inverse() const {
     const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
-    return Matrix_(3, 3, 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
+    return (Mat(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
         1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0);
   }
 

gtsam/geometry/CalibratedCamera.cpp

@@ -36,7 +36,7 @@ Point2 CalibratedCamera::project_to_camera(const Point3& P,
     boost::optional<Matrix&> H1) {
   if (H1) {
     double d = 1.0 / P.z(), d2 = d * d;
-    *H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
+    *H1 = (Mat(2, 3) <<  d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
   }
   return Point2(P.x() / P.z(), P.y() / P.z());
 }
@@ -84,12 +84,12 @@ Point2 CalibratedCamera::project(const Point3& point,
     const double z = q.z(), d = 1.0 / z;
     const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
     if (Dpose)
-      *Dpose = Matrix_(2, 6, uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
+      *Dpose = (Mat(2, 6) <<  uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
           -uv, -u, 0., -d, d * v);
     if (Dpoint) {
       const Matrix R(pose_.rotation().matrix());
       *Dpoint = d
-          * Matrix_(2, 3, R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
+          * (Mat(2, 3) <<  R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
               R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
               R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2));
     }

gtsam/geometry/Point2.cpp

@@ -27,7 +27,7 @@ namespace gtsam {
 /** Explicit instantiation of base class to export members */
 INSTANTIATE_LIE(Point2);
 
-static const Matrix oneOne = Matrix_(1, 2, 1.0, 1.0);
+static const Matrix oneOne = (Mat(1, 2) <<  1.0, 1.0);
 
 /* ************************************************************************* */
 void Point2::print(const string& s) const {
@@ -45,7 +45,7 @@ double Point2::norm(boost::optional<Matrix&> H) const {
   if (H) {
     Matrix D_result_d;
     if (fabs(r) > 1e-10)
-      D_result_d = Matrix_(1, 2, x_ / r, y_ / r);
+      D_result_d = (Mat(1, 2) <<  x_ / r, y_ / r);
     else
       D_result_d = oneOne; // TODO: really infinity, why 1 here??
     *H = D_result_d;

gtsam/geometry/Pose2.cpp

@@ -40,7 +40,7 @@ static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
 Matrix Pose2::matrix() const {
   Matrix R = r_.matrix();
   R = stack(2, &R, &Z12);
-  Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0);
+  Matrix T = (Mat(3, 1) <<  t_.x(), t_.y(), 1.0);
   return collect(2, &R, &T);
 }
 
@@ -110,7 +110,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const {
 // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
 Matrix Pose2::AdjointMap() const {
   double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
-  return Matrix_(3,3,
+  return (Mat(3, 3) <<
       c,  -s,   y,
       s,   c,  -x,
       0.0, 0.0, 1.0
@@ -130,7 +130,7 @@ Point2 Pose2::transform_to(const Point2& point,
   Point2 d = point - t_;
   Point2 q = r_.unrotate(d);
   if (!H1 && !H2) return q;
-  if (H1) *H1 = Matrix_(2, 3,
+  if (H1) *H1 = (Mat(2, 3) <<
       -1.0, 0.0,  q.y(),
       0.0, -1.0, -q.x());
   if (H2) *H2 = r_.transpose();
@@ -154,7 +154,7 @@ Point2 Pose2::transform_from(const Point2& p,
   const Point2 q = r_ * p;
   if (H1 || H2) {
     const Matrix R = r_.matrix();
-    const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x());
+    const Matrix Drotate1 = (Mat(2, 1) <<  -q.y(), q.x());
     if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
     if (H2) *H2 = R;                         // R
   }
@@ -184,7 +184,7 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
   if (H1) {
     double dt1 = -s2 * x + c2 * y;
     double dt2 = -c2 * x - s2 * y;
-    *H1 = Matrix_(3,3,
+    *H1 = (Mat(3, 3) <<
         -c,  -s,  dt1,
         s,  -c,  dt2,
         0.0, 0.0,-1.0);
@@ -225,7 +225,7 @@ double Pose2::range(const Point2& point,
   if (!H1 && !H2) return d.norm();
   Matrix H;
   double r = d.norm(H);
-  if (H1) *H1 = H * Matrix_(2, 3,
+  if (H1) *H1 = H * (Mat(2, 3) <<
       -r_.c(),  r_.s(),  0.0,
       -r_.s(), -r_.c(),  0.0);
   if (H2) *H2 = H;
@@ -240,10 +240,10 @@ double Pose2::range(const Pose2& pose2,
   if (!H1 && !H2) return d.norm();
   Matrix H;
   double r = d.norm(H);
-  if (H1) *H1 = H * Matrix_(2, 3,
+  if (H1) *H1 = H * (Mat(2, 3) <<
       -r_.c(),  r_.s(),  0.0,
       -r_.s(), -r_.c(),  0.0);
-  if (H2) *H2 = H * Matrix_(2, 3,
+  if (H2) *H2 = H * (Mat(2, 3) <<
       pose2.r_.c(), -pose2.r_.s(),  0.0,
       pose2.r_.s(),  pose2.r_.c(),  0.0);
   return r;

gtsam/geometry/Rot2.cpp

@@ -65,12 +65,12 @@ Rot2& Rot2::normalize() {
 
 /* ************************************************************************* */
 Matrix Rot2::matrix() const {
-  return Matrix_(2, 2, c_, -s_, s_, c_);
+  return (Mat(2, 2) <<  c_, -s_, s_, c_);
 }
 
 /* ************************************************************************* */
 Matrix Rot2::transpose() const {
-  return Matrix_(2, 2, c_, s_, -s_, c_);
+  return (Mat(2, 2) <<  c_, s_, -s_, c_);
 }
 
 /* ************************************************************************* */
@@ -78,7 +78,7 @@ Matrix Rot2::transpose() const {
 Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
     boost::optional<Matrix&> H2) const {
   const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
-  if (H1) *H1 = Matrix_(2, 1, -q.y(), q.x());
+  if (H1) *H1 = (Mat(2, 1) <<  -q.y(), q.x());
   if (H2) *H2 = matrix();
   return q;
 }
@@ -88,7 +88,7 @@ Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
 Point2 Rot2::unrotate(const Point2& p,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
   const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
-  if (H1) *H1 = Matrix_(2, 1, q.y(), -q.x());  // R_{pi/2}q
+  if (H1) *H1 = (Mat(2, 1) << q.y(), -q.x());  // R_{pi/2}q
   if (H2) *H2 = transpose();
   return q;
 }
@@ -97,10 +97,10 @@ Point2 Rot2::unrotate(const Point2& p,
 Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
   double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
   if(fabs(n) > 1e-5) {
-    if (H) *H = Matrix_(1, 2, -y / d2, x / d2);
+    if (H) *H = (Mat(1, 2) << -y / d2, x / d2);
     return Rot2::fromCosSin(x / n, y / n);
   } else {
-    if (H) *H = Matrix_(1,2, 0.0, 0.0);
+    if (H) *H = (Mat(1, 2) << 0.0, 0.0);
     return Rot2();
   }
 }

gtsam/geometry/StereoCamera.cpp

@@ -64,7 +64,7 @@ namespace gtsam {
       // optimized version, see StereoCamera.nb
       if (H1) {
         const double v1 = v/fy, v2 = fx*v1, dx=d*x;
-        *H1 = Matrix_(3, 6,
+        *H1 = (Mat(3, 6) <<
                 uL*v1, -fx-dx*uL,     v2, -dfx,  0.0, d*uL,
                 uR*v1, -fx-dx*uR,     v2, -dfx,  0.0, d*uR,
             fy + v*v1,    -dx*v , -x*dfy,  0.0, -dfy, d*v
@@ -72,7 +72,7 @@ namespace gtsam {
       }
       if (H2) {
         const Matrix R(leftCamPose_.rotation().matrix());
-        *H2 = d * Matrix_(3, 3,
+        *H2 = d * (Mat(3, 3) <<
              fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
              fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
              fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v
@@ -90,7 +90,7 @@ namespace gtsam {
     double d = 1.0 / P.z(), d2 = d*d;
     const Cal3_S2Stereo& K = *K_;
     double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
-    return Matrix_(3, 3,
+    return (Mat(3, 3) <<
          f_x*d,   0.0, -d2*f_x* P.x(),
          f_x*d,   0.0, -d2*f_x*(P.x() - b),
            0.0, f_y*d, -d2*f_y* P.y()

gtsam/geometry/tests/testPose2.cpp

@@ -92,7 +92,7 @@ TEST(Pose2, expmap2) {
 TEST(Pose2, expmap3) {
   // do an actual series exponential map
   // see e.g. http://www.cis.upenn.edu/~cis610/cis610lie1.ps
-  Matrix A = Matrix_(3,3,
+  Matrix A = (Mat(3,3) <<
       0.0, -0.99,  0.01,
       0.99,  0.0, -0.015,
       0.0,   0.0,  0.0);
@@ -204,8 +204,8 @@ TEST( Pose2, transform_to )
 
   // expected
   Point2 expected(2,2);
-  Matrix expectedH1 = Matrix_(2,3, -1.0, 0.0, 2.0,  0.0, -1.0, -2.0);
+  Matrix expectedH1 = (Mat(2,3) << -1.0, 0.0, 2.0,  0.0, -1.0, -2.0);
-  Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0,  -1.0, 0.0);
+  Matrix expectedH2 = (Mat(2,2) << 0.0, 1.0,  -1.0, 0.0);
 
   // actual
   Matrix actualH1, actualH2;
@@ -236,8 +236,8 @@ TEST (Pose2, transform_from)
   Point2 expected(0., 2.);
   EXPECT(assert_equal(expected, actual));
 
-  Matrix H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.);
+  Matrix H1_expected = (Mat(2, 3) << 0., -1., -2., 1., 0., -1.);
-  Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.);
+  Matrix H2_expected = (Mat(2, 2) << 0., -1., 1., 0.);
 
   Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt);
   EXPECT(assert_equal(H1_expected, H1));
@@ -261,7 +261,7 @@ TEST(Pose2, compose_a)
   Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5)));
   EXPECT(assert_equal(expected, actual));
 
-  Matrix expectedH1 = Matrix_(3,3,
+  Matrix expectedH1 = (Mat(3,3) <<
       0.0, 1.0, 0.0,
        -1.0, 0.0, 2.0,
       0.0, 0.0, 1.0
@@ -355,7 +355,7 @@ namespace {
   Matrix matrix(const Pose2& gTl) {
     Matrix gRl = gTl.r().matrix();
     Point2 gt = gTl.t();
-    return Matrix_(3, 3,
+    return (Mat(3, 3) <<
       gRl(0, 0), gRl(0, 1), gt.x(),
       gRl(1, 0), gRl(1, 1), gt.y(),
       0.0,       0.0,   1.0);
@@ -368,7 +368,7 @@ TEST( Pose2, matrix )
   Point2 origin, t(1,2);
   Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
   Matrix gMl = matrix(gTl);
-  EXPECT(assert_equal(Matrix_(3,3,
+  EXPECT(assert_equal((Mat(3,3) <<
       0.0, -1.0, 1.0,
       1.0,  0.0, 2.0,
       0.0,  0.0, 1.0),
@@ -376,7 +376,7 @@ TEST( Pose2, matrix )
   Rot2 gR1 = gTl.r();
   EXPECT(assert_equal(homogeneous(t),gMl*homogeneous(origin)));
   Point2 x_axis(1,0), y_axis(0,1);
-  EXPECT(assert_equal(Matrix_(2,2,
+  EXPECT(assert_equal((Mat(2,2) <<
       0.0, -1.0,
       1.0,  0.0),
       gR1.matrix()));
@@ -387,7 +387,7 @@ TEST( Pose2, matrix )
 
   // check inverse pose
   Matrix lMg = matrix(gTl.inverse());
-  EXPECT(assert_equal(Matrix_(3,3,
+  EXPECT(assert_equal((Mat(3,3) <<
       0.0,  1.0,-2.0,
      -1.0,  0.0, 1.0,
       0.0,  0.0, 1.0),
@@ -421,7 +421,7 @@ TEST( Pose2, between )
   EXPECT(assert_equal(expected,actual1));
   EXPECT(assert_equal(expected,actual2));
 
-  Matrix expectedH1 = Matrix_(3,3,
+  Matrix expectedH1 = (Mat(3,3) <<
       0.0,-1.0,-2.0,
       1.0, 0.0,-2.0,
       0.0, 0.0,-1.0
@@ -432,7 +432,7 @@ TEST( Pose2, between )
   // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
   EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
 
-  Matrix expectedH2 = Matrix_(3,3,
+  Matrix expectedH2 = (Mat(3,3) <<
        1.0, 0.0, 0.0,
        0.0, 1.0, 0.0,
        0.0, 0.0, 1.0

gtsam/geometry/tests/testRot3M.cpp

@@ -57,7 +57,7 @@ TEST( Rot3, constructor)
 /* ************************************************************************* */
 TEST( Rot3, constructor2)
 {
-  Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.);
+  Matrix R = (Mat(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.);
   Rot3 actual(R);
   Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
   CHECK(assert_equal(actual,expected));
@@ -501,7 +501,7 @@ TEST( Rot3, RQ)
   CHECK(assert_equal((Vector)(Vec(3) << 0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
 
   // Try RQ to recover calibration from 3*3 sub-block of projection matrix
-  Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
+  Matrix K = (Mat(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
   Matrix A = K * R.matrix();
   boost::tie(actualK, actual) = RQ(A);
   CHECK(assert_equal(K,actualK));
@@ -514,7 +514,7 @@ TEST( Rot3, expmapStability ) {
   double theta = w.norm();
   double theta2 = theta*theta;
   Rot3 actualR = Rot3::Expmap(w);
-  Matrix W = Matrix_(3,3, 0.0, -w(2), w(1),
+  Matrix W = (Mat(3, 3) << 0.0, -w(2), w(1),
                           w(2), 0.0, -w(0),
                           -w(1), w(0), 0.0 );
   Matrix W2 = W*W;
@@ -541,13 +541,13 @@ TEST( Rot3, logmapStability ) {
 TEST(Rot3, quaternion) {
   // NOTE: This is also verifying the ability to convert Vector to Quaternion
   Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
-  Rot3 R1 = Rot3(Matrix_(3,3,
+  Rot3 R1 = Rot3((Matrix)(Mat(3, 3) <<
       0.271018623057411,   0.278786459830371,   0.921318086098018,
       0.578529366719085,   0.717799701969298,  -0.387385285854279,
      -0.769319620053772,   0.637998195662053,   0.033250932803219));
 
   Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
-  Rot3 R2 = Rot3(Matrix_(3,3,
+  Rot3 R2 = Rot3((Matrix)(Mat(3, 3) <<
       -0.207341903877828,   0.250149415542075,   0.945745528564780,
        0.881304914479026,  -0.371869043667957,   0.291573424846290,
        0.424630407073532,   0.893945571198514,  -0.143353873763946));

gtsam/geometry/tests/testRot3Q.cpp

@@ -52,7 +52,7 @@ TEST( Rot3, constructor)
 /* ************************************************************************* */
 TEST( Rot3, constructor2)
 {
-  Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.);
+  Matrix R = (Mat(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.);
   Rot3 actual(R);
   Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
   CHECK(assert_equal(actual,expected));
@@ -298,7 +298,7 @@ TEST( Rot3, between )
   Rot3 r1 = Rot3::Rz(M_PI/3.0);
   Rot3 r2 = Rot3::Rz(2.0*M_PI/3.0);
 
-  Matrix expectedr1 = Matrix_(3,3,
+  Matrix expectedr1 = (Mat(3, 3) <<
       0.5, -sqrt(3.0)/2.0, 0.0,
       sqrt(3.0)/2.0, 0.5, 0.0,
       0.0, 0.0, 1.0);
@@ -399,7 +399,7 @@ TEST( Rot3, RQ)
   CHECK(assert_equal((Vector)(Vec(3) <<0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
 
   // Try RQ to recover calibration from 3*3 sub-block of projection matrix
-  Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
+  Matrix K = (Mat(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
   Matrix A = K * R.matrix();
   boost::tie(actualK, actual) = RQ(A);
   CHECK(assert_equal(K,actualK));
@@ -412,7 +412,7 @@ TEST( Rot3, expmapStability ) {
   double theta = w.norm();
   double theta2 = theta*theta;
   Rot3 actualR = Rot3::Expmap(w);
-  Matrix W = Matrix_(3,3, 0.0, -w(2), w(1),
+  Matrix W = (Mat(3, 3) << 0.0, -w(2), w(1),
                           w(2), 0.0, -w(0),
                           -w(1), w(0), 0.0 );
   Matrix W2 = W*W;
@@ -440,13 +440,13 @@ TEST( Rot3, expmapStability ) {
 TEST(Rot3, quaternion) {
   // NOTE: This is also verifying the ability to convert Vector to Quaternion
   Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
-  Rot3 R1 = Rot3(Matrix_(3,3,
+  Rot3 R1 = Rot3((Mat(3, 3) <<
       0.271018623057411,   0.278786459830371,   0.921318086098018,
       0.578529366719085,   0.717799701969298,  -0.387385285854279,
      -0.769319620053772,   0.637998195662053,   0.033250932803219));
 
   Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
-  Rot3 R2 = Rot3(Matrix_(3,3,
+  Rot3 R2 = Rot3((Mat(3, 3) <<
       -0.207341903877828,   0.250149415542075,   0.945745528564780,
        0.881304914479026,  -0.371869043667957,   0.291573424846290,
        0.424630407073532,   0.893945571198514,  -0.143353873763946));

gtsam/geometry/tests/testSimpleCamera.cpp

@@ -28,7 +28,7 @@ using namespace gtsam;
 
 static const Cal3_S2 K(625, 625, 0, 0, 0);
 
-static const Pose3 pose1(Matrix_(3,3,
+static const Pose3 pose1((Matrix)(Mat(3,3) <<
               1., 0., 0.,
               0.,-1., 0.,
               0., 0.,-1.
@@ -144,7 +144,7 @@ TEST( SimpleCamera, simpleCamera)
   Point3 T(1000,2000,1500);
   SimpleCamera expected(Pose3(R.inverse(),T),K);
   // H&Z example, 2nd edition, page 163
-  Matrix P = Matrix_(3,4,
+  Matrix P = (Mat(3,4) <<
       3.53553e2, 3.39645e2, 2.77744e2, -1.44946e6,
       -1.03528e2, 2.33212e1, 4.59607e2, -6.32525e5,
       7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2);