ApproxMethod.cpp
Go to the documentation of this file.
1 #include "methods.h"
2 #include "util.h"
3 
12 {
13  Matrix4 U,U2;
14  Vector4 s,q1,q2,q3,q4,q;
15  MatrixXf::Index index;
16 
17  /* Equivalent Matlab code:
18  U = [1+R(1,1)+R(2,2)+R(3,3) R(3,2)-R(2,3) R(1,3)-R(3,1) R(2,1)-R(1,2) ;
19  R(3,2)-R(2,3) 1+R(1,1)-R(2,2)-R(3,3) R(1,2)+R(2,1) R(3,1)+R(1,3) ;
20  R(1,3)-R(3,1) R(1,2)+R(2,1) 1-R(1,1)+R(2,2)-R(3,3) R(2,3)+R(3,2) ;
21  R(2,1)-R(1,2) R(3,1)+R(1,3) R(2,3)+R(3,2) 1-R(1,1)-R(2,2)+R(3,3)];
22  */
23  U(0,0) = 1+R(0,0)+R(1,1)+R(2,2);
24  U(0,1) = R(2,1)-R(1,2);
25  U(0,2) = R(0,2)-R(2,0);
26  U(0,3) = R(1,0)-R(0,1);
27 
28  U(1,0) = U(0,1);
29  U(1,1) = 1+R(0,0)-R(1,1)-R(2,2);
30  U(1,2) = R(0,1)+R(1,0);
31  U(1,3) = R(2,0)+R(0,2);
32 
33  U(2,0) = U(0,2);
34  U(2,1) = U(1,2);
35  U(2,2) = 1-R(0,0)+R(1,1)-R(2,2);
36  U(2,3) = R(1,2)+R(2,1);
37 
38  U(3,0) = U(0,3);
39  U(3,1) = U(1,3);
40  U(3,2) = U(2,3);
41  U(3,3) = 1-R(0,0)-R(1,1)+R(2,2);
42 
43  /* Equivalent Matlab code:
44 
45  [~,index] = max(sum(U.^2,2));
46  */
47  U2=(U.array()*U.array());
48  s=U2.rowwise().sum();
49  s.maxCoeff(&index);
50 
51  /* Equivalent Matlab code:
52 
53  q1 = sign(dot(U(index,:),U(1,:)))*U(1,:);
54  q2 = sign(dot(U(index,:),U(2,:)))*U(2,:);
55  q3 = sign(dot(U(index,:),U(3,:)))*U(3,:);
56  q4 = sign(dot(U(index,:),U(4,:)))*U(4,:);
57  */
58  q1 = sign(U.row(index).dot(U.row(0)))*U.row(0);
59  q2 = sign(U.row(index).dot(U.row(1)))*U.row(1);
60  q3 = sign(U.row(index).dot(U.row(2)))*U.row(2);
61  q4 = sign(U.row(index).dot(U.row(3)))*U.row(3);
62 
63  /* Equivalent Matlab code:
64 
65  X = Quat2Mat(q1+q2+q3+q4);
66  */
67  q = q1+q2+q3+q4;
68  Quat2Mat(X,q);
69 }