1 | function q = vgg_quat_from_rotation_matrix( R )
|
---|
2 | % vgg_quat_from_rotation_matrix Generates quaternion from rotation matrix
|
---|
3 | % q = vgg_quat_from_rotation_matrix(R)
|
---|
4 |
|
---|
5 | q = [ (1 + R(1,1) + R(2,2) + R(3,3))
|
---|
6 | (1 + R(1,1) - R(2,2) - R(3,3))
|
---|
7 | (1 - R(1,1) + R(2,2) - R(3,3))
|
---|
8 | (1 - R(1,1) - R(2,2) + R(3,3)) ];
|
---|
9 |
|
---|
10 | if ~issym(q)
|
---|
11 | % Pivot to avoid division by small numbers
|
---|
12 | [b I] = max(abs(q));
|
---|
13 | else
|
---|
14 | % For symbolic quats, just make sure we're nonzero
|
---|
15 | for k=1:4
|
---|
16 | if q(k) ~= 0
|
---|
17 | I = k;
|
---|
18 | break
|
---|
19 | end
|
---|
20 | end
|
---|
21 | end
|
---|
22 |
|
---|
23 | q(I) = sqrt(q(I)) / 2 ;
|
---|
24 |
|
---|
25 | if I == 1
|
---|
26 | q(2) = (R(3,2) - R(2,3)) / (4*q(I));
|
---|
27 | q(3) = (R(1,3) - R(3,1)) / (4*q(I));
|
---|
28 | q(4) = (R(2,1) - R(1,2)) / (4*q(I));
|
---|
29 | elseif I==2
|
---|
30 | q(1) = (R(3,2) - R(2,3)) / (4*q(I));
|
---|
31 | q(3) = (R(2,1) + R(1,2)) / (4*q(I));
|
---|
32 | q(4) = (R(1,3) + R(3,1)) / (4*q(I));
|
---|
33 | elseif I==3
|
---|
34 | q(1) = (R(1,3) - R(3,1)) / (4*q(I));
|
---|
35 | q(2) = (R(2,1) + R(1,2)) / (4*q(I));
|
---|
36 | q(4) = (R(3,2) + R(2,3)) / (4*q(I));
|
---|
37 | elseif I==4
|
---|
38 | q(1) = (R(2,1) - R(1,2)) / (4*q(I));
|
---|
39 | q(2) = (R(1,3) + R(3,1)) / (4*q(I));
|
---|
40 | q(3) = (R(3,2) + R(2,3)) / (4*q(I));
|
---|
41 | end
|
---|