public void SubscriptionHandler(sensor_msgs.PointCloud2 message) { NumberOfClouds = message.data.Length / message.point_step; // Cut the pointcloud to points if (NumberOfClouds > layer * MaxVerts) { NumberToGenerate = (NumberOfClouds - layer * MaxVerts) / MaxVerts + 1; layer += NumberToGenerate; createGO = true; } RgbPoint3[] Points = new RgbPoint3[NumberOfClouds]; byte[] byteSlice = new byte[message.point_step]; for (long i = 0; i < NumberOfClouds; i++) { Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step); Points[i] = new RgbPoint3(byteSlice, message.fields); } lastMessage = Points; vertices2d = new Vector3[layer][]; triangles2d = new int[layer][]; colors2d = new Color[layer][]; SetupMeshArrays(Points); updateMesh = true; }
public void SubscriptionHandler(sensor_msgs.PointCloud2 message) { long I = message.data.Length / message.point_step; RgbPoint3[] Points = new RgbPoint3[I]; byte[] byteSlice = new byte[message.point_step]; for (long i = 0; i < I; i++) { Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step); Points[i] = new RgbPoint3(byteSlice, message.fields); } newverts = new Vector3[I]; double[] y_array = new double[I]; //Assign all PointCloud points to the Vecto3[] for (var i = 0; i < I; i++) { newverts[i] = new Vector3(Points[i].x, Points[i].z, Points[i].y); y_array[i] = newverts[i].y; } if (I < Ik) { flag_object_init = true; } if (I > ((layer + 1) * Ik)) { if (flag == true) { layer++; flag = false; } if (I > (layer * Ik)) { Ic = (int)Iold; //layer*Ik; flag = true; flag_object = true; create_mesh = false; objcount++; } } CreateMesh(I, Ic, newverts); Iold = I; }
public void SubscriptionHandler(sensor_msgs.PointCloud2 message) { long I = message.data.Length / message.point_step; Debug.Log("Long I " + I); RgbPoint3[] Points = new RgbPoint3[I]; byte[] byteSlice = new byte[message.point_step]; for (long i = 0; i < I; i++) { Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step); Points[i] = new RgbPoint3(byteSlice, message.fields); } newverts = new Vector3[I]; double[] y_array = new double[I]; //Assign all PointCloud points to the Vecto3[] for (var i = 0; i < I; i++) { newverts[i] = new Vector3(Points[i].x, Points[i].z, Points[i].y); y_array[i] = newverts[i].y; VectorList.Add(newverts[i]); } float inc = 0.15f; //Assign all the vertices vertices = new Vector3[I * 8]; int vinc = 0; for (int k = 0; k < I; k++) { for (int i = 0; i < I * 8; i++) { if (i == 0 + vinc) { vertices[i] = new Vector3(newverts[k].x - inc, newverts[k].y - inc, newverts[k].z + inc); } else if (i == 1 + vinc) { vertices[i] = new Vector3(newverts[k].x - inc, newverts[k].y - inc, newverts[k].z - inc); } else if (i == 2 + vinc) { vertices[i] = new Vector3(newverts[k].x - inc, newverts[k].y + inc, newverts[k].z - inc); } else if (i == 3 + vinc) { vertices[i] = new Vector3(newverts[k].x - inc, newverts[k].y + inc, newverts[k].z + inc); } else if (i == 4 + vinc) { vertices[i] = new Vector3(newverts[k].x + inc, newverts[k].y + inc, newverts[k].z + inc); } else if (i == 5 + vinc) { vertices[i] = new Vector3(newverts[k].x + inc, newverts[k].y + inc, newverts[k].z - inc); } else if (i == 6 + vinc) { vertices[i] = new Vector3(newverts[k].x + inc, newverts[k].y - inc, newverts[k].z - inc); } else if (i == 7 + vinc) { vertices[i] = new Vector3(newverts[k].x + inc, newverts[k].y - inc, newverts[k].z + inc); } } vinc = vinc + 8; } int tris = 0; //36; int vert = 0; //8; triangles = new int[36 * I]; //Assign triangules of all the vertices for (int k = 0; k < I; k++) { for (int i = 0; i < 36; i++) { if (i == 0) { triangles[i + tris] = 0 + vert; } else if (i == 1) { triangles[i + tris] = 2 + vert; } else if (i == 2) { triangles[i + tris] = 1 + vert; } else if (i == 3) { triangles[i + tris] = 0 + vert; } else if (i == 4) { triangles[i + tris] = 3 + vert; } else if (i == 5) { triangles[i + tris] = 2 + vert; } else if (i == 6) { triangles[i + tris] = 2 + vert; } else if (i == 7) { triangles[i + tris] = 3 + vert; } else if (i == 8) { triangles[i + tris] = 4 + vert; } else if (i == 9) { triangles[i + tris] = 2 + vert; } else if (i == 10) { triangles[i + tris] = 4 + vert; } else if (i == 11) { triangles[i + tris] = 5 + vert; } else if (i == 12) { triangles[i + tris] = 1 + vert; } else if (i == 13) { triangles[i + tris] = 2 + vert; } else if (i == 14) { triangles[i + tris] = 5 + vert; } else if (i == 15) { triangles[i + tris] = 1 + vert; } else if (i == 16) { triangles[i + tris] = 5 + vert; } else if (i == 17) { triangles[i + tris] = 6 + vert; } else if (i == 18) { triangles[i + tris] = 0 + vert; } else if (i == 19) { triangles[i + tris] = 7 + vert; } else if (i == 20) { triangles[i + tris] = 4 + vert; } else if (i == 21) { triangles[i + tris] = 0 + vert; } else if (i == 22) { triangles[i + tris] = 4 + vert; } else if (i == 23) { triangles[i + tris] = 3 + vert; } else if (i == 24) { triangles[i + tris] = 5 + vert; } else if (i == 25) { triangles[i + tris] = 4 + vert; } else if (i == 26) { triangles[i + tris] = 7 + vert; } else if (i == 27) { triangles[i + tris] = 5 + vert; } else if (i == 28) { triangles[i + tris] = 7 + vert; } else if (i == 29) { triangles[i + tris] = 6 + vert; } else if (i == 30) { triangles[i + tris] = 0 + vert; } else if (i == 31) { triangles[i + tris] = 6 + vert; } else if (i == 32) { triangles[i + tris] = 7 + vert; } else if (i == 33) { triangles[i + tris] = 0 + vert; } else if (i == 34) { triangles[i + tris] = 1 + vert; } else if (i == 35) { triangles[i + tris] = 6 + vert; } } vert += 8; tris += 36; } colors = new Color[vertices.Length]; index = 0; ci = 0; //Arrange array [y] with no repeat values List <double> unique_y = new List <double>(); for (int i = 0; i < y_array.Length; i++) { bool found = false; for (int prev = 0; prev < i; prev++) { if (y_array[prev] == y_array[i]) { found = true; break; } } if (!found) { //unique_y.Add(Math.Round(y_array[i],2)); unique_y.Add(y_array[i]); } } //Debug.Log("Y_ARRAY " + y_array.Length); y_array = unique_y.ToArray(); //Color the octree cubes for (int i = 0; i < y_array.Length; i++) { for (int k = 0; k < I; k++) { if (y_array[i] == newverts[k].y) { ColorLayer(k, index); } } index++; if (index == 6) { index = 0; } } update_mesh = true; }
public void SubscriptionHandler(sensor_msgs.PointCloud2 message) { long NumberOfClouds = message.data.Length / message.point_step; // Cut the pointcloud to points Debug.Log(NumberOfClouds); if (NumberOfClouds > layer * MaxVerts) { layer++; createGO = true; } RgbPoint3[] Points = new RgbPoint3[NumberOfClouds]; byte[] byteSlice = new byte[message.point_step]; for (long i = 0; i < NumberOfClouds; i++) { Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step); Points[i] = new RgbPoint3(byteSlice, message.fields); } newverts = new Vector3[NumberOfClouds]; //Assign all PointCloud points to the Vector3[] for (var i = 0; i < NumberOfClouds; i++) { newverts[i] = new Vector3(Points[i].x, Points[i].z, Points[i].y); } vertices2d = new Vector3[layer][]; triangles2d = new int[layer][]; colors2d = new Color[layer][]; long arraySize; for (int l = 0; l < layer; l++) { if (l + 1 == layer) { arraySize = -(layer - 1) * MaxVerts + NumberOfClouds; } else { arraySize = MaxVerts; } float inc = 0.3f; int vinc = 0; int tris = 0; int vert = 0; int first = l * MaxVerts; int last = l * (int)MaxVerts + (int)arraySize; vertices2d[l] = new Vector3[arraySize * 8]; triangles2d[l] = new int[36 * (last - first)]; colors2d[l] = new Color[arraySize * 8]; for (int k = first; k < last; k++) { for (int i = 0; i < (last - first) * 8; i++) { if (i == 0 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x - inc, newverts[k].y - inc, newverts[k].z + inc); } else if (i == 1 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x - inc, newverts[k].y - inc, newverts[k].z - inc); } else if (i == 2 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x - inc, newverts[k].y + inc, newverts[k].z - inc); } else if (i == 3 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x - inc, newverts[k].y + inc, newverts[k].z + inc); } else if (i == 4 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x + inc, newverts[k].y + inc, newverts[k].z + inc); } else if (i == 5 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x + inc, newverts[k].y + inc, newverts[k].z - inc); } else if (i == 6 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x + inc, newverts[k].y - inc, newverts[k].z - inc); } else if (i == 7 + vinc) { vertices2d[l][i] = new Vector3(newverts[k].x + inc, newverts[k].y - inc, newverts[k].z + inc); } } vinc = vinc + 8; for (int i = 0; i < 36; i++) { if (i == 0) { triangles2d[l][i + tris] = 0 + vert; } else if (i == 1) { triangles2d[l][i + tris] = 2 + vert; } else if (i == 2) { triangles2d[l][i + tris] = 1 + vert; } else if (i == 3) { triangles2d[l][i + tris] = 0 + vert; } else if (i == 4) { triangles2d[l][i + tris] = 3 + vert; } else if (i == 5) { triangles2d[l][i + tris] = 2 + vert; } else if (i == 6) { triangles2d[l][i + tris] = 2 + vert; } else if (i == 7) { triangles2d[l][i + tris] = 3 + vert; } else if (i == 8) { triangles2d[l][i + tris] = 4 + vert; } else if (i == 9) { triangles2d[l][i + tris] = 2 + vert; } else if (i == 10) { triangles2d[l][i + tris] = 4 + vert; } else if (i == 11) { triangles2d[l][i + tris] = 5 + vert; } else if (i == 12) { triangles2d[l][i + tris] = 1 + vert; } else if (i == 13) { triangles2d[l][i + tris] = 2 + vert; } else if (i == 14) { triangles2d[l][i + tris] = 5 + vert; } else if (i == 15) { triangles2d[l][i + tris] = 1 + vert; } else if (i == 16) { triangles2d[l][i + tris] = 5 + vert; } else if (i == 17) { triangles2d[l][i + tris] = 6 + vert; } else if (i == 18) { triangles2d[l][i + tris] = 0 + vert; } else if (i == 19) { triangles2d[l][i + tris] = 7 + vert; } else if (i == 20) { triangles2d[l][i + tris] = 4 + vert; } else if (i == 21) { triangles2d[l][i + tris] = 0 + vert; } else if (i == 22) { triangles2d[l][i + tris] = 4 + vert; } else if (i == 23) { triangles2d[l][i + tris] = 3 + vert; } else if (i == 24) { triangles2d[l][i + tris] = 5 + vert; } else if (i == 25) { triangles2d[l][i + tris] = 4 + vert; } else if (i == 26) { triangles2d[l][i + tris] = 7 + vert; } else if (i == 27) { triangles2d[l][i + tris] = 5 + vert; } else if (i == 28) { triangles2d[l][i + tris] = 7 + vert; } else if (i == 29) { triangles2d[l][i + tris] = 6 + vert; } else if (i == 30) { triangles2d[l][i + tris] = 0 + vert; } else if (i == 31) { triangles2d[l][i + tris] = 6 + vert; } else if (i == 32) { triangles2d[l][i + tris] = 7 + vert; } else if (i == 33) { triangles2d[l][i + tris] = 0 + vert; } else if (i == 34) { triangles2d[l][i + tris] = 1 + vert; } else if (i == 35) { triangles2d[l][i + tris] = 6 + vert; } } vert += 8; tris += 36; for (int i = 0; i < 8; i++) { ci = i + (8 * (k - first)); colors2d[l][ci] = new Color32((byte)Points[k].rgb[0], (byte)Points[k].rgb[1], (byte)Points[k].rgb[2], 255); } } } updateMesh = true; }