Undid overzealous merge
Frank
parent
7cfeb442f3
commit
aa42501737
|
|
@ -21,9 +21,6 @@
|
|||
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/config.h> // GTSAM_USE_TBB
|
||||
|
||||
#include <boost/random/mersenne_twister.hpp>
|
||||
#include <gtsam/config.h> // for GTSAM_USE_TBB
|
||||
|
||||
#ifdef __clang__
|
||||
|
|
@ -45,14 +42,13 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
|
||||
Unit3 direction(point);
|
||||
if (H) {
|
||||
// 3*3 Derivative of representation with respect to point is 3*3:
|
||||
Matrix3 D_p_point;
|
||||
point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
|
||||
// Calculate the 2*3 Jacobian
|
||||
// 3*3 Derivative of representation with respect to point is 3*3:
|
||||
Matrix3 D_p_point;
|
||||
Point3 normalized = point.normalize(H ? &D_p_point : 0);
|
||||
Unit3 direction;
|
||||
direction.p_ = normalized.vector();
|
||||
if (H)
|
||||
*H << direction.basis().transpose() * D_p_point;
|
||||
}
|
||||
return direction;
|
||||
}
|
||||
|
||||
|
|
@ -90,19 +86,20 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
|
|||
|
||||
// Get the unit vector and derivative wrt this.
|
||||
// NOTE(hayk): We can't call point3(), because it would recursively call basis().
|
||||
Point3 n(p_);
|
||||
const Point3 n(p_);
|
||||
|
||||
// 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))
|
||||
double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
|
||||
if ((mx <= my) && (mx <= mz)) {
|
||||
axis = Point3(1.0, 0.0, 0.0);
|
||||
else if ((my <= mx) && (my <= mz))
|
||||
} else if ((my <= mx) && (my <= mz)) {
|
||||
axis = Point3(0.0, 1.0, 0.0);
|
||||
else if ((mz <= mx) && (mz <= my))
|
||||
} else if ((mz <= mx) && (mz <= my)) {
|
||||
axis = Point3(0.0, 0.0, 1.0);
|
||||
else
|
||||
} else {
|
||||
assert(false);
|
||||
}
|
||||
|
||||
// Choose the direction of the first basis vector b1 in the tangent plane by crossing n with
|
||||
// the chosen axis.
|
||||
|
|
@ -148,7 +145,7 @@ Point3 Unit3::point3(OptionalJacobian<3, 2> H) const {
|
|||
Vector3 Unit3::unitVector(OptionalJacobian<3, 2> H) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return (p_);
|
||||
return p_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -171,10 +168,10 @@ Matrix3 Unit3::skew() const {
|
|||
double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const {
|
||||
// Get the unit vectors of each, and the derivative.
|
||||
Matrix32 H_pn_p;
|
||||
const Point3& pn = point3(H_p ? &H_pn_p : 0);
|
||||
Point3 pn = point3(H_p ? &H_pn_p : 0);
|
||||
|
||||
Matrix32 H_qn_q;
|
||||
const Point3& qn = q.point3(H_q ? &H_qn_q : 0);
|
||||
Point3 qn = q.point3(H_q ? &H_qn_q : 0);
|
||||
|
||||
// Compute the dot product of the Point3s.
|
||||
Matrix13 H_dot_pn, H_dot_qn;
|
||||
|
|
@ -206,7 +203,7 @@ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
|
|||
Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const {
|
||||
// Get the point3 of this, and the derivative.
|
||||
Matrix32 H_qn_q;
|
||||
const Point3& qn = q.point3(H_q ? &H_qn_q : 0);
|
||||
Point3 qn = q.point3(H_q ? &H_qn_q : 0);
|
||||
|
||||
// 2D error here is projecting q into the tangent plane of this (p).
|
||||
Matrix62 H_B_p;
|
||||
|
|
|
|||
|
|
@ -20,12 +20,17 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
|
||||
#include <boost/random/mersenne_twister.hpp>
|
||||
#include <boost/optional.hpp>
|
||||
#include <boost/random/mersenne_twister.hpp>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
||||
#include <string>
|
||||
|
||||
#ifdef GTSAM_USE_TBB
|
||||
#include <tbb/mutex.h>
|
||||
|
|
@ -76,6 +81,12 @@ public:
|
|||
p_.normalize();
|
||||
}
|
||||
|
||||
/// Construct from 2D point in plane at focal length f
|
||||
/// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point
|
||||
explicit Unit3(const Point2& p, double f = 1.0) : p_(p.x(), p.y(), f) {
|
||||
p_.normalize();
|
||||
}
|
||||
|
||||
/// Named constructor from Point3 with optional Jacobian
|
||||
static Unit3 FromPoint3(const Point3& point, //
|
||||
OptionalJacobian<2, 3> H = boost::none);
|
||||
|
|
|
|||
|
|
@ -18,19 +18,20 @@
|
|||
* @brief Tests the Unit3 class
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/slam/expressions.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/random.hpp>
|
||||
|
|
@ -49,6 +50,7 @@ GTSAM_CONCEPT_MANIFOLD_INST(Unit3)
|
|||
Point3 point3_(const Unit3& p) {
|
||||
return p.point3();
|
||||
}
|
||||
|
||||
TEST(Unit3, point3) {
|
||||
vector<Point3> ps;
|
||||
ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0)
|
||||
|
|
@ -98,6 +100,7 @@ TEST(Unit3, unrotate) {
|
|||
Unit3 expected = Unit3(1, 1, 0);
|
||||
Unit3 actual = R.unrotate(p);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
|
||||
Matrix actualH, expectedH;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
{
|
||||
|
|
@ -237,19 +240,66 @@ TEST(Unit3, localCoordinates0) {
|
|||
EXPECT(assert_equal(zero(2), actual, 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, localCoordinates1) {
|
||||
Unit3 p, q(1, 6.12385e-21, 0);
|
||||
Vector actual = p.localCoordinates(q);
|
||||
CHECK(assert_equal(zero(2), actual, 1e-8));
|
||||
}
|
||||
TEST(Unit3, localCoordinates) {
|
||||
{
|
||||
Unit3 p, q;
|
||||
Vector2 expected = Vector2::Zero();
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(zero(2), actual, 1e-8));
|
||||
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p, q(1, 6.12385e-21, 0);
|
||||
Vector2 expected = Vector2::Zero();
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(zero(2), actual, 1e-8));
|
||||
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p, q(-1, 0, 0);
|
||||
Vector2 expected(M_PI, 0);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p, q(0, 1, 0);
|
||||
Vector2 expected(0,-M_PI_2);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p, q(0, -1, 0);
|
||||
Vector2 expected(0, M_PI_2);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p(0,1,0), q(0,-1,0);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(q, p.retract(actual), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p(0,0,1), q(0,0,-1);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(q, p.retract(actual), 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, localCoordinates2) {
|
||||
Unit3 p, q(-1, 0, 0);
|
||||
Vector expected = (Vector(2) << M_PI, 0).finished();
|
||||
Vector actual = p.localCoordinates(q);
|
||||
CHECK(assert_equal(expected, actual, 1e-8));
|
||||
double twist = 1e-4;
|
||||
{
|
||||
Unit3 p(0, 1, 0), q(0 - twist, -1 + twist, 0);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(actual(0) < 1e-2);
|
||||
EXPECT(actual(1) > M_PI - 1e-2)
|
||||
}
|
||||
{
|
||||
Unit3 p(0, 1, 0), q(0 + twist, -1 - twist, 0);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(actual(0) < 1e-2);
|
||||
EXPECT(actual(1) < -M_PI + 1e-2)
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
|
|
@ -296,98 +346,33 @@ TEST(Unit3, basis_derivatives) {
|
|||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, retract) {
|
||||
Unit3 p;
|
||||
Vector v(2);
|
||||
v << 0.5, 0;
|
||||
Unit3 expected(0.877583, 0, 0.479426);
|
||||
Unit3 actual = p.retract(v);
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
{
|
||||
Unit3 p;
|
||||
Vector2 v(0.5, 0);
|
||||
Unit3 expected(0.877583, 0, 0.479426);
|
||||
Unit3 actual = p.retract(v);
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p;
|
||||
Vector2 v(0, 0);
|
||||
Unit3 actual = p.retract(v);
|
||||
EXPECT(assert_equal(p, actual, 1e-6));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, retract_expmap) {
|
||||
Unit3 p;
|
||||
Vector v(2);
|
||||
v << (M_PI / 2.0), 0;
|
||||
Vector2 v((M_PI / 2.0), 0);
|
||||
Unit3 expected(Point3(0, 0, 1));
|
||||
Unit3 actual = p.retract(v);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
/// 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 Unit3's.
|
||||
// The equality x.localCoordinates(x.retract(v)) == v should hold.
|
||||
TEST(Unit3, localCoordinates_retract) {
|
||||
|
||||
size_t numIterations = 10000;
|
||||
Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit =
|
||||
Vector3(1.0, 1.0, 1.0);
|
||||
Vector minXiLimit = Vector2(-1.0, -1.0), maxXiLimit = Vector2(1.0, 1.0);
|
||||
for (size_t i = 0; i < numIterations; i++) {
|
||||
|
||||
// Create the two Unit3s.
|
||||
// NOTE: You can not create two totally random Unit3's because you cannot always compute
|
||||
// between two any Unit3's. (For instance, they might be at the different sides of the circle).
|
||||
Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
Vector v12 = randomVector(minXiLimit, maxXiLimit);
|
||||
Unit3 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));
|
||||
Unit3 actual_s2 = s1.retract(actual_v12);
|
||||
EXPECT(assert_equal(s2, actual_s2, 1e-3));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
// Let x and y be two Unit3's.
|
||||
// The equality x.localCoordinates(x.retract(v)) == v should hold.
|
||||
TEST(Unit3, localCoordinates_retract_expmap) {
|
||||
|
||||
size_t numIterations = 10000;
|
||||
Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit =
|
||||
Vector3(1.0, 1.0, 1.0);
|
||||
Vector minXiLimit = (Vector(2) << -M_PI, -M_PI).finished(), maxXiLimit = (Vector(2) << M_PI, M_PI).finished();
|
||||
for (size_t i = 0; i < numIterations; i++) {
|
||||
|
||||
// Create the two Unit3s.
|
||||
// Unlike the above case, we can use any two Unit3's.
|
||||
Unit3 s1(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;
|
||||
Unit3 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));
|
||||
Unit3 actual_s2 = s1.retract(actual_v12);
|
||||
EXPECT(assert_equal(s2, actual_s2, 1e-3));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, Random) {
|
||||
boost::mt19937 rng(42);
|
||||
|
|
@ -399,6 +384,26 @@ TEST(Unit3, Random) {
|
|||
EXPECT(assert_equal(expectedMean,actualMean,0.1));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
// New test that uses Unit3::Random
|
||||
TEST(Unit3, localCoordinates_retract) {
|
||||
boost::mt19937 rng(42);
|
||||
size_t numIterations = 10000;
|
||||
|
||||
for (size_t i = 0; i < numIterations; i++) {
|
||||
// Create two random Unit3s
|
||||
const Unit3 s1 = Unit3::Random(rng);
|
||||
const Unit3 s2 = Unit3::Random(rng);
|
||||
// Check that they are not at opposite ends of the sphere, which is ill defined
|
||||
if (s1.unitVector().dot(s2.unitVector())<-0.9) continue;
|
||||
|
||||
// Check if the local coordinates and retract return consistent results.
|
||||
Vector v12 = s1.localCoordinates(s2);
|
||||
Unit3 actual_s2 = s1.retract(v12);
|
||||
EXPECT(assert_equal(s2, actual_s2, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (Unit3, FromPoint3) {
|
||||
Matrix actualH;
|
||||
|
|
@ -450,6 +455,14 @@ TEST(Unit3, ErrorBetweenFactor) {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(actualH, Serialization) {
|
||||
Unit3 p(0, 1, 0);
|
||||
EXPECT(serializationTestHelpers::equalsObj(p));
|
||||
EXPECT(serializationTestHelpers::equalsXML(p));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(p));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
srand(time(NULL));
|
||||
|
|
|
|||
Loading…
Reference in New Issue