dsgdfgdhfh

最新推荐文章于 2024-09-10 13:01:50 发布

Zenker0418

最新推荐文章于 2024-09-10 13:01:50 发布

阅读量30

点赞数

文章标签： python spring boot

本文链接：https://blog.csdn.net/Zenker0418/article/details/134401005

版权

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);

}