50 lines
No EOL
1.4 KiB
Matlab
50 lines
No EOL
1.4 KiB
Matlab
function [y_comp,x_comp,h_ball] = find3DOrthogonal(top_ball_x_px, top_ball_y_px, side_ball_x_px, side_ball_y_px)
|
|
% constants
|
|
|
|
TOP_CAM_H = 13;
|
|
TOP_CAM_FOV_H = 90;
|
|
TOP_CAM_FOV_V = 58.70567;
|
|
TOP_CAM_IMG_W = 3840;
|
|
TOP_CAM_IMG_H = 2160;
|
|
|
|
SIDE_CAM_H = .6;
|
|
SIDE_CAM_Y = 9.5;
|
|
SIDE_CAM_FOV_H = 133.1735;
|
|
SIDE_CAM_FOV_V = 120;
|
|
SIDE_CAM_IMG_W = 3840;
|
|
SIDE_CAM_IMG_H = 2160;
|
|
|
|
|
|
%Calculations% x position
|
|
theta_x = (top_ball_x_px - (TOP_CAM_IMG_W / 2)) * (TOP_CAM_FOV_H / TOP_CAM_IMG_W);
|
|
x_ball = TOP_CAM_H * tand(theta_x);
|
|
|
|
% y position
|
|
theta_y = (top_ball_y_px - (TOP_CAM_IMG_H / 2)) * (TOP_CAM_FOV_V / TOP_CAM_IMG_H);
|
|
y_ball = TOP_CAM_H * tand(theta_y);
|
|
|
|
% height
|
|
theta_h = ((SIDE_CAM_IMG_H / 2) - side_ball_y_px) * (SIDE_CAM_FOV_V / SIDE_CAM_IMG_H);
|
|
h_ball = (SIDE_CAM_H + (tand(theta_h) * (SIDE_CAM_Y - y_ball))) / (1 - (y_ball*tand(theta_h) / TOP_CAM_H));
|
|
|
|
|
|
% x & y positions compensated based on height
|
|
x_comp = x_ball * (1 - (h_ball / TOP_CAM_H));
|
|
|
|
y_comp = y_ball * (1 - (h_ball / TOP_CAM_H)); % THIS ONE
|
|
|
|
%x_comp =x_ball;
|
|
%y_comp = y_ball;
|
|
|
|
h_ball = h_ball - .2;
|
|
h_error = .38*h_ball;
|
|
h_ball = h_ball + h_error;
|
|
h_error = .16*h_ball;
|
|
h_ball = h_ball + h_error;
|
|
|
|
end
|
|
|
|
|
|
%TODO
|
|
% 1) take out rgb2grey and find circle with rgb image
|
|
% 2) figure out how are calculated based on camera as refences |