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

fix: correct some inappropriate floating point error, and its test

release/4.3a0
zhaoyang 2015-06-27 17:49:45 -04:00
parent 6895b75ecd
commit aa1e5962c9
2 changed files with 57 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/Unit3.cpp
View File

@ -148,12 +148,11 @@ Unit3 Unit3::retract(const Vector2& v) const {
// Compute the 3D xi_hat vector // Compute the 3D xi_hat vector
Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
double xi_hat_norm = xi_hat.norm(); double xi_hat_norm = xi_hat.norm();
// Avoid nan // Avoid nan
if (xi_hat_norm == 0.0) { if (xi_hat_norm < 1e-16) {
if (v.norm() == 0.0) if (v.norm() < 1e-16)
return Unit3(point3()); return Unit3(point3());
else else
return Unit3(-point3()); return Unit3(-point3());
@ -162,13 +161,12 @@ Unit3 Unit3::retract(const Vector2& v) const {
Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p
+ sin(xi_hat_norm) * (xi_hat / xi_hat_norm); + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
return Unit3(exp_p_xi_hat); return Unit3(exp_p_xi_hat);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector2 Unit3::localCoordinates(const Unit3& y) const { Vector2 Unit3::localCoordinates(const Unit3& y) const {
Vector3 p = p_.vector(), q = y.p_.vector(); Vector3 p = p_.vector(), q = y.unitVector();
double dot = p.dot(q); double dot = p.dot(q);
// Check for special cases // Check for special cases

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

@ -66,7 +66,7 @@ TEST(Unit3, rotate) {
Unit3 actual = R * p; Unit3 actual = R * p;
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-8));
Matrix actualH, expectedH; Matrix actualH, expectedH;
// Use numerical derivatives to calculate the expected Jacobian
{ {
expectedH = numericalDerivative21(rotate_, R, p); expectedH = numericalDerivative21(rotate_, R, p);
R.rotate(p, actualH, boost::none); R.rotate(p, actualH, boost::none);
@ -90,8 +90,8 @@ TEST(Unit3, unrotate) {
Unit3 expected = Unit3(1, 1, 0); Unit3 expected = Unit3(1, 1, 0);
Unit3 actual = R.unrotate(p); Unit3 actual = R.unrotate(p);
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-8));
Matrix actualH, expectedH; Matrix actualH, expectedH;
// Use numerical derivatives to calculate the expected Jacobian
{ {
expectedH = numericalDerivative21(unrotate_, R, p); expectedH = numericalDerivative21(unrotate_, R, p);
R.unrotate(p, actualH, boost::none); R.unrotate(p, actualH, boost::none);
@ -113,7 +113,6 @@ TEST(Unit3, error) {
EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
Matrix actual, expected; Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{ {
expected = numericalDerivative11<Vector2,Unit3>( expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), q); boost::bind(&Unit3::error, &p, _1, boost::none), q);
@ -153,31 +152,29 @@ TEST(Unit3, distance) {
} }
//******************************************************************************* //*******************************************************************************
TEST(Unit3, localCoordinates0) { TEST(Unit3, localCoordinates) {
{
Unit3 p; Unit3 p;
Vector actual = p.localCoordinates(p); Vector2 actual = p.localCoordinates(p);
EXPECT(assert_equal(zero(2), actual, 1e-8)); EXPECT(assert_equal(zero(2), actual, 1e-8));
} }
{
//*******************************************************************************
TEST(Unit3, localCoordinates1) {
Unit3 p, q(1, 6.12385e-21, 0); Unit3 p, q(1, 6.12385e-21, 0);
Vector actual = p.localCoordinates(q); Vector2 actual = p.localCoordinates(q);
CHECK(assert_equal(zero(2), actual, 1e-8)); CHECK(assert_equal(zero(2), actual, 1e-8));
} }
{
//*******************************************************************************
TEST(Unit3, localCoordinates2) {
Unit3 p, q(-1, 0, 0); Unit3 p, q(-1, 0, 0);
Vector expected = (Vector(2) << M_PI, 0).finished(); Vector2 expected(M_PI, 0);
Vector actual = p.localCoordinates(q); Vector2 actual = p.localCoordinates(q);
CHECK(assert_equal(expected, actual, 1e-8)); CHECK(assert_equal(expected, actual, 1e-8));
} }
}
//******************************************************************************* //*******************************************************************************
TEST(Unit3, basis) { TEST(Unit3, basis) {
Unit3 p; Unit3 p;
Matrix expected(3, 2); Matrix32 expected;
expected << 0, 0, 0, -1, 1, 0; expected << 0, 0, 0, -1, 1, 0;
Matrix actual = p.basis(); Matrix actual = p.basis();
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-8));
@ -185,20 +182,27 @@ TEST(Unit3, basis) {
//******************************************************************************* //*******************************************************************************
TEST(Unit3, retract) { TEST(Unit3, retract) {
{
Unit3 p; Unit3 p;
Vector v(2); Vector2 v(0.5, 0);
v << 0.5, 0;
Unit3 expected(0.877583, 0, 0.479426); Unit3 expected(0.877583, 0, 0.479426);
Unit3 actual = p.retract(v); Unit3 actual = p.retract(v);
EXPECT(assert_equal(expected, actual, 1e-6)); EXPECT(assert_equal(expected, actual, 1e-6));
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
} }
{
Unit3 p;
Vector2 v(0, 0);
Unit3 actual = p.retract(v);
EXPECT(assert_equal(p, actual, 1e-6));
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
}
}
//******************************************************************************* //*******************************************************************************
TEST(Unit3, retract_expmap) { TEST(Unit3, retract_expmap) {
Unit3 p; Unit3 p;
Vector v(2); Vector2 v((M_PI / 2.0), 0);
v << (M_PI / 2.0), 0;
Unit3 expected(Point3(0, 0, 1)); Unit3 expected(Point3(0, 0, 1));
Unit3 actual = p.retract(v); Unit3 actual = p.retract(v);
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-8));
@ -228,9 +232,11 @@ inline static Vector randomVector(const Vector& minLimits,
TEST(Unit3, localCoordinates_retract) { TEST(Unit3, localCoordinates_retract) {
size_t numIterations = 10000; size_t numIterations = 10000;
Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit = Vector3 minSphereLimit(-1.0, -1.0, -1.0);
Vector3(1.0, 1.0, 1.0); Vector3 maxSphereLimit(1.0, 1.0, 1.0);
Vector minXiLimit = Vector2(-1.0, -1.0), maxXiLimit = Vector2(1.0, 1.0); Vector2 minXiLimit(-1.0, -1.0);
Vector2 maxXiLimit(1.0, 1.0);
for (size_t i = 0; i < numIterations; i++) { for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first). // Sleep for the random number generator (TODO?: Better create all of them first).
@ -246,9 +252,9 @@ TEST(Unit3, localCoordinates_retract) {
// 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 = s1.localCoordinates(s2); Vector actual_v12 = s1.localCoordinates(s2);
EXPECT(assert_equal(v12, actual_v12, 1e-3)); EXPECT(assert_equal(v12, actual_v12, 1e-6));
Unit3 actual_s2 = s1.retract(actual_v12); Unit3 actual_s2 = s1.retract(actual_v12);
EXPECT(assert_equal(s2, actual_s2, 1e-3)); EXPECT(assert_equal(s2, actual_s2, 1e-6));
} }
} }
@ -258,30 +264,26 @@ TEST(Unit3, localCoordinates_retract) {
TEST(Unit3, localCoordinates_retract_expmap) { TEST(Unit3, localCoordinates_retract_expmap) {
size_t numIterations = 10000; size_t numIterations = 10000;
Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit = Vector3 minSphereLimit = Vector3(-1.0, -1.0, -1.0);
Vector3(1.0, 1.0, 1.0); Vector3 maxSphereLimit = Vector3(1.0, 1.0, 1.0);
Vector minXiLimit = (Vector(2) << -M_PI, -M_PI).finished(), maxXiLimit = (Vector(2) << M_PI, M_PI).finished(); Vector2 minXiLimit = Vector2(-M_PI, -M_PI);
Vector2 maxXiLimit = Vector2(M_PI, M_PI);
for (size_t i = 0; i < numIterations; i++) { for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first). // Sleep for the random number generator (TODO?: Better create all of them first).
// boost::this_thread::sleep(boost::posix_time::milliseconds(0)); // boost::this_thread::sleep(boost::posix_time::milliseconds(0));
// Create the two Unit3s.
// Unlike the above case, we can use any two Unit3's.
Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); // Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
Vector v12 = randomVector(minXiLimit, maxXiLimit); Vector v = randomVector(minXiLimit, maxXiLimit);
// Magnitude of the rotation can be at most pi // Magnitude of the rotation can be at most pi
if (v12.norm() > M_PI) if (v.norm() > M_PI)
v12 = v12 / M_PI; v = v / M_PI;
Unit3 s2 = s1.retract(v12); Unit3 s2 = s1.retract(v);
// Check if the local coordinates and retract return the same results. EXPECT(assert_equal(v, s1.localCoordinates(s1.retract(v)), 1e-6));
Vector actual_v12 = s1.localCoordinates(s2); EXPECT(assert_equal(s2, s1.retract(s1.localCoordinates(s2)), 1e-6));
EXPECT(assert_equal(v12, actual_v12, 1e-3));
Unit3 actual_s2 = s1.retract(actual_v12);
EXPECT(assert_equal(s2, actual_s2, 1e-3));
} }
} }