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

Added global functions to Lie to allow for template access to full expmap/logmap

release/4.3a0
Alex Cunningham 2011-02-02 06:31:23 +00:00
parent 8398c8a53d
commit 86c0c06689
5 changed files with 86 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/base/Lie.h
View File

@ -183,4 +183,25 @@ namespace gtsam {
return expm(xhat,K);
}
/**
* function wrappers for full versions of expmap/logmap
* these will default simple types to using the existing expmap/logmap,
* but more complex ones can be specialized to use improved versions
*
* TODO: replace this approach with a naming scheme that doesn't call
* non-expmap operations "expmap" - use same approach, but with "update"
*/
/** unary versions */
template<class T>
T ExpmapFull(const Vector& xi) { return T::Expmap(xi); }
template<class T>
Vector LogmapFull(const T& p) { return T::Logmap(p); }
/** binary versions */
template<class T>
T expmapFull(const T& t, const Vector& v) { return t.expmap(v); }
template<class T>
Vector logmapFull(const T& t, const T& p2) { return t.logmap(p2); }
} // namespace gtsam

18
gtsam/geometry/Pose2.h
View File

@ -222,5 +222,23 @@ namespace gtsam {
typedef std::pair<Point2,Point2> Point2Pair;
boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
/**
* Specializations for access to full expmap/logmap in templated functions
*
* NOTE: apparently, these *must* be indicated as inline to prevent compile error
*/
/** unary versions */
template<>
inline Pose2 ExpmapFull<Pose2>(const Vector& xi) { return Pose2::ExpmapFull(xi); }
template<>
inline Vector LogmapFull<Pose2>(const Pose2& p) { return Pose2::LogmapFull(p); }
/** binary versions */
template<>
inline Pose2 expmapFull<Pose2>(const Pose2& t, const Vector& v) { return t.expmapFull(v); }
template<>
inline Vector logmapFull<Pose2>(const Pose2& t, const Pose2& p2) { return t.logmapFull(p2); }
} // namespace gtsam

18
gtsam/geometry/Pose3.h
View File

@ -195,4 +195,22 @@ namespace gtsam {
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
}
/**
* Specializations for access to full expmap/logmap in templated functions
*
* NOTE: apparently, these *must* be indicated as inline to prevent compile error
*/
/** unary versions */
template<>
inline Pose3 ExpmapFull<Pose3>(const Vector& xi) { return Pose3::ExpmapFull(xi); }
template<>
inline Vector LogmapFull<Pose3>(const Pose3& p) { return Pose3::LogmapFull(p); }
/** binary versions */
template<>
inline Pose3 expmapFull<Pose3>(const Pose3& t, const Vector& v) { return t.expmapFull(v); }
template<>
inline Vector logmapFull<Pose3>(const Pose3& t, const Pose3& p2) { return t.logmapFull(p2); }
} // namespace gtsam

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

@ -77,6 +77,14 @@ TEST(Pose2, expmap_full) {
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
TEST(Pose2, expmap_full2) {
Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = expmapFull<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
TEST(Pose2, expmap2) {
// do an actual series exponential map
@ -113,6 +121,14 @@ TEST(Pose2, expmap0_full) {
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
TEST(Pose2, expmap0_full2) {
Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(1.01491, 2.01013, 1.5888);
Pose2 actual = pose * ExpmapFull<Pose2>(Vector_(3, 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5));
}
#ifdef SLOW_BUT_CORRECT_EXPMAP
/* ************************************************************************* */
// test case for screw motion in the plane

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

@ -63,11 +63,23 @@ TEST( Pose3, expmap_a_full)
Vector v(6);
fill(v.begin(), v.end(), 0);
v(0) = 0.3;
CHECK(assert_equal(id.expmap(v), Pose3(R, Point3())));
CHECK(assert_equal(id.expmapFull(v), Pose3(R, Point3())));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
CHECK(assert_equal(Pose3(R, P),id.expmapFull(v),1e-5));
}
/* ************************************************************************* */
TEST( Pose3, expmap_a_full2)
{
Pose3 id;
Vector v(6);
fill(v.begin(), v.end(), 0);
v(0) = 0.3;
CHECK(assert_equal(expmapFull<Pose3>(id,v), Pose3(R, Point3())));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
CHECK(assert_equal(Pose3(R, P),expmapFull<Pose3>(id,v),1e-5));
}
/* ************************************************************************* */
TEST(Pose3, expmap_b)
{