lib/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 edition ARToolKit class library.\r
  11  * Copyright (C)2008-2009 Ryo Iizuka\r
  12  *\r
  13  * This program is free software: you can redistribute it and/or modify\r
  14  * it under the terms of the GNU General Public License as published by\r
  15  * the Free Software Foundation, either version 3 of the License, or\r
  16  * (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 program.  If not, see <http://www.gnu.org/licenses/>.\r
  25  * \r
  26  * For further information please contact.\r
  27  *      http://nyatla.jp/nyatoolkit/\r
  28  *      <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>\r
  29  * \r
  30  */\r
  31 package jp.nyatla.nyartoolkit.core.param;\r
  32 \r
  33 import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint2d;\r
  34 \r
  35 /**\r
  36  * カメラの歪み成分を格納するクラスと、補正関数群\r
  37  * http://www.hitl.washington.edu/artoolkit/Papers/ART02-Tutorial.pdf\r
  38  * 11ページを読むといいよ。\r
  39  * \r
  40  * x=x(xi-x0),y=s(yi-y0)\r
  41  * d^2=x^2+y^2\r
  42  * p=(1-fd^2)\r
  43  * xd=px+x0,yd=py+y0\r
  44  */\r
  45 public class NyARCameraDistortionFactor implements INyARCameraDistortionFactor\r
  46 {\r
  47         private static final int PD_LOOP = 3;\r
  48         private double _f0;//x0\r
  49         private double _f1;//y0\r
  50         private double _f2;//100000000.0*ｆ\r
  51         private double _f3;//s\r
  52         public void copyFrom(NyARCameraDistortionFactor i_ref)\r
  53         {\r
  54                 this._f0=i_ref._f0;\r
  55                 this._f1=i_ref._f1;\r
  56                 this._f2=i_ref._f2;\r
  57                 this._f3=i_ref._f3;\r
  58                 return;\r
  59         }\r
  60         /**\r
  61          * 配列の値をファクタ値としてセットする。\r
  62          * @param i_factor\r
  63          * 4要素以上の配列\r
  64          */\r
  65         public void setValue(double[] i_factor)\r
  66         {\r
  67                 this._f0=i_factor[0];\r
  68                 this._f1=i_factor[1];\r
  69                 this._f2=i_factor[2];\r
  70                 this._f3=i_factor[3];\r
  71                 return;\r
  72         }\r
  73         public void getValue(double[] o_factor)\r
  74         {\r
  75                 o_factor[0]=this._f0;\r
  76                 o_factor[1]=this._f1;\r
  77                 o_factor[2]=this._f2;\r
  78                 o_factor[3]=this._f3;\r
  79                 return;\r
  80         }       \r
  81         public void changeScale(double i_scale)\r
  82         {\r
  83                 this._f0=this._f0*i_scale;// newparam->dist_factor[0] =source->dist_factor[0] *scale;\r
  84                 this._f1=this._f1*i_scale;// newparam->dist_factor[1] =source->dist_factor[1] *scale;\r
  85                 this._f2=this._f2/ (i_scale * i_scale);// newparam->dist_factor[2]=source->dist_factor[2]/ (scale*scale);\r
  86                 //this.f3=this.f3;// newparam->dist_factor[3] =source->dist_factor[3];\r
  87                 return;\r
  88         }\r
  89         /**\r
  90          * int arParamIdeal2Observ( const double dist_factor[4], const double ix,const double iy,double *ox, double *oy ) 関数の代替関数\r
  91          * \r
  92          * @param i_in\r
  93          * @param o_out\r
  94          */\r
  95         public void ideal2Observ(final NyARDoublePoint2d i_in, NyARDoublePoint2d o_out)\r
  96         {\r
  97                 final double x = (i_in.x - this._f0) * this._f3;\r
  98                 final double y = (i_in.y - this._f1) * this._f3;\r
  99                 if (x == 0.0 && y == 0.0) {\r
 100                         o_out.x = this._f0;\r
 101                         o_out.y = this._f1;\r
 102                 } else {\r
 103                         final double d = 1.0 - this._f2 / 100000000.0 * (x * x + y * y);\r
 104                         o_out.x = x * d + this._f0;\r
 105                         o_out.y = y * d + this._f1;\r
 106                 }\r
 107                 return;\r
 108         }\r
 109 \r
 110         /**\r
 111          * ideal2Observをまとめて実行します。\r
 112          * @param i_in\r
 113          * @param o_out\r
 114          */\r
 115         public void ideal2ObservBatch(final NyARDoublePoint2d[] i_in, NyARDoublePoint2d[] o_out, int i_size)\r
 116         {\r
 117                 double x, y;\r
 118                 final double d0 = this._f0;\r
 119                 final double d1 = this._f1;\r
 120                 final double d3 = this._f3;\r
 121                 final double d2_w = this._f2 / 100000000.0;\r
 122                 for (int i = 0; i < i_size; i++) {\r
 123                         x = (i_in[i].x - d0) * d3;\r
 124                         y = (i_in[i].y - d1) * d3;\r
 125                         if (x == 0.0 && y == 0.0) {\r
 126                                 o_out[i].x = d0;\r
 127                                 o_out[i].y = d1;\r
 128                         } else {\r
 129                                 final double d = 1.0 - d2_w * (x * x + y * y);\r
 130                                 o_out[i].x = x * d + d0;\r
 131                                 o_out[i].y = y * d + d1;\r
 132                         }\r
 133                 }\r
 134                 return;\r
 135         }\r
 136 \r
 137         /**\r
 138          * int arParamObserv2Ideal( const double dist_factor[4], const double ox,const double oy,double *ix, double *iy );\r
 139          * \r
 140          * @param ix\r
 141          * @param iy\r
 142          * @param ix\r
 143          * @param iy\r
 144          * @return\r
 145          */\r
 146         public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point)\r
 147         {\r
 148                 double z02, z0, p, q, z, px, py, opttmp_1;\r
 149                 final double d0 = this._f0;\r
 150                 final double d1 = this._f1;\r
 151 \r
 152                 px = ix - d0;\r
 153                 py = iy - d1;\r
 154                 p = this._f2 / 100000000.0;\r
 155                 z02 = px * px + py * py;\r
 156                 q = z0 = Math.sqrt(z02);// Optimize//q = z0 = Math.sqrt(px*px+ py*py);\r
 157 \r
 158                 for (int i = 1;; i++) {\r
 159                         if (z0 != 0.0) {\r
 160                                 // Optimize opttmp_1\r
 161                                 opttmp_1 = p * z02;\r
 162                                 z = z0 - ((1.0 - opttmp_1) * z0 - q) / (1.0 - 3.0 * opttmp_1);\r
 163                                 px = px * z / z0;\r
 164                                 py = py * z / z0;\r
 165                         } else {\r
 166                                 px = 0.0;\r
 167                                 py = 0.0;\r
 168                                 break;\r
 169                         }\r
 170                         if (i == PD_LOOP) {\r
 171                                 break;\r
 172                         }\r
 173                         z02 = px * px + py * py;\r
 174                         z0 = Math.sqrt(z02);// Optimize//z0 = Math.sqrt(px*px+ py*py);\r
 175                 }\r
 176                 o_point.x = px / this._f3 + d0;\r
 177                 o_point.y = py / this._f3 + d1;\r
 178                 return;\r
 179         }\r
 180 \r
 181         /**\r
 182          * 指定範囲のobserv2Idealをまとめて実行して、結果をo_idealに格納します。\r
 183          * @param i_x_coord\r
 184          * @param i_y_coord\r
 185          * @param i_start\r
 186          *            coord開始点\r
 187          * @param i_num\r
 188          *            計算数\r
 189          * @param o_ideal\r
 190          *            出力バッファ[i_num][2]であること。\r
 191          */\r
 192         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
 193         {\r
 194                 double z02, z0, q, z, px, py, opttmp_1;\r
 195                 final double d0 = this._f0;\r
 196                 final double d1 = this._f1;\r
 197                 final double d3 = this._f3;\r
 198                 final double p = this._f2 / 100000000.0;\r
 199                 for (int j = 0; j < i_num; j++) {\r
 200 \r
 201                         px = i_x_coord[i_start + j] - d0;\r
 202                         py = i_y_coord[i_start + j] - d1;\r
 203 \r
 204                         z02 = px * px + py * py;\r
 205                         q = z0 = Math.sqrt(z02);// Optimize//q = z0 = Math.sqrt(px*px+py*py);\r
 206 \r
 207                         for (int i = 1;; i++) {\r
 208                                 if (z0 != 0.0) {\r
 209                                         // Optimize opttmp_1\r
 210                                         opttmp_1 = p * z02;\r
 211                                         z = z0 - ((1.0 - opttmp_1) * z0 - q)/ (1.0 - 3.0 * opttmp_1);\r
 212                                         px = px * z / z0;\r
 213                                         py = py * z / z0;\r
 214                                 } else {\r
 215                                         px = 0.0;\r
 216                                         py = 0.0;\r
 217                                         break;\r
 218                                 }\r
 219                                 if (i == PD_LOOP) {\r
 220                                         break;\r
 221                                 }\r
 222                                 z02 = px * px + py * py;\r
 223                                 z0 = Math.sqrt(z02);// Optimize//z0 = Math.sqrt(px*px+ py*py);\r
 224                         }\r
 225                         o_x_coord[j] = px / d3 + d0;\r
 226                         o_y_coord[j] = py / d3 + d1;\r
 227                 }\r
 228                 return;\r
 229         }       \r
 230 }\r