trunk/src/jp/nyatla/nyartoolkit/core/param/NyARCameraDistortionFactor.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.param;\r
  33 \r
  34 import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint2d;\r
  35 \r
  36 /**\r
  37  * カメラの歪み成分を格納するクラスと、補正関数群\r
  38  * http://www.hitl.washington.edu/artoolkit/Papers/ART02-Tutorial.pdf\r
  39  * 11ページを読むといいよ。\r
  40  * \r
  41  * x=x(xi-x0),y=s(yi-y0)\r
  42  * d^2=x^2+y^2\r
  43  * p=(1-fd^2)\r
  44  * xd=px+x0,yd=py+y0\r
  45  */\r
  46 final public class NyARCameraDistortionFactor\r
  47 {\r
  48         private static final int PD_LOOP = 3;\r
  49         private double _f0;//x0\r
  50         private double _f1;//y0\r
  51         private double _f2;//100000000.0*ｆ\r
  52         private double _f3;//s\r
  53         /**\r
  54          * 配列の値をファクタ値としてセットする。\r
  55          * @param i_factor\r
  56          * 4要素以上の配列\r
  57          */\r
  58         public void setValue(double[] i_factor)\r
  59         {\r
  60                 this._f0=i_factor[0];\r
  61                 this._f1=i_factor[1];\r
  62                 this._f2=i_factor[2];\r
  63                 this._f3=i_factor[3];\r
  64                 return;\r
  65         }\r
  66         public void getValue(double[] o_factor)\r
  67         {\r
  68                 o_factor[0]=this._f0;\r
  69                 o_factor[1]=this._f1;\r
  70                 o_factor[2]=this._f2;\r
  71                 o_factor[3]=this._f3;\r
  72                 return;\r
  73         }       \r
  74         public void changeScale(double i_scale)\r
  75         {\r
  76                 this._f0=this._f0*i_scale;// newparam->dist_factor[0] =source->dist_factor[0] *scale;\r
  77                 this._f1=this._f1*i_scale;// newparam->dist_factor[1] =source->dist_factor[1] *scale;\r
  78                 this._f2=this._f2/ (i_scale * i_scale);// newparam->dist_factor[2]=source->dist_factor[2]/ (scale*scale);\r
  79                 //this.f3=this.f3;// newparam->dist_factor[3] =source->dist_factor[3];\r
  80                 return;\r
  81         }\r
  82         /**\r
  83          * int arParamIdeal2Observ( const double dist_factor[4], const double ix,const double iy,double *ox, double *oy ) 関数の代替関数\r
  84          * \r
  85          * @param i_in\r
  86          * @param o_out\r
  87          */\r
  88         public void ideal2Observ(final NyARDoublePoint2d i_in, NyARDoublePoint2d o_out)\r
  89         {\r
  90                 final double x = (i_in.x - this._f0) * this._f3;\r
  91                 final double y = (i_in.y - this._f1) * this._f3;\r
  92                 if (x == 0.0 && y == 0.0) {\r
  93                         o_out.x = this._f0;\r
  94                         o_out.y = this._f1;\r
  95                 } else {\r
  96                         final double d = 1.0 - this._f2 / 100000000.0 * (x * x + y * y);\r
  97                         o_out.x = x * d + this._f0;\r
  98                         o_out.y = y * d + this._f1;\r
  99                 }\r
 100                 return;\r
 101         }\r
 102 \r
 103         /**\r
 104          * ideal2Observをまとめて実行します。\r
 105          * @param i_in\r
 106          * @param o_out\r
 107          */\r
 108         public void ideal2ObservBatch(final NyARDoublePoint2d[] i_in, NyARDoublePoint2d[] o_out, int i_size)\r
 109         {\r
 110                 double x, y;\r
 111                 final double d0 = this._f0;\r
 112                 final double d1 = this._f1;\r
 113                 final double d3 = this._f3;\r
 114                 final double d2_w = this._f2 / 100000000.0;\r
 115                 for (int i = 0; i < i_size; i++) {\r
 116                         x = (i_in[i].x - d0) * d3;\r
 117                         y = (i_in[i].y - d1) * d3;\r
 118                         if (x == 0.0 && y == 0.0) {\r
 119                                 o_out[i].x = d0;\r
 120                                 o_out[i].y = d1;\r
 121                         } else {\r
 122                                 final double d = 1.0 - d2_w * (x * x + y * y);\r
 123                                 o_out[i].x = x * d + d0;\r
 124                                 o_out[i].y = y * d + d1;\r
 125                         }\r
 126                 }\r
 127                 return;\r
 128         }\r
 129 \r
 130         /**\r
 131          * int arParamObserv2Ideal( const double dist_factor[4], const double ox,const double oy,double *ix, double *iy );\r
 132          * \r
 133          * @param ix\r
 134          * @param iy\r
 135          * @param ix\r
 136          * @param iy\r
 137          * @return\r
 138          */\r
 139         public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point)\r
 140         {\r
 141                 double z02, z0, p, q, z, px, py, opttmp_1;\r
 142                 final double d0 = this._f0;\r
 143                 final double d1 = this._f1;\r
 144 \r
 145                 px = ix - d0;\r
 146                 py = iy - d1;\r
 147                 p = this._f2 / 100000000.0;\r
 148                 z02 = px * px + py * py;\r
 149                 q = z0 = Math.sqrt(z02);// Optimize//q = z0 = Math.sqrt(px*px+ py*py);\r
 150 \r
 151                 for (int i = 1;; i++) {\r
 152                         if (z0 != 0.0) {\r
 153                                 // Optimize opttmp_1\r
 154                                 opttmp_1 = p * z02;\r
 155                                 z = z0 - ((1.0 - opttmp_1) * z0 - q) / (1.0 - 3.0 * opttmp_1);\r
 156                                 px = px * z / z0;\r
 157                                 py = py * z / z0;\r
 158                         } else {\r
 159                                 px = 0.0;\r
 160                                 py = 0.0;\r
 161                                 break;\r
 162                         }\r
 163                         if (i == PD_LOOP) {\r
 164                                 break;\r
 165                         }\r
 166                         z02 = px * px + py * py;\r
 167                         z0 = Math.sqrt(z02);// Optimize//z0 = Math.sqrt(px*px+ py*py);\r
 168                 }\r
 169                 o_point.x = px / this._f3 + d0;\r
 170                 o_point.y = py / this._f3 + d1;\r
 171                 return;\r
 172         }\r
 173 \r
 174         /**\r
 175          * 指定範囲のobserv2Idealをまとめて実行して、結果をo_idealに格納します。\r
 176          * \r
 177          * @param i_x_coord\r
 178          * @param i_y_coord\r
 179          * @param i_start\r
 180          *            coord開始点\r
 181          * @param i_num\r
 182          *            計算数\r
 183          * @param o_ideal\r
 184          *            出力バッファ[i_num][2]であること。\r
 185          */\r
 186         public void observ2IdealBatch(int[] i_x_coord, int[] i_y_coord,int i_start, int i_num, double[][] o_ideal)\r
 187         {\r
 188                 double z02, z0, q, z, px, py, opttmp_1;\r
 189                 final double d0 = this._f0;\r
 190                 final double d1 = this._f1;\r
 191                 final double d3 = this._f3;\r
 192                 final double p = this._f2 / 100000000.0;\r
 193                 for (int j = 0; j < i_num; j++) {\r
 194 \r
 195                         px = i_x_coord[i_start + j] - d0;\r
 196                         py = i_y_coord[i_start + j] - d1;\r
 197 \r
 198                         z02 = px * px + py * py;\r
 199                         q = z0 = Math.sqrt(z02);// Optimize//q = z0 = Math.sqrt(px*px+py*py);\r
 200 \r
 201                         for (int i = 1;; i++) {\r
 202                                 if (z0 != 0.0) {\r
 203                                         // Optimize opttmp_1\r
 204                                         opttmp_1 = p * z02;\r
 205                                         z = z0 - ((1.0 - opttmp_1) * z0 - q)/ (1.0 - 3.0 * opttmp_1);\r
 206                                         px = px * z / z0;\r
 207                                         py = py * z / z0;\r
 208                                 } else {\r
 209                                         px = 0.0;\r
 210                                         py = 0.0;\r
 211                                         break;\r
 212                                 }\r
 213                                 if (i == PD_LOOP) {\r
 214                                         break;\r
 215                                 }\r
 216                                 z02 = px * px + py * py;\r
 217                                 z0 = Math.sqrt(z02);// Optimize//z0 = Math.sqrt(px*px+ py*py);\r
 218                         }\r
 219                         o_ideal[j][0] = px / d3 + d0;\r
 220                         o_ideal[j][1] = py / d3 + d1;\r
 221                 }\r
 222                 return;\r
 223         }\r
 224         public void observ2IdealBatch(int[] i_x_coord, int[] i_y_coord,int i_start, int i_num, double[] o_x_coord,double[] o_y_coord)\r
 225         {\r
 226                 double z02, z0, q, z, px, py, opttmp_1;\r
 227                 final double d0 = this._f0;\r
 228                 final double d1 = this._f1;\r
 229                 final double d3 = this._f3;\r
 230                 final double p = this._f2 / 100000000.0;\r
 231                 for (int j = 0; j < i_num; j++) {\r
 232 \r
 233                         px = i_x_coord[i_start + j] - d0;\r
 234                         py = i_y_coord[i_start + j] - d1;\r
 235 \r
 236                         z02 = px * px + py * py;\r
 237                         q = z0 = Math.sqrt(z02);// Optimize//q = z0 = Math.sqrt(px*px+py*py);\r
 238 \r
 239                         for (int i = 1;; i++) {\r
 240                                 if (z0 != 0.0) {\r
 241                                         // Optimize opttmp_1\r
 242                                         opttmp_1 = p * z02;\r
 243                                         z = z0 - ((1.0 - opttmp_1) * z0 - q)/ (1.0 - 3.0 * opttmp_1);\r
 244                                         px = px * z / z0;\r
 245                                         py = py * z / z0;\r
 246                                 } else {\r
 247                                         px = 0.0;\r
 248                                         py = 0.0;\r
 249                                         break;\r
 250                                 }\r
 251                                 if (i == PD_LOOP) {\r
 252                                         break;\r
 253                                 }\r
 254                                 z02 = px * px + py * py;\r
 255                                 z0 = Math.sqrt(z02);// Optimize//z0 = Math.sqrt(px*px+ py*py);\r
 256                         }\r
 257                         o_x_coord[j] = px / d3 + d0;\r
 258                         o_y_coord[j] = py / d3 + d1;\r
 259                 }\r
 260                 return;\r
 261         }       \r
 262 }\r