From 7ebce58a69649fb0fc44e8ffa32a890a6ce92abc Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Mon, 21 Oct 2013 05:27:27 +0000 Subject: [PATCH] Fix Vector_() to Vec() in gtsam/slam --- gtsam/slam/BoundingConstraint.h | 8 ++++---- gtsam/slam/dataset.cpp | 8 ++++---- gtsam/slam/tests/testGeneralSFMFactor.cpp | 6 +++--- .../slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp | 6 +++--- gtsam/slam/tests/testPoseRotationPrior.cpp | 10 +++++----- gtsam/slam/tests/testPoseTranslationPrior.cpp | 12 ++++++------ gtsam/slam/tests/testProjectionFactor.cpp | 4 ++-- gtsam/slam/tests/testRangeFactor.cpp | 8 ++++---- gtsam/slam/tests/testStereoFactor.cpp | 4 ++-- 9 files changed, 33 insertions(+), 33 deletions(-) diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 58a5deb34..a1c263099 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -74,9 +74,9 @@ struct BoundingConstraint1: public NoiseModelFactor1 { } if (isGreaterThan_) - return Vector_(1, error); + return (Vec(1) << error); else - return -1.0 * Vector_(1, error); + return -1.0 * (Vec(1) << error); } private: @@ -147,9 +147,9 @@ struct BoundingConstraint2: public NoiseModelFactor2 { } if (isGreaterThan_) - return Vector_(1, error); + return (Vec(1) << error); else - return -1.0 * Vector_(1, error); + return -1.0 * (Vec(1) << error); } private: diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index d84163b95..0f9988521 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -151,7 +151,7 @@ pair load2D( // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart); if (!model) { - Vector variances = Vector_(3, m(0, 0), m(1, 1), m(2, 2)); + Vector variances = (Vec(3) << m(0, 0), m(1, 1), m(2, 2)); model = noiseModel::Diagonal::Variances(variances, smart); } @@ -179,7 +179,7 @@ pair load2D( continue; noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std)); + noiseModel::Diagonal::Sigmas((Vec(2) << bearing_std, range_std)); *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); // Insert poses or points if they do not exist yet @@ -211,7 +211,7 @@ pair load2D( { double rangeVar = v1; double bearingVar = v1 / 10.0; - measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, bearingVar, rangeVar)); + measurementNoise = noiseModel::Diagonal::Sigmas((Vec(2) << bearingVar, rangeVar)); } else { @@ -386,7 +386,7 @@ pair load2D_robust( continue; noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std)); + noiseModel::Diagonal::Sigmas((Vec(2) << bearing_std, range_std)); *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); // Insert poses or points if they do not exist yet diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index e05140875..89c95a9d9 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal - Vector z = Vector_(2,323.,240.); + Vector z = (Vec(2) << 323.,240.); const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr @@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) { Pose3 x1(R,t1); values.insert(X(1), GeneralCamera(x1)); Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); + EXPECT(assert_equal((Vec(2) << -3.0, 0.0), factor->unwhitenedError(values))); } static const double baseline = 5.0 ; @@ -309,7 +309,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { } else { - Vector delta = Vector_(11, + Vector delta = (Vec(11) << rot_noise, rot_noise, rot_noise, // rotation trans_noise, trans_noise, trans_noise, // translation focal_noise, focal_noise, // f_x, f_y diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 0c89fba18..79a6245d2 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); TEST( GeneralSFMFactor_Cal3Bundler, equals ) { // Create two identical factors and make sure they're equal - Vector z = Vector_(2,323.,240.); + Vector z = (Vec(2) << 323.,240.); const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr @@ -111,7 +111,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, error ) { Pose3 x1(R,t1); values.insert(X(1), GeneralCamera(x1)); Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); + EXPECT(assert_equal((Vec(2) << -3.0, 0.0), factor->unwhitenedError(values))); } @@ -308,7 +308,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) { } else { - Vector delta = Vector_(9, + Vector delta = (Vec(9) << rot_noise, rot_noise, rot_noise, // rotation trans_noise, trans_noise, trans_noise, // translation focal_noise, distort_noise, distort_noise // f, k1, k2 diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index 9f5c3f426..ce5e34b06 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -18,8 +18,8 @@ using namespace gtsam; -const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas(Vector_(1, 0.1)); -const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); +const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vec(1) << 0.1)); +const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3)); typedef PoseRotationPrior Pose2RotationPrior; typedef PoseRotationPrior Pose3RotationPrior; @@ -30,7 +30,7 @@ const gtsam::Key poseKey = 1; // Pose3 examples const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); -const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector_(3, 0.1, 0.2, 0.3)); +const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vec(3) << 0.1, 0.2, 0.3)); // Pose2 examples const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); @@ -62,7 +62,7 @@ TEST( testPoseRotationFactor, level3_error ) { Pose3 pose1(rot3A, point3A); Pose3RotationPrior factor(poseKey, rot3C, model3); Matrix actH1; - EXPECT(assert_equal(Vector_(3,-0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vec(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); @@ -84,7 +84,7 @@ TEST( testPoseRotationFactor, level2_error ) { Pose2 pose1(rot2A, point2A); Pose2RotationPrior factor(poseKey, rot2B, model1); Matrix actH1; - EXPECT(assert_equal(Vector_(1,-M_PI_2), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vec(1) << -M_PI_2), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 724e0e7e8..265ab3ef0 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -15,8 +15,8 @@ using namespace gtsam; -const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); -const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); +const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.2)); +const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3)); typedef PoseTranslationPrior Pose2TranslationPrior; typedef PoseTranslationPrior Pose3TranslationPrior; @@ -59,7 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) { Pose3 pose1(rot3A, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal(Vector_(3,-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vec(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); @@ -81,7 +81,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) { Pose3 pose1(rot3B, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal(Vector_(3,-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vec(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); @@ -103,7 +103,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) { Pose3 pose1(rot3C, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal(Vector_(3,-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vec(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); @@ -125,7 +125,7 @@ TEST( testPoseTranslationFactor, level2_error ) { Pose2 pose1(rot2A, point2A); Pose2TranslationPrior factor(poseKey, point2B, model2); Matrix actH1; - EXPECT(assert_equal(Vector_(2,-3.0,-4.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vec(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 9c7236054..a7f23493e 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -108,7 +108,7 @@ TEST( ProjectionFactor, Error ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = Vector_(2, -3.0, 0.0); + Vector expectedError = (Vec(2) << -3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -131,7 +131,7 @@ TEST( ProjectionFactor, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = Vector_(2, -3.0, 0.0); + Vector expectedError = (Vec(2) << -3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index b4cda70ff..98ee0201f 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -118,7 +118,7 @@ TEST( RangeFactor, Error2D ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance - Vector expectedError = Vector_(1, 0.295630141); + Vector expectedError = (Vec(1) << 0.295630141); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -143,7 +143,7 @@ TEST( RangeFactor, Error2DWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance - Vector expectedError = Vector_(1, 0.295630141); + Vector expectedError = (Vec(1) << 0.295630141); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -165,7 +165,7 @@ TEST( RangeFactor, Error3D ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance - Vector expectedError = Vector_(1, 0.295630141); + Vector expectedError = (Vec(1) << 0.295630141); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -190,7 +190,7 @@ TEST( RangeFactor, Error3DWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance - Vector expectedError = Vector_(1, 0.295630141); + Vector expectedError = (Vec(1) << 0.295630141); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 3a2fbd4f4..3b0e37954 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -102,7 +102,7 @@ TEST( StereoFactor, Error ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance - Vector expectedError = Vector_(3, -3.0, +2.0, -1.0); + Vector expectedError = (Vec(3) << -3.0, +2.0, -1.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -123,7 +123,7 @@ TEST( StereoFactor, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance - Vector expectedError = Vector_(3, -3.0, +2.0, -1.0); + Vector expectedError = (Vec(3) << -3.0, +2.0, -1.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9));