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!";
            }
        }