際工件)
要求輸入 n 個理論模型點(diǎn)、n 個實(shí)際工件的儀器測量點(diǎn)輸出實(shí)際工件回到理論模型位姿所需的旋轉(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 個點(diǎn)。\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)平移矩陣計(jì)算程序\n); printf(\n); printf(\n目標(biāo)\n); printf(儀器測量坐標(biāo) Q -- 初始坐標(biāo) P\n); printf(\n請輸入初始點(diǎn)數(shù)量 N); if (scanf(%d, N) ! 1) { printf(輸入錯誤。\n); return 1; } if (N 3 || N MAX_POINTS) { printf(錯誤點(diǎn)數(shù)必須滿足 3 N %d。\n, MAX_POINTS); return 1; } printf(\n請輸入 %d 個初始點(diǎn)坐標(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請輸入儀器測量最終點(diǎn)數(shù)量 M); if (scanf(%d, M) ! 1) { printf(輸入錯誤。\n); return 1; } if (M ! N) { printf(\n\n); printf(錯誤兩組點(diǎn)數(shù)量不一致\n); printf(初始點(diǎn)數(shù)量 N %d\n, N); printf(測量點(diǎn)數(shù)量 M %d\n, M); printf(必須滿足 N M。\n); printf(程序終止。\n); printf(\n); return 1; } printf(\n請輸入 %d 個儀器測量點(diǎn)坐標(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錯誤初始點(diǎn)退化。\n); printf(所有初始點(diǎn)不能全部共線。\n); return 1; } if (!check_geometry(Q, N)) { printf(\n錯誤儀器測量點(diǎn)退化。\n); printf(所有測量點(diǎn)不能全部共線。\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逐點(diǎn)驗(yà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)平移矩陣計(jì)算程序 目標(biāo) 儀器測量坐標(biāo) Q -- 初始坐標(biāo) P 請輸入初始點(diǎn)數(shù)量 N3 請輸入 3 個初始點(diǎn)坐標(biāo) P[1] 1 0 0 P[2] 10 0 0 P[3] 10 50 0 請輸入儀器測量最終點(diǎn)數(shù)量 M3 請輸入 3 個儀器測量點(diǎn)坐標(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 逐點(diǎn)驗(yàn)證 初始 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)平移矩陣計(jì)算程序 目標(biāo) 儀器測量坐標(biāo) Q -- 初始坐標(biāo) P 請輸入初始點(diǎn)數(shù)量 N3 請輸入 3 個初始點(diǎn)坐標(biāo) P[1] 1 0 0 P[2] 10 0 0 P[3] 10 50 0 請輸入儀器測量最終點(diǎn)數(shù)量 M3 請輸入 3 個儀器測量點(diǎn)坐標(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 逐點(diǎn)驗(yàn)證 初始 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)平移矩陣計(jì)算程序 目標(biāo) 儀器測量坐標(biāo) Q -- 初始坐標(biāo) P 請輸入初始點(diǎn)數(shù)量 N8 請輸入 8 個初始點(diǎn)坐標(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 請輸入儀器測量最終點(diǎn)數(shù)量 M8 請輸入 8 個儀器測量點(diǎn)坐標(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 逐點(diǎn)驗(yàn)證 初始 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) 匹配理論模型與實(shí)際工件
要求輸入 n 個理論模型點(diǎn)、n 個實(shí)際工件的儀器測量點(diǎn)輸出實(shí)際工件回到理論模型位姿所需的旋轉(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 個點(diǎn)。\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)平移矩陣計(jì)算程序\n); printf(\n); printf(\n目標(biāo)\n); printf(儀器測量坐標(biāo) Q -- 初始坐標(biāo) P\n); printf(\n請輸入初始點(diǎn)數(shù)量 N); if (scanf(%d, N) ! 1) { printf(輸入錯誤。\n); return 1; } if (N 3 || N MAX_POINTS) { printf(錯誤點(diǎn)數(shù)必須滿足 3 N %d。\n, MAX_POINTS); return 1; } printf(\n請輸入 %d 個初始點(diǎn)坐標(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請輸入儀器測量最終點(diǎn)數(shù)量 M); if (scanf(%d, M) ! 1) { printf(輸入錯誤。\n); return 1; } if (M ! N) { printf(\n\n); printf(錯誤兩組點(diǎn)數(shù)量不一致\n); printf(初始點(diǎn)數(shù)量 N %d\n, N); printf(測量點(diǎn)數(shù)量 M %d\n, M); printf(必須滿足 N M。\n); printf(程序終止。\n); printf(\n); return 1; } printf(\n請輸入 %d 個儀器測量點(diǎn)坐標(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錯誤初始點(diǎn)退化。\n); printf(所有初始點(diǎn)不能全部共線。\n); return 1; } if (!check_geometry(Q, N)) { printf(\n錯誤儀器測量點(diǎn)退化。\n); printf(所有測量點(diǎn)不能全部共線。\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逐點(diǎn)驗(yà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)平移矩陣計(jì)算程序 目標(biāo) 儀器測量坐標(biāo) Q -- 初始坐標(biāo) P 請輸入初始點(diǎn)數(shù)量 N3 請輸入 3 個初始點(diǎn)坐標(biāo) P[1] 1 0 0 P[2] 10 0 0 P[3] 10 50 0 請輸入儀器測量最終點(diǎn)數(shù)量 M3 請輸入 3 個儀器測量點(diǎn)坐標(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 逐點(diǎn)驗(yàn)證 初始 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)平移矩陣計(jì)算程序 目標(biāo) 儀器測量坐標(biāo) Q -- 初始坐標(biāo) P 請輸入初始點(diǎn)數(shù)量 N3 請輸入 3 個初始點(diǎn)坐標(biāo) P[1] 1 0 0 P[2] 10 0 0 P[3] 10 50 0 請輸入儀器測量最終點(diǎn)數(shù)量 M3 請輸入 3 個儀器測量點(diǎn)坐標(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 逐點(diǎn)驗(yàn)證 初始 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)平移矩陣計(jì)算程序 目標(biāo) 儀器測量坐標(biāo) Q -- 初始坐標(biāo) P 請輸入初始點(diǎn)數(shù)量 N8 請輸入 8 個初始點(diǎn)坐標(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 請輸入儀器測量最終點(diǎn)數(shù)量 M8 請輸入 8 個儀器測量點(diǎn)坐標(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 逐點(diǎn)驗(yàn)證 初始 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)