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

Removed RENORM version

release/4.3a0
dellaert 2014-02-22 14:30:40 -05:00
parent 1d5da1c35e
commit 2b323d5cb7
3 changed files with 64 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
gtsam/geometry/Sphere2.cpp
View File

@ -29,8 +29,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Sphere2 Sphere2::FromPoint3(const Point3& point, Sphere2 Sphere2::FromPoint3(const Point3& point, boost::optional<Matrix&> H) {
boost::optional<Matrix&> H) {
Sphere2 direction(point); Sphere2 direction(point);
if (H) { if (H) {
// 3*3 Derivative of representation with respect to point is 3*3: // 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 // Get the vector form of the point and the basis matrix
Vector p = Point3::Logmap(p_); Vector p = Point3::Logmap(p_);
@ -123,7 +122,6 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
// Compute the 3D xi_hat vector // Compute the 3D xi_hat vector
Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
if (mode == Sphere2::EXPMAP) {
double xi_hat_norm = xi_hat.norm(); double xi_hat_norm = xi_hat.norm();
// Avoid nan // Avoid nan
@ -134,25 +132,15 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
return Sphere2(-point3()); return Sphere2(-point3());
} }
Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); 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); 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();
return Sphere2(Point3::Expmap(projected));
} else {
assert (false);
exit (1);
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const { Vector Sphere2::localCoordinates(const Sphere2& y) const {
if (mode == Sphere2::EXPMAP) {
Matrix B = basis(); Matrix B = basis();
Vector p = Point3::Logmap(p_); Vector p = Point3::Logmap(p_);
@ -169,28 +157,6 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
Vector result = B.transpose() * result_hat; Vector result = B.transpose() * result_hat;
return result; 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);
}
} }
/* ************************************************************************* */ /* ************************************************************************* */

12
gtsam/geometry/Sphere2.h
View File

@ -23,10 +23,6 @@
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.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 // (Cumbersome) forward declaration for random generator
namespace boost { namespace boost {
namespace random { namespace random {
@ -71,8 +67,8 @@ public:
} }
/// Named constructor from Point3 with optional Jacobian /// Named constructor from Point3 with optional Jacobian
static Sphere2 FromPoint3(const Point3& point, static Sphere2 FromPoint3(const Point3& point, boost::optional<Matrix&> H =
boost::optional<Matrix&> H = boost::none); boost::none);
/// Random direction, using boost::uniform_on_sphere /// Random direction, using boost::uniform_on_sphere
static Sphere2 Random(boost::random::mt19937 & rng); static Sphere2 Random(boost::random::mt19937 & rng);
@ -145,10 +141,10 @@ public:
}; };
/// The retract function /// 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 /// The local coordinates function
Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const; Vector localCoordinates(const Sphere2& s) const;
/// @} /// @}
}; };

31
gtsam/geometry/tests/testSphere2.cpp
View File

@ -105,11 +105,11 @@ TEST(Sphere2, unrotate) {
//******************************************************************************* //*******************************************************************************
TEST(Sphere2, error) { TEST(Sphere2, error) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), // Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM); 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, 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.479426, 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.717356, 0), p.error(r), 1e-5));
Matrix actual, expected; Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
@ -129,11 +129,11 @@ TEST(Sphere2, error) {
//******************************************************************************* //*******************************************************************************
TEST(Sphere2, distance) { TEST(Sphere2, distance) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), // Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM); r = p.retract((Vector(2) << 0.8, 0));
EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8); EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8);
EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8); EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-8);
Matrix actual, expected; Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
@ -173,10 +173,10 @@ TEST(Sphere2, retract) {
Sphere2 p; Sphere2 p;
Vector v(2); Vector v(2);
v << 0.5, 0; v << 0.5, 0;
Sphere2 expected(Point3(1, 0, 0.5)); Sphere2 expected(0.877583, 0, 0.479426);
Sphere2 actual = p.retract(v, Sphere2::RENORM); Sphere2 actual = p.retract(v);
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-6));
EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8)); EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
} }
//******************************************************************************* //*******************************************************************************
@ -185,9 +185,9 @@ TEST(Sphere2, retract_expmap) {
Vector v(2); Vector v(2);
v << (M_PI / 2.0), 0; v << (M_PI / 2.0), 0;
Sphere2 expected(Point3(0, 0, 1)); 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(expected, actual, 1e-8));
EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8)); EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
} }
//******************************************************************************* //*******************************************************************************
@ -245,7 +245,8 @@ TEST(Sphere2, localCoordinates_retract_expmap) {
size_t numIterations = 10000; size_t numIterations = 10000;
Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit = Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
Vector_(3, 1.0, 1.0, 1.0); 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++) { 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).