tags/2.3.1/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 version ARToolkit class library.\r
  11  * Copyright (C)2008 R.Iizuka\r
  12  *\r
  13  * This program is free software; you can redistribute it and/or\r
  14  * modify it under the terms of the GNU General Public License\r
  15  * as published by the Free Software Foundation; either version 2\r
  16  * of the License, or (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 framework; if not, write to the Free Software\r
  25  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  26  * \r
  27  * For further information please contact.\r
  28  *      http://nyatla.jp/nyatoolkit/\r
  29  *      <airmail(at)ebony.plala.or.jp>\r
  30  * \r
  31  */\r
  32 package jp.nyatla.nyartoolkit.core.pca2d;\r
  33 \r
  34 import jp.nyatla.nyartoolkit.NyARException;\r
  35 import jp.nyatla.nyartoolkit.core.types.*;\r
  36 import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
  37 \r
  38 /**\r
  39  * ARToolkitのPCA関数を二次元に特化させて単純化したもの\r
  40  *\r
  41  */\r
  42 public class NyARPca2d_MatrixPCA_O2 implements INyARPca2d\r
  43 {\r
  44         private static final double PCA_EPS = 1e-6; // #define EPS 1e-6\r
  45 \r
  46         private static final int PCA_MAX_ITER = 100; // #define MAX_ITER 100\r
  47 \r
  48         private static final double PCA_VZERO = 1e-16; // #define VZERO 1e-16\r
  49 \r
  50         /**\r
  51          * static int QRM( ARMat *a, ARVec *dv )の代替関数\r
  52          * \r
  53          * @param a\r
  54          * @param dv\r
  55          * @throws NyARException\r
  56          */\r
  57         private static void PCA_QRM(NyARDoubleMatrix22 o_matrix, NyARDoublePoint2d dv) throws NyARException\r
  58         {\r
  59                 double w, t, s, x, y, c;\r
  60                 double ev1;\r
  61                 double dv_x,dv_y;\r
  62                 double mat00,mat01,mat10,mat11;\r
  63                 // <this.vecTridiagonalize2d(i_mat, dv, ev);>\r
  64                 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
  65                 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
  66                 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
  67                 // 単位行列にする。\r
  68                 mat00 = mat11 = 1;\r
  69                 mat01 = mat10 = 0;\r
  70                 // </this.vecTridiagonalize2d(i_mat, dv, ev);>\r
  71 \r
  72                 // int j = 1;\r
  73                 // // while(j>0 && fabs(ev->v[j])>EPS*(fabs(dv->v[j-1])+fabs(dv->v[j])))\r
  74                 // while (j > 0 && Math.abs(ev1) > PCA_EPS * (Math.abs(dv.x) + Math.abs(dv.y))) {\r
  75                 // j--;\r
  76                 // }\r
  77                 // if (j == 0) {\r
  78                 int iter = 0;\r
  79                 do {\r
  80                         iter++;\r
  81                         if (iter > PCA_MAX_ITER) {\r
  82                                 break;\r
  83                         }\r
  84                         w = (dv_x - dv_y) / 2;// w = (dv->v[h-1] -dv->v[h]) / 2;//ここ？\r
  85                         t = ev1 * ev1;// t = ev->v[h] * ev->v[h];\r
  86                         s = Math.sqrt(w * w + t);\r
  87                         if (w < 0) {\r
  88                                 s = -s;\r
  89                         }\r
  90                         x = dv_x - dv_y + t / (w + s);// x = dv->v[j] -dv->v[h] +t/(w+s);\r
  91                         y = ev1;// y = ev->v[j+1];\r
  92 \r
  93                         if (Math.abs(x) >= Math.abs(y)) {\r
  94                                 if (Math.abs(x) > PCA_VZERO) {\r
  95                                         t = -y / x;\r
  96                                         c = 1 / Math.sqrt(t * t + 1);\r
  97                                         s = t * c;\r
  98                                 } else {\r
  99                                         c = 1.0;\r
 100                                         s = 0.0;\r
 101                                 }\r
 102                         } else {\r
 103                                 t = -x / y;\r
 104                                 s = 1.0 / Math.sqrt(t * t + 1);\r
 105                                 c = t * s;\r
 106                         }\r
 107                         w = dv_x - dv_y;// w = dv->v[k] -dv->v[k+1];\r
 108                         t = (w * s + 2 * c * ev1) * s;// t = (w * s +2 * c *ev->v[k+1]) *s;\r
 109                         dv_x -= t;// dv->v[k] -= t;\r
 110                         dv_y += t;// dv->v[k+1] += t;\r
 111                         ev1 += s * (c * w - 2 * s * ev1);// ev->v[k+1]+= s * (c* w- 2* s *ev->v[k+1]);\r
 112 \r
 113                         x = mat00;// x = a->m[k*dim+i];\r
 114                         y = mat10;// y = a->m[(k+1)*dim+i];\r
 115                         mat00 = c * x - s * y;// a->m[k*dim+i] = c * x - s* y;\r
 116                         mat10 = s * x + c * y;// a->m[(k+1)*dim+i] = s* x + c * y;\r
 117                         \r
 118                         x = mat01;// x = a->m[k*dim+i];\r
 119                         y = mat11;// y = a->m[(k+1)*dim+i];\r
 120                         mat01 = c * x - s * y;// a->m[k*dim+i] = c * x - s* y;\r
 121                         mat11 = s * x + c * y;// a->m[(k+1)*dim+i] = s* x + c * y;\r
 122                 } while (Math.abs(ev1) > PCA_EPS * (Math.abs(dv_x) + Math.abs(dv_y)));\r
 123                 // }\r
 124 \r
 125                 t = dv_x;// t = dv->v[h];\r
 126                 if (dv_y > t) {// if( dv->v[i] > t ) {\r
 127                         t = dv_y;// t = dv->v[h];\r
 128                         dv_y = dv_x;// dv->v[h] = dv->v[k];\r
 129                         dv_x = t;// dv->v[k] = t;\r
 130                         // 行の入れ替え\r
 131                         o_matrix.m00 = mat10;\r
 132                         o_matrix.m01 = mat11;\r
 133                         o_matrix.m10 = mat00;           \r
 134                         o_matrix.m11 = mat01;\r
 135                         \r
 136                 } else {\r
 137                         // 行の入れ替えはなし\r
 138                         o_matrix.m00 = mat00;\r
 139                         o_matrix.m01 = mat01;\r
 140                         o_matrix.m10 = mat10;           \r
 141                         o_matrix.m11 = mat11;\r
 142                 }\r
 143                 dv.x=dv_x;\r
 144                 dv.y=dv_y;\r
 145                 return;\r
 146         }\r
 147 \r
 148         /**\r
 149          * static int PCA( ARMat *input, ARMat *output, ARVec *ev )\r
 150          * \r
 151          * @param output\r
 152          * @param o_ev\r
 153          * @throws NyARException\r
 154          */\r
 155         private void PCA_PCA(double[] i_x,double[] i_y,int i_number_of_data,NyARDoubleMatrix22 o_matrix, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
 156         {\r
 157                 // double[] mean_array=mean.getArray();\r
 158                 // mean.zeroClear();\r
 159 \r
 160                 //PCA_EXの処理\r
 161                 double sx = 0;\r
 162                 double sy = 0;\r
 163                 for (int i = 0; i < i_number_of_data; i++) {\r
 164                         sx += i_x[i];\r
 165                         sy += i_y[i];\r
 166                 }\r
 167                 sx = sx / i_number_of_data;\r
 168                 sy = sy / i_number_of_data;\r
 169                 \r
 170                 //PCA_CENTERとPCA_xt_by_xを一緒に処理\r
 171                 final double srow = Math.sqrt((double) i_number_of_data);\r
 172                 double w00, w11, w10;\r
 173                 w00 = w11 = w10 = 0.0;// *out = 0.0;\r
 174                 for (int i = 0; i < i_number_of_data; i++) {\r
 175                         final double x = (i_x[i] - sx) / srow;\r
 176                         final double y = (i_y[i] - sy) / srow;\r
 177                         w00 += (x * x);// *out += *in1 * *in2;\r
 178                         w10 += (x * y);// *out += *in1 * *in2;\r
 179                         w11 += (y * y);// *out += *in1 * *in2;\r
 180                 }\r
 181                 o_matrix.m00=w00;\r
 182                 o_matrix.m01=o_matrix.m10=w10;\r
 183                 o_matrix.m11=w11;\r
 184                 \r
 185                 //PCA_PCAの処理\r
 186                 PCA_QRM(o_matrix, o_ev);\r
 187                 // m2 = o_output.m;// m2 = output->m;\r
 188                 if (o_ev.x < PCA_VZERO) {// if( ev->v[i] < VZERO ){\r
 189                         o_ev.x = 0.0;// ev->v[i] = 0.0;\r
 190                         o_matrix.m00 = 0.0;// *(m2++) = 0.0;\r
 191                         o_matrix.m01 = 0.0;// *(m2++) = 0.0;\r
 192                 }\r
 193 \r
 194                 if (o_ev.y < PCA_VZERO) {// if( ev->v[i] < VZERO ){\r
 195                         o_ev.y = 0.0;// ev->v[i] = 0.0;\r
 196                         o_matrix.m10 = 0.0;// *(m2++) = 0.0;\r
 197                         o_matrix.m11 = 0.0;// *(m2++) = 0.0;\r
 198                 }\r
 199                 o_mean.x=sx;\r
 200                 o_mean.y=sy;\r
 201                 // }\r
 202                 return;\r
 203         }\r
 204         public void pca(double[] i_x,double[] i_y,int i_number_of_point,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
 205         {\r
 206                 PCA_PCA(i_x,i_y,i_number_of_point,o_evec, o_ev,o_mean);\r
 207 \r
 208                 final double sum = o_ev.x + o_ev.y;\r
 209                 // For順変更禁止\r
 210                 o_ev.x /= sum;// ev->v[i] /= sum;\r
 211                 o_ev.y /= sum;// ev->v[i] /= sum;\r
 212                 return; \r
 213         }\r
 214 \r
 215 }\r