mirror of
https://github.com/storytold/LiveScan3D.git
synced 2026-10-09 00:09:58 +00:00
Initial commit
This commit is contained in:
marek
@@ -0,0 +1,246 @@
|
||||
// Copyright (C) 2015 Marek Kowalski (M.Kowalski@ire.pw.edu.pl), Jacek Naruniec (J.Naruniec@ire.pw.edu.pl)
|
||||
// License: MIT Software License See LICENSE.txt for the full license.
|
||||
|
||||
// If you use this software in your research, then please use the following citation:
|
||||
|
||||
// Kowalski, M.; Naruniec, J.; Daniluk, M.: "LiveScan3D: A Fast and Inexpensive 3D Data
|
||||
// Acquisition System for Multiple Kinect v2 Sensors". in 3D Vision (3DV), 2015 International Conference on, Lyon, France, 2015
|
||||
|
||||
// @INPROCEEDINGS{Kowalski15,
|
||||
// author={Kowalski, M. and Naruniec, J. and Daniluk, M.},
|
||||
// booktitle={3D Vision (3DV), 2015 International Conference on},
|
||||
// title={LiveScan3D: A Fast and Inexpensive 3D Data Acquisition System for Multiple Kinect v2 Sensors},
|
||||
// year={2015},
|
||||
// }
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
using System.Net.Sockets;
|
||||
|
||||
namespace KinectServer
|
||||
{
|
||||
public delegate void SocketChangedHandler();
|
||||
|
||||
public class KinectSocket
|
||||
{
|
||||
Socket oSocket;
|
||||
byte[] byteToSend = new byte[1];
|
||||
public bool bFrameCaptured = false;
|
||||
public bool bLatestFrameReceived = false;
|
||||
public bool bStoredFrameReceived = false;
|
||||
public bool bCalibrated = false;
|
||||
//The pose of the sensor in the scene (used by the OpenGLWindow to show the sensor)
|
||||
public AffineTransform oCameraPose = new AffineTransform();
|
||||
//The transform that maps the vertices in the sensor coordinate system to the world corrdinate system.
|
||||
public AffineTransform oWorldTransform = new AffineTransform();
|
||||
|
||||
public string sSocketState;
|
||||
|
||||
public List<byte> lFrameRGB = new List<byte>();
|
||||
public List<Single> lFrameVerts = new List<Single>();
|
||||
|
||||
public event SocketChangedHandler eChanged;
|
||||
|
||||
public KinectSocket(Socket clientSocket)
|
||||
{
|
||||
oSocket = clientSocket;
|
||||
sSocketState = oSocket.RemoteEndPoint.ToString() + " Calibrated = false";
|
||||
}
|
||||
|
||||
public void CaptureFrame()
|
||||
{
|
||||
bFrameCaptured = false;
|
||||
byteToSend[0] = 0;
|
||||
SendByte();
|
||||
}
|
||||
|
||||
public void Calibrate()
|
||||
{
|
||||
bCalibrated = false;
|
||||
sSocketState = oSocket.RemoteEndPoint.ToString() + " Calibrated = false";
|
||||
|
||||
byteToSend[0] = 1;
|
||||
SendByte();
|
||||
|
||||
UpdateSocketState();
|
||||
}
|
||||
|
||||
public void SendSettings(KinectSettings settings)
|
||||
{
|
||||
List<byte> lData = settings.ToByteList();
|
||||
|
||||
byte[] bTemp = BitConverter.GetBytes(lData.Count);
|
||||
lData.InsertRange(0, bTemp);
|
||||
lData.Insert(0, 2);
|
||||
|
||||
if (SocketConnected())
|
||||
oSocket.Send(lData.ToArray());
|
||||
}
|
||||
|
||||
public void RequestStoredFrame()
|
||||
{
|
||||
byteToSend[0] = 3;
|
||||
SendByte();
|
||||
bStoredFrameReceived = false;
|
||||
}
|
||||
|
||||
public void RequestLastFrame()
|
||||
{
|
||||
byteToSend[0] = 4;
|
||||
SendByte();
|
||||
bLatestFrameReceived = false;
|
||||
}
|
||||
|
||||
public void SendCalibrationData()
|
||||
{
|
||||
int size = 1 + 2 * (9 + 3) * sizeof(float);
|
||||
byte[] data = new byte[size];
|
||||
int i = 0;
|
||||
|
||||
data[i] = 5;
|
||||
i++;
|
||||
|
||||
Buffer.BlockCopy(oWorldTransform.R, 0, data, i, 9 * sizeof(float));
|
||||
i += 9 * sizeof(float);
|
||||
Buffer.BlockCopy(oWorldTransform.t, 0, data, i, 3 * sizeof(float));
|
||||
i += 3 * sizeof(float);
|
||||
|
||||
Buffer.BlockCopy(oCameraPose.R, 0, data, i, 9 * sizeof(float));
|
||||
i += 9 * sizeof(float);
|
||||
Buffer.BlockCopy(oCameraPose.t, 0, data, i, 3 * sizeof(float));
|
||||
i += 3 * sizeof(float);
|
||||
|
||||
|
||||
if (SocketConnected())
|
||||
oSocket.Send(data);
|
||||
}
|
||||
|
||||
public void ClearStoredFrames()
|
||||
{
|
||||
byteToSend[0] = 6;
|
||||
SendByte();
|
||||
}
|
||||
|
||||
public void ReceiveCalibrationData()
|
||||
{
|
||||
bCalibrated = true;
|
||||
|
||||
byte[] buffer = Receive(sizeof(int) * 1);
|
||||
//currently not used
|
||||
int markerId = BitConverter.ToInt32(buffer, 0);
|
||||
|
||||
buffer = Receive(sizeof(float) * 9);
|
||||
Buffer.BlockCopy(buffer, 0, oWorldTransform.R, 0, sizeof(float) * 9);
|
||||
|
||||
buffer = Receive(sizeof(float) * 3);
|
||||
Buffer.BlockCopy(buffer, 0, oWorldTransform.t, 0, sizeof(float) * 3);
|
||||
|
||||
buffer = Receive(sizeof(float) * 9);
|
||||
Buffer.BlockCopy(buffer, 0, oCameraPose.R, 0, sizeof(float) * 9);
|
||||
|
||||
buffer = Receive(sizeof(float) * 3);
|
||||
Buffer.BlockCopy(buffer, 0, oCameraPose.t, 0, sizeof(float) * 3);
|
||||
|
||||
UpdateSocketState();
|
||||
}
|
||||
|
||||
public void ReceiveFrame()
|
||||
{
|
||||
lFrameRGB.Clear();
|
||||
lFrameVerts.Clear();
|
||||
|
||||
int nToRead;
|
||||
byte[] buffer = new byte[1024];
|
||||
|
||||
while (oSocket.Available == 0)
|
||||
{
|
||||
if (!SocketConnected())
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
oSocket.Receive(buffer, 4, SocketFlags.None);
|
||||
|
||||
//string result = System.Text.Encoding.UTF8.GetString(buffer);
|
||||
//n_to_read = Int32.Parse(result);
|
||||
|
||||
nToRead = BitConverter.ToInt32(buffer, 0);
|
||||
|
||||
if (nToRead == 0)
|
||||
return;
|
||||
|
||||
buffer = new byte[nToRead];
|
||||
int nAlreadyRead = 0;
|
||||
|
||||
while (nAlreadyRead != nToRead)
|
||||
{
|
||||
while (oSocket.Available == 0)
|
||||
{
|
||||
if (!SocketConnected())
|
||||
return;
|
||||
}
|
||||
|
||||
nAlreadyRead += oSocket.Receive(buffer, nAlreadyRead, nToRead - nAlreadyRead, SocketFlags.None);
|
||||
}
|
||||
|
||||
int point_size = 3 + 3 * 4;
|
||||
int n_vertices = nToRead / point_size;
|
||||
|
||||
for (int i = 0; i < n_vertices; i++)
|
||||
{
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
lFrameRGB.Add(buffer[i * point_size + j]);
|
||||
lFrameVerts.Add(BitConverter.ToSingle(buffer, i * point_size + j * 4 + 3));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public byte[] Receive(int nBytes)
|
||||
{
|
||||
byte[] buffer;
|
||||
if (oSocket.Available != 0)
|
||||
{
|
||||
buffer = new byte[Math.Min(nBytes, oSocket.Available)];
|
||||
oSocket.Receive(buffer, nBytes, SocketFlags.None);
|
||||
}
|
||||
else
|
||||
buffer = new byte[0];
|
||||
|
||||
return buffer;
|
||||
}
|
||||
|
||||
public bool SocketConnected()
|
||||
{
|
||||
bool part1 = oSocket.Poll(1000, SelectMode.SelectRead);
|
||||
bool part2 = (oSocket.Available == 0);
|
||||
|
||||
if (part1 && part2)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
private void SendByte()
|
||||
{
|
||||
oSocket.Send(byteToSend);
|
||||
}
|
||||
|
||||
public void UpdateSocketState()
|
||||
{
|
||||
if (bCalibrated)
|
||||
sSocketState = oSocket.RemoteEndPoint.ToString() + " Calibrated = true";
|
||||
else
|
||||
sSocketState = oSocket.RemoteEndPoint.ToString() + " Calibrated = false";
|
||||
|
||||
if (eChanged != null)
|
||||
eChanged();
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user