diff --git a/gtsam_unstable/base/tests/testFixedVector.cpp b/gtsam_unstable/base/tests/testFixedVector.cpp index 1fddf80ca..54452e2c4 100644 --- a/gtsam_unstable/base/tests/testFixedVector.cpp +++ b/gtsam_unstable/base/tests/testFixedVector.cpp @@ -28,7 +28,7 @@ static const double tol = 1e-9; /* ************************************************************************* */ TEST( testFixedVector, conversions ) { double data1[] = {1.0, 2.0, 3.0}; - Vector v1 = Vector_(3, data1); + Vector v1 = (Vector(3) << data1[0], data1[1], data1[2]); TestVector3 fv1(v1), fv2(data1); Vector actFv2(fv2); diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index df9e14a36..3e1af8eb3 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -69,7 +69,7 @@ BearingS2 BearingS2::retract(const Vector& v) const { /* ************************************************************************* */ Vector BearingS2::localCoordinates(const BearingS2& x) const { - return Vector_(2, azimuth_.localCoordinates(x.azimuth_)(0), + return (Vector(2) << azimuth_.localCoordinates(x.azimuth_)(0), elevation_.localCoordinates(x.elevation_)(0)); } diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index 85b592b98..15fd47236 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -145,7 +145,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( // extend line by random dist and angle to get BC double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len); double tABC = randomAngle().theta(); - Pose2 xB = xA.retract(Vector_(3, dAB, 0.0, tABC)); + Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC)); // extend from B to find C double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len); diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index 78347a077..894312556 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -128,7 +128,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, // calculate angle to change by Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta()); - Pose2 test_pose = cur_pose.retract(Vector_(3, step_size, 0.0, Rot2::Logmap(dtheta)(0))); + Pose2 test_pose = cur_pose.retract((Vector(3) << step_size, 0.0, Rot2::Logmap(dtheta)(0))); // create a segment to use for intersection checking // find the closest intersection diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp index 806f7dc16..dacf28992 100644 --- a/gtsam_unstable/geometry/tests/testPose3Upright.cpp +++ b/gtsam_unstable/geometry/tests/testPose3Upright.cpp @@ -72,7 +72,7 @@ TEST( testPose3Upright, manifold ) { EXPECT(assert_equal(x1, x1.retract(zero(4)), tol)); EXPECT(assert_equal(x2, x2.retract(zero(4)), tol)); - Vector delta12 = Vector_(4, 3.0, 0.0, 4.0, 0.0), delta21 = -delta12; + Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0), delta21 = -delta12; EXPECT(assert_equal(x2, x1.retract(delta12), tol)); EXPECT(assert_equal(x1, x2.retract(delta21), tol)); diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 8eff62acb..234ad25bc 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -266,7 +266,7 @@ namespace gtsam { } } - return Vector_(2, p_inlier, p_outlier); + return (Vector(2) << p_inlier, p_outlier); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 5f09ec216..dec1b2378 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -537,7 +537,7 @@ public: Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5)); omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; // Calculating g @@ -551,7 +551,7 @@ public: double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); double g_calc( g0/( pow(1 + height/Ro, 2) ) ); - g_ENU = Vector_(3, 0.0, 0.0, -g_calc); + g_ENU = (Vector(3) << 0.0, 0.0, -g_calc); // Calculate rho @@ -560,7 +560,7 @@ public: double rho_E = -Vn/(Rm + height); double rho_N = Ve/(Rp + height); double rho_U = Ve*tan(lat_new)/(Rp + height); - rho_ENU = Vector_(3, rho_E, rho_N, rho_U); + rho_ENU = (Vector(3) << rho_E, rho_N, rho_U); } static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 3cd87b8d2..29f9d4972 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -96,7 +96,7 @@ public: " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (gtsam::Vector(1) << 0.0); } /** return the measurement */ diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 5e67a39d3..d61787358 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -96,7 +96,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (gtsam::Vector(1) << 0.0); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 2da440425..9d4113431 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -99,7 +99,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (gtsam::Vector(1) << 0.0); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 711a44bf9..89afad040 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -35,7 +35,7 @@ public: } Vector b_g(double g_e) { - Vector n_g = Vector_(3, 0.0, 0.0, g_e); + Vector n_g = (Vector_(3) << 0.0, 0.0, g_e); return (bRn_ * n_g).vector(); } diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index bf0b2b6e3..1925a3fe4 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -70,7 +70,7 @@ namespace gtsam { /** Single Element Constructor: acts on a single parameter specified by idx */ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { + Base(model, key), prior_((Vector(1) << prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { assert(model->dim() == 1); this->fillH(); } diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index 11d8ed3a3..292e3f68f 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -32,7 +32,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p *H2 = zeros(1, 3); (*H2)(0, 2) = -1.0; } - return Vector_(1, hx - measured_); + return (Vector(1) << hx - measured_); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index 5ff8f66e7..0411765e8 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -276,7 +276,7 @@ namespace gtsam { } } - return Vector_(2, p_inlier, p_outlier); + return (Vector(2) << p_inlier, p_outlier); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index cc3d693ad..a3c963a58 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -75,7 +75,7 @@ TEST (AHRS, Mechanization_integrate) { Mechanization_bRn2 mech; KalmanFilter::State state; // boost::tie(mech,state) = ahrs.initialize(g_e); -// Vector u = Vector_(3,0.05,0.0,0.0); +// Vector u = (Vector(3) << 0.05,0.0,0.0); // double dt = 2; // Rot3 expected; // Mechanization_bRn2 mech2 = mech.integrate(u,dt); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index ea34afa79..0b84f137d 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -199,7 +199,7 @@ TEST(GaussianJunctionTreeB, simpleMarginal) { // Create a simple graph NonlinearFactorGraph fg; fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); - fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0)))); + fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas((Vector(3) << 10.0, 1.0, 1.0)))); Values init; init.insert(X(0), Pose2()); diff --git a/tests/testSimulated2DOriented.cpp b/tests/testSimulated2DOriented.cpp index cf505c485..dcc31e0ec 100644 --- a/tests/testSimulated2DOriented.cpp +++ b/tests/testSimulated2DOriented.cpp @@ -58,7 +58,7 @@ TEST( simulated2DOriented, Dprior ) TEST( simulated2DOriented, constructor ) { Pose2 measurement(0.2, 0.3, 0.1); - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1., 1., 1.)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1., 1., 1.)); simulated2DOriented::Odometry factor(measurement, model, X(1), X(2)); simulated2DOriented::Values config;