OSDN -- Develop and Download Open Source Software
  • R/O
  • HTTP
  • SSH
  • HTTPS
Fork

base: Commit

This repository is a base of Eos.


Commit MetaInfo

Revisionaceb7f91b289ce18506b08a5e5ed0aca0b397181 (tree)
Time2020-04-08 18:31:01
Author本多康久 <hnd_1110_pc@iclo...>
Commiter本多康久

Log Message

Implement eosPointICP

Change Summary

Incremental Difference

Binary files a/.DS_Store and b/.DS_Store differ
--- /dev/null
+++ b/bin/eosPointICP
@@ -0,0 +1 @@
1+../sbin/MachineIndependent
\ No newline at end of file
Binary files a/src/.DS_Store and b/src/.DS_Store differ
Binary files a/src/Tools/.DS_Store and b/src/Tools/.DS_Store differ
--- a/src/Tools/Config/Define.inc
+++ b/src/Tools/Config/Define.inc
@@ -8,3 +8,5 @@ WORLDNAME=Tools
88 WORLDNAME=Tools
99 WORLDNAME=Tools
1010 WORLDNAME=Tools
11+WORLDNAME=Tools
12+WORLDNAME=Tools
--- /dev/null
+++ b/src/Tools/eosPoint/eosPointICP/.vscode/settings.json
@@ -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
--- a/src/Tools/eosPoint/eosPointICP/inc/config.h
+++ b/src/Tools/eosPoint/eosPointICP/inc/config.h
@@ -1,6 +1,17 @@
11 #ifndef CONFIG_H
22 #define CONFIG_H
33
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;
516
617 #endif /* CONFIG_H */
--- a/src/Tools/eosPoint/eosPointICP/inc/eosPointICP.h
+++ b/src/Tools/eosPoint/eosPointICP/inc/eosPointICP.h
@@ -33,6 +33,18 @@ typedef struct eosPointICPInfo {
3333
3434 long flagOutType;
3535 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;
3648
3749 long flagconfigFile;
3850 char* configFile;
--- /dev/null
+++ b/src/Tools/eosPoint/eosPointICP/src/.vscode/launch.json
@@ -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
--- /dev/null
+++ b/src/Tools/eosPoint/eosPointICP/src/.vscode/settings.json
@@ -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
--- /dev/null
+++ b/src/Tools/eosPoint/eosPointICP/src/.vscode/tasks.json
@@ -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
--- a/src/Tools/eosPoint/eosPointICP/src/argCheck.c
+++ b/src/Tools/eosPoint/eosPointICP/src/argCheck.c
@@ -94,6 +94,44 @@ argCheck(eosPointICPInfo* info, int argc, char* argv[])
9494 }
9595 SBREAK;
9696 }
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+ }
97135 SCASE("c") {
98136 if(i+1<argc) {
99137 info->configFile = stringGetNthWord(argv[i+1], 1, " ,");
--- a/src/Tools/eosPoint/eosPointICP/src/eosPointICP.c
+++ b/src/Tools/eosPoint/eosPointICP/src/eosPointICP.c
@@ -11,13 +11,10 @@
1111 #include <stdio.h>
1212 #include <stdlib.h>
1313 #include <string.h>
14-#include <math.h>
1514 #define GLOBAL_DECLARATION
1615 #include "../inc/config.h"
1716
1817 #define DEBUG
19-#include "genUtil.h"
20-#include "eosPoint.h"
2118
2219 /*
2320 Example:
@@ -32,33 +29,626 @@ typedef enum leosPointICPMode {
3229 } leosPointICPMode;
3330 */
3431
35-int
36-main(int argc, char* argv[])
32+// trace of matrix
33+double trace(int cnt_row, double matrix[cnt_row][cnt_row])
3734 {
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); //第２引数は意味なし
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];
38614 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;
42619
43620 init0(&info);
44- argCheck(&info, argc, argv);
45- init1(&info);
621+ argCheck(&info, argc, argv);
622+ init1(&info);
46623
47624 DEBUGPRINT("Program Start\n");
48625
49626 eosPointRead(info.fptIn, &inP, info.InType);
50627 eosPointRead(info.fptRef, &refP, info.RefType);
51628
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+ }
52637
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);
54646 exit(EXIT_SUCCESS);
55647 }
56648
57-void
58-additionalUsage()
649+void additionalUsage()
59650 {
60651 fprintf(stderr, "----- Additional Usage -----\n");
61652 fprintf(stderr, "eosPointFormat\n");
62653 eosPointFileFormatUsage(stderr);
63-
64654 }
--- /dev/null
+++ b/src/Tools/eosPoint/eosPointICP/src/eosPointICP.html
@@ -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
--- a/src/Tools/eosPoint/eosPointICP/src/init.c
+++ b/src/Tools/eosPoint/eosPointICP/src/init.c
@@ -13,11 +13,15 @@ void
1313 init0(eosPointICPInfo* info)
1414 {
1515 info->fptIn = NULL; info->flagIn = 0;
16- info->InType = 2; info->flagInType = 0;
16+ info->InType = 0; info->flagInType = 0;
1717 info->fptRef = NULL; info->flagRef = 0;
18- info->RefType = 2; info->flagRefType = 0;
18+ info->RefType = 0; info->flagRefType = 0;
1919 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;
2125 info->fptconfigFile = NULL; info->flagconfigFile = 0;
2226 info->mode = 0; info->flagmode = 0;
2327 }
@@ -67,6 +71,15 @@ init1(eosPointICPInfo* info)
6771 }
6872 if(info->flagOutType) {
6973 }
74+ if(info->flagEAMode) {
75+ }
76+ if(info->flagIterationLimit) {
77+ }
78+ if(info->flagPattern){
79+
80+ }
81+ if(info->flagScoreThreshold){
82+ }
7083
7184 if(info->flagconfigFile) {
7285 info->fptconfigFile = fileOpen(info->configFile, "r");
--- a/src/Tools/eosPoint/eosPointICP/src/usage.c
+++ b/src/Tools/eosPoint/eosPointICP/src/usage.c
@@ -7,14 +7,18 @@ usage(char* thisProgram)
77 {
88 fprintf(stderr, "Usage: %s\n", thisProgram);
99 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");
1822 additionalUsage();
1923 }
2024
Binary files a/src/Tools/mrcImage/.DS_Store and b/src/Tools/mrcImage/.DS_Store differ
Show on old repository browser