Revived Sphere2, the S^2 manifold that can be used for directions in 3D space
Frank Dellaert
parent
92f822af28
commit
7e8095c2ee
32
.cproject
32
.cproject
|
|
@ -904,6 +904,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testEssentialMatrix.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testEssentialMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1540,6 +1548,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testParticleFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1892,6 +1908,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSphere2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSphere2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -2132,6 +2156,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSampler.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSampler.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/geometry/Sphere2.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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 <gtsam/geometry/Point3.h>
|
||||
|
||||
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
|
||||
|
||||
|
|
@ -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 <gtsam/base/Testable.h>
|
||||
#include <gtsam/geometry/Sphere2.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue