This repository is a base of Eos.
Revision | aceb7f91b289ce18506b08a5e5ed0aca0b397181 (tree) |
---|---|
Time | 2020-04-08 18:31:01 |
Author | 本多康久 <hnd_1110_pc@iclo...> |
Commiter | 本多康久 |
Implement eosPointICP
@@ -0,0 +1 @@ | ||
1 | +../sbin/MachineIndependent | |
\ No newline at end of file |
@@ -8,3 +8,5 @@ WORLDNAME=Tools | ||
8 | 8 | WORLDNAME=Tools |
9 | 9 | WORLDNAME=Tools |
10 | 10 | WORLDNAME=Tools |
11 | +WORLDNAME=Tools | |
12 | +WORLDNAME=Tools |
@@ -0,0 +1,10 @@ | ||
1 | +{ | |
2 | + "files.associations": { | |
3 | + "iterator": "c", | |
4 | + "array": "c", | |
5 | + "string": "c", | |
6 | + "string_view": "c", | |
7 | + "vector": "c", | |
8 | + "__config": "cpp" | |
9 | + } | |
10 | +} | |
\ No newline at end of file |
@@ -1,6 +1,17 @@ | ||
1 | 1 | #ifndef CONFIG_H |
2 | 2 | #define CONFIG_H |
3 | 3 | |
4 | -#include "../inc/eosPointICP.h" | |
4 | +#include "eosPointICP.h" | |
5 | +#include "genUtil.h" | |
6 | +#include "eosString.h" | |
7 | +#include "eosPoint.h" | |
8 | +#include "Matrix3D.h" | |
9 | +#include "Random.h" | |
10 | + | |
11 | +typedef struct eosPointIcpResult{ | |
12 | + eosPoint resultP; | |
13 | + Matrix3D matrix; | |
14 | + double score; | |
15 | +}eosPointIcpResult; | |
5 | 16 | |
6 | 17 | #endif /* CONFIG_H */ |
@@ -33,6 +33,18 @@ typedef struct eosPointICPInfo { | ||
33 | 33 | |
34 | 34 | long flagOutType; |
35 | 35 | long OutType; |
36 | + | |
37 | + long flagEAMode; | |
38 | + char* EAMode; | |
39 | + | |
40 | + long flagIterationLimit; | |
41 | + long IterationLimit; | |
42 | + | |
43 | + long flagPattern; | |
44 | + long Pattern; | |
45 | + | |
46 | + long flagScoreThreshold; | |
47 | + double ScoreThreshold; | |
36 | 48 | |
37 | 49 | long flagconfigFile; |
38 | 50 | char* configFile; |
@@ -0,0 +1,21 @@ | ||
1 | +{ | |
2 | + // Use IntelliSense to learn about possible attributes. | |
3 | + // Hover to view descriptions of existing attributes. | |
4 | + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 | |
5 | + "version": "0.2.0", | |
6 | + "configurations": [ | |
7 | + { | |
8 | + "name": "gcc - アクティブ ファイルのビルドとデバッグ", | |
9 | + "type": "cppdbg", | |
10 | + "request": "launch", | |
11 | + "program": "${fileDirname}/${fileBasenameNoExtension}.c", | |
12 | + "args": [], | |
13 | + "stopAtEntry": false, | |
14 | + "cwd": "${workspaceFolder}", | |
15 | + "environment": [], | |
16 | + "externalConsole": false, | |
17 | + "MIMode": "lldb", | |
18 | + "preLaunchTask": "gcc build active file" | |
19 | + } | |
20 | + ] | |
21 | +} | |
\ No newline at end of file |
@@ -0,0 +1,11 @@ | ||
1 | +{ | |
2 | + "files.associations": { | |
3 | + "fstream": "cpp", | |
4 | + "array": "c", | |
5 | + "string": "c", | |
6 | + "string_view": "c", | |
7 | + "vector": "c", | |
8 | + "iterator": "c", | |
9 | + "eospointicp.h": "c" | |
10 | + } | |
11 | +} | |
\ No newline at end of file |
@@ -0,0 +1,33 @@ | ||
1 | +{ | |
2 | + "tasks": [ | |
3 | + { | |
4 | + "type": "shell", | |
5 | + "label": "gcc build active file", | |
6 | + "command": "/usr/bin/gcc", | |
7 | + "args": [ | |
8 | + "-g", | |
9 | + "${file}", | |
10 | + "-o", | |
11 | + "${fileDirname}/${fileBasenameNoExtension}" | |
12 | + ], | |
13 | + "options": { | |
14 | + "cwd": "/usr/bin" | |
15 | + } | |
16 | + }, | |
17 | + { | |
18 | + "type": "shell", | |
19 | + "label": "clang build active file", | |
20 | + "command": "/usr/bin/clang", | |
21 | + "args": [ | |
22 | + "-g", | |
23 | + "${file}", | |
24 | + "-o", | |
25 | + "${fileDirname}/${fileBasenameNoExtension}" | |
26 | + ], | |
27 | + "options": { | |
28 | + "cwd": "/usr/bin" | |
29 | + } | |
30 | + } | |
31 | + ], | |
32 | + "version": "2.0.0" | |
33 | +} | |
\ No newline at end of file |
@@ -94,6 +94,44 @@ argCheck(eosPointICPInfo* info, int argc, char* argv[]) | ||
94 | 94 | } |
95 | 95 | SBREAK; |
96 | 96 | } |
97 | + SCASE("EA"){ | |
98 | + info->EAMode = stringGetNthWord(argv[i+1], 1, " ,"); | |
99 | + i++; | |
100 | + info->flagEAMode++; | |
101 | + } | |
102 | + SCASE("il"){ | |
103 | + if(i+1<argc) { | |
104 | + info->IterationLimit = stringGetNthIntegerData(argv[i+1], 1, " ,"); | |
105 | + i++; | |
106 | + info->flagIterationLimit++; | |
107 | + } else { | |
108 | + usage(argv[0]); | |
109 | + exit(EXIT_FAILURE); | |
110 | + } | |
111 | + SBREAK; | |
112 | + } | |
113 | + SCASE("p"){ | |
114 | + if(i+1<argc) { | |
115 | + info->Pattern = stringGetNthIntegerData(argv[i+1], 1, " ,"); | |
116 | + i++; | |
117 | + info->flagPattern++; | |
118 | + } else { | |
119 | + usage(argv[0]); | |
120 | + exit(EXIT_FAILURE); | |
121 | + } | |
122 | + SBREAK; | |
123 | + } | |
124 | + SCASE("ST"){ | |
125 | + if(i+1<argc) { | |
126 | + info->ScoreThreshold = stringGetNthRealData(argv[i+1], 1, " ,"); | |
127 | + i++; | |
128 | + info->flagScoreThreshold++; | |
129 | + } else { | |
130 | + usage(argv[0]); | |
131 | + exit(EXIT_FAILURE); | |
132 | + } | |
133 | + SBREAK; | |
134 | + } | |
97 | 135 | SCASE("c") { |
98 | 136 | if(i+1<argc) { |
99 | 137 | info->configFile = stringGetNthWord(argv[i+1], 1, " ,"); |
@@ -11,13 +11,10 @@ | ||
11 | 11 | #include <stdio.h> |
12 | 12 | #include <stdlib.h> |
13 | 13 | #include <string.h> |
14 | -#include <math.h> | |
15 | 14 | #define GLOBAL_DECLARATION |
16 | 15 | #include "../inc/config.h" |
17 | 16 | |
18 | 17 | #define DEBUG |
19 | -#include "genUtil.h" | |
20 | -#include "eosPoint.h" | |
21 | 18 | |
22 | 19 | /* |
23 | 20 | Example: |
@@ -32,33 +29,626 @@ typedef enum leosPointICPMode { | ||
32 | 29 | } leosPointICPMode; |
33 | 30 | */ |
34 | 31 | |
35 | -int | |
36 | -main(int argc, char* argv[]) | |
32 | +// trace of matrix | |
33 | +double trace(int cnt_row, double matrix[cnt_row][cnt_row]) | |
37 | 34 | { |
35 | + int i, j; | |
36 | + double sum = 0.0; | |
37 | + | |
38 | + for (i = 0; i < cnt_row; i++) { | |
39 | + sum += matrix[i][i]; | |
40 | + } | |
41 | + return sum; | |
42 | +} | |
43 | + | |
44 | +//座標数カウント | |
45 | +int countPoint(eosPoint point) | |
46 | +{ | |
47 | + int count = 0; | |
48 | + eosPointTop(&point); | |
49 | + while(point.current != NULL){ | |
50 | + count++; | |
51 | + eosPointNext(&point); | |
52 | + } | |
53 | + return count; | |
54 | +} | |
55 | + | |
56 | +//inP_updに対するrefPの最近傍点の特定 -線形探索 | |
57 | +void identifyCP(eosPoint *inP, eosPoint *refP, int *closest_index_pair) | |
58 | +{ | |
59 | + int i = 0, j = 1; | |
60 | + double pair[2]; //0:index 1:distance | |
61 | + double distance; | |
62 | + eosPoint *_inP, *_refP; | |
63 | + eosPointTop(inP); | |
64 | + eosPointTop(refP); | |
65 | + for (_inP = inP;; eosPointNext(_inP)) { | |
66 | + pair[0] = 0; | |
67 | + pair[1] = sqrt( SQR(refP->top->p.coord.data[0] - _inP->current->p.coord.data[0]) + | |
68 | + SQR(refP->top->p.coord.data[1] - _inP->current->p.coord.data[1]) + | |
69 | + SQR(refP->top->p.coord.data[2] - _inP->current->p.coord.data[2])); | |
70 | + if (j == 1) { | |
71 | + eosPointTop(refP); | |
72 | + eosPointNext(refP); | |
73 | + } | |
74 | + for (_refP = refP;; eosPointNext(_refP)){ | |
75 | + distance = sqrt(SQR(refP->current->p.coord.data[0] - _inP->current->p.coord.data[0]) + | |
76 | + SQR(refP->current->p.coord.data[1] - _inP->current->p.coord.data[1]) + | |
77 | + SQR(refP->current->p.coord.data[2] - _inP->current->p.coord.data[2])); | |
78 | + if (pair[1] > distance){ | |
79 | + pair[0] = j; | |
80 | + pair[1] = distance; | |
81 | + } | |
82 | + j++; | |
83 | + if (_refP->current == refP->bottom){ | |
84 | + break; | |
85 | + } | |
86 | + } | |
87 | + closest_index_pair[i] = (int)pair[0]; | |
88 | + i++; | |
89 | + j = 1; | |
90 | + if (_inP->current == inP->bottom){ | |
91 | + break; | |
92 | + } | |
93 | + } | |
94 | +} | |
95 | + | |
96 | +//重心計算 | |
97 | +eosPointCoord calculatePointsG(eosPoint *P, int count) | |
98 | +{ | |
99 | + int i, j; | |
100 | + eosPointCoord g; | |
101 | + eosPointCoordInit(&g, 4); //第2引数は意味なし | |
102 | + eosPointTop(P); | |
103 | + for (i = 0; i < 3; i++){ | |
104 | + g.coord.data[0] = 0; | |
105 | + } | |
106 | + for (i = 0; i < count; i++){ | |
107 | + for (j = 0; j < 3; j++){ | |
108 | + g.coord.data[j] += P->current->p.coord.data[j]; | |
109 | + } | |
110 | + eosPointNext(P); | |
111 | + } | |
112 | + for (i = 0; i < 3; i++){ | |
113 | + g.coord.data[i] /= count; | |
114 | + } | |
115 | + return g; | |
116 | +} | |
117 | + | |
118 | +//in_updに対する最近傍点であるrefに対応するようにrefのポインタを移動 | |
119 | +void moveRefAccordingToCP(int closest_index_pair_value, eosPoint *refP) | |
120 | +{ | |
121 | + int i; | |
122 | + eosPointTop(refP); | |
123 | + for (i = 0; i < closest_index_pair_value; i++){ | |
124 | + eosPointNext(refP); | |
125 | + } | |
126 | +} | |
127 | + | |
128 | +//共分散行列の生成 | |
129 | +void generateCovM(double covm[3][3], int cnt_in_point, int *closest_index_pair, eosPoint *inP_upd, eosPoint *refP, eosPointCoord inP_upd_G, eosPointCoord refP_G) | |
130 | +{ | |
131 | + int i, j; | |
132 | + for (i = 0; i < 3; i++){ | |
133 | + for (j = 0; j < 3; j++){ | |
134 | + covm[i][j] = 0; | |
135 | + } | |
136 | + } | |
137 | + eosPointTop(inP_upd); | |
138 | + for (i = 0; i < cnt_in_point; i++){ | |
139 | + moveRefAccordingToCP(closest_index_pair[i], refP); | |
140 | + covm[0][0] += refP->current->p.coord.data[0] * inP_upd->current->p.coord.data[0] - refP_G.coord.data[0] * inP_upd_G.coord.data[0]; | |
141 | + covm[0][1] += refP->current->p.coord.data[0] * inP_upd->current->p.coord.data[1] - refP_G.coord.data[0] * inP_upd_G.coord.data[1]; | |
142 | + covm[0][2] += refP->current->p.coord.data[0] * inP_upd->current->p.coord.data[2] - refP_G.coord.data[0] * inP_upd_G.coord.data[2]; | |
143 | + | |
144 | + covm[1][0] += refP->current->p.coord.data[1] * inP_upd->current->p.coord.data[0] - refP_G.coord.data[1] * inP_upd_G.coord.data[0]; | |
145 | + covm[1][1] += refP->current->p.coord.data[1] * inP_upd->current->p.coord.data[1] - refP_G.coord.data[1] * inP_upd_G.coord.data[1]; | |
146 | + covm[1][2] += refP->current->p.coord.data[1] * inP_upd->current->p.coord.data[2] - refP_G.coord.data[1] * inP_upd_G.coord.data[2]; | |
147 | + | |
148 | + covm[2][0] += refP->current->p.coord.data[2] * inP_upd->current->p.coord.data[0] - refP_G.coord.data[2] * inP_upd_G.coord.data[0]; | |
149 | + covm[2][1] += refP->current->p.coord.data[2] * inP_upd->current->p.coord.data[1] - refP_G.coord.data[2] * inP_upd_G.coord.data[1]; | |
150 | + covm[2][2] += refP->current->p.coord.data[2] * inP_upd->current->p.coord.data[2] - refP_G.coord.data[2] * inP_upd_G.coord.data[2]; | |
151 | + eosPointNext(inP_upd); | |
152 | + } | |
153 | + for (i = 0; i < 3; i++){ | |
154 | + for (j = 0; j < 3; j++){ | |
155 | + covm[i][j] /= cnt_in_point; | |
156 | + } | |
157 | + } | |
158 | +} | |
159 | + | |
160 | +//対称行列の生成 | |
161 | +void generateSymM(double symm[4][4], double covm[3][3]) | |
162 | +{ | |
163 | + int i, j; | |
164 | + for (i = 0; i < 4; i++){ | |
165 | + for (j = 0; j < 4; j++){ | |
166 | + symm[i][j] = 0; | |
167 | + } | |
168 | + } | |
169 | + | |
170 | + symm[0][0] = trace(3, covm); | |
171 | + symm[0][1] = covm[1][2] - covm[2][1]; | |
172 | + symm[0][2] = covm[2][0] - covm[0][2]; | |
173 | + symm[0][3] = covm[0][1] - covm[1][0]; | |
174 | + | |
175 | + symm[1][0] = covm[1][2] - covm[2][1]; | |
176 | + symm[1][1] = 2 * covm[0][0] - trace(3, covm); | |
177 | + symm[1][2] = covm[0][1] - covm[1][0]; | |
178 | + symm[1][3] = covm[0][2] - covm[2][0]; | |
179 | + | |
180 | + symm[2][0] = covm[2][0] - covm[0][2]; | |
181 | + symm[2][1] = covm[0][1] - covm[1][0]; | |
182 | + symm[2][2] = 2 * covm[1][1] - trace(3, covm); | |
183 | + symm[2][3] = covm[1][2] - covm[2][1]; | |
184 | + | |
185 | + symm[3][0] = covm[0][1] - covm[1][0]; | |
186 | + symm[3][1] = covm[0][2] - covm[2][0]; | |
187 | + symm[3][2] = covm[1][2] - covm[2][1]; | |
188 | + symm[3][3] = 2 * covm[2][2] - trace(3, covm); | |
189 | +} | |
190 | + | |
191 | +//対称行列から絶対値最大の固有値に対応する固有ベクトル(正規化)を求める。 | |
192 | +void jacobi(double symm[4][4], double *eigen_vector) | |
193 | +{ | |
194 | + int i, j, k, p, q; | |
195 | + double max; | |
196 | + int max_columns; | |
197 | + double theta; | |
198 | + double P[4][4]; //ギブンス回転行列 | |
199 | + double Ps[4][4]; //Pの積み重ね | |
200 | + double Ps_cache[4][4]; //Psのキャッシュ | |
201 | + double B[4][4]; //更新される行列 | |
202 | + double B_cache[4][4]; //Bの更新で一つ前のBを使うのでキャッシュする | |
203 | + int end_flag = 0; | |
204 | + double norm; | |
205 | + int loop_count = 1; | |
206 | + | |
207 | + //Bの生成,Psを単位行列で初期化 | |
208 | + for (i = 0; i < 4; i++){ | |
209 | + for (j = 0; j < 4; j++){ | |
210 | + B[i][j] = symm[i][j]; | |
211 | + Ps[i][j] = 0; | |
212 | + } | |
213 | + Ps[i][i] = 1.0; | |
214 | + } | |
215 | + | |
216 | + while (1){ | |
217 | + //行列の絶対値最大のnondiagonal-indexを探索-線形探索 | |
218 | + max = 0; | |
219 | + p = 0; | |
220 | + q = 0; | |
221 | + for (i = 0; i < 4; i++){ | |
222 | + for (j = 0; j < 4; j++){ | |
223 | + if (i != j && max < fabs(B[i][j])){ | |
224 | + p = i; | |
225 | + q = j; | |
226 | + max = fabs(B[i][j]); | |
227 | + } | |
228 | + } | |
229 | + } | |
230 | + // printf("max=B[%d][%d]\n", p, q); | |
231 | + //maxindexが更新されない maxがない | |
232 | + if (p == 0 && q == 0){ | |
233 | + end_flag = 1; | |
234 | + } | |
235 | + | |
236 | + if (!end_flag){ | |
237 | + //B,Psをキャッシュ | |
238 | + for (i = 0; i < 4; i++){ | |
239 | + for (j = 0; j < 4; j++){ | |
240 | + B_cache[i][j] = B[i][j]; | |
241 | + Ps_cache[i][j] = Ps[i][j]; | |
242 | + } | |
243 | + } | |
244 | + | |
245 | + //θを計算 | |
246 | + if (B[p][p] != B[q][q]){ | |
247 | + theta = atan(-1 * 2 * B[p][q] / (B[p][p] - B[q][q])) / 2; | |
248 | + }else{ | |
249 | + theta = PI / 4.0; | |
250 | + } | |
251 | + | |
252 | + //Bの更新 | |
253 | + for (i = 0; i < 4; i++){ | |
254 | + for (j = 0; j < 4; j++){ | |
255 | + if ((i == p && j == q) || (i == q && j == p)){ | |
256 | + B[i][j] = 0; | |
257 | + }else if (i == p && j == p){ | |
258 | + B[i][j] = (B_cache[p][p] + B_cache[q][q]) / 2.0 + (B_cache[p][p] - B_cache[q][q]) / 2.0 * cos(2.0 * theta) - B_cache[p][q] * sin(2.0 * theta); | |
259 | + }else if (i == q && j == q){ | |
260 | + B[i][j] = (B_cache[p][p] + B_cache[q][q]) / 2.0 - (B_cache[p][p] - B_cache[q][q]) / 2.0 * cos(2.0 * theta) + B_cache[p][q] * sin(2.0 * theta); | |
261 | + }else if (i == p || j == p){ | |
262 | + if (i == p){ | |
263 | + B[i][j] = B_cache[p][j] * cos(theta) - B_cache[q][j] * sin(theta); | |
264 | + }else{ //j==p | |
265 | + B[i][j] = B_cache[i][p] * cos(theta) - B_cache[i][q] * sin(theta); | |
266 | + } | |
267 | + }else if (i == q || j == q){ | |
268 | + if (i == q){ | |
269 | + B[i][j] = B_cache[p][j] * sin(theta) + B_cache[q][j] * cos(theta); | |
270 | + }else{//j==q | |
271 | + B[i][j] = B_cache[i][p] * sin(theta) + B_cache[i][q] * cos(theta); | |
272 | + } | |
273 | + }else{ | |
274 | + B[i][j] = B_cache[i][j]; | |
275 | + } | |
276 | + } | |
277 | + } | |
278 | + | |
279 | + //ギブンス回転行列の構築 | |
280 | + for (i = 0; i < 4; i++){ | |
281 | + P[i][j] = 1.0; | |
282 | + for (j = 3; j > i; j--){ | |
283 | + P[i][j] = P[j][i] = 0; | |
284 | + } | |
285 | + } | |
286 | + P[p][p] = cos(theta); | |
287 | + P[p][q] = (q > p) ? sin(theta) : -1 * sin(theta); | |
288 | + P[q][p] = (q > p) ? -1 * sin(theta) : sin(theta); | |
289 | + P[q][q] = cos(theta); | |
290 | + | |
291 | + //ギブンスの積み重ね 行列の掛け算 P = P1・P2...Pn | |
292 | + for (i = 0; i < 4; i++){ | |
293 | + for (j = 0; j < 4; j++){ | |
294 | + Ps[i][j] = 0; | |
295 | + for (k = 0; k < 4; k++){ | |
296 | + Ps[i][j] += Ps_cache[i][k] * P[k][j]; | |
297 | + } | |
298 | + } | |
299 | + } | |
300 | + } | |
301 | + | |
302 | + //非対角成分が0判定 | |
303 | + end_flag = 1; | |
304 | + for (i = 0; i < 4; i++){ | |
305 | + for (j = 3; j > i; j--){ | |
306 | + if (fabs(B[j][i]) >= 0.00001 || fabs(B[i][j]) >= 0.00001){ | |
307 | + end_flag = 0; | |
308 | + break; | |
309 | + } | |
310 | + } | |
311 | + } | |
312 | + if (end_flag || loop_count >= 50){ | |
313 | + // 対角化された行列に現れる固有値の絶対値最大が現れる列に対応する固有ベクトルを正規化してeigen_vectorにする。 | |
314 | + max = fabs(B[0][0]); | |
315 | + max_columns = 0; | |
316 | + for (i = 1; i < 4; i++){ | |
317 | + if (max < fabs(B[i][i])){ | |
318 | + max = fabs(B[i][i]); | |
319 | + max_columns = i; | |
320 | + } | |
321 | + } | |
322 | + norm = sqrt(SQR(Ps[0][max_columns]) + SQR(Ps[1][max_columns]) + SQR(Ps[2][max_columns]) + SQR(Ps[3][max_columns])); | |
323 | + // printf("norm=%lf\n",norm); | |
324 | + for (i = 0; i < 4; i++){ | |
325 | + eigen_vector[i] = Ps[i][max_columns] / norm; | |
326 | + // printf("eigen_vec[%d]=%lf\n", i, eigen_vector[i]); | |
327 | + } | |
328 | + break; | |
329 | + } | |
330 | + loop_count++; | |
331 | + } | |
332 | +} | |
333 | + | |
334 | +//回転行列の生成 | |
335 | +void generateRotationMatrix(double rotation_mat[3][3], double qr[4]) | |
336 | +{ | |
337 | + rotation_mat[0][0] = SQR(qr[0]) + SQR(qr[1]) - SQR(qr[2]) - SQR(qr[3]); | |
338 | + rotation_mat[0][1] = 2 * (qr[1] * qr[2] - qr[0] * qr[3]); | |
339 | + rotation_mat[0][2] = 2 * (qr[1] * qr[3] + qr[0] * qr[2]); | |
340 | + rotation_mat[1][0] = 2 * (qr[1] * qr[2] + qr[0] * qr[3]); | |
341 | + rotation_mat[1][1] = SQR(qr[0]) + SQR(qr[2]) - SQR(qr[1]) - SQR(qr[3]); | |
342 | + rotation_mat[1][2] = 2 * (qr[2] * qr[3] - qr[0] * qr[1]); | |
343 | + rotation_mat[2][0] = 2 * (qr[1] * qr[3] - qr[0] * qr[2]); | |
344 | + rotation_mat[2][1] = 2 * (qr[2] * qr[3] + qr[0] * qr[1]); | |
345 | + rotation_mat[2][2] = SQR(qr[0]) + SQR(qr[3]) - SQR(qr[1]) - SQR(qr[2]); | |
346 | +} | |
347 | + | |
348 | +//平行移動パラメータの算出 | |
349 | +void calculateTranslation(eosPointCoord *inP_upd_G, eosPointCoord *refP_G, double rotation_mat[3][3], double qt[3]) | |
350 | +{ | |
351 | + qt[0] = refP_G->coord.data[0] - (rotation_mat[0][0] * inP_upd_G->coord.data[0] + | |
352 | + rotation_mat[0][1] * inP_upd_G->coord.data[1] + | |
353 | + rotation_mat[0][2] * inP_upd_G->coord.data[2] ); | |
354 | + qt[1] = refP_G->coord.data[1] - (rotation_mat[1][0] * inP_upd_G->coord.data[0] + | |
355 | + rotation_mat[1][1] * inP_upd_G->coord.data[1] + | |
356 | + rotation_mat[1][2] * inP_upd_G->coord.data[2] ); | |
357 | + qt[2] = refP_G->coord.data[2] - (rotation_mat[2][0] * inP_upd_G->coord.data[0] + | |
358 | + rotation_mat[2][1] * inP_upd_G->coord.data[1] + | |
359 | + rotation_mat[2][2] * inP_upd_G->coord.data[2] ); | |
360 | +} | |
361 | + | |
362 | +//変換行列を適用 | |
363 | +void applyMatrix(eosPoint *inP_upd, double rotation_mat[3][3], double qt[3], int cnt_in_point) | |
364 | +{ | |
365 | + int i = 0; | |
366 | + eosPoint cacheP; | |
367 | + eosPointCopy(&cacheP, inP_upd); | |
368 | + eosPointTop(inP_upd); | |
369 | + eosPointTop(&cacheP); | |
370 | + for (i = 0; i < cnt_in_point; i++){ | |
371 | + inP_upd->current->p.coord.data[0] = rotation_mat[0][0] * cacheP.current->p.coord.data[0] + | |
372 | + rotation_mat[0][1] * cacheP.current->p.coord.data[1] + | |
373 | + rotation_mat[0][2] * cacheP.current->p.coord.data[2] + | |
374 | + qt[0]; | |
375 | + | |
376 | + inP_upd->current->p.coord.data[1] = rotation_mat[1][0] * cacheP.current->p.coord.data[0] + | |
377 | + rotation_mat[1][1] * cacheP.current->p.coord.data[1] + | |
378 | + rotation_mat[1][2] * cacheP.current->p.coord.data[2] + | |
379 | + qt[1]; | |
380 | + | |
381 | + inP_upd->current->p.coord.data[2] = rotation_mat[2][0] * cacheP.current->p.coord.data[0] + | |
382 | + rotation_mat[2][1] * cacheP.current->p.coord.data[1] + | |
383 | + rotation_mat[2][2] * cacheP.current->p.coord.data[2] + | |
384 | + qt[2]; | |
385 | + eosPointNext(inP_upd); | |
386 | + eosPointNext(&cacheP); | |
387 | + } | |
388 | +} | |
389 | + | |
390 | +//Rmaxの算出 | |
391 | +void calculateRmax(eosPoint inP,eosPointCoord inP_upd_G, double* r_max, int cnt_in_point) | |
392 | +{ | |
393 | + int i,j; | |
394 | + double r; | |
395 | + *r_max = 0; | |
396 | + eosPointTop(&inP); | |
397 | + for(i=0;i<cnt_in_point; i++){ | |
398 | + r = sqrt(SQR(inP.current->p.coord.data[0] - inP_upd_G.coord.data[0]) + | |
399 | + SQR(inP.current->p.coord.data[1] - inP_upd_G.coord.data[1]) + | |
400 | + SQR(inP.current->p.coord.data[2] - inP_upd_G.coord.data[2])); | |
401 | + | |
402 | + if((*r_max) < r){ | |
403 | + *r_max = r; | |
404 | + } | |
405 | + eosPointNext(&inP); | |
406 | + } | |
407 | +} | |
408 | + | |
409 | +//SCOREの算出 | |
410 | +double calculateScore(eosPoint *inP_upd, eosPoint *refP, int cnt_in_point, int *closest_index_pair, double r_max) | |
411 | +{ | |
412 | + int i; | |
413 | + double se = 0; | |
414 | + eosPointTop(inP_upd); | |
415 | + for (i = 0; i < cnt_in_point; i++){ | |
416 | + moveRefAccordingToCP(closest_index_pair[i], refP); | |
417 | + se += SQR(inP_upd->current->p.coord.data[0] - refP->current->p.coord.data[0]) | |
418 | + + SQR(inP_upd->current->p.coord.data[1] - refP->current->p.coord.data[1]) | |
419 | + + SQR(inP_upd->current->p.coord.data[2] - refP->current->p.coord.data[2]); | |
420 | + eosPointNext(inP_upd); | |
421 | + } | |
422 | + // se = sqrt(se)/cnt_in_point; | |
423 | + se = se/cnt_in_point; | |
424 | + // return se/r_max; | |
425 | + return se; | |
426 | +} | |
427 | + | |
428 | +//アフィン変換行列を計算 | |
429 | +void MakeAffine(eosPointIcpResult* icp_result_set, double rotation_matrix[3][3], double* translation) | |
430 | +{ | |
431 | + int i,j; | |
432 | + Matrix3D affine_matrix; | |
433 | + matrix3DInit(affine_matrix); | |
434 | + for(i=0;i<3;i++){ | |
435 | + for(j=0;j<3;j++){ | |
436 | + affine_matrix[i][j] = rotation_matrix[j][i]; | |
437 | + } | |
438 | + affine_matrix[3][i] = translation[i]; | |
439 | + } | |
440 | + matrix3DMultiplyInv(affine_matrix, icp_result_set->matrix); | |
441 | +} | |
442 | + | |
443 | +// SCOREが最小の状態を記憶 | |
444 | +void MemorizeStateOfMinimumScore(eosPointIcpResult* icp_result_set, eosPointIcpResult icp_current_set, int count) | |
445 | +{ | |
446 | + int i,j; | |
447 | + if( count==1 || icp_result_set->score > icp_current_set.score){ | |
448 | + eosPointCopy(&icp_result_set->resultP, &icp_current_set.resultP); | |
449 | + for(i=0;i<4;i++){ | |
450 | + for(j=0;j<4;j++){ | |
451 | + icp_result_set->matrix[i][j] = icp_current_set.matrix[i][j]; | |
452 | + } | |
453 | + } | |
454 | + icp_result_set->score = icp_current_set.score; | |
455 | + } | |
456 | +} | |
457 | + | |
458 | +//ICPアルゴリズム | |
459 | +void icpAlgorhythm(eosPoint *inP, eosPoint *refP, double score_threshold, int iteration_limit, eosPointIcpResult* icp_result_set) | |
460 | +{ | |
461 | + eosPointCoord inP_upd_G, refP_G; | |
462 | + eosPointIcpResult icp_current_set; | |
463 | + int i, j; | |
464 | + double covm[3][3]; //相互共分散行列(Covariance matrix) | |
465 | + double symm[4][4]; //qR,qT算出のための行列-対称行列(Symmetric matrix) | |
466 | + double qr[4]; //四元数 回転パラメータ | |
467 | + double rotation_mat[3][3]; //回転行列 | |
468 | + double qt[3]; //平行移動パラメータ | |
469 | + double r_max; //重心からの最大距離 | |
470 | + double score; //独自スコア | |
471 | + int count = 1; //アルゴリズム反復回数カウント | |
472 | + double min_score; //反復の中での最小二乗法の最小スコア | |
473 | + int cnt_in_point = countPoint(*inP); | |
474 | + int cnt_ref_point = countPoint(*refP); | |
475 | + int closest_index_pair[cnt_in_point]; //inとrefの最近傍点のindexのペア | |
476 | + matrix3DInit(icp_current_set.matrix); | |
477 | + eosPointCopy(&icp_current_set.resultP, inP); | |
478 | + identifyCP(&icp_current_set.resultP, refP, closest_index_pair); | |
479 | + inP_upd_G = calculatePointsG(&icp_current_set.resultP, cnt_in_point); | |
480 | + refP_G = calculatePointsG(refP, cnt_ref_point); | |
481 | + calculateRmax(icp_current_set.resultP, inP_upd_G, &r_max, cnt_in_point); | |
482 | + | |
483 | + while (1){ | |
484 | + inP_upd_G = calculatePointsG(&icp_current_set.resultP, cnt_in_point); | |
485 | + generateCovM(covm, cnt_in_point, closest_index_pair, &icp_current_set.resultP, refP, inP_upd_G, refP_G); | |
486 | + generateSymM(symm, covm); | |
487 | + jacobi(symm, qr); | |
488 | + generateRotationMatrix(rotation_mat, qr); | |
489 | + calculateTranslation(&inP_upd_G, &refP_G, rotation_mat, qt); | |
490 | + MakeAffine(&icp_current_set, rotation_mat, qt); | |
491 | + applyMatrix(&icp_current_set.resultP, rotation_mat, qt, cnt_in_point); | |
492 | + identifyCP(&icp_current_set.resultP, refP, closest_index_pair); | |
493 | + icp_current_set.score = calculateScore(&icp_current_set.resultP, refP, cnt_in_point, closest_index_pair, r_max); | |
494 | + MemorizeStateOfMinimumScore(icp_result_set, icp_current_set, count); | |
495 | + if( count==iteration_limit || icp_result_set->score < score_threshold){ | |
496 | + break; | |
497 | + } | |
498 | + count++; | |
499 | + } | |
500 | +} | |
501 | + | |
502 | +//座標をピックアップする pickup_percentの割合でflagが立つ | |
503 | +void pickupPoint(eosPoint* inP, eosPoint* pickup_inP, int cnt_in_point) | |
504 | +{ | |
505 | + int i, j, pickup_flag; | |
506 | + float pickup_percent = 0.8; | |
507 | + | |
508 | + eosPointTop(inP); | |
509 | + eosPointInit(pickup_inP, NULL); | |
510 | + eosPointTop(pickup_inP); | |
511 | + while(inP->current!=NULL){ | |
512 | + pickup_flag = (random()<pickup_percent) ? 1 : 0; | |
513 | + if(pickup_flag){ | |
514 | + eosPointAppend(pickup_inP, &(inP->current->p), 0); | |
515 | + } | |
516 | + eosPointNext(inP); | |
517 | + } | |
518 | +} | |
519 | + | |
520 | +//結果をファイルに出力 | |
521 | +void writeOutput(eosPointICPInfo* info, eosPointIcpResult best_icp_result_set, eosPoint* inP, eosPoint* refP, char* euler_angle_Mode) | |
522 | +{ | |
523 | + int i, count=0; | |
524 | + matrix3DParaTypeReal rot1, rot2, rot3; | |
525 | + eosPoint final_inP; | |
526 | + eosPointCopy(&final_inP, inP); | |
527 | + fprintf(info->fptOut,"Result eosPointICP\n\n"); | |
528 | + fprintf(info->fptOut,"In file: %s\ttype: %ld\n",info->In,info->InType); | |
529 | + fprintf(info->fptOut,"Ref file: %s\ttype: %ld\n\n",info->Ref, info->RefType); | |
530 | + | |
531 | + //SCOREを表示 | |
532 | + if(info->flagScoreThreshold){ | |
533 | + fprintf(info->fptOut, "SCORE threshold: %.2f\n", info->ScoreThreshold); | |
534 | + }else{ | |
535 | + fprintf(info->fptOut, "SCORE Threshold: Not set\n"); | |
536 | + } | |
537 | + fprintf(info->fptOut, "SCORE: %lf\n\n", best_icp_result_set.score); | |
538 | + //アフィン変換を表示 | |
539 | + fprintf(info->fptOut, "Affine transformation matrix: \n"); | |
540 | + for(i=0;i<4;i++){ | |
541 | + fprintf(info->fptOut, "%f\t%f\t%f\t%f\n", best_icp_result_set.matrix[0][i], | |
542 | + best_icp_result_set.matrix[1][i], | |
543 | + best_icp_result_set.matrix[2][i], | |
544 | + best_icp_result_set.matrix[3][i]); | |
545 | + } | |
546 | + //EAMODE,EulerAngleとTranslationを表示 | |
547 | + matrix3DEulerAngleGetFromMatrix3D( best_icp_result_set.matrix, euler_angle_Mode, &rot1, &rot2, &rot3, 1); | |
548 | + fprintf(info->fptOut, "\nRotation angle(Euler Angle):\n"); | |
549 | + fprintf(info->fptOut, "MODE: %s\nRot1: %.2f, Rot2: %.2f, Rot3: %.2f\n\n", euler_angle_Mode, rot1, rot2, rot3); | |
550 | + fprintf(info->fptOut, "Translation:\n"); | |
551 | + fprintf(info->fptOut, "\tx: %lf\ty: %lf\tz: %lf\n\n",best_icp_result_set.matrix[3][0],best_icp_result_set.matrix[3][1],best_icp_result_set.matrix[3][2]); | |
552 | + fflush(info->fptOut); | |
553 | + //afiine変換をinPに適用 | |
554 | + eosPointTop(&final_inP); | |
555 | + eosPointTop(inP); | |
556 | + while(inP->current != NULL){ | |
557 | + for(i=0;i<3;i++){ | |
558 | + final_inP.current->p.coord.data[i] = best_icp_result_set.matrix[0][i] * inP->current->p.coord.data[0] + | |
559 | + best_icp_result_set.matrix[1][i] * inP->current->p.coord.data[1] + | |
560 | + best_icp_result_set.matrix[2][i] * inP->current->p.coord.data[2] + | |
561 | + best_icp_result_set.matrix[3][i]; | |
562 | + } | |
563 | + eosPointNext(&final_inP); | |
564 | + eosPointNext(inP); | |
565 | + count++; | |
566 | + } | |
567 | + int closest_index_pair[count]; | |
568 | + //final inPとrefを表示 | |
569 | + identifyCP(&final_inP, refP, closest_index_pair); | |
570 | + eosPointTop(refP); | |
571 | + eosPointTop(&final_inP); | |
572 | + eosPointTop(&best_icp_result_set.resultP); | |
573 | + | |
574 | + fprintf(info->fptOut,"Coordinate\n"); | |
575 | + fprintf(info->fptOut,"ref\t\t\t\ticp_result\n"); | |
576 | + for (i = 0; i < count; i++){ | |
577 | + moveRefAccordingToCP(closest_index_pair[i], refP); | |
578 | + fprintf(info->fptOut, "%lf %lf %lf\t%lf %lf %lf\n", refP->current->p.coord.data[0], refP->current->p.coord.data[1], refP->current->p.coord.data[2], | |
579 | + final_inP.current->p.coord.data[0], final_inP.current->p.coord.data[1], final_inP.current->p.coord.data[2]); | |
580 | + eosPointNext(&final_inP); | |
581 | + } | |
582 | +} | |
583 | + | |
584 | +void progressBar(int now, int end) | |
585 | +{ | |
586 | + int i; | |
587 | + int count = now * 10 / end; | |
588 | + printf("\r|"); | |
589 | + for(i=0;i<count;i++){ | |
590 | + printf("####"); | |
591 | + } | |
592 | + for(i=count;i<10;i++){ | |
593 | + printf("----"); | |
594 | + } | |
595 | + printf("|"); | |
596 | + if(now==end){ | |
597 | + printf("\n"); | |
598 | + } | |
599 | + fflush(stdout); | |
600 | +} | |
601 | + | |
602 | +void coordError() | |
603 | +{ | |
604 | + printf("\nError: The number of coordinate(in) over the number of coordinate(ref).\n"); | |
605 | + printf(" You must set the number of coordinate(ref) graeter than or equal to the number of coordinate(in).\n"); | |
606 | + printf(" in <= ref\n\n"); | |
607 | + exit(EXIT_SUCCESS); | |
608 | +} | |
609 | + | |
610 | +int main(int argc, char *argv[]) | |
611 | +{ | |
612 | + int i; | |
613 | + char input[15]; | |
38 | 614 | eosPointICPInfo info; |
39 | - eosPoint inP; | |
40 | - eosPoint refP; | |
41 | - eosPoint outP; | |
615 | + int icp_pattern; | |
616 | + int cnt_in_point, cnt_ref_point; | |
617 | + eosPoint inP, refP, pickup_inP; | |
618 | + eosPointIcpResult icp_result_set, best_icp_result_set; | |
42 | 619 | |
43 | 620 | init0(&info); |
44 | - argCheck(&info, argc, argv); | |
45 | - init1(&info); | |
621 | + argCheck(&info, argc, argv); | |
622 | + init1(&info); | |
46 | 623 | |
47 | 624 | DEBUGPRINT("Program Start\n"); |
48 | 625 | |
49 | 626 | eosPointRead(info.fptIn, &inP, info.InType); |
50 | 627 | eosPointRead(info.fptRef, &refP, info.RefType); |
51 | 628 | |
629 | + matrix3DInit(icp_result_set.matrix); | |
630 | + matrix3DInit(best_icp_result_set.matrix); | |
631 | + | |
632 | + cnt_in_point = countPoint(inP); | |
633 | + cnt_ref_point = countPoint(refP); | |
634 | + if (cnt_in_point > cnt_ref_point){ | |
635 | + coordError(); | |
636 | + } | |
52 | 637 | |
53 | - eosPointWrite(info.fptOut, &outP, info.OutType); | |
638 | + for(icp_pattern=1;icp_pattern<=info.Pattern;icp_pattern++){ | |
639 | + progressBar(icp_pattern, info.Pattern); | |
640 | + pickupPoint(&inP, &pickup_inP, cnt_in_point); | |
641 | + icpAlgorhythm(&pickup_inP, &refP, info.ScoreThreshold, info.IterationLimit, &icp_result_set); | |
642 | + MemorizeStateOfMinimumScore(&best_icp_result_set, icp_result_set, icp_pattern); | |
643 | + } | |
644 | + | |
645 | + writeOutput(&info, best_icp_result_set, &inP, &refP, info.EAMode); | |
54 | 646 | exit(EXIT_SUCCESS); |
55 | 647 | } |
56 | 648 | |
57 | -void | |
58 | -additionalUsage() | |
649 | +void additionalUsage() | |
59 | 650 | { |
60 | 651 | fprintf(stderr, "----- Additional Usage -----\n"); |
61 | 652 | fprintf(stderr, "eosPointFormat\n"); |
62 | 653 | eosPointFileFormatUsage(stderr); |
63 | - | |
64 | 654 | } |
@@ -0,0 +1,3 @@ | ||
1 | +dyld: Library not loaded: @rpath/libcudart.6.5.dylib | |
2 | + Referenced from: /Users/honda/Eos/src/Tools/eosPoint/eosPointICP/src/eosPointICP | |
3 | + Reason: image not found |
@@ -13,11 +13,15 @@ void | ||
13 | 13 | init0(eosPointICPInfo* info) |
14 | 14 | { |
15 | 15 | info->fptIn = NULL; info->flagIn = 0; |
16 | - info->InType = 2; info->flagInType = 0; | |
16 | + info->InType = 0; info->flagInType = 0; | |
17 | 17 | info->fptRef = NULL; info->flagRef = 0; |
18 | - info->RefType = 2; info->flagRefType = 0; | |
18 | + info->RefType = 0; info->flagRefType = 0; | |
19 | 19 | info->fptOut = NULL; info->flagOut = 0; |
20 | - info->OutType = 2; info->flagOutType = 0; | |
20 | + info->OutType = 0; info->flagOutType = 0; | |
21 | + info->EAMode = stringGetNthWord("ZONS", 1, "\0"); info->flagEAMode = 0; | |
22 | + info->IterationLimit=10000; info->flagIterationLimit = 0; | |
23 | + info->Pattern = 10; info->flagPattern = 0; | |
24 | + info->ScoreThreshold = 0.0; info->flagScoreThreshold = 0; | |
21 | 25 | info->fptconfigFile = NULL; info->flagconfigFile = 0; |
22 | 26 | info->mode = 0; info->flagmode = 0; |
23 | 27 | } |
@@ -67,6 +71,15 @@ init1(eosPointICPInfo* info) | ||
67 | 71 | } |
68 | 72 | if(info->flagOutType) { |
69 | 73 | } |
74 | + if(info->flagEAMode) { | |
75 | + } | |
76 | + if(info->flagIterationLimit) { | |
77 | + } | |
78 | + if(info->flagPattern){ | |
79 | + | |
80 | + } | |
81 | + if(info->flagScoreThreshold){ | |
82 | + } | |
70 | 83 | |
71 | 84 | if(info->flagconfigFile) { |
72 | 85 | info->fptconfigFile = fileOpen(info->configFile, "r"); |
@@ -7,14 +7,18 @@ usage(char* thisProgram) | ||
7 | 7 | { |
8 | 8 | fprintf(stderr, "Usage: %s\n", thisProgram); |
9 | 9 | fprintf(stderr, "Options:\n"); |
10 | - fprintf(stderr, " [-i[nput] In (NULL ).as(inFile ) ] :Essential :Input: eosPoint\n"); | |
11 | - fprintf(stderr, " [-i[nput]t[ype] InType (2 ).as(Integer ) ] :Optional :eosPointType\n"); | |
12 | - fprintf(stderr, " [-r[eference] Ref (NULL ).as(inFile ) ] :Essential :Input: esoPoint\n"); | |
13 | - fprintf(stderr, " [-r[eference]t[ype] RefType (2 ).as(Integer ) ] :Optional :Input: esoPoint\n"); | |
14 | - fprintf(stderr, " [-o[utput] Out (NULL ).as(outFile ) ] :Essential :OutputDataFile\n"); | |
15 | - fprintf(stderr, " [-o[utput]t[ype] OutType (2 ).as(Integer ) ] :Essential :OutputDataFile\n"); | |
16 | - fprintf(stderr, " [-c[onfig] configFile (NULL ).as(inFile ) ] :Optional :ConfigurationFile\n"); | |
17 | - fprintf(stderr, " [-m[ode] mode (0 ).as(Integer ) ] :Optional :Mode\n"); | |
10 | + fprintf(stderr, " [-i[nput] In (NULL ).as(inFile ) ] :Essential :Input: eosPoint\n"); | |
11 | + fprintf(stderr, " [-i[nput]t[ype] InType (2 ).as(Integer ) ] :Optional :eosPointType\n"); | |
12 | + fprintf(stderr, " [-r[eference] Ref (NULL ).as(inFile ) ] :Essential :Input: eosPoint\n"); | |
13 | + fprintf(stderr, " [-r[eference]t[ype] RefType (2 ).as(Integer ) ] :Optional :Input: eosPoint\n"); | |
14 | + fprintf(stderr, " [-o[utput] Out (NULL ).as(outFile ) ] :Essential :OutputDataFile\n"); | |
15 | + fprintf(stderr, " [-o[utput]t[ype] OutType (2 ).as(Integer ) ] :Essential :OutputDataFile\n"); | |
16 | + fprintf(stderr, " [-i[teration]l[imit] IterationLimit (10000 ).as(Integer ) ] :Optional :Iteration Limit\n"); | |
17 | + fprintf(stderr, " [-p[erttern]] Pattern (10 ).as(Integer ) ] :Optional :Pattern\n"); | |
18 | + fprintf(stderr, " [-E[uler]A[ngle] EulerAngle (ZONS ).as(String ) ] :Optional :EulerAngle\n"); | |
19 | + fprintf(stderr, " [-S[core]T[hreshold]] ScoreThreshold (0 ).as(Real ) ] :Optional :ScoreThreshold\n"); | |
20 | + fprintf(stderr, " [-c[onfig] configFile (NULL ).as(inFile ) ] :Optional :ConfigurationFile\n"); | |
21 | + fprintf(stderr, " [-m[ode] mode (0 ).as(Integer ) ] :Optional :Mode\n"); | |
18 | 22 | additionalUsage(); |
19 | 23 | } |
20 | 24 |