diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h new file mode 100644 index 000000000..26dc2229b --- /dev/null +++ b/gtsam/geometry/SOn-inl.h @@ -0,0 +1,127 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +#pragma once + +/** + * @file SOn-inl.h + * @brief Template implementations for SO(n) + * @author Frank Dellaert + * @date March 2019 + */ + +#include + +namespace gtsam { + +template +Matrix SO::Hat(const Vector& xi) { + size_t n = AmbientDim(xi.size()); + if (n < 2) throw std::invalid_argument("SO::Hat: n<2 not supported"); + + Matrix X(n, n); // allocate space for n*n skew-symmetric matrix + X.setZero(); + if (n == 2) { + // Handle SO(2) case as recursion bottom + assert(xi.size() == 1); + X << 0, -xi(0), xi(0), 0; + } else { + // Recursively call SO(n-1) call for top-left block + const size_t dmin = (n - 1) * (n - 2) / 2; + X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin)); + + // Now fill last row and column + double sign = 1.0; + for (size_t i = 0; i < n - 1; i++) { + const size_t j = n - 2 - i; + X(n - 1, j) = sign * xi(i); + X(j, n - 1) = -X(n - 1, j); + sign = -sign; + } + } + return X; +} + +template +Vector SO::Vee(const Matrix& X) { + const size_t n = X.rows(); + if (n < 2) throw std::invalid_argument("SO::Hat: n<2 not supported"); + + if (n == 2) { + // Handle SO(2) case as recursion bottom + Vector xi(1); + xi(0) = X(1, 0); + return xi; + } else { + // Calculate dimension and allocate space + const size_t d = n * (n - 1) / 2; + Vector xi(d); + + // Fill first n-1 spots from last row of X + double sign = 1.0; + for (size_t i = 0; i < n - 1; i++) { + const size_t j = n - 2 - i; + xi(i) = sign * X(n - 1, j); + sign = -sign; + } + + // Recursively call Vee to fill remainder of x + const size_t dmin = (n - 1) * (n - 2) / 2; + xi.tail(dmin) = Vee(X.topLeftCorner(n - 1, n - 1)); + return xi; + } +} + +template +SO SO::ChartAtOrigin::Retract(const TangentVector& xi, ChartJacobian H) { + const Matrix X = Hat(xi / 2.0); + size_t n = AmbientDim(xi.size()); + const auto I = Eigen::MatrixXd::Identity(n, n); + return SO((I + X) * (I - X).inverse()); +} + +template +typename SO::TangentVector SO::ChartAtOrigin::Local(const SO& R, + ChartJacobian H) { + const size_t n = R.rows(); + const auto I = Eigen::MatrixXd::Identity(n, n); + const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse(); + return -2 * Vee(X); +} + +template +typename SO::VectorN2 SO::vec( + OptionalJacobian H) const { + const size_t n = rows(); + const size_t n2 = n * n; + + // Vectorize + VectorN2 X(n2); + X << Eigen::Map(matrix_.data(), n2, 1); + + // If requested, calculate H as (I \oplus Q) * P + if (H) { + // Calculate P matrix of vectorized generators + const size_t d = dim(); + Matrix P(n2, d); + for (size_t j = 0; j < d; j++) { + const auto X = Hat(Eigen::VectorXd::Unit(d, j)); + P.col(j) = Eigen::Map(X.data(), n2, 1); + } + H->resize(n2, d); + for (size_t i = 0; i < n; i++) { + H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d); + } + } + return X; +} + +} // namespace gtsam diff --git a/gtsam/geometry/SOn.cpp b/gtsam/geometry/SOn.cpp new file mode 100644 index 000000000..67b780db8 --- /dev/null +++ b/gtsam/geometry/SOn.cpp @@ -0,0 +1,40 @@ +/* ---------------------------------------------------------------------------- + + * 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 SOn.cpp + * @brief Definitions of dynamic specializations of SO(n) + * @author Frank Dellaert + * @date March 2019 + */ + +#include + +namespace gtsam { + +template <> +SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const { + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = SOn::IdentityJacobian(g.rows()); + return derived() * g; +} + +template <> +SOn LieGroup::between(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const { + SOn result = derived().inverse() * g; + if (H1) *H1 = -result.inverse().AdjointMap(); + if (H2) *H2 = SOn::IdentityJacobian(g.rows()); + return result; +} + +} // namespace gtsam diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 4f4fcd157..64718ed5f 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -188,64 +188,12 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * -d c -b a 0 * This scheme behaves exactly as expected for SO(2) and SO(3). */ - static Matrix Hat(const Vector& xi) { - size_t n = AmbientDim(xi.size()); - if (n < 2) throw std::invalid_argument("SOn::Hat: n<2 not supported"); - - Matrix X(n, n); // allocate space for n*n skew-symmetric matrix - X.setZero(); - if (n == 2) { - // Handle SO(2) case as recursion bottom - assert(xi.size() == 1); - X << 0, -xi(0), xi(0), 0; - } else { - // Recursively call SO(n-1) call for top-left block - const size_t dmin = (n - 1) * (n - 2) / 2; - X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin)); - - // Now fill last row and column - double sign = 1.0; - for (size_t i = 0; i < n - 1; i++) { - const size_t j = n - 2 - i; - X(n - 1, j) = sign * xi(i); - X(j, n - 1) = -X(n - 1, j); - sign = -sign; - } - } - return X; - } + static Matrix Hat(const Vector& xi); /** * Inverse of Hat. See note about xi element order in Hat. */ - static Vector Vee(const Matrix& X) { - const size_t n = X.rows(); - if (n < 2) throw std::invalid_argument("SOn::Hat: n<2 not supported"); - - if (n == 2) { - // Handle SO(2) case as recursion bottom - Vector xi(1); - xi(0) = X(1, 0); - return xi; - } else { - // Calculate dimension and allocate space - const size_t d = n * (n - 1) / 2; - Vector xi(d); - - // Fill first n-1 spots from last row of X - double sign = 1.0; - for (size_t i = 0; i < n - 1; i++) { - const size_t j = n - 2 - i; - xi(i) = sign * X(n - 1, j); - sign = -sign; - } - - // Recursively call Vee to fill remainder of x - const size_t dmin = (n - 1) * (n - 2) / 2; - xi.tail(dmin) = Vee(X.topLeftCorner(n - 1, n - 1)); - return xi; - } - } + static Vector Vee(const Matrix& X); // Chart at origin struct ChartAtOrigin { @@ -253,21 +201,12 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * Retract uses Cayley map. See note about xi element order in Hat. * Deafault implementation has no Jacobian implemented */ - static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none) { - const Matrix X = Hat(xi / 2.0); - size_t n = AmbientDim(xi.size()); - const auto I = Eigen::MatrixXd::Identity(n, n); - return SO((I + X) * (I - X).inverse()); - } + static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none); + /** * Inverse of Retract. See note about xi element order in Hat. */ - static TangentVector Local(const SO& R, ChartJacobian H = boost::none) { - const size_t n = R.rows(); - const auto I = Eigen::MatrixXd::Identity(n, n); - const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse(); - return -2 * Vee(X); - } + static TangentVector Local(const SO& R, ChartJacobian H = boost::none); }; // Return dynamic identity DxD Jacobian for given SO(n) @@ -300,6 +239,9 @@ class SO : public LieGroup, internal::DimensionSO(N)> { throw std::runtime_error("SO::Logmap only implemented for SO3 and SO4."); } + // template > + static Matrix3 LogmapDerivative(const Vector3& omega); + // inverse with optional derivative using LieGroup, internal::DimensionSO(N)>::inverse; @@ -313,39 +255,26 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * X and fixed-size Jacobian if dimension is known at compile time. * */ VectorN2 vec(OptionalJacobian H = - boost::none) const { - const size_t n = rows(); - const size_t n2 = n * n; - - // Vectorize - VectorN2 X(n2); - X << Eigen::Map(matrix_.data(), n2, 1); - - // If requested, calculate H as (I \oplus Q) * P - if (H) { - // Calculate P matrix of vectorized generators - const size_t d = dim(); - Matrix P(n2, d); - for (size_t j = 0; j < d; j++) { - const auto X = Hat(Eigen::VectorXd::Unit(d, j)); - P.col(j) = Eigen::Map(X.data(), n2, 1); - } - H->resize(n2, d); - for (size_t i = 0; i < n; i++) { - H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d); - } - } - return X; - } + boost::none) const; /// @} }; +using SOn = SO; + /* * Fully specialize compose and between, because the derivative is unknowable by * the LieGroup implementations, who return a fixed-size matrix for H2. */ -using SOn = SO; +using DynamicJacobian = OptionalJacobian; + +template <> +SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const; + +template <> +SOn LieGroup::between(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const; /* * Define the traits. internal::LieGroup provides both Lie group and Testable @@ -358,3 +287,5 @@ template struct traits> : public internal::LieGroup> {}; } // namespace gtsam + +#include "SOn-inl.h" \ No newline at end of file diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 30f20b495..1e4aee927 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -15,7 +15,6 @@ * @author Frank Dellaert **/ -#include #include #include @@ -36,27 +35,6 @@ #include #include -namespace gtsam { -using DynamicJacobian = OptionalJacobian; - -template <> -SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, - DynamicJacobian H2) const { - if (H1) *H1 = g.inverse().AdjointMap(); - if (H2) *H2 = SOn::IdentityJacobian(g.rows()); - return derived() * g; -} - -template <> -SOn LieGroup::between(const SOn& g, DynamicJacobian H1, - DynamicJacobian H2) const { - SOn result = derived().inverse() * g; - if (H1) *H1 = -result.inverse().AdjointMap(); - if (H2) *H2 = SOn::IdentityJacobian(g.rows()); - return result; -} -} // namespace gtsam - using namespace std; using namespace gtsam; @@ -146,420 +124,6 @@ TEST(SOn, vec) { CHECK(assert_equal(H, actualH)); } -//****************************************************************************** -// SO4 -//****************************************************************************** - -TEST(SO4, Identity) { - const SO4 R; - EXPECT_LONGS_EQUAL(4, R.rows()); - EXPECT_LONGS_EQUAL(6, SO4::dimension); - EXPECT_LONGS_EQUAL(6, SO4::Dim()); - EXPECT_LONGS_EQUAL(6, R.dim()); -} - -//****************************************************************************** -TEST(SO4, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); -} - -//****************************************************************************** -SO4 I4; -Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished(); -SO4 Q1 = SO4::Expmap(v1); -Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).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.tail<3>()).matrix(); - EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1))); - - // Same here - auto R2 = SO3::Expmap(v2.tail<3>()).matrix(); - EXPECT((Q2.matrix().topLeftCorner<3, 3>().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(assert_equal(Q1 * Q2, Q2 * Q1)); - } -} - -//****************************************************************************** -TEST(SO4, Cayley) { - CHECK(assert_equal(I4.retract(v1 / 100), SO4::Expmap(v1 / 100))); - CHECK(assert_equal(I4.retract(v2 / 100), SO4::Expmap(v2 / 100))); -} - -//****************************************************************************** -TEST(SO4, Retract) { - auto v = Vector6::Zero(); - SO4 actual = traits::Retract(I4, v); - EXPECT(assert_equal(I4, actual)); -} - -//****************************************************************************** -TEST(SO4, Local) { - auto v0 = Vector6::Zero(); - Vector6 actual = traits::Local(I4, I4); - EXPECT(assert_equal((Vector)v0, actual)); -} - -//****************************************************************************** -TEST(SO4, Invariants) { - EXPECT(check_group_invariants(I4, I4)); - EXPECT(check_group_invariants(I4, Q1)); - EXPECT(check_group_invariants(Q2, I4)); - EXPECT(check_group_invariants(Q2, Q1)); - EXPECT(check_group_invariants(Q1, Q2)); - - EXPECT(check_manifold_invariants(I4, I4)); - EXPECT(check_manifold_invariants(I4, Q1)); - EXPECT(check_manifold_invariants(Q2, I4)); - 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, Q1, Q2, 1e-2); - CHECK(assert_equal(numericalH1, actualH1)); - - Matrix numericalH2 = - numericalDerivative22(testing::compose, Q1, Q2, 1e-2); - CHECK(assert_equal(numericalH2, actualH2)); -} - -//****************************************************************************** -TEST(SO4, vec) { - using Vector16 = SO4::VectorN2; - const Vector16 expected = Eigen::Map(Q2.matrix().data()); - Matrix actualH; - const Vector16 actual = Q2.vec(actualH); - CHECK(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)); -} - -// /* ************************************************************************* -// */ 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, 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)); -// } - -//****************************************************************************** -// SO3 -//****************************************************************************** - -TEST(SO3, Identity) { - const SO3 R; - EXPECT_LONGS_EQUAL(3, R.rows()); - EXPECT_LONGS_EQUAL(3, SO3::dimension); - EXPECT_LONGS_EQUAL(3, SO3::Dim()); - EXPECT_LONGS_EQUAL(3, R.dim()); -} - -//****************************************************************************** -TEST(SO3, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); -} - -//****************************************************************************** -TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } - -//****************************************************************************** -SO3 I3; -Vector3 z_axis(0, 0, 1); -SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); -SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); - -//****************************************************************************** -// TEST(SO3, Logmap) { -// Vector3 expected(0, 0, 0.1); -// Vector3 actual = SO3::Logmap(R1.between(R2)); -// EXPECT(assert_equal((Vector)expected, actual)); -// } - -//****************************************************************************** -TEST(SO3, Expmap) { - Vector3 v(0, 0, 0.1); - SO3 actual = R1 * SO3::Expmap(v); - EXPECT(assert_equal(R2, actual)); -} - -//****************************************************************************** -TEST(SO3, Invariants) { - EXPECT(check_group_invariants(I3, I3)); - EXPECT(check_group_invariants(I3, R1)); - EXPECT(check_group_invariants(R2, I3)); - EXPECT(check_group_invariants(R2, R1)); - - EXPECT(check_manifold_invariants(I3, I3)); - EXPECT(check_manifold_invariants(I3, R1)); - EXPECT(check_manifold_invariants(R2, I3)); - EXPECT(check_manifold_invariants(R2, R1)); -} - -//****************************************************************************** -// TEST(SO3, LieGroupDerivatives) { -// CHECK_LIE_GROUP_DERIVATIVES(I3, I3); -// CHECK_LIE_GROUP_DERIVATIVES(I3, R2); -// CHECK_LIE_GROUP_DERIVATIVES(R2, I3); -// CHECK_LIE_GROUP_DERIVATIVES(R2, R1); -// } - -//****************************************************************************** -// TEST(SO3, ChartDerivatives) { -// CHECK_CHART_DERIVATIVES(I3, I3); -// CHECK_CHART_DERIVATIVES(I3, R2); -// CHECK_CHART_DERIVATIVES(R2, I3); -// CHECK_CHART_DERIVATIVES(R2, R1); -// } - -// //****************************************************************************** -// namespace exmap_derivative { -// static const Vector3 w(0.1, 0.27, -0.2); -// } -// // Left trivialized Derivative of exp(w) wrpt w: -// // How does exp(w) change when w changes? -// // We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// // => y = log (exp(-w) * exp(w+dw)) -// Vector3 testDexpL(const Vector3& dw) { -// using exmap_derivative::w; -// return SO3::Logmap(SO3::Expmap(-w) * SO3::Expmap(w + dw)); -// } - -// TEST(SO3, ExpmapDerivative) { -// using exmap_derivative::w; -// const Matrix actualDexpL = SO3::ExpmapDerivative(w); -// const Matrix expectedDexpL = -// numericalDerivative11(testDexpL, Vector3::Zero(), -// 1e-2); -// EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-7)); - -// const Matrix actualDexpInvL = SO3::LogmapDerivative(w); -// EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7)); -// } - -// //****************************************************************************** -// TEST(SO3, ExpmapDerivative2) { -// const Vector3 theta(0.1, 0, 0.1); -// const Matrix Jexpected = numericalDerivative11( -// boost::bind(&SO3::Expmap, _1, boost::none), theta); - -// CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); -// CHECK(assert_equal(Matrix3(Jexpected.transpose()), -// SO3::ExpmapDerivative(-theta))); -// } - -// //****************************************************************************** -// TEST(SO3, ExpmapDerivative3) { -// const Vector3 theta(10, 20, 30); -// const Matrix Jexpected = numericalDerivative11( -// boost::bind(&SO3::Expmap, _1, boost::none), theta); - -// CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); -// CHECK(assert_equal(Matrix3(Jexpected.transpose()), -// SO3::ExpmapDerivative(-theta))); -// } - -// //****************************************************************************** -// TEST(SO3, ExpmapDerivative4) { -// // Iserles05an (Lie-group Methods) says: -// // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) -// // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) -// // where A(t): R -> so(3) is a trajectory in the tangent space of SO(3) -// // and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3) -// // Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3) - -// // In GTSAM, we don't work with the skew-symmetric matrices A directly, but -// // with 3-vectors. -// // omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t) - -// // Let's verify the above formula. - -// auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; -// auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; - -// // We define a function R -// auto R = [w](double t) { return SO3::Expmap(w(t)); }; - -// for (double t = -2.0; t < 2.0; t += 0.3) { -// const Matrix expected = numericalDerivative11(R, t); -// const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t); -// CHECK(assert_equal(expected, actual, 1e-7)); -// } -// } - -// //****************************************************************************** -// TEST(SO3, ExpmapDerivative5) { -// auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; -// auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; - -// // Now define R as mapping local coordinates to neighborhood around R0. -// const SO3 R0 = SO3::Expmap(Vector3(0.1, 0.4, 0.2)); -// auto R = [R0, w](double t) { return R0.expmap(w(t)); }; - -// for (double t = -2.0; t < 2.0; t += 0.3) { -// const Matrix expected = numericalDerivative11(R, t); -// const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t); -// CHECK(assert_equal(expected, actual, 1e-7)); -// } -// } - -// //****************************************************************************** -// TEST(SO3, ExpmapDerivative6) { -// const Vector3 thetahat(0.1, 0, 0.1); -// const Matrix Jexpected = numericalDerivative11( -// boost::bind(&SO3::Expmap, _1, boost::none), thetahat); -// Matrix3 Jactual; -// SO3::Expmap(thetahat, Jactual); -// EXPECT(assert_equal(Jexpected, Jactual)); -// } - -// /* ************************************************************************* -// */ -// TEST(SO3, LogmapDerivative) { -// const Vector3 thetahat(0.1, 0, 0.1); -// const SO3 R = SO3::Expmap(thetahat); // some rotation -// const Matrix Jexpected = numericalDerivative11( -// boost::bind(&SO3::Logmap, _1, boost::none), R); -// const Matrix3 Jactual = SO3::LogmapDerivative(thetahat); -// EXPECT(assert_equal(Jexpected, Jactual)); -// } - -// //****************************************************************************** -// TEST(SO3, JacobianLogmap) { -// const Vector3 thetahat(0.1, 0, 0.1); -// const SO3 R = SO3::Expmap(thetahat); // some rotation -// const Matrix Jexpected = numericalDerivative11( -// boost::bind(&SO3::Logmap, _1, boost::none), R); -// Matrix3 Jactual; -// SO3::Logmap(R, Jactual); -// EXPECT(assert_equal(Jexpected, Jactual)); -// } - -//****************************************************************************** -TEST(SO3, ApplyDexp) { - Matrix aH1, aH2; - for (bool nearZeroApprox : {true, false}) { - boost::function f = - [=](const Vector3& omega, const Vector3& v) { - return sot::DexpFunctor(omega, nearZeroApprox).applyDexp(v); - }; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - sot::DexpFunctor local(omega, nearZeroApprox); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { - EXPECT(assert_equal(Vector3(local.dexp() * v), - local.applyDexp(v, aH1, aH2))); - EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); - EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); - EXPECT(assert_equal(local.dexp(), aH2)); - } - } - } -} - -//****************************************************************************** -TEST(SO3, ApplyInvDexp) { - Matrix aH1, aH2; - for (bool nearZeroApprox : {true, false}) { - boost::function f = - [=](const Vector3& omega, const Vector3& v) { - return sot::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); - }; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - sot::DexpFunctor local(omega, nearZeroApprox); - Matrix invDexp = local.dexp().inverse(); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { - EXPECT(assert_equal(Vector3(invDexp * v), - local.applyInvDexp(v, aH1, aH2))); - EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); - EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); - EXPECT(assert_equal(invDexp, aH2)); - } - } - } -} - -//****************************************************************************** -TEST(SO3, vec) { - const Vector9 expected = Eigen::Map(R2.matrix().data()); - Matrix actualH; - const Vector9 actual = R2.vec(actualH); - CHECK(assert_equal(expected, actual)); - boost::function f = [](const SO3& Q) { return Q.vec(); }; - const Matrix numericalH = numericalDerivative11(f, R2, 1e-5); - CHECK(assert_equal(numericalH, actualH)); -} - -// //****************************************************************************** -// TEST(Matrix, compose) { -// Matrix3 M; -// M << 1, 2, 3, 4, 5, 6, 7, 8, 9; -// SO3 R = SO3::Expmap(Vector3(1, 2, 3)); -// const Matrix3 expected = M * R.matrix(); -// Matrix actualH; -// const Matrix3 actual = sot::compose(M, R, actualH); -// CHECK(assert_equal(expected, actual)); -// boost::function f = [R](const Matrix3& M) { -// return sot::compose(M, R); -// }; -// Matrix numericalH = numericalDerivative11(f, M, 1e-2); -// CHECK(assert_equal(numericalH, actualH)); -// } - //****************************************************************************** int main() { TestResult tr;