要求輸入 n 個理論模型點、n 個實際工件的儀器測量點輸出實際工件回到理論模型位姿所需的旋轉(zhuǎ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èi)存分配失敗。\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旋轉(zhuǎ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( 三維剛體配準(zhǔn) / 旋轉(zhuǎn)平移矩陣計算程序\n); printf(\n); printf(\n目標(biāo)\n); printf(儀器測量坐標(biāo) Q -- 初始坐標(biāo) P\n); printf(\n請輸入初始點數(shù)量 N); if (scanf(%d, N) ! 1) { printf(輸入錯誤。\n); return 1; } if (N 3 || N MAX_POINTS) { printf(錯誤點數(shù)必須滿足 3 N %d。\n, MAX_POINTS); return 1; } printf(\n請輸入 %d 個初始點坐標(biāo)\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請輸入儀器測量最終點數(shù)量 M); if (scanf(%d, M) ! 1) { printf(輸入錯誤。\n); return 1; } if (M ! N) { printf(\n\n); printf(錯誤兩組點數(shù)量不一致\n); printf(初始點數(shù)量 N %d\n, N); printf(測量點數(shù)量 M %d\n, M); printf(必須滿足 N M。\n); printf(程序終止。\n); printf(\n); return 1; } printf(\n請輸入 %d 個儀器測量點坐標(biāo)\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(剛體配準(zhǔn)失敗。\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(配準(zhǔn) RMS 誤差 %.10f\n, rms); printf(\n); printf(\n逐點驗證\n); printf(\n); printf( 初始 P 測量 Q Q經(jīng)過變換后的結(jié)果\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及其結(jié)果 三維剛體配準(zhǔn) / 旋轉(zhuǎn)平移矩陣計算程序 目標(biāo) 儀器測量坐標(biāo) Q -- 初始坐標(biāo) P 請輸入初始點數(shù)量 N3 請輸入 3 個初始點坐標(biāo) P[1] 1 0 0 P[2] 10 0 0 P[3] 10 50 0 請輸入儀器測量最終點數(shù)量 M3 請輸入 3 個儀器測量點坐標(biāo) Q[1] 6 6 7 Q[2] 15 6 7 Q[3] 15 56 7 旋轉(zhuǎn)矩陣 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 ] 配準(zhǔn) RMS 誤差 0.0000000000 逐點驗證 初始 P 測量 Q Q經(jīng)過變換后的結(jié)果 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及其結(jié)果alientekalientek-virtual-machine:~/axis$ ./rigid_registration 三維剛體配準(zhǔn) / 旋轉(zhuǎn)平移矩陣計算程序 目標(biāo) 儀器測量坐標(biāo) Q -- 初始坐標(biāo) P 請輸入初始點數(shù)量 N3 請輸入 3 個初始點坐標(biāo) P[1] 1 0 0 P[2] 10 0 0 P[3] 10 50 0 請輸入儀器測量最終點數(shù)量 M3 請輸入 3 個儀器測量點坐標(biāo) Q[1] 5 7 7 Q[2] 5 16 7 Q[3] -45 16 7 旋轉(zhuǎn)矩陣 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 ] 配準(zhǔn) RMS 誤差 0.0000000000 逐點驗證 初始 P 測量 Q Q經(jīng)過變換后的結(jié)果 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及其結(jié)果alientekalientek-virtual-machine:~/axis$ ./rigid_registration 三維剛體配準(zhǔn) / 旋轉(zhuǎn)平移矩陣計算程序 目標(biāo) 儀器測量坐標(biāo) Q -- 初始坐標(biāo) P 請輸入初始點數(shù)量 N8 請輸入 8 個初始點坐標(biāo) 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 請輸入儀器測量最終點數(shù)量 M8 請輸入 8 個儀器測量點坐標(biāo) 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 旋轉(zhuǎn)矩陣 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 ] 配準(zhǔn) RMS 誤差 0.0447805923 逐點驗證 初始 P 測量 Q Q經(jīng)過變換后的結(jié)果 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) 匹配理論模型與實際工件
要求輸入 n 個理論模型點、n 個實際工件的儀器測量點輸出實際工件回到理論模型位姿所需的旋轉(zhuǎ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èi)存分配失敗。\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旋轉(zhuǎ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( 三維剛體配準(zhǔn) / 旋轉(zhuǎn)平移矩陣計算程序\n); printf(\n); printf(\n目標(biāo)\n); printf(儀器測量坐標(biāo) Q -- 初始坐標(biāo) P\n); printf(\n請輸入初始點數(shù)量 N); if (scanf(%d, N) ! 1) { printf(輸入錯誤。\n); return 1; } if (N 3 || N MAX_POINTS) { printf(錯誤點數(shù)必須滿足 3 N %d。\n, MAX_POINTS); return 1; } printf(\n請輸入 %d 個初始點坐標(biāo)\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請輸入儀器測量最終點數(shù)量 M); if (scanf(%d, M) ! 1) { printf(輸入錯誤。\n); return 1; } if (M ! N) { printf(\n\n); printf(錯誤兩組點數(shù)量不一致\n); printf(初始點數(shù)量 N %d\n, N); printf(測量點數(shù)量 M %d\n, M); printf(必須滿足 N M。\n); printf(程序終止。\n); printf(\n); return 1; } printf(\n請輸入 %d 個儀器測量點坐標(biāo)\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(剛體配準(zhǔn)失敗。\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(配準(zhǔn) RMS 誤差 %.10f\n, rms); printf(\n); printf(\n逐點驗證\n); printf(\n); printf( 初始 P 測量 Q Q經(jīng)過變換后的結(jié)果\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及其結(jié)果 三維剛體配準(zhǔn) / 旋轉(zhuǎn)平移矩陣計算程序 目標(biāo) 儀器測量坐標(biāo) Q -- 初始坐標(biāo) P 請輸入初始點數(shù)量 N3 請輸入 3 個初始點坐標(biāo) P[1] 1 0 0 P[2] 10 0 0 P[3] 10 50 0 請輸入儀器測量最終點數(shù)量 M3 請輸入 3 個儀器測量點坐標(biāo) Q[1] 6 6 7 Q[2] 15 6 7 Q[3] 15 56 7 旋轉(zhuǎn)矩陣 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 ] 配準(zhǔn) RMS 誤差 0.0000000000 逐點驗證 初始 P 測量 Q Q經(jīng)過變換后的結(jié)果 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及其結(jié)果alientekalientek-virtual-machine:~/axis$ ./rigid_registration 三維剛體配準(zhǔn) / 旋轉(zhuǎn)平移矩陣計算程序 目標(biāo) 儀器測量坐標(biāo) Q -- 初始坐標(biāo) P 請輸入初始點數(shù)量 N3 請輸入 3 個初始點坐標(biāo) P[1] 1 0 0 P[2] 10 0 0 P[3] 10 50 0 請輸入儀器測量最終點數(shù)量 M3 請輸入 3 個儀器測量點坐標(biāo) Q[1] 5 7 7 Q[2] 5 16 7 Q[3] -45 16 7 旋轉(zhuǎn)矩陣 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 ] 配準(zhǔn) RMS 誤差 0.0000000000 逐點驗證 初始 P 測量 Q Q經(jīng)過變換后的結(jié)果 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及其結(jié)果alientekalientek-virtual-machine:~/axis$ ./rigid_registration 三維剛體配準(zhǔn) / 旋轉(zhuǎn)平移矩陣計算程序 目標(biāo) 儀器測量坐標(biāo) Q -- 初始坐標(biāo) P 請輸入初始點數(shù)量 N8 請輸入 8 個初始點坐標(biāo) 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 請輸入儀器測量最終點數(shù)量 M8 請輸入 8 個儀器測量點坐標(biāo) 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 旋轉(zhuǎn)矩陣 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 ] 配準(zhǔn) RMS 誤差 0.0447805923 逐點驗證 初始 P 測量 Q Q經(jīng)過變換后的結(jié)果 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)