public static unsafe SpreadBuilder <Vector3> CollectPoints(
SpreadBuilder <Vector3> builder,
DepthImage image,
float minZ,
float maxZ,
int decimation = 2)
{
if (builder is null)
{
throw new ArgumentNullException(nameof(builder));
}
if (image is null)
{
throw new ArgumentNullException(nameof(image));
}
var frame = image.frame;
var pixelCount = frame.FrameDescription.LengthInPixels;
using (var depthBuffer = frame.LockImageBuffer())
using (var cameraSpacePoints = MemoryPool <CameraSpacePoint> .Shared.Rent((int)pixelCount))
{
using (var cameraSpacePointsHandle = cameraSpacePoints.Memory.Pin())
{
var cameraSpacePointsPtr = new IntPtr(cameraSpacePointsHandle.Pointer);
frame.DepthFrameSource.KinectSensor.CoordinateMapper.MapDepthFrameToCameraSpaceUsingIntPtr(
depthBuffer.UnderlyingBuffer, depthBuffer.Size,
cameraSpacePointsPtr, (uint)(pixelCount * sizeof(CameraSpacePoint)));
}
var step = Math.Max(1, decimation);
var width = image.Info.Width;
var height = image.Info.Height;
builder.Clear();
// The memory we got from the pool might be bigger than what we need. So let's slice it down to our expected size so we get index out of bounds checks.
var csps = cameraSpacePoints.Memory.Span.Slice(0, (int)pixelCount);
for (int y = 0; y < height; y += step)
{
var r = y * width;
for (int x = 0; x < width; x += step)
{
var csp = csps[r + x];
if (csp.Z > minZ && csp.Z < maxZ)
{
builder.Add(new Vector3(csp.X, csp.Y, csp.Z));
}
}
}
return(builder);
}
}
public static SpreadBuilder <ColoredPoint> AddPoints(
SpreadBuilder <ColoredPoint> builder,
DepthSensor sensor,
ColorFrame colorFrame,
DepthFrame depthFrame,
int minZ = 100,
int maxZ = ushort.MaxValue,
int decimation = 1)
{
if (builder is null)
{
throw new ArgumentNullException(nameof(builder));
}
if (sensor is null)
{
throw new ArgumentNullException(nameof(sensor));
}
if (colorFrame is null)
{
throw new ArgumentNullException(nameof(colorFrame));
}
if (depthFrame is null)
{
throw new ArgumentNullException(nameof(depthFrame));
}
if (colorFrame.Cols < depthFrame.Cols || colorFrame.Rows < depthFrame.Rows)
{
throw new ArgumentException("The color image must have at least the same size as the depth image.");
}
var width = depthFrame.Cols;
var height = depthFrame.Rows;
var strideX = colorFrame.Cols / depthFrame.Cols;
var strideY = colorFrame.Rows / depthFrame.Rows;
var step = Math.Max(1, decimation);
for (var y = 0; y < height; y += step)
{
for (var x = 0; x < width; x += step)
{
var z = depthFrame[y, x];
if (z >= minZ && z <= maxZ)
{
var c = colorFrame[y * strideY, x *strideX];
var p = sensor.ConvertProjToRealCoords(x, y, z);
builder.Add(new ColoredPoint(AsVector3(ref p), ToColor(in c)));
}
}
}
return(builder);
}
public FftProcessor(int fftLength)
{
this.fftLength = fftLength;
m = (int)Math.Log(fftLength, 2.0);
input = new Complex[fftLength];
input2 = new Complex[fftLength];
output = new Complex[fftLength];
pinIn = new PinnedArray <Complex>(input);
pinIn2 = new PinnedArray <Complex>(input2);
pinOut = new PinnedArray <Complex>(output);
fftPos = 0;
fftPos2 = fftLength / 2;
spreadBuilder = new SpreadBuilder <float>(fftLength / 2);
}
public static Spread <Vector3> GetPointCloudData(this Image image, float scaling = 1f)
{
if (image.Format != ImageFormat.Custom)
{
throw new UnsupportedImageFormatException(image.Format);
}
var data = image.GetPixels <XYZ>();
var result = new SpreadBuilder <Vector3>(data.Length);
foreach (var pixel in data.Span)
{
result.Add(new Vector3(pixel.x * scaling, pixel.y * scaling, pixel.z * scaling));
}
return(result.ToSpread());
}
public Spread <String> GetDriverInfo()
{
FMOD.System coreSystem;
_system.getCoreSystem(out coreSystem);
int numDrivers;
int speakerModeChannels, systemRate;
SPEAKERMODE speakerMode;
Guid guid;
string name;
coreSystem.getNumDrivers(out numDrivers);
SpreadBuilder <string> sb = new SpreadBuilder <string>();
for (int i = 0; i < numDrivers; i++)
{
coreSystem.getDriverInfo(i, out name, 255, out guid, out systemRate, out speakerMode, out speakerModeChannels);
sb.Add(string.Format("{0} @ {1}, Channels: {2}, Mode: {3}", new object[] { name, systemRate, speakerModeChannels, speakerMode.ToString() }));
}
return(sb.ToSpread());
}
//static CoordinateMapper FCoordinateMapper = new CoordinateMapper();
public static unsafe Spread <Vector3> GetPoints(DepthImage image, float minZ, float maxZ, int decimation)
{
DepthFrame frame = image.frame;
var depthSize = frame.FrameDescription.LengthInPixels;
var depthData = new ushort[depthSize];
var cameraSpacePoints = new CameraSpacePoint[depthSize];
frame.CopyFrameDataToArray(depthData);
frame.DepthFrameSource.KinectSensor.CoordinateMapper.MapDepthFrameToCameraSpace(depthData, cameraSpacePoints);
var step = Math.Max(1, decimation + 1);
var width = image.Info.Width / step;
var wRemainder = image.Info.Width % step;
var height = image.Info.Height / step;
var count = width * height;
SpreadBuilder <Vector3> sb = new SpreadBuilder <Vector3>(count);
//using (var buffer = frame.LockImageBuffer())
{
//ushort* ptr = (ushort*)buffer.UnderlyingBuffer.ToPointer();
for (int row = 0; row < height; row++)
{
//var rowStart = ptr;
var r = row * step * image.Info.Width;
for (int col = 0; col < width; col++)
{
var csp = cameraSpacePoints[r + col * step];
if (csp.Z > minZ && csp.Z < maxZ)
{
sb.Add(new Vector3(csp.X, csp.Y, csp.Z));
}
//ptr += step;
}
//ptr += wRemainder;
//ptr += image.Info.Width * (step-1);
}
return(sb.ToSpread());
}
}
public static unsafe SpreadBuilder <int> GetValues(KinectBuffer buffer, SpreadBuilder <int> builder, int width, int height, Spread <Vector2> pixels)
{
builder.Clear();
short *data = (short *)buffer.UnderlyingBuffer;
int pixelX = 0;
int pixelY = 0;
foreach (var item in pixels)
{
pixelX = (int)item.X;
pixelY = (int)item.Y;
pixelX = pixelX < 0 ? 0 : pixelX;
pixelY = pixelY < 0 ? 0 : pixelY;
pixelX = pixelX > width - 1 ? width - 1 : pixelX;
pixelY = pixelY > height - 1 ? height - 1 : pixelY;
int pixel = data[pixelY * width + pixelX];
builder.Add(pixel);
}
return(builder);
}
private static string model = "yolov2-voc.weights"; //YOLOv2 544x544
public static YOLODescriptor Detect(Mat image, float threshold, bool enabled)
{
SpreadBuilder <Rect> rectSB = Spread.CreateBuilder <Rect>();
SpreadBuilder <float> confidenceSB = Spread.CreateBuilder <float>();
SpreadBuilder <int> detectedClassSB = Spread.CreateBuilder <int>();
SpreadBuilder <float> classProbabilitySB = Spread.CreateBuilder <float>();
SpreadBuilder <string> labelSB = Spread.CreateBuilder <string>();
if (enabled && image != DefaultMat.Damon)
{
var w = image.Width;
var h = image.Height;
//setting blob, parameter are important
var blob = CvDnn.BlobFromImage(image, 1 / 255.0, new Size(544, 544), new Scalar(), true, false);
if (net == null || net.IsDisposed)
{
net = CvDnn.ReadNetFromDarknet(cfg, model);
}
net.SetInput(blob, "data");
//forward model
var prob = net.Forward();
/* YOLO2 VOC output
* 0 1 : center 2 3 : w/h
* 4 : confidence 5 ~24 : class probability */
const int prefix = 5; //skip 0~4
for (int i = 0; i < prob.Rows; i++)
{
var matchConfidence = prob.At <float>(i, 4);
if (matchConfidence > threshold)
{
//get classes probability
Point min, max;
Cv2.MinMaxLoc(prob.Row[i].ColRange(prefix, prob.Cols), out min, out max);
var classes = max.X;
var probability = prob.At <float>(i, classes + prefix);
if (probability > threshold) //more accuracy
{
//get center and width/height
var centerX = prob.At <float>(i, 0) * w;
var centerY = prob.At <float>(i, 1) * h;
var width = prob.At <float>(i, 2) * w;
var height = prob.At <float>(i, 3) * h;
var x1 = (centerX - width / 2) < 0 ? 0 : centerX - width / 2; //avoid left side over edge
//org.Rectangle(new Point(centerX - width / 2, centerY - height / 2), new Point(centerX + width / 2, centerY + height / 2), Colors[classes], 2);
rectSB.Add(new Rect(new Point(x1, centerY - height / 2), new Size(width, height)));
confidenceSB.Add(matchConfidence);
detectedClassSB.Add(classes);
classProbabilitySB.Add(probability);
labelSB.Add(Labels[classes]);
}
}
}
}
YOLODescriptor result = new YOLODescriptor();
result.confidence = confidenceSB.ToSpread();
result.detectedClass = detectedClassSB.ToSpread();
result.classProbability = classProbabilitySB.ToSpread();
result.classLabel = labelSB.ToSpread();
result.rectangles = rectSB.ToSpread <Rect>();
return(result);
}
