Frame ApplyFilter(DepthFrame depth, FrameSource frameSource)
{
using (var p = depth.Profile)
{
var count = depth.Width * depth.Height;
if (depthData == null || depthData.Length != count)
{
depthData = new ushort[count];
}
depth.CopyTo(depthData);
for (int i = 0; i < count; i++)
{
if (depthData[i] > Distance)
{
depthData[i] = 0;
}
}
var v = frameSource.AllocateVideoFrame <DepthFrame>(p, depth, depth.BitsPerPixel, depth.Width, depth.Height, depth.Stride, Extension.DepthFrame);
v.CopyFrom(depthData);
return(v);
}
}
private void wallDistanceCalibration(DepthFrame depthFrame)
{
try
{
ushort[] depthCurrent = new ushort[depthFrame.Width * depthFrame.Height]; //Assign each index of the depth frame to the matrix
depthFrame.CopyTo(depthCurrent);
if (calibrationCounter < 100) //the first 100 frames are used for calibration
{
calibrationCounter++;
Dispatcher.Invoke(new Action(() =>
{
txtCalibrationStatus.Text = "Calibration : Calibrating.. %" + calibrationCounter.ToString();
}));
int x, y;
for (int i = 0; i < depthCurrent.Length; i++) //find the x and y values while returning for each index and add to the sum if it is in roi
{
x = i % depthFrame.Width;
y = Convert.ToInt32(Math.Floor(Convert.ToDouble(i / depthFrame.Width)));
if (x < roiFrame.minX || x > roiFrame.maxX || y < roiFrame.minY || y > roiFrame.maxY)
{
depthCurrent[i] = 0;
}
else
{
depthFrameSum[y] += depthCurrent[i];
}
}
if (calibrationCounter == 99)
{
for (int i = 0; i < depthFrameSum.Length; i++)
{
depthFrameAvarage[i] = (ushort)(depthFrameSum[i] / ((roiFrame.maxX - roiFrame.minX) * calibrationCounter));
}
Dispatcher.Invoke(new Action(() =>
{
txtCalibrationStatus.Text = "Calibration : Calibration Completed.";
}));
wallDistanceIsCalibrated = true;
depthFrameResult = depthFrameAvarage; //assign values to the result array
xmlUpdate();
}
}
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
}
}
/// <summary>
/// 將DepthFrame格式轉換為1維 ushort 陣列 [height*width]
/// </summary>
internal ushort[] FrameTo_1D_ushort(DepthFrame depFrame)
{
if (depFrame.Width == 0)
{
return(null);
}
ushort[] data = new ushort[depFrame.Width * depFrame.Height];
depFrame.CopyTo(data);
return(data);
}
private Point3D findObjectLocation(DepthFrame depthFrame)
{
try
{
ushort[] depthCurrent = new ushort[depthFrame.Width * depthFrame.Height];
depthFrame.CopyTo(depthCurrent);
Point3D tempLocation = new Point3D();
ushort objectSize = Convert.ToUInt16((int)sldBallSize.Value);
if (isCalibrated)
{
double sumDistance = 0;
bool isCatched = false;
for (int i = 0; i < depthCurrent.Length - objectSize; i++)
{
isCatched = false;
sumDistance = 0;
for (int j = 0; j < objectSize; j++)
{
sumDistance += depthCurrent[i + j];
if (depthCurrent[i + j] == 0)
{
isCatched = true;
break;
}
}
if ((!isCatched) && (depthFrameResult[(int)Math.Floor((decimal)(i / (depthFrame.Width)))] - (sldWallSense.Value + sldWallPerception.Value)) < (double)(sumDistance / objectSize))
{
tempLocation.Y = (double)(Math.Floor((decimal)(i / (depthFrame.Width))));
tempLocation.X = i % (depthFrame.Width);
tempLocation.Z = (double)(sumDistance / objectSize);
return(tempLocation);
}
}
}
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
}
return(objectLocation);
}
/// <summary>
/// 將DepthFrame格式轉換為2維 ushort 陣列 [height, width, 1]
/// </summary>
internal ushort[,] FrameTo_2D_ushort(DepthFrame depFrame)
{
if (depFrame.Width == 0)
{
return(null);
}
ushort[] data = new ushort[depFrame.Width * depFrame.Height];
depFrame.CopyTo(data);
ushort[,] bmp = new ushort[depFrame.Height, depFrame.Width];
for (int x = 0; x < depFrame.Height; x++)
{
for (int y = 0; y < depFrame.Width; y++)
{
bmp[x, y] = (ushort)(data[x * depFrame.Width + y]); // data是一維的，bmp二維
}
}
return(bmp);
}
private void WaitForFrames()
{
while (!_streamingEvent.WaitOne(0))
{
using (FrameSet set = _pipeline.WaitForFrames())
{
_frameData.Timestamp = DateTime.Now;
using (VideoFrame colorFrame = set.ColorFrame)
{
colorFrame.CopyTo(_frameData.ColorData);
}
using (FrameSet processed = _aligner.Process(set))
using (DepthFrame depthFrame = processed.DepthFrame)
{
depthFrame.CopyTo(_frameData.DepthData);
}
OnFrameDataArrived?.Invoke(_frameData);
}
}
}
private DepthFrame calcWallSensitive(DepthFrame depthFrame)
{
try
{
/*check each index of the received depth frame and
* pull the image forward to the given wall sensitivity.
* In this way, both noise will be removed.*/
ushort[] depthCurrent = new ushort[depthFrame.Width * depthFrame.Height];
depthFrame.CopyTo(depthCurrent);
int x, y;
for (int i = 0; i < depthCurrent.Length; i++)
{
x = i % depthFrame.Width;
y = Convert.ToInt32(Math.Floor(Convert.ToDouble(i / depthFrame.Width)));
if (depthCurrent[i] < 100 || depthCurrent[i] > (depthFrameResult[y] - (int)sldWallSense.Value))
{
depthCurrent[i] = 0;
}
if (x < roiFrame.minX || x > roiFrame.maxX || y < roiFrame.minY || y > roiFrame.maxY)
{
depthCurrent[i] = 0;
}
}
depthFrame.CopyFrom(depthCurrent);
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
}
return(depthFrame);
}
int avgPivot = 0; //現在張數
Mat RS_depth(DepthFrame DF)
{
Size RS_depthSize = new Size(1280, 720);
int SizeOfDepth = 1280 * 720;
Mat rtn = new Mat(RS_depthSize, DepthType.Cv8U, 1);
ushort[,] udepth = new ushort[SizeOfDepth, avgNum];
ushort[] ubuffer = new ushort[SizeOfDepth];
byte[] bdepth = new byte[SizeOfDepth];
//copy to buffer
DF.CopyTo(ubuffer);
for (int i = 0; i < SizeOfDepth; i++)
{
udepth[i, avgPivot] = ubuffer[i];
}
float max = 1000;
for (int i = 0; i < SizeOfDepth; i++)
{
ushort sum = 0;
int C = 0;
//---加總 所有這個pixel的buffer值
if (avgPivot < avgNum) //如果 現有張數不夠，則先直接平均
{
for (int n = 0; n <= avgPivot; n++) //pivot 是 index 所以要<=
{
if (udepth[i, n] > 0) //有值 才算 沒值跳過
{
sum += udepth[i, n];
C++;
}
}
}
else
{
for (int n = 0; n < avgNum; n++) //總共avgNum張
{
if (udepth[i, n] > 0) //有值 才算 沒值跳過
{
sum += udepth[i, n];
C++;
}
}
}
//---計算平均
if (C == 0)//全部都沒值 (不然除法會算錯)
{
ubuffer[i] = 0;
}
else//如果有值 就平均
{
ubuffer[i] = (ushort)(sum / C);
}
//---將值map成byte
if (ubuffer[i] > max)
{
bdepth[i] = 0;
}
else
{
bdepth[i] = (byte)(ubuffer[i] * (255.0f / max));
}
}//each pixel
avgPivot++;
if (avgPivot == avgNum)
{
avgPivot = 0;
}
Marshal.Copy(bdepth, 0, rtn.DataPointer, SizeOfDepth);
return(rtn);
}
