Modernized all of Expmap, moved tests from SO3, added ApplyExpmapDerivative
Frank Dellaert
parent
4a38c4f802
commit
29416436eb
|
|
@ -11,8 +11,10 @@
|
|||
|
||||
/**
|
||||
* @file SO3.cpp
|
||||
* @brief 3*3 matrix representation o SO(3)
|
||||
* @brief 3*3 matrix representation of SO(3)
|
||||
* @author Frank Dellaert
|
||||
* @author Luca Carlone
|
||||
* @author Duy Nguyen Ta
|
||||
* @date December 2014
|
||||
*/
|
||||
|
||||
|
|
@ -24,65 +26,94 @@
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Near zero, we just have I + skew(omega)
|
||||
static SO3 FirstOrder(const Vector3& omega) {
|
||||
Matrix3 R;
|
||||
R(0, 0) = 1.;
|
||||
R(1, 0) = omega.z();
|
||||
R(2, 0) = -omega.y();
|
||||
R(0, 1) = -omega.z();
|
||||
R(1, 1) = 1.;
|
||||
R(2, 1) = omega.x();
|
||||
R(0, 2) = omega.y();
|
||||
R(1, 2) = -omega.x();
|
||||
R(2, 2) = 1.;
|
||||
return R;
|
||||
}
|
||||
// Functor that helps implement Exponential map and its derivatives
|
||||
struct ExpmapImpl {
|
||||
const Vector3 omega;
|
||||
const double theta2;
|
||||
Matrix3 W;
|
||||
bool nearZero;
|
||||
double theta, s1, s2, c_1;
|
||||
|
||||
// omega: element of Lie algebra so(3): W = omega^, normalized by normx
|
||||
ExpmapImpl(const Vector3& omega) : omega(omega), theta2(omega.dot(omega)) {
|
||||
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
|
||||
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; // Skew[omega]
|
||||
nearZero = (theta2 <= std::numeric_limits<double>::epsilon());
|
||||
if (!nearZero) {
|
||||
theta = std::sqrt(theta2); // rotation angle
|
||||
s1 = std::sin(theta) / theta;
|
||||
s2 = std::sin(theta / 2.0);
|
||||
c_1 = 2.0 * s2 * s2; // numerically better behaved than [1 - cos(theta)]
|
||||
}
|
||||
}
|
||||
|
||||
SO3 operator()() const {
|
||||
if (nearZero)
|
||||
return I_3x3 + W;
|
||||
else
|
||||
return I_3x3 + s1 * W + c_1 * W * W / theta2;
|
||||
}
|
||||
|
||||
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
|
||||
// (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
|
||||
// Information Theory, and Lie Groups", Volume 2, 2008.
|
||||
// expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
|
||||
// This maps a perturbation v in the tangent space to
|
||||
// a perturbation on the manifold Expmap(dexp * v) */
|
||||
SO3 dexp() const {
|
||||
if (nearZero) {
|
||||
return I_3x3 - 0.5 * W;
|
||||
} else {
|
||||
const double a = c_1 / theta2;
|
||||
const double b = (1.0 - s1) / theta2;
|
||||
return I_3x3 - a * W + b * W * W;
|
||||
}
|
||||
}
|
||||
|
||||
// Just multiplies with dexp()
|
||||
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const {
|
||||
if (nearZero) {
|
||||
if (H1) *H1 = 0.5 * skewSymmetric(v);
|
||||
if (H2) *H2 = I_3x3;
|
||||
return v;
|
||||
} else {
|
||||
const double a = c_1 / theta2;
|
||||
const double b = (1.0 - s1) / theta2;
|
||||
Matrix3 dexp = I_3x3 - a * W + b * W * W;
|
||||
if (H1) {
|
||||
const Vector3 Wv = omega.cross(v);
|
||||
const Vector3 WWv = omega.cross(Wv);
|
||||
const Matrix3 T = skewSymmetric(v);
|
||||
const double Da = (s1 - 2.0 * a) / theta2;
|
||||
const double Db = (3.0 * s1 - std::cos(theta) - 2.0) / theta2 / theta2;
|
||||
*H1 = (-Da * Wv + Db * WWv) * omega.transpose() + a * T -
|
||||
b * skewSymmetric(Wv) - b * W * T;
|
||||
}
|
||||
if (H2) *H2 = dexp;
|
||||
return dexp * v;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
|
||||
if (theta*theta > std::numeric_limits<double>::epsilon()) {
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
// get components of axis \omega, where is a unit vector
|
||||
const double& wx = axis.x(), wy = axis.y(), wz = axis.z();
|
||||
|
||||
const double costheta = cos(theta), sintheta = sin(theta), s2 = sin(theta/2.0), c_1 = 2.0*s2*s2;
|
||||
const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta,
|
||||
wz_sintheta = wz * sintheta;
|
||||
|
||||
const double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
|
||||
const double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz;
|
||||
const double C22 = c_1 * wz * wz;
|
||||
|
||||
Matrix3 R;
|
||||
R(0, 0) = costheta + C00;
|
||||
R(1, 0) = wz_sintheta + C01;
|
||||
R(2, 0) = -wy_sintheta + C02;
|
||||
R(0, 1) = -wz_sintheta + C01;
|
||||
R(1, 1) = costheta + C11;
|
||||
R(2, 1) = wx_sintheta + C12;
|
||||
R(0, 2) = wy_sintheta + C02;
|
||||
R(1, 2) = -wx_sintheta + C12;
|
||||
R(2, 2) = costheta + C22;
|
||||
return R;
|
||||
} else {
|
||||
return FirstOrder(axis*theta);
|
||||
}
|
||||
|
||||
return ExpmapImpl(axis*theta)();
|
||||
}
|
||||
|
||||
/// simply convert omega to axis/angle representation
|
||||
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
|
||||
if (H) *H = ExpmapDerivative(omega);
|
||||
ExpmapImpl impl(omega);
|
||||
if (H) *H = impl.dexp();
|
||||
return impl();
|
||||
}
|
||||
|
||||
double theta2 = omega.dot(omega);
|
||||
if (theta2 > std::numeric_limits<double>::epsilon()) {
|
||||
double theta = std::sqrt(theta2);
|
||||
return AxisAngle(omega / theta, theta);
|
||||
} else {
|
||||
return FirstOrder(omega);
|
||||
}
|
||||
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
|
||||
return ExpmapImpl(omega).dexp();
|
||||
}
|
||||
|
||||
Vector3 SO3::ApplyExpmapDerivative(const Vector3& omega, const Vector3& v,
|
||||
OptionalJacobian<3, 3> H1,
|
||||
OptionalJacobian<3, 3> H2) {
|
||||
return ExpmapImpl(omega).applyDexp(v, H1, H2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -129,44 +160,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
|
||||
using std::sin;
|
||||
|
||||
const double theta2 = omega.dot(omega);
|
||||
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
|
||||
const double theta = std::sqrt(theta2); // rotation angle
|
||||
#ifdef DUY_VERSION
|
||||
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
|
||||
Matrix3 X = skewSymmetric(omega);
|
||||
Matrix3 X2 = X*X;
|
||||
double vi = theta/2.0;
|
||||
double s1 = sin(vi)/vi;
|
||||
double s2 = (theta - sin(theta))/(theta*theta*theta);
|
||||
return I_3x3 - 0.5*s1*s1*X + s2*X2;
|
||||
#else // Luca's version
|
||||
/**
|
||||
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
|
||||
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
|
||||
* expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
|
||||
* where Jr = ExpmapDerivative(thetahat);
|
||||
* This maps a perturbation in the tangent space (omega) to
|
||||
* a perturbation on the manifold (expmap(Jr * omega))
|
||||
*/
|
||||
// element of Lie algebra so(3): X = omega^, normalized by normx
|
||||
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
|
||||
Matrix3 Y;
|
||||
Y << 0.0, -wz, +wy,
|
||||
+wz, 0.0, -wx,
|
||||
-wy, +wx, 0.0;
|
||||
const double s2 = sin(theta / 2.0);
|
||||
const double a = (2.0 * s2 * s2 / theta2);
|
||||
const double b = (1.0 - sin(theta) / theta) / theta2;
|
||||
return I_3x3 - a * Y + b * Y * Y;
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
|
||||
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
|
|
|
|||
|
|
@ -13,6 +13,8 @@
|
|||
* @file SO3.h
|
||||
* @brief 3*3 matrix representation of SO(3)
|
||||
* @author Frank Dellaert
|
||||
* @author Luca Carlone
|
||||
* @author Duy Nguyen Ta
|
||||
* @date December 2014
|
||||
*/
|
||||
|
||||
|
|
@ -97,15 +99,20 @@ public:
|
|||
*/
|
||||
static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
|
||||
|
||||
/// Derivative of Expmap
|
||||
static Matrix3 ExpmapDerivative(const Vector3& omega);
|
||||
|
||||
/// Implement ExpmapDerivative(omega) * v, with derivatives
|
||||
static Vector3 ApplyExpmapDerivative(const Vector3& omega, const Vector3& v,
|
||||
OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none);
|
||||
|
||||
/**
|
||||
* Log map at identity - returns the canonical coordinates
|
||||
* \f$ [R_x,R_y,R_z] \f$ of this rotation
|
||||
*/
|
||||
static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
|
||||
|
||||
/// Derivative of Expmap
|
||||
static Matrix3 ExpmapDerivative(const Vector3& omega);
|
||||
|
||||
/// Derivative of Logmap
|
||||
static Matrix3 LogmapDerivative(const Vector3& omega);
|
||||
|
||||
|
|
|
|||
|
|
@ -243,129 +243,6 @@ TEST(Rot3, retract_localCoordinates2)
|
|||
Vector d21 = t2.localCoordinates(t1);
|
||||
EXPECT(assert_equal(t1, t2.retract(d21)));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
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 Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
|
||||
}
|
||||
|
||||
TEST( Rot3, ExpmapDerivative) {
|
||||
using exmap_derivative::w;
|
||||
const Matrix actualDexpL = Rot3::ExpmapDerivative(w);
|
||||
const Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
|
||||
Vector3::Zero(), 1e-2);
|
||||
EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7));
|
||||
|
||||
const Matrix actualDexpInvL = Rot3::LogmapDerivative(w);
|
||||
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, ExpmapDerivative2)
|
||||
{
|
||||
const Vector3 theta(0.1, 0, 0.1);
|
||||
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
|
||||
boost::bind(&Rot3::Expmap, _1, boost::none), theta);
|
||||
|
||||
CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
|
||||
CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, ExpmapDerivative3)
|
||||
{
|
||||
const Vector3 theta(10, 20, 30);
|
||||
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
|
||||
boost::bind(&Rot3::Expmap, _1, boost::none), theta);
|
||||
|
||||
CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
|
||||
CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, 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 Rot3::Expmap(w(t)); };
|
||||
|
||||
for (double t = -2.0; t < 2.0; t += 0.3) {
|
||||
const Matrix expected = numericalDerivative11<Rot3, double>(R, t);
|
||||
const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t);
|
||||
CHECK(assert_equal(expected, actual, 1e-7));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, 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); };
|
||||
|
||||
// Same as above, but define R as mapping local coordinates to neighborhood aroud R0.
|
||||
const Rot3 R0 = Rot3::Rodrigues(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<Rot3, double>(R, t);
|
||||
const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t);
|
||||
CHECK(assert_equal(expected, actual, 1e-7));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, jacobianExpmap )
|
||||
{
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(boost::bind(
|
||||
&Rot3::Expmap, _1, boost::none), thetahat);
|
||||
Matrix3 Jactual;
|
||||
const Rot3 R = Rot3::Expmap(thetahat, Jactual);
|
||||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, LogmapDerivative )
|
||||
{
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const Rot3 R = Rot3::Expmap(thetahat); // some rotation
|
||||
const Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
|
||||
&Rot3::Logmap, _1, boost::none), R);
|
||||
const Matrix3 Jactual = Rot3::LogmapDerivative(thetahat);
|
||||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, JacobianLogmap )
|
||||
{
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const Rot3 R = Rot3::Expmap(thetahat); // some rotation
|
||||
const Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
|
||||
&Rot3::Logmap, _1, boost::none), R);
|
||||
Matrix3 Jactual;
|
||||
Rot3::Logmap(R, Jactual);
|
||||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, manifold_expmap)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
|
@ -24,16 +24,14 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<SO3 >));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<SO3 >));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<SO3 >));
|
||||
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)));
|
||||
}
|
||||
TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); }
|
||||
|
||||
//******************************************************************************
|
||||
SO3 id;
|
||||
|
|
@ -42,46 +40,220 @@ SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
|
|||
SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , Local) {
|
||||
TEST(SO3, Local) {
|
||||
Vector3 expected(0, 0, 0.1);
|
||||
Vector3 actual = traits<SO3>::Local(R1, R2);
|
||||
EXPECT(assert_equal((Vector)expected,actual));
|
||||
EXPECT(assert_equal((Vector)expected, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , Retract) {
|
||||
TEST(SO3, Retract) {
|
||||
Vector3 v(0, 0, 0.1);
|
||||
SO3 actual = traits<SO3>::Retract(R1, v);
|
||||
EXPECT(actual.isApprox(R2));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , Invariants) {
|
||||
EXPECT(check_group_invariants(id,id));
|
||||
EXPECT(check_group_invariants(id,R1));
|
||||
EXPECT(check_group_invariants(R2,id));
|
||||
EXPECT(check_group_invariants(R2,R1));
|
||||
TEST(SO3, Invariants) {
|
||||
EXPECT(check_group_invariants(id, id));
|
||||
EXPECT(check_group_invariants(id, R1));
|
||||
EXPECT(check_group_invariants(R2, id));
|
||||
EXPECT(check_group_invariants(R2, R1));
|
||||
|
||||
EXPECT(check_manifold_invariants(id,id));
|
||||
EXPECT(check_manifold_invariants(id,R1));
|
||||
EXPECT(check_manifold_invariants(R2,id));
|
||||
EXPECT(check_manifold_invariants(R2,R1));
|
||||
EXPECT(check_manifold_invariants(id, id));
|
||||
EXPECT(check_manifold_invariants(id, R1));
|
||||
EXPECT(check_manifold_invariants(R2, id));
|
||||
EXPECT(check_manifold_invariants(R2, R1));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , LieGroupDerivatives) {
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id,R2);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
|
||||
TEST(SO3, LieGroupDerivatives) {
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id, id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id, R2);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2, id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2, R1);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , ChartDerivatives) {
|
||||
CHECK_CHART_DERIVATIVES(id,id);
|
||||
CHECK_CHART_DERIVATIVES(id,R2);
|
||||
CHECK_CHART_DERIVATIVES(R2,id);
|
||||
CHECK_CHART_DERIVATIVES(R2,R1);
|
||||
TEST(SO3, ChartDerivatives) {
|
||||
CHECK_CHART_DERIVATIVES(id, id);
|
||||
CHECK_CHART_DERIVATIVES(id, R2);
|
||||
CHECK_CHART_DERIVATIVES(R2, id);
|
||||
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, ApplyExpmapDerivative1) {
|
||||
Matrix aH1, aH2;
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none);
|
||||
for (Vector3 omega : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) {
|
||||
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) {
|
||||
Matrix3 H = SO3::ExpmapDerivative(omega);
|
||||
Vector3 expected = H * v;
|
||||
EXPECT(assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v)));
|
||||
EXPECT(assert_equal(expected,
|
||||
SO3::ApplyExpmapDerivative(omega, v, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
|
||||
EXPECT(assert_equal(H, aH2));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, ApplyExpmapDerivative2) {
|
||||
Matrix aH1, aH2;
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none);
|
||||
const Vector3 omega(0, 0, 0);
|
||||
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) {
|
||||
Matrix3 H = SO3::ExpmapDerivative(omega);
|
||||
Vector3 expected = H * v;
|
||||
EXPECT(assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v)));
|
||||
EXPECT(
|
||||
assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
|
||||
EXPECT(assert_equal(H, aH2));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, ApplyExpmapDerivative3) {
|
||||
Matrix aH1, aH2;
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none);
|
||||
const Vector3 omega(0.1, 0.2, 0.3), v(0.4, 0.3, 0.2);
|
||||
Matrix3 H = SO3::ExpmapDerivative(omega);
|
||||
Vector3 expected = H * v;
|
||||
EXPECT(assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v)));
|
||||
EXPECT(
|
||||
assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
|
||||
EXPECT(assert_equal(H, aH2));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -90,4 +262,3 @@ int main() {
|
|||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue