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

change: correct the naming, and inappropriate tests

release/4.3a0
zhaoyang 2015-06-24 22:15:25 -04:00
parent b7c1097f58
commit cd0fe5d98c
1 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -30,7 +30,7 @@ GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
//******************************************************************************* //*******************************************************************************
TEST (OrientedPlane3, get) { TEST (OrientedPlane3, getMethods) {
Vector4 c; Vector4 c;
c << -1, 0, 0, 5; c << -1, 0, 0, 5;
OrientedPlane3 plane1(c); OrientedPlane3 plane1(c);
@ -38,11 +38,13 @@ TEST (OrientedPlane3, get) {
Vector4 coefficient1 = plane1.planeCoefficients(); Vector4 coefficient1 = plane1.planeCoefficients();
double distance1 = plane1.distance(); double distance1 = plane1.distance();
EXPECT(assert_equal(coefficient1, c, 1e-8)); EXPECT(assert_equal(coefficient1, c, 1e-8));
EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane1.normal().unitVector()));
EXPECT_DOUBLES_EQUAL(distance1, 5, 1e-8); EXPECT_DOUBLES_EQUAL(distance1, 5, 1e-8);
Vector4 coefficient2 = plane2.planeCoefficients(); Vector4 coefficient2 = plane2.planeCoefficients();
double distance2 = plane2.distance(); double distance2 = plane2.distance();
EXPECT(assert_equal(coefficient2, c, 1e-8)); EXPECT(assert_equal(coefficient2, c, 1e-8));
EXPECT_DOUBLES_EQUAL(distance2, 5, 1e-8); EXPECT_DOUBLES_EQUAL(distance2, 5, 1e-8);
EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane2.normal().unitVector()));
} }
//******************************************************************************* //*******************************************************************************
@ -98,11 +100,11 @@ inline static Vector randomVector(const Vector& minLimits,
TEST(OrientedPlane3, localCoordinates_retract) { TEST(OrientedPlane3, localCoordinates_retract) {
size_t numIterations = 10000; size_t numIterations = 10000;
gtsam::Vector minPlaneLimit(4), maxPlaneLimit(4); Vector4 minPlaneLimit, maxPlaneLimit;
minPlaneLimit << -1.0, -1.0, -1.0, 0.01; minPlaneLimit << -1.0, -1.0, -1.0, 0.01;
maxPlaneLimit << 1.0, 1.0, 1.0, 10.0; maxPlaneLimit << 1.0, 1.0, 1.0, 10.0;
Vector minXiLimit(3), maxXiLimit(3); Vector3 minXiLimit, maxXiLimit;
minXiLimit << -M_PI, -M_PI, -10.0; minXiLimit << -M_PI, -M_PI, -10.0;
maxXiLimit << M_PI, M_PI, 10.0; maxXiLimit << M_PI, M_PI, 10.0;
for (size_t i = 0; i < numIterations; i++) { for (size_t i = 0; i < numIterations; i++) {
@ -114,15 +116,15 @@ TEST(OrientedPlane3, localCoordinates_retract) {
Vector v12 = randomVector(minXiLimit, maxXiLimit); Vector v12 = randomVector(minXiLimit, maxXiLimit);
// Magnitude of the rotation can be at most pi // Magnitude of the rotation can be at most pi
if (v12.segment<3>(0).norm() > M_PI) if (v12.head<3>().norm() > M_PI)
v12.segment<3>(0) = v12.segment<3>(0) / M_PI; v12.head<3>() = v12.head<3>() / M_PI;
OrientedPlane3 p2 = p1.retract(v12); OrientedPlane3 p2 = p1.retract(v12);
// Check if the local coordinates and retract return the same results. // Check if the local coordinates and retract return the same results.
Vector actual_v12 = p1.localCoordinates(p2); Vector actual_v12 = p1.localCoordinates(p2);
EXPECT(assert_equal(v12, actual_v12, 1e-3)); EXPECT(assert_equal(v12, actual_v12, 1e-6));
OrientedPlane3 actual_p2 = p1.retract(actual_v12); OrientedPlane3 actual_p2 = p1.retract(actual_v12);
EXPECT(assert_equal(p2, actual_p2, 1e-3)); EXPECT(assert_equal(p2, actual_p2, 1e-6));
} }
} }