diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp index 1f265ecba..93c5f7f8e 100644 --- a/gtsam/geometry/SO4.cpp +++ b/gtsam/geometry/SO4.cpp @@ -24,20 +24,23 @@ #include #include +#include #include #include +#include using namespace std; namespace gtsam { -// /* ************************************************************************* */ -// static Vector3 randomOmega(boost::mt19937 &rng) { +// /* ************************************************************************* +// */ static Vector3 randomOmega(boost::mt19937 &rng) { // static boost::uniform_real 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; @@ -45,185 +48,179 @@ namespace gtsam { // 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 { -// return equal_with_abs_tol(*this, R, tol); -// } +//****************************************************************************** +template <> +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) { -// // 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 */ +template <> +SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) { + using namespace std; + if (H) throw std::runtime_error("SO4::Expmap Jacobian"); -// //****************************************************************************** -// /* 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 <> -// 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^ + const Matrix4 X = Hat(xi); -// // skew symmetric matrix X = xi^ -// const Matrix4 X = Hat(xi); + // do eigen-decomposition + auto eig = Eigen::EigenSolver(X); + Eigen::Vector4cd e = eig.eigenvalues(); + using std::abs; + sort(e.data(), e.data() + 4, [](complex a, complex b) { + return abs(a.imag()) > abs(b.imag()); + }); -// // do eigen-decomposition -// auto eig = Eigen::EigenSolver(X); -// Eigen::Vector4cd e = eig.eigenvalues(); -// using std::abs; -// sort(e.data(), e.data() + 4, [](complex a, complex 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."); + } -// // 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(); + } +} -// // 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(); -// } -// } +//****************************************************************************** +// local vectorize +static SO4::VectorN2 vec4(const Matrix4& Q) { + return Eigen::Map(Q.data()); +} -// //****************************************************************************** -// static SO4::VectorN2 vec4(const Matrix4& Q) { -// return Eigen::Map(Q.data()); -// } +// so<4> generators +static const std::vector 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 std::vector 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))}); +// vectorized generators +static const Eigen::Matrix P4 = + (Eigen::Matrix() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]), + vec4(G4[3]), vec4(G4[4]), vec4(G4[5])) + .finished(); -// static const Eigen::Matrix P4 = -// (Eigen::Matrix() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]), -// vec4(G4[3]), vec4(G4[4]), vec4(G4[5])) -// .finished(); +//****************************************************************************** +template <> +Matrix6 SO4::AdjointMap() const { + // 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 <> -// Matrix6 SO4::AdjointMap() const { -// // 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 <> +SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const { + 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); +} -// //****************************************************************************** -// template <> -// SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const { -// 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); -// } +///****************************************************************************** +template <> +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 SO4((I_4x4 + X) * (I_4x4 - X).inverse()); +} +//****************************************************************************** +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) { -// if (H) throw std::runtime_error("SO4::Logmap Jacobian"); -// throw std::runtime_error("SO4::Logmap"); -// } +//****************************************************************************** +Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) { + const Matrix4& R = Q.matrix(); + 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) { -// 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(); -// } +//****************************************************************************** +Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) { + const Matrix4& R = Q.matrix(); + const Matrix43 M = R.leftCols<3>(); + if (H) { + 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 diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index aed0d482d..a92cdfc5c 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -38,120 +38,40 @@ using SO4 = SO<4>; // /// Random SO(4) element (no big claims about uniformity) // static SO4 Random(boost::mt19937 &rng); -// 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; +// Below are all declarations of SO<4> specializations. +// They are *defined* in SO4.cpp. -//****************************************************************************** -/* 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 <> -SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) { - using namespace std; - if (H) throw std::runtime_error("SO4::Expmap Jacobian"); +Matrix4 SO4::Hat(const TangentVector& xi); - // skew symmetric matrix X = xi^ - const Matrix4 X = Hat(xi); - - // do eigen-decomposition - auto eig = Eigen::EigenSolver(X); - Eigen::Vector4cd e = eig.eigenvalues(); - using std::abs; - sort(e.data(), e.data() + 4, [](complex a, complex 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(Q.data()); -} - -static const std::vector 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 P4 = - (Eigen::Matrix() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]), - vec4(G4[3]), vec4(G4[4]), vec4(G4[5])) - .finished(); - -//****************************************************************************** template <> -Matrix6 SO4::AdjointMap() const { - // 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; -} +Vector6 SO4::Vee(const Matrix4& X); -//****************************************************************************** template <> -SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const { - 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); -} +SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H); -// /// Vectorize -// Vector16 vec(OptionalJacobian<16, 6> H = boost::none) const; +template <> +Matrix6 SO4::AdjointMap() 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; +template <> +SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) 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; +template <> +SO4 SO4::ChartAtOrigin::Retract(const Vector6& omega, ChartJacobian H); + +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: // /** Serialization function */ diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp index 29ae2d50f..d343e9397 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -19,8 +19,8 @@ #include #include #include -#include #include +#include #include @@ -30,7 +30,6 @@ using namespace std; using namespace gtsam; //****************************************************************************** - TEST(SO4, Identity) { const SO4 R; EXPECT_LONGS_EQUAL(4, R.rows()); @@ -83,13 +82,32 @@ TEST(SO4, Expmap) { } //****************************************************************************** -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, Hat) { + // Check that Hat specialization is equal to dynamic version + 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) { + // 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(); SO4 actual = traits::Retract(id, v); EXPECT(assert_equal(id, actual)); @@ -97,6 +115,12 @@ TEST(SO4, Retract) { //****************************************************************************** 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(); Vector6 actual = traits::Local(id, id); EXPECT(assert_equal((Vector)v0, actual)); @@ -122,15 +146,15 @@ TEST(SO4, compose) { SO4 expected = Q1 * Q2; Matrix actualH1, actualH2; SO4 actual = Q1.compose(Q2, actualH1, actualH2); - CHECK(assert_equal(expected, actual)); + EXPECT(assert_equal(expected, actual)); Matrix numericalH1 = numericalDerivative21(testing::compose, Q1, Q2, 1e-2); - CHECK(assert_equal(numericalH1, actualH1)); + EXPECT(assert_equal(numericalH1, actualH1)); Matrix numericalH2 = numericalDerivative22(testing::compose, 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(Q2.matrix().data()); Matrix actualH; const Vector16 actual = Q2.vec(actualH); - CHECK(assert_equal(expected, actual)); + EXPECT(assert_equal(expected, actual)); boost::function f = [](const SO4& Q) { return Q.vec(); }; const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); - CHECK(assert_equal(numericalH, actualH)); + EXPECT(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 f = [](const SO4& Q3) { -// return Q3.topLeft(); -// }; -// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); -// CHECK(assert_equal(numericalH, actualH)); -// } +//****************************************************************************** +TEST(SO4, topLeft) { + const Matrix3 expected = Q3.matrix().topLeftCorner<3, 3>(); + Matrix actualH; + const Matrix3 actual = topLeft(Q3, actualH); + EXPECT(assert_equal(expected, actual)); + boost::function f = [](const SO4& Q3) { + return topLeft(Q3); + }; + const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); + EXPECT(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 f = [](const SO4& Q3) { -// return Q3.stiefel(); -// }; -// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); -// CHECK(assert_equal(numericalH, actualH)); -// } +//****************************************************************************** +TEST(SO4, stiefel) { + const Matrix43 expected = Q3.matrix().leftCols<3>(); + Matrix actualH; + const Matrix43 actual = stiefel(Q3, actualH); + EXPECT(assert_equal(expected, actual)); + boost::function f = [](const SO4& Q3) { + return stiefel(Q3); + }; + const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); + EXPECT(assert_equal(numericalH, actualH)); +} //****************************************************************************** int main() {