private DepthData deepCopyData(DepthData data)
{
DepthData ret = new DepthData();
ret.exposureTimes = new uint[data.exposureTimes.Length];
Array.Copy(data.exposureTimes, ret.exposureTimes, data.exposureTimes.Length);
ret.height = data.height;
ret.width = data.width;
ret.version = data.version;
ret.timeStamp = data.timeStamp;
ret.points = new DepthPoint[data.points.Length];
int i = 0;
foreach (var x in data.points)
{
ret.points[i] = new DepthPoint();
ret.points[i].x = x.x;
ret.points[i].y = x.y;
ret.points[i].z = x.z;
ret.points[i].noise = x.noise;
ret.points[i].grayValue = x.grayValue;
ret.points[i].depthConfidence = x.depthConfidence;
i++;
}
return(ret);
}
// Use this for initialization
void Start()
{
// AssetDatabase.ImportPackage("/packages/UnityToolkit",false);
#region Instantiate HandData and DepthData
if (WithDepth)
{
DepthData = new HumanRobotInteraction.DepthData(); // Configure and setting of the getting of the depth data
}
HandsData = new HandsData(MaxHands, MaxJoints); // Configure and setting of the getting of the hand data
#endregion
#region Instantiate the SARI Algorithm
/* Instantiation of the SARI and initializing the filters */
for (int i = 0; i < MaxHands; i++)
{
Sari[i] = new SARI(MaxJoints);
Sari[i].InitializeFilter();
}
SariPlane = new SARI();
#endregion
#region Instatiate the visualization
for (int i = 0; i < MaxHands; i++)
{
VisPredicted[i] = new Visualization("Hand Predicted " + (i + 1).ToString(), MaxJoints, new Color(0, 0, 1), new Color(0, 0, 1), new Color(0, 0, 1), new Color(0.47f, 0.108f, 0.221f));
VisFiltered[i] = new Visualization("Hand Filtered " + (i + 1).ToString(), MaxJoints, new Color(0.65f, 0.22f, 0f), new Color(0.9f, 0.62f, 0.43f), new Color(0.65f, 0.22f, 0f), new Color(1, 0, 0));
VisRaw[i] = new Visualization("Hand Raw " + (i + 1).ToString(), MaxJoints, new Color(0.65f, 0.22f, 0f), new Color(0.9f, 0.62f, 0.43f), new Color(0.65f, 0.22f, 0f), new Color(1, 0, 0));
}
VisPlane = new Visualization("Plane");
#endregion
#region Prepare arrays for coordinates
/* Declaration of the array for the hand data*/
for (int i = 0; i < MaxHands; i++)
{
RawCoord[i] = new HandCoord(6, MaxJoints);
FilteredCoord[i] = new HandCoord(6, MaxJoints);
PredictedCoord[i] = new HandCoord(6, MaxJoints);
}
for (int i = 0; i < 10000; i++)
{
for (int j = 0; j < 3; j++)
{
RecordedData[j, i] = new HandCoord(6, MaxJoints);
}
if (i < 200)
{
CalibrationData[i] = new HandCoord(6, MaxJoints);
}
}
#endregion
}
public void Start(IServiceProvider sp, string[] args)
{
var bus1 = sp.GetService <ITopicEventBus>();
var bus2 = sp.GetService <IFanoutEventBus>();
var bus4 = sp.GetService <IDirectEventBus>();
var fact = sp.GetService <EventBusFactory>();
var bus3 = fact.GetFanout(new DefaultYDSeralizer(Encoding.UTF8), brokerName: "hkex.hsi");
bus1.RegisterConsumer("q1", x =>
{
x.Subscribe <DepthData, Test1Handler>("HKEX.#");
x.SubscribeBytes <Test1Handler>("test.#");
}, autodel: true, durable: false, autoAck: false, queueLength: 1000);
bus2.RegisterConsumer("q2", x =>
{
x.SubscribeBytes <Test2Handler>();
x.Subscribe <DepthData, Test2Handler>();
}, autodel: true, durable: false, autoAck: false, queueLength: 1000);
bus3.RegisterConsumer("q3", x =>
{
x.SubscribeBytes <Test3Handler>();
x.Subscribe <DepthData, Test3Handler>();
}, autodel: true, durable: false, autoAck: false, queueLength: 1000);
bus4.RegisterConsumer("q4", x =>
{
x.SubscribeBytes <Test4Handler>("test.a");
x.SubscribeBytes <Test4Handler>("test.b");
x.Subscribe <DepthData, Test4Handler>();
}, autodel: true, durable: false, autoAck: false, queueLength: 1000);
var dp = new DepthData()
{
ExchangeID = "HKEX",
CommodityNo = "HSI",
InstrumentID = "2007",
LastPrice = 12817,
Volume = 5
};
for (; ;)
{
bus1.PublishBytes(Encoding.UTF8.GetBytes("hello world"), "test.a", out _);
bus1.PublishBytes(Encoding.UTF8.GetBytes("hello world"), "test.b", out _);
bus1.Publish(dp);
bus2.PublishBytes(Encoding.UTF8.GetBytes("hello world"), "test.a", out _);
bus2.PublishBytes(Encoding.UTF8.GetBytes("hello world"), "test.b", out _);
bus2.Publish(dp);
bus3.PublishBytes(Encoding.UTF8.GetBytes("hello world"), "test.a", out _);
bus3.PublishBytes(Encoding.UTF8.GetBytes("hello world"), "test.b", out _);
bus3.Publish(dp, out _, true);
bus4.PublishBytes(Encoding.UTF8.GetBytes("hello world"), "test.a", out _);
bus4.PublishBytes(Encoding.UTF8.GetBytes("hello world"), "test.b", out _);
bus4.Publish(dp, out _, true);
Console.ReadKey();
}
}
/*~DepthDataListener()
* {
* if (dlgt != null)
* {
* dlgt.EndInvoke(ar);
* }
* }*/
public void OnNewData(DepthData data)
{
this.data = deepCopyData(data);
/*if (dlgt != null)
* {
* dlgt.EndInvoke(ar);
* }
* dlgt = new showDataDelegate(mainWindow.showData);
* ar = dlgt.BeginInvoke(this.data, null, null);*/
mainWindow.imgDataGray.Dispatcher.Invoke(new UpdateImageCallback(mainWindow.UpdateImage), data);
}
private bool AlterDepth(int id, int depth)
{
for (int i = 0; i < g_Depth.Count; i++)
{
DepthData item = g_Depth[i];
if (item.ID == id)
{
g_Depth[i] = new DepthData(id, depth);
return(true);
}
}
return(false);
}
public int AddToQueue(DepthData depthData)
{
//Decrement the semaphore to make sure the spot is available
this.requestDepthData.WaitOne();
lock (_threadLock)
{
dataQueue.Enqueue(depthData);
}
//Increament the semaphore to indicate there is work to do
int previousCount = handleRequests.Release();
return previousCount;
}
public static void SetDepthData(TrackableId trackableId, Vector3[] positions, float[] confidenceValues = null, ulong[] identifiers = null)
{
lock (stateLock)
{
if (all.TryGetValue(trackableId, out var tmp))
{
if (positions == null)
{
throw new ArgumentNullException("positions");
}
if (confidenceValues != null && positions.Length != confidenceValues.Length)
{
throw new ArgumentException("confidenceValues must be the same length as positions");
}
if (identifiers != null && positions.Length != identifiers.Length)
{
throw new ArgumentException("positions must be the same length as positions");
}
if (!datas.TryGetValue(tmp, out var data))
{
data = new DepthData();
datas[tmp] = data;
}
data.positions = positions;
data.confidenceValues = confidenceValues;
data.identifiers = identifiers;
if (!added.Contains(tmp) && !updated.Contains(tmp))
{
updated.Add(tmp);
}
}
else
{
throw new ArgumentException("Unknown trackableID");
}
}
}
/// <summary>
/// 设置元器件新的深度值，这常常是点选元器件后需要将其置顶
/// </summary>
public void SetTopDepth()
{
//如果当前没有两个及其以上对象，则不需要设置深度
if (g_Depth.Count < 2)
{
return;
}
//如果是最高那个，则不需要设置深度
int topMostDepth = FindTopMostDepth();
if (topMostDepth >= m_iDepth)
{
return;
}
int index = FindIndex(ID);
if (index == -1)//如果未找到当前组件，则报出异常
{
return;
}
int startIndex = MinDepth;
for (int i = 0; i < g_Depth.Count; i++)//设置其他元器件深度
{
DepthData item = g_Depth[i];
if (item.ID == ID)
{
startIndex++;
continue;
}
NDlabObject obj = FindLabObject(item.ID);
if (obj != null)
{
obj.SetDepth(startIndex - i);
}
}
SetDepth(topMostDepth);//设置自身置顶深度
}
/// <summary>
/// take a single depth color int array shot
/// </summary>
/// <returns></returns>
public int[] DepthintArray()
{
//short[] depthPixel = new short[1];
for (; ;)
{
if (senseManager.AcquireFrame(true).IsError())
{
throw new Exception("Failed to acquire frame");
}
else
{
// capture
PXCMCapture.Sample sample = senseManager.QuerySample();
PXCMImage depth = sample.depth;
//test
PXCMImage.ImageData DepthData;
depth.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH_F32, out DepthData);
int dwith = depth.info.width;
int dheight = depth.info.height;
int[] depthPixel = DepthData.ToIntArray(0, dwith * dheight);
depth.Dispose();
// release
senseManager.ReleaseFrame();
return(depthPixel);
//break;
}
}
//should be used if we want to shut down the cam .
//senseManager.Dispose();
return(null);
}
public static void UpdateDepthData(int newDepth)
{
if (currentDepth == newDepth)
{
return;
}
currentDepth = newDepth;
DepthData newDepthData = Resources.Load <DepthData>(DEPTH_DATA_DIR + DEPTH_FILE_PREFIX + newDepth);
if (newDepthData != null)
{
currentDepthData = newDepthData;
TextAsset eventsText = newDepthData.eventPool;
JSONObject layerEventObject = new JSONObject(eventsText.text);
currentLayerEvents = layerEventObject[EVENTS_KEY].list;
}
else
{
Debug.LogError("Couldn't load depth data for depth: " + newDepth);
}
}
private int ProcessNewDepthData(DepthData newDepthdata)
{
PlanarImage Image = newDepthdata.ImageFrame.Image;
CheckPlayerDepth(Image.Bits);
return (int)ResultCodes.Success;
}
public void UpdateImage(DepthData data)
{
WriteableBitmap bmp = new WriteableBitmap(data.width, data.height, 96, 96, PixelFormats.Gray16, null);
int stride = bmp.PixelWidth * (bmp.Format.BitsPerPixel / 8);
WriteableBitmap bmpDepthColor = new WriteableBitmap(data.width, data.height, 96, 96, PixelFormats.Bgr32, null);
int strideDepthColor = bmpDepthColor.PixelWidth * (bmp.Format.BitsPerPixel / 8);
ushort[] gray = new ushort[data.width * data.height];
int i = 0;
foreach (var x in data.points)
{
gray[i] = x.grayValue;
i++;
}
int max = gray.Max();
int min = gray.Min();
for (int j = 0; j < gray.Count(); j++)
{
gray[j] = (ushort)map(gray[j], min, max, 0, ushort.MaxValue);
}
bmp.WritePixels(new Int32Rect(0, 0, data.width, data.height), gray, stride, 0);
imgDataGray.Source = bmp;
float[] depthF = new float[data.width * data.height];
i = 0;
foreach (var x in data.points)
{
depthF[i] = x.z;
i++;
}
float maxDepth = depthF.Max();
float minDepth = depthF.Min();
byte[] colorPixels;
colorPixels = new byte[depthF.Count() * sizeof(int)];
int colorPixelIndex = 0;
for (i = 0; i < depthF.Count(); ++i)
{
ushort intensity = (ushort)mapF(depthF[i], minDepth, maxDepth, 0, ushort.MaxValue);
Color c = MapRainbowColor(intensity, ushort.MaxValue, 0);
colorPixels[colorPixelIndex++] = c.B;
colorPixels[colorPixelIndex++] = c.G;
colorPixels[colorPixelIndex++] = c.R;
++colorPixelIndex;
}
bmpDepthColor.WritePixels(new Int32Rect(0, 0, data.width, data.height), colorPixels, bmpDepthColor.PixelWidth * sizeof(int), 0);
imgDataDepth.Source = bmpDepthColor;
if (save_next_img)
{
save_next_img = false;
SaveImage(bmp, "img/gray.jpg");
SaveImage(bmpDepthColor, "img/color.jpg");
SaveArrayAsCSV(gray, "img/gray.csv");
SaveArrayAsCSV(depthF, "img/depth.csv");
}
}
/// <summary>
/// Initializes a new instance of the MainWindow class.
/// </summary>
public MainWindow()
{
// create nuiApp instance and setting NuiApplication's name
nuiApp = new NuiApplication("OpenNUI.Samples.DepthBasics");
// register delegate when nui sensor connected, disconnected
nuiApp.OnSensorConnected += nuiApp_OnSensorConnected;
nuiApp.OnSensorDisconnected += nuiApp_OnSensorDisconnected;
try
{
nuiApp.Start();
}
// when exception catched. it means OpenNUI dosen't installed
catch
{
//show error messagebox
MessageBox.Show("OpenNUI dosen't installed", "error", MessageBoxButton.OK, MessageBoxImage.Error);
//program exit
Environment.Exit(0);
}
// worker for openNUIFrameTimer
act = new Action(delegate()
{
//do not work when usesensor is null
if (useSensor == null)
{
return;
}
//get the depth frame
DepthData depthFrame = useSensor.GetDepthFrame();
//do not work when depth frame is null (faild to GetDepthFrame())
if (depthFrame == null)
{
return;
}
for (int i = 0; i < depthFrame.FrameData.Length; ++i)
{
// Get the depth for this pixel
ushort depth = depthFrame.FrameData[i];
// To convert to a byte, we're mapping the depth value to the byte range.
// Values outside the reliable depth range are mapped to 0 (black).
ushort minDepth = 0;
ushort maxDepth = ushort.MaxValue;
this.depthPixels[i] = (byte)(depth >= minDepth && depth <= maxDepth ? (depth / MapDepthToByte) : 0);
}
bitmap.WritePixels(
new Int32Rect(0, 0, bitmap.PixelWidth, bitmap.PixelHeight),
this.depthPixels,
bitmap.PixelWidth,
0);
depthImage.Source = bitmap;
});
// create openNUIFrameTimer instance
openNUIFrameTimer = new System.Timers.Timer();
//set the timer interval to 60fps
openNUIFrameTimer.Interval = 1000 / 60;
//set the timer elapsed callback
openNUIFrameTimer.Elapsed += (object sender, System.Timers.ElapsedEventArgs e) =>
{
try
{
//use invoke for access to the this wpf window's ui
Dispatcher.Invoke(System.Windows.Threading.DispatcherPriority.Render, act);
}
catch { }
};
//set the timer autoreset to true for loop
openNUIFrameTimer.AutoReset = true;
//openNUIFrameTimer start
openNUIFrameTimer.Start();
// initialize the components (controls) of the window
InitializeComponent();
}