要求输入 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; }使用方法alientekalientek-virtual-machine:~$ alientekalientek-virtual-machine:~$ alientekalientek-virtual-machine:~$ alientekalientek-virtual-machine:~$ cd axis/ alientekalientek-virtual-machine:~/axis$ gcc rigid_registration.c -o rigid_registration -lm alientekalientek-virtual-machine:~/axis$ ./rigid_registration示例1及其结果 三维刚体配准 / 旋转平移矩阵计算程序 目标 仪器测量坐标 Q -- 初始坐标 P 请输入初始点数量 N3 请输入 3 个初始点坐标 P[1] 1 0 0 P[2] 10 0 0 P[3] 10 50 0 请输入仪器测量最终点数量 M3 请输入 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及其结果alientekalientek-virtual-machine:~/axis$ ./rigid_registration 三维刚体配准 / 旋转平移矩阵计算程序 目标 仪器测量坐标 Q -- 初始坐标 P 请输入初始点数量 N3 请输入 3 个初始点坐标 P[1] 1 0 0 P[2] 10 0 0 P[3] 10 50 0 请输入仪器测量最终点数量 M3 请输入 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及其结果alientekalientek-virtual-machine:~/axis$ ./rigid_registration 三维刚体配准 / 旋转平移矩阵计算程序 目标 仪器测量坐标 Q -- 初始坐标 P 请输入初始点数量 N8 请输入 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 请输入仪器测量最终点数量 M8 请输入 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) 
长河君 · 2026/9/30 18:54:37 · 阅读 1.2k 匹配理论模型与实际工件
要求输入 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; }使用方法alientekalientek-virtual-machine:~$ alientekalientek-virtual-machine:~$ alientekalientek-virtual-machine:~$ alientekalientek-virtual-machine:~$ cd axis/ alientekalientek-virtual-machine:~/axis$ gcc rigid_registration.c -o rigid_registration -lm alientekalientek-virtual-machine:~/axis$ ./rigid_registration示例1及其结果 三维刚体配准 / 旋转平移矩阵计算程序 目标 仪器测量坐标 Q -- 初始坐标 P 请输入初始点数量 N3 请输入 3 个初始点坐标 P[1] 1 0 0 P[2] 10 0 0 P[3] 10 50 0 请输入仪器测量最终点数量 M3 请输入 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及其结果alientekalientek-virtual-machine:~/axis$ ./rigid_registration 三维刚体配准 / 旋转平移矩阵计算程序 目标 仪器测量坐标 Q -- 初始坐标 P 请输入初始点数量 N3 请输入 3 个初始点坐标 P[1] 1 0 0 P[2] 10 0 0 P[3] 10 50 0 请输入仪器测量最终点数量 M3 请输入 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及其结果alientekalientek-virtual-machine:~/axis$ ./rigid_registration 三维刚体配准 / 旋转平移矩阵计算程序 目标 仪器测量坐标 Q -- 初始坐标 P 请输入初始点数量 N8 请输入 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 请输入仪器测量最终点数量 M8 请输入 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)