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))); }