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

Replaced flaky random tests with more compact one that excludes degeneracies explicitly

release/4.3a0
Frank Dellaert 2015-07-12 10:30:30 -07:00
parent 3245a2304e
commit 297f8fcc9b
1 changed files with 20 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
gtsam/geometry/tests/testUnit3.cpp
View File

@ -246,124 +246,6 @@ TEST(Unit3, retract_expmap) {
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
}
//*******************************************************************************
/// Returns a random vector
inline static Vector randomVector(const Vector& minLimits,
const Vector& maxLimits) {
// Get the number of dimensions and create the return vector
size_t numDims = dim(minLimits);
Vector vector = zero(numDims);
// Create the random vector
for (size_t i = 0; i < numDims; i++) {
double range = maxLimits(i) - minLimits(i);
vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i);
}
return vector;
}
//*******************************************************************************
// Let x and y be two Unit3's.
// The equality x.localCoordinates(x.retract(v)) == v should hold.
TEST(Unit3, localCoordinates_retract) {
size_t numIterations = 10000;
Vector3 minSphereLimit(-1.0, -1.0, -1.0);
Vector3 maxSphereLimit(1.0, 1.0, 1.0);
Vector2 minXiLimit(-1.0, -1.0);
Vector2 maxXiLimit(1.0, 1.0);
for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first).
// boost::this_thread::sleep(boost::posix_time::milliseconds(0));
// Create the two Unit3s.
// NOTE: You can not create two totally random Unit3's because you cannot always compute
// between two any Unit3's. (For instance, they might be at the different sides of the circle).
Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
Vector v12 = randomVector(minXiLimit, maxXiLimit);
Unit3 s2 = s1.retract(v12);
// Check if the local coordinates and retract return the same results.
Vector actual_v12 = s1.localCoordinates(s2);
EXPECT(assert_equal(v12, actual_v12, 1e-8));
Unit3 actual_s2 = s1.retract(actual_v12);
EXPECT(assert_equal(s2, actual_s2, 1e-8));
}
}
//*******************************************************************************
// Let x and y be two Unit3's.
// The equality x.localCoordinates(x.retract(v)) == v should hold.
TEST(Unit3, localCoordinates_retract_expmap) {
size_t numIterations = 10000;
Vector3 minSphereLimit = Vector3(-1.0, -1.0, -1.0);
Vector3 maxSphereLimit = Vector3(1.0, 1.0, 1.0);
Vector2 minXiLimit = Vector2(-M_PI, -M_PI);
Vector2 maxXiLimit = Vector2(M_PI, M_PI);
for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first).
// boost::this_thread::sleep(boost::posix_time::milliseconds(0));
Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
Vector v = randomVector(minXiLimit, maxXiLimit);
// Magnitude of the rotation can be at most pi
if (v.norm() > M_PI)
v = v / M_PI;
Unit3 s2 = s1.retract(v);
EXPECT(assert_equal(v, s1.localCoordinates(s1.retract(v)), 1e-6));
EXPECT(assert_equal(s2, s1.retract(s1.localCoordinates(s2)), 1e-6));
}
}
//*******************************************************************************
//TEST( Pose2, between )
//{
// // <
// //
// // ^
// //
// // *--0--*--*
// Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
// Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
//
// Matrix actualH1,actualH2;
// Pose2 expected(M_PI/2.0, Point2(2,2));
// Pose2 actual1 = gT1.between(gT2);
// Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
// EXPECT(assert_equal(expected,actual1));
// EXPECT(assert_equal(expected,actual2));
//
// Matrix expectedH1 = (Matrix(3,3) <<
// 0.0,-1.0,-2.0,
// 1.0, 0.0,-2.0,
// 0.0, 0.0,-1.0
// );
// Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
// EXPECT(assert_equal(expectedH1,actualH1));
// EXPECT(assert_equal(numericalH1,actualH1));
// // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
// EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
//
// Matrix expectedH2 = (Matrix(3,3) <<
// 1.0, 0.0, 0.0,
// 0.0, 1.0, 0.0,
// 0.0, 0.0, 1.0
// );
// Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
// EXPECT(assert_equal(expectedH2,actualH2));
// EXPECT(assert_equal(numericalH2,actualH2));
//
//}
//*******************************************************************************
TEST(Unit3, Random) {
boost::mt19937 rng(42);
@ -375,6 +257,26 @@ TEST(Unit3, Random) {
EXPECT(assert_equal(expectedMean,actualMean,0.1));
}
//*******************************************************************************
// New test that uses Unit3::Random
TEST(Unit3, localCoordinates_retract) {
boost::mt19937 rng(42);
size_t numIterations = 10000;
for (size_t i = 0; i < numIterations; i++) {
// Create two random Unit3s
const Unit3 s1 = Unit3::Random(rng);
const Unit3 s2 = Unit3::Random(rng);
// Check that they are not at opposite ends of the sphere, which is ill defined
if (s1.unitVector().dot(s2.unitVector())<-0.9) continue;
// Check if the local coordinates and retract return consistent results.
Vector v12 = s1.localCoordinates(s2);
Unit3 actual_s2 = s1.retract(v12);
EXPECT(assert_equal(s2, actual_s2, 1e-9));
}
}
//*************************************************************************
TEST (Unit3, FromPoint3) {
Matrix actualH;