Removed RENORM version
dellaert
parent
1d5da1c35e
commit
2b323d5cb7
|
|
@ -29,8 +29,7 @@ using namespace std;
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Sphere2::FromPoint3(const Point3& point,
|
||||
boost::optional<Matrix&> H) {
|
||||
Sphere2 Sphere2::FromPoint3(const Point3& point, boost::optional<Matrix&> H) {
|
||||
Sphere2 direction(point);
|
||||
if (H) {
|
||||
// 3*3 Derivative of representation with respect to point is 3*3:
|
||||
|
|
@ -114,7 +113,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
|
||||
Sphere2 Sphere2::retract(const Vector& v) const {
|
||||
|
||||
// Get the vector form of the point and the basis matrix
|
||||
Vector p = Point3::Logmap(p_);
|
||||
|
|
@ -122,75 +121,42 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
|
|||
|
||||
// Compute the 3D xi_hat vector
|
||||
Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
|
||||
|
||||
if (mode == Sphere2::EXPMAP) {
|
||||
double xi_hat_norm = xi_hat.norm();
|
||||
|
||||
// Avoid nan
|
||||
if (xi_hat_norm == 0.0) {
|
||||
if (v.norm () == 0.0)
|
||||
return Sphere2 (point3 ());
|
||||
else
|
||||
return Sphere2 (-point3 ());
|
||||
}
|
||||
|
||||
Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
|
||||
return Sphere2(exp_p_xi_hat);
|
||||
} else if (mode == Sphere2::RENORM) {
|
||||
// Project onto the manifold, i.e. the closest point on the circle to the new location;
|
||||
// same as putting it onto the unit circle
|
||||
Vector newPoint = p + xi_hat;
|
||||
Vector projected = newPoint / newPoint.norm();
|
||||
double xi_hat_norm = xi_hat.norm();
|
||||
|
||||
return Sphere2(Point3::Expmap(projected));
|
||||
} else {
|
||||
assert (false);
|
||||
exit (1);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const {
|
||||
|
||||
if (mode == Sphere2::EXPMAP) {
|
||||
Matrix B = basis();
|
||||
|
||||
Vector p = Point3::Logmap(p_);
|
||||
Vector q = Point3::Logmap(y.p_);
|
||||
double theta = acos(p.transpose() * q);
|
||||
|
||||
// the below will be nan if theta == 0.0
|
||||
if (p == q)
|
||||
return (Vector (2) << 0, 0);
|
||||
else if (p == (Vector)-q)
|
||||
return (Vector (2) << M_PI, 0);
|
||||
|
||||
Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta));
|
||||
Vector result = B.transpose() * result_hat;
|
||||
|
||||
return result;
|
||||
} else if (mode == Sphere2::RENORM) {
|
||||
// Make sure that the angle different between x and y is less than 90. Otherwise,
|
||||
// we can project x + xi_hat from the tangent space at x to y.
|
||||
assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y.");
|
||||
|
||||
// Get the basis matrix
|
||||
Matrix B = basis();
|
||||
|
||||
// Create the vector forms of p and q (the Point3 of y).
|
||||
Vector p = Point3::Logmap(p_);
|
||||
Vector q = Point3::Logmap(y.p_);
|
||||
|
||||
// Compute the basis coefficients [v0,v1] = (B'q)/(p'q).
|
||||
double alpha = p.transpose() * q;
|
||||
assert(alpha != 0.0);
|
||||
Matrix coeffs = (B.transpose() * q) / alpha;
|
||||
Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0));
|
||||
return result;
|
||||
} else {
|
||||
assert (false);
|
||||
exit (1);
|
||||
// Avoid nan
|
||||
if (xi_hat_norm == 0.0) {
|
||||
if (v.norm() == 0.0)
|
||||
return Sphere2(point3());
|
||||
else
|
||||
return Sphere2(-point3());
|
||||
}
|
||||
|
||||
Vector exp_p_xi_hat = cos(xi_hat_norm) * p
|
||||
+ sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
|
||||
return Sphere2(exp_p_xi_hat);
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Sphere2::localCoordinates(const Sphere2& y) const {
|
||||
|
||||
Matrix B = basis();
|
||||
|
||||
Vector p = Point3::Logmap(p_);
|
||||
Vector q = Point3::Logmap(y.p_);
|
||||
double theta = acos(p.transpose() * q);
|
||||
|
||||
// the below will be nan if theta == 0.0
|
||||
if (p == q)
|
||||
return (Vector(2) << 0, 0);
|
||||
else if (p == (Vector) -q)
|
||||
return (Vector(2) << M_PI, 0);
|
||||
|
||||
Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta));
|
||||
Vector result = B.transpose() * result_hat;
|
||||
|
||||
return result;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -23,10 +23,6 @@
|
|||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
|
||||
#ifndef SPHERE2_DEFAULT_COORDINATES_MODE
|
||||
#define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::RENORM
|
||||
#endif
|
||||
|
||||
// (Cumbersome) forward declaration for random generator
|
||||
namespace boost {
|
||||
namespace random {
|
||||
|
|
@ -71,8 +67,8 @@ public:
|
|||
}
|
||||
|
||||
/// Named constructor from Point3 with optional Jacobian
|
||||
static Sphere2 FromPoint3(const Point3& point,
|
||||
boost::optional<Matrix&> H = boost::none);
|
||||
static Sphere2 FromPoint3(const Point3& point, boost::optional<Matrix&> H =
|
||||
boost::none);
|
||||
|
||||
/// Random direction, using boost::uniform_on_sphere
|
||||
static Sphere2 Random(boost::random::mt19937 & rng);
|
||||
|
|
@ -113,7 +109,7 @@ public:
|
|||
|
||||
/// Return scaled direction as Point3
|
||||
friend Point3 operator*(double s, const Sphere2& d) {
|
||||
return s*d.p_;
|
||||
return s * d.p_;
|
||||
}
|
||||
|
||||
/// Signed, vector-valued error between two directions
|
||||
|
|
@ -145,10 +141,10 @@ public:
|
|||
};
|
||||
|
||||
/// The retract function
|
||||
Sphere2 retract(const Vector& v, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
|
||||
Sphere2 retract(const Vector& v) const;
|
||||
|
||||
/// The local coordinates function
|
||||
Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
|
||||
Vector localCoordinates(const Sphere2& s) const;
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -80,14 +80,14 @@ TEST(Sphere2, rotate) {
|
|||
|
||||
//*******************************************************************************
|
||||
static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) {
|
||||
return R.unrotate (p);
|
||||
return R.unrotate(p);
|
||||
}
|
||||
|
||||
TEST(Sphere2, unrotate) {
|
||||
Rot3 R = Rot3::yaw(-M_PI/4.0);
|
||||
Rot3 R = Rot3::yaw(-M_PI / 4.0);
|
||||
Sphere2 p(1, 0, 0);
|
||||
Sphere2 expected = Sphere2(1, 1, 0);
|
||||
Sphere2 actual = R.unrotate (p);
|
||||
Sphere2 actual = R.unrotate(p);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
Matrix actualH, expectedH;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
|
|
@ -105,11 +105,11 @@ TEST(Sphere2, unrotate) {
|
|||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, error) {
|
||||
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
|
||||
r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
|
||||
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
|
||||
r = p.retract((Vector(2) << 0.8, 0));
|
||||
EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8));
|
||||
EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5));
|
||||
EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5));
|
||||
EXPECT(assert_equal((Vector(2) << 0.479426, 0), p.error(q), 1e-5));
|
||||
EXPECT(assert_equal((Vector(2) << 0.717356, 0), p.error(r), 1e-5));
|
||||
|
||||
Matrix actual, expected;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
|
|
@ -129,11 +129,11 @@ TEST(Sphere2, error) {
|
|||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, distance) {
|
||||
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
|
||||
r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
|
||||
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
|
||||
r = p.retract((Vector(2) << 0.8, 0));
|
||||
EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
|
||||
EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8);
|
||||
EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8);
|
||||
EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8);
|
||||
EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-8);
|
||||
|
||||
Matrix actual, expected;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
|
|
@ -173,21 +173,21 @@ TEST(Sphere2, retract) {
|
|||
Sphere2 p;
|
||||
Vector v(2);
|
||||
v << 0.5, 0;
|
||||
Sphere2 expected(Point3(1, 0, 0.5));
|
||||
Sphere2 actual = p.retract(v, Sphere2::RENORM);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8));
|
||||
Sphere2 expected(0.877583, 0, 0.479426);
|
||||
Sphere2 actual = p.retract(v);
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Sphere2, retract_expmap) {
|
||||
Sphere2 p;
|
||||
Vector v(2);
|
||||
v << (M_PI/2.0), 0;
|
||||
v << (M_PI / 2.0), 0;
|
||||
Sphere2 expected(Point3(0, 0, 1));
|
||||
Sphere2 actual = p.retract(v, Sphere2::EXPMAP);
|
||||
Sphere2 actual = p.retract(v);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
|
|
@ -241,11 +241,12 @@ TEST(Sphere2, localCoordinates_retract) {
|
|||
// Let x and y be two Sphere2's.
|
||||
// The equality x.localCoordinates(x.retract(v)) == v should hold.
|
||||
TEST(Sphere2, localCoordinates_retract_expmap) {
|
||||
|
||||
|
||||
size_t numIterations = 10000;
|
||||
Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
|
||||
Vector_(3, 1.0, 1.0, 1.0);
|
||||
Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, M_PI, M_PI);
|
||||
Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, 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).
|
||||
|
|
@ -256,9 +257,9 @@ TEST(Sphere2, localCoordinates_retract_expmap) {
|
|||
Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
Vector v12 = randomVector(minXiLimit, maxXiLimit);
|
||||
|
||||
|
||||
// Magnitude of the rotation can be at most pi
|
||||
if (v12.norm () > M_PI)
|
||||
if (v12.norm() > M_PI)
|
||||
v12 = v12 / M_PI;
|
||||
Sphere2 s2 = s1.retract(v12);
|
||||
|
||||
|
|
@ -321,7 +322,7 @@ TEST(Sphere2, Random) {
|
|||
Point3 expectedMean, actualMean;
|
||||
for (size_t i = 0; i < 100; i++)
|
||||
actualMean = actualMean + Sphere2::Random(rng).point3();
|
||||
actualMean = actualMean/100;
|
||||
actualMean = actualMean / 100;
|
||||
EXPECT(assert_equal(expectedMean,actualMean,0.1));
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue