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

fix bug on computation of SO(3) logmap 

when theta near 0 (or 2pi, 4pi...)
release/4.3a0
Tim McGrath 2021-06-01 09:38:42 -04:00 committed by GitHub
parent d5dbaf7ce7
commit 4ef43387bd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/geometry/SO3.cpp
View File

@ -278,7 +278,8 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
} else { } else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3 * tr_3 / 12.0; // see https://github.com/borglab/gtsam/issues/746 for details
magnitude = 0.5 - tr_3 / 12.0;
} }
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
} }