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

Added comment and sped up both Logmap (by a little) and localCoordinates (by more, esp. in combination with unrotate improvement)

release/4.3a0
Frank Dellaert 2012-01-07 21:12:40 +00:00
parent 164ff7ecd8
commit 651259e26b
1 changed files with 9 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/geometry/Pose3.cpp
View File

@ -14,10 +14,11 @@
* @brief 3D Pose * @brief 3D Pose
*/ */
#include <iostream>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Lie-inl.h> #include <gtsam/base/Lie-inl.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/concepts.h> #include <gtsam/geometry/concepts.h>
#include <iostream>
#include <cmath>
using namespace std; using namespace std;
@ -85,16 +86,15 @@ namespace gtsam {
return concatVectors(2, &w, &T); return concatVectors(2, &w, &T);
else { else {
Matrix W = skewSymmetric(w/t); Matrix W = skewSymmetric(w/t);
Matrix Ainv = I3 - (0.5*t)*W + ((2*sin(t)-t*(1+cos(t)))/(2*sin(t))) * (W * W); // Formula from Agrawal06iros, equation (14)
Vector u = Ainv*T; // simplified with Mathematica, and multiplying in T to avoid matrix math
double Tan = tan(0.5*t);
Vector WT = W*T;
Vector u = T - (0.5*t)*WT + (1 - t/(2.*Tan)) * (W * WT);
return concatVectors(2, &w, &u); return concatVectors(2, &w, &u);
} }
} }
/* ************************************************************************* */
// Changes default to use the full verions of expmap/logmap
/* ************************************************************************* */
/* ************************************************************************* */ /* ************************************************************************* */
// Different versions of retract // Different versions of retract
Pose3 Pose3::retract(const Vector& xi) const { Pose3 Pose3::retract(const Vector& xi) const {
@ -138,11 +138,10 @@ namespace gtsam {
// Correct first order t inverse // Correct first order t inverse
Point3 d = R_.unrotate(T.translation() - t_); Point3 d = R_.unrotate(T.translation() - t_);
Vector v = d.vector();
// TODO: correct second order t inverse // TODO: correct second order t inverse
return concatVectors(2, &omega, &v); return Vector_(6,omega(0),omega(1),omega(2),d.x(),d.y(),d.z());
#endif #endif
} }