CS计算机代考程序代写 matlab interpreter %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% NOTES:
% – The implementations follow the notation of Ethan Eade’s tech report,
% which is a concise reference for various relevant Lie groups. Simple
% arithmetic manipulation (using the properties of w_hat discussed in the
% tutorial) shows the equivalence to the formulation on the lecture
% slides. Of course you can also implement those variants.
% – Taylor expansions are used for small theta to avoid numerical issues of
% dividing by something close to 0. This is not strictly needed, but at
% least a check for theta != 0 should be added.
%
% Additional ressources on common Lie Groups used in 3D computer vision:
% – Lie Groups for 2D and 3D Transformations (Ethan Eade)
% – A tutorial on SE(3) transformation parameterizations and on-manifold optimization (Jose-Luis Blanco)

close all;

%% setup

% try large and small vector (to test Taylor expansion for small angles)

w1 = rand(3,1);
v1 = rand(3,1);
w2 = w1/norm(w1) * 1e-6;
v2 = v1/norm(v1) * 1e-6;
xi1 = [v1;w1];
xi2 = [v2;w2];

%% SO(3)

% compute exponential
R1 = Exp_SO3(w1);
R2 = Exp_SO3(w2);

% compute logarithm to get back to the original value
w1_b = Log_SO3(R1);
w2_b = Log_SO3(R2,0);

% compare original w with log(exp(w)) –> compute relative difference
% we expect small values (1e-10 or smaller)
test_exp_log_so3_1 = norm(w1 – w1_b) / norm(w1)
test_exp_log_so3_2 = norm(w2 – w2_b) / norm(w2)

% Compare to Matlab’s generic expm function –> compute relative difference
% we expect small values (1e-10 or smaller)
test_exp_so3_1 = norm(R1 – expm(hat_SO3(w1))) / norm(w1)
test_exp_so3_2 = norm(R2 – expm(hat_SO3(w2))) / norm(w2)
test_log_so3_1 = norm(w1_b – vee_SO3(logm(R1))) / norm(w1)
test_log_so3_2 = norm(w2_b – vee_SO3(logm(R2))) / norm(w2)

%% SE(3)

% compute exponential
T1 = Exp_SE3(xi1);
T2 = Exp_SE3(xi2);

% compute logarithm to get back to the original value
xi1_b = Log_SE3(T1);
xi2_b = Log_SE3(T2);

% compare original w with log(exp(w)) –> compute relative difference
% we expect small values (1e-10 or smaller)
test_exp_log_se3_1 = norm(xi1 – xi1_b) / norm(xi1)
test_exp_log_se3_2 = norm(xi2 – xi2_b) / norm(xi2)

% Compare to Matlab’s generic expm function –> compute relative difference
% we expect small values (1e-10 or smaller)
test_exp_se3_1 = norm(T1 – expm(hat_SE3(xi1))) / norm(xi1)
test_exp_se3_2 = norm(T2 – expm(hat_SE3(xi2))) / norm(xi2)
test_log_se3_1 = norm(xi1_b – vee_SE3(logm(T1))) / norm(xi1)
test_log_se3_2 = norm(xi2_b – vee_SE3(logm(T2))) / norm(xi2)

%% Bonus: Section about epsilon and Taylor expansion
% This explores the effect of using the Taylor expansion for small inputs
% by comparing to expm / logm. This is not a perfect test, since we are not
% certain about the accuracy of expm / logm. It can be seen that the Taylor
% expansion is only valid for small angles (as expected). For exp we can
% also clearly see that for input norms around 1e-4 or smaller the Taylor
% expansion seems to be more consistent with expm. For log, maybe the
% Taylor expansion is less relevant, as both seem to be consistent with
% logm for small values.

figure(1)
test_taylor_exp_so3();
figure(2)
test_taylor_exp_se3();
figure(3)
test_taylor_log_so3();
figure(4)
test_taylor_log_se3();

%% implementation:

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% hat and vee (inverse of hat) operators for SO(3) and SE(3)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function S = hat_SO3(w)
S = [ 0 -w(3) w(2)
w(3) 0 -w(1)
-w(2) w(1) 0 ];
end

function w = vee_SO3(S)
% check S is skew symmetric
assert(norm(S+S’) < 1e-10) w = [S(3,2); S(1,3); S(2,1)]; end function Xi = hat_SE3(xi) v = xi(1:3); w = xi(4:6); Xi = zeros(4); Xi(1:3,1:3) = hat_SO3(w); Xi(1:3,4) = v; end function xi = vee_SE3(Xi) S = Xi(1:3,1:3); v = Xi(1:3,4); % check last row is 0 assert(norm(Xi(4,:)) < 1e-10); % this checks if S is skew symmetric w = vee_SO3(S); xi = [v; w]; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Exponential for so(3) --> SO(3)
% exp[w_hat] = I + sin(|w|)/|w| * w_hat + (1-cos(|w|))/|w| * w_hat^2
% Compare:
% – Lie Groups for 2D and 3D Transformations (Ethan Eade) p4
% – Ex. 3 Part I 3.
% – Lecture Ch. 2, p15
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function R = Exp_SO3(w, epsilon)
if ~exist(‘epsilon’,’var’)
% set default value for epsilon
epsilon = 1e-5;
end
S = hat_SO3(w);
theta_2 = w’*w;
theta = sqrt(theta_2);
% Use Taylor expansion to avoid numerical instabilities for small theta
% At least a check for theta != 0 would be needed.
if theta <= epsilon % http://www.wolframalpha.com/input/?i=sin(theta)%2Ftheta A = 1 - theta_2/6; % https://www.wolframalpha.com/input/?i=(1+-+cos(theta))%2Ftheta%5E2 B = 0.5 - theta_2/24; else A = sin(theta)/theta; B = (1 - cos(theta))/theta_2; end % Rodrigues' formula R = eye(3) + A*S + B*S*S; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Logarithm for SO(3) --> so(3)
% |w| = acos((trace(R)-1)/2)
% w = |w| / (2*sin(|w|)) * vee(R-R’)
% Compare:
% – Lie Groups for 2D and 3D Transformations (Ethan Eade) p4
% – Lecture Ch. 2, p13
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function w = Log_SO3(R, epsilon)
if ~exist(‘epsilon’,’var’)
% set default value for epsilon
epsilon = 1e-5;
end
theta = acos((trace(R)-1)/2);
% Use Taylor expansion to avoid numerical instabilities for small theta
% At least a check for theta != 0 would be needed.
if theta <= epsilon % http://www.wolframalpha.com/input/?i=theta+%2F+sin(theta) A = 1 + (theta^2)/6; else A = theta / sin(theta); end w = 0.5 * A * vee_SO3(R-R'); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Exponential for se(3) --> SE(3)
% [ exp[w_hat] V*v ]
% exp[xi_hat] = [ ]
% [ 0 1 ]
% V = ((I – exp[w_hat])*w_hat + w*w’)/|w|^2
% Equivalent formulation:
% V = I + B*w_hat + C*w_hat^2
% B = (1-cos(|w|)) / |w|^2
% C = (1 – sin(|w|)/|w|) / |w|^2
% Compare:
% – Lie Groups for 2D and 3D Transformations (Ethan Eade) p9
% – Lecture Ch. 2, p19
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function T = Exp_SE3(xi, epsilon)
if ~exist(‘epsilon’,’var’)
% set default value for epsilon
epsilon = 1e-5;
end
v = xi(1:3);
w = xi(4:6);
S = hat_SO3(w);
S_2 = S*S;
theta_2 = w’*w;
theta = sqrt(theta_2);
% Use Taylor expansion to avoid numerical instabilities for small theta
% At least a check for theta != 0 would be needed.
if theta <= epsilon % http://www.wolframalpha.com/input/?i=sin(theta)%2Ftheta A = 1 - theta_2/6; % https://www.wolframalpha.com/input/?i=(1+-+cos(theta))%2Ftheta%5E2 B = 0.5 - theta_2/24; % http://www.wolframalpha.com/input/?i=(1+-+sin(theta)%2Ftheta)+%2F+theta%5E2 C = 1/6 - theta_2/120; else A = sin(theta)/theta; B = (1 - cos(theta))/theta_2; C = (1 - A) / theta_2; end R = eye(3) + A*S + B*S_2; % Rodrigues' formula V = eye(3) + B*S + C*S_2; T = eye(4); T(1:3,1:3) = R; T(1:3,4) = V*v; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Logarithm for SE(3) --> se(3)
% [ v = inv(V)*t ]
% xi = [ ]
% [ w = vee(log(R)) ]
% V = ((I – exp[w_hat])*w_hat + w*w’)/|w|^2
% Note: “inv” is the matrix inverse
% Equivalent closed-form solution:
% inv(V) = eye(3) – 0.5*S + D*S^2
% D = (1 – (sin(|w|)*|w|)/(2*(1 – cos(|w|)))) / |w|^2
% Compare:
% – Lecture Ch. 2, p19 (doesn’t give closed-form inverse of V)
% – Lie Groups for 2D and 3D Transformations (Ethan Eade) p9/10
% (includes closed-form inverse of V)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xi = Log_SE3(T, epsilon)
if ~exist(‘epsilon’,’var’)
% set default value for epsilon
epsilon = 1e-5;
end
R = T(1:3,1:3);
t = T(1:3, 4);
assert(norm(T(4,:) – [0 0 0 1]) < 1e-10); theta = acos((trace(R)-1)/2); theta_2 = theta^2; % Use Taylor expansion to avoid numerical instabilities for small theta % At least a check for theta != 0 would be needed. if theta <= epsilon % http://www.wolframalpha.com/input/?i=theta+%2F+sin(theta) A = 1 + (theta_2)/6; % http://www.wolframalpha.com/input/?i=(1+-+(sin(theta)%2Ftheta)%2F(2*(1+-+cos(theta))%2Ftheta%5E2))%2Ftheta%5E2 D = 1/12 - theta_2/720; else A = theta / sin(theta); A_temp = sin(theta)/theta; B_temp = (1 - cos(theta))/theta_2; D = (1 - A_temp/(2*B_temp))/theta_2; end S = 0.5 * A * (R-R'); w = vee_SO3(S); S_2 = S*S; % Note: It is not obvious to derive the closed-form solution of V, and % one possibility is to use Matlab's general matrix inverse inv(V) % to compute it (but the closed-form solution is probably faster). V_inv = eye(3) - 0.5*S + D*S_2; v = V_inv * t; xi = [v; w]; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Taylor expansion tests: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function test_taylor_exp_so3() n = 20; x = zeros(n,1); diff_taylor = zeros(n,1); diff_no_taylor = zeros(n,1); w0 = rand(3,1); w0 = 100 * w0 / norm(w0); for i = 1:n w = w0 / 10^i; x(i) = norm(w); diff_taylor(i) = norm(Exp_SO3(w, inf) - expm(hat_SO3(w)),'fro'); diff_no_taylor(i) = norm(Exp_SO3(w, 0) - expm(hat_SO3(w)),'fro'); end loglog(x, diff_taylor./x, 'g') hold on loglog(x, diff_no_taylor./x, 'r') legend('Taylor', 'no Taylor') xlabel('||w||') ylabel('||Exp\_SO3 - expm||_F') title('Exp_{SO3}(w) vs expm(hat_{SO3}(w))', 'Interpreter', 'none') end function test_taylor_exp_se3() n = 20; x = zeros(n,1); diff_taylor = zeros(n,1); diff_no_taylor = zeros(n,1); xi0 = rand(6,1); xi0 = 100 * xi0 / norm(xi0); for i = 1:n xi = xi0 / 10^i; x(i) = norm(xi); diff_taylor(i) = norm(Exp_SE3(xi, inf) - expm(hat_SE3(xi)),'fro'); diff_no_taylor(i) = norm(Exp_SE3(xi, 0) - expm(hat_SE3(xi)),'fro'); end loglog(x, diff_taylor./x, 'g') hold on loglog(x, diff_no_taylor./x, 'r') legend('Taylor', 'no Taylor') xlabel('||xi||') ylabel('||Exp\_SE3 - expm||_F') title('Exp_{SE3}(xi) vs expm(hat_{SE3}(xi))', 'Interpreter', 'none') end function test_taylor_log_so3() n = 20; x = zeros(n,1); diff_taylor = zeros(n,1); diff_no_taylor = zeros(n,1); w0 = rand(3,1); w0 = 10 * w0 / norm(w0); for i = 1:n w = w0 / 10^i; x(i) = norm(w); R = expm(hat_SO3(w)); diff_taylor(i) = norm(expm(hat_SO3(Log_SO3(R, inf))) - R,'fro'); diff_no_taylor(i) = norm(expm(hat_SO3(Log_SO3(R, 0))) - R,'fro'); end loglog(x, diff_taylor./x, 'g') hold on loglog(x, diff_no_taylor./x, 'r') legend('Taylor', 'no Taylor') xlabel('||w||') ylabel('||Log\_SO3 - logm||_F') title('Log_{SO3}(R) vs vee_{SO3}(logm(R))', 'Interpreter', 'none') end function test_taylor_log_se3() n = 20; x = zeros(n,1); diff_taylor = zeros(n,1); diff_no_taylor = zeros(n,1); xi0 = rand(6,1); xi0 = 10 * xi0 / norm(xi0); for i = 1:n xi = xi0 / 10^i; x(i) = norm(xi); T = expm(hat_SE3(xi)); diff_taylor(i) = norm(expm(hat_SE3(Log_SE3(T, inf))) - T,'fro'); diff_no_taylor(i) = norm(expm(hat_SE3(Log_SE3(T, 0))) - T,'fro'); end loglog(x, diff_taylor./x, 'g') hold on loglog(x, diff_no_taylor./x, 'r') legend('Taylor', 'no Taylor') xlabel('||xi||') ylabel('||Log\_SE3 - logm||_F') title('Log_{SE3}(T) vs vee\_{SE3}(logm(T))', 'Interpreter', 'none') end