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