private void setAngleRelativeToCameraForward(ref PlaneResult io_PlaneResult)
{
// angle relative to the real surface which is not true when the camera is at an angle
Vector3 n = io_PlaneResult.P.Normal;
io_PlaneResult.Angle = (float)(Math.Acos(Math.Abs(n.Z)) / Math.PI * 180);
var cam = new Vector3(0, 0, 1f);
var dot = Vector3.Dot(n, cam);
var u = cam - n * dot;
io_PlaneResult.AngleX = u.X;
io_PlaneResult.AngleY = u.Y;
}
private bool isPlaneValid(PlaneResult i_Plane)
{
Vector3[] p = { i_Plane.PlaneCorners[0], i_Plane.PlaneCorners[1], i_Plane.PlaneCorners[2], i_Plane.PlaneCorners[3] };
List <float> angles = new List <float>();
bool res = false;
for (int i = 0; i < p.Length; i++)
{
Vector3 p1 = p[i];
Vector3 p2 = p[(i + 1) % p.Length];
if ((p2 - p1).Length() < 1e-3)
{
break;
}
p1 = Vector3.Normalize(p1);
p2 = Vector3.Normalize(p2);
angles.Add((float)Math.Acos(Vector3.Dot(p1, p2) / Math.Sqrt(p1.Length() * p2.Length())));
}
int minuses = 0, pluses = 0;
for (int i = 0; i < angles.Count; i++)
{
if (angles[i] < 0)
{
minuses++;
}
else if (angles[i] > 0)
{
pluses++;
}
}
if (minuses == 4 || pluses == 4)
{
res = true;
}
return(res);
}
private void OnRes(PlaneResult i_Result)
{
MediatorResultFunc?.Invoke(i_Result);
}
private void AnalyzeDepthImage()
{
while (m_IsRunning)
{
if (m_Queue != null)
{
DepthFrame f;
if (m_Queue.Dequeue(out f))
{
using (f)
{
var o_Result = new PlaneResult();
o_Result.IsPlaneReconstructed = true;
var profile = f.GetProfile();
var streamType = profile.Stream;
var streamFormat = profile.Format;
if (streamType == Stream.Depth && streamFormat == Format.Z16)
{
m_Intrinsics = profile.GetIntrinsics();
m_Roi = setROI();
var data = f.Data;
int width = f.Width;
int height = f.Height;
var pixels = f.Pixels; //new short[width * height];
//System.Runtime.InteropServices.Marshal.Copy(data, pixels, 0, width * height);
List <Vector3> roiPixels = new List <Vector3>();
// converting pixels to point in 3d space
for (int y = m_Roi.MinY; y < m_Roi.MaxY; y++)
{
for (int x = m_Roi.MinX; x < m_Roi.MaxX; x++)
{
ushort depthRaw = (ushort)pixels[y * width + x];
// depthRaw if 0 means there's no depth, the pixel is at depth zero just like any other pixel in a photo
if (depthRaw != 0)
{
float[] pixel = { x, y };
Vector3 point = new Vector3();
var distance = depthRaw * m_Units;
projectPixelToPoint(ref point, m_Intrinsics, pixel, distance);
roiPixels.Add(new Vector3(point.X, -point.Y, point.Z));
}
}
}
if (roiPixels.Count < 3)
{
continue;
}
roiPixels.Reverse();
var plane = planeFromPoints(roiPixels);
// The points in RoI don't span a valid plane which is kind of useless
if (plane == new Plane(0, 0, 0, 0))
{
continue;
}
// Vector3 planeFitPivot = approximateIntersection(plane, m_Intrinsics.width / 2f, m_Intrinsics.height / 2f, 0f, 1000f);
Vector3[] planeCorners = new Vector3[4];
planeCorners[0] = approximateIntersection(plane, m_Roi.MinX, m_Roi.MinY, 0f, 1000f);
planeCorners[1] = approximateIntersection(plane, m_Roi.MaxX, m_Roi.MinY, 0f, 1000f);
planeCorners[2] = approximateIntersection(plane, m_Roi.MaxX, m_Roi.MaxY, 0f, 1000f);
planeCorners[3] = approximateIntersection(plane, m_Roi.MinX, m_Roi.MaxY, 0f, 1000f);
o_Result.P = plane;
o_Result.PlaneCorners = planeCorners;
if (!isPlaneValid(o_Result))
{
o_Result.PlaneCorners = null;
o_Result.IsPlaneReconstructed = false;
}
else
{
setAngleRelativeToCameraForward(ref o_Result);
}
// Resulting plance distance from the camera
o_Result.Distance = -plane.D * 1000;
OnRes?.Invoke(o_Result);
}
}
}
else
{
Thread.Sleep(4);
}
}
}
}
