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

網膜座標系でのオプティカルフローを算出したいと考えています。

現在はドライビングシナリオデザイナーで白線のセンサ情報を取得し、白線の情報を1枚目の写真のように網膜座標系に変換することができました。ここから２枚目の写真のようにオプティカルフローを求めたいと考えています。手順として、白線の移動先の座標を求め、その網膜座標系の座標から差をとることでフローを求めたいと考えています。そこでmainプログラムの手順を教えていただきたいです。よろしくお願いします。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%シミュレーション　メインプログラム
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% オプティカルフロー算出プログラム

%% 変数宣言
%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)')