From 90786c0203e8f2325be7c7211c5b95736ec1d8ad Mon Sep 17 00:00:00 2001 From: jing Date: Thu, 23 Jan 2014 01:44:55 -0500 Subject: [PATCH] fix Vector_() in rest of gtsam folder --- gtsam/linear/tests/testSampler.cpp | 2 +- gtsam/navigation/ImuBias.h | 2 +- gtsam/nonlinear/ISAM2.h | 4 ++-- gtsam/nonlinear/tests/testLinearContainerFactor.cpp | 2 +- gtsam/slam/BearingRangeFactor.h | 2 +- gtsam/slam/RangeFactor.h | 2 +- 6 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/tests/testSampler.cpp b/gtsam/linear/tests/testSampler.cpp index 1d041d017..9db8b7edc 100644 --- a/gtsam/linear/tests/testSampler.cpp +++ b/gtsam/linear/tests/testSampler.cpp @@ -24,7 +24,7 @@ const double tol = 1e-5; /* ************************************************************************* */ TEST(testSampler, basic) { - Vector sigmas = Vector_(3, 1.0, 0.1, 0.0); + Vector sigmas = (Vector(3) << 1.0, 0.1, 0.0); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas); char seed = 'A'; Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index b535f5179..32911e589 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -113,7 +113,7 @@ namespace imuBias { // // return measurement - biasGyro_ - w_earth_rate_I; // -//// Vector bias_gyro_temp(Vector_(3, -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2))); +//// Vector bias_gyro_temp((Vector(3) << -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2))); //// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; // } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index d00be006b..469c78f33 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -122,8 +122,8 @@ struct GTSAM_EXPORT ISAM2Params { * entries would be added with: * \code FastMap thresholds; - thresholds['x'] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold - thresholds['l'] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold + thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold + thresholds['l'] = (Vector(3) << 1.0, 1.0, 1.0); // 1.0 m landmark position threshold params.relinearizeThreshold = thresholds; \endcode */ diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index bf75f134b..fd70519dc 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -19,7 +19,7 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)); +const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0)); const double tol = 1e-5; gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2; diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 6f00c81a6..cf5760695 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -96,7 +96,7 @@ namespace gtsam { Vector e1 = Rot::Logmap(measuredBearing_.between(y1)); double y2 = pose.range(point, H21_, H22_); - Vector e2 = Vector_(1, y2 - measuredRange_); + Vector e2 = (Vector(1) << y2 - measuredRange_); if (H1) *H1 = gtsam::stack(2, &H11, &H21); if (H2) *H2 = gtsam::stack(2, &H12, &H22); diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 7b807d88f..fe140a298 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -73,7 +73,7 @@ namespace gtsam { } else { hx = pose.range(point, H1, H2); } - return Vector_(1, hx - measured_); + return (Vector(1) << hx - measured_); } /** return the measured */