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

Moved expmap and logmap to .cpp, also tested correct "Agrawal06iros" versions, and they are indeed correct (see testPose3).

release/4.3a0
Frank Dellaert 2010-01-10 12:24:31 +00:00
parent ec1b57ed08
commit d5eade62ef
2 changed files with 25 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cpp/Pose3.cpp
View File

@ -26,10 +26,12 @@ namespace gtsam {
return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol);
}
/** Agrawal06iros versions
Pose3 expmap(const Vector& d) {
/* ************************************************************************* */
// From
#ifdef SLOW_BUT_CORRECT_EXPMAP
/** Agrawal06iros versions of expmap and logmap*/
template<> Pose3 expmap(const Vector& d) {
Vector w = vector_range<const Vector>(d, range(0,3));
Vector u = vector_range<const Vector>(d, range(3,6));
double t = norm_2(w);
@ -54,7 +56,24 @@ namespace gtsam {
return concatVectors(2, &w, &u);
}
}
*/
#else
/* incorrect versions for which we know how to compute derivatives */
template<> Pose3 expmap(const Vector& d) {
Vector w = sub(d, 0,3);
Vector u = sub(d, 3,6);
return Pose3(expmap<Rot3> (w), expmap<Point3> (u));
}
// Log map at identity - return the translation and canonical rotation
// coordinates of a pose.
Vector logmap(const Pose3& p) {
const Vector w = logmap(p.rotation()), u = logmap(p.translation());
return concatVectors(2, &w, &u);
}
#endif
/* ************************************************************************* */
Matrix Pose3::matrix() const {

11
cpp/Pose3.h
View File

@ -85,18 +85,11 @@ namespace gtsam {
// Exponential map at identity - create a pose with a translation and
// rotation (in canonical coordinates)
template<> inline Pose3 expmap(const Vector& d) {
Vector w = sub(d, 0,3);
Vector u = sub(d, 3,6);
return Pose3(expmap<Rot3> (w), expmap<Point3> (u));
}
template<> Pose3 expmap(const Vector& d);
// Log map at identity - return the translation and canonical rotation
// coordinates of a pose.
inline Vector logmap(const Pose3& p) {
const Vector w = logmap(p.rotation()), u = logmap(p.translation());
return concatVectors(2, &w, &u);
}
Vector logmap(const Pose3& p);
// todo: these are the "old-style" expmap and logmap about the specified
// pose.