diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index 37d5b3f51..e3a5d651e 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -60,8 +60,8 @@ TEST( BatchFixedLagSmoother, Example ) // SETDEBUG("BatchFixedLagSmoother calculateMarginalFactors", true); // Set up parameters - SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); + SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); // Create a Fixed-Lag Smoother typedef BatchFixedLagSmoother::KeyTimestampMap Timestamps; diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index ced9721f1..4fd088d55 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -39,13 +39,13 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index d2a74f1a6..ee3b2b5da 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -39,13 +39,13 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index c831e9d94..435965087 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -38,14 +38,14 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -//const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); -const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.1, 0.02, -0.1)), Point3(0.5, -0.05, 0.2) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +//const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.1, 0.02, -0.1)), Point3(0.5, -0.05, 0.2) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index b17344ef1..a25b97550 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -39,13 +39,13 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 158c584b5..73d2a0e9e 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -38,13 +38,13 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx((Vec(3) << 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx((Vec(3) << 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); -const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vec(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vec(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); /* ************************************************************************* */ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) { diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 56675187d..a770de6e0 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -59,8 +59,8 @@ TEST( IncrementalFixedLagSmoother, Example ) SETDEBUG("IncrementalFixedLagSmoother update", true); // Set up parameters - SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); + SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); // Create a Fixed-Lag Smoother typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps;