public void draw(List <Mat> corners, Points.Vertex[] vertices, ref Matrix4x4 transformationM, int width, float markerLength)
{
for (int i = 0; i < corners.Count; i++)
{
Mat points = corners[i];
Vector3[] _verts = new Vector3[4];
int[] triangles = { 0, 1, 2, 0, 2, 3 };
for (int j = 0; j < points.total(); j++)
{
//pixel coordinate (x, y) in (width, height)
double[] pointCoord = points.get(0, j);
Points.Vertex vert = vertices[width * (int)pointCoord[1] + (int)pointCoord[0]];
Vector4 camCoord = new Vector4(vert.x, vert.y * -1, vert.z, 1.0f);
camCoord = transformationM * camCoord;
Vector3 worldCoord = camCoord / camCoord.w;
_verts[j] = worldCoord;
}
Mesh mesh = new Mesh();
mesh.vertices = _verts;
mesh.triangles = triangles;
mesh.RecalculateNormals();
if (showMesh)
{
MeshFilter meshFilter = GetComponent <MeshFilter>();
meshFilter.mesh = mesh;
}
if (showBox)
{
GameObject coordObj = Resources.Load <GameObject>("Box");
coordObj.transform.localScale = Vector3.one * markerLength;
Vector3 norm = mesh.normals[0].normalized;
Vector3 center = mesh.bounds.center + (norm * markerLength / 2);
Quaternion q = Quaternion.FromToRotation(Vector3.up, norm);
Instantiate(coordObj, center, q, this.gameObject.transform);
}
}
}
private void OnFrame(Frame frame)
{
if (frame.Profile.Stream == Intel.RealSense.Stream.Depth)
{
var depthFrame = frame as DepthFrame;
if (depthFrame == null)
{
Debug.Log("Frame is not a depth frame");
return;
}
UpdateParticleParams(depthFrame.Width, depthFrame.Height, depthFrame.Profile.Format);
var points = pc.Calculate(frame);
Points.Vertex[] vertices = new Points.Vertex[points.Count];
points.CopyTo(vertices);
for (int index = 0; index < vertices.Length; index += skipParticles)
{
var v = vertices[index];
if (v.z > 0)
{
particles[index].position = new Vector3(v.x, v.y, v.z);
particles[index].startSize = pointsSize;
particles[index].startColor = gradient.Evaluate(v.z);
}
else
{
particles[index].position = new Vector3(0, 0, 0);
particles[index].startSize = (float)0.0;
particles[index].startColor = new Color32(0, 0, 0, 0);
}
}
}
else if (frame.Profile.Stream == Intel.RealSense.Stream.Color)
{
//pc.MapTexture(frame);
}
}
