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

Merge remote-tracking branch 'origin/feature/SO3_refactor' into feature/ImuFactorPush2

release/4.3a0
Frank Dellaert 2016-02-01 08:44:02 -08:00
parent 8f0622c834 85fbde030b
commit 01ed8810a0
4 changed files with 337 additions and 264 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

232
gtsam/geometry/SO3.cpp
View File

@ -11,8 +11,10 @@
/** /**
* @file SO3.cpp * @file SO3.cpp
* @brief 3*3 matrix representation o SO(3) * @brief 3*3 matrix representation of SO(3)
* @author Frank Dellaert * @author Frank Dellaert
* @author Luca Carlone
* @author Duy Nguyen Ta
* @date December 2014 * @date December 2014
*/ */
@ -24,65 +26,127 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Near zero, we just have I + skew(omega) // Functor implementing Exponential map
static SO3 FirstOrder(const Vector3& omega) { struct ExpmapImpl {
Matrix3 R; const double theta2;
R(0, 0) = 1.; Matrix3 W, K, KK;
R(1, 0) = omega.z(); bool nearZero;
R(2, 0) = -omega.y(); double theta, sin_theta, one_minus_cos; // only defined if !nearZero
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;
}
void init() {
nearZero = (theta2 <= std::numeric_limits<double>::epsilon());
if (nearZero) return;
theta = std::sqrt(theta2); // rotation angle
sin_theta = std::sin(theta);
const double s2 = std::sin(theta / 2.0);
one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
}
// Constructor with element of Lie algebra so(3)
ExpmapImpl(const Vector3& 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;
init();
if (!nearZero) {
theta = std::sqrt(theta2);
K = W / theta;
KK = K * K;
}
}
// Constructor with axis-angle
ExpmapImpl(const Vector3& axis, double angle) : theta2(angle * angle) {
const double ax = axis.x(), ay = axis.y(), az = axis.z();
K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
W = K * angle;
init();
if (!nearZero) {
theta = angle;
KK = K * K;
}
}
// Rodrgues formula
SO3 expmap() const {
if (nearZero)
return I_3x3 + W;
else
return I_3x3 + sin_theta * K + one_minus_cos * K * K;
}
};
/* ************************************************************************* */
SO3 SO3::AxisAngle(const Vector3& axis, double theta) { SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
if (theta*theta > std::numeric_limits<double>::epsilon()) { return ExpmapImpl(axis, theta).expmap();
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);
}
} }
/// simply convert omega to axis/angle representation /* ************************************************************************* */
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { // Functor that implements Exponential map *and* its derivatives
if (H) *H = ExpmapDerivative(omega); struct DexpImpl : ExpmapImpl {
const Vector3 omega;
double a, b; // constants used in dexp and applyDexp
double theta2 = omega.dot(omega); // Constructor with element of Lie algebra so(3)
if (theta2 > std::numeric_limits<double>::epsilon()) { DexpImpl(const Vector3& omega) : ExpmapImpl(omega), omega(omega) {
double theta = std::sqrt(theta2); if (nearZero) return;
return AxisAngle(omega / theta, theta); a = one_minus_cos / theta;
} else { b = 1.0 - sin_theta / theta;
return FirstOrder(omega);
} }
// 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
return I_3x3 - a * K + b * KK;
}
// Just multiplies with dexp()
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
if (nearZero) {
if (H1) *H1 = 0.5 * skewSymmetric(v);
if (H2) *H2 = I_3x3;
return v - 0.5 * omega.cross(v);
}
const Vector3 Kv = omega.cross(v / theta);
const Vector3 KKv = omega.cross(Kv / theta);
if (H1) {
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
const Matrix3 T = skewSymmetric(v / theta);
const double Da = (sin_theta - 2.0 * a) / theta2;
const double Db = (one_minus_cos - 3.0 * b) / theta2;
*H1 = (-Da * Kv + Db * KKv) * omega.transpose() + a * T -
skewSymmetric(Kv * b / theta) - b * K * T;
}
if (H2) *H2 = dexp();
return v - a * Kv + b * KKv;
}
};
/* ************************************************************************* */
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) {
DexpImpl impl(omega);
*H = impl.dexp();
return impl.expmap();
} else
return ExpmapImpl(omega).expmap();
}
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
return DexpImpl(omega).dexp();
}
Vector3 SO3::ApplyExpmapDerivative(const Vector3& omega, const Vector3& v,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) {
return DexpImpl(omega).applyDexp(v, H1, H2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -96,7 +160,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
// Get trace(R) // Get trace(R)
double tr = R.trace(); const double tr = R.trace();
Vector3 omega; Vector3 omega;
@ -112,7 +176,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else { } else {
double magnitude; double magnitude;
double tr_3 = tr - 3.0; // always negative const double tr_3 = tr - 3.0; // always negative
if (tr_3 < -1e-7) { if (tr_3 < -1e-7) {
double theta = acos((tr - 1.0) / 2.0); double theta = acos((tr - 1.0) / 2.0);
magnitude = theta / (2.0 * sin(theta)); magnitude = theta / (2.0 * sin(theta));
@ -129,58 +193,13 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { Matrix3 SO3::LogmapDerivative(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) {
using std::cos; using std::cos;
using std::sin; using std::sin;
double theta2 = omega.dot(omega); double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3; if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle double theta = std::sqrt(theta2); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B11, 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 s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
return I_3x3 + 0.5*X - s2*X2;
#else // Luca's version
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
@ -188,11 +207,10 @@ Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
* This maps a perturbation on the manifold (expmap(omega)) * This maps a perturbation on the manifold (expmap(omega))
* to a perturbation in the tangent space (Jrinv * omega) * to a perturbation in the tangent space (Jrinv * omega)
*/ */
const Matrix3 X = skewSymmetric(omega); // element of Lie algebra so(3): X = omega^ const Matrix3 W = skewSymmetric(omega); // element of Lie algebra so(3): W = omega^
return I_3x3 + 0.5 * X return I_3x3 + 0.5 * W +
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
* X; W * W;
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */

13
gtsam/geometry/SO3.h
View File

@ -13,6 +13,8 @@
* @file SO3.h * @file SO3.h
* @brief 3*3 matrix representation of SO(3) * @brief 3*3 matrix representation of SO(3)
* @author Frank Dellaert * @author Frank Dellaert
* @author Luca Carlone
* @author Duy Nguyen Ta
* @date December 2014 * @date December 2014
*/ */
@ -97,15 +99,20 @@ public:
*/ */
static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none); 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 * Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation * \f$ [R_x,R_y,R_z] \f$ of this rotation
*/ */
static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
/// Derivative of Expmap
static Matrix3 ExpmapDerivative(const Vector3& omega);
/// Derivative of Logmap /// Derivative of Logmap
static Matrix3 LogmapDerivative(const Vector3& omega); static Matrix3 LogmapDerivative(const Vector3& omega);

123
gtsam/geometry/tests/testRot3.cpp
View File

@ -243,129 +243,6 @@ TEST(Rot3, retract_localCoordinates2)
Vector d21 = t2.localCoordinates(t1); Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21))); 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) TEST(Rot3, manifold_expmap)
{ {

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

@ -24,16 +24,14 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
//****************************************************************************** //******************************************************************************
TEST(SO3 , Concept) { TEST(SO3, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SO3 >)); BOOST_CONCEPT_ASSERT((IsGroup<SO3>));
BOOST_CONCEPT_ASSERT((IsManifold<SO3 >)); BOOST_CONCEPT_ASSERT((IsManifold<SO3>));
BOOST_CONCEPT_ASSERT((IsLieGroup<SO3 >)); BOOST_CONCEPT_ASSERT((IsLieGroup<SO3>));
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , Constructor) { TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); }
SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
}
//****************************************************************************** //******************************************************************************
SO3 id; SO3 id;
@ -42,46 +40,220 @@ SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
//****************************************************************************** //******************************************************************************
TEST(SO3 , Local) { TEST(SO3, Local) {
Vector3 expected(0, 0, 0.1); Vector3 expected(0, 0, 0.1);
Vector3 actual = traits<SO3>::Local(R1, R2); 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); Vector3 v(0, 0, 0.1);
SO3 actual = traits<SO3>::Retract(R1, v); SO3 actual = traits<SO3>::Retract(R1, v);
EXPECT(actual.isApprox(R2)); EXPECT(actual.isApprox(R2));
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , Invariants) { TEST(SO3, Invariants) {
EXPECT(check_group_invariants(id,id)); EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id,R1)); EXPECT(check_group_invariants(id, R1));
EXPECT(check_group_invariants(R2,id)); EXPECT(check_group_invariants(R2, id));
EXPECT(check_group_invariants(R2,R1)); EXPECT(check_group_invariants(R2, R1));
EXPECT(check_manifold_invariants(id,id)); EXPECT(check_manifold_invariants(id, id));
EXPECT(check_manifold_invariants(id,R1)); EXPECT(check_manifold_invariants(id, R1));
EXPECT(check_manifold_invariants(R2,id)); EXPECT(check_manifold_invariants(R2, id));
EXPECT(check_manifold_invariants(R2,R1)); EXPECT(check_manifold_invariants(R2, R1));
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , LieGroupDerivatives) { TEST(SO3, LieGroupDerivatives) {
CHECK_LIE_GROUP_DERIVATIVES(id,id); CHECK_LIE_GROUP_DERIVATIVES(id, id);
CHECK_LIE_GROUP_DERIVATIVES(id,R2); CHECK_LIE_GROUP_DERIVATIVES(id, R2);
CHECK_LIE_GROUP_DERIVATIVES(R2,id); CHECK_LIE_GROUP_DERIVATIVES(R2, id);
CHECK_LIE_GROUP_DERIVATIVES(R2,R1); CHECK_LIE_GROUP_DERIVATIVES(R2, R1);
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , ChartDerivatives) { TEST(SO3, ChartDerivatives) {
CHECK_CHART_DERIVATIVES(id,id); CHECK_CHART_DERIVATIVES(id, id);
CHECK_CHART_DERIVATIVES(id,R2); CHECK_CHART_DERIVATIVES(id, R2);
CHECK_CHART_DERIVATIVES(R2,id); CHECK_CHART_DERIVATIVES(R2, id);
CHECK_CHART_DERIVATIVES(R2,R1); 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); return TestRegistry::runAllTests(tr);
} }
//****************************************************************************** //******************************************************************************