Works 4/25/2024
This commit is contained in:
parent
03275540b9
commit
3639f18ae1
1 changed files with 116 additions and 14 deletions
130
CameraTest.m
130
CameraTest.m
|
|
@ -5,16 +5,26 @@ clear all;
|
|||
close all;
|
||||
|
||||
%constants
|
||||
|
||||
RGB = 1;
|
||||
|
||||
TOP_CAM_H=13;
|
||||
SIDE_CAM_H = .6;
|
||||
SIDE_CAM_Y = 9.5;
|
||||
thresholdNum = 50;
|
||||
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;
|
||||
|
||||
NA = 0;
|
||||
|
||||
Data = [0, 0, 0, 0, 0, 0];
|
||||
%cent = [0, 0, 0, 0, 0, 0]; %test array
|
||||
|
||||
%Include source files in path
|
||||
addpath(genpath('../src'))
|
||||
|
|
@ -58,9 +68,9 @@ M = readmatrix(filename);
|
|||
for i = 1:1:(length(M)) % [m]
|
||||
|
||||
%Pull position from serve data
|
||||
heightZ = M(i,2);
|
||||
widthY = M(i,3);
|
||||
lengthX = M(i,1);
|
||||
heightZ = M(i,2);%1;
|
||||
lengthX = M(i,1);%2;
|
||||
widthY = M(i,3);%2;
|
||||
|
||||
pose = [lengthX,heightZ,widthY,90,-90,3]; % move TennisBall
|
||||
unityLink(client,pose);
|
||||
|
|
@ -72,26 +82,45 @@ for i = 1:1:(length(M)) % [m]
|
|||
TenisBallOnly = imsubtract(unityImage, baselineTop);
|
||||
|
||||
% use RGB values here instead grey Scales a problem
|
||||
[row,col] = find(TenisBallOnly(:,:,RGB) > thresholdNum);%find tennis ball indexes
|
||||
[rowR,colR] = find(abs(TenisBallOnly(:,:,1)) ~= 0);% thresholdNum);%find tennis ball indexes
|
||||
[rowG,colG] = find(abs(TenisBallOnly(:,:,2)) ~= 0);%> thresholdNum);
|
||||
[rowB,colB] = find(abs(TenisBallOnly(:,:,3)) ~= 0);%> thresholdNum);
|
||||
|
||||
xTop = mean(col);
|
||||
yTop = mean(row);
|
||||
centers = [xTop yTop];
|
||||
|
||||
xTopR = mean(colR);
|
||||
yTopR = mean(rowR);
|
||||
|
||||
%xTopG = mean(colG);
|
||||
%yTopG = mean(rowG);
|
||||
|
||||
%xTopB = mean(colB);
|
||||
%yTopB = mean(rowB);
|
||||
|
||||
%cent(end+1,:) = [xTopR, yTopR, xTopG, yTopG,xTopB, yTopB]; %test
|
||||
%array
|
||||
|
||||
%uses Red
|
||||
xTop = xTopR;
|
||||
yTop = yTopR;
|
||||
%----------------------------------------------------------------------------------------------------------
|
||||
%Camera Position Side
|
||||
pose = [NA,NA,NA,NA,NA,1]; % move camera Side position
|
||||
unityImage = unityLink(client,pose);
|
||||
TenisBallOnly = imsubtract(unityImage, baselineSide); %imsubtract
|
||||
|
||||
% imwrite(unityImage,"Side.png");
|
||||
TenisBallOnly = imsubtract(unityImage, baselineSide); %imsubtract
|
||||
%imshow(unityImage);
|
||||
% use RGB values here instead grey Scales a problem
|
||||
[row,col] = find(TenisBallOnly(:,:,RGB) > thresholdNum);%find tennis ball indexes
|
||||
[row,col] = find(TenisBallOnly(:,:,RGB) ~= 0);%find tennis ball indexes
|
||||
|
||||
xSide = mean(col);
|
||||
ySide = mean(row);
|
||||
%------------------------------------------------------------------------------------------------------------
|
||||
%Calculate 3D position
|
||||
[X,Y,Z] = find3DOrthogonal(xTop, yTop, xSide, ySide);
|
||||
Data(end+1,:) = [lengthX, widthY, heightZ, X, Y, Z];
|
||||
if(~(isnan(X)))
|
||||
Data(end+1,:) = [lengthX, widthY, heightZ, X, Y, Z];%
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
|
|
@ -135,5 +164,78 @@ hold off
|
|||
|
||||
%--------------------------------------------------------------------------------------
|
||||
|
||||
%Poly fit stuff uncomment all to see results
|
||||
|
||||
%t = 0:(size(Data)-2);
|
||||
%
|
||||
%tz = 0:(size(Data)-2);
|
||||
%d = (Data(2:end,5));
|
||||
%
|
||||
%p = polyfit(tz,d',2);
|
||||
%zfit = polyval(p,tz);
|
||||
%
|
||||
%figure
|
||||
%title("Z postion vs sample number PolyFit")
|
||||
%hold on
|
||||
%plot(Data(2:end,2))
|
||||
%plot(zfit)
|
||||
%hold off
|
||||
%
|
||||
%tx = 0:(size(Data)-2);
|
||||
%d = (Data(2:end,4));
|
||||
%
|
||||
%p = polyfit(tx,d',2);
|
||||
%xfit = polyval(p,tz);
|
||||
%
|
||||
%figure
|
||||
%title("X postion vs sample number PolyFit")
|
||||
%hold on
|
||||
%plot(Data(2:end,1))
|
||||
%plot(xfit)
|
||||
%hold off
|
||||
%
|
||||
%d = (Data(2:end,6));
|
||||
%[zeros,zIdx] = min(d);
|
||||
%ty1 = 1:(zIdx-1);
|
||||
%ty2 = (zIdx-1):(size(Data)-2);
|
||||
%d1 = (Data(2:zIdx,6));
|
||||
%d2 = (Data(zIdx:size(Data)-1,6));
|
||||
%
|
||||
%
|
||||
%p1 = polyfit(ty1,d1',3);
|
||||
%%yfit1 = polyval(p1,t);
|
||||
%
|
||||
%p2 = polyfit(ty2,d2',3);
|
||||
%%yfit2 = polyval(p2,ty);
|
||||
%
|
||||
%
|
||||
%
|
||||
%yfit = horzcat(polyval(p1,ty1),polyval(p2,ty2));
|
||||
%
|
||||
%figure
|
||||
%title("Y(height) postion vs sample number PolyFit")
|
||||
%hold on
|
||||
%plot(Data(2:end,3))
|
||||
%plot(yfit)
|
||||
%hold off
|
||||
%
|
||||
%figure
|
||||
%scatter3(0,0,TOP_CAM_H); %Top Camera
|
||||
%hold on
|
||||
%scatter3(SIDE_CAM_Y,0,SIDE_CAM_H); %Side Camera
|
||||
%scatter3(Data(:,1),Data(:,2),Data(:,3),10,'filled') %Actual Position
|
||||
%scatter3(Data(2,1),Data(2,2),Data(2,3), 200,'x','red') %Starting Postion
|
||||
%
|
||||
%scatter3(xfit,zfit,yfit,10,'filled','blue') %Calculated Position
|
||||
%patch([5.02 -5.02 -5.02 5.02], [11.88 11.88 -11.88 -11.88], [0 0 0 0],'Green') %Tennis Court Plane
|
||||
%scatter3([15 15 -15 -15 15 15 -15 -15],[15 -15 15 -15 15 -15 15 -15],[-1 -1 -1 -1 TOP_CAM_H TOP_CAM_H TOP_CAM_H TOP_CAM_H],1,'x'); %Corrners
|
||||
%
|
||||
%xlabel("Unity X")
|
||||
%ylabel("Unity Z")
|
||||
%zlabel("Unity Y")
|
||||
%
|
||||
%hold off
|
||||
|
||||
|
||||
%Close Gracefully
|
||||
fprintf(1,"Disconnected from server\n");
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue