small changes so that SLOW_BUT_CORRECT_EXPMAP compiles
parent
fdae3d6f02
commit
14cb0be4af
|
@ -39,7 +39,7 @@ namespace gtsam {
|
|||
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
|
||||
template<> Pose2 expmap(const Vector& xi) {
|
||||
Pose2 Pose2::Expmap(const Vector& xi) {
|
||||
Point2 v(xi(0),xi(1));
|
||||
double w = xi(2);
|
||||
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 Point2& t = p.t();
|
||||
double w = R.theta();
|
||||
|
|
|
@ -105,7 +105,7 @@ TEST(Pose3, expmap_c)
|
|||
{
|
||||
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::xi, logmap(screw::expected),1e-6));
|
||||
EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6));
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
Loading…
Reference in New Issue