# オプティカルフローの算出

4 views (last 30 days)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%シミュレーション　メインプログラム
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% オプティカルフロー算出プログラム
%% 変数宣言
%para.mを実行
%% Driving Scenario Designerのデータを抽出
%フレーム番号
frame = 1;
%車両データ格納配列番号
num_car_sense_data = 1;
%検出数の格納
num_obj_detect_right = size(sense_data_ds_right(frame).ObjectDetections, 1);
num_obj_detect_left = size(sense_data_ds_left(frame).ObjectDetections, 1);
%格納配列の作成
obj_pos_right = zeros(num_obj_detect_right, 3);
obj_pos_right2 = zeros(num_obj_detect_right, 3);
obj_pos_left = zeros(num_obj_detect_left, 3);
obj_pos_left2 = zeros(num_obj_detect_left, 3);
obj_pos_right3 = zeros(num_obj_detect_right, 3);
obj_pos_left3 = zeros(num_obj_detect_left, 3);
%% データの抽出
% 車速
tmp_vehicle_speed = sense_data_ds_right(frame).ActorPoses(num_car_sense_data).Velocity(1:2);
vehicle_speed = hypot(tmp_vehicle_speed(1), tmp_vehicle_speed(2));
%yaw_rate
yaw_rate_vehicle = sense_data_ds_right(frame).ActorPoses(num_car_sense_data).AngularVelocity(3);
yaw_rate_vehicle = yaw_rate_vehicle*(pi/180);
%%検出位置の格納
%直交座標から車両座標系への変換
for i=1:num_obj_detect_right
obj_pos_right(i, :) = sense_data_ds_right(frame).ActorPoses(i, 1).Position(1:3);
obj_pos_right2(i, 1) = obj_pos_right(i, 1)-obj_pos_right(1,1)
obj_pos_right2(i, 2) = obj_pos_right(i, 2)-obj_pos_right(1,2)
x = obj_pos_right2(i, 1)
obj_pos_right2(i, 1) = -obj_pos_right2(i, 2)
obj_pos_right2(i, 2) = x
% 方位角
r = hypot(obj_pos_right2(i, 1),obj_pos_right2(i, 2))
obj_pos_right3(i, 1) = asin(obj_pos_right2(i, 1)/r)*180/3.14
% 仰角
r = hypot(1.5, obj_pos_right2(i, 2))
obj_pos_right3(i, 2) = -asin(1.5/r)*180/3.14
end
for i=1:num_obj_detect_left
obj_pos_left(i, :) = sense_data_ds_left(frame).ActorPoses(i, 1).Position(1:3);
obj_pos_left2(i, 1) = obj_pos_left(i, 1)-obj_pos_left(1,1)
obj_pos_left2(i, 2) = obj_pos_left(i, 2)-obj_pos_left(1,2)
y = obj_pos_left2(i, 1)
obj_pos_left2(i, 1) = -obj_pos_left2(i, 2)
obj_pos_left2(i, 2) = y
% 方位角
r = hypot(obj_pos_left2(i, 1),obj_pos_left2(i, 2))
obj_pos_left3(i, 1) = asin(obj_pos_left2(i, 1)/r)*180/3.14
% 仰角
r = hypot(1.5, obj_pos_left2(i, 2))
obj_pos_left3(i, 2) = -asin(1.5/r)*180/3.14
end
%% オプティカルフロー算出
for frame = num_frame_st:num_frame_en
%% シミュレーションデータ抽出
%左の白線
[v,yaw_rate,pylon_left_p] = get_sense_data_ds(sense_data_ds_left, frame);
num_pylon_left = size(pylon_left_p, 1);
%右の白線
[~,~, pylon_right_p] = get_sense_data_ds(sense_data_ds_right, frame);
%%　地面部分のみ抽出したときの画像配列の作成(トリム)
I_trim = zeros(size_Itri_h, size_Itri_w);
%% 地面用の画像作成
I_ground = ones(siz_Itri_h, size_Itri_w);
%% 時間t-1の車両座標系から見た白線の座標算出
end
%% プロット
fig
ure(1)
for i = 2:num_obj_detect_right
plot(obj_pos_right3(i, 1),obj_pos_right3(i, 2),'ro')
hold on
end
for i = 2:num_obj_detect_left
plot(obj_pos_left3(i, 1),obj_pos_left3(i, 2),'ro')
hold on
end
% title('degoshi_flow')
xlim([-90 90])
ylim([-40 40])
grid on
ax = gca;
% ax.XDir = 'reverse';
xlabel('方位角(deg)')
ylabel('仰角(deg)')