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

All tests for SO3 now uncommented

release/4.3a0
Frank Dellaert 2019-05-06 16:38:05 -04:00 committed by Fan Jiang
parent f7ad80673c
commit d376d0158d
4 changed files with 29 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/SOn-inl.h
View File

@ -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>

12
gtsam/geometry/SOt.cpp
View File

@ -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();

22
gtsam/geometry/SOt.h
View File

@ -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 <>

14
gtsam/geometry/tests/testSO3.cpp
View File

@ -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