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

Added test for Rot3 - all is good

release/4.3a0
dellaert 2014-10-21 23:07:38 +02:00
parent 1eb5e185e5
commit 06af482d61
2 changed files with 53 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -2553,6 +2553,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testManifold.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testManifold.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

72
tests/testManifold.cpp
View File

@ -69,29 +69,42 @@ TEST(Manifold, DefaultChart) {
EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0));
EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0));
Vector v2(2);
v2 << 1, 0;
DefaultChart<Vector2> chart2(Vector2(0, 0));
EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0));
EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0));
EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0))));
EXPECT(chart2.retract(v2)==Vector2(1,0));
DefaultChart<double> chart3(0);
Eigen::Matrix<double, 1, 1> v1;
Vector v1(1);
v1 << 1;
EXPECT(chart3.apply(1)==v1);
EXPECT(chart3.retract(v1)==1);
EXPECT(assert_equal(v1,chart3.apply(1)));
EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9);
// Dynamic does not work yet !
// Vector z = zero(2), v(2);
// v << 1, 0;
// DefaultChart<Vector> chart4(z);
// EXPECT(chart4.apply(v)==v);
// EXPECT(chart4.retract(v)==v);
Vector z = zero(2), v(2);
v << 1, 0;
DefaultChart<Vector> chart4(z);
EXPECT(assert_equal(chart4.apply(v),v));
EXPECT(assert_equal(chart4.retract(v),v));
Vector v3(3);
v3 << 1, 1, 1;
Rot3 I = Rot3::identity();
Rot3 R = I.retractCayley(v3);
DefaultChart<Rot3> chart5(I);
EXPECT(assert_equal(v3,chart5.apply(R)));
EXPECT(assert_equal(chart5.retract(v3),R));
// Check zero vector
DefaultChart<Rot3> chart6(R);
EXPECT(assert_equal(zero(3),chart6.apply(R)));
}
/* ************************************************************************* */
// zero
TEST(Manifold, _zero) {
EXPECT(assert_equal(Pose3(),traits::zero<Pose3>::value()));
Cal3Bundler cal(0,0,0);
Cal3Bundler cal(0, 0, 0);
EXPECT(assert_equal(cal,traits::zero<Cal3Bundler>::value()));
EXPECT(assert_equal(Camera(Pose3(),cal),traits::zero<Camera>::value()));
}
@ -104,39 +117,44 @@ TEST(Manifold, Canonical) {
EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0));
EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0));
Vector v2(2);
v2 << 1, 0;
Canonical<Vector2> chart2;
EXPECT(assert_equal((Vector)chart2.apply(Vector2(1,0)),Vector2(1,0)));
EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0));
EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0))));
EXPECT(chart2.retract(v2)==Vector2(1,0));
Canonical<double> chart3;
Eigen::Matrix<double, 1, 1> v1;
v1 << 1;
EXPECT(chart3.apply(1)==v1);
EXPECT(chart3.retract(v1)==1);
EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9);
Canonical<Point3> chart4;
Point3 point(1,2,3);
Vector3 v3(1,2,3);
EXPECT(assert_equal((Vector)chart4.apply(point),v3));
Point3 point(1, 2, 3);
Vector v3(3);
v3 << 1, 2, 3;
EXPECT(assert_equal(v3,chart4.apply(point)));
EXPECT(assert_equal(chart4.retract(v3),point));
Canonical<Pose3> chart5;
Pose3 pose(Rot3::identity(),point);
Vector6 v6; v6 << 0,0,0,1,2,3;
EXPECT(assert_equal((Vector)chart5.apply(pose),v6));
Pose3 pose(Rot3::identity(), point);
Vector v6(6);
v6 << 0, 0, 0, 1, 2, 3;
EXPECT(assert_equal(v6,chart5.apply(pose)));
EXPECT(assert_equal(chart5.retract(v6),pose));
Canonical<Camera> chart6;
Cal3Bundler cal0(0,0,0);
Camera camera(Pose3(),cal0);
Vector9 z9 = Vector9::Zero();
EXPECT(assert_equal((Vector)chart6.apply(camera),z9));
Cal3Bundler cal0(0, 0, 0);
Camera camera(Pose3(), cal0);
Vector z9 = Vector9::Zero();
EXPECT(assert_equal(z9,chart6.apply(camera)));
EXPECT(assert_equal(chart6.retract(z9),camera));
Cal3Bundler cal; // Note !! Cal3Bundler() != zero<Cal3Bundler>::value()
Camera camera2(pose,cal);
Vector9 v9; v9 << 0,0,0,1,2,3,1,0,0;
EXPECT(assert_equal((Vector)chart6.apply(camera2),v9));
Camera camera2(pose, cal);
Vector v9(9);
v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0;
EXPECT(assert_equal(v9,chart6.apply(camera2)));
EXPECT(assert_equal(chart6.retract(v9),camera2));
}