src/Tools/eosPoint/eosPointICP/src/eosPointICP.c

   1 /*
   2 # eosPointICP : $Revision$
   3 # $Date$
   4 # Created by $Author$
   5 # Usage : eosPointICP
   6 # Attention
   7 #   $Loccker$
   8 #       $State$
   9 #
  10 */
  11 #include <stdio.h>
  12 #include <stdlib.h>
  13 #include <string.h>
  14 #define GLOBAL_DECLARATION
  15 #include "../inc/config.h"
  16
  17 #define DEBUG
  18
  19 static int __debug_mode__=0;
  20
  21 /*
  22 Example:
  23 typedef struct leosPointICPInfo {
  24         float a;
  25         int   b;
  26 } leosPointICPInfo;
  27
  28 typedef enum leosPointICPMode {
  29         a=0,
  30         b=1
  31 } leosPointICPMode;
  32 */
  33
  34 // trace of matrix
  35 double trace(int cnt_row, double matrix[cnt_row][cnt_row])
  36 {
  37         int i, j;
  38         double sum = 0.0;
  39
  40         for (i = 0; i < cnt_row; i++) {
  41                 sum += matrix[i][i];
  42         }
  43         return sum;
  44 }
  45
  46 //座標数カウント
  47 int countPoint(eosPoint point)
  48 {
  49         int count = 0;
  50         eosPointTop(&point);
  51         while(point.current != NULL){
  52                 count++;
  53                 eosPointNext(&point);
  54         }
  55         return count;
  56 }
  57
  58 //inP_updに対するrefPの最近傍点の特定　-線形探索
  59 void identifyCP(eosPoint *inP, eosPoint *refP, int *closest_index_pair)
  60 {
  61         int i = 0, j = 1;
  62         double pair[2]; //0:index 1:distance
  63         double distance;
  64         eosPoint *_inP, *_refP;
  65         eosPointTop(inP);
  66         eosPointTop(refP);
  67         for (_inP = inP;; eosPointNext(_inP)) {
  68                 pair[0] = 0;
  69                 pair[1] = sqrt( SQR(refP->top->p.coord.data[0] - _inP->current->p.coord.data[0]) +
  70                                                 SQR(refP->top->p.coord.data[1] - _inP->current->p.coord.data[1]) +
  71                                                 SQR(refP->top->p.coord.data[2] - _inP->current->p.coord.data[2]));
  72                 if (j == 1) {
  73                         eosPointTop(refP);
  74                         eosPointNext(refP);
  75                 }
  76                 for (_refP = refP;; eosPointNext(_refP)){
  77                         distance = sqrt(SQR(refP->current->p.coord.data[0] - _inP->current->p.coord.data[0]) +
  78                                                         SQR(refP->current->p.coord.data[1] - _inP->current->p.coord.data[1]) +
  79                                                         SQR(refP->current->p.coord.data[2] - _inP->current->p.coord.data[2]));
  80                         if (pair[1] > distance){
  81                                 pair[0] = j;
  82                                 pair[1] = distance;
  83                         }
  84                         j++;
  85                         if (_refP->current == refP->bottom){
  86                                 break;
  87                         }
  88                 }
  89                 closest_index_pair[i] = (int)pair[0];
  90                 i++;
  91                 j = 1;
  92                 if (_inP->current == inP->bottom){
  93                         break;
  94                 }
  95         }
  96 }
  97
  98 //重心計算
  99 eosPointCoord calculatePointsG(eosPoint *P, int count)
 100 {
 101         int i, j;
 102         eosPointCoord g;
 103         eosPointCoordInit(&g, 4); //第２引数は意味なし
 104         eosPointTop(P);
 105         for (i = 0; i < 3; i++){
 106                 g.coord.data[0] = 0;
 107         }
 108         for (i = 0; i < count; i++){
 109                 for (j = 0; j < 3; j++){
 110                         g.coord.data[j] += P->current->p.coord.data[j];
 111                 }
 112                 eosPointNext(P);
 113         }
 114         for (i = 0; i < 3; i++){
 115                 g.coord.data[i] /= count;
 116         }
 117         return g;
 118 }
 119
 120 //in_updに対する最近傍点であるrefに対応するようにrefのポインタを移動
 121 void moveRefAccordingToCP(int closest_index_pair_value, eosPoint *refP)
 122 {
 123         int i;
 124         eosPointTop(refP);
 125         for (i = 0; i < closest_index_pair_value; i++){
 126                 eosPointNext(refP);
 127         }
 128 }
 129
 130 //共分散行列の生成
 131 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)
 132 {
 133         int i, j;
 134         for (i = 0; i < 3; i++){
 135                 for (j = 0; j < 3; j++){
 136                         covm[i][j] = 0;
 137                 }
 138         }
 139         eosPointTop(inP_upd);
 140         for (i = 0; i < cnt_in_point; i++){
 141                 moveRefAccordingToCP(closest_index_pair[i], refP);
 142                 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];
 143                 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];
 144                 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];
 145
 146                 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];
 147                 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];
 148                 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];
 149
 150                 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];
 151                 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];
 152                 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];
 153                 eosPointNext(inP_upd);
 154         }
 155         for (i = 0; i < 3; i++){
 156                 for (j = 0; j < 3; j++){
 157                         covm[i][j] /= cnt_in_point;
 158                 }
 159         }
 160 }
 161
 162 //対称行列の生成
 163 void generateSymM(double symm[4][4], double covm[3][3])
 164 {
 165         int i, j;
 166         for (i = 0; i < 4; i++){
 167                 for (j = 0; j < 4; j++){
 168                         symm[i][j] = 0;
 169                 }
 170         }
 171
 172         symm[0][0] = trace(3, covm);
 173         symm[0][1] = covm[1][2] - covm[2][1];
 174         symm[0][2] = covm[2][0] - covm[0][2];
 175         symm[0][3] = covm[0][1] - covm[1][0];
 176
 177         symm[1][0] = covm[1][2] - covm[2][1];
 178         symm[1][1] = 2 * covm[0][0] - trace(3, covm);
 179         symm[1][2] = covm[0][1] - covm[1][0];
 180         symm[1][3] = covm[0][2] - covm[2][0];
 181
 182         symm[2][0] = covm[2][0] - covm[0][2];
 183         symm[2][1] = covm[0][1] - covm[1][0];
 184         symm[2][2] = 2 * covm[1][1] - trace(3, covm);
 185         symm[2][3] = covm[1][2] - covm[2][1];
 186
 187         symm[3][0] = covm[0][1] - covm[1][0];
 188         symm[3][1] = covm[0][2] - covm[2][0];
 189         symm[3][2] = covm[1][2] - covm[2][1];
 190         symm[3][3] = 2 * covm[2][2] - trace(3, covm);
 191 }
 192
 193 //対称行列から絶対値最大の固有値に対応する固有ベクトル(正規化)を求める。
 194 void jacobi(double symm[4][4], double *eigen_vector)
 195 {
 196         int i, j, k, p, q;
 197         double max;
 198         int max_columns;
 199         double theta;
 200         double P[4][4];            //ギブンス回転行列
 201         double Ps[4][4];           //Pの積み重ね
 202         double Ps_cache[4][4]; //Psのキャッシュ
 203         double B[4][4];            //更新される行列
 204         double B_cache[4][4];  //Bの更新で一つ前のBを使うのでキャッシュする
 205         int end_flag = 0;
 206         double norm;
 207         int loop_count = 1;
 208
 209         //Bの生成,Psを単位行列で初期化
 210         for (i = 0; i < 4; i++){
 211                 for (j = 0; j < 4; j++){
 212                         B[i][j] = symm[i][j];
 213                         Ps[i][j] = 0;
 214                 }
 215                 Ps[i][i] = 1.0;
 216         }
 217
 218         while (1){
 219                 //行列の絶対値最大のnondiagonal-indexを探索-線形探索
 220                 max = 0;
 221                 p = 0;
 222                 q = 0;
 223                 for (i = 0; i < 4; i++){
 224                         for (j = 0; j < 4; j++){
 225                                 if (i != j && max < fabs(B[i][j])){
 226                                         p = i;
 227                                         q = j;
 228                                         max = fabs(B[i][j]);
 229                                 }
 230                         }
 231                 }
 232                 // printf("max=B[%d][%d]\n", p, q);
 233                 //maxindexが更新されない　maxがない
 234                 if (p == 0 && q == 0){
 235                         end_flag = 1;
 236                 }
 237
 238                 if (!end_flag){
 239                         //B,Psをキャッシュ
 240                         for (i = 0; i < 4; i++){
 241                                 for (j = 0; j < 4; j++){
 242                                         B_cache[i][j] = B[i][j];
 243                                         Ps_cache[i][j] = Ps[i][j];
 244                                 }
 245                         }
 246
 247                         //θを計算
 248                         if (B[p][p] != B[q][q]){
 249                                 theta = atan(-1 * 2 * B[p][q] / (B[p][p] - B[q][q])) / 2;
 250                         }else{
 251                                 theta = PI / 4.0;
 252                         }
 253
 254                         //Bの更新
 255                         for (i = 0; i < 4; i++){
 256                                 for (j = 0; j < 4; j++){
 257                                         if ((i == p && j == q) || (i == q && j == p)){
 258                                                 B[i][j] = 0;
 259                                         }else if (i == p && j == p){
 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 == q && j == q){
 262                                                 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);
 263                                         }else if (i == p || j == p){
 264                                                 if (i == p){
 265                                                         B[i][j] = B_cache[p][j] * cos(theta) - B_cache[q][j] * sin(theta);
 266                                                 }else{ //j==p
 267                                                         B[i][j] = B_cache[i][p] * cos(theta) - B_cache[i][q] * sin(theta);
 268                                                 }
 269                                         }else if (i == q || j == q){
 270                                                 if (i == q){
 271                                                         B[i][j] = B_cache[p][j] * sin(theta) + B_cache[q][j] * cos(theta);
 272                                                 }else{//j==q
 273                                                         B[i][j] = B_cache[i][p] * sin(theta) + B_cache[i][q] * cos(theta);
 274                                                 }
 275                                         }else{
 276                                                 B[i][j] = B_cache[i][j];
 277                                         }
 278                                 }
 279                         }
 280
 281                         //ギブンス回転行列の構築
 282                         for (i = 0; i < 4; i++){
 283                                 P[i][i] = 1.0;
 284                                 for (j = 3; j > i; j--){
 285                                         P[i][j] = P[j][i] = 0;
 286                                 }
 287                         }
 288                         P[p][p] = cos(theta);
 289                         P[p][q] = (q > p) ? sin(theta) : -1 * sin(theta);
 290                         P[q][p] = (q > p) ? -1 * sin(theta) : sin(theta);
 291                         P[q][q] = cos(theta);
 292
 293                         //ギブンスの積み重ね 行列の掛け算 P = P1・P2...Pn
 294                         for (i = 0; i < 4; i++){
 295                                 for (j = 0; j < 4; j++){
 296                                         Ps[i][j] = 0;
 297                                         for (k = 0; k < 4; k++){
 298                                                 Ps[i][j] += Ps_cache[i][k] * P[k][j];
 299                                         }
 300                                 }
 301                         }
 302                 }
 303
 304                 //非対角成分が0判定
 305                 end_flag = 1;
 306                 for (i = 0; i < 4; i++){
 307                         for (j = 3; j > i; j--){
 308                                 if (fabs(B[j][i]) >= 0.00001 || fabs(B[i][j]) >= 0.00001){
 309                                         end_flag = 0;
 310                                         break;
 311                                 }
 312                         }
 313                 }
 314                 if (end_flag || loop_count >= 50){
 315                         // 対角化された行列に現れる固有値の絶対値最大が現れる列に対応する固有ベクトルを正規化してeigen_vectorにする。
 316                         max = fabs(B[0][0]);
 317                         max_columns = 0;
 318                         for (i = 1; i < 4; i++){
 319                                 if (max < fabs(B[i][i])){
 320                                         max = fabs(B[i][i]);
 321                                         max_columns = i;
 322                                 }
 323                         }
 324                         norm = sqrt(SQR(Ps[0][max_columns]) + SQR(Ps[1][max_columns]) + SQR(Ps[2][max_columns]) + SQR(Ps[3][max_columns]));
 325                         // printf("norm=%lf\n",norm);
 326                         for (i = 0; i < 4; i++){
 327                                 eigen_vector[i] = Ps[i][max_columns] / norm;
 328                                 // printf("eigen_vec[%d]=%lf\n", i, eigen_vector[i]);
 329                         }
 330                         break;
 331                 }
 332                 loop_count++;
 333         }
 334 }
 335
 336 //回転行列の生成
 337 void generateRotationMatrix(double rotation_mat[3][3], double qr[4])
 338 {
 339         rotation_mat[0][0] = SQR(qr[0]) + SQR(qr[1]) - SQR(qr[2]) - SQR(qr[3]);
 340         rotation_mat[0][1] = 2 * (qr[1] * qr[2] - qr[0] * qr[3]);
 341         rotation_mat[0][2] = 2 * (qr[1] * qr[3] + qr[0] * qr[2]);
 342         rotation_mat[1][0] = 2 * (qr[1] * qr[2] + qr[0] * qr[3]);
 343         rotation_mat[1][1] = SQR(qr[0]) + SQR(qr[2]) - SQR(qr[1]) - SQR(qr[3]);
 344         rotation_mat[1][2] = 2 * (qr[2] * qr[3] - qr[0] * qr[1]);
 345         rotation_mat[2][0] = 2 * (qr[1] * qr[3] - qr[0] * qr[2]);
 346         rotation_mat[2][1] = 2 * (qr[2] * qr[3] + qr[0] * qr[1]);
 347         rotation_mat[2][2] = SQR(qr[0]) + SQR(qr[3]) - SQR(qr[1]) - SQR(qr[2]);
 348 }
 349
 350 //平行移動パラメータの算出
 351 void calculateTranslation(eosPointCoord *inP_upd_G, eosPointCoord *refP_G, double rotation_mat[3][3], double qt[3])
 352 {
 353         qt[0] = refP_G->coord.data[0] - (rotation_mat[0][0] * inP_upd_G->coord.data[0] +
 354                                                                          rotation_mat[0][1] * inP_upd_G->coord.data[1] +
 355                                                                          rotation_mat[0][2] * inP_upd_G->coord.data[2] );
 356         qt[1] = refP_G->coord.data[1] - (rotation_mat[1][0] * inP_upd_G->coord.data[0] +
 357                                                                          rotation_mat[1][1] * inP_upd_G->coord.data[1] +
 358                                                                          rotation_mat[1][2] * inP_upd_G->coord.data[2] );
 359         qt[2] = refP_G->coord.data[2] - (rotation_mat[2][0] * inP_upd_G->coord.data[0] +
 360                                                                          rotation_mat[2][1] * inP_upd_G->coord.data[1] +
 361                                                                          rotation_mat[2][2] * inP_upd_G->coord.data[2] );
 362 }
 363
 364
 365 //変換行列を適用
 366 void applyMatrix(eosPoint *inP_upd, double rotation_mat[3][3], double qt[3], int cnt_in_point)
 367 {
 368         int i = 0;
 369         eosPoint cacheP;
 370         eosPointCopy(&cacheP, inP_upd);
 371         eosPointTop(inP_upd);
 372         eosPointTop(&cacheP);
 373         for (i = 0; i < cnt_in_point; i++){
 374                 inP_upd->current->p.coord.data[0] = rotation_mat[0][0] * cacheP.current->p.coord.data[0] +
 375                                                                                         rotation_mat[0][1] * cacheP.current->p.coord.data[1] +
 376                                                                                         rotation_mat[0][2] * cacheP.current->p.coord.data[2] +
 377                                                                                         qt[0];
 378
 379                 inP_upd->current->p.coord.data[1] = rotation_mat[1][0] * cacheP.current->p.coord.data[0] +
 380                                                                                         rotation_mat[1][1] * cacheP.current->p.coord.data[1] +
 381                                                                                         rotation_mat[1][2] * cacheP.current->p.coord.data[2] +
 382                                                                                         qt[1];
 383
 384                 inP_upd->current->p.coord.data[2] = rotation_mat[2][0] * cacheP.current->p.coord.data[0] +
 385                                                                                         rotation_mat[2][1] * cacheP.current->p.coord.data[1] +
 386                                                                                         rotation_mat[2][2] * cacheP.current->p.coord.data[2] +
 387                                                                                         qt[2];
 388                 eosPointNext(inP_upd);
 389                 eosPointNext(&cacheP);
 390         }
 391 }
 392
 393 //Rmaxの算出
 394 void calculateRmax(eosPoint inP,eosPointCoord inP_upd_G, double* r_max, int cnt_in_point)
 395 {
 396         int i,j;
 397         double r;
 398         *r_max = 0;
 399         eosPointTop(&inP);
 400         for(i=0;i<cnt_in_point; i++){
 401                 r = sqrt(SQR(inP.current->p.coord.data[0] - inP_upd_G.coord.data[0]) +
 402                                  SQR(inP.current->p.coord.data[1] - inP_upd_G.coord.data[1]) +
 403                                  SQR(inP.current->p.coord.data[2] - inP_upd_G.coord.data[2]));
 404
 405                 if((*r_max) < r){
 406                         *r_max = r;
 407                 }
 408                 eosPointNext(&inP);
 409         }
 410 }
 411
 412 //SCOREの算出
 413 double calculateScore(eosPoint *inP_upd, eosPoint *refP, int cnt_in_point, int *closest_index_pair, double r_max, double omit, int mode)
 414 {
 415         int i;
 416         double se = 0;
 417     double seRobust = 0;
 418     double dis;
 419     double count;
 420         eosPointTop(inP_upd);
 421         for (i = 0, count=0; i < cnt_in_point; i++){
 422                 moveRefAccordingToCP(closest_index_pair[i], refP);
 423                 dis = SQR(inP_upd->current->p.coord.data[0] - refP->current->p.coord.data[0])
 424                         + SQR(inP_upd->current->p.coord.data[1] - refP->current->p.coord.data[1])
 425                         + SQR(inP_upd->current->p.coord.data[2] - refP->current->p.coord.data[2]);
 426         se += dis;
 427         if(sqrt(dis)<=omit) {
 428             seRobust += dis;
 429             count++;
 430         }
 431                 eosPointNext(inP_upd);
 432         }
 433         // se = se/cnt_in_point;
 434         se = sqrt(se/cnt_in_point);
 435     if(3<=count) {
 436             seRobust = sqrt(seRobust/count);
 437     } else {
 438             seRobust = se;
 439     }
 440         // return se/r_max;
 441     switch(mode) {
 442         case 0: {
 443                 return se;
 444             break;
 445         }
 446         case 1: {
 447             return seRobust;
 448             break;
 449         }
 450         default: {
 451             fprintf(stderr, "Not supported mode: %d\n", mode);
 452             return -1;
 453             break;
 454         }
 455     }
 456 }
 457
 458 //SCOREの算出
 459 double calculateScore2D(eosPoint *inP_upd, eosPoint *refP, int cnt_in_point, int *closest_index_pair, double r_max, double omit, int mode)
 460 {
 461         int i;
 462         double se = 0;
 463     double seRobust = 0;
 464     double dis;
 465     double count;
 466         eosPointTop(inP_upd);
 467         for (i = 0, count=0; i < cnt_in_point; i++, count++){
 468                 moveRefAccordingToCP(closest_index_pair[i], refP);
 469                 dis = SQR(inP_upd->current->p.coord.data[0] - refP->current->p.coord.data[0])
 470                         + SQR(inP_upd->current->p.coord.data[1] - refP->current->p.coord.data[1]);
 471         se += dis;
 472         if(sqrt(dis)<=omit) {
 473             seRobust += dis;
 474         }
 475                 eosPointNext(inP_upd);
 476         }
 477         // se = se/cnt_in_point;
 478         se = sqrt(se/cnt_in_point);
 479     if(3<count) {
 480             seRobust = sqrt(seRobust/count);
 481     } else {
 482             seRobust = se;
 483     }
 484         // return se/r_max;
 485     switch(mode) {
 486         case 0: {
 487                 return se;
 488             break;
 489         }
 490         case 1: {
 491             return seRobust;
 492             break;
 493         }
 494         default: {
 495             fprintf(stderr, "Not supported mode: %d\n", mode);
 496             return -1;
 497             break;
 498         }
 499     }
 500 }
 501
 502
 503 //アフィン変換行列を計算
 504 void MakeAffine(eosPointIcpResult* icp_result_set, double rotation_matrix[3][3], double* translation)
 505 {
 506         int i,j;
 507         Matrix3D affine_matrix;
 508         matrix3DInit(affine_matrix);
 509         for(i=0;i<3;i++){
 510                 for(j=0;j<3;j++){
 511                         affine_matrix[i][j] = rotation_matrix[j][i];
 512                 }
 513                 affine_matrix[3][i] = translation[i];
 514         }
 515         matrix3DMultiplyInv(affine_matrix, icp_result_set->matrix);
 516 }
 517
 518 // SCOREが最小の状態を記憶
 519 void MemorizeStateOfMinimumScore(eosPointIcpResult* icp_result_set, eosPointIcpResult icp_current_set, int count)
 520 {
 521         int i,j;
 522         if( count==1 || icp_result_set->score > icp_current_set.score){
 523                 eosPointCopy(&icp_result_set->resultP, &icp_current_set.resultP);
 524                 for(i=0;i<4;i++){
 525                         for(j=0;j<4;j++){
 526                                 icp_result_set->matrix[i][j] = icp_current_set.matrix[i][j];
 527                         }
 528                 }
 529                 icp_result_set->score = icp_current_set.score;
 530         }
 531 }
 532
 533 //ICPアルゴリズム
 534 void icpAlgorhythm(eosPoint *inP, eosPoint *refP, double score_threshold, int iteration_limit, eosPointIcpResult* icp_result_set, double omit, int flag2D, int mode)
 535 {
 536         eosPointCoord inP_upd_G, refP_G;
 537         eosPointIcpResult icp_current_set;
 538         int i, j;
 539         double covm[3][3];                                              //相互共分散行列(Covariance matrix)
 540         double symm[4][4];                                              //qR,qT算出のための行列-対称行列(Symmetric matrix)
 541         double qr[4];                                                   //四元数　回転パラメータ
 542         double rotation_mat[3][3];                      //回転行列
 543         double qt[3];                                                   //平行移動パラメータ
 544         double r_max;                                                   //重心からの最大距離
 545         double score;                                                   //独自スコア
 546         int count = 1;                                                  //アルゴリズム反復回数カウント
 547         double min_score;                                               //反復の中での最小二乗法の最小スコア
 548         int cnt_in_point = countPoint(*inP);
 549         int cnt_ref_point = countPoint(*refP);
 550         int closest_index_pair[cnt_in_point];   //inとrefの最近傍点のindexのペア
 551         matrix3DInit(icp_current_set.matrix);
 552         eosPointCopy(&icp_current_set.resultP, inP);
 553         identifyCP(&icp_current_set.resultP, refP, closest_index_pair);
 554         inP_upd_G = calculatePointsG(&icp_current_set.resultP, cnt_in_point);
 555         refP_G = calculatePointsG(refP, cnt_ref_point);
 556         calculateRmax(icp_current_set.resultP, inP_upd_G, &r_max, cnt_in_point);
 557
 558         while (1){
 559                 inP_upd_G = calculatePointsG(&icp_current_set.resultP, cnt_in_point);
 560                 generateCovM(covm, cnt_in_point, closest_index_pair, &icp_current_set.resultP, refP, inP_upd_G, refP_G);
 561                 generateSymM(symm, covm);
 562                 jacobi(symm, qr);
 563                 generateRotationMatrix(rotation_mat, qr);
 564                 calculateTranslation(&inP_upd_G, &refP_G, rotation_mat, qt);
 565                 MakeAffine(&icp_current_set, rotation_mat, qt);
 566                 applyMatrix(&icp_current_set.resultP, rotation_mat, qt, cnt_in_point);
 567                 identifyCP(&icp_current_set.resultP, refP, closest_index_pair);
 568         if(flag2D) {
 569                 if( iteration_limit*0.5<count || icp_result_set->score < score_threshold*10){
 570                 icp_current_set.score = calculateScore2D(&icp_current_set.resultP, refP, cnt_in_point, closest_index_pair, r_max, omit, mode);
 571             } else {
 572                 icp_current_set.score = calculateScore2D(&icp_current_set.resultP, refP, cnt_in_point, closest_index_pair, r_max, omit, 0);
 573             }
 574         } else {
 575                 if( iteration_limit*0.5<count || icp_result_set->score < score_threshold*10){
 576                 icp_current_set.score = calculateScore(&icp_current_set.resultP, refP, cnt_in_point, closest_index_pair, r_max, omit, mode);
 577             } else {
 578                 icp_current_set.score = calculateScore(&icp_current_set.resultP, refP, cnt_in_point, closest_index_pair, r_max, omit, 0);
 579             }
 580         }
 581                 MemorizeStateOfMinimumScore(icp_result_set, icp_current_set, count);
 582                 if( count==iteration_limit || icp_result_set->score < score_threshold){
 583                         break;
 584                 }
 585                 count++;
 586         }
 587 }
 588
 589 //座標をピックアップする pickup_percentの割合でflagが立つ
 590 void pickupPoint(eosPoint* inP, eosPoint* pickup_inP, int cnt_in_point, double pickup_percent)
 591 {
 592         int i, j, pickup_flag;
 593     double p;
 594
 595         eosPointTop(inP);
 596         eosPointInit(pickup_inP, NULL);
 597         eosPointTop(pickup_inP);
 598         while(inP->current!=NULL){
 599         p = randomUniformGet(0,100,0);
 600                 pickup_flag = (p<pickup_percent) ? 1 : 0;
 601                 if(pickup_flag){
 602                         eosPointAppend(pickup_inP, &(inP->current->p), 0);
 603                 }
 604                 eosPointNext(inP);
 605         }
 606 }
 607
 608 //結果をファイルに出力
 609 void writeOutput(eosPointICPInfo* info, eosPointIcpResult best_icp_result_set, eosPoint* inP, eosPoint* refP, char* euler_angle_Mode)
 610 {
 611         int i, count=0;
 612         matrix3DParaTypeReal rot1, rot2, rot3;
 613         eosPoint final_inP;
 614         eosPointCopy(&final_inP, inP);
 615         fprintf(info->fptOut,"Result eosPointICP\n\n");
 616         fprintf(info->fptOut,"In  file: %s\ttype: %ld\n",info->In,info->InType);
 617         fprintf(info->fptOut,"Ref file: %s\ttype: %ld\n\n",info->Ref, info->RefType);
 618
 619         //SCOREを表示
 620         if(info->flagScoreThreshold){
 621                 fprintf(info->fptOut, "SCORE threshold: %.2f\n", info->ScoreThreshold);
 622         }else{
 623                 fprintf(info->fptOut, "SCORE Threshold: Not set\n");
 624         }
 625         fprintf(info->fptOut, "SCORE: %lf\n\n", best_icp_result_set.score);
 626         //アフィン変換を表示
 627         fprintf(info->fptOut, "Affine transformation matrix: \n");
 628         for(i=0;i<4;i++){
 629                 fprintf(info->fptOut, "%f\t%f\t%f\t%f\n", best_icp_result_set.matrix[0][i],
 630                                                                                                   best_icp_result_set.matrix[1][i],
 631                                                                                                   best_icp_result_set.matrix[2][i],
 632                                                                                                   best_icp_result_set.matrix[3][i]);
 633         }
 634         //EAMODE,EulerAngleとTranslationを表示
 635         matrix3DEulerAngleGetFromMatrix3D( best_icp_result_set.matrix, euler_angle_Mode, &rot1, &rot2, &rot3, 1);
 636         fprintf(info->fptOut, "\nRotation angle(Euler Angle):\n");
 637         fprintf(info->fptOut, "MODE: %s\nRot1: %.2f, Rot2: %.2f, Rot3: %.2f\n\n", euler_angle_Mode, rot1*DEGREE, rot2*DEGREE, rot3*DEGREE);
 638         fprintf(info->fptOut, "Translation:\n");
 639         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]);
 640         fflush(info->fptOut);
 641         //afiine変換をinPに適用
 642         eosPointTop(&final_inP);
 643         eosPointTop(inP);
 644         while(inP->current != NULL){
 645                 for(i=0;i<3;i++){
 646                         final_inP.current->p.coord.data[i] = best_icp_result_set.matrix[0][i] * inP->current->p.coord.data[0] +
 647                                                                                                  best_icp_result_set.matrix[1][i] * inP->current->p.coord.data[1] +
 648                                                                                                  best_icp_result_set.matrix[2][i] * inP->current->p.coord.data[2] +
 649                                                                                                  best_icp_result_set.matrix[3][i];
 650                 }
 651                 eosPointNext(&final_inP);
 652                 eosPointNext(inP);
 653                 count++;
 654         }
 655         int closest_index_pair[count];
 656         //final inPとrefを表示
 657         identifyCP(&final_inP, refP, closest_index_pair);
 658         eosPointTop(refP);
 659         eosPointTop(&final_inP);
 660         eosPointTop(&best_icp_result_set.resultP);
 661
 662         fprintf(info->fptOut,"Coordinate\n");
 663         fprintf(info->fptOut,"ref\t\t\t\ticp_result(in)\n");
 664         for (i = 0; i < count; i++){
 665                 moveRefAccordingToCP(closest_index_pair[i], refP);
 666                 fprintf(info->fptOut, "%lf %lf %lf\t%lf %lf %lf\n",
 667       refP->current->p.coord.data[0],
 668       refP->current->p.coord.data[1],
 669       refP->current->p.coord.data[2],
 670                         final_inP.current->p.coord.data[0],
 671       final_inP.current->p.coord.data[1],
 672       final_inP.current->p.coord.data[2]);
 673                 eosPointNext(&final_inP);
 674         }
 675 }
 676
 677 void progressBar(int now, int end)
 678 {
 679         int i;
 680         int count = now * 10 / end;
 681         printf("\r|");
 682         for(i=0;i<count;i++){
 683                 printf("####");
 684         }
 685         for(i=count;i<10;i++){
 686                 printf("----");
 687         }
 688         printf("|");
 689         if(now==end){
 690                 printf("\n");
 691         }
 692         fflush(stdout);
 693 }
 694
 695 void coordError()
 696 {
 697         printf("\nError: The number of coordinate(in) over the number of coordinate(ref).\n");
 698         printf("       You must set the number of coordinate(ref) graeter than or equal to the number of coordinate(in).\n");
 699         printf("       in <= ref\n\n");
 700         exit(EXIT_SUCCESS);
 701 }
 702
 703 int main(int argc, char *argv[])
 704 {
 705         int i;
 706         char input[15];
 707         eosPointICPInfo info;
 708         int icp_pattern;
 709         int cnt_in_point, cnt_ref_point;
 710         eosPoint inP, refP, pickup_inP;
 711         eosPointIcpResult icp_result_set, best_icp_result_set;
 712     Matrix3D inMat;
 713     Matrix3D refMat;
 714
 715         init0(&info);
 716         argCheck(&info, argc, argv);
 717         init1(&info);
 718
 719         DEBUGPRINT("Program Start\n");
 720     __debug_mode__ = info.Debug;
 721
 722         eosPointRead(info.fptIn, &inP, info.InType);
 723     if(1<=__debug_mode__) {
 724         eosPointWrite(stderr, &inP, info.InType);
 725     }
 726     matrix3DInit(inMat);
 727     if(info.flagInMat) {
 728         matrix3DFileRead(info.fptInMat, inMat);
 729     }
 730     if(info.flagRatio) {
 731         matrix3DScale(inMat, info.Ratio);
 732     }
 733     eosPointRotate(&inP, inMat);
 734
 735     if(1<=__debug_mode__) {
 736         eosPointWrite(stderr, &inP, info.InType);
 737     }
 738
 739         eosPointRead(info.fptRef, &refP, info.RefType);
 740     matrix3DInit(refMat);
 741     if(info.flagRefMat) {
 742         matrix3DFileRead(info.fptRefMat, refMat);
 743     }
 744     if(info.flagRatio) {
 745         matrix3DScale(refMat, info.Ratio);
 746     }
 747     eosPointRotate(&refP, refMat);
 748
 749         matrix3DInit(icp_result_set.matrix);
 750         matrix3DInit(best_icp_result_set.matrix);
 751
 752         cnt_in_point = countPoint(inP);
 753         cnt_ref_point = countPoint(refP);
 754         if (cnt_in_point > cnt_ref_point){
 755                 coordError();
 756         }
 757
 758         for(icp_pattern=1;icp_pattern<=info.Pattern;icp_pattern++){
 759                 progressBar(icp_pattern, info.Pattern);
 760                 pickupPoint(&inP, &pickup_inP, cnt_in_point, info.Pickup);
 761
 762                 icpAlgorhythm(&pickup_inP, &refP, info.ScoreThreshold, info.IterationLimit, &icp_result_set, info.Omit, info.flag2D, info.mode);
 763                 MemorizeStateOfMinimumScore(&best_icp_result_set, icp_result_set, icp_pattern);
 764         }
 765
 766         writeOutput(&info, best_icp_result_set, &inP, &refP, info.EAMode);
 767         exit(EXIT_SUCCESS);
 768 }
 769
 770 void additionalUsage()
 771 {
 772         fprintf(stderr, "----- Additional Usage -----\n");
 773         fprintf(stderr, "eosPointFormat\n");
 774         eosPointFileFormatUsage(stderr);
 775 }