diff --git a/gtsam/base/LieVector.cpp b/gtsam/base/LieVector.cpp index c86b51a24..f6cae28e2 100644 --- a/gtsam/base/LieVector.cpp +++ b/gtsam/base/LieVector.cpp @@ -30,19 +30,6 @@ LieVector::LieVector(size_t m, const double* const data) (*this)(i) = data[i]; } -/* ************************************************************************* */ -LieVector::LieVector(size_t m, ...) -: Vector(m) -{ - va_list ap; - va_start(ap, m); - for( size_t i = 0 ; i < m ; i++) { - double value = va_arg(ap, double); - (*this)(i) = value; - } - va_end(ap); -} - /* ************************************************************************* */ void LieVector::print(const std::string& name) const { gtsam::print(vector(), name); diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index f1b05b34b..6989bbfd2 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -44,9 +44,6 @@ struct LieVector : public Vector, public DerivedValue { /** constructor with size and initial data, row order ! */ GTSAM_EXPORT LieVector(size_t m, const double* const data); - /** Specify arguments directly, as in Vector_() - always force these to be doubles */ - GTSAM_EXPORT LieVector(size_t m, ...); - /** get the underlying vector */ Vector vector() const { return static_cast(*this); diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index 67eed6656..f66678c25 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -39,14 +39,9 @@ TEST( testLieVector, construction ) { TEST( testLieVector, other_constructors ) { Vector init = (Vector(2) << 10.0, 20.0); LieVector exp(init); - LieVector a(2,10.0,20.0); double data[] = {10,20}; LieVector b(2,data); - LieVector c(2.3), c_exp(LieVector(1, 2.3)); - EXPECT(assert_equal(exp, a)); EXPECT(assert_equal(exp, b)); - EXPECT(assert_equal(b, a)); - EXPECT(assert_equal(c_exp, c)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index bd3c2c217..f7e4d3baa 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -48,7 +48,8 @@ double f2(const LieVector& x) { /* ************************************************************************* */ TEST(testNumericalDerivative, numericalHessian2) { - LieVector center(2, 0.5, 1.0); + Vector v_center = (Vector(2) << 0.5, 1.0); + LieVector center(v_center); Matrix expected = (Matrix(2,2) << -cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)), @@ -67,7 +68,9 @@ double f3(const LieVector& x1, const LieVector& x2) { /* ************************************************************************* */ TEST(testNumericalDerivative, numericalHessian211) { - LieVector center1(1, 1.0), center2(1, 5.0); + Vector v_center1 = (Vector(1) << 1.0); + Vector v_center2 = (Vector(1) << 5.0); + LieVector center1(v_center1), center2(v_center2); Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0))); Matrix actual11 = numericalHessian211(f3, center1, center2); @@ -90,7 +93,11 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) { /* ************************************************************************* */ TEST(testNumericalDerivative, numericalHessian311) { - LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0); + Vector v_center1 = (Vector(1) << 1.0); + Vector v_center2 = (Vector(1) << 2.0); + Vector v_center3 = (Vector(1) << 3.0); + LieVector center1(v_center1), center2(v_center2), center3(v_center3); + double x = center1(0), y = center2(0), z = center3(0); Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); Matrix actual11 = numericalHessian311(f4, center1, center2, center3); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 25a2765eb..2d766dc6a 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -168,9 +168,9 @@ TEST( CombinedImuFactor, ErrorWithBiases ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 2db7c7c68..cc88505bd 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -167,9 +167,9 @@ TEST( ImuFactor, Error ) // Linearization point imuBias::ConstantBias bias; // Bias Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -238,16 +238,16 @@ TEST( ImuFactor, ErrorWithBiases ) // Linearization point // Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1(3, 0.5, 0.0, 0.0); +// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2(3, 0.5, 0.0, 0.0); +// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -445,9 +445,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) //{ // // Linearization point // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1(3, 0.5, 0.0, 0.0); +// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2(3, 0.5, 0.0, 0.0); +// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); // // // Pre-integrator @@ -501,9 +501,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index ec386950d..041ea0387 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -65,7 +65,8 @@ public: TEST( Values, equals1 ) { Values expected; - LieVector v(3, 5.0, 6.0, 7.0); + LieVector v((Vector(3) << 5.0, 6.0, 7.0)); + expected.insert(key1,v); Values actual; actual.insert(key1,v); @@ -76,8 +77,9 @@ TEST( Values, equals1 ) TEST( Values, equals2 ) { Values cfg1, cfg2; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 5.0, 6.0, 8.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 5.0, 6.0, 8.0)); + cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -88,8 +90,9 @@ TEST( Values, equals2 ) TEST( Values, equals_nan ) { Values cfg1, cfg2; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, inf, inf, inf); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << inf, inf, inf)); + cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -100,10 +103,11 @@ TEST( Values, equals_nan ) TEST( Values, insert_good ) { Values cfg1, cfg2, expected; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 8.0, 9.0, 1.0); - LieVector v3(3, 2.0, 4.0, 3.0); - LieVector v4(3, 8.0, 3.0, 7.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); + LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); + LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); + cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key3, v4); @@ -121,10 +125,11 @@ TEST( Values, insert_good ) TEST( Values, insert_bad ) { Values cfg1, cfg2; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 8.0, 9.0, 1.0); - LieVector v3(3, 2.0, 4.0, 3.0); - LieVector v4(3, 8.0, 3.0, 7.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); + LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); + LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); + cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key2, v3); @@ -137,8 +142,8 @@ TEST( Values, insert_bad ) TEST( Values, update_element ) { Values cfg; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 8.0, 9.0, 1.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); cfg.insert(key1, v1); CHECK(cfg.size() == 1); @@ -181,8 +186,8 @@ TEST(Values, basic_functions) //TEST(Values, dim_zero) //{ // Values config0; -// config0.insert(key1, LieVector(2, 2.0, 3.0)); -// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); +// config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0)); +// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)); // LONGS_EQUAL(5, config0.dim()); // // VectorValues expected; @@ -195,16 +200,16 @@ TEST(Values, basic_functions) TEST(Values, expmap_a) { Values config0; - config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); VectorValues increment = pair_list_of (key1, (Vector(3) << 1.0, 1.1, 1.2)) (key2, (Vector(3) << 1.3, 1.4, 1.5)); Values expected; - expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); - expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); + expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); + expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); CHECK(assert_equal(expected, config0.retract(increment))); } @@ -213,15 +218,15 @@ TEST(Values, expmap_a) TEST(Values, expmap_b) { Values config0; - config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); VectorValues increment = pair_list_of (key2, (Vector(3) << 1.3, 1.4, 1.5)); Values expected; - expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); + expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); CHECK(assert_equal(expected, config0.retract(increment))); } @@ -230,16 +235,16 @@ TEST(Values, expmap_b) //TEST(Values, expmap_c) //{ // Values config0; -// config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); -// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); +// config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); +// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); // // Vector increment = LieVector(6, // 1.0, 1.1, 1.2, // 1.3, 1.4, 1.5); // // Values expected; -// expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); -// expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); +// expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); +// expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); // // CHECK(assert_equal(expected, config0.retract(increment))); //} @@ -248,8 +253,8 @@ TEST(Values, expmap_b) TEST(Values, expmap_d) { Values config0; - config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); //config0.print("config0"); CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); @@ -266,8 +271,8 @@ TEST(Values, expmap_d) TEST(Values, localCoordinates) { Values valuesA; - valuesA.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); VectorValues expDelta = pair_list_of (key1, (Vector(3) << 0.1, 0.2, 0.3)) @@ -314,17 +319,17 @@ TEST(Values, exists_) TEST(Values, update) { Values config0; - config0.insert(key1, LieVector(1, 1.)); - config0.insert(key2, LieVector(1, 2.)); + config0.insert(key1, LieVector((Vector(1) << 1.))); + config0.insert(key2, LieVector((Vector(1) << 2.))); Values superset; - superset.insert(key1, LieVector(1, -1.)); - superset.insert(key2, LieVector(1, -2.)); + superset.insert(key1, LieVector((Vector(1) << -1.))); + superset.insert(key2, LieVector((Vector(1) << -2.))); config0.update(superset); Values expected; - expected.insert(key1, LieVector(1, -1.)); - expected.insert(key2, LieVector(1, -2.)); + expected.insert(key1, LieVector((Vector(1) << -1.))); + expected.insert(key2, LieVector((Vector(1) << -2.))); CHECK(assert_equal(expected,config0)); } diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index b37b36546..a7c91de3f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -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(((Vector) (Vector(2) << -3.0, 0.0)), factor->unwhitenedError(values))); } static const double baseline = 5.0 ; diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index c33505c54..4eb7992a2 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -166,7 +166,7 @@ public: gtsam::Point3 ray = pw - pt; double theta = atan2(ray.y(), ray.x()); // longitude double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); - return std::make_pair(gtsam::LieVector(5, pt.x(),pt.y(),pt.z(), theta, phi), + return std::make_pair(gtsam::LieVector((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)), gtsam::LieScalar(1./depth)); } diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index e899da0c9..b477d3e44 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark(5, 1., 0., 1., 0., 0.); + LieVector inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); LieScalar inv_depth(1./4); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); EXPECT(assert_equal(expected_uv, actual_uv)); @@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))); + LieVector diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); LieScalar inv_depth(1/sqrt(3.0)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))); + LieVector diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); LieScalar inv_depth( 1./sqrt(1.0+1+4)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))); + LieVector diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); LieScalar inv_depth(1./sqrt(1.+16.+4.)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -88,7 +88,7 @@ Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& i TEST( InvDepthFactor, Dproject_pose) { - LieVector landmark(6,0.1,0.2,0.3, 0.1,0.2); + LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_landmark) { - LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); + LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_inv_depth) { - LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); + LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -124,7 +124,7 @@ TEST( InvDepthFactor, Dproject_inv_depth) /* ************************************************************************* */ TEST(InvDepthFactor, backproject) { - LieVector expected(5,0.,0.,1., 0.1,0.2); + LieVector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); LieScalar inv_depth(1./4); InvDepthCamera3 inv_camera(level_pose,K); Point2 z = inv_camera.project(expected, inv_depth); @@ -140,7 +140,7 @@ TEST(InvDepthFactor, backproject) TEST(InvDepthFactor, backproject2) { // backwards facing camera - LieVector expected(5,-5.,-5.,2., 3., -0.1); + LieVector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); LieScalar inv_depth(1./10); InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); Point2 z = inv_camera.project(expected, inv_depth); diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 53d94a4bc..7756e79d3 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -119,10 +119,10 @@ TEST( InertialNavFactor_GlobalVelocity, Predict) InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - LieVector Vel1(3, 0.50, -0.50, 0.40); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector expectedVel2(3, 0.51, -0.48, 0.43); + LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); Pose3 actualPose2; LieVector actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); @@ -157,8 +157,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector Vel1(3, 0.50, -0.50, 0.40); - LieVector Vel2(3, 0.51, -0.48, 0.43); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); + LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -192,8 +192,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot) Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0)); - LieVector Vel1(3,0.0,0.0,0.0); - LieVector Vel2(3,0.0,0.0,0.0); + LieVector Vel1((Vector(3) << 0.0, 0.0, 0.0)); + LieVector Vel2((Vector(3) << 0.0, 0.0, 0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -230,7 +230,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); @@ -302,13 +302,13 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) { -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Pose3 Pose2(R2, t2); - LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); + LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); imuBias::ConstantBias Bias1; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; @@ -447,10 +447,10 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - LieVector Vel1(3, 0.50, -0.50, 0.40); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector expectedVel2(3, 0.51, -0.48, 0.43); + LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); Pose3 actualPose2; LieVector actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); @@ -488,8 +488,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector Vel1(3, 0.50, -0.50, 0.40); - LieVector Vel2(3, 0.51, -0.48, 0.43); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); + LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -527,8 +527,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); Pose3 Pose2(Rot3::Expmap(body_P_sensor.rotation().matrix()*measurement_gyro*measurement_dt), Point3(2.0, 1.0, 3.0)); - LieVector Vel1(3,0.0,0.0,0.0); - LieVector Vel2(3,0.0,0.0,0.0); + LieVector Vel1((Vector(3) << 0.0,0.0,0.0)); + LieVector Vel2((Vector(3) << 0.0,0.0,0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -569,7 +569,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); @@ -618,13 +618,13 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Pose3 Pose2(R2, t2); - LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); + LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); imuBias::ConstantBias Bias1; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 65cc1adbc..5ea9fe29d 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -38,7 +38,7 @@ TEST( InvDepthFactor, optimize) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark(5, 0., 0., 1., 0., 0.); + LieVector inv_landmark((Vector(5) << 0., 0., 1., 0., 0.)); // initialize inverse depth with "incorrect" depth of 1/4 // in reality this is 1/5, but initial depth is guessed LieScalar inv_depth(1./4); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 7bbee65ee..24535854d 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -45,7 +45,7 @@ TEST( InvDepthFactorVariant1, optimize) { double theta = atan2(ray.y(), ray.x()); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double rho = 1./ray.norm(); - LieVector expected(6, x, y, z, theta, phi, rho); + LieVector expected((Vector(6) << x, y, z, theta, phi, rho)); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index 1512d0fc2..e99c9bcdf 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant2, optimize) { double theta = atan2(ray.y(), ray.x()); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double rho = 1./ray.norm(); - LieVector expected(3, theta, phi, rho); + LieVector expected((Vector(3) << theta, phi, rho)); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index 799ebdf36..e65b7cacb 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant3, optimize) { double theta = atan2(landmark_p1.x(), landmark_p1.z()); double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z())); double rho = 1./landmark_p1.norm(); - LieVector expected(3, theta, phi, rho); + LieVector expected((Vector(3) << theta, phi, rho)); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 247c6a6b0..390257f02 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -266,10 +266,10 @@ public: TEST(NonlinearFactor, NoiseModelFactor4) { TestFactor4 tf; Values tv; - tv.insert(X(1), LieVector(1, 1.0)); - tv.insert(X(2), LieVector(1, 2.0)); - tv.insert(X(3), LieVector(1, 3.0)); - tv.insert(X(4), LieVector(1, 4.0)); + tv.insert(X(1), LieVector((Vector(1) << 1.0))); + tv.insert(X(2), LieVector((Vector(1) << 2.0))); + tv.insert(X(3), LieVector((Vector(1) << 3.0))); + tv.insert(X(4), LieVector((Vector(1) << 4.0))); EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); @@ -312,11 +312,11 @@ public: TEST(NonlinearFactor, NoiseModelFactor5) { TestFactor5 tf; Values tv; - tv.insert(X(1), LieVector(1, 1.0)); - tv.insert(X(2), LieVector(1, 2.0)); - tv.insert(X(3), LieVector(1, 3.0)); - tv.insert(X(4), LieVector(1, 4.0)); - tv.insert(X(5), LieVector(1, 5.0)); + tv.insert(X(1), LieVector((Vector(1) << 1.0))); + tv.insert(X(2), LieVector((Vector(1) << 2.0))); + tv.insert(X(3), LieVector((Vector(1) << 3.0))); + tv.insert(X(4), LieVector((Vector(1) << 4.0))); + tv.insert(X(5), LieVector((Vector(1) << 5.0))); EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); @@ -364,12 +364,12 @@ public: TEST(NonlinearFactor, NoiseModelFactor6) { TestFactor6 tf; Values tv; - tv.insert(X(1), LieVector(1, 1.0)); - tv.insert(X(2), LieVector(1, 2.0)); - tv.insert(X(3), LieVector(1, 3.0)); - tv.insert(X(4), LieVector(1, 4.0)); - tv.insert(X(5), LieVector(1, 5.0)); - tv.insert(X(6), LieVector(1, 6.0)); + tv.insert(X(1), LieVector((Vector(1) << 1.0))); + tv.insert(X(2), LieVector((Vector(1) << 2.0))); + tv.insert(X(3), LieVector((Vector(1) << 3.0))); + tv.insert(X(4), LieVector((Vector(1) << 4.0))); + tv.insert(X(5), LieVector((Vector(1) << 5.0))); + tv.insert(X(6), LieVector((Vector(1) << 6.0))); EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv)));