diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 7b2a1b9fe..9cf92cce5 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -134,7 +134,7 @@ public: double H34 = 0; double H35 = cos_phi/rho; - *H2 = J2 * gtsam::Matrix_(3,5, + *H2 = J2 * (Mat(3, 5) << H11, H12, H13, H14, H15, H21, H22, H23, H24, H25, H31, H32, H33, H34, H35); @@ -143,7 +143,7 @@ public: double H16 = -cos_phi*cos_theta/rho2; double H26 = -cos_phi*sin_theta/rho2; double H36 = -sin_phi/rho2; - *H3 = J2 * gtsam::Matrix_(3,1, + *H3 = J2 * (Mat(3, 1) << H16, H26, H36); diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 45b640999..a6e88ae61 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -72,7 +72,7 @@ std::pair AHRS::initialize(double g_e) double sp = sin(mech0_.bRn().inverse().pitch()); double cy = cos(0.0); double sy = sin(0.0); - Matrix Omega_T = Matrix_(3, 3, cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0); + Matrix Omega_T = (Mat(3, 3) << cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0); // Calculate Jacobian of roll/pitch/yaw wrpt (g1,g2,g3), see doc/ypr.nb Vector b_g = mech0_.b_g(g_e); @@ -82,7 +82,7 @@ std::pair AHRS::initialize(double g_e) double g23 = g2 * g2 + g3 * g3; double g123 = g1 * g1 + g23; double f = 1 / (std::sqrt(g23) * g123); - Matrix H_g = Matrix_(3, 3, + Matrix H_g = (Mat(3, 3) << 0.0, g3 / g23, -(g2 / g23), // roll std::sqrt(g23) / g123, -f * (g1 * g2), -f * (g1 * g3), // pitch 0.0, 0.0, 0.0); // we don't know anything on yaw diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 54a557e03..6cccbc1cd 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -557,12 +557,12 @@ public: static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { - Matrix ENU_to_NED = Matrix_(3, 3, + Matrix ENU_to_NED = (Mat(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); - Matrix NED_to_ENU = Matrix_(3, 3, + Matrix NED_to_ENU = (Mat(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 0b7848325..6c5e845b9 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -481,12 +481,12 @@ public: static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { - Matrix ENU_to_NED = Matrix_(3, 3, + Matrix ENU_to_NED = (Mat(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); - Matrix NED_to_ENU = Matrix_(3, 3, + Matrix NED_to_ENU = (Mat(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index c16194254..5376ff812 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -290,12 +290,12 @@ public: static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { - Matrix ENU_to_NED = Matrix_(3, 3, + Matrix ENU_to_NED = (Mat(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); - Matrix NED_to_ENU = Matrix_(3, 3, + Matrix NED_to_ENU = (Mat(3, 3) << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index 682a5ad84..882575650 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -13,8 +13,8 @@ namespace gtsam { /* ************************************************************************* */ Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list& U, const std::list& F, const double g_e, bool flat) { - Matrix Umat = Matrix_(3,U.size(), concatVectors(U)); - Matrix Fmat = Matrix_(3,F.size(), concatVectors(F)); + Matrix Umat = (Mat(3, U.size()) << concatVectors(U)); + Matrix Fmat = (Mat(3, F.size()) << concatVectors(F)); return initialize(Umat, Fmat, g_e, flat); } diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index c0c4bf636..1c6c4c474 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -16,22 +16,22 @@ using namespace std; using namespace gtsam; // stationary interval of gyro U and acc F -Matrix stationaryU = trans(Matrix_(3,3,-0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003)); -Matrix stationaryF = trans(Matrix_(3,3,0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021)); +Matrix stationaryU = trans((Mat(3, 3) << -0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003)); +Matrix stationaryF = trans((Mat(3, 3) << 0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021)); double g_e = 9.7963; // Atlanta /* ************************************************************************* */ TEST (AHRS, cov) { // samples stored by row - Matrix A = Matrix_(4, 3, + Matrix A = (Mat(4, 3) << 1.0, 2.0, 3.0, 5.0, 7.0, 0.0, 9.0, 4.0, 7.0, 6.0, 3.0, 2.0); Matrix actual = cov(trans(A)); - Matrix expected = Matrix_(3, 3, + Matrix expected = (Mat(3, 3) << 10.9167, 2.3333, 5.0000, 2.3333, 4.6667, -2.6667, 5.0000, -2.6667, 8.6667); @@ -43,7 +43,7 @@ TEST (AHRS, cov) { TEST (AHRS, covU) { Matrix actual = cov(10000*stationaryU); - Matrix expected = Matrix_(3, 3, + Matrix expected = (Mat(3, 3) << 33.3333333, -1.66666667, 53.3333333, -1.66666667, 0.333333333, -5.16666667, 53.3333333, -5.16666667, 110.333333); @@ -55,7 +55,7 @@ TEST (AHRS, covU) { TEST (AHRS, covF) { Matrix actual = cov(100*stationaryF); - Matrix expected = Matrix_(3, 3, + Matrix expected = (Mat(3, 3) << 63.3808333, -0.432166667, -48.1706667, -0.432166667, 8.31053333, -16.6792667, -48.1706667, -16.6792667, 71.4297333); diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index ff6ae4457..b1326e66f 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -193,8 +193,8 @@ TEST( MultiProjectionFactor, create ){ // factor.evaluateError(pose, point, H1Actual, H2Actual); // // // The expected Jacobians -// Matrix H1Expected = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); -// Matrix H2Expected = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.); +// Matrix H1Expected = (Mat(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); +// Matrix H2Expected = (Mat(2, 3) << 92.376, 0., 0., 0., 92.376, 0.); // // // Verify the Jacobians are correct // CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); @@ -219,8 +219,8 @@ TEST( MultiProjectionFactor, create ){ // factor.evaluateError(pose, point, H1Actual, H2Actual); // // // The expected Jacobians -// Matrix H1Expected = Matrix_(2, 6, -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); -// Matrix H2Expected = Matrix_(2, 3, 0., -92.376, 0., 0., 0., -92.376); +// Matrix H1Expected = (Mat(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); +// Matrix H2Expected = (Mat(2, 3) << 0., -92.376, 0., 0., 0., -92.376); // // // Verify the Jacobians are correct // CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 385bab9ac..179cbf13e 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -88,8 +88,8 @@ TEST( SmartRangeFactor, unwhitenedError ) { // Check keys and Jacobian Vector actual4 = f.unwhitenedError(values, H); // with H now ! EXPECT(assert_equal((Vec(1) << 0.0), actual4)); - CHECK(assert_equal(Matrix_(1,3, 0.0,-1.0,0.0), H.front())); - CHECK(assert_equal(Matrix_(1,3, sqrt(2.0)/2,-sqrt(2.0)/2,0.0), H.back())); + CHECK(assert_equal((Mat(1, 3) << 0.0,-1.0,0.0), H.front())); + CHECK(assert_equal((Mat(1, 3) << sqrt(2.0)/2,-sqrt(2.0)/2,0.0), H.back())); // Test clone NonlinearFactor::shared_ptr clone = f.clone(); @@ -109,7 +109,7 @@ TEST( SmartRangeFactor, optimization ) { initial.insert(2, pose2); initial.insert(3, Pose2(5, 6, 0)); // does not satisfy range measurement Vector actual5 = f.unwhitenedError(initial); - EXPECT(assert_equal(Vector_(1,sqrt(25.0+16.0)-sqrt(50.0)), actual5)); + EXPECT(assert_equal((Vec(1) << sqrt(25.0+16.0)-sqrt(50.0)), actual5)); // Create Factor graph NonlinearFactorGraph graph;