1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
|
clc; clear; #close all;
path = "/home/user/";
%file_base = "se2_interp_slerp";
%file_base = "se2_interp_cubic";
file_base = "se2_interp_cnsmooth";
extension = ".csv";
file = fullfile(path, [file_base extension]);
data = load(file);
'num k points'
num_k_pts = data(1,1)
'Interpolation method'
interp_method = data(1,2)
switch interp_method
case 0
method = 'SLERP'
case 1
method = 'CUBIC'
case 2
method = 'CN smooth'
otherwise
method = 'Unknown'
end
title = strcat('SE2 Interpolation (', method, ')');
'num interpolated points'
total_pts = size(data(2+num_k_pts:end,1), 1)
r = 0.1; % magnitude (length) of arrow to plot
x = data(2:end,1);
y = data(2:end,2);
t = data(2:end,3);
u = r * cos(t); % convert polar (theta,r) to cartesian
v = r * sin(t);
figure;
quiver(x(1:num_k_pts),y(1:num_k_pts),u(1:num_k_pts),v(1:num_k_pts),'color',[0 0 1]);
hold on
quiver(x(num_k_pts+1:end),y(num_k_pts+1:end),u(num_k_pts+1:end),v(num_k_pts+1:end),'color',[1 0 0]);
set(get(gca, 'title'), 'string', title);
hold off
img_file = fullfile(path, [file_base '.png']);
saveas(gcf, img_file);
return;
|
