This repository has been archived by the owner on Oct 22, 2021. It is now read-only.
forked from andybarry/simflight
-
Notifications
You must be signed in to change notification settings - Fork 0
/
LoadStereo.m
68 lines (47 loc) · 1.68 KB
/
LoadStereo.m
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
function stereo = LoadStereo(stereo_values)
% Loads stereo values from a log
% stereo
% #stereo <class 'lcmt_stereo.lcmt_stereo'> :
% #[
% #1- timestamp
% #2- number_of_points
% #3- frame_number
% #4- video_number
% #5- x(0)
% #5- y(0)
% #5- z(0)
% #5- log_timestamp
% #]
stereo.utime = stereo_values(:,1);
stereo.number_of_points = stereo_values(:,2);
stereo.frame_number = stereo_values(:,3);
stereo.video_number = stereo_values(:,4);
if (size(stereo_values, 2) < 6)
% if we got no stereo hits, the array won't be initialized
stereo.x = [];
stereo.y = [];
stereo.z = [];
stereo.logtime = stereo_values(:,5);
else
% init variables
stereo.x = zeros(length(stereo.frame_number), size(stereo_values,2));
stereo.y = zeros(length(stereo.frame_number), size(stereo_values,2));
stereo.z = zeros(length(stereo.frame_number), size(stereo_values,2));
% parse the nubmer of points
for i=1:length(stereo.number_of_points)
this_number = stereo.number_of_points(i);
if (this_number > 0)
positions = 0:this_number;
offset_index = 5;
stereo.x(i,1:this_number) = stereo_values(i, offset_index:this_number+offset_index-1);
offset_index = offset_index + this_number;
stereo.y(i,1:this_number) = stereo_values(i, offset_index:this_number+offset_index-1);
offset_index = offset_index + this_number;
stereo.z(i,1:this_number) = stereo_values(i, offset_index:this_number+offset_index-1);
stereo.logtime(i) = stereo_values(i, this_number*3+5);
else
stereo.logtime(i) = stereo_values(i, 5);
end
end
end
end