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

correct Qr coefficients in approximation

release/4.3a0
JIanzhu Huai 2020-09-25 20:26:24 +08:00
parent 52fc9cf4ba
commit 2550bf76a4
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Pose3.cpp
View File

@ -222,8 +222,8 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThr
} }
else { else {
Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W)
+ 1./24.*(W*W*V + V*W*W - 3*W*V*W) - 1./24.*(W*W*V + V*W*W - 3*W*V*W)
- 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); + 1./120.*(W*V*W*W + W*W*V*W);
} }
#endif #endif