demo_mean_rotation

% DEMO  3D Mean Rotation

 Wolfgang Förstner
 wfoerstn@uni-bonn.de

 last changes: Susanne Wenzel 07/17
 wenzel@igg.uni-bonn.de

0001 %% DEMO  3D Mean Rotation
0002 %
0003 % Wolfgang Förstner
0004 % wfoerstn@uni-bonn.de
0005 %
0006 % last changes: Susanne Wenzel 07/17
0007 % wenzel@igg.uni-bonn.de
0008 
0009 clc
0010 close all
0011 
0012 addpath(genpath('../General-Functions'))
0013 
0014 fprintf('\n------ DEMO  3D Mean Rotation ------\n')
0015 fprintf('\nIn order to shorten the output, we display transposed vectors\n')
0016 
0017 % control of random numbers
0018 sigma = 0.1;
0019 
0020 % generate data
0021 q_true = rand(4,1);
0022 % Given: true 3D rotation
0023 q_true = q_true/norm(q_true);
0024 R_true = calc_Rot_q(q_true);
0025 
0026 fprintf('\nGiven: noisy 3D rotations\n')
0027 fprintf('\nQuaternions\n')
0028 q1 = q_true + rand(4,1) * sigma; disp(['q1 = [', num2str(q1'),']'])
0029 q2 = q_true*sign(rand) + rand(4,1) * sigma ; disp(['q2 = [', num2str(q2'),']'])
0030 fprintf('\nAccording rotation matrices\n')
0031 R1 = calc_Rot_q(q1)                                                        %#ok<NOPTS>
0032 R2 = calc_Rot_q(q2)                                                        %#ok<NOPTS>
0033 
0034 %% Average quaternions
0035 fprintf('\nAverage quaternion\n')
0036 M = q1 * q1' + q1 * q1';
0037 [Ev,lambdas] = eig(M);
0038 q_est = Ev(:,4); disp(['q_est = [', num2str(q_est'),']'])
0039 
0040 fprintf('\nResidual\n')
0041 dq = calc_Mq(q_true)\q_est;
0042 difference_q_qtrue = norm(dq(2:4));
0043 disp(['diff_q = ', num2str(difference_q_qtrue)])
0044 
0045 %% Average rotation matrices
0046 fprintf('\nAverage rotation matrices\n')
0047 R_sum = R1 + R2;
0048 [U,D,V] = svd(R_sum);
0049 R_est = U * V'                                                             %#ok<NOPTS>
0050 
0051 fprintf('\nResiduals\n')
0052 difference_R_Rtrue = norm((R_est*R_true'-eye(3))/2);
0053 disp(['diff_R = ', num2str(difference_R_Rtrue)])
0054 
0055 fprintf('\nComparision: get rotation matrix from estimated quaternion\n')
0056 R_true_from_q = calc_Rot_q(q_est)                                          %#ok<NOPTS>