diff --git a/.cproject b/.cproject index 4056d4b86..dcbcc50fa 100644 --- a/.cproject +++ b/.cproject @@ -904,6 +904,14 @@ true true + + make + -j5 + testEssentialMatrix.run + true + true + true + make -j2 @@ -1540,6 +1548,14 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -1892,6 +1908,14 @@ true true + + make + -j5 + testSphere2.run + true + true + true + make -j2 @@ -2132,6 +2156,14 @@ true true + + make + -j5 + testSampler.run + true + true + true + make -j2 diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp new file mode 100644 index 000000000..35bd71342 --- /dev/null +++ b/gtsam/geometry/Sphere2.cpp @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file Sphere2.h + * @date Feb 02, 2011 + * @author Can Erdogan + * @brief Develop a Sphere2 class - basically a point on a unit sphere + */ + +#include +#include + +using namespace std; + +namespace gtsam { + +Sphere2::~Sphere2() { +} + +Sphere2 Sphere2::retract(const Vector& v) const { + + // Get the vector form of the point and the basis matrix + Vector p = Point3::Logmap(p_); + Vector axis; + Matrix B = getBasis(&axis); + + // Compute the 3D ξ^ 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(); + +#ifdef DEBUG_SPHERE2_RETRACT + cout << "retract output for Matlab visualization (copy/paste =/): \n"; + cout << "p = [" << p.transpose() << "];\n"; + cout << "b1 = [" << B.col(0).transpose() << "];\n"; + cout << "b2 = [" << B.col(1).transpose() << "];\n"; + cout << "axis = [" << axis.transpose() << "];\n"; + cout << "xi_hat = [" << xi_hat.transpose() << "];\n"; + cout << "newPoint = [" << newPoint.transpose() << "];\n"; + cout << "projected = [" << projected.transpose() << "];\n"; +#endif + + Sphere2 result(Point3::Expmap(projected)); + return result; +} + +Vector Sphere2::localCoordinates(const Sphere2& y) const { + + // 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. + double cosAngle = y.p_.dot(p_); + assert(cosAngle > 0.0 && "Can not retract from x to y in the first place."); + + // Get the basis matrix + Matrix B = getBasis(); + + // 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 [ξ1,ξ2] = (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; +} + +Matrix Sphere2::getBasis(Vector* axisOutput) const { + + // Get the axis of rotation with the minimum projected length of the point + Point3 axis; + double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z()); + if ((mx <= my) && (mx <= mz)) + axis = Point3(1.0, 0.0, 0.0); + else if ((my <= mx) && (my <= mz)) + axis = Point3(0.0, 1.0, 0.0); + else if ((mz <= mx) && (mz <= my)) + axis = Point3(0.0, 0.0, 1.0); + else + assert(false); + + // Create the two basis vectors + Point3 b1 = p_.cross(axis); + b1 = b1 / b1.norm(); + Point3 b2 = p_.cross(b1); + b2 = b2 / b2.norm(); + + // Create the basis matrix + Matrix basis = Matrix_(3, 2, b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z()); + + // Return the axis if requested + if (axisOutput != NULL) + *axisOutput = Point3::Logmap(axis); + return basis; +} + +} diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h new file mode 100644 index 000000000..45464e959 --- /dev/null +++ b/gtsam/geometry/Sphere2.h @@ -0,0 +1,90 @@ +/* ---------------------------------------------------------------------------- + + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file Sphere2.h + * @date Feb 02, 2011 + * @author Can Erdogan + * @brief Develop a Sphere2 class - basically a point on a unit sphere + */ + +#pragma once + +#include + +namespace gtsam { + +/// Represents a 3D point on a unit sphere. The Sphere2 with the 3D ξ^ variable and two +/// coefficients ξ_1 and ξ_2 that scale the 3D basis vectors of the tangent space. +struct Sphere2 { + + gtsam::Point3 p_; ///< The location of the point on the unit sphere + + /// The constructors + Sphere2() : + p_(gtsam::Point3(1.0, 0.0, 0.0)) { + } + + /// Copy constructor + Sphere2(const Sphere2& s) { + p_ = s.p_ / s.p_.norm(); + } + + /// Destructor + ~Sphere2(); + + /// Field constructor + Sphere2(const gtsam::Point3& p) { + p_ = p / p.norm(); + } + + /// @name Testable + /// @{ + + /// The print fuction + void print(const std::string& s = std::string()) const { + printf("%s(x, y, z): (%.3lf, %.3lf, %.3lf)\n", s.c_str(), p_.x(), p_.y(), + p_.z()); + } + + /// The equals function with tolerance + bool equals(const Sphere2& s, double tol = 1e-9) const { + return p_.equals(s.p_, tol); + } + /// @} + + /// @name Manifold + /// @{ + + /// Dimensionality of tangent space = 2 DOF + inline static size_t Dim() { + return 2; + } + + /// Dimensionality of tangent space = 2 DOF + inline size_t dim() const { + return 2; + } + + /// The retract function + Sphere2 retract(const gtsam::Vector& v) const; + + /// The local coordinates function + gtsam::Vector localCoordinates(const Sphere2& s) const; + + /// @} + + /// Returns the axis of rotations + gtsam::Matrix getBasis(gtsam::Vector* axisOutput = NULL) const; +}; + +} // namespace gtsam + diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp new file mode 100644 index 000000000..ef28048ca --- /dev/null +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -0,0 +1,81 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testSphere2.cpp + * @date Feb 03, 2012 + * @author Can Erdogan + * @brief Tests the Sphere2 class + */ + +#include +#include +#include + +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(Sphere2) +GTSAM_CONCEPT_MANIFOLD_INST(Sphere2) + +/// Returns a random vector +inline static Vector randomVector(const Vector& minLimits, + const Vector& maxLimits) { + + // Get the number of dimensions and create the return vector + size_t numDims = dim(minLimits); + Vector vector = zero(numDims); + + // Create the random vector + for (size_t i = 0; i < numDims; i++) { + double range = maxLimits(i) - minLimits(i); + vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); + } + return vector; +} + +/* ************************************************************************* */ +// Let x and y be two Sphere2's. +// The equality x.localCoordinates(x.retract(v)) == v should hold. +TEST(Sphere2, localCoordinates_retract) { + + 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, -1.0, -1.0), maxXiLimit = Vector_(2, 1.0, 1.0); + 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. + // NOTE: You can not create two totally random Sphere2's because you cannot always compute + // between two any Sphere2's. (For instance, they might be at the different sides of the circle). + Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); +// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); + Vector v12 = randomVector(minXiLimit, maxXiLimit); + 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)); + } +} + +/* ************************************************************************* */ +int main() { + srand(time(NULL)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */