Hat and Vee for SO4

release/4.3a0
Frank Dellaert 2019-05-06 18:45:41 -04:00 committed by Fan Jiang
parent ab1be9c4de
commit a765886e5a
3 changed files with 247 additions and 306 deletions

View File

@ -24,20 +24,23 @@
#include <Eigen/Eigenvalues> #include <Eigen/Eigenvalues>
#include <boost/random.hpp> #include <boost/random.hpp>
#include <algorithm>
#include <cmath> #include <cmath>
#include <iostream> #include <iostream>
#include <vector>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
// /* ************************************************************************* */ // /* *************************************************************************
// static Vector3 randomOmega(boost::mt19937 &rng) { // */ static Vector3 randomOmega(boost::mt19937 &rng) {
// static boost::uniform_real<double> randomAngle(-M_PI, M_PI); // static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
// return Unit3::Random(rng).unitVector() * randomAngle(rng); // return Unit3::Random(rng).unitVector() * randomAngle(rng);
// } // }
// /* ************************************************************************* */ // /* *************************************************************************
// */
// // Create random SO(4) element using direct product of lie algebras. // // Create random SO(4) element using direct product of lie algebras.
// SO4 SO4::Random(boost::mt19937 &rng) { // SO4 SO4::Random(boost::mt19937 &rng) {
// Vector6 delta; // Vector6 delta;
@ -45,185 +48,179 @@ namespace gtsam {
// return SO4::Expmap(delta); // return SO4::Expmap(delta);
// } // }
// /* ************************************************************************* */ //******************************************************************************
// void SO4::print(const string &s) const { cout << s << *this << endl; } template <>
Matrix4 SO4::Hat(const Vector6& xi) {
// skew symmetric matrix X = xi^
// Unlike Luca, makes upper-left the SO(3) subgroup.
Matrix4 Y = Z_4x4;
Y(0, 1) = -xi(5);
Y(0, 2) = +xi(4);
Y(1, 2) = -xi(3);
Y(0, 3) = -xi(2);
Y(1, 3) = +xi(1);
Y(2, 3) = -xi(0);
return Y - Y.transpose();
}
// /* ************************************************************************* */ //******************************************************************************
// bool SO4::equals(const SO4 &R, double tol) const { template <>
// return equal_with_abs_tol(*this, R, tol); Vector6 SO4::Vee(const Matrix4& X) {
// } Vector6 xi;
xi(5) = -X(0, 1);
xi(4) = +X(0, 2);
xi(3) = -X(1, 2);
xi(2) = -X(0, 3);
xi(1) = +X(1, 3);
xi(0) = -X(2, 3);
return xi;
}
// //****************************************************************************** //******************************************************************************
// Matrix4 SO4::Hat(const Vector6 &xi) { /* Exponential map, porting MATLAB implementation by Luca, which follows
// // skew symmetric matrix X = xi^ * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
// // Unlike Luca, makes upper-left the SO(3) subgroup. * Ramona-Andreaa Rohan */
// // See also template <>
// // http://scipp.ucsc.edu/~haber/archives/physics251_13/groups13_sol4.pdf SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
// Matrix4 Y = Z_4x4; using namespace std;
// Y(0, 1) = -xi(2); if (H) throw std::runtime_error("SO4::Expmap Jacobian");
// 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;
// }
// //****************************************************************************** // skew symmetric matrix X = xi^
// /* Exponential map, porting MATLAB implementation by Luca, which follows const Matrix4 X = Hat(xi);
// * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
// * Ramona-Andreaa Rohan */
// template <>
// SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
// using namespace std;
// if (H) throw std::runtime_error("SO4::Expmap Jacobian");
// // skew symmetric matrix X = xi^ // do eigen-decomposition
// const Matrix4 X = Hat(xi); 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());
});
// // do eigen-decomposition // Get a and b from eigenvalues +/i ai and +/- bi
// auto eig = Eigen::EigenSolver<Matrix4>(X); double a = e[0].imag(), b = e[2].imag();
// Eigen::Vector4cd e = eig.eigenvalues(); if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
// using std::abs; throw runtime_error("SO4::Expmap: wrong eigenvalues.");
// 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 // Build expX = exp(xi^)
// double a = e[0].imag(), b = e[2].imag(); Matrix4 expX;
// if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) { using std::cos;
// throw runtime_error("SO4::Expmap: wrong eigenvalues."); 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 SO4(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 SO4(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 SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
} else {
return SO4();
}
}
// // Build expX = exp(xi^) //******************************************************************************
// Matrix4 expX; // local vectorize
// using std::cos; static SO4::VectorN2 vec4(const Matrix4& Q) {
// using std::sin; return Eigen::Map<const SO4::VectorN2>(Q.data());
// 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 SO4(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 SO4(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 SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
// } else {
// return SO4();
// }
// }
// //****************************************************************************** // so<4> generators
// static SO4::VectorN2 vec4(const Matrix4& Q) { static const std::vector<const Matrix4> G4(
// return Eigen::Map<const SO4::VectorN2>(Q.data()); {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 std::vector<const Matrix4> G4( // vectorized generators
// {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)), static const Eigen::Matrix<double, 16, 6> P4 =
// SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)), (Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]),
// SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))}); vec4(G4[3]), vec4(G4[4]), vec4(G4[5]))
.finished();
// static const Eigen::Matrix<double, 16, 6> P4 = //******************************************************************************
// (Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]), template <>
// vec4(G4[3]), vec4(G4[4]), vec4(G4[5])) Matrix6 SO4::AdjointMap() const {
// .finished(); // Elaborate way of calculating the AdjointMap
// TODO(frank): find a closed form solution. In SO(3) is just R :-/
const Matrix4& Q = matrix_;
const Matrix4 Qt = Q.transpose();
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 * G4[i] * Qt);
}
return A;
}
// //****************************************************************************** //******************************************************************************
// template <> template <>
// Matrix6 SO4::AdjointMap() const { SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const {
// // Elaborate way of calculating the AdjointMap const Matrix& Q = matrix_;
// // TODO(frank): find a closed form solution. In SO(3) is just R :-/ if (H) {
// const Matrix4& Q = matrix_; // As Luca calculated, this is (I4 \oplus Q) * P4
// const Matrix4 Qt = Q.transpose(); *H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0),
// Matrix6 A; Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0);
// for (size_t i = 0; i < 6; i++) { }
// // Calculate column i of linear map for coeffcient of Gi return gtsam::vec4(Q);
// A.col(i) = SO4::Vee(Q * G4[i] * Qt); }
// }
// return A;
// }
// //****************************************************************************** ///******************************************************************************
// template <> template <>
// SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const { SO4 SO4::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
// const Matrix& Q = matrix_; if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
// if (H) { gttic(SO4_Retract);
// // As Luca calculated, this is (I4 \oplus Q) * P4 const Matrix4 X = Hat(xi / 2);
// *H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0), return SO4((I_4x4 + X) * (I_4x4 - X).inverse());
// Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0); }
// }
// return gtsam::vec4(Q);
// }
//******************************************************************************
template <>
Vector6 SO4::ChartAtOrigin::Local(const SO4& Q, ChartJacobian H) {
if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
const Matrix4& R = Q.matrix();
const Matrix4 X = (I_4x4 - R) * (I_4x4 + R).inverse();
return -2 * Vee(X);
}
// //****************************************************************************** //******************************************************************************
// Vector6 SO4::Logmap(const SO4 &Q, ChartJacobian H) { Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) {
// if (H) throw std::runtime_error("SO4::Logmap Jacobian"); const Matrix4& R = Q.matrix();
// throw std::runtime_error("SO4::Logmap"); const Matrix3 M = R.topLeftCorner<3, 3>();
// } if (H) {
const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
q = R.topRightCorner<3, 1>();
*H << Z_3x1, Z_3x1, q, Z_3x1, -m3, m2, //
Z_3x1, -q, Z_3x1, m3, Z_3x1, -m1, //
q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
}
return M;
}
// /* ************************************************************************* */ //******************************************************************************
// SO4 SO4::ChartAtOrigin::Retract(const Vector6 &xi, ChartJacobian H) { Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) {
// if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian"); const Matrix4& R = Q.matrix();
// gttic(SO4_Retract); const Matrix43 M = R.leftCols<3>();
// const Matrix4 X = Hat(xi / 2); if (H) {
// return (I_4x4 + X) * (I_4x4 - X).inverse(); const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3);
// } *H << Z_4x1, Z_4x1, q, Z_4x1, -m3, m2, //
Z_4x1, -q, Z_4x1, m3, Z_4x1, -m1, //
q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
}
return M;
}
// /* ************************************************************************* */ //******************************************************************************
// 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);
// }
// /* ************************************************************************* */
// 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 } // end namespace gtsam

View File

@ -38,120 +38,40 @@ using SO4 = SO<4>;
// /// Random SO(4) element (no big claims about uniformity) // /// Random SO(4) element (no big claims about uniformity)
// static SO4 Random(boost::mt19937 &rng); // static SO4 Random(boost::mt19937 &rng);
// static Matrix4 Hat(const Vector6 &xi); ///< make skew symmetric matrix // Below are all declarations of SO<4> specializations.
// static Vector6 Vee(const Matrix4 &X); ///< inverse of Hat // They are *defined* in SO4.cpp.
// 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;
//******************************************************************************
/* 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 */
template <> template <>
SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) { Matrix4 SO4::Hat(const TangentVector& xi);
using namespace std;
if (H) throw std::runtime_error("SO4::Expmap Jacobian");
// 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 SO4(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 SO4(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 SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
} else {
return SO4();
}
}
//******************************************************************************
static SO4::VectorN2 vec4(const Matrix4& Q) {
return Eigen::Map<const SO4::VectorN2>(Q.data());
}
static const std::vector<const Matrix4> G4(
{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> P4 =
(Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]),
vec4(G4[3]), vec4(G4[4]), vec4(G4[5]))
.finished();
//******************************************************************************
template <> template <>
Matrix6 SO4::AdjointMap() const { Vector6 SO4::Vee(const Matrix4& X);
// Elaborate way of calculating the AdjointMap
// TODO(frank): find a closed form solution. In SO(3) is just R :-/
const Matrix4& Q = matrix_;
const Matrix4 Qt = Q.transpose();
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 * G4[i] * Qt);
}
return A;
}
//******************************************************************************
template <> template <>
SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const { SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H);
const Matrix& Q = matrix_;
if (H) {
// As Luca calculated, this is (I4 \oplus Q) * P4
*H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0),
Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0);
}
return gtsam::vec4(Q);
}
// /// Vectorize template <>
// Vector16 vec(OptionalJacobian<16, 6> H = boost::none) const; Matrix6 SO4::AdjointMap() const;
// /// Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3). template <>
// Matrix3 topLeft(OptionalJacobian<9, 6> H = boost::none) const; SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const;
// /// Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., template <>
// pi(Q) -> S \in St(3,4). Matrix43 stiefel(OptionalJacobian<12, 6> H = SO4 SO4::ChartAtOrigin::Retract(const Vector6& omega, ChartJacobian H);
// boost::none) const;
template <>
Vector6 SO4::ChartAtOrigin::Local(const SO4& R, ChartJacobian H);
/**
* Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3).
*/
Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H = boost::none);
/**
* Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q)
* -> S \in St(3,4).
*/
Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H = boost::none);
// private: // private:
// /** Serialization function */ // /** Serialization function */

View File

@ -19,8 +19,8 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/lieProxies.h> #include <gtsam/base/lieProxies.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/SOt.h>
#include <gtsam/geometry/SO4.h> #include <gtsam/geometry/SO4.h>
#include <gtsam/geometry/SOt.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -30,7 +30,6 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
//****************************************************************************** //******************************************************************************
TEST(SO4, Identity) { TEST(SO4, Identity) {
const SO4 R; const SO4 R;
EXPECT_LONGS_EQUAL(4, R.rows()); EXPECT_LONGS_EQUAL(4, R.rows());
@ -83,13 +82,32 @@ TEST(SO4, Expmap) {
} }
//****************************************************************************** //******************************************************************************
TEST(SO4, Cayley) { TEST(SO4, Hat) {
CHECK(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100))); // Check that Hat specialization is equal to dynamic version
CHECK(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100))); EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1)));
EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2)));
EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3)));
}
//******************************************************************************
TEST(SO4, Vee) {
// Check that Hat specialization is equal to dynamic version
auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1)));
EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2)));
EXPECT(assert_equal(SO4::Vee(X3), SOn::Vee(X3)));
} }
//****************************************************************************** //******************************************************************************
TEST(SO4, Retract) { TEST(SO4, Retract) {
// Check that Cayley yields the same as Expmap for small values
EXPECT(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100)));
EXPECT(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100)));
// Check that Cayley is identical to dynamic version
EXPECT(assert_equal(SOn(id.retract(v1 / 100)), SOn(4).retract(v1 / 100)));
EXPECT(assert_equal(SOn(id.retract(v2 / 100)), SOn(4).retract(v2 / 100)));
auto v = Vector6::Zero(); auto v = Vector6::Zero();
SO4 actual = traits<SO4>::Retract(id, v); SO4 actual = traits<SO4>::Retract(id, v);
EXPECT(assert_equal(id, actual)); EXPECT(assert_equal(id, actual));
@ -97,6 +115,12 @@ TEST(SO4, Retract) {
//****************************************************************************** //******************************************************************************
TEST(SO4, Local) { TEST(SO4, Local) {
// Check that Cayley is identical to dynamic version
EXPECT(
assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1))));
EXPECT(
assert_equal(id.localCoordinates(Q2), SOn(4).localCoordinates(SOn(Q2))));
auto v0 = Vector6::Zero(); auto v0 = Vector6::Zero();
Vector6 actual = traits<SO4>::Local(id, id); Vector6 actual = traits<SO4>::Local(id, id);
EXPECT(assert_equal((Vector)v0, actual)); EXPECT(assert_equal((Vector)v0, actual));
@ -122,15 +146,15 @@ TEST(SO4, compose) {
SO4 expected = Q1 * Q2; SO4 expected = Q1 * Q2;
Matrix actualH1, actualH2; Matrix actualH1, actualH2;
SO4 actual = Q1.compose(Q2, actualH1, actualH2); SO4 actual = Q1.compose(Q2, actualH1, actualH2);
CHECK(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
Matrix numericalH1 = Matrix numericalH1 =
numericalDerivative21(testing::compose<SO4>, Q1, Q2, 1e-2); numericalDerivative21(testing::compose<SO4>, Q1, Q2, 1e-2);
CHECK(assert_equal(numericalH1, actualH1)); EXPECT(assert_equal(numericalH1, actualH1));
Matrix numericalH2 = Matrix numericalH2 =
numericalDerivative22(testing::compose<SO4>, Q1, Q2, 1e-2); numericalDerivative22(testing::compose<SO4>, Q1, Q2, 1e-2);
CHECK(assert_equal(numericalH2, actualH2)); EXPECT(assert_equal(numericalH2, actualH2));
} }
//****************************************************************************** //******************************************************************************
@ -139,39 +163,39 @@ TEST(SO4, vec) {
const Vector16 expected = Eigen::Map<const Vector16>(Q2.matrix().data()); const Vector16 expected = Eigen::Map<const Vector16>(Q2.matrix().data());
Matrix actualH; Matrix actualH;
const Vector16 actual = Q2.vec(actualH); const Vector16 actual = Q2.vec(actualH);
CHECK(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
boost::function<Vector16(const SO4&)> f = [](const SO4& Q) { boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
return Q.vec(); return Q.vec();
}; };
const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
CHECK(assert_equal(numericalH, actualH)); EXPECT(assert_equal(numericalH, actualH));
} }
// /* ************************************************************************* //******************************************************************************
// */ TEST(SO4, topLeft) { TEST(SO4, topLeft) {
// const Matrix3 expected = Q3.topLeftCorner<3, 3>(); const Matrix3 expected = Q3.matrix().topLeftCorner<3, 3>();
// Matrix actualH; Matrix actualH;
// const Matrix3 actual = Q3.topLeft(actualH); const Matrix3 actual = topLeft(Q3, actualH);
// CHECK(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
// boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) { boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
// return Q3.topLeft(); return topLeft(Q3);
// }; };
// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
// CHECK(assert_equal(numericalH, actualH)); EXPECT(assert_equal(numericalH, actualH));
// } }
// /* ************************************************************************* //******************************************************************************
// */ TEST(SO4, stiefel) { TEST(SO4, stiefel) {
// const Matrix43 expected = Q3.leftCols<3>(); const Matrix43 expected = Q3.matrix().leftCols<3>();
// Matrix actualH; Matrix actualH;
// const Matrix43 actual = Q3.stiefel(actualH); const Matrix43 actual = stiefel(Q3, actualH);
// CHECK(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
// boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) { boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
// return Q3.stiefel(); return stiefel(Q3);
// }; };
// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
// CHECK(assert_equal(numericalH, actualH)); EXPECT(assert_equal(numericalH, actualH));
// } }
//****************************************************************************** //******************************************************************************
int main() { int main() {