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

return identity

release/4.3a0
Frank Dellaert 2010-01-12 02:09:43 +00:00
parent 5163684077
commit 4b459c45c3
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/Pose3.cpp
View File

@ -36,7 +36,7 @@ namespace gtsam {
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);
if (t < 1e-5) if (t < 1e-5)
return Pose3(expmap<Rot3> (w), expmap<Point3> (u)); return Pose3(Rot3(), expmap<Point3> (u));
else { else {
Matrix W = skewSymmetric(w/t); Matrix W = skewSymmetric(w/t);
Matrix A = eye(3, 3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); Matrix A = eye(3, 3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);