Robotics
/
Modern_Robotics
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

2 functions added in Matlab version. 

Possibly need to make more comments.
This commit is contained in:
HuanWeng 2018-06-27 00:50:41 -05:00
parent f42f860b80
commit 7e2b5fc602
2 changed files with 48 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
code/MATLAB/ProjectToSE3.m Normal file
View File

@ -0,0 +1,23 @@
%*** BASIC HELPER FUNCTIONS ***
function T = ProjectToSE3(mat)
% Takes mat: A matrix near SE(3) to project to SE(3).
% Returns T representing the closest rotation matrix that is in SE(3)
% Example Inputs:
%{
clear; clc;
mat = [ 0.675, 0.150, 0.720, 1.2;
0.370, 0.771, -0.511, 5.4;
-0.630, 0.619, 0.472, 3.6;
0.003, 0.002, 0.010, 0.9];
T = ProjectToSE3(mat)
%}
% Output:
% T =
% 0.6790 0.1489 0.7189 1.2000
% 0.3732 0.7732 -0.5127 5.4000
% -0.6322 0.6164 0.4694 3.6000
% 0 0 0 1.0000
T = RpToTrans(ProjectToSO3(mat(1: 3, 1: 3)), mat(1: 3, 4));
end

25
code/MATLAB/ProjectToSO3.m Normal file
View File

@ -0,0 +1,25 @@
%*** BASIC HELPER FUNCTIONS ***
function R = ProjectToSO3(mat)
% Takes mat: A matrix near SO(3) to project to SO(3).
% Returns R representing the closest rotation matrix that is in SO(3)
% Example Inputs:
%{
clear; clc;
mat = [ 0.675, 0.150, 0.720;
0.370, 0.771, -0.511;
-0.630, 0.619, 0.472];
R = ProjectToSO3(mat)
%}
% Output:
% R =
% 0.6790 0.1489 0.7189
% 0.3732 0.7732 -0.5127
% -0.6322 0.6164 0.4694
[U, S, V] = svd(mat);
R = U * V';
if det(R) < 0
R = [R(:, 1: 2); -R(:, 3)];
end
end