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

minor code optimization in ExpmapDerivative

release/4.3a0
Luca 2014-12-10 16:56:21 -05:00
parent b3f0f3877c
commit 36358308fd
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Rot3.cpp
View File

@ -183,9 +183,9 @@ Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
Jr = I_3x3;
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jr = I_3x3 - ((1-cos(normx))/(normx*normx)) * X +
((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
const Matrix3 X = skewSymmetric(x) / normx; // element of Lie algebra so(3): X = x^, normalized by normx
Jr = I_3x3 - ((1-cos(normx))/(normx)) * X +
(1 -sin(normx)/normx) * X * X; // right Jacobian
}
return Jr;
}