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

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

release/4.3a0
Jing Dong 2013-10-20 07:10:17 +00:00
parent 019cd62d3e
commit 615c223f81
10 changed files with 79 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Cal3Bundler.cpp
View File

@ -42,17 +42,17 @@ Matrix Cal3Bundler::K() const {
/* ************************************************************************* */ /* ************************************************************************* */
Vector 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 { Vector Cal3Bundler::vector() const {
return Vector_(3, f_, k1_, k2_); return (Vec(3) << f_, k1_, k2_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Cal3Bundler::print(const std::string& s) const { 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");
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/geometry/Pose2.cpp
View File

@ -75,13 +75,13 @@ Vector Pose2::Logmap(const Pose2& p) {
const Point2& t = p.t(); const Point2& t = p.t();
double w = R.theta(); double w = R.theta();
if (std::abs(w) < 1e-10) if (std::abs(w) < 1e-10)
return Vector_(3, t.x(), t.y(), w); return (Vec(3) << t.x(), t.y(), w);
else { else {
double c_1 = R.c()-1.0, s = R.s(); double c_1 = R.c()-1.0, s = R.s();
double det = c_1*c_1 + s*s; double det = c_1*c_1 + s*s;
Point2 p = R_PI_2 * (R.unrotate(t) - t); Point2 p = R_PI_2 * (R.unrotate(t) - t);
Point2 v = (w/det) * p; 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)); return Logmap(between(p2));
#else #else
Pose2 r = between(p2); Pose2 r = between(p2);
return Vector_(3, r.x(), r.y(), r.theta()); return (Vec(3) << r.x(), r.y(), r.theta());
#endif #endif
} }

4
gtsam/geometry/tests/testPoint2.cpp
View File

@ -46,8 +46,8 @@ TEST(Point2, Lie) {
EXPECT(assert_equal(-eye(2), H1)); EXPECT(assert_equal(-eye(2), H1));
EXPECT(assert_equal(eye(2), H2)); EXPECT(assert_equal(eye(2), H2));
EXPECT(assert_equal(Point2(5,7), p1.retract(Vector_(2, 4.,5.)))); EXPECT(assert_equal(Point2(5,7), p1.retract((Vec(2) << 4., 5.))));
EXPECT(assert_equal(Vector_(2, 3.,3.), p1.localCoordinates(p2))); EXPECT(assert_equal((Vec(2) << 3.,3.), p1.localCoordinates(p2)));
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/geometry/tests/testPoint3.cpp
View File

@ -39,7 +39,7 @@ TEST(Point3, Lie) {
EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(-eye(3), H1));
EXPECT(assert_equal(eye(3), H2)); 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))); EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2)));
} }

28
gtsam/geometry/tests/testPose2.cpp
View File

@ -68,7 +68,7 @@ TEST(Pose2, retract) {
#else #else
Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01)); Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01));
#endif #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)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -76,7 +76,7 @@ TEST(Pose2, retract) {
TEST(Pose2, expmap) { TEST(Pose2, expmap) {
Pose2 pose(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608); Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99)); Pose2 actual = expmap_default<Pose2>(pose, (Vec(3) << 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -84,7 +84,7 @@ TEST(Pose2, expmap) {
TEST(Pose2, expmap2) { TEST(Pose2, expmap2) {
Pose2 pose(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608); Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99)); Pose2 actual = expmap_default<Pose2>(pose, (Vec(3) << 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5)); 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 A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0;
Matrix expected = eye(3) + A + A2 + A3 + A4; 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 pose = Pose2::Expmap(v);
Pose2 pose2(v); Pose2 pose2(v);
EXPECT(assert_equal(pose, pose2)); EXPECT(assert_equal(pose, pose2));
@ -110,7 +110,7 @@ TEST(Pose2, expmap3) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap0a) { TEST(Pose2, expmap0a) {
Pose2 expected(0.0101345, -0.0149092, 0.018); 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)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -118,7 +118,7 @@ TEST(Pose2, expmap0a) {
TEST(Pose2, expmap0b) { TEST(Pose2, expmap0b) {
// a quarter turn // a quarter turn
Pose2 expected(1.0, 1.0, M_PI/2); 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)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -126,7 +126,7 @@ TEST(Pose2, expmap0b) {
TEST(Pose2, expmap0c) { TEST(Pose2, expmap0c) {
// a half turn // a half turn
Pose2 expected(0.0, 2.0, M_PI); 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)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -134,7 +134,7 @@ TEST(Pose2, expmap0c) {
TEST(Pose2, expmap0d) { TEST(Pose2, expmap0d) {
// a full turn // a full turn
Pose2 expected(0, 0, 0); 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)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -143,7 +143,7 @@ TEST(Pose2, expmap0d) {
// test case for screw motion in the plane // test case for screw motion in the plane
namespace screw { namespace screw {
double w=0.3; 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); Rot2 expectedR = Rot2::fromAngle(w);
Point2 expectedT(-0.0446635, 0.29552); Point2 expectedT(-0.0446635, 0.29552);
Pose2 expected(expectedR, expectedT); Pose2 expected(expectedR, expectedT);
@ -161,7 +161,7 @@ TEST(Pose3, expmap_c)
TEST(Pose2, expmap_c_full) TEST(Pose2, expmap_c_full)
{ {
double w=0.3; 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); Rot2 expectedR = Rot2::fromAngle(w);
Point2 expectedT(-0.0446635, 0.29552); Point2 expectedT(-0.0446635, 0.29552);
Pose2 expected(expectedR, expectedT); Pose2 expected(expectedR, expectedT);
@ -175,9 +175,9 @@ TEST(Pose2, logmap) {
Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose0(M_PI/2.0, Point2(1, 2));
Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
#ifdef SLOW_BUT_CORRECT_EXPMAP #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 #else
Vector expected = Vector_(3, 0.01, -0.015, 0.018); Vector expected = (Vec(3) << 0.01, -0.015, 0.018);
#endif #endif
Vector actual = pose0.localCoordinates(pose); Vector actual = pose0.localCoordinates(pose);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -187,7 +187,7 @@ TEST(Pose2, logmap) {
TEST(Pose2, logmap_full) { TEST(Pose2, logmap_full) {
Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose0(M_PI/2.0, Point2(1, 2));
Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); 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<Pose2>(pose0, pose); Vector actual = logmap_default<Pose2>(pose0, pose);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -348,7 +348,7 @@ TEST(Pose2, inverse )
namespace { namespace {
/* ************************************************************************* */ /* ************************************************************************* */
Vector homogeneous(const Point2& p) { Vector homogeneous(const Point2& p) {
return Vector_(3, p.x(), p.y(), 1.0); return (Vec(3) << p.x(), p.y(), 1.0);
} }
/* ************************************************************************* */ /* ************************************************************************* */

20
gtsam/geometry/tests/testPose3.cpp
View File

@ -104,7 +104,7 @@ TEST( Pose3, expmap_a_full2)
TEST(Pose3, expmap_b) TEST(Pose3, expmap_b)
{ {
Pose3 p1(Rot3(), Point3(100, 0, 0)); 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)); Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0));
EXPECT(assert_equal(expected, p2,1e-2)); EXPECT(assert_equal(expected, p2,1e-2));
} }
@ -113,7 +113,7 @@ TEST(Pose3, expmap_b)
// test case for screw motion in the plane // test case for screw motion in the plane
namespace screw { namespace screw {
double a=0.3, c=cos(a), s=sin(a), w=0.3; 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); Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
Point3 expectedT(0.29552, 0.0446635, 1); Point3 expectedT(0.29552, 0.0446635, 1);
Pose3 expected(expectedR, expectedT); Pose3 expected(expectedR, expectedT);
@ -163,13 +163,13 @@ Pose3 Agrawal06iros(const Vector& xi) {
TEST(Pose3, expmaps_galore_full) TEST(Pose3, expmaps_galore_full)
{ {
Vector xi; Pose3 actual; 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); actual = Pose3::Expmap(xi);
EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6)); EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6));
EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
EXPECT(assert_equal(xi, Pose3::Logmap(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) { for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
Vector txi = xi*theta; Vector txi = xi*theta;
actual = Pose3::Expmap(txi); actual = Pose3::Expmap(txi);
@ -181,7 +181,7 @@ TEST(Pose3, expmaps_galore_full)
} }
// Works with large v as well, but expm needs 10 iterations! // 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); actual = Pose3::Expmap(xi);
EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5)); EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
@ -194,7 +194,7 @@ TEST(Pose3, Adjoint_compose_full)
// To debug derivatives of compose, assert that // To debug derivatives of compose, assert that
// T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
const Pose3& T1 = T; 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; Pose3 expected = T1 * Pose3::Expmap(x) * T2;
Vector y = T2.inverse().Adjoint(x); Vector y = T2.inverse().Adjoint(x);
Pose3 actual = T1 * T2 * Pose3::Expmap(y); Pose3 actual = T1 * T2 * Pose3::Expmap(y);
@ -510,7 +510,7 @@ TEST(Pose3, subgroups)
{ {
// Frank - Below only works for correct "Agrawal06iros style expmap // Frank - Below only works for correct "Agrawal06iros style expmap
// lines in canonical coordinates correspond to Abelian subgroups in SE(3) // 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)) // exp(-d)=inverse(exp(d))
EXPECT(assert_equal(Pose3::Expmap(-d),Pose3::Expmap(d).inverse())); 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) // 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) /// exp(xi) exp(y) = exp(xi + x)
/// Hence, y = log (exp(-xi)*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 testDerivExpmapInv(const LieVector& dxi) {
Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+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) { TEST( Pose3, adjointTranspose) {
Vector xi = Vector_(6, 0.01, 0.02, 0.03, 1.0, 2.0, 3.0); Vector xi = (Vec(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 v = (Vec(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0);
Vector expected = testDerivAdjointTranspose(xi, v); Vector expected = testDerivAdjointTranspose(xi, v);
Matrix actualH; Matrix actualH;

40
gtsam/geometry/tests/testRot3M.cpp
View File

@ -101,7 +101,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
TEST( Rot3, rodriguez) TEST( Rot3, rodriguez)
{ {
Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); 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); Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1)); CHECK(assert_equal(R2,R1));
} }
@ -109,7 +109,7 @@ TEST( Rot3, rodriguez)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, rodriguez2) 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; double angle = 3.14 / 4.0;
Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 actual = Rot3::rodriguez(axis, angle);
Rot3 expected(0.707388, 0, 0.706825, Rot3 expected(0.707388, 0, 0.706825,
@ -121,7 +121,7 @@ TEST( Rot3, rodriguez2)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, rodriguez3) 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 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w); Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1)); CHECK(assert_equal(R2,R1));
@ -130,7 +130,7 @@ TEST( Rot3, rodriguez3)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, rodriguez4) 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; double angle = M_PI/2.0;
Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 actual = Rot3::rodriguez(axis, angle);
double c=cos(angle),s=sin(angle); double c=cos(angle),s=sin(angle);
@ -156,7 +156,7 @@ TEST(Rot3, log)
Rot3 R; Rot3 R;
#define CHECK_OMEGA(X,Y,Z) \ #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); \ R = Rot3::rodriguez(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12));
@ -190,7 +190,7 @@ TEST(Rot3, log)
// Check 360 degree rotations // Check 360 degree rotations
#define CHECK_OMEGA_ZERO(X,Y,Z) \ #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); \ R = Rot3::rodriguez(w); \
EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); EXPECT(assert_equal(zero(3), Rot3::Logmap(R)));
@ -217,7 +217,7 @@ TEST(Rot3, manifold_caley)
CHECK(assert_equal(d12,-d21)); CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3) // 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)) // exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); 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) // 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)); CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3) // 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)) // exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); 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) // 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)); CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3) // 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)) // exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); 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) // 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? // 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 // 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); 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(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; Matrix actualK;
Vector actual; Vector actual;
boost::tie(actualK, actual) = RQ(R.matrix()); 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(I3,actualK));
CHECK(assert_equal(expected,actual,1e-6)); CHECK(assert_equal(expected,actual,1e-6));
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] // 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(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] // 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)(Vec(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.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
// Try ypr for pure yaw-pitch-roll matrices // 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)(Vec(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)(Vec(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.0,0.0,0.1),Rot3::roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix // 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); 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 ) { 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 theta = w.norm();
double theta2 = theta*theta; double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w); Rot3 actualR = Rot3::Expmap(w);
@ -526,7 +526,7 @@ TEST( Rot3, expmapStability ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, logmapStability ) { 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); Rot3 R = Rot3::Expmap(w);
// double tr = R.r1().x()+R.r2().y()+R.r3().z(); // double tr = R.r1().x()+R.r2().y()+R.r3().z();
// std::cout.precision(5000); // std::cout.precision(5000);

44
gtsam/geometry/tests/testRot3Q.cpp
View File

@ -88,7 +88,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
TEST( Rot3, rodriguez) TEST( Rot3, rodriguez)
{ {
Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); 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); Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1)); CHECK(assert_equal(R2,R1));
} }
@ -96,7 +96,7 @@ TEST( Rot3, rodriguez)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, rodriguez2) 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; double angle = 3.14 / 4.0;
Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 actual = Rot3::rodriguez(axis, angle);
Rot3 expected(0.707388, 0, 0.706825, Rot3 expected(0.707388, 0, 0.706825,
@ -108,7 +108,7 @@ TEST( Rot3, rodriguez2)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, rodriguez3) 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 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w); Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1)); CHECK(assert_equal(R2,R1));
@ -117,7 +117,7 @@ TEST( Rot3, rodriguez3)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, rodriguez4) 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; double angle = M_PI_2;
Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 actual = Rot3::rodriguez(axis, angle);
double c=cos(angle),s=sin(angle); double c=cos(angle),s=sin(angle);
@ -138,35 +138,35 @@ TEST( Rot3, expmap)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3, log) 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); Rot3 R1 = Rot3::rodriguez(w1);
CHECK(assert_equal(w1, Rot3::Logmap(R1))); 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); Rot3 R2 = Rot3::rodriguez(w2);
CHECK(assert_equal(w2, Rot3::Logmap(R2))); 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); Rot3 R3 = Rot3::rodriguez(w3);
CHECK(assert_equal(w3, Rot3::Logmap(R3))); 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); Rot3 R = Rot3::rodriguez(w);
CHECK(assert_equal(w, Rot3::Logmap(R))); 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); Rot3 R5 = Rot3::rodriguez(w5);
CHECK(assert_equal(w5, Rot3::Logmap(R5))); CHECK(assert_equal(w5, Rot3::Logmap(R5)));
Vector w6 = Vector_(3, boost::math::constants::pi<double>(), 0.0, 0.0); Vector w6 = (Vec(3) << boost::math::constants::pi<double>(), 0.0, 0.0);
Rot3 R6 = Rot3::rodriguez(w6); Rot3 R6 = Rot3::rodriguez(w6);
CHECK(assert_equal(w6, Rot3::Logmap(R6))); CHECK(assert_equal(w6, Rot3::Logmap(R6)));
Vector w7 = Vector_(3, 0.0, boost::math::constants::pi<double>(), 0.0); Vector w7 = (Vec(3) << 0.0, boost::math::constants::pi<double>(), 0.0);
Rot3 R7 = Rot3::rodriguez(w7); Rot3 R7 = Rot3::rodriguez(w7);
CHECK(assert_equal(w7, Rot3::Logmap(R7))); CHECK(assert_equal(w7, Rot3::Logmap(R7)));
Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi<double>()); Vector w8 = (Vec(3) << 0.0, 0.0, boost::math::constants::pi<double>());
Rot3 R8 = Rot3::rodriguez(w8); Rot3 R8 = Rot3::rodriguez(w8);
CHECK(assert_equal(w8, Rot3::Logmap(R8))); CHECK(assert_equal(w8, Rot3::Logmap(R8)));
} }
@ -190,7 +190,7 @@ TEST(Rot3, manifold)
CHECK(assert_equal(d12,-d21)); CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3) // 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)) // exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); 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) // 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; Matrix actualK;
Vector actual; Vector actual;
boost::tie(actualK, actual) = RQ(R.matrix()); 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(eye(3),actualK));
CHECK(assert_equal(expected,actual,1e-6)); CHECK(assert_equal(expected,actual,1e-6));
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] // 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(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] // 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((Vec(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.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
// Try ypr for pure yaw-pitch-roll matrices // 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((Vec(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((Vec(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.0,0.0,0.1),Rot3::roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix // 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); 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 ) { 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 theta = w.norm();
double theta2 = theta*theta; double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w); Rot3 actualR = Rot3::Expmap(w);
@ -425,7 +425,7 @@ TEST( Rot3, expmapStability ) {
// Does not work with Quaternions // Does not work with Quaternions
///* ************************************************************************* */ ///* ************************************************************************* */
//TEST( Rot3, logmapStability ) { //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); // Rot3 R = Rot3::Expmap(w);
//// double tr = R.r1().x()+R.r2().y()+R.r3().z(); //// double tr = R.r1().x()+R.r2().y()+R.r3().z();
//// std::cout.precision(5000); //// std::cout.precision(5000);

4
gtsam/geometry/tests/testStereoPoint2.cpp
View File

@ -44,8 +44,8 @@ TEST(StereoPoint2, Lie) {
EXPECT(assert_equal(StereoPoint2(3,3,3), p1.between(p2))); 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(StereoPoint2(5,7,9), p1.retract((Vec(3) << 4., 5., 6.))));
EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2))); EXPECT(assert_equal((Vec(3) << 3., 3., 3.), p1.localCoordinates(p2)));
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
gtsam/geometry/tests/timePose3.cpp
View File

@ -37,8 +37,8 @@ int main()
double norm=sqrt(1.0+16.0+4.0); double norm=sqrt(1.0+16.0+4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm; 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); Vector v = (Vec(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); 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; Matrix H1,H2;
TEST(retract, T.retract(v)) TEST(retract, T.retract(v))