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.param.NyARCameraDistortionFactor;\r
  36 import jp.nyatla.nyartoolkit.core.types.*;\r
  37 import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
  38 \r
  39 /**\r
  40  * ARToolkitのPCA関数を二次元に特化させて単純化したもの\r
  41  *\r
  42  */\r
  43 public class NyARPca2d_MatrixPCA_O2 implements INyARPca2d\r
  44 {\r
  45         private double[] _x;\r
  46 \r
  47         private double[] _y;\r
  48 \r
  49         private int _number_of_data;\r
  50 \r
  51         private static final double PCA_EPS = 1e-6; // #define EPS 1e-6\r
  52 \r
  53         private static final int PCA_MAX_ITER = 100; // #define MAX_ITER 100\r
  54 \r
  55         private static final double PCA_VZERO = 1e-16; // #define VZERO 1e-16\r
  56         public NyARPca2d_MatrixPCA_O2(int i_max_points)\r
  57         {\r
  58                 this._x=new double[i_max_points];\r
  59                 this._y=new double[i_max_points];\r
  60                 this._number_of_data=0;\r
  61                 return;\r
  62         }\r
  63 \r
  64 \r
  65         /**\r
  66          * static int QRM( ARMat *a, ARVec *dv )の代替関数\r
  67          * \r
  68          * @param a\r
  69          * @param dv\r
  70          * @throws NyARException\r
  71          */\r
  72         private static void PCA_QRM(NyARDoubleMatrix22 o_matrix, NyARDoublePoint2d dv) throws NyARException\r
  73         {\r
  74                 double w, t, s, x, y, c;\r
  75                 double ev1;\r
  76                 double dv_x,dv_y;\r
  77                 double mat00,mat01,mat10,mat11;\r
  78                 // <this.vecTridiagonalize2d(i_mat, dv, ev);>\r
  79                 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
  80                 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
  81                 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
  82                 // 単位行列にする。\r
  83                 mat00 = mat11 = 1;\r
  84                 mat01 = mat10 = 0;\r
  85                 // </this.vecTridiagonalize2d(i_mat, dv, ev);>\r
  86 \r
  87                 // int j = 1;\r
  88                 // // while(j>0 && fabs(ev->v[j])>EPS*(fabs(dv->v[j-1])+fabs(dv->v[j])))\r
  89                 // while (j > 0 && Math.abs(ev1) > PCA_EPS * (Math.abs(dv.x) + Math.abs(dv.y))) {\r
  90                 // j--;\r
  91                 // }\r
  92                 // if (j == 0) {\r
  93                 int iter = 0;\r
  94                 do {\r
  95                         iter++;\r
  96                         if (iter > PCA_MAX_ITER) {\r
  97                                 break;\r
  98                         }\r
  99                         w = (dv_x - dv_y) / 2;// w = (dv->v[h-1] -dv->v[h]) / 2;//ここ？\r
 100                         t = ev1 * ev1;// t = ev->v[h] * ev->v[h];\r
 101                         s = Math.sqrt(w * w + t);\r
 102                         if (w < 0) {\r
 103                                 s = -s;\r
 104                         }\r
 105                         x = dv_x - dv_y + t / (w + s);// x = dv->v[j] -dv->v[h] +t/(w+s);\r
 106                         y = ev1;// y = ev->v[j+1];\r
 107 \r
 108                         if (Math.abs(x) >= Math.abs(y)) {\r
 109                                 if (Math.abs(x) > PCA_VZERO) {\r
 110                                         t = -y / x;\r
 111                                         c = 1 / Math.sqrt(t * t + 1);\r
 112                                         s = t * c;\r
 113                                 } else {\r
 114                                         c = 1.0;\r
 115                                         s = 0.0;\r
 116                                 }\r
 117                         } else {\r
 118                                 t = -x / y;\r
 119                                 s = 1.0 / Math.sqrt(t * t + 1);\r
 120                                 c = t * s;\r
 121                         }\r
 122                         w = dv_x - dv_y;// w = dv->v[k] -dv->v[k+1];\r
 123                         t = (w * s + 2 * c * ev1) * s;// t = (w * s +2 * c *ev->v[k+1]) *s;\r
 124                         dv_x -= t;// dv->v[k] -= t;\r
 125                         dv_y += t;// dv->v[k+1] += t;\r
 126                         ev1 += s * (c * w - 2 * s * ev1);// ev->v[k+1]+= s * (c* w- 2* s *ev->v[k+1]);\r
 127 \r
 128                         x = mat00;// x = a->m[k*dim+i];\r
 129                         y = mat10;// y = a->m[(k+1)*dim+i];\r
 130                         mat00 = c * x - s * y;// a->m[k*dim+i] = c * x - s* y;\r
 131                         mat10 = s * x + c * y;// a->m[(k+1)*dim+i] = s* x + c * y;\r
 132                         \r
 133                         x = mat01;// x = a->m[k*dim+i];\r
 134                         y = mat11;// y = a->m[(k+1)*dim+i];\r
 135                         mat01 = c * x - s * y;// a->m[k*dim+i] = c * x - s* y;\r
 136                         mat11 = s * x + c * y;// a->m[(k+1)*dim+i] = s* x + c * y;\r
 137                 } while (Math.abs(ev1) > PCA_EPS * (Math.abs(dv_x) + Math.abs(dv_y)));\r
 138                 // }\r
 139 \r
 140                 t = dv_x;// t = dv->v[h];\r
 141                 if (dv_y > t) {// if( dv->v[i] > t ) {\r
 142                         t = dv_y;// t = dv->v[h];\r
 143                         dv_y = dv_x;// dv->v[h] = dv->v[k];\r
 144                         dv_x = t;// dv->v[k] = t;\r
 145                         // 行の入れ替え\r
 146                         o_matrix.m00 = mat10;\r
 147                         o_matrix.m01 = mat11;\r
 148                         o_matrix.m10 = mat00;           \r
 149                         o_matrix.m11 = mat01;\r
 150                         \r
 151                 } else {\r
 152                         // 行の入れ替えはなし\r
 153                         o_matrix.m00 = mat00;\r
 154                         o_matrix.m01 = mat01;\r
 155                         o_matrix.m10 = mat10;           \r
 156                         o_matrix.m11 = mat11;\r
 157                 }\r
 158                 dv.x=dv_x;\r
 159                 dv.y=dv_y;\r
 160                 return;\r
 161         }\r
 162 \r
 163         /**\r
 164          * static int PCA( ARMat *input, ARMat *output, ARVec *ev )\r
 165          * \r
 166          * @param output\r
 167          * @param o_ev\r
 168          * @throws NyARException\r
 169          */\r
 170         private void PCA_PCA(NyARDoubleMatrix22 o_matrix, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
 171         {\r
 172                 // double[] mean_array=mean.getArray();\r
 173                 // mean.zeroClear();\r
 174 \r
 175                 //PCA_EXの処理\r
 176                 double sx = 0;\r
 177                 double sy = 0;\r
 178                 final int number_of_data = this._number_of_data;\r
 179                 for (int i = 0; i < number_of_data; i++) {\r
 180                         sx += this._x[i];\r
 181                         sy += this._y[i];\r
 182                 }\r
 183                 sx = sx / number_of_data;\r
 184                 sy = sy / number_of_data;\r
 185                 \r
 186                 //PCA_CENTERとPCA_xt_by_xを一緒に処理\r
 187                 final double srow = Math.sqrt((double) this._number_of_data);\r
 188                 double w00, w11, w10;\r
 189                 w00 = w11 = w10 = 0.0;// *out = 0.0;\r
 190                 for (int i = 0; i < number_of_data; i++) {\r
 191                         final double x = (this._x[i] - sx) / srow;\r
 192                         final double y = (this._y[i] - sy) / srow;\r
 193                         w00 += (x * x);// *out += *in1 * *in2;\r
 194                         w10 += (x * y);// *out += *in1 * *in2;\r
 195                         w11 += (y * y);// *out += *in1 * *in2;\r
 196                 }\r
 197                 o_matrix.m00=w00;\r
 198                 o_matrix.m01=o_matrix.m10=w10;\r
 199                 o_matrix.m11=w11;\r
 200                 \r
 201                 //PCA_PCAの処理\r
 202                 PCA_QRM(o_matrix, o_ev);\r
 203                 // m2 = o_output.m;// m2 = output->m;\r
 204                 if (o_ev.x < PCA_VZERO) {// if( ev->v[i] < VZERO ){\r
 205                         o_ev.x = 0.0;// ev->v[i] = 0.0;\r
 206                         o_matrix.m00 = 0.0;// *(m2++) = 0.0;\r
 207                         o_matrix.m01 = 0.0;// *(m2++) = 0.0;\r
 208                 }\r
 209 \r
 210                 if (o_ev.y < PCA_VZERO) {// if( ev->v[i] < VZERO ){\r
 211                         o_ev.y = 0.0;// ev->v[i] = 0.0;\r
 212                         o_matrix.m10 = 0.0;// *(m2++) = 0.0;\r
 213                         o_matrix.m11 = 0.0;// *(m2++) = 0.0;\r
 214                 }\r
 215                 o_mean.x=sx;\r
 216                 o_mean.y=sy;\r
 217                 // }\r
 218                 return;\r
 219         }\r
 220         public void pca(double[] i_x,double[] i_y,int i_start,int i_number_of_point,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
 221         {\r
 222                 NyARException.trap("未チェックの関数");\r
 223                 assert(this._x.length>i_number_of_point);               \r
 224                 this._number_of_data = i_number_of_point;\r
 225                 System.arraycopy(i_x, 0, this._x, i_start, i_number_of_point);\r
 226                 System.arraycopy(i_y, 0, this._y, i_start, i_number_of_point);\r
 227                 \r
 228                 PCA_PCA(o_evec, o_ev,o_mean);\r
 229 \r
 230                 final double sum = o_ev.x + o_ev.y;\r
 231                 // For順変更禁止\r
 232                 o_ev.x /= sum;// ev->v[i] /= sum;\r
 233                 o_ev.y /= sum;// ev->v[i] /= sum;\r
 234                 return; \r
 235         }\r
 236         \r
 237         public void pcaWithDistortionFactor(int[] i_x,int[] i_y,int i_start,int i_number_of_point,NyARCameraDistortionFactor i_factor,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
 238         {\r
 239                 assert(this._x.length>i_number_of_point);\r
 240                 this._number_of_data = i_number_of_point;\r
 241                 i_factor.observ2IdealBatch(i_x, i_y, i_start, i_number_of_point, this._x, this._y);\r
 242 \r
 243                 PCA_PCA(o_evec,o_ev,o_mean);\r
 244 \r
 245                 final double sum = o_ev.x + o_ev.y;\r
 246                 // For順変更禁止\r
 247                 o_ev.x /= sum;// ev->v[i] /= sum;\r
 248                 o_ev.y /= sum;// ev->v[i] /= sum;\r
 249                 return; \r
 250         }\r
 251 }\r