Fix Vector_() to Vec() in gtsam/geometry
Jing Dong
parent
019cd62d3e
commit
615c223f81
|
|
@ -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");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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)));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<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));
|
||||
}
|
||||
|
||||
|
|
@ -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<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));
|
||||
}
|
||||
|
||||
|
|
@ -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<Pose2>(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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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<Pose3>(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<Pose3>(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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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<double>(), 0.0, 0.0);
|
||||
Vector w6 = (Vec(3) << boost::math::constants::pi<double>(), 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<double>(), 0.0);
|
||||
Vector w7 = (Vec(3) << 0.0, boost::math::constants::pi<double>(), 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<double>());
|
||||
Vector w8 = (Vec(3) << 0.0, 0.0, boost::math::constants::pi<double>());
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
Loading…
Reference in New Issue