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