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

New tests and explanation of ExmapDerivative

release/4.3a0
Frank Dellaert 2015-12-19 12:47:43 -08:00
parent c2b024055d
commit eb99d4c974
2 changed files with 89 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
gtsam/geometry/tests/testPose3.cpp
View File

@ -715,7 +715,41 @@ TEST( Pose3, ExpmapDerivative1) {
}
/* ************************************************************************* */
TEST( Pose3, LogmapDerivative1) {
TEST(Pose3, ExpmapDerivative2) {
// 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): T -> se(3) is a trajectory in the tangent space of SE(3)
// Hence, the above matrix equation is typed: 4*4 = SE(3) * linear_map(4*4)
// In GTSAM, we don't work with the Lie-algebra elements A directly, but with 6-vectors.
// xi is easy: d Expmap(xi(t)) / dt = ExmapDerivative[xi(t)] * xi'(t)
// Let's verify the above formula.
auto xi = [](double t) {
Vector6 v;
v << 2 * t, sin(t), 4 * t * t, 2 * t, sin(t), 4 * t * t;
return v;
};
auto xi_dot = [](double t) {
Vector6 v;
v << 2, cos(t), 8 * t, 2, cos(t), 8 * t;
return v;
};
// We define a function T
auto T = [xi](double t) { return Pose3::Expmap(xi(t)); };
for (double t = -2.0; t < 2.0; t += 0.3) {
const Matrix expected = numericalDerivative11<Pose3, double>(T, t);
const Matrix actual = Pose3::ExpmapDerivative(xi(t)) * xi_dot(t);
CHECK(assert_equal(expected, actual, 1e-7));
}
}
/* ************************************************************************* */
TEST( Pose3, LogmapDerivative) {
Matrix6 actualH;
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3 p = Pose3::Expmap(w);

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

@ -244,44 +244,74 @@ TEST(Rot3, retract_localCoordinates2)
EXPECT(assert_equal(t1, t2.retract(d21)));
}
/* ************************************************************************* */
Vector w = Vector3(0.1, 0.27, -0.2);
// Left trivialization Derivative of exp(w) wrpt w:
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) {
Matrix actualDexpL = Rot3::ExpmapDerivative(w);
Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
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));
Matrix actualDexpInvL = Rot3::LogmapDerivative(w);
const Matrix actualDexpInvL = Rot3::LogmapDerivative(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7));
}
/* ************************************************************************* */
TEST( Rot3, ExpmapDerivative2)
{
const Vector3 thetahat(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
const Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual));
const Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual2));
}
/* ************************************************************************* */
Vector3 thetahat(0.1, 0, 0.1);
TEST( Rot3, ExpmapDerivative2)
{
Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
TEST(Rot3, ExpmapDerivative3) {
// 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)
// Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3)
Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual));
// 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)
Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat);
CHECK(assert_equal(Jexpected, Jactual2));
// 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, jacobianExpmap )
{
Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(boost::bind(
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);
@ -291,18 +321,20 @@ TEST( Rot3, jacobianExpmap )
/* ************************************************************************* */
TEST( Rot3, LogmapDerivative )
{
Rot3 R = Rot3::Expmap(thetahat); // some rotation
Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
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::LogmapDerivative(thetahat);
const Matrix3 Jactual = Rot3::LogmapDerivative(thetahat);
EXPECT(assert_equal(Jexpected, Jactual));
}
/* ************************************************************************* */
TEST( Rot3, jacobianLogmap )
TEST( Rot3, JacobianLogmap )
{
Rot3 R = Rot3::Expmap(thetahat); // some rotation
Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
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);