tags/2.4.2/src/jp/nyatla/nyartoolkit/core/pca2d/NyARPca2d_MatrixPCA_O2.java

   1 /* \r
   2  * PROJECT: NyARToolkit\r
   3  * --------------------------------------------------------------------------------\r
   4  * This work is based on the original ARToolKit developed by\r
   5  *   Hirokazu Kato\r
   6  *   Mark Billinghurst\r
   7  *   HITLab, University of Washington, Seattle\r
   8  * http://www.hitl.washington.edu/artoolkit/\r
   9  *\r
  10  * The NyARToolkit is Java edition ARToolKit class library.\r
  11  * Copyright (C)2008-2009 Ryo Iizuka\r
  12  *\r
  13  * This program is free software: you can redistribute it and/or modify\r
  14  * it under the terms of the GNU General Public License as published by\r
  15  * the Free Software Foundation, either version 3 of the License, or\r
  16  * (at your option) any later version.\r
  17  * \r
  18  * This program is distributed in the hope that it will be useful,\r
  19  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  21  * GNU General Public License for more details.\r
  22  *\r
  23  * You should have received a copy of the GNU General Public License\r
  24  * along with this program.  If not, see <http://www.gnu.org/licenses/>.\r
  25  * \r
  26  * For further information please contact.\r
  27  *      http://nyatla.jp/nyatoolkit/\r
  28  *      <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>\r
  29  * \r
  30  */\r
  31 package jp.nyatla.nyartoolkit.core.pca2d;\r
  32 \r
  33 import jp.nyatla.nyartoolkit.NyARException;\r
  34 import jp.nyatla.nyartoolkit.core.types.*;\r
  35 import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
  36 \r
  37 /**\r
  38  * ARToolkitのPCA関数を二次元に特化させて単純化したもの\r
  39  *\r
  40  */\r
  41 public class NyARPca2d_MatrixPCA_O2 implements INyARPca2d\r
  42 {\r
  43         private static final double PCA_EPS = 1e-6; // #define EPS 1e-6\r
  44 \r
  45         private static final int PCA_MAX_ITER = 100; // #define MAX_ITER 100\r
  46 \r
  47         private static final double PCA_VZERO = 1e-16; // #define VZERO 1e-16\r
  48 \r
  49         /**\r
  50          * static int QRM( ARMat *a, ARVec *dv )の代替関数\r
  51          * \r
  52          * @param a\r
  53          * @param dv\r
  54          * @throws NyARException\r
  55          */\r
  56         private static void PCA_QRM(NyARDoubleMatrix22 o_matrix, double[] dv) throws NyARException\r
  57         {\r
  58                 double w, t, s, x, y, c;\r
  59                 double ev1;\r
  60                 double dv_x,dv_y;\r
  61                 double mat00,mat01,mat10,mat11;\r
  62                 // <this.vecTridiagonalize2d(i_mat, dv, ev);>\r
  63                 dv_x = o_matrix.m00;// this.m[dim - 2][dim - 2];// d.v[dim-2]=a.m[dim-2][dim-2];//d->v[dim-2]=a->m[(dim-2)*dim+(dim-2)];\r
  64                 ev1 = o_matrix.m01;// this.m[dim - 2][dim - 1];// e.v[dim-2+i_e_start]=a.m[dim-2][dim-1];//e->v[dim-2] = a->m[(dim-2)*dim+(dim-1)];\r
  65                 dv_y = o_matrix.m11;// this.m[dim - 1][dim - 1];// d.v[dim-1]=a_array[dim-1][dim-1];//d->v[dim-1] =a->m[(dim-1)*dim+(dim-1)];\r
  66                 // 単位行列にする。\r
  67                 mat00 = mat11 = 1;\r
  68                 mat01 = mat10 = 0;\r
  69                 // </this.vecTridiagonalize2d(i_mat, dv, ev);>\r
  70 \r
  71                 // int j = 1;\r
  72                 // // while(j>0 && fabs(ev->v[j])>EPS*(fabs(dv->v[j-1])+fabs(dv->v[j])))\r
  73                 // while (j > 0 && Math.abs(ev1) > PCA_EPS * (Math.abs(dv.x) + Math.abs(dv.y))) {\r
  74                 // j--;\r
  75                 // }\r
  76                 // if (j == 0) {\r
  77                 int iter = 0;\r
  78                 do {\r
  79                         iter++;\r
  80                         if (iter > PCA_MAX_ITER) {\r
  81                                 break;\r
  82                         }\r
  83                         w = (dv_x - dv_y) / 2;// w = (dv->v[h-1] -dv->v[h]) / 2;//ここ？\r
  84                         t = ev1 * ev1;// t = ev->v[h] * ev->v[h];\r
  85                         s = Math.sqrt(w * w + t);\r
  86                         if (w < 0) {\r
  87                                 s = -s;\r
  88                         }\r
  89                         x = dv_x - dv_y + t / (w + s);// x = dv->v[j] -dv->v[h] +t/(w+s);\r
  90                         y = ev1;// y = ev->v[j+1];\r
  91 \r
  92                         if (Math.abs(x) >= Math.abs(y)) {\r
  93                                 if (Math.abs(x) > PCA_VZERO) {\r
  94                                         t = -y / x;\r
  95                                         c = 1 / Math.sqrt(t * t + 1);\r
  96                                         s = t * c;\r
  97                                 } else {\r
  98                                         c = 1.0;\r
  99                                         s = 0.0;\r
 100                                 }\r
 101                         } else {\r
 102                                 t = -x / y;\r
 103                                 s = 1.0 / Math.sqrt(t * t + 1);\r
 104                                 c = t * s;\r
 105                         }\r
 106                         w = dv_x - dv_y;// w = dv->v[k] -dv->v[k+1];\r
 107                         t = (w * s + 2 * c * ev1) * s;// t = (w * s +2 * c *ev->v[k+1]) *s;\r
 108                         dv_x -= t;// dv->v[k] -= t;\r
 109                         dv_y += t;// dv->v[k+1] += t;\r
 110                         ev1 += s * (c * w - 2 * s * ev1);// ev->v[k+1]+= s * (c* w- 2* s *ev->v[k+1]);\r
 111 \r
 112                         x = mat00;// x = a->m[k*dim+i];\r
 113                         y = mat10;// y = a->m[(k+1)*dim+i];\r
 114                         mat00 = c * x - s * y;// a->m[k*dim+i] = c * x - s* y;\r
 115                         mat10 = s * x + c * y;// a->m[(k+1)*dim+i] = s* x + c * y;\r
 116                         \r
 117                         x = mat01;// x = a->m[k*dim+i];\r
 118                         y = mat11;// y = a->m[(k+1)*dim+i];\r
 119                         mat01 = c * x - s * y;// a->m[k*dim+i] = c * x - s* y;\r
 120                         mat11 = s * x + c * y;// a->m[(k+1)*dim+i] = s* x + c * y;\r
 121                 } while (Math.abs(ev1) > PCA_EPS * (Math.abs(dv_x) + Math.abs(dv_y)));\r
 122                 // }\r
 123 \r
 124                 t = dv_x;// t = dv->v[h];\r
 125                 if (dv_y > t) {// if( dv->v[i] > t ) {\r
 126                         t = dv_y;// t = dv->v[h];\r
 127                         dv_y = dv_x;// dv->v[h] = dv->v[k];\r
 128                         dv_x = t;// dv->v[k] = t;\r
 129                         // 行の入れ替え\r
 130                         o_matrix.m00 = mat10;\r
 131                         o_matrix.m01 = mat11;\r
 132                         o_matrix.m10 = mat00;           \r
 133                         o_matrix.m11 = mat01;\r
 134                         \r
 135                 } else {\r
 136                         // 行の入れ替えはなし\r
 137                         o_matrix.m00 = mat00;\r
 138                         o_matrix.m01 = mat01;\r
 139                         o_matrix.m10 = mat10;           \r
 140                         o_matrix.m11 = mat11;\r
 141                 }\r
 142                 dv[0]=dv_x;\r
 143                 dv[1]=dv_y;\r
 144                 return;\r
 145         }\r
 146 \r
 147         /**\r
 148          * static int PCA( ARMat *input, ARMat *output, ARVec *ev )\r
 149          * \r
 150          * @param output\r
 151          * @param o_ev\r
 152          * @throws NyARException\r
 153          */\r
 154         private void PCA_PCA(double[] i_v1,double[] i_v2,int i_number_of_data,NyARDoubleMatrix22 o_matrix, double[] o_ev,double[] o_mean) throws NyARException\r
 155         {\r
 156                 // double[] mean_array=mean.getArray();\r
 157                 // mean.zeroClear();\r
 158 \r
 159                 //PCA_EXの処理\r
 160                 double sx = 0;\r
 161                 double sy = 0;\r
 162                 for (int i = 0; i < i_number_of_data; i++) {\r
 163                         sx += i_v1[i];\r
 164                         sy += i_v2[i];\r
 165                 }\r
 166                 sx = sx / i_number_of_data;\r
 167                 sy = sy / i_number_of_data;\r
 168                 \r
 169                 //PCA_CENTERとPCA_xt_by_xを一緒に処理\r
 170                 final double srow = Math.sqrt((double) i_number_of_data);\r
 171                 double w00, w11, w10;\r
 172                 w00 = w11 = w10 = 0.0;// *out = 0.0;\r
 173                 for (int i = 0; i < i_number_of_data; i++) {\r
 174                         final double x = (i_v1[i] - sx) / srow;\r
 175                         final double y = (i_v2[i] - sy) / srow;\r
 176                         w00 += (x * x);// *out += *in1 * *in2;\r
 177                         w10 += (x * y);// *out += *in1 * *in2;\r
 178                         w11 += (y * y);// *out += *in1 * *in2;\r
 179                 }\r
 180                 o_matrix.m00=w00;\r
 181                 o_matrix.m01=o_matrix.m10=w10;\r
 182                 o_matrix.m11=w11;\r
 183                 \r
 184                 //PCA_PCAの処理\r
 185                 PCA_QRM(o_matrix, o_ev);\r
 186                 // m2 = o_output.m;// m2 = output->m;\r
 187                 if (o_ev[0] < PCA_VZERO) {// if( ev->v[i] < VZERO ){\r
 188                         o_ev[0] = 0.0;// ev->v[i] = 0.0;\r
 189                         o_matrix.m00 = 0.0;// *(m2++) = 0.0;\r
 190                         o_matrix.m01 = 0.0;// *(m2++) = 0.0;\r
 191                 }\r
 192 \r
 193                 if (o_ev[1] < PCA_VZERO) {// if( ev->v[i] < VZERO ){\r
 194                         o_ev[1] = 0.0;// ev->v[i] = 0.0;\r
 195                         o_matrix.m10 = 0.0;// *(m2++) = 0.0;\r
 196                         o_matrix.m11 = 0.0;// *(m2++) = 0.0;\r
 197                 }\r
 198                 o_mean[0]=sx;\r
 199                 o_mean[1]=sy;\r
 200                 // }\r
 201                 return;\r
 202         }\r
 203         public void pca(double[] i_v1,double[] i_v2,int i_number_of_point,NyARDoubleMatrix22 o_evec, double[] o_ev,double[] o_mean) throws NyARException\r
 204         {\r
 205                 PCA_PCA(i_v1,i_v2,i_number_of_point,o_evec, o_ev,o_mean);\r
 206 \r
 207                 final double sum = o_ev[0] + o_ev[1];\r
 208                 // For順変更禁止\r
 209                 o_ev[0] /= sum;// ev->v[i] /= sum;\r
 210                 o_ev[1] /= sum;// ev->v[i] /= sum;\r
 211                 return; \r
 212         }\r
 213 \r
 214 }\r