From 615c223f81281d804c33f4769bed6fbfe284d479 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Sun, 20 Oct 2013 07:10:17 +0000 Subject: [PATCH] Fix Vector_() to Vec() in gtsam/geometry --- gtsam/geometry/Cal3Bundler.cpp | 6 ++-- gtsam/geometry/Pose2.cpp | 6 ++-- gtsam/geometry/tests/testPoint2.cpp | 4 +-- gtsam/geometry/tests/testPoint3.cpp | 2 +- gtsam/geometry/tests/testPose2.cpp | 28 +++++++-------- gtsam/geometry/tests/testPose3.cpp | 20 +++++------ gtsam/geometry/tests/testRot3M.cpp | 40 ++++++++++----------- gtsam/geometry/tests/testRot3Q.cpp | 44 +++++++++++------------ gtsam/geometry/tests/testStereoPoint2.cpp | 4 +-- gtsam/geometry/tests/timePose3.cpp | 4 +-- 10 files changed, 79 insertions(+), 79 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 03c9cb4b0..927a90ac2 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -42,17 +42,17 @@ Matrix Cal3Bundler::K() const { /* ************************************************************************* */ Vector Cal3Bundler::k() const { - return Vector_(4, k1_, k2_, 0, 0); + return (Vec(4) << k1_, k2_, 0, 0); } /* ************************************************************************* */ Vector Cal3Bundler::vector() const { - return Vector_(3, f_, k1_, k2_); + return (Vec(3) << f_, k1_, k2_); } /* ************************************************************************* */ void Cal3Bundler::print(const std::string& s) const { - gtsam::print(Vector_(5, f_, k1_, k2_, u0_, v0_), s + ".K"); + gtsam::print((Vector)(Vec(5) << f_, k1_, k2_, u0_, v0_), s + ".K"); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index c0a3d43d2..a1add297d 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -75,13 +75,13 @@ Vector Pose2::Logmap(const Pose2& p) { const Point2& t = p.t(); double w = R.theta(); if (std::abs(w) < 1e-10) - return Vector_(3, t.x(), t.y(), w); + return (Vec(3) << t.x(), t.y(), w); else { double c_1 = R.c()-1.0, s = R.s(); double det = c_1*c_1 + s*s; Point2 p = R_PI_2 * (R.unrotate(t) - t); Point2 v = (w/det) * p; - return Vector_(3, v.x(), v.y(), w); + return (Vec(3) << v.x(), v.y(), w); } } @@ -101,7 +101,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const { return Logmap(between(p2)); #else Pose2 r = between(p2); - return Vector_(3, r.x(), r.y(), r.theta()); + return (Vec(3) << r.x(), r.y(), r.theta()); #endif } diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index b942d0eea..7d53f10c3 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -46,8 +46,8 @@ TEST(Point2, Lie) { EXPECT(assert_equal(-eye(2), H1)); EXPECT(assert_equal(eye(2), H2)); - EXPECT(assert_equal(Point2(5,7), p1.retract(Vector_(2, 4.,5.)))); - EXPECT(assert_equal(Vector_(2, 3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point2(5,7), p1.retract((Vec(2) << 4., 5.)))); + EXPECT(assert_equal((Vec(2) << 3.,3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 7be69f03b..60335aeab 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -39,7 +39,7 @@ TEST(Point3, Lie) { EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5,7,9), p1.retract(Vector_(3, 4.,5.,6.)))); + EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vec(3) << 4., 5., 6.)))); EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2))); } diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 779550324..2f6598143 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -68,7 +68,7 @@ TEST(Pose2, retract) { #else Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01)); #endif - Pose2 actual = pose.retract(Vector_(3, 0.01, -0.015, 0.99)); + Pose2 actual = pose.retract((Vec(3) << 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -76,7 +76,7 @@ TEST(Pose2, retract) { TEST(Pose2, expmap) { Pose2 pose(M_PI/2.0, Point2(1, 2)); Pose2 expected(1.00811, 2.01528, 2.5608); - Pose2 actual = expmap_default(pose, Vector_(3, 0.01, -0.015, 0.99)); + Pose2 actual = expmap_default(pose, (Vec(3) << 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -84,7 +84,7 @@ TEST(Pose2, expmap) { TEST(Pose2, expmap2) { Pose2 pose(M_PI/2.0, Point2(1, 2)); Pose2 expected(1.00811, 2.01528, 2.5608); - Pose2 actual = expmap_default(pose, Vector_(3, 0.01, -0.015, 0.99)); + Pose2 actual = expmap_default(pose, (Vec(3) << 0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -99,7 +99,7 @@ TEST(Pose2, expmap3) { Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0; Matrix expected = eye(3) + A + A2 + A3 + A4; - Vector v = Vector_(3, 0.01, -0.015, 0.99); + Vector v = (Vec(3) << 0.01, -0.015, 0.99); Pose2 pose = Pose2::Expmap(v); Pose2 pose2(v); EXPECT(assert_equal(pose, pose2)); @@ -110,7 +110,7 @@ TEST(Pose2, expmap3) { /* ************************************************************************* */ TEST(Pose2, expmap0a) { Pose2 expected(0.0101345, -0.0149092, 0.018); - Pose2 actual = Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); + Pose2 actual = Pose2::Expmap((Vec(3) << 0.01, -0.015, 0.018)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -118,7 +118,7 @@ TEST(Pose2, expmap0a) { TEST(Pose2, expmap0b) { // a quarter turn Pose2 expected(1.0, 1.0, M_PI/2); - Pose2 actual = Pose2::Expmap(Vector_(3, M_PI/2, 0.0, M_PI/2)); + Pose2 actual = Pose2::Expmap((Vec(3) << M_PI/2, 0.0, M_PI/2)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -126,7 +126,7 @@ TEST(Pose2, expmap0b) { TEST(Pose2, expmap0c) { // a half turn Pose2 expected(0.0, 2.0, M_PI); - Pose2 actual = Pose2::Expmap(Vector_(3, M_PI, 0.0, M_PI)); + Pose2 actual = Pose2::Expmap((Vec(3) << M_PI, 0.0, M_PI)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -134,7 +134,7 @@ TEST(Pose2, expmap0c) { TEST(Pose2, expmap0d) { // a full turn Pose2 expected(0, 0, 0); - Pose2 actual = Pose2::Expmap(Vector_(3, 2*M_PI, 0.0, 2*M_PI)); + Pose2 actual = Pose2::Expmap((Vec(3) << 2*M_PI, 0.0, 2*M_PI)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -143,7 +143,7 @@ TEST(Pose2, expmap0d) { // test case for screw motion in the plane namespace screw { double w=0.3; - Vector xi = Vector_(3, 0.0, w, w); + Vector xi = (Vec(3) << 0.0, w, w); Rot2 expectedR = Rot2::fromAngle(w); Point2 expectedT(-0.0446635, 0.29552); Pose2 expected(expectedR, expectedT); @@ -161,7 +161,7 @@ TEST(Pose3, expmap_c) TEST(Pose2, expmap_c_full) { double w=0.3; - Vector xi = Vector_(3, 0.0, w, w); + Vector xi = (Vec(3) << 0.0, w, w); Rot2 expectedR = Rot2::fromAngle(w); Point2 expectedT(-0.0446635, 0.29552); Pose2 expected(expectedR, expectedT); @@ -175,9 +175,9 @@ TEST(Pose2, logmap) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); #ifdef SLOW_BUT_CORRECT_EXPMAP - Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); + Vector expected = (Vec(3) << 0.00986473, -0.0150896, 0.018); #else - Vector expected = Vector_(3, 0.01, -0.015, 0.018); + Vector expected = (Vec(3) << 0.01, -0.015, 0.018); #endif Vector actual = pose0.localCoordinates(pose); EXPECT(assert_equal(expected, actual, 1e-5)); @@ -187,7 +187,7 @@ TEST(Pose2, logmap) { TEST(Pose2, logmap_full) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); - Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); + Vector expected = (Vec(3) << 0.00986473, -0.0150896, 0.018); Vector actual = logmap_default(pose0, pose); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -348,7 +348,7 @@ TEST(Pose2, inverse ) namespace { /* ************************************************************************* */ Vector homogeneous(const Point2& p) { - return Vector_(3, p.x(), p.y(), 1.0); + return (Vec(3) << p.x(), p.y(), 1.0); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index ce193a0e5..9d10e2865 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -104,7 +104,7 @@ TEST( Pose3, expmap_a_full2) TEST(Pose3, expmap_b) { Pose3 p1(Rot3(), Point3(100, 0, 0)); - Pose3 p2 = p1.retract(Vector_(6,0.0, 0.0, 0.1, 0.0, 0.0, 0.0)); + Pose3 p2 = p1.retract((Vec(6) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0)); Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0)); EXPECT(assert_equal(expected, p2,1e-2)); } @@ -113,7 +113,7 @@ TEST(Pose3, expmap_b) // test case for screw motion in the plane namespace screw { double a=0.3, c=cos(a), s=sin(a), w=0.3; - Vector xi = Vector_(6, 0.0, 0.0, w, w, 0.0, 1.0); + Vector xi = (Vec(6) << 0.0, 0.0, w, w, 0.0, 1.0); Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); Point3 expectedT(0.29552, 0.0446635, 1); Pose3 expected(expectedR, expectedT); @@ -163,13 +163,13 @@ Pose3 Agrawal06iros(const Vector& xi) { TEST(Pose3, expmaps_galore_full) { Vector xi; Pose3 actual; - xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); + xi = (Vec(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6); actual = Pose3::Expmap(xi); EXPECT(assert_equal(expm(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6)); - xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6); + xi = (Vec(6) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6); for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { Vector txi = xi*theta; actual = Pose3::Expmap(txi); @@ -181,7 +181,7 @@ TEST(Pose3, expmaps_galore_full) } // Works with large v as well, but expm needs 10 iterations! - xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0); + xi = (Vec(6) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0); actual = Pose3::Expmap(xi); EXPECT(assert_equal(expm(xi,10), actual,1e-5)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); @@ -194,7 +194,7 @@ TEST(Pose3, Adjoint_compose_full) // To debug derivatives of compose, assert that // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 const Pose3& T1 = T; - Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); + Vector x = (Vec(6) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8); Pose3 expected = T1 * Pose3::Expmap(x) * T2; Vector y = T2.inverse().Adjoint(x); Pose3 actual = T1 * T2 * Pose3::Expmap(y); @@ -510,7 +510,7 @@ TEST(Pose3, subgroups) { // Frank - Below only works for correct "Agrawal06iros style expmap // lines in canonical coordinates correspond to Abelian subgroups in SE(3) - Vector d = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); + Vector d = (Vec(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6); // exp(-d)=inverse(exp(d)) EXPECT(assert_equal(Pose3::Expmap(-d),Pose3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -675,7 +675,7 @@ TEST(Pose3, align_2) { /// exp(xi) exp(y) = exp(xi + x) /// Hence, y = log (exp(-xi)*exp(xi+x)) -Vector xi = Vector_(6, 0.1, 0.2, 0.3, 1.0, 2.0, 3.0); +Vector xi = (Vec(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0); Vector testDerivExpmapInv(const LieVector& dxi) { Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+dxi)); @@ -723,8 +723,8 @@ Vector testDerivAdjointTranspose(const LieVector& xi, const LieVector& v) { } TEST( Pose3, adjointTranspose) { - Vector xi = Vector_(6, 0.01, 0.02, 0.03, 1.0, 2.0, 3.0); - Vector v = Vector_(6, 0.04, 0.05, 0.06, 4.0, 5.0, 6.0); + Vector xi = (Vec(6) << 0.01, 0.02, 0.03, 1.0, 2.0, 3.0); + Vector v = (Vec(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0); Vector expected = testDerivAdjointTranspose(xi, v); Matrix actualH; diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 5874af9a0..59d752d75 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -101,7 +101,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { TEST( Rot3, rodriguez) { Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); - Vector w = Vector_(3, epsilon, 0., 0.); + Vector w = (Vec(3) << epsilon, 0., 0.); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } @@ -109,7 +109,7 @@ TEST( Rot3, rodriguez) /* ************************************************************************* */ TEST( Rot3, rodriguez2) { - Vector axis = Vector_(3,0.,1.,0.); // rotation around Y + Vector axis = (Vec(3) << 0., 1., 0.); // rotation around Y double angle = 3.14 / 4.0; Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 expected(0.707388, 0, 0.706825, @@ -121,7 +121,7 @@ TEST( Rot3, rodriguez2) /* ************************************************************************* */ TEST( Rot3, rodriguez3) { - Vector w = Vector_(3, 0.1, 0.2, 0.3); + Vector w = (Vec(3) << 0.1, 0.2, 0.3); Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); @@ -130,7 +130,7 @@ TEST( Rot3, rodriguez3) /* ************************************************************************* */ TEST( Rot3, rodriguez4) { - Vector axis = Vector_(3,0.,0.,1.); // rotation around Z + Vector axis = (Vec(3) << 0., 0., 1.); // rotation around Z double angle = M_PI/2.0; Rot3 actual = Rot3::rodriguez(axis, angle); double c=cos(angle),s=sin(angle); @@ -156,7 +156,7 @@ TEST(Rot3, log) Rot3 R; #define CHECK_OMEGA(X,Y,Z) \ - w = Vector_(3, (double)X, (double)Y, double(Z)); \ + w = (Vec(3) << (double)X, (double)Y, double(Z)); \ R = Rot3::rodriguez(w); \ EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); @@ -190,7 +190,7 @@ TEST(Rot3, log) // Check 360 degree rotations #define CHECK_OMEGA_ZERO(X,Y,Z) \ - w = Vector_(3, (double)X, (double)Y, double(Z)); \ + w = (Vec(3) << (double)X, (double)Y, double(Z)); \ R = Rot3::rodriguez(w); \ EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); @@ -217,7 +217,7 @@ TEST(Rot3, manifold_caley) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = Vector_(3, 0.1, 0.2, 0.3); + Vector d = (Vec(3) << 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -245,7 +245,7 @@ TEST(Rot3, manifold_slow_caley) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = Vector_(3, 0.1, 0.2, 0.3); + Vector d = (Vec(3) << 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -277,7 +277,7 @@ TEST(Rot3, manifold_expmap) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = Vector_(3, 0.1, 0.2, 0.3); + Vector d = (Vec(3) << 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -403,7 +403,7 @@ TEST( Rot3, between ) } /* ************************************************************************* */ -Vector w = Vector_(3, 0.1, 0.27, -0.2); +Vector w = (Vec(3) << 0.1, 0.27, -0.2); // Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes? // We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 @@ -473,7 +473,7 @@ TEST( Rot3, yaw_pitch_roll ) Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3); CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),expected.ypr())); + CHECK(assert_equal((Vector)(Vec(3) << 0.1, 0.2, 0.3),expected.ypr())); } /* ************************************************************************* */ @@ -483,22 +483,22 @@ TEST( Rot3, RQ) Matrix actualK; Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); - Vector expected = Vector_(3, 0.14715, 0.385821, 0.231671); + Vector expected = (Vec(3) << 0.14715, 0.385821, 0.231671); CHECK(assert_equal(I3,actualK)); CHECK(assert_equal(expected,actual,1e-6)); // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] CHECK(assert_equal(expected,R.xyz(),1e-6)); - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); + CHECK(assert_equal((Vector)(Vec(3) << 0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); + CHECK(assert_equal((Vector)(Vec(3) << 0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vector)(Vec(3) << 0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr())); + CHECK(assert_equal((Vector)(Vec(3) << 0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal((Vector)(Vec(3) << 0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal((Vector)(Vec(3) << 0.0,0.0,0.1),Rot3::roll (0.1).ypr())); // Try RQ to recover calibration from 3*3 sub-block of projection matrix Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); @@ -510,7 +510,7 @@ TEST( Rot3, RQ) /* ************************************************************************* */ TEST( Rot3, expmapStability ) { - Vector w = Vector_(3, 78e-9, 5e-8, 97e-7); + Vector w = (Vec(3) << 78e-9, 5e-8, 97e-7); double theta = w.norm(); double theta2 = theta*theta; Rot3 actualR = Rot3::Expmap(w); @@ -526,7 +526,7 @@ TEST( Rot3, expmapStability ) { /* ************************************************************************* */ TEST( Rot3, logmapStability ) { - Vector w = Vector_(3, 1e-8, 0.0, 0.0); + Vector w = (Vec(3) << 1e-8, 0.0, 0.0); Rot3 R = Rot3::Expmap(w); // double tr = R.r1().x()+R.r2().y()+R.r3().z(); // std::cout.precision(5000); diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index d44b8f34c..47e1dc2b5 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -88,7 +88,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { TEST( Rot3, rodriguez) { Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); - Vector w = Vector_(3, epsilon, 0., 0.); + Vector w = (Vec(3) << epsilon, 0., 0.); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } @@ -96,7 +96,7 @@ TEST( Rot3, rodriguez) /* ************************************************************************* */ TEST( Rot3, rodriguez2) { - Vector axis = Vector_(3,0.,1.,0.); // rotation around Y + Vector axis = (Vec(3) << 0.,1.,0.); // rotation around Y double angle = 3.14 / 4.0; Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 expected(0.707388, 0, 0.706825, @@ -108,7 +108,7 @@ TEST( Rot3, rodriguez2) /* ************************************************************************* */ TEST( Rot3, rodriguez3) { - Vector w = Vector_(3, 0.1, 0.2, 0.3); + Vector w = (Vec(3) << 0.1, 0.2, 0.3); Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); @@ -117,7 +117,7 @@ TEST( Rot3, rodriguez3) /* ************************************************************************* */ TEST( Rot3, rodriguez4) { - Vector axis = Vector_(3,0.,0.,1.); // rotation around Z + Vector axis = (Vec(3) << 0., 0., 1.); // rotation around Z double angle = M_PI_2; Rot3 actual = Rot3::rodriguez(axis, angle); double c=cos(angle),s=sin(angle); @@ -138,35 +138,35 @@ TEST( Rot3, expmap) /* ************************************************************************* */ TEST(Rot3, log) { - Vector w1 = Vector_(3, 0.1, 0.0, 0.0); + Vector w1 = (Vec(3) << 0.1, 0.0, 0.0); Rot3 R1 = Rot3::rodriguez(w1); CHECK(assert_equal(w1, Rot3::Logmap(R1))); - Vector w2 = Vector_(3, 0.0, 0.1, 0.0); + Vector w2 = (Vec(3) << 0.0, 0.1, 0.0); Rot3 R2 = Rot3::rodriguez(w2); CHECK(assert_equal(w2, Rot3::Logmap(R2))); - Vector w3 = Vector_(3, 0.0, 0.0, 0.1); + Vector w3 = (Vec(3) << 0.0, 0.0, 0.1); Rot3 R3 = Rot3::rodriguez(w3); CHECK(assert_equal(w3, Rot3::Logmap(R3))); - Vector w = Vector_(3, 0.1, 0.4, 0.2); + Vector w = (Vec(3) << 0.1, 0.4, 0.2); Rot3 R = Rot3::rodriguez(w); CHECK(assert_equal(w, Rot3::Logmap(R))); - Vector w5 = Vector_(3, 0.0, 0.0, 0.0); + Vector w5 = (Vec(3) << 0.0, 0.0, 0.0); Rot3 R5 = Rot3::rodriguez(w5); CHECK(assert_equal(w5, Rot3::Logmap(R5))); - Vector w6 = Vector_(3, boost::math::constants::pi(), 0.0, 0.0); + Vector w6 = (Vec(3) << boost::math::constants::pi(), 0.0, 0.0); Rot3 R6 = Rot3::rodriguez(w6); CHECK(assert_equal(w6, Rot3::Logmap(R6))); - Vector w7 = Vector_(3, 0.0, boost::math::constants::pi(), 0.0); + Vector w7 = (Vec(3) << 0.0, boost::math::constants::pi(), 0.0); Rot3 R7 = Rot3::rodriguez(w7); CHECK(assert_equal(w7, Rot3::Logmap(R7))); - Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi()); + Vector w8 = (Vec(3) << 0.0, 0.0, boost::math::constants::pi()); Rot3 R8 = Rot3::rodriguez(w8); CHECK(assert_equal(w8, Rot3::Logmap(R8))); } @@ -190,7 +190,7 @@ TEST(Rot3, manifold) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = Vector_(3, 0.1, 0.2, 0.3); + Vector d = (Vec(3) << 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -381,22 +381,22 @@ TEST( Rot3, RQ) Matrix actualK; Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); - Vector expected = Vector_(3, 0.14715, 0.385821, 0.231671); + Vector expected = (Vec(3) << 0.14715, 0.385821, 0.231671); CHECK(assert_equal(eye(3),actualK)); CHECK(assert_equal(expected,actual,1e-6)); // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] CHECK(assert_equal(expected,R.xyz(),1e-6)); - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); + CHECK(assert_equal((Vec(3) <<0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); + CHECK(assert_equal((Vec(3) <<0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vec(3) <<0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr())); + CHECK(assert_equal((Vec(3) <<0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal((Vec(3) <<0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal((Vec(3) <<0.0,0.0,0.1),Rot3::roll (0.1).ypr())); // Try RQ to recover calibration from 3*3 sub-block of projection matrix Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); @@ -408,7 +408,7 @@ TEST( Rot3, RQ) /* ************************************************************************* */ TEST( Rot3, expmapStability ) { - Vector w = Vector_(3, 78e-9, 5e-8, 97e-7); + Vector w = (Vec(3) << 78e-9, 5e-8, 97e-7); double theta = w.norm(); double theta2 = theta*theta; Rot3 actualR = Rot3::Expmap(w); @@ -425,7 +425,7 @@ TEST( Rot3, expmapStability ) { // Does not work with Quaternions ///* ************************************************************************* */ //TEST( Rot3, logmapStability ) { -// Vector w = Vector_(3, 1e-8, 0.0, 0.0); +// Vector w = (Vec(3) << 1e-8, 0.0, 0.0); // Rot3 R = Rot3::Expmap(w); //// double tr = R.r1().x()+R.r2().y()+R.r3().z(); //// std::cout.precision(5000); diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index 4faeaec42..96a950ba5 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -44,8 +44,8 @@ TEST(StereoPoint2, Lie) { EXPECT(assert_equal(StereoPoint2(3,3,3), p1.between(p2))); - EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract(Vector_(3, 4.,5.,6.)))); - EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract((Vec(3) << 4., 5., 6.)))); + EXPECT(assert_equal((Vec(3) << 3., 3., 3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/timePose3.cpp b/gtsam/geometry/tests/timePose3.cpp index a3bbdf8d9..9538ad4b4 100644 --- a/gtsam/geometry/tests/timePose3.cpp +++ b/gtsam/geometry/tests/timePose3.cpp @@ -37,8 +37,8 @@ int main() double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; - Vector v = Vector_(6,x,y,z,0.1,0.2,-0.1); - Pose3 T = Pose3::Expmap(Vector_(6,0.1,0.1,0.2,0.1, 0.4, 0.2)), T2 = T.retract(v); + Vector v = (Vec(6) << x, y, z, 0.1, 0.2, -0.1); + Pose3 T = Pose3::Expmap((Vec(6) << 0.1, 0.1, 0.2, 0.1, 0.4, 0.2)), T2 = T.retract(v); Matrix H1,H2; TEST(retract, T.retract(v))