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

Now argument of LogmapDerivative is correct

release/4.3a0
dellaert 2014-12-25 16:22:21 +01:00
parent 1b61d0a7f3
commit 4635d22d7f
3 changed files with 32 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/Pose2.cpp
View File

@ -76,7 +76,7 @@ Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) {
/* ************************************************************************* */
Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
if (H) *H = Pose2::LogmapDerivative(p);
const Rot2& R = p.r();
const Point2& t = p.t();
double w = R.theta();
@ -164,7 +164,8 @@ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) {
}
/* ************************************************************************* */
Matrix3 Pose2::LogmapDerivative(const Vector3& v) {
Matrix3 Pose2::LogmapDerivative(const Pose2& p) {
Vector3 v = Logmap(p);
double alpha = v[2];
if (fabs(alpha) > 1e-5) {
double alphaInv = 1/alpha;

2
gtsam/geometry/Pose2.h
View File

@ -157,7 +157,7 @@ public:
static Matrix3 ExpmapDerivative(const Vector3& v);
/// Left-trivialized derivative inverse of the exponential map
static Matrix3 LogmapDerivative(const Vector3& v);
static Matrix3 LogmapDerivative(const Pose2& v);
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
struct ChartAtOrigin {

65
gtsam/geometry/tests/testPose2.cpp
View File

@ -198,53 +198,44 @@ TEST(Pose2, logmap_full) {
}
/* ************************************************************************* */
Vector3 w(0.1, 0.27, -0.3);
Vector3 w0(0.1, 0.27, 0.0); // alpha = 0
// Left trivialization Derivative of exp(w) over w:
// How exp(w) changes when w changes?
// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector3 testDexpL(const Vector3 w, const Vector3& dw) {
Vector3 y = Pose2::Logmap(Pose2::Expmap(-w) * Pose2::Expmap(w + dw));
return y;
}
TEST( Pose2, ExpmapDerivative) {
Matrix actualDexpL = Pose2::ExpmapDerivative(w);
Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(
boost::bind(testDexpL, w, _1), zero(3), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix actualDexpInvL = Pose2::LogmapDerivative(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
// test case where alpha = 0
Matrix actualDexpL0 = Pose2::ExpmapDerivative(w0);
Matrix expectedDexpL0 = numericalDerivative11<Vector3, Vector3>(
boost::bind(testDexpL, w0, _1), zero(3), 1e-2);
EXPECT(assert_equal(expectedDexpL0, actualDexpL0, 1e-5));
Matrix actualDexpInvL0 = Pose2::LogmapDerivative(w0);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
TEST( Pose2, ExpmapDerivative1) {
Matrix3 actualH;
Vector3 w(0.1, 0.27, -0.3);
Pose2::Expmap(w,actualH);
Matrix3 expectedH = numericalDerivative21<Pose2, Vector3,
OptionalJacobian<3, 3> >(&Pose2::Expmap, w, boost::none, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
/* ************************************************************************* */
TEST( Pose2, ExpmapDerivative2) {
Matrix3 actualH;
Pose2::Expmap(w,actualH);
Matrix3 expectedH = numericalDerivative11<Pose2, Vector3>(
boost::bind(Pose2::Expmap, _1, boost::none), w, 1e-2);
Vector3 w0(0.1, 0.27, 0.0); // alpha = 0
Pose2::Expmap(w0,actualH);
Matrix3 expectedH = numericalDerivative21<Pose2, Vector3,
OptionalJacobian<3, 3> >(&Pose2::Expmap, w0, boost::none, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
/* ************************************************************************* */
TEST( Pose2, LogmapDerivative1) {
Matrix3 actualH;
Vector3 w(0.1, 0.27, -0.3);
Pose2 p = Pose2::Expmap(w);
EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5));
Matrix3 expectedH = numericalDerivative21<Vector3, Pose2,
OptionalJacobian<3, 3> >(&Pose2::Logmap, p, boost::none, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
/* ************************************************************************* */
TEST( Pose2, LogmapDerivative2) {
Pose2 p = Pose2::Expmap(w);
EXPECT(assert_equal(w, Pose2::Logmap(p), 1e-5));
Matrix3 actualH = Pose2::LogmapDerivative(w);
Matrix3 expectedH = numericalDerivative11<Vector3, Pose2>(
boost::bind(Pose2::Logmap, _1, boost::none), p, 1e-2);
Matrix3 actualH;
Vector3 w0(0.1, 0.27, 0.0); // alpha = 0
Pose2 p = Pose2::Expmap(w0);
EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5));
Matrix3 expectedH = numericalDerivative21<Vector3, Pose2,
OptionalJacobian<3, 3> >(&Pose2::Logmap, p, boost::none, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}