Merge remote-tracking branch 'origin/fix/Unit3Serialization' into feature/sam_sfm_directories
Frank Dellaert
commit
aa600c4df5
|
|
@ -38,6 +38,7 @@
|
|||
|
||||
#include <boost/random/variate_generator.hpp>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -79,8 +80,7 @@ const Matrix32& Unit3::basis() const {
|
|||
#endif
|
||||
|
||||
// Return cached version if exists
|
||||
if (B_)
|
||||
return *B_;
|
||||
if (B_) return *B_;
|
||||
|
||||
// Get the axis of rotation with the minimum projected length of the point
|
||||
Vector3 axis;
|
||||
|
|
@ -136,37 +136,39 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Unit3 Unit3::retract(const Vector2& v) const {
|
||||
|
||||
// Compute the 3D xi_hat vector
|
||||
Vector3 xi_hat = basis() * v;
|
||||
double xi_hat_norm = xi_hat.norm();
|
||||
double theta = xi_hat.norm();
|
||||
|
||||
// When v is the so small and approximate as a direction
|
||||
if (xi_hat_norm < 1e-8) {
|
||||
return Unit3(cos(xi_hat_norm) * p_ + xi_hat);
|
||||
// Treat case of very small v differently
|
||||
if (theta < std::numeric_limits<double>::epsilon()) {
|
||||
return Unit3(cos(theta) * p_ + xi_hat);
|
||||
}
|
||||
|
||||
Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p_
|
||||
+ sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
|
||||
Vector3 exp_p_xi_hat =
|
||||
cos(theta) * p_ + xi_hat * (sin(theta) / theta);
|
||||
return Unit3(exp_p_xi_hat);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector2 Unit3::localCoordinates(const Unit3& y) const {
|
||||
|
||||
double dot = p_.dot(y.p_);
|
||||
|
||||
// Check for special cases
|
||||
if (std::abs(dot - 1.0) < 1e-16)
|
||||
return Vector2(0.0, 0.0);
|
||||
else if (std::abs(dot + 1.0) < 1e-16)
|
||||
return Vector2(M_PI, 0.0);
|
||||
else {
|
||||
Vector2 Unit3::localCoordinates(const Unit3& other) const {
|
||||
const double x = p_.dot(other.p_);
|
||||
// Crucial quantitity here is y = theta/sin(theta) with theta=acos(x)
|
||||
// Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2)
|
||||
// We treat the special caes 1 and -1 below
|
||||
const double x2 = x * x;
|
||||
const double z = 1 - x2;
|
||||
double y;
|
||||
if (z < std::numeric_limits<double>::epsilon()) {
|
||||
if (x > 0) // expand at x=1
|
||||
y = 1.0 - (x - 1.0) / 3.0;
|
||||
else // cop out
|
||||
return Vector2(M_PI, 0.0);
|
||||
} else {
|
||||
// no special case
|
||||
const double theta = acos(dot);
|
||||
Vector3 result_hat = (theta / sin(theta)) * (y.p_ - p_ * dot);
|
||||
return basis().transpose() * result_hat;
|
||||
y = acos(x) / sqrt(z);
|
||||
}
|
||||
return basis().transpose() * y * (other.p_ - x * p_);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -147,7 +147,7 @@ public:
|
|||
|
||||
enum CoordinatesMode {
|
||||
EXPMAP, ///< Use the exponential map to retract
|
||||
RENORM ///< Retract with vector addtion and renormalize.
|
||||
RENORM ///< Retract with vector addition and renormalize.
|
||||
};
|
||||
|
||||
/// The retract function
|
||||
|
|
@ -167,13 +167,6 @@ private:
|
|||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(p_);
|
||||
// homebrew serialize Eigen Matrix
|
||||
ar & boost::serialization::make_nvp("B11", (*B_)(0, 0));
|
||||
ar & boost::serialization::make_nvp("B12", (*B_)(0, 1));
|
||||
ar & boost::serialization::make_nvp("B21", (*B_)(1, 0));
|
||||
ar & boost::serialization::make_nvp("B22", (*B_)(1, 1));
|
||||
ar & boost::serialization::make_nvp("B31", (*B_)(2, 0));
|
||||
ar & boost::serialization::make_nvp("B32", (*B_)(2, 1));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -22,11 +22,13 @@
|
|||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/random.hpp>
|
||||
//#include <boost/thread.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
#include <cmath>
|
||||
|
||||
|
|
@ -156,20 +158,39 @@ TEST(Unit3, distance) {
|
|||
|
||||
TEST(Unit3, localCoordinates) {
|
||||
{
|
||||
Unit3 p;
|
||||
Vector2 actual = p.localCoordinates(p);
|
||||
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);
|
||||
CHECK(assert_equal(zero(2), actual, 1e-8));
|
||||
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);
|
||||
CHECK(assert_equal(expected, actual, 1e-8));
|
||||
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));
|
||||
}
|
||||
|
||||
double twist = 1e-4;
|
||||
|
|
@ -225,124 +246,6 @@ TEST(Unit3, retract_expmap) {
|
|||
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;
|
||||
Vector3 minSphereLimit(-1.0, -1.0, -1.0);
|
||||
Vector3 maxSphereLimit(1.0, 1.0, 1.0);
|
||||
Vector2 minXiLimit(-1.0, -1.0);
|
||||
Vector2 maxXiLimit(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).
|
||||
// boost::this_thread::sleep(boost::posix_time::milliseconds(0));
|
||||
|
||||
// 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)));
|
||||
// Unit3 s2 (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-8));
|
||||
Unit3 actual_s2 = s1.retract(actual_v12);
|
||||
EXPECT(assert_equal(s2, actual_s2, 1e-8));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
// 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;
|
||||
Vector3 minSphereLimit = Vector3(-1.0, -1.0, -1.0);
|
||||
Vector3 maxSphereLimit = Vector3(1.0, 1.0, 1.0);
|
||||
Vector2 minXiLimit = Vector2(-M_PI, -M_PI);
|
||||
Vector2 maxXiLimit = Vector2(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).
|
||||
// boost::this_thread::sleep(boost::posix_time::milliseconds(0));
|
||||
Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
Vector v = randomVector(minXiLimit, maxXiLimit);
|
||||
|
||||
// Magnitude of the rotation can be at most pi
|
||||
if (v.norm() > M_PI)
|
||||
v = v / M_PI;
|
||||
Unit3 s2 = s1.retract(v);
|
||||
|
||||
EXPECT(assert_equal(v, s1.localCoordinates(s1.retract(v)), 1e-6));
|
||||
EXPECT(assert_equal(s2, s1.retract(s1.localCoordinates(s2)), 1e-6));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
//TEST( Pose2, between )
|
||||
//{
|
||||
// // <
|
||||
// //
|
||||
// // ^
|
||||
// //
|
||||
// // *--0--*--*
|
||||
// Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
|
||||
// Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
|
||||
//
|
||||
// Matrix actualH1,actualH2;
|
||||
// Pose2 expected(M_PI/2.0, Point2(2,2));
|
||||
// Pose2 actual1 = gT1.between(gT2);
|
||||
// Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
|
||||
// EXPECT(assert_equal(expected,actual1));
|
||||
// EXPECT(assert_equal(expected,actual2));
|
||||
//
|
||||
// Matrix expectedH1 = (Matrix(3,3) <<
|
||||
// 0.0,-1.0,-2.0,
|
||||
// 1.0, 0.0,-2.0,
|
||||
// 0.0, 0.0,-1.0
|
||||
// );
|
||||
// Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
|
||||
// EXPECT(assert_equal(expectedH1,actualH1));
|
||||
// EXPECT(assert_equal(numericalH1,actualH1));
|
||||
// // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
|
||||
// EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
|
||||
//
|
||||
// Matrix expectedH2 = (Matrix(3,3) <<
|
||||
// 1.0, 0.0, 0.0,
|
||||
// 0.0, 1.0, 0.0,
|
||||
// 0.0, 0.0, 1.0
|
||||
// );
|
||||
// Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
|
||||
// EXPECT(assert_equal(expectedH2,actualH2));
|
||||
// EXPECT(assert_equal(numericalH2,actualH2));
|
||||
//
|
||||
//}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, Random) {
|
||||
boost::mt19937 rng(42);
|
||||
|
|
@ -354,6 +257,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;
|
||||
|
|
@ -365,6 +288,14 @@ TEST (Unit3, FromPoint3) {
|
|||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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