[37] | 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
|
---|