All tests for SO3 now uncommented
parent
f7ad80673c
commit
d376d0158d
|
@ -115,7 +115,7 @@ typename SO<N>::MatrixDD SO<N>::ExpmapDerivative(const TangentVector& omega) {
|
|||
|
||||
template <int N>
|
||||
typename SO<N>::TangentVector SO<N>::Logmap(const SO& R, ChartJacobian H) {
|
||||
throw std::runtime_error("SO<N>::Logmap only implemented for SO3 and SO4.");
|
||||
throw std::runtime_error("SO<N>::Logmap only implemented for SO3.");
|
||||
}
|
||||
|
||||
template <int N>
|
||||
|
|
|
@ -182,9 +182,10 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
|
|||
}
|
||||
}
|
||||
|
||||
// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
|
||||
// return sot::DexpFunctor(omega).dexp();
|
||||
// }
|
||||
template <>
|
||||
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
|
||||
return sot::DexpFunctor(omega).dexp();
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
/* Right Jacobian for Log map in SO(3) - equation (10.86) and following
|
||||
|
@ -254,19 +255,22 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
|
|||
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
|
||||
}
|
||||
|
||||
if (H) *H = SO3::LogmapDerivative(omega);
|
||||
if (H) *H = LogmapDerivative(omega);
|
||||
return omega;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
// local vectorize
|
||||
static Vector9 vec3(const Matrix3& R) {
|
||||
return Eigen::Map<const Vector9>(R.data());
|
||||
}
|
||||
|
||||
// so<3> generators
|
||||
static const std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
|
||||
SO3::Hat(Vector3::Unit(1)),
|
||||
SO3::Hat(Vector3::Unit(2))});
|
||||
|
||||
// vectorized generators
|
||||
static const Matrix93 P3 =
|
||||
(Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished();
|
||||
|
||||
|
|
|
@ -42,6 +42,15 @@ using SO3 = SO<3>;
|
|||
// static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix
|
||||
// static Vector3 Vee(const Matrix3 &X); ///< inverse of Hat
|
||||
|
||||
// Below are all declarations of SO<3> specializations.
|
||||
// They are *defined* in SO3.cpp.
|
||||
|
||||
/// Adjoint map
|
||||
template <>
|
||||
Matrix3 SO3::AdjointMap() const {
|
||||
return matrix_;
|
||||
}
|
||||
|
||||
/**
|
||||
* Exponential map at identity - create a rotation from canonical coordinates
|
||||
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
|
||||
|
@ -49,13 +58,9 @@ using SO3 = SO<3>;
|
|||
template <>
|
||||
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
|
||||
|
||||
// template<>
|
||||
// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
|
||||
// return sot::DexpFunctor(omega).dexp();
|
||||
// }
|
||||
|
||||
/// Derivative of Expmap
|
||||
// static Matrix3 ExpmapDerivative(const Vector3& omega);
|
||||
template <>
|
||||
Matrix3 SO3::ExpmapDerivative(const Vector3& omega);
|
||||
|
||||
/**
|
||||
* Log map at identity - returns the canonical coordinates
|
||||
|
@ -64,10 +69,9 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
|
|||
template <>
|
||||
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H);
|
||||
|
||||
/// Derivative of Logmap
|
||||
template <>
|
||||
Matrix3 SO3::AdjointMap() const {
|
||||
return matrix_;
|
||||
}
|
||||
Matrix3 SO3::LogmapDerivative(const Vector3& omega);
|
||||
|
||||
// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
|
||||
template <>
|
||||
|
|
|
@ -157,7 +157,7 @@ TEST(SO3, ExpmapDerivative) {
|
|||
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>(
|
||||
|
@ -168,7 +168,7 @@ TEST(SO3, ExpmapDerivative2) {
|
|||
SO3::ExpmapDerivative(-theta)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, ExpmapDerivative3) {
|
||||
const Vector3 theta(10, 20, 30);
|
||||
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
|
||||
|
@ -179,7 +179,7 @@ TEST(SO3, ExpmapDerivative3) {
|
|||
SO3::ExpmapDerivative(-theta)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, ExpmapDerivative4) {
|
||||
// Iserles05an (Lie-group Methods) says:
|
||||
// scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
|
||||
|
@ -207,7 +207,7 @@ TEST(SO3, ExpmapDerivative4) {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
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); };
|
||||
|
@ -223,7 +223,7 @@ TEST(SO3, ExpmapDerivative5) {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, ExpmapDerivative6) {
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
|
||||
|
@ -233,7 +233,7 @@ TEST(SO3, ExpmapDerivative6) {
|
|||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, LogmapDerivative) {
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const SO3 R = SO3::Expmap(thetahat); // some rotation
|
||||
|
@ -243,7 +243,7 @@ TEST(SO3, LogmapDerivative) {
|
|||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, JacobianLogmap) {
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const SO3 R = SO3::Expmap(thetahat); // some rotation
|
||||
|
|
Loading…
Reference in New Issue