Added SO(4) class
Frank Dellaert
Fan Jiang
parent
6e07636302
commit
cc9b4bb497
|
|
@ -0,0 +1,226 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010-2019, 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 SO4.cpp
|
||||||
|
* @brief 4*4 matrix representation of SO(4)
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @author Luca Carlone
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/base/concepts.h>
|
||||||
|
#include <gtsam/base/timing.h>
|
||||||
|
#include <gtsam/geometry/SO4.h>
|
||||||
|
#include <gtsam/geometry/Unit3.h>
|
||||||
|
|
||||||
|
#include <Eigen/Eigenvalues>
|
||||||
|
#include <boost/random.hpp>
|
||||||
|
|
||||||
|
#include <cmath>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
static Vector3 randomOmega(boost::mt19937 &rng) {
|
||||||
|
static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
|
||||||
|
return Unit3::Random(rng).unitVector() * randomAngle(rng);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
// Create random SO(4) element using direct product of lie algebras.
|
||||||
|
SO4 SO4::Random(boost::mt19937 &rng) {
|
||||||
|
Vector6 delta;
|
||||||
|
delta << randomOmega(rng), randomOmega(rng);
|
||||||
|
return SO4::Expmap(delta);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
void SO4::print(const string &s) const { cout << s << *this << endl; }
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
bool SO4::equals(const SO4 &R, double tol) const {
|
||||||
|
return equal_with_abs_tol(*this, R, tol);
|
||||||
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
Matrix4 SO4::Hat(const Vector6 &xi) {
|
||||||
|
// skew symmetric matrix X = xi^
|
||||||
|
// Unlike Luca, makes upper-left the SO(3) subgroup.
|
||||||
|
// See also
|
||||||
|
// http://scipp.ucsc.edu/~haber/archives/physics251_13/groups13_sol4.pdf
|
||||||
|
Matrix4 Y = Z_4x4;
|
||||||
|
Y(0, 1) = -xi(2);
|
||||||
|
Y(0, 2) = +xi(1);
|
||||||
|
Y(1, 2) = -xi(0);
|
||||||
|
Y(0, 3) = -xi(3);
|
||||||
|
Y(1, 3) = -xi(4);
|
||||||
|
Y(2, 3) = -xi(5);
|
||||||
|
return Y - Y.transpose();
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Vector6 SO4::Vee(const Matrix4 &X) {
|
||||||
|
Vector6 xi;
|
||||||
|
xi(2) = -X(0, 1);
|
||||||
|
xi(1) = X(0, 2);
|
||||||
|
xi(0) = -X(1, 2);
|
||||||
|
xi(3) = -X(0, 3);
|
||||||
|
xi(4) = -X(1, 3);
|
||||||
|
xi(5) = -X(2, 3);
|
||||||
|
return xi;
|
||||||
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
/* Exponential map, porting MATLAB implementation by Luca, which follows
|
||||||
|
* "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
|
||||||
|
* Ramona-Andreaa Rohan */
|
||||||
|
SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H) {
|
||||||
|
if (H) throw std::runtime_error("SO4::Expmap Jacobian");
|
||||||
|
gttic(SO4_Expmap);
|
||||||
|
|
||||||
|
// skew symmetric matrix X = xi^
|
||||||
|
const Matrix4 X = Hat(xi);
|
||||||
|
|
||||||
|
// do eigen-decomposition
|
||||||
|
auto eig = Eigen::EigenSolver<Matrix4>(X);
|
||||||
|
Eigen::Vector4cd e = eig.eigenvalues();
|
||||||
|
using std::abs;
|
||||||
|
sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
|
||||||
|
return abs(a.imag()) > abs(b.imag());
|
||||||
|
});
|
||||||
|
|
||||||
|
// Get a and b from eigenvalues +/i ai and +/- bi
|
||||||
|
double a = e[0].imag(), b = e[2].imag();
|
||||||
|
if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
|
||||||
|
throw runtime_error("SO4::Expmap: wrong eigenvalues.");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Build expX = exp(xi^)
|
||||||
|
Matrix4 expX;
|
||||||
|
using std::cos;
|
||||||
|
using std::sin;
|
||||||
|
const auto X2 = X * X;
|
||||||
|
const auto X3 = X2 * X;
|
||||||
|
double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
|
||||||
|
if (a != 0 && b == 0) {
|
||||||
|
double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
|
||||||
|
return I_4x4 + X + c2 * X2 + c3 * X3;
|
||||||
|
} else if (a == b && b != 0) {
|
||||||
|
double sin_a = sin(a), cos_a = cos(a);
|
||||||
|
double c0 = (a * sin_a + 2 * cos_a) / 2,
|
||||||
|
c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
|
||||||
|
c3 = (sin_a - a * cos_a) / (2 * a3);
|
||||||
|
return c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3;
|
||||||
|
} else if (a != b) {
|
||||||
|
double sin_a = sin(a), cos_a = cos(a);
|
||||||
|
double sin_b = sin(b), cos_b = cos(b);
|
||||||
|
double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
|
||||||
|
c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
|
||||||
|
c2 = (cos_a - cos_b) / (b2 - a2),
|
||||||
|
c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
|
||||||
|
return c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3;
|
||||||
|
} else {
|
||||||
|
return I_4x4;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
Vector6 SO4::Logmap(const SO4 &Q, ChartJacobian H) {
|
||||||
|
if (H) throw std::runtime_error("SO4::Logmap Jacobian");
|
||||||
|
throw std::runtime_error("SO4::Logmap");
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
SO4 SO4::ChartAtOrigin::Retract(const Vector6 &xi, ChartJacobian H) {
|
||||||
|
if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
|
||||||
|
gttic(SO4_Retract);
|
||||||
|
const Matrix4 X = Hat(xi / 2);
|
||||||
|
return (I_4x4 + X) * (I_4x4 - X).inverse();
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H) {
|
||||||
|
if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
|
||||||
|
const Matrix4 X = (I_4x4 - Q) * (I_4x4 + Q).inverse();
|
||||||
|
return -2 * Vee(X);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
static SO4::Vector16 vec(const SO4 &Q) {
|
||||||
|
return Eigen::Map<const SO4::Vector16>(Q.data());
|
||||||
|
}
|
||||||
|
|
||||||
|
static const std::vector<const Matrix4> G(
|
||||||
|
{SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
|
||||||
|
SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
|
||||||
|
SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});
|
||||||
|
|
||||||
|
static const Eigen::Matrix<double, 16, 6> P =
|
||||||
|
(Eigen::Matrix<double, 16, 6>() << vec(G[0]), vec(G[1]), vec(G[2]),
|
||||||
|
vec(G[3]), vec(G[4]), vec(G[5]))
|
||||||
|
.finished();
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix6 SO4::AdjointMap() const {
|
||||||
|
gttic(SO4_AdjointMap);
|
||||||
|
// Elaborate way of calculating the AdjointMap
|
||||||
|
// TODO(frank): find a closed form solution. In SO(3) is just R :-/
|
||||||
|
const SO4 &Q = *this;
|
||||||
|
const SO4 Qt = this->inverse();
|
||||||
|
Matrix6 A;
|
||||||
|
for (size_t i = 0; i < 6; i++) {
|
||||||
|
// Calculate column i of linear map for coeffcient of Gi
|
||||||
|
A.col(i) = SO4::Vee(Q * G[i] * Qt);
|
||||||
|
}
|
||||||
|
return A;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
SO4::Vector16 SO4::vec(OptionalJacobian<16, 6> H) const {
|
||||||
|
const SO4 &Q = *this;
|
||||||
|
if (H) {
|
||||||
|
// As Luca calculated, this is (I4 \oplus Q) * P
|
||||||
|
*H << Q * P.block<4, 6>(0, 0), Q * P.block<4, 6>(4, 0),
|
||||||
|
Q * P.block<4, 6>(8, 0), Q * P.block<4, 6>(12, 0);
|
||||||
|
}
|
||||||
|
return gtsam::vec(Q);
|
||||||
|
};
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix3 SO4::topLeft(OptionalJacobian<9, 6> H) const {
|
||||||
|
const Matrix3 M = this->topLeftCorner<3, 3>();
|
||||||
|
const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
|
||||||
|
q = this->topRightCorner<3, 1>();
|
||||||
|
if (H) {
|
||||||
|
*H << Z_3x1, -m3, m2, q, Z_3x1, Z_3x1, //
|
||||||
|
m3, Z_3x1, -m1, Z_3x1, q, Z_3x1, //
|
||||||
|
-m2, m1, Z_3x1, Z_3x1, Z_3x1, q;
|
||||||
|
}
|
||||||
|
return M;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix43 SO4::stiefel(OptionalJacobian<12, 6> H) const {
|
||||||
|
const Matrix43 M = this->leftCols<3>();
|
||||||
|
const auto &m1 = col(0), m2 = col(1), m3 = col(2), q = col(3);
|
||||||
|
if (H) {
|
||||||
|
*H << Z_4x1, -m3, m2, q, Z_4x1, Z_4x1, //
|
||||||
|
m3, Z_4x1, -m1, Z_4x1, q, Z_4x1, //
|
||||||
|
-m2, m1, Z_4x1, Z_4x1, Z_4x1, q;
|
||||||
|
}
|
||||||
|
return M;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
||||||
|
} // end namespace gtsam
|
||||||
|
|
@ -0,0 +1,146 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010-2019, 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 SO4.h
|
||||||
|
* @brief 4*4 matrix representation of SO(4)
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @author Luca Carlone
|
||||||
|
* @date March 2019
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/base/Group.h>
|
||||||
|
#include <gtsam/base/Lie.h>
|
||||||
|
#include <gtsam/base/Manifold.h>
|
||||||
|
#include <gtsam/base/Matrix.h>
|
||||||
|
|
||||||
|
#include <boost/random/mersenne_twister.hpp>
|
||||||
|
|
||||||
|
#include <iosfwd>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* True SO(4), i.e., 4*4 matrix subgroup
|
||||||
|
*/
|
||||||
|
class SO4 : public Matrix4, public LieGroup<SO4, 6> {
|
||||||
|
public:
|
||||||
|
enum { N = 4 };
|
||||||
|
enum { dimension = 6 };
|
||||||
|
|
||||||
|
typedef Eigen::Matrix<double, 16, 1> Vector16;
|
||||||
|
|
||||||
|
/// @name Constructors
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// Default constructor creates identity
|
||||||
|
SO4() : Matrix4(I_4x4) {}
|
||||||
|
|
||||||
|
/// Constructor from Eigen Matrix
|
||||||
|
template <typename Derived>
|
||||||
|
SO4(const MatrixBase<Derived> &R) : Matrix4(R.eval()) {}
|
||||||
|
|
||||||
|
/// Random SO(4) element (no big claims about uniformity)
|
||||||
|
static SO4 Random(boost::mt19937 &rng);
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Testable
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
void print(const std::string &s) const;
|
||||||
|
bool equals(const SO4 &R, double tol) const;
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Group
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// identity rotation for group operation
|
||||||
|
static SO4 identity() { return I_4x4; }
|
||||||
|
|
||||||
|
/// inverse of a rotation = transpose
|
||||||
|
SO4 inverse() const { return this->transpose(); }
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Lie Group
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
static Matrix4 Hat(const Vector6 &xi); ///< make skew symmetric matrix
|
||||||
|
static Vector6 Vee(const Matrix4 &X); ///< inverse of Hat
|
||||||
|
static SO4 Expmap(const Vector6 &xi,
|
||||||
|
ChartJacobian H = boost::none); ///< exponential map
|
||||||
|
static Vector6 Logmap(const SO4 &Q,
|
||||||
|
ChartJacobian H = boost::none); ///< and its inverse
|
||||||
|
Matrix6 AdjointMap() const;
|
||||||
|
|
||||||
|
// Chart at origin
|
||||||
|
struct ChartAtOrigin {
|
||||||
|
static SO4 Retract(const Vector6 &omega, ChartJacobian H = boost::none);
|
||||||
|
static Vector6 Local(const SO4 &R, ChartJacobian H = boost::none);
|
||||||
|
};
|
||||||
|
|
||||||
|
using LieGroup<SO4, 6>::inverse;
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Other methods
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// Vectorize
|
||||||
|
Vector16 vec(OptionalJacobian<16, 6> H = boost::none) const;
|
||||||
|
|
||||||
|
/// Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3).
|
||||||
|
Matrix3 topLeft(OptionalJacobian<9, 6> H = boost::none) const;
|
||||||
|
|
||||||
|
/// Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> S \in St(3,4).
|
||||||
|
Matrix43 stiefel(OptionalJacobian<12, 6> H = boost::none) const;
|
||||||
|
|
||||||
|
/// Return matrix (for wrapper)
|
||||||
|
const Matrix4 &matrix() const { return *this; }
|
||||||
|
|
||||||
|
/// @
|
||||||
|
|
||||||
|
private:
|
||||||
|
/** Serialization function */
|
||||||
|
friend class boost::serialization::access;
|
||||||
|
template <class ARCHIVE>
|
||||||
|
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
|
||||||
|
ar &boost::serialization::make_nvp("Q11", (*this)(0, 0));
|
||||||
|
ar &boost::serialization::make_nvp("Q12", (*this)(0, 1));
|
||||||
|
ar &boost::serialization::make_nvp("Q13", (*this)(0, 2));
|
||||||
|
ar &boost::serialization::make_nvp("Q14", (*this)(0, 3));
|
||||||
|
|
||||||
|
ar &boost::serialization::make_nvp("Q21", (*this)(1, 0));
|
||||||
|
ar &boost::serialization::make_nvp("Q22", (*this)(1, 1));
|
||||||
|
ar &boost::serialization::make_nvp("Q23", (*this)(1, 2));
|
||||||
|
ar &boost::serialization::make_nvp("Q24", (*this)(1, 3));
|
||||||
|
|
||||||
|
ar &boost::serialization::make_nvp("Q31", (*this)(2, 0));
|
||||||
|
ar &boost::serialization::make_nvp("Q32", (*this)(2, 1));
|
||||||
|
ar &boost::serialization::make_nvp("Q33", (*this)(2, 2));
|
||||||
|
ar &boost::serialization::make_nvp("Q34", (*this)(2, 3));
|
||||||
|
|
||||||
|
ar &boost::serialization::make_nvp("Q41", (*this)(3, 0));
|
||||||
|
ar &boost::serialization::make_nvp("Q42", (*this)(3, 1));
|
||||||
|
ar &boost::serialization::make_nvp("Q43", (*this)(3, 2));
|
||||||
|
ar &boost::serialization::make_nvp("Q44", (*this)(3, 3));
|
||||||
|
}
|
||||||
|
|
||||||
|
}; // SO4
|
||||||
|
|
||||||
|
template <>
|
||||||
|
struct traits<SO4> : Testable<SO4>, internal::LieGroupTraits<SO4> {};
|
||||||
|
|
||||||
|
template <>
|
||||||
|
struct traits<const SO4> : Testable<SO4>, internal::LieGroupTraits<SO4> {};
|
||||||
|
|
||||||
|
} // end namespace gtsam
|
||||||
|
|
@ -0,0 +1,172 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 testSO4.cpp
|
||||||
|
* @brief Unit tests for SO4, as a GTSAM-adapted Lie Group
|
||||||
|
* @author Frank Dellaert
|
||||||
|
**/
|
||||||
|
|
||||||
|
#include <gtsam/base/Manifold.h>
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
|
#include <gtsam/base/lieProxies.h>
|
||||||
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
|
#include <gtsam/geometry/SO3.h>
|
||||||
|
#include <gtsam/geometry/SO4.h>
|
||||||
|
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
TEST(SO4, Concept) {
|
||||||
|
BOOST_CONCEPT_ASSERT((IsGroup<SO4>));
|
||||||
|
BOOST_CONCEPT_ASSERT((IsManifold<SO4>));
|
||||||
|
BOOST_CONCEPT_ASSERT((IsLieGroup<SO4>));
|
||||||
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
SO4 id;
|
||||||
|
Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
|
||||||
|
SO4 Q1 = SO4::Expmap(v1);
|
||||||
|
Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.00, 0.00, 0.00).finished();
|
||||||
|
SO4 Q2 = SO4::Expmap(v2);
|
||||||
|
Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
|
||||||
|
SO4 Q3 = SO4::Expmap(v3);
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
TEST(SO4, Random) {
|
||||||
|
boost::mt19937 rng(42);
|
||||||
|
auto Q = SO4::Random(rng);
|
||||||
|
EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
|
||||||
|
}
|
||||||
|
//******************************************************************************
|
||||||
|
TEST(SO4, Expmap) {
|
||||||
|
// If we do exponential map in SO(3) subgroup, topleft should be equal to R1.
|
||||||
|
auto R1 = SO3::Expmap(v1.head<3>());
|
||||||
|
EXPECT((Q1.topLeft().isApprox(R1)));
|
||||||
|
|
||||||
|
// Same here
|
||||||
|
auto R2 = SO3::Expmap(v2.head<3>());
|
||||||
|
EXPECT((Q2.topLeft().isApprox(R2)));
|
||||||
|
|
||||||
|
// Check commutative subgroups
|
||||||
|
for (size_t i = 0; i < 6; i++) {
|
||||||
|
Vector6 xi = Vector6::Zero();
|
||||||
|
xi[i] = 2;
|
||||||
|
SO4 Q1 = SO4::Expmap(xi);
|
||||||
|
xi[i] = 3;
|
||||||
|
SO4 Q2 = SO4::Expmap(xi);
|
||||||
|
EXPECT(((Q1 * Q2).isApprox(Q2 * Q1)));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
TEST(SO4, Cayley) {
|
||||||
|
CHECK(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100)));
|
||||||
|
CHECK(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100)));
|
||||||
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
TEST(SO4, Retract) {
|
||||||
|
auto v = Vector6::Zero();
|
||||||
|
SO4 actual = traits<SO4>::Retract(id, v);
|
||||||
|
EXPECT(actual.isApprox(id));
|
||||||
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
TEST(SO4, Local) {
|
||||||
|
auto v0 = Vector6::Zero();
|
||||||
|
Vector6 actual = traits<SO4>::Local(id, id);
|
||||||
|
EXPECT(assert_equal((Vector)v0, actual));
|
||||||
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
TEST(SO4, Invariants) {
|
||||||
|
EXPECT(check_group_invariants(id, id));
|
||||||
|
EXPECT(check_group_invariants(id, Q1));
|
||||||
|
EXPECT(check_group_invariants(Q2, id));
|
||||||
|
EXPECT(check_group_invariants(Q2, Q1));
|
||||||
|
EXPECT(check_group_invariants(Q1, Q2));
|
||||||
|
|
||||||
|
EXPECT(check_manifold_invariants(id, id));
|
||||||
|
EXPECT(check_manifold_invariants(id, Q1));
|
||||||
|
EXPECT(check_manifold_invariants(Q2, id));
|
||||||
|
EXPECT(check_manifold_invariants(Q2, Q1));
|
||||||
|
EXPECT(check_manifold_invariants(Q1, Q2));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(SO4, compose) {
|
||||||
|
SO4 expected = Q1 * Q2;
|
||||||
|
Matrix actualH1, actualH2;
|
||||||
|
SO4 actual = Q1.compose(Q2, actualH1, actualH2);
|
||||||
|
CHECK(assert_equal(expected, actual));
|
||||||
|
|
||||||
|
Matrix numericalH1 =
|
||||||
|
numericalDerivative21(testing::compose<SO4>, Q1, Q2, 1e-2);
|
||||||
|
CHECK(assert_equal(numericalH1, actualH1));
|
||||||
|
|
||||||
|
Matrix numericalH2 =
|
||||||
|
numericalDerivative22(testing::compose<SO4>, Q1, Q2, 1e-2);
|
||||||
|
CHECK(assert_equal(numericalH2, actualH2));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(SO4, vec) {
|
||||||
|
using Vector16 = SO4::Vector16;
|
||||||
|
const Vector16 expected = Eigen::Map<Vector16>(Q2.data());
|
||||||
|
Matrix actualH;
|
||||||
|
const Vector16 actual = Q2.vec(actualH);
|
||||||
|
CHECK(assert_equal(expected, actual));
|
||||||
|
boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
|
||||||
|
return Q.vec();
|
||||||
|
};
|
||||||
|
const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
|
||||||
|
CHECK(assert_equal(numericalH, actualH));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(SO4, topLeft) {
|
||||||
|
const Matrix3 expected = Q3.topLeftCorner<3, 3>();
|
||||||
|
Matrix actualH;
|
||||||
|
const Matrix3 actual = Q3.topLeft(actualH);
|
||||||
|
CHECK(assert_equal(expected, actual));
|
||||||
|
boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
|
||||||
|
return Q3.topLeft();
|
||||||
|
};
|
||||||
|
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
|
||||||
|
CHECK(assert_equal(numericalH, actualH));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(SO4, stiefel) {
|
||||||
|
const Matrix43 expected = Q3.leftCols<3>();
|
||||||
|
Matrix actualH;
|
||||||
|
const Matrix43 actual = Q3.stiefel(actualH);
|
||||||
|
CHECK(assert_equal(expected, actual));
|
||||||
|
boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
|
||||||
|
return Q3.stiefel();
|
||||||
|
};
|
||||||
|
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
|
||||||
|
CHECK(assert_equal(numericalH, actualH));
|
||||||
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
return TestRegistry::runAllTests(tr);
|
||||||
|
}
|
||||||
|
//******************************************************************************
|
||||||
Loading…
Reference in New Issue