diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index 5647a0283..55563c7bf 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/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)); } diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index d578e56dc..d2704fbf3 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -88,19 +88,19 @@ TEST (Serialization, matrix_vector) { EXPECT(equality(Vector2(1.0, 2.0))); EXPECT(equality(Vector3(1.0, 2.0, 3.0))); EXPECT(equality((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished())); - EXPECT(equality(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equality((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0))); EXPECT(equalityXML((Vec(4) << 1.0, 2.0, 3.0, 4.0))); EXPECT(equalityXML(Vector2(1.0, 2.0))); EXPECT(equalityXML(Vector3(1.0, 2.0, 3.0))); EXPECT(equalityXML((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished())); - EXPECT(equalityXML(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityXML((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0))); EXPECT(equalityBinary((Vec(4) << 1.0, 2.0, 3.0, 4.0))); EXPECT(equalityBinary(Vector2(1.0, 2.0))); EXPECT(equalityBinary(Vector3(1.0, 2.0, 3.0))); EXPECT(equalityBinary((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished())); - EXPECT(equalityBinary(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityBinary((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index c47da2581..c286db17d 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/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 DR = Matrix_(2, 2, g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy, + Matrix DK = (Mat(2, 2) << fx, s_, 0.0, fy); + 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 DR = Matrix_(2, 2, g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy, + Matrix DK = (Mat(2, 2) << fx_, s_, 0.0, fy_); + 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) ); } diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 79e563d3f..6f009fceb 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -76,9 +76,9 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, boost::optional 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_); } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 54ca03ef0..6adc35d0f 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/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); } diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 12b35c8a0..e3838f50f 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -36,7 +36,7 @@ Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional 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)); } diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 6f219b9db..71956854f 100644 --- a/gtsam/geometry/Point2.cpp +++ b/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 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; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index a1add297d..a4a5e8769 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/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 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; diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 73d45d1a6..2e66c5e6d 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/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 H1, boost::optional 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 H1, Point2 Rot2::unrotate(const Point2& p, boost::optional H1, boost::optional 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 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(); } } diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index f44f68ac4..55b93c399 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/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() diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 2f6598143..3d26303cd 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/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 expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0); + Matrix expectedH1 = (Mat(2,3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.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 H2_expected = Matrix_(2, 2, 0., -1., 1., 0.); + Matrix H1_expected = (Mat(2, 3) << 0., -1., -2., 1., 0., -1.); + 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 diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 59d752d75..38c8a58b0 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/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)); diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index dc6fd52e6..1d9f10dc5 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/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)); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 3f6e4377f..1f35846f8 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/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);