public FxKalman1D()
{
m_x = new FxMatrixF(1, 2);
m_p = new FxMatrixF(2, 2);
m_q = new FxMatrixF(2, 2);
m_r = 0.1f;
}
public Form1()
{
InitializeComponent();
resize = new FxVector2f(targetRes.x / (float)origRes.x, targetRes.y / (float)origRes.y);
// create a shop
shopPlan = FxMatrixF.Load("Katopsi.jpg", ColorSpace.Grayscale);
shopPlan = shopPlan.Resize(targetRes.x, targetRes.y);
shop = new Shop("Unisol", shopPlan);
// simulation = new SimulationSimple(shop, 10, simulationMethod1);
simulationMethod2_Setup();
simulation = new SimulationSimple(shop, 1, simulationMethod2);
// Defind entrance for the shop
shop.entrancePositionsDirection.Add(new Tuple<FxVector2f, FxVector2f>(new FxVector2f(1800 * resize.x, 500 * resize.y), new FxVector2f(-1, 0)));
// load a person image for the moving image
imPerson = FxMatrixF.Load("person.jpg", ColorSpace.Grayscale);
// imPerson.Exec(x => (x > 0.1) ? 0 : 1);
imPerson.MedianFilt();
imPersonMask = imPerson < 0.5f;
// Init the matrix to be HD
mat = new FxMatrixF(targetRes.x, targetRes.y);
mat.DrawMatrix(shopPlan, new FxVector2f(0, 0), new FxVector2f(mat.Width, mat.Height), FxMatrixF.DrawInterpolationMethod.NearestNeighbor);
// Add image element to canvas to showing the matrix
im = new ImageElement(mat);
canvas1.AddElement(im, true);
}
public FxKalman1D(float qx, float qv, float r, float pd, float ix)
{
m_x = new FxMatrixF(1, 2);
m_p = new FxMatrixF(2, 2);
m_q = new FxMatrixF(2, 2);
m_r = r;
this.Reset(qx, qv, r, pd, ix);
}
public Shop(String Name,
FxMatrixF shopPlan)
{
this.Name = Name;
listCamera = new List<SerialCamera>();
personList = new List<Person>();
entrancePositionsDirection = new List<Tuple<FxVector2f, FxVector2f>>();
this.ShopPlan = shopPlan;
// create a runtime state base on the size of the shop plan
ShopRuntimeState = new FxMatrixF(shopPlan.Width, shopPlan.Height, 0);
}
public FxBlobTracker(FxMatrixF firstFrame)
{
m = firstFrame.Copy();
s = new FxMatrixF(firstFrame.Width, firstFrame.Height,0.05f);
G = m != -1; /* force a mask with all 1 */
a = 0.005f;
G_small = new FxMatrixMask(G_small_width, G_small_height);
step_w = (int)Math.Ceiling(G.Width / (float)G_small_width);
step_h = (int)Math.Ceiling(G.Height / (float)G_small_height);
cG_thd = step_w * step_h / 2;
ListBlobs = new List<FxBlob>();
numProcessingFrames = 0;
}
public PeopleOverview(FxMatrixF im)
{
InitializeComponent();
// add building
ImageElement ie = new ImageElement(im, new ColorMap(ColorMapDefaults.Bones));
ie.Name = "Katopsi";
ie.lockMoving = true;
canvas1.AddElement(ie);
// add geometry plot
gpe = new GeometryPlotElement();
gpe.Name = "People";
gpe.lockMoving = true;
canvas1.AddElement(gpe);
}
public MainForm()
{
InitializeComponent();
katopsi = FxMatrixF.Load("Katopsi.jpg", FxMaths.Matrix.ColorSpace.Grayscale);
// init the console
UIConsole = new ConsoleOutput();
UIConsole.Show(dockPanel1, DockState.DockBottomAutoHide);
consoleOutputToolStripMenuItem.Checked = true;
// init the people over view
UIPeopleOverview = new PeopleOverview(katopsi);
UIPeopleOverview.Show(dockPanel1, DockState.Document);
peopleOverviewToolStripMenuItem.Checked = true;
// Init Serial debugiing
UISerialInput = new SerialInput();
UISerialInput.Show(dockPanel1, DockState.Document);
// Init serial Capture menu
UISerialCapture = new SerialCapture();
UISerialCapture.Show(dockPanel1, DockState.Document);
}
private void ellipseDetectionToolStripMenuItem_Click(object sender, EventArgs e)
{
OpenFileDialog ofd = new OpenFileDialog();
if(ofd.ShowDialog() == System.Windows.Forms.DialogResult.OK)
{
// load image
imMat = FxMatrixF.Load(ofd.FileName);
ieEllipseImage = new ImageElement(imMat, new ColorMap(ColorMapDefaults.Jet));
ieEllipseImage.lockMoving = true;
canvas_ellipse.AddElement(ieEllipseImage);
// add plot element
gpeEllipseImage = new GeometryPlotElement();
canvas_ellipse.AddElement(gpeEllipseImage);
var contours = new FxContour(imMat<0.2f);
WriteLine("Num Contours : " + contours.NumChains);
int i = 0;
float pe_pos_y = ieEllipseImage._Position.y;
foreach (var cont in contours.ChainList)
{
// draw the rectanges in main image
FxVector2f start = cont.RectStart ;
FxVector2f end = start + cont.RectSize;
FxMaths.Geometry.Rectangle r = new FxMaths.Geometry.Rectangle(start, end);
gpeEllipseImage.AddGeometry(r, false);
// draw the centroids
FxVector2f cen = cont.GetCentroid();
var l = new FxMaths.Geometry.Line(cen - new FxVector2f(0, 2), cen + new FxVector2f(0, 2));
l.LineWidth = 0.5f;
gpeEllipseImage.AddGeometry(l, false);
l = new FxMaths.Geometry.Line(cen - new FxVector2f(2, 0), cen + new FxVector2f(2, 0));
l.LineWidth = 0.5f;
gpeEllipseImage.AddGeometry(l, false);
// add the numbering of the contours
var t = new TextElement(i.ToString());
t._Position = cen;
t.FontColor = SharpDX.Color.Green;
t._TextFormat.fontSize = 16.0f;
canvas_ellipse.AddElement(t);
// show the chain vector in plot
{
FxVectorF vec_i = new FxVectorF(cont.Count);
FxVectorF vec_r = new FxVectorF(cont.Count);
vec_i[0] = cont[0].i;
vec_r[0] = cont[0].r;
for (int j = 1; j < cont.Count; j++)
{
vec_i[j] = vec_i[j - 1] + cont[j].i;
vec_r[j] = vec_r[j - 1] + cont[j].r;
}
// show the plot of this vector
var pe = new PloterElement(vec_i, PlotType.Lines, System.Drawing.Color.Blue);
pe._Position.x = ieEllipseImage._Position.x + ieEllipseImage.Size.x;
pe._Position.y = pe_pos_y;
pe.AddPlot(vec_r, PlotType.Lines, System.Drawing.Color.Red);
pe.CenterYOrigin();
pe.FitPlots();
canvas_ellipse.AddElement(pe);
// update the y of pe for the next one
pe_pos_y += pe.Size.y;
// debug the ellipse
for (int j = 0; j < cont.Count; j++)
{
imMat[(int)(vec_r[j] + cont.StartPoint.x), (int)(vec_i[j] + cont.StartPoint.y)] = 0.8f/contours.NumChains + 0.1f;
}
ieEllipseImage.UpdateInternalImage(imMat, new ColorMap(ColorMapDefaults.Jet));
}
i++;
}
canvas_ellipse.ReDraw();
}
}
private void peopleRefreshCB(PeopleSimulation ps)
{
/* now we must update People Overview */
if (UIPeopleOverview != null)
UIPeopleOverview.PeopleUpdate(ps.PersonList);
/* create a heet map */
if (refreshCount < 10)
{
if (refreshCount == 0)
{
heatMap = katopsi.Copy();
}
foreach(Person p in ps.PersonList)
{
heatMap.DrawCircle(p.Position, 10.0f, 0.5f);
}
}
else
{
refreshCount = 0;
}
refreshCount++;
}
public FxMatrixF ToFxMatrixF(int Width, int Height)
{
FxMatrixF result = new FxMatrixF(Width, Height);
int i = 0;
foreach(FxContourChain chain in ChainList)
{
//chain.Equalization(20);
i++;
FxVector2f prevPoint = chain.StartPoint;
foreach (FxComplexF c in chain.ListComplex)
{
if (prevPoint.x >= Width)
prevPoint.x = Width-1;
if (prevPoint.y >= Height)
prevPoint.y = Height-1;
result[(int)prevPoint.x, (int)prevPoint.y] = i;
// calc the new prev point
prevPoint.x += c.r;
prevPoint.y += c.i;
}
// Draw boundrary rect
result.DrawRect(chain.RectStart, chain.RectSize, i);
}
result.Divide(result.Max());
return result;
}
public FxContour(FxMatrixMask mask, int minPix = 0, int maxPix = int.MaxValue)
{
int maskSize = mask.Width * mask.Height;
var stack = new Stack<Tuple<int, int>>(1000);
var remainMask = mask.Copy();
var labelMap = new FxMatrixF(mask.Width, mask.Height);
ChainList = new List<FxContourChain>();
int labelCount = 1;
var maskG = mask.ToFxMatrixF().Gradient(FxMatrixF.GradientMethod.Roberts) > 0;
for (int i = 1; i < maskSize - 1; i++)
{
/* check if we have edge */
if (maskG[i])
{
int x;
int y = Math.DivRem(i, mask.Width, out x);
labelMap[x, y] = labelCount;
maskG[x, y] = false;
// create a new chain
FxContourChain newChain = new FxContourChain(x, y);
/* propacate the search in sub pixels */
Labeling_addStack(stack, x, y);
while (stack.Count > 0)
{
var dxy = stack.Pop();
x = dxy.Item1;
y = dxy.Item2;
if (x < 1 || (x >= mask.Width - 1) || y < 1 || (y >= mask.Height - 1))
continue;
if (maskG[x, y] && check(maskG, x, y))
{
// add the labeling
labelMap[x, y] = labelCount;
maskG[x, y] = false;
// add the new point to the chain
newChain.PushPoint(x, y);
Labeling_addStack(stack, x, y);
}
}
labelCount++;
// calc boundary rect
// add the new chain to the list
ChainList.Add(newChain);
}
}
// filter the chain based on min/max size
if (minPix != 0 || maxPix != int.MaxValue)
{
List<FxContourChain> tmpChainList = ChainList;
ChainList = new List<FxContourChain>();
foreach(FxContourChain c in tmpChainList)
{
if (c.Count > minPix && c.Count < maxPix)
ChainList.Add(c);
}
}
}
private void KinectForm_Load(object sender, EventArgs e)
{
// Look through all sensors and start the first connected one.
// This requires that a Kinect is connected at the time of app startup.
// To make your app robust against plug/unplug,
// it is recommended to use KinectSensorChooser provided in Microsoft.Kinect.Toolkit
foreach(var potentialSensor in KinectSensor.KinectSensors) {
if(potentialSensor.Status == KinectStatus.Connected) {
this.sensor = potentialSensor;
break;
}
}
if(null == this.sensor) {
this.Text = "No kinnect Ready";
return;
}
Size depthImageSize = GetImageSize(DepthFormat);
depthWidth = (int)depthImageSize.Width;
depthHeight = (int)depthImageSize.Height;
Size colorImageSize = GetImageSize(ColorFormat);
colorWidth = (int)colorImageSize.Width;
colorHeight = (int)colorImageSize.Height;
// Turn on the depth and color streams to receive frames
sensor.DepthStream.Enable(DepthFormat);
sensor.ColorStream.Enable(ColorFormat);
sensor.DepthStream.Range = DepthRange.Near;
frameDataLength = sensor.DepthStream.FramePixelDataLength;
sensor.AllFramesReady += sensor_AllFramesReady;
int depthImageArraySize = depthWidth * depthHeight;
int colorImageArraySize = colorWidth * colorHeight * sizeof(int);
// Create local color pixels buffer
colorImagePixels = new byte[colorImageArraySize];
// Create local depth pixels buffer
depthImagePixels = new DepthImagePixel[depthImageArraySize];
// Allocate the depth-color mapping points
colorCoordinates = new ColorImagePoint[depthImageArraySize];
// allocate the Matrix
colorImageMatrix = new FxMatrixF(colorWidth, colorHeight);
depthImageMatrix = new FxMatrixF(depthWidth, depthHeight);
depthImageMatrixAve = new FxMatrixF(depthWidth, depthHeight);
// create a new image element
colorImageElement = new ImageElement(colorImageMatrix);
depthImageElement = new ImageElement(depthImageMatrix);
depthImageElement.MouseClickEvent += depthImageElement_MouseClickEvent;
canvas1.AddElements(colorImageElement,false);
canvas1.AddElements(depthImageElement, false);
depthImageElement.Position = new FxMaths.Vector.FxVector2f(colorWidth, 0);
G = new FxMatrixMask(depthWidth, depthHeight, false);
s = new FxMatrixF(depthWidth, depthHeight, 1f);
m = new FxMatrixF(depthWidth, depthHeight, 1f);
// init the dispatcher class
dispatcher = Dispatcher.CurrentDispatcher;
// Start the sensor!
try {
this.sensor.Start();
} catch(IOException ex) {
// Device is in use
this.sensor = null;
this.Text = ex.Message;
return;
} catch(InvalidOperationException ex) {
// Device is not valid, not supported or hardware feature unavailable
this.sensor = null;
this.Text = ex.Message;
return;
}
// Initialize and start the FPS timer
this.fpsTimer = new DispatcherTimer();
this.fpsTimer.Tick += new EventHandler(this.FpsTimerTick);
this.fpsTimer.Interval = new TimeSpan(0, 0, 1);
this.fpsTimer.Start();
this.lastFPSTimestamp = DateTime.UtcNow;
}
private void ProcessMatrix()
{
try {
// proccessing ...
depthImageMatrixAve = a * depthImageMatrix + (1 - a) * depthImageMatrixAve;
var depth = depthImageMatrixAve / depthImageMatrixAve.Max();
//depthImageMatrix.Divide(depthImageMatrix.Max());
//depthImageMatrix.MultiplyPointwise(colorImageMatrix);
if(null == mapper) {
// Create a coordinate mapper
mapper = new CoordinateMapper(this.sensor);
}
//mapper.MapColorFrameToDepthFrame(ColorFormat, DepthFormat, depthImagePixels, depthImagePoints);
mapper.MapDepthFrameToColorFrame(DepthFormat, depthImagePixels, ColorFormat, colorCoordinates);
for(int i=0; i < depthWidth * depthHeight; i++) {
depth[i] *= colorImageMatrix[colorCoordinates[i].X, colorCoordinates[i].Y];
}
// m = a * depthImageMatrix + (1 - a) * m;
// Updating viewports...
colorImageElement.UpdateInternalImage(colorImageMatrix);
depthImageElement.UpdateInternalImage(depth, depthColorMap, true);
canvas1.ReDraw();
// The input frame was processed successfully, increase the processed frame count
++this.processedFrameCount;
} catch(InvalidOperationException ex) {
Console.WriteLine(ex.Message);
} finally {
this.processingFrame = false;
}
}
public void Process(FxMatrixF NextFrame)
{
// evrey 100 frames reset the s;
if (numProcessingFrames % 100 == 0)
{
s = new FxMatrixF(NextFrame.Width, NextFrame.Height, 0.05f);
}
int Width = NextFrame.Width;
int Height = NextFrame.Height;
Parallel.For(0, Height, (y) =>
{
var diff2 = 0f;
for (int x = 0; x < Width; x++)
{
m[x, y] = (1 - a) * m[x, y] + a * NextFrame[x, y];
diff2 = NextFrame[x, y] - m[x, y];
diff2 *= diff2;
s[x, y] = (1 - a) * s[x, y] + a * diff2;
G[x, y] = diff2 > s[x, y];
}
});
#if false
// create a resize value
G_small.SetValueFunc((x, y) =>
{
int sum = 0;
for (int ix = x * step_w; ix < x * step_w + step_w; ix++)
{
for (int iy = y * step_h; iy < y * step_h + step_h; iy++)
{
sum += G[ix, iy] ? 1 : 0;
if (sum >= cG_thd)
return true;
}
}
return false;
});
Process(G_small);
#else
Process(G);
#endif
}
private void Read()
{
int count = 0;
Byte[] buffer = new Byte[130];
int numBytes = (isColor) ? 130 : 10;
Byte[,] imageBytes = new Byte[130, numBytes];
int row_id = 0;
FxBlobTracker fxtracker = new FxBlobTracker(imageMask.ToFxMatrixF());
FxMatrixF image = new FxMatrixF(64,64);
while (_continue)
{
try
{
// Read one row
row_id = readRow(buffer, numBytes) - 32;
// save the row
if (row_id >= 0 &&
row_id < 256)
{
for (int i = 0; i < numBytes; i++)
imageBytes[row_id, i] = buffer[i];
}
// Show results
if (isColor)
{
if (row_id == 0)
{
for (int i = 0; i < 64; i++)
{
for (int j = 0; j < 64; j++)
{
byte c = imageBytes[i, j * 2];
byte d = imageBytes[i, j * 2 + 1];
byte r = (byte)(c & 0xF8);
byte g = (byte)(((c & 0x07) << 5) + ((d & 0xE0) >> 3));
byte b = (byte)((d & 0x1F) << 3);
// Select the bit
if (false)
{
// RGB332
image[j, i] = ((r & 0xE0) |
((g >> 3) & 0x1C) |
((b >> 6) & 0x03)
) / 255.0f;
}
else
{
image[j, i] = ((b * 0.3f) + (g * 0.59f) + (r * 0.11f)) / 255.0f;
if (image[j, i] > 1.0f)
image[j, i] = 1.0f;
}
}
}
// Update the show image
imageMaskView.UpdateInternalImage(image, imageMaskColorMap);
/* refresh images */
fpsCount++;
canvas1.ReDraw();
}
}
else
{
if (row_id == 63)
{
//Console.WriteLine("Read Image");
for (int i = 0; i < 64; i++)
{
int bindex = 0;
for (int j = 0; j < 64; j++)
{
byte b = imageBytes[i, bindex];
// Select the bit
imageMask[j, i] = ((b & (1 << 7 - j % 8)) > 0);
// Move to the next byte
if (j % 8 == 7)
bindex++;
}
}
/* process the new matrix */
//fxtracker.Process(imageMask.ToFxMatrixF());
fxtracker.Process(imageMask);
image = imageMask.ToFxMatrixF();
var blobs = new FxContour(fxtracker.G_small);
var result = blobs.ToFxMatrixF(64, 64);
foreach (FxBlob b in fxtracker.ListBlobs)
{
result.DrawCircle(b.Center, b.Radius, 0.5f);
image.DrawCircle(b.Center, b.Radius, 0.5f);
}
// Update the show image
imageMaskView.UpdateInternalImage(image, imageMaskColorMap);
imageView.UpdateInternalImage(result, imageMaskColorMap);
/* refresh images */
fpsCount++;
canvas1.ReDraw();
}
}
}
catch (Exception ex) { Console.WriteLine(ex.Message); }
}
Console.WriteLine(count);
}
/// <summary>
/// Update the state by measurement m at dt time from last measurement.
/// </summary>
/// <param name="mx">The next position.</param>
/// <param name="mv">The next velocity.</param>
/// <param name="dt">The time interval.</param>
/// <returns></returns>
public float Update(float mx, float mv, float dt)
{
// Predict to now, then update.
// Predict:
// X = F*X + H*U
// P = F*P*F^T + Q.
// Update:
// Y = M – H*X Called the innovation = measurement – state transformed by H.
// S = H*P*H^T + R S= Residual covariance = covariane transformed by H + R
// K = P * H^T *S^-1 K = Kalman gain = variance / residual covariance.
// X = X + K*Y Update with gain the new measurement
// P = (I – K * H) * P Update covariance to this time.
//
// Same as 1D but mv is used instead of delta m_x[0], and H = [1,1].
// X = F*X + H*U
FxMatrixF f = new FxMatrixF(2, 2) { Data = new float[] { 1, dt, 0, 1 } };
// U = {0,0};
m_x = f.Multiply(m_x) as FxMatrixF;
// P = F*P*F^T + Q
m_p = f.MultiplyABAT(m_p) as FxMatrixF;
m_p.Add(m_q);
// Y = M – H*X
FxMatrixF y = new FxMatrixF(1, 2) { Data = new float[] { mx - m_x[0], mv - m_x[1] } };
// S = H*P*H^T + R
FxMatrixF s = m_p.Copy();
s[0] += m_r;
s[3] += m_r * 0.1f;
// K = P * H^T *S^-1
FxMatrixF sinv = s.Inverse() as FxMatrixF;
FxMatrixF k = new FxMatrixF(2, 2, 0f); // inited to zero.
if (sinv as Object != null)
{
k = m_p.Multiply(sinv) as FxMatrixF;
}
// X = X + K*Y
m_x.Add(k.Multiply(y));
// P = (I – K * H) * P
FxMatrixF id = new FxMatrixF(2, 2) { Data = new float[] { 1, 0, 0, 1 } };
id.Subtract(k);
id.Multiply(m_p);
m_p = id;
// return latest estimate
return m_x[0];
}
/// <summary>
/// Draw external matrix to this matrix.
/// </summary>
/// <param name="matrix">The matrix that store the copied image</param>
/// <param name="start"></param>
/// <param name="size"></param>
public void DrawMatrix(FxMatrixF matrix, FxVector2f start, FxVector2f size, DrawInterpolationMethod method)
{
int h = matrix.Height;
int w = matrix.Width;
float stepW = (float)(w-1) / size.x;
float stepH = (float)(h-1) / size.y;
start.x = (float)Math.Floor(start.x);
start.y = (float)Math.Floor(start.y);
FxVector2f end = start + size;
if (end.x > Width)
end.x = Width - 1;
if (end.y > Height)
end.y = Height - 1;
switch (method)
{
case DrawInterpolationMethod.NearestNeighbor:
Parallel.For((int)start.y, (int)end.y, (j) =>
{
int yp = (int)Math.Floor((j - start.y) * stepH);
for (int i = (int)start.x, ix = 0; i < end.x; i++, ix++)
{
int xp = (int)Math.Floor(ix * stepW);
this[i, j] = matrix[xp, yp];
}
});
break;
case DrawInterpolationMethod.Linear:
Parallel.For((int)start.y, (int)end.y, (j) =>
{
float yp = (j - start.y) * stepH;
for (int i = (int)start.x, ix = 0; i < end.x; i++, ix++)
{
float xp = ix * stepW;
if (xp < matrix.Width - 1)
this[i, j] = matrix.Sample(xp, yp);
}
});
break;
case DrawInterpolationMethod.Cubic:
Parallel.For((int)start.y, (int)end.y, (j) =>
{
float yp = (j - start.y) * stepH;
for (int i = (int)start.x, ix = 0; i < end.x; i++, ix++)
{
float xp = ix * stepW;
if (xp < matrix.Width - 1)
this[i, j] = matrix.SampleCubic(xp, yp);
}
});
break;
}
}
private void toolStripButton_ellipse_extract_Click(object sender, EventArgs e)
{
// extract the rectangle that contain
if ((imMat as object != null) &&
(rect != null))
{
int numLabels;
// get the sub matrix
var subMat = imMat.GetSubMatrix(rect.StartPoint, rect.EndPoint) as FxMatrixF;
// make binary the image based on the start and end point of rectangle
float t = subMat[0, 0];
if (t > subMat[subMat.Width - 1, subMat.Height - 1])
t = subMat[subMat.Width - 1, subMat.Height - 1];
var binMat = subMat < t - 0.02f;
// find the labels of the image
var labels = binMat.Labeling(out numLabels);
var imSub = new ImageElement(binMat.ToFxMatrixF(), new ColorMap(ColorMapDefaults.Jet));
imSub._Position.x = ieEllipseImage.Size.x + ieEllipseImage.Position.x;
imSub.lockMoving = true;
canvas_ellipse.AddElement(imSub, false, false);
var imSub2 = new ImageElement(labels, new ColorMap(ColorMapDefaults.Jet));
imSub2._Position.x = ieEllipseImage.Size.x + ieEllipseImage.Position.x;
imSub2._Position.y = imSub.Size.y + imSub.Position.y;
imSub2.lockMoving = true;
canvas_ellipse.AddElement(imSub2, false, false);
WriteLine("Num Labels : " + numLabels.ToString());
var contours = new FxContour(binMat, 10, 300);
WriteLine("Num Contours : " + contours.NumChains);
results = new FxMatrixF(3, contours.NumChains);
int i=0;
foreach (var x in contours.ChainList)
{
float delta = 1;
// draw the rectanges in the sub image
FxVector2f start = x.RectStart + imSub2._Position + new FxVector2f(1, 1);
FxVector2f end = start + x.RectSize;
FxMaths.Geometry.Rectangle r = new FxMaths.Geometry.Rectangle(start, end);
gpeEllipseImage.AddGeometry(r, false);
// draw the rectanges in main image
start = x.RectStart + rect.StartPoint + new FxVector2f(-delta, -delta);
end = start + x.RectSize + new FxVector2f(2 * delta, 2 * delta);
r = new FxMaths.Geometry.Rectangle(start, end);
gpeEllipseImage.AddGeometry(r, false);
// draw the centroids
FxVector2f cen = x.GetCentroid() + rect.StartPoint;
var l = new FxMaths.Geometry.Line(cen - new FxVector2f(0, 2), cen + new FxVector2f(0, 2));
l.LineWidth = 0.5f;
gpeEllipseImage.AddGeometry(l, false);
l = new FxMaths.Geometry.Line(cen - new FxVector2f(2, 0), cen + new FxVector2f(2, 0));
l.LineWidth = 0.5f;
gpeEllipseImage.AddGeometry(l, false);
// calculate the depth
float[] depth = new float[4];
FxVector2f pos = x.RectStart + rect.StartPoint;
depth[0] = imMat[pos.x - delta, pos.y - delta];
depth[1] = imMat[pos.x + x.RectSize.x + delta, pos.y - delta];
depth[2] = imMat[pos.x - delta, pos.y + x.RectSize.y + delta];
depth[3] = imMat[pos.x + x.RectSize.x + delta, pos.y + x.RectSize.y + delta];
results[2, i] = (depth[0] + depth[1] + depth[2] + depth[3]) / 4.0f;
// save centroid
results[0, i] = cen.x;
results[1, i] = cen.y;
// print the centroid
WriteLine("[" + i.ToString() + "] Depth:" + results[2, i].ToString() + " Pixels:" + x.Count + " Centroid: " + cen.ToString());
i++;
// show the vector of one blob
if (i == 1)
{
FxVectorF vec_i = new FxVectorF(x.Count);
FxVectorF vec_r = new FxVectorF(x.Count);
vec_i[0] = x[0].i;
vec_r[0] = x[0].r;
for (int j = 1; i < x.Count; j++)
{
vec_i[j] = vec_i[j - 1] + x[j].i;
vec_r[j] = vec_r[j - 1] + x[j].r;
}
}
}
canvas_ellipse.ReDraw();
results.SaveCsv("ellipseResults.csv");
}
}
FxMatrixF results = null; // Results of centers
private void toolStripButton_EllipseOpenImage_Click(object sender, EventArgs e)
{
OpenFileDialog ofd = new OpenFileDialog();
if(ofd.ShowDialog() == System.Windows.Forms.DialogResult.OK)
{
imMat = FxMatrixF.LoadCsv(ofd.FileName);
ieEllipseImage = new ImageElement(imMat, new ColorMap(ColorMapDefaults.Jet));
ieEllipseImage.lockMoving = true;
canvas_ellipse.AddElement(ieEllipseImage);
}
}
private void DepthFrameHandling(MultiSourceFrame frame)
{
try
{
DepthFrame depthFrame = frame.DepthFrameReference.AcquireFrame();
DepthFrameReference frameReference = frame.DepthFrameReference;
if (depthFrame != null)
{
// DepthFrame is IDisposable
using (depthFrame)
{
FrameDescription frameDescription = depthFrame.FrameDescription;
#region FPS
this.framesSinceUpdate++;
// update status unless last message is sticky for a while
if (DateTime.Now >= nextStatusUpdate)
{
// calcuate fps based on last frame received
double fps = 0.0;
if (stopwatch.IsRunning)
{
stopwatch.Stop();
fps = framesSinceUpdate / stopwatch.Elapsed.TotalSeconds;
stopwatch.Reset();
}
nextStatusUpdate = DateTime.Now + TimeSpan.FromSeconds(1);
toolStripLabel_fps.Text = fps + " " + (frameReference.RelativeTime - this.startTime).ToString() + "mS";
}
if (!stopwatch.IsRunning)
{
framesSinceUpdate = 0;
stopwatch.Start();
}
#endregion
// verify data and write the new depth frame data to the display bitmap
if (((frameDescription.Width * frameDescription.Height) == frameData.Length))
{
// Copy the pixel data from the image to a temporary array
depthFrame.CopyFrameDataToArray(frameData);
coordinateMapper.MapDepthFrameToColorSpace(frameData, colorPoints);
// Get the min and max reliable depth for the current frame
ushort minDepth = depthFrame.DepthMinReliableDistance;
ushort maxDepth = depthFrame.DepthMaxReliableDistance;
float imaxDepth = 1.0f / maxDepth;
for (int i = 0; i < this.frameData.Length; ++i)
{
// Get the depth for this pixel
ushort depth = this.frameData[i];
// To convert to a byte, we're discarding the most-significant
// rather than least-significant bits.
// We're preserving detail, although the intensity will "wrap."
// Values outside the reliable depth range are mapped to 0 (black).
//pixels[i] = 1.0f - ((depth >= minDepth && depth <= maxDepth) ? depth : 0) * imaxDepth;
depthImageMatrix[i] = 1.0f - depth * imaxDepth;
}
depthImageMatrixAve.Multiply(0.85f);
depthImageMatrixAve += 0.15f*depthImageMatrix;
// Updating viewports...
depthImageElement.UpdateInternalImage(depthImageMatrix, depthColorMap, true);
colorImageElement.UpdateInternalImage(depthImageMatrixAve, depthColorMap, true);
canvas1.ReDraw();
}
}
}
}
catch (Exception) { }
}
private void toolStripButton1_Click(object sender, EventArgs e)
{
// create a stopwatch for FPS calculation
this.stopwatch = new Stopwatch();
// for Alpha, one sensor is supported
kinectSensor = KinectSensor.GetDefault();
if (kinectSensor != null)
{
// open sensor
kinectSensor.Open();
var frameDescription = kinectSensor.DepthFrameSource.FrameDescription;
// get the coordinate mapper
coordinateMapper = kinectSensor.CoordinateMapper;
// create a new particle depth infos
#region Point Cloud
#if false
List<FxVector3f> Points = new List<FxVector3f>();
List<FxVector3f> Colors = new List<FxVector3f>();
for (int x = 0; x < 512; x += 2)
{
for (int y = 0; y < 420; y += 2)
{
for (int z = 0; z < 1; z++)
{
FxVector3f p;
p.x = x * 0.1f;
p.z = y * 0.1f;
p.y = (float)Math.Log(p.x * p.x * p.x + p.z * p.z * p.z - 3 * p.x - 3 * p.z);
Points.Add(p);
Colors.Add(rand.NextFxVector3f());
}
}
}
PointCloud pc = new PointCloud(Points, Colors);
/// add the mesh to the engine mesh list
Engine.g_MeshManager.AddMesh(pc);
#endif
#endregion
// open the reader for the depth frames
reader = kinectSensor.OpenMultiSourceFrameReader(FrameSourceTypes.Depth | FrameSourceTypes.Color);
// allocate space to put the pixels being received and converted
frameData = new ushort[frameDescription.Width * frameDescription.Height];
depthImageMatrix = new FxMatrixF(frameDescription.Width, frameDescription.Height);
depthImageMatrixAve = depthImageMatrix.Copy();
colorPoints = new ColorSpacePoint[frameDescription.Width * frameDescription.Height];
reader.MultiSourceFrameArrived += reader_MultiSourceFrameArrived;
}
else
MessageBox.Show("Error: failed to open kinect sensor");
}
public KinectV2Form(Engine engine)
{
InitializeComponent();
#if false
// save localy the graphic engine
this.engine = engine;
// set the first viewport
RenderArea_Viewport = new GraphicsEngine.Core.Viewport(RenderArea.Width, RenderArea.Height, RenderArea.Handle, Format.R8G8B8A8_UNorm);
ViewportManager.AddViewport(RenderArea_Viewport);
// set the moving camera
Engine.g_MoveCamera = RenderArea_Viewport.m_Camera;
#endif
// allocate the Matrix
colorImageMatrix = new FxMatrixF(640, 480);
depthImageMatrix = new FxMatrixF(640, 480);
depthImageMatrixAve = new FxMatrixF(640, 480);
// create a new image element
colorImageElement = new ImageElement(colorImageMatrix);
depthImageElement = new ImageElement(depthImageMatrix);
canvas1.AddElement(colorImageElement, false);
canvas1.AddElement(depthImageElement, false);
}
