Upload files to "/"
Simulation works 4/17/2024
This commit is contained in:
parent
ce2f0082d2
commit
b847e76e97
5 changed files with 1919 additions and 0 deletions
123
CameraTest.m
Normal file
123
CameraTest.m
Normal file
|
|
@ -0,0 +1,123 @@
|
|||
%Camera to position
|
||||
|
||||
clc;
|
||||
clear all;
|
||||
%close all;
|
||||
|
||||
%constants
|
||||
RGB = 1;
|
||||
TOP_CAM_H=13;
|
||||
SIDE_CAM_H = 0;
|
||||
SIDE_CAM_Y = 9.5;
|
||||
thresholdNum = 50;
|
||||
|
||||
NA = 0;
|
||||
|
||||
Data = [0, 0, 0, 0, 0, 0];
|
||||
|
||||
%Include source files in path
|
||||
addpath(genpath('../src'))
|
||||
|
||||
%Initialization Parameters
|
||||
server_ip = '127.0.0.1'; %IP address of the Unity Server
|
||||
server_port = 55001; %Server Port of the Unity Sever
|
||||
|
||||
client = tcpclient(server_ip,server_port);
|
||||
fprintf(1,"Connected to server\n");
|
||||
|
||||
%--------------------------------------------------------------------------------------
|
||||
|
||||
% x,y,z,yaw[z],pitch[y],roll[x]
|
||||
pose = [0,44,0,90,-90,3]; % move TennisBall out of the way
|
||||
unityLink(client,pose);
|
||||
|
||||
%TopCamera Baseline
|
||||
pose = [NA,NA,NA,NA,NA,0]; % move camera Top
|
||||
unityLink(client,pose);
|
||||
|
||||
unityImage = unityLink(client,pose);
|
||||
|
||||
baselineTop = unityImage;
|
||||
imwrite(baselineTop,"baselineTop.png");
|
||||
|
||||
%SideCamera Baseline
|
||||
pose = [NA,NA,NA,NA,NA,1]; % move camera Side
|
||||
unityLink(client,pose);
|
||||
|
||||
unityImage = unityLink(client,pose);
|
||||
|
||||
baselineSide = unityImage;
|
||||
imwrite(baselineSide,"baselineSide.png");
|
||||
|
||||
%import serve data
|
||||
filename = 'serve4.dat';
|
||||
M = readmatrix(filename);
|
||||
|
||||
%Enter Loop
|
||||
for i = 1:10:(length(M)) % [m]
|
||||
|
||||
%Pull position from serve data
|
||||
heightZ = M(i,2);
|
||||
widthY = M(i,3);
|
||||
lengthX = M(i,1);
|
||||
|
||||
pose = [lengthX,heightZ,widthY,90,-90,3]; % move TennisBall
|
||||
unityLink(client,pose);
|
||||
%----------------------------------------------------------------------------------------------------------
|
||||
%Camera position Top
|
||||
pose = [NA,NA,NA,NA,NA,0]; % move camera Top position
|
||||
unityImage = unityLink(client,pose);
|
||||
|
||||
TenisBallOnly = imsubtract(unityImage, baselineTop);
|
||||
|
||||
% use RGB values here instead grey Scales a problem
|
||||
[row,col] = find(TenisBallOnly(:,:,RGB) > thresholdNum);%find tennis ball indexes
|
||||
|
||||
xTop = mean(col);
|
||||
yTop = mean(row);
|
||||
centers = [xTop yTop];
|
||||
%----------------------------------------------------------------------------------------------------------
|
||||
%Camera Position Side
|
||||
pose = [NA,NA,NA,NA,NA,1]; % move camera Side position
|
||||
unityImage = unityLink(client,pose);
|
||||
TenisBallOnly = imsubtract(unityImage, baselineSide); %imsubtract
|
||||
|
||||
% use RGB values here instead grey Scales a problem
|
||||
[row,col] = find(TenisBallOnly(:,:,RGB) > thresholdNum);%find tennis ball indexes
|
||||
|
||||
xSide = mean(col);
|
||||
ySide = mean(row);
|
||||
%------------------------------------------------------------------------------------------------------------
|
||||
%Calculate 3D position
|
||||
[X,Y,Z] = find3DOrthogonal(xTop, yTop, ySide);
|
||||
|
||||
%error calculations if wanted
|
||||
%errorX = ((((X))-widthY)/widthY)*100;
|
||||
%errorY = ((((Y))-lengthX)/lengthX)*100;
|
||||
%errorZ = (((Z)-heightZ)/heightZ)*100;
|
||||
%Xsub = (X)-widthY;
|
||||
%Ysub = (Y)-lengthX;
|
||||
%Zsub = (Z)-heightZ;
|
||||
|
||||
Data(end+1,:) = [lengthX, widthY, heightZ, X, Y, Z];
|
||||
|
||||
end
|
||||
|
||||
figure
|
||||
scatter3(0,0,TOP_CAM_H);
|
||||
hold on
|
||||
scatter3(SIDE_CAM_Y,0,SIDE_CAM_H);
|
||||
scatter3(Data(:,1),Data(:,2),Data(:,3),'*')
|
||||
|
||||
scatter3(0,0,13);
|
||||
scatter3(Data(:,4),Data(:,5),Data(:,6),'*','blue')
|
||||
patch([5.02 -5.02 -5.02 5.02], [11.88 11.88 -11.88 -11.88], [0 0 0 0],'Green')
|
||||
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],'x');
|
||||
|
||||
hold off
|
||||
|
||||
|
||||
%--------------------------------------------------------------------------------------
|
||||
|
||||
%Close Gracefully
|
||||
fprintf(1,"Disconnected from server\n");
|
||||
298
Server.cs
Normal file
298
Server.cs
Normal file
|
|
@ -0,0 +1,298 @@
|
|||
using System;
|
||||
using System.Net.Sockets;
|
||||
using System.Text;
|
||||
using System.Collections;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using UnityEngine;
|
||||
using System.Net;
|
||||
using System.IO;
|
||||
|
||||
public class Server : MonoBehaviour {
|
||||
//General Init
|
||||
private List<ServerClient> clients;
|
||||
private List<int> disconnectIndex;
|
||||
|
||||
//Camera Variables
|
||||
private GameObject MainCam;
|
||||
RenderTexture CameraRender;
|
||||
byte[] EncodedPng;
|
||||
int Height;
|
||||
int Width;
|
||||
bool ScreenshotDone;
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////
|
||||
// main GUI components that appear for the user.
|
||||
/////////////////////////////////////////////////////////////////////////////
|
||||
[Header("Server Settings")]
|
||||
public int Port = 55001;
|
||||
private TcpListener server;
|
||||
private bool serverStarted;
|
||||
|
||||
[Header("Game Object Settings")]
|
||||
public string ObjectName = "";
|
||||
//public Vector2 Resolution = new Vector2();
|
||||
/////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// Use this for initialization
|
||||
void Start () {
|
||||
clients = new List<ServerClient>();
|
||||
disconnectIndex = new List<int>();
|
||||
|
||||
try {
|
||||
server = new TcpListener(IPAddress.Any, Port);
|
||||
server.Start();
|
||||
|
||||
Startlistening();
|
||||
serverStarted = true;
|
||||
Debug.Log("Server has started on port:" + Port.ToString());
|
||||
}
|
||||
catch (Exception e) {
|
||||
Debug.Log("Socket Error " + e.Message);
|
||||
}
|
||||
|
||||
InvokeRepeating("UpdateLoop", 0f, 0.003f);
|
||||
}
|
||||
|
||||
private void UpdateLoop(){
|
||||
if(this.MainCam == null)
|
||||
this.MainCam = GameObject.Find("cameraTop");
|
||||
|
||||
if (!serverStarted)
|
||||
return;
|
||||
if(clients.Count == 0)
|
||||
return;
|
||||
|
||||
for(int c = 0; c < clients.Count; c++ ) {
|
||||
//Check if clients are connected
|
||||
if(!isConnected(clients[c].tcp)) {
|
||||
clients[c].tcp.Close();
|
||||
disconnectIndex.Add(c);
|
||||
Debug.Log(clients[c].clientName + " has disconnected from the server");
|
||||
continue;
|
||||
}
|
||||
// Check for data from client
|
||||
else {
|
||||
float[] myFloat = new float[8];
|
||||
NetworkStream s = clients[c].tcp.GetStream();
|
||||
if (s.DataAvailable) {
|
||||
byte[] RecievedString = new byte[sizeof(float)*8];
|
||||
|
||||
if (RecievedString != null){
|
||||
s.Read(RecievedString, 0, sizeof(float)*8);
|
||||
myFloat = ConvertBytes2Float(RecievedString);
|
||||
|
||||
moveCamera(myFloat);
|
||||
|
||||
StartCoroutine(SendCamCapture(clients[c], MainCam.GetComponent<Camera>(), myFloat[0].ToString(), myFloat[1].ToString()));
|
||||
}
|
||||
s.Flush();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//Clean up Disconnected Clients
|
||||
for(int i = 0; i < disconnectIndex.Count; i++) {
|
||||
clients.RemoveAt(disconnectIndex[i]);
|
||||
}
|
||||
disconnectIndex.Clear();
|
||||
}
|
||||
|
||||
private byte[] ConvertFloat2Bytes(float[] FloatArray){
|
||||
var byteArray = new byte[FloatArray.Length * sizeof(float)];
|
||||
Buffer.BlockCopy(FloatArray, 0, byteArray, 0, byteArray.Length);
|
||||
return byteArray;
|
||||
}
|
||||
|
||||
private float[] ConvertBytes2Float(byte[] byteArray){
|
||||
var floatArray = new float[byteArray.Length / sizeof(float)];
|
||||
Buffer.BlockCopy(byteArray, 0, floatArray, 0, byteArray.Length);
|
||||
return floatArray;
|
||||
}
|
||||
|
||||
//Checks if client is connected
|
||||
private bool isConnected(TcpClient c){
|
||||
try {
|
||||
if(c != null && c.Client != null && c.Client.Connected){ //Makes sure the client is connected
|
||||
if(c.Client.Poll(0, SelectMode.SelectRead)){ //Polls the Client for activity
|
||||
return !(c.Client.Receive(new byte[1], SocketFlags.Peek) == 0); //Checks for response
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
}
|
||||
catch {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
//Begins connection with client
|
||||
private void AcceptServerClient(IAsyncResult ar){
|
||||
TcpListener listener = (TcpListener)ar.AsyncState;
|
||||
ServerClient NewClient = new ServerClient(listener.EndAcceptTcpClient(ar), null);
|
||||
Debug.Log("Someone has connected");
|
||||
clients.Add(NewClient);
|
||||
Startlistening();
|
||||
}
|
||||
|
||||
//Starts listening on server socket
|
||||
private void Startlistening(){
|
||||
server.BeginAcceptTcpClient(AcceptServerClient, server);
|
||||
}
|
||||
|
||||
//Try to close all the connections gracefully
|
||||
void OnApplicationQuit(){
|
||||
for (int i = 0; i < clients.Count; i++){
|
||||
try{
|
||||
clients[i].tcp.GetStream().Close();
|
||||
clients[i].tcp.Close();
|
||||
}
|
||||
catch { }
|
||||
}
|
||||
Debug.Log("Connections Closed");
|
||||
}
|
||||
|
||||
//Sends out data
|
||||
public void OutgoingData(ServerClient c, byte[] data){
|
||||
NetworkStream ClientStream = c.tcp.GetStream();
|
||||
try{
|
||||
ClientStream.Write(data, 0, data.Length);
|
||||
}
|
||||
catch (Exception e) {
|
||||
Debug.LogError("Could not write to client.\n Error:" + e);
|
||||
}
|
||||
}
|
||||
|
||||
// Matlab used NED right-handed coordinate system
|
||||
// +x forward [optical axis]
|
||||
// +y right
|
||||
// +z down
|
||||
|
||||
// Unity uses a wild left-handed coordinate system
|
||||
// +x right
|
||||
// +y up
|
||||
// +z forward [optical axis]
|
||||
|
||||
// matlab unity
|
||||
// forward x z
|
||||
// right y x
|
||||
// down z -y
|
||||
|
||||
private void moveCamera(float[] pose){
|
||||
float x_rot = pose[7];
|
||||
|
||||
if(x_rot == 0)
|
||||
{
|
||||
ObjectName = "cameraTop";
|
||||
this.MainCam = GameObject.Find("cameraTop");
|
||||
}
|
||||
else if(x_rot == 1)
|
||||
{
|
||||
ObjectName = "cameraSide";
|
||||
this.MainCam = GameObject.Find("cameraSide");
|
||||
}
|
||||
else
|
||||
{
|
||||
ObjectName = "TennisBall";
|
||||
}
|
||||
|
||||
GameObject ClientObj = GameObject.Find(ObjectName);
|
||||
// x,y,z,yaw[z],pitch[y],roll[x]
|
||||
float x_trans = pose[2];
|
||||
float y_trans = pose[3];
|
||||
float z_trans = pose[4];
|
||||
float z_rot = pose[5];
|
||||
float y_rot = pose[6];
|
||||
|
||||
|
||||
Debug.Log("hello:" + x_trans);
|
||||
//Vector3 matlabTranslate = new Vector3(y_trans, -z_trans, x_trans);
|
||||
//Vector3 matlabTranslate = new Vector3(x_trans, y_trans, z_trans);
|
||||
// perform translation
|
||||
//ClientObj.transform.position = transform.TransformVector(matlabTranslate);
|
||||
|
||||
//ClientObj.transform.position = ClientObj.transform.position + new Vector3(x_trans, y_trans, z_trans);
|
||||
if(ObjectName == "TennisBall")
|
||||
{
|
||||
ClientObj.transform.position = new Vector3(x_trans, y_trans, z_trans);
|
||||
|
||||
|
||||
// perform rotation in yaw, pitch, roll order while converting to left hand coordinate system.
|
||||
ClientObj.transform.rotation = Quaternion.AngleAxis(z_rot, Vector3.up) * // yaw [z]
|
||||
Quaternion.AngleAxis(-y_rot, Vector3.right)* // pitch [y]
|
||||
Quaternion.AngleAxis(0, Vector3.forward); // roll [x]
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//Organizes and Sends Picture
|
||||
IEnumerator SendCamCapture(ServerClient c, Camera CameraSelect, string Width, string Height){
|
||||
CaptureImage(CameraSelect, int.Parse(Width), int.Parse(Height));
|
||||
while (!this.CaptureDone()){
|
||||
yield return null;
|
||||
}
|
||||
OutgoingData(c, this.ReturnCaptureBytes());
|
||||
Debug.Log("Captured Image");
|
||||
}
|
||||
|
||||
IEnumerator GetRender(Camera cam){
|
||||
this.CameraRender = new RenderTexture(this.Width, this.Height, 16);
|
||||
cam.enabled = true;
|
||||
cam.targetTexture = CameraRender;
|
||||
Texture2D tempTex = new Texture2D(CameraRender.width, CameraRender.height, TextureFormat.RGB24, false);
|
||||
cam.Render();
|
||||
RenderTexture.active = CameraRender;//Sets the Render
|
||||
tempTex.ReadPixels(new Rect(0, 0, CameraRender.width, CameraRender.height), 0, 0);
|
||||
tempTex.Apply();
|
||||
EncodedPng = tempTex.GetRawTextureData();
|
||||
Destroy(tempTex);
|
||||
yield return null;
|
||||
CameraRender.Release();
|
||||
cam.targetTexture = null;
|
||||
ScreenshotDone = true;
|
||||
}
|
||||
|
||||
public void CaptureImage(Camera SelectedCamera, int UsrWidth, int UsrHeight){
|
||||
ScreenshotDone = false;
|
||||
this.Width = UsrWidth;
|
||||
this.Height = UsrHeight;
|
||||
|
||||
if (SelectedCamera != null){
|
||||
StartCoroutine(GetRender(SelectedCamera));
|
||||
}
|
||||
}
|
||||
|
||||
public int GetWidth(){
|
||||
return this.Width;
|
||||
}
|
||||
|
||||
public int GetHeight(){
|
||||
return this.Height;
|
||||
}
|
||||
|
||||
public bool CaptureDone(){
|
||||
return ScreenshotDone;
|
||||
}
|
||||
|
||||
public byte[] ReturnCaptureBytes(){
|
||||
return EncodedPng;
|
||||
}
|
||||
|
||||
IEnumerator SerializeCapture(ServerClient c, byte[] PixelData, int Width, int Length){
|
||||
OutgoingData(c, PixelData);
|
||||
yield return null;
|
||||
}
|
||||
}
|
||||
|
||||
public class ServerClient {
|
||||
public TcpClient tcp;
|
||||
public string clientName;
|
||||
public List<GameObject> ClientObj;
|
||||
|
||||
public ServerClient(TcpClient clientSocket, string Name) {
|
||||
clientName = Name;
|
||||
tcp = clientSocket;
|
||||
ClientObj = new List<GameObject>();
|
||||
}
|
||||
}
|
||||
38
find3DOrthogonal.m
Normal file
38
find3DOrthogonal.m
Normal file
|
|
@ -0,0 +1,38 @@
|
|||
function [y_comp,x_comp,h_ball] = find3DOrthogonal(top_ball_x_px, top_ball_y_px, side_ball_y_px)
|
||||
% constants
|
||||
TOP_CAM_FOV_H = 90;
|
||||
TOP_CAM_FOV_V = 58.72538;
|
||||
TOP_CAM_IMG_W = 3840;
|
||||
TOP_CAM_IMG_H = 2160;
|
||||
TOP_CAM_H = 13;
|
||||
|
||||
|
||||
SIDE_CAM_FOV_V = 120;
|
||||
SIDE_CAM_IMG_H = 2160;
|
||||
SIDE_CAM_H = 0;
|
||||
SIDE_CAM_Y = 9.5;
|
||||
|
||||
|
||||
%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 - (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));
|
||||
end
|
||||
|
||||
|
||||
%TODO
|
||||
% 1) take out rgb2grey and find circle with rgb image
|
||||
% 2) figure out how are calculated based on camera as refences
|
||||
1439
serve4.dat
Normal file
1439
serve4.dat
Normal file
File diff suppressed because it is too large
Load diff
21
unityLink.m
Normal file
21
unityLink.m
Normal file
|
|
@ -0,0 +1,21 @@
|
|||
function image = unityLink(TCP_Handle,pose)
|
||||
% x,y,z,yaw[z],pitch[y],roll[x]
|
||||
|
||||
width = 3840; %480
|
||||
height = 2160; %752
|
||||
|
||||
x = pose(1);
|
||||
y = pose(2);
|
||||
z = pose(3);
|
||||
yaw = pose(4);
|
||||
pitch = pose(5);
|
||||
roll = pose(6);
|
||||
|
||||
%Set Position
|
||||
write(TCP_Handle,single([width,height,x,y,z,yaw,pitch,roll]));
|
||||
|
||||
%Get image data
|
||||
data = read(TCP_Handle,width*height*3);
|
||||
|
||||
temp = reshape(data,[3,width*height]);
|
||||
image = imrotate(reshape(temp',[width,height,3]),90);
|
||||
Loading…
Add table
Reference in a new issue