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); 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 w = vector_range<const Vector>(d, range(0,3));
Vector u = vector_range<const Vector>(d, range(3,6)); Vector u = vector_range<const Vector>(d, range(3,6));
double t = norm_2(w); double t = norm_2(w);
@ -54,7 +56,24 @@ namespace gtsam {
return concatVectors(2, &w, &u); 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 { 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 // Exponential map at identity - create a pose with a translation and
// rotation (in canonical coordinates) // rotation (in canonical coordinates)
template<> inline Pose3 expmap(const Vector& d) { 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 // Log map at identity - return the translation and canonical rotation
// coordinates of a pose. // coordinates of a pose.
inline Vector logmap(const Pose3& p) { Vector logmap(const Pose3& p);
const Vector w = logmap(p.rotation()), u = logmap(p.translation());
return concatVectors(2, &w, &u);
}
// todo: these are the "old-style" expmap and logmap about the specified // todo: these are the "old-style" expmap and logmap about the specified
// pose. // pose.