int main()
{
int n = 100000; long timeLog, timeLog2; double seconds;
// create a random direction:
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_(3,x,y,z);
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v);
TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001))
TEST("Rodriguez formula given canonical coordinates", Rot3::rodriguez(v))
TEST("Expmap", R*Rot3::Expmap(v))
TEST("Retract", R.retract(v))
TEST("Logmap", Rot3::Logmap(R.between(R2)))
TEST("localCoordinates", R.localCoordinates(R2))
TEST("Slow rotation matrix",Rot3::Rz(z)*Rot3::Ry(y)*Rot3::Rx(x))
TEST("Fast Rotation matrix", Rot3::RzRyRx(x,y,z))
return 0;
}
//******************************************************************************
// charts
TEST(Manifold, DefaultChart) {
//DefaultChart<Point2> chart1;
EXPECT(traits<Point2>::Local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0));
EXPECT(traits<Point2>::Retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0));
Vector v2(2);
v2 << 1, 0;
//DefaultChart<Vector2> chart2;
EXPECT(assert_equal(v2, traits<Vector2>::Local(Vector2(0, 0), Vector2(1, 0))));
EXPECT(traits<Vector2>::Retract(Vector2(0, 0), v2) == Vector2(1, 0));
{
typedef Matrix2 ManifoldPoint;
ManifoldPoint m;
//DefaultChart<ManifoldPoint> chart;
m << 1, 3,
2, 4;
// m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)!
EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
EXPECT(traits<ManifoldPoint>::Retract(m, Vector4(1, 2, 3, 4)) == 2 * m);
}
{
typedef Eigen::Matrix<double, 1, 2> ManifoldPoint;
ManifoldPoint m;
//DefaultChart<ManifoldPoint> chart;
m << 1, 2;
EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
EXPECT(traits<ManifoldPoint>::Retract(m, Vector2(1, 2)) == 2 * m);
}
{
typedef Eigen::Matrix<double, 1, 1> ManifoldPoint;
ManifoldPoint m;
//DefaultChart<ManifoldPoint> chart;
m << 1;
EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
EXPECT(traits<ManifoldPoint>::Retract(m, ManifoldPoint::Ones()) == 2 * m);
}
//DefaultChart<double> chart3;
Vector v1(1);
v1 << 1;
EXPECT(assert_equal(v1, traits<double>::Local(0, 1)));
EXPECT_DOUBLES_EQUAL(traits<double>::Retract(0, v1), 1, 1e-9);
// Dynamic does not work yet !
Vector z = zero(2), v(2);
v << 1, 0;
//DefaultChart<Vector> chart4;
// EXPECT(assert_equal(traits<Vector>::Local(z, v), v));
// EXPECT(assert_equal(traits<Vector>::Retract(z, v), v));
Vector v3(3);
v3 << 1, 1, 1;
Rot3 I = Rot3::identity();
Rot3 R = I.retract(v3);
//DefaultChart<Rot3> chart5;
EXPECT(assert_equal(v3, traits<Rot3>::Local(I, R)));
EXPECT(assert_equal(traits<Rot3>::Retract(I, v3), R));
// Check zero vector
//DefaultChart<Rot3> chart6;
EXPECT(assert_equal(zero(3), traits<Rot3>::Local(R, R)));
}
