匹配理论模型与实际工件-拓冰网站定制

要求：输入 n 个理论模型点、n 个实际工件的仪器测量点，输出实际工件回到理论模型位姿所需的旋转、平移矩阵。

代码：

#include <stdio.h> #include <stdlib.h> #include <math.h> #define MAX_POINTS 1000 #define EPS 1e-10 typedef struct { double x; double y; double z; } Point3D; static Point3D point_sub(Point3D a, Point3D b) { Point3D r; r.x = a.x - b.x; r.y = a.y - b.y; r.z = a.z - b.z; return r; } static Point3D point_add(Point3D a, Point3D b) { Point3D r; r.x = a.x + b.x; r.y = a.y + b.y; r.z = a.z + b.z; return r; } static Point3D point_scale(Point3D a, double s) { Point3D r; r.x = a.x * s; r.y = a.y * s; r.z = a.z * s; return r; } static double dot(Point3D a, Point3D b) { return a.x*b.x + a.y*b.y + a.z*b.z; } static Point3D cross(Point3D a, Point3D b) { Point3D r; r.x = a.y*b.z - a.z*b.y; r.y = a.z*b.x - a.x*b.z; r.z = a.x*b.y - a.y*b.x; return r; } static double norm(Point3D a) { return sqrt(dot(a, a)); } static Point3D compute_centroid(const Point3D *p, int n) { Point3D c = {0.0, 0.0, 0.0}; int i; for (i = 0; i < n; i++) { c.x += p[i].x; c.y += p[i].y; c.z += p[i].z; } c.x /= n; c.y /= n; c.z /= n; return c; } static void jacobi_4x4( double A[4][4], double eigenvalue[4], double V[4][4]) { int i, j, p, q; int iter; /* V = I */ for (i = 0; i < 4; i++) { for (j = 0; j < 4; j++) { V[i][j] = (i == j) ? 1.0 : 0.0; } } for (iter = 0; iter < 100; iter++) { p = 0; q = 1; double max_val = fabs(A[0][1]); for (i = 0; i < 4; i++) { for (j = i + 1; j < 4; j++) { if (fabs(A[i][j]) > max_val) { max_val = fabs(A[i][j]); p = i; q = j; } } } if (max_val < 1e-14) break; double app = A[p][p]; double aqq = A[q][q]; double apq = A[p][q]; double phi = 0.5 * atan2(2.0 * apq, aqq - app); double c = cos(phi); double s = sin(phi); /* 更新 A */ for (i = 0; i < 4; i++) { if (i != p && i != q) { double aip = A[i][p]; double aiq = A[i][q]; A[i][p] = c*aip - s*aiq; A[p][i] = A[i][p]; A[i][q] = s*aip + c*aiq; A[q][i] = A[i][q]; } } A[p][p] = c*c*app - 2.0*s*c*apq + s*s*aqq; A[q][q] = s*s*app + 2.0*s*c*apq + c*c*aqq; A[p][q] = 0.0; A[q][p] = 0.0; /* 更新特征向量 */ for (i = 0; i < 4; i++) { double vip = V[i][p]; double viq = V[i][q]; V[i][p] = c*vip - s*viq; V[i][q] = s*vip + c*viq; } } for (i = 0; i < 4; i++) { eigenvalue[i] = A[i][i]; } } static void quaternion_to_rotation( const double q[4], double R[3][3]) { double w = q[0]; double x = q[1]; double y = q[2]; double z = q[3]; double n = sqrt(w*w + x*x + y*y + z*z); if (n < EPS) { R[0][0] = 1.0; R[0][1] = 0.0; R[0][2] = 0.0; R[1][0] = 0.0; R[1][1] = 1.0; R[1][2] = 0.0; R[2][0] = 0.0; R[2][1] = 0.0; R[2][2] = 1.0; return; } w /= n; x /= n; y /= n; z /= n; R[0][0] = 1.0 - 2.0*(y*y + z*z); R[0][1] = 2.0*(x*y - z*w); R[0][2] = 2.0*(x*z + y*w); R[1][0] = 2.0*(x*y + z*w); R[1][1] = 1.0 - 2.0*(x*x + z*z); R[1][2] = 2.0*(y*z - x*w); R[2][0] = 2.0*(x*z - y*w); R[2][1] = 2.0*(y*z + x*w); R[2][2] = 1.0 - 2.0*(x*x + y*y); } static int rigid_registration( const Point3D *P, const Point3D *Q, int n, double R[3][3], Point3D *t) { int i, j; if (n < 3) { printf("错误：至少需要 3 个点。\n"); return -1; } Point3D cP = compute_centroid(P, n); Point3D cQ = compute_centroid(Q, n); Point3D *p = (Point3D *)malloc(sizeof(Point3D) * n); Point3D *q = (Point3D *)malloc(sizeof(Point3D) * n); if (p == NULL || q == NULL) { printf("错误：内存分配失败。\n"); free(p); free(q); return -1; } for (i = 0; i < n; i++) { p[i] = point_sub(P[i], cP); q[i] = point_sub(Q[i], cQ); } double Sxx = 0.0; double Sxy = 0.0; double Sxz = 0.0; double Syx = 0.0; double Syy = 0.0; double Syz = 0.0; double Szx = 0.0; double Szy = 0.0; double Szz = 0.0; for (i = 0; i < n; i++) { Sxx += q[i].x * p[i].x; Sxy += q[i].x * p[i].y; Sxz += q[i].x * p[i].z; Syx += q[i].y * p[i].x; Syy += q[i].y * p[i].y; Syz += q[i].y * p[i].z; Szx += q[i].z * p[i].x; Szy += q[i].z * p[i].y; Szz += q[i].z * p[i].z; } double K[4][4]; double trace = Sxx + Syy + Szz; K[0][0] = trace; K[0][1] = Syz - Szy; K[0][2] = Szx - Sxz; K[0][3] = Sxy - Syx; K[1][0] = Syz - Szy; K[1][1] = Sxx - Syy - Szz; K[1][2] = Sxy + Syx; K[1][3] = Szx + Sxz; K[2][0] = Szx - Sxz; K[2][1] = Sxy + Syx; K[2][2] = -Sxx + Syy - Szz; K[2][3] = Syz + Szy; K[3][0] = Sxy - Syx; K[3][1] = Szx + Sxz; K[3][2] = Syz + Szy; K[3][3] = -Sxx - Syy + Szz; double eigenvalue[4]; double V[4][4]; jacobi_4x4(K, eigenvalue, V); int max_index = 0; for (i = 1; i < 4; i++) { if (eigenvalue[i] > eigenvalue[max_index]) { max_index = i; } } double quat[4]; for (i = 0; i < 4; i++) { quat[i] = V[i][max_index]; } quaternion_to_rotation(quat, R); t->x = cP.x - (R[0][0]*cQ.x + R[0][1]*cQ.y + R[0][2]*cQ.z); t->y = cP.y - (R[1][0]*cQ.x + R[1][1]*cQ.y + R[1][2]*cQ.z); t->z = cP.z - (R[2][0]*cQ.x + R[2][1]*cQ.y + R[2][2]*cQ.z); free(p); free(q); return 0; } static Point3D transform_point( Point3D p, double R[3][3], Point3D t) { Point3D r; r.x = R[0][0]*p.x + R[0][1]*p.y + R[0][2]*p.z + t.x; r.y = R[1][0]*p.x + R[1][1]*p.y + R[1][2]*p.z + t.y; r.z = R[2][0]*p.x + R[2][1]*p.y + R[2][2]*p.z + t.z; return r; } static int check_geometry( const Point3D *P, int n) { if (n < 3) return 0; Point3D base = P[0]; int i; Point3D a = {0, 0, 0}; int found_a = 0; for (i = 1; i < n; i++) { a = point_sub(P[i], base); if (norm(a) > EPS) { found_a = 1; break; } } if (!found_a) return 0; for (i = i + 1; i < n; i++) { Point3D b = point_sub(P[i], base); Point3D c = cross(a, b); if (norm(c) > EPS) { return 1; } } return 0; } static void print_rotation(double R[3][3]) { printf("\n旋转矩阵 R：\n"); printf("[ % .10f % .10f % .10f ]\n", R[0][0], R[0][1], R[0][2]); printf("[ % .10f % .10f % .10f ]\n", R[1][0], R[1][1], R[1][2]); printf("[ % .10f % .10f % .10f ]\n", R[2][0], R[2][1], R[2][2]); } static void print_transform( double R[3][3], Point3D t) { printf("\n4×4 齐次变换矩阵 T：\n"); printf("[ % .10f % .10f % .10f % .10f ]\n", R[0][0], R[0][1], R[0][2], t.x); printf("[ % .10f % .10f % .10f % .10f ]\n", R[1][0], R[1][1], R[1][2], t.y); printf("[ % .10f % .10f % .10f % .10f ]\n", R[2][0], R[2][1], R[2][2], t.z); printf("[ 0.0000000000 0.0000000000 0.0000000000 1.0000000000 ]\n"); } static double compute_rms( const Point3D *P, const Point3D *Q, int n, double R[3][3], Point3D t) { double sum = 0.0; int i; for (i = 0; i < n; i++) { Point3D result = transform_point(Q[i], R, t); double dx = result.x - P[i].x; double dy = result.y - P[i].y; double dz = result.z - P[i].z; sum += dx*dx + dy*dy + dz*dz; } return sqrt(sum / n); } int main(void) { Point3D P[MAX_POINTS]; Point3D Q[MAX_POINTS]; int N; int M; int i; printf("=============================================\n"); printf(" 三维刚体配准 / 旋转平移矩阵计算程序\n"); printf("=============================================\n"); printf("\n目标：\n"); printf("仪器测量坐标 Q --> 初始坐标 P\n"); printf("\n请输入初始点数量 N："); if (scanf("%d", &N) != 1) { printf("输入错误。\n"); return 1; } if (N < 3 || N > MAX_POINTS) { printf("错误：点数必须满足 3 <= N <= %d。\n", MAX_POINTS); return 1; } printf("\n请输入 %d 个初始点坐标：\n", N); for (i = 0; i < N; i++) { printf("P[%d] = ", i + 1); if (scanf("%lf %lf %lf", &P[i].x, &P[i].y, &P[i].z) != 3) { printf("输入错误。\n"); return 1; } } printf("\n请输入仪器测量最终点数量 M："); if (scanf("%d", &M) != 1) { printf("输入错误。\n"); return 1; } if (M != N) { printf("\n=============================================\n"); printf("错误：两组点数量不一致！\n"); printf("初始点数量 N = %d\n", N); printf("测量点数量 M = %d\n", M); printf("必须满足 N = M。\n"); printf("程序终止。\n"); printf("=============================================\n"); return 1; } printf("\n请输入 %d 个仪器测量点坐标：\n", M); for (i = 0; i < M; i++) { printf("Q[%d] = ", i + 1); if (scanf("%lf %lf %lf", &Q[i].x, &Q[i].y, &Q[i].z) != 3) { printf("输入错误。\n"); return 1; } } if (!check_geometry(P, N)) { printf("\n错误：初始点退化。\n"); printf("所有初始点不能全部共线。\n"); return 1; } if (!check_geometry(Q, N)) { printf("\n错误：仪器测量点退化。\n"); printf("所有测量点不能全部共线。\n"); return 1; } double R[3][3]; Point3D t; if (rigid_registration(P, Q, N, R, &t) != 0) { printf("刚体配准失败。\n"); return 1; } print_rotation(R); printf("\n平移向量 t：\n"); printf("[ % .10f ]\n", t.x); printf("[ % .10f ]\n", t.y); printf("[ % .10f ]\n", t.z); print_transform(R, t); double rms = compute_rms(P, Q, N, R, t); printf("\n=============================================\n"); printf("配准 RMS 误差 = %.10f\n", rms); printf("=============================================\n"); printf("\n逐点验证：\n"); printf("\n"); printf(" 初始 P 测量 Q" " Q经过变换后的结果\n"); for (i = 0; i < N; i++) { Point3D result = transform_point(Q[i], R, t); printf("\nP[%d]\n", i + 1); printf("Initial : (% .8f, % .8f, % .8f)\n", P[i].x, P[i].y, P[i].z); printf("Measured: (% .8f, % .8f, % .8f)\n", Q[i].x, Q[i].y, Q[i].z); printf("Result : (% .8f, % .8f, % .8f)\n", result.x, result.y, result.z); printf("Error : (% .8f, % .8f, % .8f)\n", result.x - P[i].x, result.y - P[i].y, result.z - P[i].z); } return 0; }

使用方法：

alientek@alientek-virtual-machine:~$ alientek@alientek-virtual-machine:~$ alientek@alientek-virtual-machine:~$ alientek@alientek-virtual-machine:~$ cd axis/ alientek@alientek-virtual-machine:~/axis$ gcc rigid_registration.c -o rigid_registration -lm alientek@alientek-virtual-machine:~/axis$ ./rigid_registration

示例1及其结果：

============================================= 三维刚体配准 / 旋转平移矩阵计算程序 ============================================= 目标： 仪器测量坐标 Q --> 初始坐标 P 请输入初始点数量 N：3 请输入 3 个初始点坐标： P[1] = 1 0 0 P[2] = 10 0 0 P[3] = 10 50 0 请输入仪器测量最终点数量 M：3 请输入 3 个仪器测量点坐标： Q[1] = 6 6 7 Q[2] = 15 6 7 Q[3] = 15 56 7 旋转矩阵 R： [ 1.0000000000 0.0000000000 0.0000000000 ] [ 0.0000000000 1.0000000000 0.0000000000 ] [ 0.0000000000 0.0000000000 1.0000000000 ] 平移向量 t： [ -5.0000000000 ] [ -6.0000000000 ] [ -7.0000000000 ] 4×4 齐次变换矩阵 T： [ 1.0000000000 0.0000000000 0.0000000000 -5.0000000000 ] [ 0.0000000000 1.0000000000 0.0000000000 -6.0000000000 ] [ 0.0000000000 0.0000000000 1.0000000000 -7.0000000000 ] [ 0.0000000000 0.0000000000 0.0000000000 1.0000000000 ] ============================================= 配准 RMS 误差 = 0.0000000000 ============================================= 逐点验证： 初始 P 测量 Q Q经过变换后的结果 P[1] Initial : ( 1.00000000, 0.00000000, 0.00000000) Measured: ( 6.00000000, 6.00000000, 7.00000000) Result : ( 1.00000000, 0.00000000, 0.00000000) Error : ( 0.00000000, 0.00000000, 0.00000000) P[2] Initial : ( 10.00000000, 0.00000000, 0.00000000) Measured: ( 15.00000000, 6.00000000, 7.00000000) Result : ( 10.00000000, 0.00000000, 0.00000000) Error : ( 0.00000000, 0.00000000, 0.00000000) P[3] Initial : ( 10.00000000, 50.00000000, 0.00000000) Measured: ( 15.00000000, 56.00000000, 7.00000000) Result : ( 10.00000000, 50.00000000, 0.00000000) Error : ( 0.00000000, 0.00000000, 0.00000000)

示例2及其结果：

alientek@alientek-virtual-machine:~/axis$ ./rigid_registration ============================================= 三维刚体配准 / 旋转平移矩阵计算程序 ============================================= 目标： 仪器测量坐标 Q --> 初始坐标 P 请输入初始点数量 N：3 请输入 3 个初始点坐标： P[1] = 1 0 0 P[2] = 10 0 0 P[3] = 10 50 0 请输入仪器测量最终点数量 M：3 请输入 3 个仪器测量点坐标： Q[1] = 5 7 7 Q[2] = 5 16 7 Q[3] = -45 16 7 旋转矩阵 R： [ -0.0000000000 1.0000000000 0.0000000000 ] [ -1.0000000000 -0.0000000000 0.0000000000 ] [ 0.0000000000 0.0000000000 1.0000000000 ] 平移向量 t： [ -6.0000000000 ] [ 5.0000000000 ] [ -7.0000000000 ] 4×4 齐次变换矩阵 T： [ -0.0000000000 1.0000000000 0.0000000000 -6.0000000000 ] [ -1.0000000000 -0.0000000000 0.0000000000 5.0000000000 ] [ 0.0000000000 0.0000000000 1.0000000000 -7.0000000000 ] [ 0.0000000000 0.0000000000 0.0000000000 1.0000000000 ] ============================================= 配准 RMS 误差 = 0.0000000000 ============================================= 逐点验证： 初始 P 测量 Q Q经过变换后的结果 P[1] Initial : ( 1.00000000, 0.00000000, 0.00000000) Measured: ( 5.00000000, 7.00000000, 7.00000000) Result : ( 1.00000000, 0.00000000, 0.00000000) Error : (-0.00000000, 0.00000000, 0.00000000) P[2] Initial : ( 10.00000000, 0.00000000, 0.00000000) Measured: ( 5.00000000, 16.00000000, 7.00000000) Result : ( 10.00000000, 0.00000000, 0.00000000) Error : (-0.00000000, 0.00000000, 0.00000000) P[3] Initial : ( 10.00000000, 50.00000000, 0.00000000) Measured: (-45.00000000, 16.00000000, 7.00000000) Result : ( 10.00000000, 50.00000000, 0.00000000) Error : ( 0.00000000, 0.00000000, 0.00000000)

示例3及其结果：

alientek@alientek-virtual-machine:~/axis$ ./rigid_registration ============================================= 三维刚体配准 / 旋转平移矩阵计算程序 ============================================= 目标： 仪器测量坐标 Q --> 初始坐标 P 请输入初始点数量 N：8 请输入 8 个初始点坐标： P[1] = 0 0 0 P[2] = 10 0 0 P[3] = 20 0 0 P[4] = 0 20 0 P[5] = 10 20 0 P[6] = 20 20 0 P[7] = 5 40 0 P[8] = 30 40 0 请输入仪器测量最终点数量 M：8 请输入 8 个仪器测量点坐标： Q[1] = 100.05 -50.03 30.02 Q[2] = 108.8854 -45.8180 31.7265 Q[3] = 117.8808 -41.6660 33.5130 Q[4] = 91.0001 -32.9114 35.1077 Q[5] = 99.9564 -28.6794 36.8042 Q[6] = 108.8418 -24.5274 38.5907 Q[7] = 86.5447 -13.6717 41.0437 Q[8] = 108.8081 -3.2168 45.4349 旋转矩阵 R： [ 0.8921809403 0.4166291904 0.1744513901 ] [ -0.4495830720 0.8562901966 0.2542482265 ] [ -0.0434537824 -0.3052658137 0.9512752240 ] 平移向量 t： [ -73.6396763058 ] [ 80.1495526971 ] [ -39.4633120475 ] 4×4 齐次变换矩阵 T： [ 0.8921809403 0.4166291904 0.1744513901 -73.6396763058 ] [ -0.4495830720 0.8562901966 0.2542482265 80.1495526971 ] [ -0.0434537824 -0.3052658137 0.9512752240 -39.4633120475 ] [ 0.0000000000 0.0000000000 0.0000000000 1.0000000000 ] ============================================= 配准 RMS 误差 = 0.0447805923 ============================================= 逐点验证： 初始 P 测量 Q Q经过变换后的结果 P[1] Initial : ( 0.00000000, 0.00000000, 0.00000000) Measured: ( 100.05000000, -50.03000000, 30.02000000) Result : ( 0.01609911, -0.03890043, 0.01886791) Error : ( 0.01609911, -0.03890043, 0.01886791) P[2] Initial : ( 10.00000000, 0.00000000, 0.00000000) Measured: ( 108.88540000, -45.81800000, 31.72650000) Result : ( 9.95141803, 0.02942221, -0.02749208) Error : (-0.04858197, 0.02942221, -0.02749208) P[3] Initial : ( 20.00000000, 0.00000000, 0.00000000) Measured: ( 117.88080000, -41.66600000, 33.51300000) Result : ( 20.01844427, -0.00522601, 0.01361330) Error : ( 0.01844427, -0.00522601, 0.01361330) P[4] Initial : ( 0.00000000, 20.00000000, 0.00000000) Measured: ( 91.00010000, -32.91140000, 35.10770000) Result : (-0.03838439, 19.98180948, 0.02619989) Error : (-0.03838439, -0.01819052, 0.02619989) P[5] Initial : ( 10.00000000, 20.00000000, 0.00000000) Measured: ( 99.95640000, -28.67940000, 36.80420000) Result : ( 10.01138728, 20.01036084, -0.04103172) Error : ( 0.01138728, 0.01036084, -0.04103172) P[6] Initial : ( 20.00000000, 20.00000000, 0.00000000) Measured: ( 108.84180000, -24.52740000, 38.59070000) Result : ( 19.98027362, 20.02516676, 0.00485357) Error : (-0.01972638, 0.02516676, 0.00485357) P[7] Initial : ( 5.00000000, 40.00000000, 0.00000000) Measured: ( 86.54470000, -13.67170000, 41.04370000) Result : ( 5.03795673, 39.96886586, -0.00664907) Error : ( 0.03795673, -0.03113414, -0.00664907) P[8] Initial : ( 30.00000000, 40.00000000, 0.00000000) Measured: ( 108.80810000, -3.21680000, 45.43490000) Result : ( 30.02280535, 40.02850129, 0.01163820) Error : ( 0.02280535, 0.02850129, 0.01163820)

资讯详情

匹配理论模型与实际工件