src/fusion-sensor/rotation_vector/design/lib/rot_mat2quat.m

   1 function q = rot_mat2quat(R)
   2 % rot_mat2quat
   3 %
   4 % Copyright (c) 2014 Samsung Electronics Co., Ltd.
   5 %
   6 % Licensed under the Apache License, Version 2.0 (the "License");
   7 % you may not use this file except in compliance with the License.
   8 % You may obtain a copy of the License at
   9 %
  10 % http://www.apache.org/licenses/LICENSE-2.0
  11 %
  12 % Unless required by applicable law or agreed to in writing, software
  13 % distributed under the License is distributed on an "AS IS" BASIS,
  14 % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15 % See the License for the specific language governing permissions and
  16 % limitations under the License.
  17
  18 % Converts a rotation matrix orientation to a quaternion.
  19
  20     [row col numR] = size(R);
  21     q = zeros(numR, 4);
  22     K = zeros(4,4);
  23     for i = 1:numR
  24         K(1,1) = (1/3) * (R(1,1,i) - R(2,2,i) - R(3,3,i));
  25         K(1,2) = (1/3) * (R(2,1,i) + R(1,2,i));
  26         K(1,3) = (1/3) * (R(3,1,i) + R(1,3,i));
  27         K(1,4) = (1/3) * (R(2,3,i) - R(3,2,i));
  28         K(2,1) = (1/3) * (R(2,1,i) + R(1,2,i));
  29         K(2,2) = (1/3) * (R(2,2,i) - R(1,1,i) - R(3,3,i));
  30         K(2,3) = (1/3) * (R(3,2,i) + R(2,3,i));
  31         K(2,4) = (1/3) * (R(3,1,i) - R(1,3,i));
  32         K(3,1) = (1/3) * (R(3,1,i) + R(1,3,i));
  33         K(3,2) = (1/3) * (R(3,2,i) + R(2,3,i));
  34         K(3,3) = (1/3) * (R(3,3,i) - R(1,1,i) - R(2,2,i));
  35         K(3,4) = (1/3) * (R(1,2,i) - R(2,1,i));
  36         K(4,1) = (1/3) * (R(2,3,i) - R(3,2,i));
  37         K(4,2) = (1/3) * (R(3,1,i) - R(1,3,i));
  38         K(4,3) = (1/3) * (R(1,2,i) - R(2,1,i));
  39         K(4,4) = (1/3) * (R(1,1,i) + R(2,2,i) + R(3,3,i));
  40         [V,D] = eig(K);
  41         q(i,:) = V(:,4)';
  42         q(i,:) = [q(i,4) q(i,1) q(i,2) q(i,3)];
  43     end
  44 end