tags/2.2.0/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatPca2d.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.sandbox.x2;\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 import jp.nyatla.nyartoolkit.core.pca2d.*;\r
  38 import jp.nyatla.nyartoolkit.core2.types.NyARI64Point2d;\r
  39 import jp.nyatla.nyartoolkit.core2.types.matrix.NyARI64Matrix22;\r
  40 /**\r
  41  * 64bit(小数部16bit)の固定小数点を利用したPCA関数\r
  42  *\r
  43  */\r
  44 public class NyARFixedFloatPca2d implements INyARPca2d\r
  45 {\r
  46 //      private static final double PCA_EPS = 1e-6; // #define EPS 1e-6\r
  47 \r
  48         private static final int PCA_MAX_ITER = 100; // #define MAX_ITER 100\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(NyARI64Matrix22 o_matrix, NyARI64Point2d dv) throws NyARException\r
  57         {\r
  58                 long abs_x;\r
  59                 long w, t, s, x, y, c;\r
  60                 long ev1;\r
  61                 long dv_x,dv_y;\r
  62                 long 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<<16);\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) / 2L;//S16\r
  85                         t = (ev1 * ev1)>>16;// t = ev->v[h] * ev->v[h];\r
  86                         s = NyMath.sqrtFixdFloat16(((w * w)>>16) + t);\r
  87                         if (w < 0) {\r
  88                                 s = -s;\r
  89                         }\r
  90                         x = dv_x - dv_y + (t<<16) / (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                         abs_x=(x>0?x:-x);\r
  94                         if (abs_x >= (y>0?y:-y)) { //if (Math.abs(x) >= Math.abs(y)) {\r
  95                                 if ((abs_x>>16) > 0) {\r
  96                                         t = -(y<<16) / x;\r
  97                                         c = (1L<<32) / NyMath.sqrtFixdFloat16(((t * t)>>16) + (1L<<16));\r
  98                                         s = (t * c)>>16;\r
  99                                 } else {\r
 100                                         c = (1L<<16);\r
 101                                         s = 0;\r
 102                                 }\r
 103                         } else {\r
 104                                 t = -(x<<16) / y;\r
 105                                 s = (1L<<32) / NyMath.sqrtFixdFloat16(((t * t)>>16) + (1L<<16));\r
 106                                 c = (t * s)>>16;\r
 107                         }\r
 108                         w = dv_x - dv_y;// w = dv->v[k] -dv->v[k+1];\r
 109                         t = (((w * s + 2L * c * ev1)>>16) * s)>>16;// t = (w * s +2 * c *ev->v[k+1]) *s;\r
 110                         dv_x -= t;// dv->v[k] -= t;\r
 111                         dv_y += t;// dv->v[k+1] += t;\r
 112                         ev1 += (s * ((c * w - 2L * s * ev1)>>16)>>16);// ev->v[k+1]+= s * (c* w- 2* s *ev->v[k+1]);\r
 113 \r
 114                         x = mat00;// x = a->m[k*dim+i];\r
 115                         y = mat10;// y = a->m[(k+1)*dim+i];\r
 116                         mat00 = (c * x - s * y)>>16;// a->m[k*dim+i] = c * x - s* y;\r
 117                         mat10 = (s * x + c * y)>>16;// a->m[(k+1)*dim+i] = s* x + c * y;\r
 118                         \r
 119                         x = mat01;// x = a->m[k*dim+i];\r
 120                         y = mat11;// y = a->m[(k+1)*dim+i];\r
 121                         mat01 = (c * x - s * y)>>16;// a->m[k*dim+i] = c * x - s* y;\r
 122                         mat11 = (s * x + c * y)>>16;// a->m[(k+1)*dim+i] = s* x + c * y;\r
 123                 } while (((ev1>0?ev1:-ev1)>>16) > ((((dv_x>0?dv_x:-dv_x) + (dv_y>0?dv_y:-dv_y))>>16)/1000000L));\r
 124                 // }\r
 125 \r
 126                 t = dv_x;// t = dv->v[h];\r
 127                 if (dv_y > t) {// if( dv->v[i] > t ) {\r
 128                         t = dv_y;// t = dv->v[h];\r
 129                         dv_y = dv_x;// dv->v[h] = dv->v[k];\r
 130                         dv_x = t;// dv->v[k] = t;\r
 131                         // 行の入れ替え\r
 132                         o_matrix.m00 = mat10;\r
 133                         o_matrix.m01 = mat11;\r
 134                         o_matrix.m10 = mat00;           \r
 135                         o_matrix.m11 = mat01;\r
 136                         \r
 137                 } else {\r
 138                         // 行の入れ替えはなし\r
 139                         o_matrix.m00 = mat00;\r
 140                         o_matrix.m01 = mat01;\r
 141                         o_matrix.m10 = mat10;           \r
 142                         o_matrix.m11 = mat11;\r
 143                 }\r
 144                 dv.x=dv_x;\r
 145                 dv.y=dv_y;\r
 146                 return; \r
 147         }\r
 148 \r
 149         /**\r
 150          * static int PCA( ARMat *input, ARMat *output, ARVec *ev )\r
 151          * \r
 152          * @param output\r
 153          * @param o_ev\r
 154          * @throws NyARException\r
 155          */\r
 156         /**\r
 157          * static int PCA( ARMat *input, ARMat *output, ARVec *ev )\r
 158          * \r
 159          * @param output\r
 160          * @param o_ev\r
 161          * @throws NyARException\r
 162          */\r
 163         private void PCA_PCA(int[] i_x,int[] i_y,int i_number_of_data,NyARI64Matrix22 o_matrix, NyARI64Point2d o_ev,NyARI64Point2d o_mean) throws NyARException\r
 164         {\r
 165                 // double[] mean_array=mean.getArray();\r
 166                 // mean.zeroClear();\r
 167 \r
 168                 //PCA_EXの処理\r
 169                 long sx = 0;\r
 170                 long sy = 0;\r
 171                 for (int i = 0; i < i_number_of_data; i++) {\r
 172                         sx += i_x[i];//S16\r
 173                         sy += i_y[i];//S16\r
 174                 }\r
 175                 sx = sx / i_number_of_data;//S16\r
 176                 sy = sy / i_number_of_data;//S16\r
 177                 \r
 178                 //PCA_CENTERとPCA_xt_by_xを一緒に処理\r
 179                 final long srow = NyMath.sqrtFixdFloat16((long)i_number_of_data<<16);//S16\r
 180                 long w00, w11, w10;\r
 181                 w00 = w11 = w10 = 0L;// *out = 0.0;\r
 182                 for (int i = 0; i < i_number_of_data; i++) {\r
 183                         final long x = ((i_x[i] - sx)<<16) / srow;//S6\r
 184                         final long y = ((i_y[i] - sy)<<16) / srow;//S6\r
 185                         w00 += (x * x)>>16;//S16\r
 186                         w10 += (x * y)>>16;//S16\r
 187                         w11 += (y * y)>>16;//S16\r
 188                 }\r
 189                 o_matrix.m00=w00;\r
 190                 o_matrix.m01=o_matrix.m10=w10;\r
 191                 o_matrix.m11=w11;\r
 192                 \r
 193                 //PCA_PCAの処理\r
 194                 PCA_QRM(o_matrix, o_ev);\r
 195                 // m2 = o_output.m;// m2 = output->m;\r
 196                 if (o_ev.x < 0) {// if( ev->v[i] < VZERO ){\r
 197                         o_ev.x = 0;// ev->v[i] = 0.0;\r
 198                         o_matrix.m00 = 0;// *(m2++) = 0.0;\r
 199                         o_matrix.m01 = 0;// *(m2++) = 0.0;\r
 200                 }\r
 201 \r
 202                 if (o_ev.y < 0) {// if( ev->v[i] < VZERO ){\r
 203                         o_ev.y = 0;// ev->v[i] = 0.0;\r
 204                         o_matrix.m10 = 0;// *(m2++) = 0.0;\r
 205                         o_matrix.m11 = 0;// *(m2++) = 0.0;\r
 206                 }\r
 207                 o_mean.x=sx;\r
 208                 o_mean.y=sy;\r
 209                 // }\r
 210                 return;\r
 211         }\r
 212         private int[] __pca_tmpx=null;\r
 213         private int[] __pca_tmpy=null;\r
 214         private NyARI64Matrix22 __pca_evec=null;\r
 215         private NyARI64Point2d __pca_ev=null;\r
 216         private NyARI64Point2d __pca_mean=null;\r
 217         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
 218         {\r
 219                 //変換用のワーク変数作成\r
 220                 if(__pca_tmpx==null)\r
 221                 {\r
 222                         this.__pca_tmpx=new int[i_number_of_point];\r
 223                         this.__pca_tmpy=new int[i_number_of_point];\r
 224                         this.__pca_evec=new NyARI64Matrix22 ();\r
 225                         this.__pca_ev=new NyARI64Point2d ();\r
 226                         this.__pca_mean=new NyARI64Point2d ();\r
 227                 }else if(i_number_of_point>this.__pca_tmpx.length)\r
 228                 {\r
 229                         this.__pca_tmpx=new int[i_number_of_point];\r
 230                         this.__pca_tmpy=new int[i_number_of_point];\r
 231                 }\r
 232                 //値のセット\r
 233                 final int[] x_ptr=this.__pca_tmpx;\r
 234                 final int[] y_ptr=this.__pca_tmpy;\r
 235                 for(int i=0;i<i_number_of_point;i++)\r
 236                 {\r
 237                         x_ptr[i]=(int)(i_x[i]*65536L);\r
 238                         y_ptr[i]=(int)(i_y[i]*65536L);\r
 239                 }\r
 240                 //計算\r
 241                 pcaF16(x_ptr,y_ptr,i_number_of_point,this.__pca_evec,this.__pca_ev,this.__pca_mean);\r
 242                 //結果値を変換\r
 243                 o_evec.m00=(double)this.__pca_evec.m00/65536.0;\r
 244                 o_evec.m01=(double)this.__pca_evec.m01/65536.0;\r
 245                 o_evec.m10=(double)this.__pca_evec.m10/65536.0;\r
 246                 o_evec.m11=(double)this.__pca_evec.m11/65536.0;\r
 247                 o_ev.x=this.__pca_ev.x/65536.0;\r
 248                 o_ev.y=this.__pca_ev.y/65536.0;\r
 249                 o_mean.x=this.__pca_mean.x/65536.0;\r
 250                 o_mean.y=this.__pca_mean.y/65536.0;\r
 251                 return; \r
 252         }\r
 253         /**\r
 254          * 値は全て小数点部16bitの固定小数点です。\r
 255          * @param i_x\r
 256          * @param i_y\r
 257          * @param i_number_of_point\r
 258          * @param o_evec\r
 259          * @param o_ev\r
 260          * @param o_mean\r
 261          * @throws NyARException\r
 262          */\r
 263         public void pcaF16(int[] i_x,int[] i_y,int i_number_of_point,NyARI64Matrix22 o_evec, NyARI64Point2d o_ev,NyARI64Point2d o_mean) throws NyARException\r
 264         {\r
 265                 //計算\r
 266                 PCA_PCA(i_x,i_y,i_number_of_point,o_evec,o_ev,o_mean);\r
 267                 final long sum = o_ev.x + o_ev.y;\r
 268                 // For順変更禁止\r
 269                 o_ev.x = (o_ev.x<<16)/sum;// ev->v[i] /= sum;\r
 270                 o_ev.y = (o_ev.y<<16)/sum;// ev->v[i] /= sum;\r
 271                 return; \r
 272         }       \r
 273 }\r