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

small changes so that SLOW_BUT_CORRECT_EXPMAP compiles

release/4.3a0
Alex Cunningham 2010-09-17 14:53:09 +00:00
parent fdae3d6f02
commit 14cb0be4af
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
geometry/Pose2.cpp
View File

@ -39,7 +39,7 @@ namespace gtsam {
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifdef SLOW_BUT_CORRECT_EXPMAP
template<> Pose2 expmap(const Vector& xi) { Pose2 Pose2::Expmap(const Vector& xi) {
Point2 v(xi(0),xi(1)); Point2 v(xi(0),xi(1));
double w = xi(2); double w = xi(2);
if (fabs(w) < 1e-5) if (fabs(w) < 1e-5)
@ -52,7 +52,7 @@ namespace gtsam {
} }
} }
Vector logmap(const Pose2& p) { Vector Pose2::Logmap(const Pose2& p) {
const Rot2& R = p.r(); const Rot2& R = p.r();
const Point2& t = p.t(); const Point2& t = p.t();
double w = R.theta(); double w = R.theta();

2
geometry/tests/testPose2.cpp
View File

@ -105,7 +105,7 @@ TEST(Pose3, expmap_c)
{ {
EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6)); EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6)); EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6));
EXPECT(assert_equal(screw::xi, logmap(screw::expected),1e-6)); EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6));
} }
#endif #endif