void TCP_evalue(float DH[6], vector Point, double T_tool[], double Error[3])
{
vector<double> Pointtemp;
int pointNum = Point.size() / 6;
int Comb_no = 0;
int Comb_num = Combination(pointNum, 4);
int Comb_array[Comb_num][4] = {{0}};
double Comb_error[Comb_num] = {0};
int Comb_error_min_no = 0;
double Comb_error_min = 100000000000;
// 从点集中抽出四个点
for (int i = 0; i < pointNum - 3; i++)
{
for (int j = i + 1; j < pointNum - 2; j++)
{
for (int k = j + 1; k < pointNum - 1; k++)
{
for (int l = k + 1; l < pointNum; l++)
{
Comb_array[Comb_no][0] = i;
Comb_array[Comb_no][1] = j;
Comb_array[Comb_no][2] = k;
Comb_array[Comb_no][3] = l;
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * i, Point.begin() + 6 * (i + 1));
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * j, Point.begin() + 6 * (j + 1));
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * k, Point.begin() + 6 * (k + 1));
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * l, Point.begin() + 6 * (l + 1));
Comb_error[Comb_no] = error(Pointtemp);
TcpCalibration(DH, Pointtemp, T_tool, Error);
Comb_error[Comb_no] = Error[0];
if (Comb_error[Comb_no] < Comb_error_min)
{
Comb_error_min = Comb_error[Comb_no];
Comb_error_min_no = Comb_no;
}
Comb_no++;
Pointtemp.clear();
}
}
}
}
// 给出误差较小的标定点
Comb_no = Comb_error_min_no;
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * Comb_array[Comb_no][0], Point.begin() + 6 * (Comb_array[Comb_no][0] + 1));
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * Comb_array[Comb_no][1], Point.begin() + 6 * (Comb_array[Comb_no][1] + 1));
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * Comb_array[Comb_no][2], Point.begin() + 6 * (Comb_array[Comb_no][2] + 1));
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * Comb_array[Comb_no][3], Point.begin() + 6 * (Comb_array[Comb_no][3] + 1));
TcpCalibration(DH, Pointtemp, T_tool, Error);
}