12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix Vector_() to Vec() in gtsam/slam

release/4.3a0
Jing Dong 2013-10-21 05:27:27 +00:00
parent a71c258100
commit 7ebce58a69
9 changed files with 33 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/slam/BoundingConstraint.h
View File

@ -74,9 +74,9 @@ struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {
} }
if (isGreaterThan_) if (isGreaterThan_)
return Vector_(1, error); return (Vec(1) << error);
else else
return -1.0 * Vector_(1, error); return -1.0 * (Vec(1) << error);
} }
private: private:
@ -147,9 +147,9 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
} }
if (isGreaterThan_) if (isGreaterThan_)
return Vector_(1, error); return (Vec(1) << error);
else else
return -1.0 * Vector_(1, error); return -1.0 * (Vec(1) << error);
} }
private: private:

8
gtsam/slam/dataset.cpp
View File

@ -151,7 +151,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
// SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart); // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
if (!model) { 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); model = noiseModel::Diagonal::Variances(variances, smart);
} }
@ -179,7 +179,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
continue; continue;
noiseModel::Diagonal::shared_ptr measurementNoise = 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<Pose2, Point2>(id1, id2, bearing, range, measurementNoise); *graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise);
// Insert poses or points if they do not exist yet // Insert poses or points if they do not exist yet
@ -211,7 +211,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
{ {
double rangeVar = v1; double rangeVar = v1;
double bearingVar = v1 / 10.0; double bearingVar = v1 / 10.0;
measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, bearingVar, rangeVar)); measurementNoise = noiseModel::Diagonal::Sigmas((Vec(2) << bearingVar, rangeVar));
} }
else else
{ {
@ -386,7 +386,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
continue; continue;
noiseModel::Diagonal::shared_ptr measurementNoise = 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<Pose2, Point2>(id1, id2, bearing, range, measurementNoise); *graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise);
// Insert poses or points if they do not exist yet // Insert poses or points if they do not exist yet

6
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
TEST( GeneralSFMFactor, equals ) TEST( GeneralSFMFactor, equals )
{ {
// Create two identical factors and make sure they're equal // 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 Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> boost::shared_ptr<Projection>
@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) {
Pose3 x1(R,t1); Pose3 x1(R,t1);
values.insert(X(1), GeneralCamera(x1)); values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1); 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 ; static const double baseline = 5.0 ;
@ -309,7 +309,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
} }
else { else {
Vector delta = Vector_(11, Vector delta = (Vec(11) <<
rot_noise, rot_noise, rot_noise, // rotation rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation trans_noise, trans_noise, trans_noise, // translation
focal_noise, focal_noise, // f_x, f_y focal_noise, focal_noise, // f_x, f_y

6
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
TEST( GeneralSFMFactor_Cal3Bundler, equals ) TEST( GeneralSFMFactor_Cal3Bundler, equals )
{ {
// Create two identical factors and make sure they're equal // 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 Symbol cameraFrameNumber('x',1), landmarkNumber('l',1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> boost::shared_ptr<Projection>
@ -111,7 +111,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, error ) {
Pose3 x1(R,t1); Pose3 x1(R,t1);
values.insert(X(1), GeneralCamera(x1)); values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1); 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 { else {
Vector delta = Vector_(9, Vector delta = (Vec(9) <<
rot_noise, rot_noise, rot_noise, // rotation rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2 focal_noise, distort_noise, distort_noise // f, k1, k2

10
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

@ -18,8 +18,8 @@
using namespace gtsam; using namespace gtsam;
const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas(Vector_(1, 0.1)); const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vec(1) << 0.1));
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3));
typedef PoseRotationPrior<Pose2> Pose2RotationPrior; typedef PoseRotationPrior<Pose2> Pose2RotationPrior;
typedef PoseRotationPrior<Pose3> Pose3RotationPrior; typedef PoseRotationPrior<Pose3> Pose3RotationPrior;
@ -30,7 +30,7 @@ const gtsam::Key poseKey = 1;
// Pose3 examples // Pose3 examples
const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); 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 // Pose2 examples
const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
@ -62,7 +62,7 @@ TEST( testPoseRotationFactor, level3_error ) {
Pose3 pose1(rot3A, point3A); Pose3 pose1(rot3A, point3A);
Pose3RotationPrior factor(poseKey, rot3C, model3); Pose3RotationPrior factor(poseKey, rot3C, model3);
Matrix actH1; 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<LieVector,Pose3>( Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
@ -84,7 +84,7 @@ TEST( testPoseRotationFactor, level2_error ) {
Pose2 pose1(rot2A, point2A); Pose2 pose1(rot2A, point2A);
Pose2RotationPrior factor(poseKey, rot2B, model1); Pose2RotationPrior factor(poseKey, rot2B, model1);
Matrix actH1; 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<LieVector,Pose2>( Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));

12
gtsam/slam/tests/testPoseTranslationPrior.cpp
View File

@ -15,8 +15,8 @@
using namespace gtsam; using namespace gtsam;
const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.2));
const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3));
typedef PoseTranslationPrior<Pose2> Pose2TranslationPrior; typedef PoseTranslationPrior<Pose2> Pose2TranslationPrior;
typedef PoseTranslationPrior<Pose3> Pose3TranslationPrior; typedef PoseTranslationPrior<Pose3> Pose3TranslationPrior;
@ -59,7 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) {
Pose3 pose1(rot3A, point3A); Pose3 pose1(rot3A, point3A);
Pose3TranslationPrior factor(poseKey, point3B, model3); Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1; 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<LieVector,Pose3>( Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
@ -81,7 +81,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) {
Pose3 pose1(rot3B, point3A); Pose3 pose1(rot3B, point3A);
Pose3TranslationPrior factor(poseKey, point3B, model3); Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1; 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<LieVector,Pose3>( Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
@ -103,7 +103,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) {
Pose3 pose1(rot3C, point3A); Pose3 pose1(rot3C, point3A);
Pose3TranslationPrior factor(poseKey, point3B, model3); Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1; 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<LieVector,Pose3>( Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
@ -125,7 +125,7 @@ TEST( testPoseTranslationFactor, level2_error ) {
Pose2 pose1(rot2A, point2A); Pose2 pose1(rot2A, point2A);
Pose2TranslationPrior factor(poseKey, point2B, model2); Pose2TranslationPrior factor(poseKey, point2B, model2);
Matrix actH1; 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<LieVector,Pose2>( Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));

4
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -108,7 +108,7 @@ TEST( ProjectionFactor, Error ) {
Vector actualError(factor.evaluateError(pose, point)); Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, 0.0) pixels / UnitCovariance // 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 // Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9)); CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -131,7 +131,7 @@ TEST( ProjectionFactor, ErrorWithTransform ) {
Vector actualError(factor.evaluateError(pose, point)); Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, 0.0) pixels / UnitCovariance // 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 // Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9)); CHECK(assert_equal(expectedError, actualError, 1e-9));

8
gtsam/slam/tests/testRangeFactor.cpp
View File

@ -118,7 +118,7 @@ TEST( RangeFactor, Error2D ) {
Vector actualError(factor.evaluateError(pose, point)); Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance // 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 // Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9)); CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -143,7 +143,7 @@ TEST( RangeFactor, Error2DWithTransform ) {
Vector actualError(factor.evaluateError(pose, point)); Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance // 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 // Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9)); CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -165,7 +165,7 @@ TEST( RangeFactor, Error3D ) {
Vector actualError(factor.evaluateError(pose, point)); Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance // 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 // Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9)); CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -190,7 +190,7 @@ TEST( RangeFactor, Error3DWithTransform ) {
Vector actualError(factor.evaluateError(pose, point)); Vector actualError(factor.evaluateError(pose, point));
// The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance // 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 // Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9)); CHECK(assert_equal(expectedError, actualError, 1e-9));

4
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -102,7 +102,7 @@ TEST( StereoFactor, Error ) {
Vector actualError(factor.evaluateError(pose, point)); Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance // 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 // Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9)); CHECK(assert_equal(expectedError, actualError, 1e-9));
@ -123,7 +123,7 @@ TEST( StereoFactor, ErrorWithTransform ) {
Vector actualError(factor.evaluateError(pose, point)); Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance // 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 // Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9)); CHECK(assert_equal(expectedError, actualError, 1e-9));