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

Merged changes from develop

release/4.3a0
dellaert 2014-01-25 10:07:34 -05:00
parent 6be91a2df2
commit 0464b38ca0
9 changed files with 696 additions and 560 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/geometry/Rot3.cpp
View File

@ -81,6 +81,17 @@ Sphere2 Rot3::rotate(const Sphere2& p,
return q;
}
/* ************************************************************************* */
Sphere2 Rot3::unrotate(const Sphere2& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Sphere2 q = unrotate(p.point3(Hp));
if (Hp)
(*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
if (HR)
(*HR) = q.basis().transpose() * q.skew();
return q;
}
/* ************************************************************************* */
Sphere2 Rot3::operator*(const Sphere2& p) const {
return rotate(p);

4
gtsam/geometry/Rot3.h
View File

@ -331,6 +331,10 @@ namespace gtsam {
Sphere2 rotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const;
/// unrotate 3D direction from world frame to rotated coordinate frame
Sphere2 unrotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const;
/// rotate 3D direction from rotated coordinate frame to world frame
Sphere2 operator*(const Sphere2& p) const;

5
gtsam/geometry/Rot3Q.cpp
View File

@ -62,11 +62,6 @@ namespace gtsam {
/* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}
/* ************************************************************************* */
void Rot3::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s);
}
/* ************************************************************************* */
Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }

83
gtsam/geometry/Sphere2.cpp
View File

@ -14,6 +14,7 @@
* @date Feb 02, 2011
* @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @brief The Sphere2 class - basically a point on a unit sphere
*/
@ -113,7 +114,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
}
/* ************************************************************************* */
Sphere2 Sphere2::retract(const Vector& v) const {
Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
// Get the vector form of the point and the basis matrix
Vector p = Point3::Logmap(p_);
@ -121,35 +122,75 @@ Sphere2 Sphere2::retract(const Vector& v) const {
// Compute the 3D xi_hat vector
Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
Vector newPoint = p + xi_hat;
// 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 projected = newPoint / newPoint.norm();
if (mode == Sphere2::EXPMAP) {
double xi_hat_norm = xi_hat.norm();
return Sphere2(Point3::Expmap(projected));
// 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();
return Sphere2(Point3::Expmap(projected));
} else {
assert (false);
exit (1);
}
}
/* ************************************************************************* */
Vector Sphere2::localCoordinates(const Sphere2& y) const {
Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const {
// 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.");
if (mode == Sphere2::EXPMAP) {
Matrix B = basis();
// Get the basis matrix
Matrix B = basis();
Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_);
double theta = acos(p.transpose() * q);
// Create the vector forms of p and q (the Point3 of y).
Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_);
// 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);
// 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;
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);
}
}
/* ************************************************************************* */

21
gtsam/geometry/Sphere2.h
View File

@ -14,6 +14,7 @@
* @date Feb 02, 2011
* @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @brief Develop a Sphere2 class - basically a point on a unit sphere
*/
@ -22,6 +23,10 @@
#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 {
@ -106,6 +111,13 @@ public:
return p_;
}
/// Return unit-norm Vector
Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
if (H)
*H = basis();
return (p_.vector ());
}
/// Signed, vector-valued error between two directions
Vector error(const Sphere2& q,
boost::optional<Matrix&> H = boost::none) const;
@ -129,11 +141,16 @@ public:
return 2;
}
enum CoordinatesMode {
EXPMAP, ///< Use the exponential map to retract
RENORM ///< Retract with vector addtion and renormalize.
};
/// The retract function
Sphere2 retract(const Vector& v) const;
Sphere2 retract(const Vector& v, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
/// The local coordinates function
Vector localCoordinates(const Sphere2& s) const;
Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
/// @}
};

5
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -89,10 +89,7 @@ TEST (EssentialMatrix, rotate) {
// Derivatives
Matrix actH1, actH2;
try {
bodyE.rotate(cRb, actH1, actH2);
} catch (exception e) {
} // avoid exception
try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception
Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
expH2 = numericalDerivative22(rotate_, bodyE, cRb);
EXPECT(assert_equal(expH1, actH1, 1e-8));

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

@ -13,6 +13,8 @@
* @file testSphere2.cpp
* @date Feb 03, 2012
* @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @brief Tests the Sphere2 class
*/
@ -76,10 +78,35 @@ TEST(Sphere2, rotate) {
}
}
//*******************************************************************************
static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) {
return R.unrotate (p);
}
TEST(Sphere2, unrotate) {
Rot3 R = Rot3::yaw(-M_PI/4.0);
Sphere2 p(1, 0, 0);
Sphere2 expected = Sphere2(1, 1, 0);
Sphere2 actual = R.unrotate (p);
EXPECT(assert_equal(expected, actual, 1e-8));
Matrix actualH, expectedH;
// Use numerical derivatives to calculate the expected Jacobian
{
expectedH = numericalDerivative21(unrotate_, R, p);
R.unrotate(p, actualH, boost::none);
EXPECT(assert_equal(expectedH, actualH, 1e-9));
}
{
expectedH = numericalDerivative22(unrotate_, R, p);
R.unrotate(p, boost::none, actualH);
EXPECT(assert_equal(expectedH, actualH, 1e-9));
}
}
//*******************************************************************************
TEST(Sphere2, error) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
r = p.retract((Vector(2) << 0.8, 0));
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);
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));
@ -102,8 +129,8 @@ TEST(Sphere2, error) {
//*******************************************************************************
TEST(Sphere2, distance) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
r = p.retract((Vector(2) << 0.8, 0));
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);
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);
@ -147,9 +174,20 @@ TEST(Sphere2, retract) {
Vector v(2);
v << 0.5, 0;
Sphere2 expected(Point3(1, 0, 0.5));
Sphere2 actual = p.retract(v);
Sphere2 actual = p.retract(v, Sphere2::RENORM);
EXPECT(assert_equal(expected, actual, 1e-8));
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8));
}
//*******************************************************************************
TEST(Sphere2, retract_expmap) {
Sphere2 p;
Vector v(2);
v << (M_PI/2.0), 0;
Sphere2 expected(Point3(0, 0, 1));
Sphere2 actual = p.retract(v, Sphere2::EXPMAP);
EXPECT(assert_equal(expected, actual, 1e-8));
EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8));
}
//*******************************************************************************
@ -199,6 +237,39 @@ 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);
for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first).
sleep(0);
// Create the two Sphere2s.
// Unlike the above case, we can use any two sphers.
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)
v12 = v12 / M_PI;
Sphere2 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-3));
Sphere2 actual_s2 = s1.retract(actual_v12);
EXPECT(assert_equal(s2, actual_s2, 1e-3));
}
}
//*******************************************************************************
//TEST( Pose2, between )
//{
@ -245,13 +316,13 @@ TEST(Sphere2, Random) {
// Check that is deterministic given same random seed
Point3 expected(-0.667578, 0.671447, 0.321713);
Point3 actual = Sphere2::Random(rng).point3();
EXPECT(assert_equal(expected, actual, 1e-5));
EXPECT(assert_equal(expected,actual,1e-5));
// Check that means are all zero at least
Point3 expectedMean, actualMean;
for (size_t i = 0; i < 100; i++)
actualMean = actualMean + Sphere2::Random(rng).point3();
actualMean = actualMean / 100;
EXPECT(assert_equal(expectedMean, actualMean, 0.1));
actualMean = actualMean/100;
EXPECT(assert_equal(expectedMean,actualMean,0.1));
}
//*************************************************************************