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

There was a serious mistake in rodriguez, instead of R was returning R'

release/4.3a0
Alireza Fathi 2009-09-03 17:08:52 +00:00
parent 24c8242782
commit 2357e8aefc
1 changed files with 3 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cpp/Rot3.cpp
View File

@ -23,12 +23,9 @@ namespace gtsam {
double ct = cos(t), st = sin(t);
Point3 r1 = Point3(ct + w(0) * w(0) * (1 - ct), w(1) * w(0) * (1 - ct) - w(
2) * st, w(1) * st + w(2) * w(0) * (1 - ct));
Point3 r2 = Point3(w(2) * st + w(0) * w(1) * (1 - ct), w(1) * w(1) * (1
- ct) + ct, -w(0) * st + w(2) * w(1) * (1 - ct));
Point3 r3 = Point3(-w(1) * st + w(0) * w(2) * (1 - ct), w(1) * w(2) * (1
- ct) + w(0) * st, ct + w(2) * w(2) * (1 - ct));
Point3 r1 = Point3(ct + w(0) * w(0) * (1 - ct), w(2) * st + w(0) * w(1) * (1 - ct), -w(1) * st + w(0) * w(2) * (1 - ct));
Point3 r2 = Point3(w(1) * w(0) * (1 - ct) - w(2) * st, w(1) * w(1) * (1 - ct) + ct, w(1) * w(2) * (1 - ct) + w(0) * st);
Point3 r3 = Point3(w(1) * st + w(2) * w(0) * (1 - ct), -w(0) * st + w(2) * w(1) * (1 - ct), ct + w(2) * w(2) * (1 - ct));
return Rot3(r1, r2, r2);
}