Shape-Sensing/conversion_plotting.m at master · IRIS-Lab-ND/Shape-Sensing · GitHub

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
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
clc
clear all


% imports LUNA x-y-z points generated by Dr. Coad's MATLAB script
x_coords_luna = readmatrix('luna_data/x_luna.csv');
y_coords_luna = readmatrix('luna_data/y_luna.csv');
z_coords_luna = readmatrix('luna_data/z_luna.csv');

phasespace_data_raw = readmatrix('luna_data/LunaComparisonTest2.csv');
phasespace_data_raw([1:10],:) = [];


% extracts points of interest which we want to plot
% currently just the "origin" based on the gage point
relevant_x_coords_luna = x_coords_luna(1,2);
relevant_y_coords_luna = y_coords_luna(1,2);
relevant_z_coords_luna = z_coords_luna(1,2);

% creates a 4x1 matrix of the "current point of interest"
% again, this is currently just the "origin" of LUNA according to gage
current_point_luna = [relevant_x_coords_luna;
    relevant_y_coords_luna;
    relevant_z_coords_luna;
    1];

luna_freq = 10.4167;
total_time_luna = length(x_coords_luna(:,1)) / luna_freq;
luna_numframes = length(x_coords_luna(:,1));
total_time_message_luna = ['total time recorded by LUNA =        ', num2str(total_time_luna), ' seconds. (', num2str(luna_numframes),' frames)'];
disp(total_time_message_luna)

ps_freq = 960;
ps_num_leds = 8;
total_time_ps = length(phasespace_data_raw) / ps_num_leds / ps_freq;
phasespace_numframes = length(phasespace_data_raw) / ps_num_leds;
total_time_message_ps = ['total time recorded by phasespace =  ', num2str(total_time_ps), ' seconds. (', num2str(phasespace_numframes),' frames)'];
disp(total_time_message_ps)

desired_time = input("Desired time (s): ")

corresp_framenum_luna = luna_freq * desired_time
corresp_framenum_ps = ps_freq * desired_time

useframe_luna = round(corresp_framenum_luna)
useframe_ps = round(corresp_framenum_ps)


% assigns rotation and transformation matrices as generated by the python script
r_pl = [[-0.07498702 -0.03920627 -0.99641348];
 [ 0.05312915  0.97481294 -0.04235468];
 [ 0.98890621 -0.04778638  0.14064482]];


t_pl = [[-7.49870187e-02  5.31291476e-02  9.88906212e-01 -3.13145721e+02];
    [-3.92062661e-02  9.74812941e-01 -4.77863763e-02  8.18591125e+02];
 [-9.96413476e-01 -4.23546780e-02  1.40644820e-01 -1.81380829e+02];
  [0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]];


% multiplies r_pl (3x3) by luna point xyz (3x1 created above)
luna_origin_converted_to_phasespace = t_pl * current_point_luna;


% LEDs of interest are 0, 4, 1, 3, 5, 6, 7
% xyz data for ONE FRAME for each LED
% 7x3 array of raw phasespace data

% finds rownum of led #0 for desired frame
led_0_rownum = 8 * (useframe_ps-1) + 1
phasespace_points = [[phasespace_data_raw(1,4) phasespace_data_raw(1,5) phasespace_data_raw(1,6)];
                     [phasespace_data_raw(led_0_rownum+4,4) phasespace_data_raw(led_0_rownum+4,5) phasespace_data_raw(led_0_rownum+4,6)];
                     [phasespace_data_raw(led_0_rownum+1,4) phasespace_data_raw(led_0_rownum+1,5) phasespace_data_raw(led_0_rownum+1,6)];
                     [phasespace_data_raw(led_0_rownum+3,4) phasespace_data_raw(led_0_rownum+3,5) phasespace_data_raw(led_0_rownum+3,6)];
                     [phasespace_data_raw(led_0_rownum+5,4) phasespace_data_raw(led_0_rownum+5,5) phasespace_data_raw(led_0_rownum+5,6)];
                     [phasespace_data_raw(led_0_rownum+6,4) phasespace_data_raw(led_0_rownum+6,5) phasespace_data_raw(led_0_rownum+6,6)];
                     [phasespace_data_raw(led_0_rownum+7,4) phasespace_data_raw(led_0_rownum+7,5) phasespace_data_raw(led_0_rownum+7,6)]];

% xyz data for ONE FRAME of luna data
% array of raw luna data
% 3 columns, xyz, n number of rows
luna_points = [x_coords_luna(useframe_luna,2:324); y_coords_luna(useframe_luna,2:324); z_coords_luna(useframe_luna,2:324)];
luna_points_xyz_cols = transpose(luna_points);

for i = 1:323
    current_point_luna_loop = [luna_points_xyz_cols(i,1); luna_points_xyz_cols(i,2); luna_points_xyz_cols(i,3); 1];
    current_luna_point_converted_to_phasespace = t_pl * current_point_luna_loop;
    luna_points_converted(i,1) = current_luna_point_converted_to_phasespace(1);
    luna_points_converted(i,2) = current_luna_point_converted_to_phasespace(2);
    luna_points_converted(i,3) = current_luna_point_converted_to_phasespace(3);
end

plot3(phasespace_points(:,1), phasespace_points(:,2), phasespace_points(:,3), 'r-', ...
      luna_points_converted(:,1), luna_points_converted(:,2), luna_points_converted(:,3), 'b-', ...
      luna_origin_converted_to_phasespace(1), luna_origin_converted_to_phasespace(2), luna_origin_converted_to_phasespace(3), 'bo', ...
      phasespace_data_raw(3,4), phasespace_data_raw(3,5), phasespace_data_raw(3,6), 'ko', ...
      phasespace_data_raw(5,4), phasespace_data_raw(5,5), phasespace_data_raw(5,6), 'co', ...
      LineWidth = 2);

xlabel('x');
ylabel('y');
zlabel('z');
title(['Plot for t=', num2str(desired_time), 's'])
xlim([-550 -10]);
ylim([700 900]);
zlim([-350 -100]);
legend('main string of LEDs', 'converted luna points', 'origin (LED #0)', 'perpendicular (LED #2)', 'along (LED #4)', 'Location','northeast')