tags/2.4.1/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 /**\r
  39  * 64bit(小数部16bit)の固定小数点を利用したPCA関数\r
  40  *\r
  41  */\r
  42 public class NyARFixedFloatPca2d 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          * static int QRM( ARMat *a, ARVec *dv )の代替関数\r
  49          * \r
  50          * @param a\r
  51          * @param dv\r
  52          * @throws NyARException\r
  53          */\r
  54         private static void PCA_QRM(NyARI64Matrix22 o_matrix, NyARI64Point2d dv) throws NyARException\r
  55         {\r
  56                 long abs_x;\r
  57                 long w, t, s, x, y, c;\r
  58                 long ev1;\r
  59                 long dv_x,dv_y;\r
  60                 long mat00,mat01,mat10,mat11;\r
  61                 // <this.vecTridiagonalize2d(i_mat, dv, ev);>\r
  62                 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
  63                 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
  64                 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
  65                 // 単位行列にする。\r
  66                 mat00 = mat11 = (1<<16);\r
  67                 mat01 = mat10 = 0;\r
  68                 // </this.vecTridiagonalize2d(i_mat, dv, ev);>\r
  69 \r
  70                 // int j = 1;\r
  71                 // // while(j>0 && fabs(ev->v[j])>EPS*(fabs(dv->v[j-1])+fabs(dv->v[j])))\r
  72                 // while (j > 0 && Math.abs(ev1) > PCA_EPS * (Math.abs(dv.x) + Math.abs(dv.y))) {\r
  73                 // j--;\r
  74                 // }\r
  75                 // if (j == 0) {\r
  76                 int iter = 0;\r
  77                 do {\r
  78                         iter++;\r
  79                         if (iter > PCA_MAX_ITER) {\r
  80                                 break;\r
  81                         }\r
  82                         w = (dv_x - dv_y) / 2L;//S16\r
  83                         t = (ev1 * ev1)>>16;// t = ev->v[h] * ev->v[h];\r
  84                         s = NyMath.sqrtFixdFloat16(((w * w)>>16) + t);\r
  85                         if (w < 0) {\r
  86                                 s = -s;\r
  87                         }\r
  88                         x = dv_x - dv_y + (t<<16) / (w + s);// x = dv->v[j] -dv->v[h] +t/(w+s);\r
  89                         y = ev1;// y = ev->v[j+1];\r
  90 \r
  91                         abs_x=(x>0?x:-x);\r
  92                         if (abs_x >= (y>0?y:-y)) { //if (Math.abs(x) >= Math.abs(y)) {\r
  93                                 if ((abs_x>>16) > 0) {\r
  94                                         t = -(y<<16) / x;\r
  95                                         c = (1L<<32) / NyMath.sqrtFixdFloat16(((t * t)>>16) + (1L<<16));\r
  96                                         s = (t * c)>>16;\r
  97                                 } else {\r
  98                                         c = (1L<<16);\r
  99                                         s = 0;\r
 100                                 }\r
 101                         } else {\r
 102                                 t = -(x<<16) / y;\r
 103                                 s = (1L<<32) / NyMath.sqrtFixdFloat16(((t * t)>>16) + (1L<<16));\r
 104                                 c = (t * s)>>16;\r
 105                         }\r
 106                         w = dv_x - dv_y;// w = dv->v[k] -dv->v[k+1];\r
 107                         t = (((w * s + 2L * c * ev1)>>16) * s)>>16;// 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 - 2L * s * ev1)>>16)>>16);// 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)>>16;// a->m[k*dim+i] = c * x - s* y;\r
 115                         mat10 = (s * x + c * y)>>16;// 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)>>16;// a->m[k*dim+i] = c * x - s* y;\r
 120                         mat11 = (s * x + c * y)>>16;// a->m[(k+1)*dim+i] = s* x + c * y;\r
 121                 } while (((ev1>0?ev1:-ev1)>>16) > ((((dv_x>0?dv_x:-dv_x) + (dv_y>0?dv_y:-dv_y))>>16)/1000000L));\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.x=dv_x;\r
 143                 dv.y=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         /**\r
 155          * static int PCA( ARMat *input, ARMat *output, ARVec *ev )\r
 156          * \r
 157          * @param output\r
 158          * @param o_ev\r
 159          * @throws NyARException\r
 160          */\r
 161         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
 162         {\r
 163                 // double[] mean_array=mean.getArray();\r
 164                 // mean.zeroClear();\r
 165 \r
 166                 //PCA_EXの処理\r
 167                 long sx = 0;\r
 168                 long sy = 0;\r
 169                 for (int i = 0; i < i_number_of_data; i++) {\r
 170                         sx += i_x[i];//S16\r
 171                         sy += i_y[i];//S16\r
 172                 }\r
 173                 sx = sx / i_number_of_data;//S16\r
 174                 sy = sy / i_number_of_data;//S16\r
 175                 \r
 176                 //PCA_CENTERとPCA_xt_by_xを一緒に処理\r
 177                 final long srow = NyMath.sqrtFixdFloat16((long)i_number_of_data<<16);//S16\r
 178                 long w00, w11, w10;\r
 179                 w00 = w11 = w10 = 0L;// *out = 0.0;\r
 180                 for (int i = 0; i < i_number_of_data; i++) {\r
 181                         final long x = ((i_x[i] - sx)<<16) / srow;//S6\r
 182                         final long y = ((i_y[i] - sy)<<16) / srow;//S6\r
 183                         w00 += (x * x)>>16;//S16\r
 184                         w10 += (x * y)>>16;//S16\r
 185                         w11 += (y * y)>>16;//S16\r
 186                 }\r
 187                 o_matrix.m00=w00;\r
 188                 o_matrix.m01=o_matrix.m10=w10;\r
 189                 o_matrix.m11=w11;\r
 190                 \r
 191                 //PCA_PCAの処理\r
 192                 PCA_QRM(o_matrix, o_ev);\r
 193                 // m2 = o_output.m;// m2 = output->m;\r
 194                 if (o_ev.x < 0) {// if( ev->v[i] < VZERO ){\r
 195                         o_ev.x = 0;// ev->v[i] = 0.0;\r
 196                         o_matrix.m00 = 0;// *(m2++) = 0.0;\r
 197                         o_matrix.m01 = 0;// *(m2++) = 0.0;\r
 198                 }\r
 199 \r
 200                 if (o_ev.y < 0) {// if( ev->v[i] < VZERO ){\r
 201                         o_ev.y = 0;// ev->v[i] = 0.0;\r
 202                         o_matrix.m10 = 0;// *(m2++) = 0.0;\r
 203                         o_matrix.m11 = 0;// *(m2++) = 0.0;\r
 204                 }\r
 205                 o_mean.x=sx;\r
 206                 o_mean.y=sy;\r
 207                 // }\r
 208                 return;\r
 209         }\r
 210         private int[] __pca_tmpx=null;\r
 211         private int[] __pca_tmpy=null;\r
 212         private NyARI64Matrix22 __pca_evec=null;\r
 213         private NyARI64Point2d __pca_ev=null;\r
 214         private NyARI64Point2d __pca_mean=null;\r
 215         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
 216         {\r
 217                 //変換用のワーク変数作成\r
 218                 if(__pca_tmpx==null)\r
 219                 {\r
 220                         this.__pca_tmpx=new int[i_number_of_point];\r
 221                         this.__pca_tmpy=new int[i_number_of_point];\r
 222                         this.__pca_evec=new NyARI64Matrix22 ();\r
 223                         this.__pca_ev=new NyARI64Point2d ();\r
 224                         this.__pca_mean=new NyARI64Point2d ();\r
 225                 }else if(i_number_of_point>this.__pca_tmpx.length)\r
 226                 {\r
 227                         this.__pca_tmpx=new int[i_number_of_point];\r
 228                         this.__pca_tmpy=new int[i_number_of_point];\r
 229                 }\r
 230                 //値のセット\r
 231                 final int[] x_ptr=this.__pca_tmpx;\r
 232                 final int[] y_ptr=this.__pca_tmpy;\r
 233                 for(int i=0;i<i_number_of_point;i++)\r
 234                 {\r
 235                         x_ptr[i]=(int)(i_x[i]*65536L);\r
 236                         y_ptr[i]=(int)(i_y[i]*65536L);\r
 237                 }\r
 238                 //計算\r
 239                 pcaF16(x_ptr,y_ptr,i_number_of_point,this.__pca_evec,this.__pca_ev,this.__pca_mean);\r
 240                 //結果値を変換\r
 241                 o_evec.m00=(double)this.__pca_evec.m00/65536.0;\r
 242                 o_evec.m01=(double)this.__pca_evec.m01/65536.0;\r
 243                 o_evec.m10=(double)this.__pca_evec.m10/65536.0;\r
 244                 o_evec.m11=(double)this.__pca_evec.m11/65536.0;\r
 245                 o_ev.x=this.__pca_ev.x/65536.0;\r
 246                 o_ev.y=this.__pca_ev.y/65536.0;\r
 247                 o_mean.x=this.__pca_mean.x/65536.0;\r
 248                 o_mean.y=this.__pca_mean.y/65536.0;\r
 249                 return; \r
 250         }\r
 251         /**\r
 252          * 値は全て小数点部16bitの固定小数点です。\r
 253          * @param i_x\r
 254          * @param i_y\r
 255          * @param i_number_of_point\r
 256          * @param o_evec\r
 257          * @param o_ev\r
 258          * @param o_mean\r
 259          * @throws NyARException\r
 260          */\r
 261         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
 262         {\r
 263                 //計算\r
 264                 PCA_PCA(i_x,i_y,i_number_of_point,o_evec,o_ev,o_mean);\r
 265                 final long sum = o_ev.x + o_ev.y;\r
 266                 // For順変更禁止\r
 267                 o_ev.x = (o_ev.x<<16)/sum;// ev->v[i] /= sum;\r
 268                 o_ev.y = (o_ev.y<<16)/sum;// ev->v[i] /= sum;\r
 269                 return; \r
 270         }       \r
 271 }\r