private void GetPointcloud(Image colorImage, Image depth, Image xyzImage, robotState state) { flag = false; times++; //make sure this program has been processed once //give out the tcp value and the tranformation matrix if (state.cartesianInfo != null) { tcp = new Vector6 <double>(); ROT = new rot(); tcp = state.cartesianInfo.tcp; ROT.Rx = state.cartesianInfo.tcp.Rx; ROT.Ry = state.cartesianInfo.tcp.Ry; ROT.Rz = state.cartesianInfo.tcp.Rz; matrix_R = RobMath.RotVec2Matrix(ROT); matrix_T_tcp2base.SetSubMatrix(0, 3, 0, 3, matrix_R); matrix_T_tcp2base[3, 0] = 0; matrix_T_tcp2base[3, 1] = 0; matrix_T_tcp2base[3, 2] = 0; matrix_T_tcp2base[3, 3] = 1; matrix_T_tcp2base[0, 3] = tcp.X; matrix_T_tcp2base[1, 3] = tcp.Y; matrix_T_tcp2base[2, 3] = tcp.Z; matrix_T = (DenseMatrix)(matrix_T_tcp2base * matrix_T_tool2tcp); //info.Text = tcp.ToString(); //info.Text = matrix_T_tcp2base.ToString(); BGRA[] colorArray = colorImage.GetPixels <BGRA>().ToArray(); Short3[] xyzArray = xyzImage.GetPixels <Short3>().ToArray(); MathNet.Numerics.LinearAlgebra.Double.Matrix matrix_P1 = new DenseMatrix(4, 1); MathNet.Numerics.LinearAlgebra.Double.Matrix matrix_P0 = new DenseMatrix(4, 1); Vector3[] vertices = new Vector3[num]; Color32[] colors = new Color32[num]; //indices = new int[num]; //描画する点の配列番号を記録。(全ての点を描画) //for (int i = 0; i < num; i++) //{ // indices[i] = i; //} for (int i = 0; i < num; i++) { //頂点座標の代入 vertices[i].x = xyzArray[i].X * 0.001f; vertices[i].y = xyzArray[i].Y * 0.001f; vertices[i].z = xyzArray[i].Z * 0.001f; //色の代入 colors[i].b = colorArray[i].B; colors[i].g = colorArray[i].G; colors[i].r = colorArray[i].R; colors[i].a = 255; ////for each point-1 , use the T-matrix to tansform it to point-0 matrix_P1[0, 0] = vertices[i].x; matrix_P1[1, 0] = vertices[i].y; matrix_P1[2, 0] = vertices[i].z; matrix_P1[3, 0] = 1; matrix_P0 = (DenseMatrix)(matrix_T * matrix_P1); //according to the position of the arm to save the pointcloud data switch (position) { case 1: { vertices_0_b1[i].x = (float)matrix_P0[0, 0]; vertices_0_b1[i].y = (float)matrix_P0[1, 0]; vertices_0_b1[i].z = (float)matrix_P0[2, 0]; colors_0_b1[i] = colors[i]; } break; case 2: { vertices_0_b2[i].x = (float)matrix_P0[0, 0]; vertices_0_b2[i].y = (float)matrix_P0[1, 0]; vertices_0_b2[i].z = (float)matrix_P0[2, 0]; colors_0_b2[i] = colors[i]; } break; case 3: { vertices_0_b3[i].x = (float)matrix_P0[0, 0]; vertices_0_b3[i].y = (float)matrix_P0[1, 0]; vertices_0_b3[i].z = (float)matrix_P0[2, 0]; colors_0_b1[i] = colors[i]; colors_0_b3[i] = colors[i]; } break; case 4: { vertices_0_b4[i].x = (float)matrix_P0[0, 0]; vertices_0_b4[i].y = (float)matrix_P0[1, 0]; vertices_0_b4[i].z = (float)matrix_P0[2, 0]; colors_0_b4[i] = colors[i]; } break; case 5: { vertices_0_b5[i].x = (float)matrix_P0[0, 0]; vertices_0_b5[i].y = (float)matrix_P0[1, 0]; vertices_0_b5[i].z = (float)matrix_P0[2, 0]; colors_0_b5[i] = colors[i]; } break; } } info.Text = "times:" + times.ToString() + "," + "position:" + position.ToString() + "," + "scanning and processing done!"; } else { info.Text = "times:" + times.ToString() + "state.cartesianInfo is null!"; } }