12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Distributed SOn functionality over three files

release/4.3a0
Frank Dellaert 2019-05-06 15:37:04 -04:00 committed by Fan Jiang
parent 6cff36975f
commit 7213fd2c62
4 changed files with 189 additions and 527 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

127
gtsam/geometry/SOn-inl.h Normal file
View File

@ -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 <gtsam/base/Matrix.h>
namespace gtsam {
template <int N>
Matrix SO<N>::Hat(const Vector& xi) {
size_t n = AmbientDim(xi.size());
if (n < 2) throw std::invalid_argument("SO<N>::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 <int N>
Vector SO<N>::Vee(const Matrix& X) {
const size_t n = X.rows();
if (n < 2) throw std::invalid_argument("SO<N>::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 <int N>
SO<N> SO<N>::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 <int N>
typename SO<N>::TangentVector SO<N>::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 <int N>
typename SO<N>::VectorN2 SO<N>::vec(
OptionalJacobian<internal::NSquaredSO(N), dimension> H) const {
const size_t n = rows();
const size_t n2 = n * n;
// Vectorize
VectorN2 X(n2);
X << Eigen::Map<const Matrix>(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<const Matrix>(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

40
gtsam/geometry/SOn.cpp Normal file
View File

@ -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 <gtsam/geometry/SOn.h>
namespace gtsam {
template <>
SOn LieGroup<SOn, Eigen::Dynamic>::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<SOn, Eigen::Dynamic>::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

113
gtsam/geometry/SOn.h
View File

@ -188,64 +188,12 @@ class SO : public LieGroup<SO<N>, 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<SO<N>, 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<SO<N>, internal::DimensionSO(N)> {
throw std::runtime_error("SO<N>::Logmap only implemented for SO3 and SO4.");
}
// template <int N_ = N, typename = IsSO3<N_>>
static Matrix3 LogmapDerivative(const Vector3& omega);
// inverse with optional derivative
using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse;
@ -313,39 +255,26 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
* X and fixed-size Jacobian if dimension is known at compile time.
* */
VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
boost::none) const {
const size_t n = rows();
const size_t n2 = n * n;
// Vectorize
VectorN2 X(n2);
X << Eigen::Map<const Matrix>(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<const Matrix>(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<Eigen::Dynamic>;
/*
* 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<Eigen::Dynamic>;
using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
template <>
SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
DynamicJacobian H2) const;
template <>
SOn LieGroup<SOn, Eigen::Dynamic>::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 <int N>
struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
} // namespace gtsam
#include "SOn-inl.h"

436
gtsam/geometry/tests/testSOn.cpp
View File

@ -15,7 +15,6 @@
* @author Frank Dellaert
**/
#include <gtsam/geometry/SO4.h>
#include <gtsam/geometry/SOn.h>
#include <gtsam/geometry/SOt.h>
@ -36,27 +35,6 @@
#include <stdexcept>
#include <type_traits>
namespace gtsam {
using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
template <>
SOn LieGroup<SOn, Eigen::Dynamic>::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<SOn, Eigen::Dynamic>::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<SO4>));
BOOST_CONCEPT_ASSERT((IsManifold<SO4>));
BOOST_CONCEPT_ASSERT((IsLieGroup<SO4>));
}
//******************************************************************************
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<SO4>::Retract(I4, v);
EXPECT(assert_equal(I4, actual));
}
//******************************************************************************
TEST(SO4, Local) {
auto v0 = Vector6::Zero();
Vector6 actual = traits<SO4>::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<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::VectorN2;
const Vector16 expected = Eigen::Map<const Vector16>(Q2.matrix().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));
// }
//******************************************************************************
// 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<SO3>));
BOOST_CONCEPT_ASSERT((IsManifold<SO3>));
BOOST_CONCEPT_ASSERT((IsLieGroup<SO3>));
}
//******************************************************************************
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<Vector3, Vector3>(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<SO3, Vector3>(
// 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<SO3, Vector3>(
// 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<SO3, double>(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<SO3, double>(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<SO3, Vector3>(
// 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<Vector, SO3>(
// 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<Vector, SO3>(
// 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<Vector3(const Vector3&, const Vector3&)> 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<Vector3(const Vector3&, const Vector3&)> 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<const Vector9>(R2.matrix().data());
Matrix actualH;
const Vector9 actual = R2.vec(actualH);
CHECK(assert_equal(expected, actual));
boost::function<Vector9(const SO3&)> 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<Matrix3(const Matrix3&)> 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;