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

Comments, unprintable characters

release/4.3a0
Frank Dellaert 2013-12-17 01:57:31 +00:00
parent 8256a6a5d2
commit 179f17eb1e
2 changed files with 7 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/Sphere2.cpp
View File

@ -64,20 +64,17 @@ void Sphere2::print(const std::string& s) const {
/* ************************************************************************* */ /* ************************************************************************* */
Sphere2 Sphere2::retract(const Vector& v) const { Sphere2 Sphere2::retract(const Vector& v) const {
// If we had a 3D point, we could just add and normalize, as in Absil
// Point3 newPoint = p_ + z;
// 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_);
Vector axis; Vector axis;
Matrix B = getBasis(&axis); Matrix B = getBasis(&axis);
// Compute the 3D ξ^ 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);
Vector newPoint = p + xi_hat; Vector newPoint = p + xi_hat;
// Project onto the manifold, i.e. the closest point on the circle to the new location; same as // Project onto the manifold, i.e. the closest point on the circle to the new location;
// putting it onto the unit circle // same as putting it onto the unit circle
Vector projected = newPoint / newPoint.norm(); Vector projected = newPoint / newPoint.norm();
return Sphere2(Point3::Expmap(projected)); return Sphere2(Point3::Expmap(projected));
@ -87,9 +84,9 @@ Sphere2 Sphere2::retract(const Vector& v) const {
Vector Sphere2::localCoordinates(const Sphere2& y) const { Vector Sphere2::localCoordinates(const Sphere2& y) const {
// Make sure that the angle different between x and y is less than 90. Otherwise, // Make sure that the angle different between x and y is less than 90. Otherwise,
// we can project x + ξ^ from the tangent space at x to y. // we can project x + xi_hat from the tangent space at x to y.
double cosAngle = y.p_.dot(p_); double cosAngle = y.p_.dot(p_);
assert(cosAngle > 0.0 && "Can not retract from x to y in the first place."); assert(cosAngle > 0.0 && "Can not retract from x to y.");
// Get the basis matrix // Get the basis matrix
Matrix B = getBasis(); Matrix B = getBasis();
@ -98,7 +95,7 @@ Vector Sphere2::localCoordinates(const Sphere2& y) const {
Vector p = Point3::Logmap(p_); Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_); Vector q = Point3::Logmap(y.p_);
// Compute the basis coefficients [ξ1,ξ2] = (B'q)/(p'q). // Compute the basis coefficients [v0,v1] = (B'q)/(p'q).
double alpha = p.transpose() * q; double alpha = p.transpose() * q;
assert(alpha != 0.0); assert(alpha != 0.0);
Matrix coeffs = (B.transpose() * q) / alpha; Matrix coeffs = (B.transpose() * q) / alpha;

3
gtsam/geometry/Sphere2.h
View File

@ -22,8 +22,7 @@
namespace gtsam { namespace gtsam {
/// Represents a 3D point on a unit sphere. The Sphere2 with the 3D ξ^ variable and two /// Represents a 3D point on a unit sphere.
/// coefficients ξ_1 and ξ_2 that scale the 3D basis vectors of the tangent space.
class Sphere2 { class Sphere2 {
private: private: