tags/2.3.1/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 public class NyARCameraDistortionFactor implements INyARCameraDistortionFactor\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         public void copyFrom(NyARCameraDistortionFactor i_ref)\r
  54         {\r
  55                 this._f0=i_ref._f0;\r
  56                 this._f1=i_ref._f1;\r
  57                 this._f2=i_ref._f2;\r
  58                 this._f3=i_ref._f3;\r
  59                 return;\r
  60         }\r
  61         /**\r
  62          * 配列の値をファクタ値としてセットする。\r
  63          * @param i_factor\r
  64          * 4要素以上の配列\r
  65          */\r
  66         public void setValue(double[] i_factor)\r
  67         {\r
  68                 this._f0=i_factor[0];\r
  69                 this._f1=i_factor[1];\r
  70                 this._f2=i_factor[2];\r
  71                 this._f3=i_factor[3];\r
  72                 return;\r
  73         }\r
  74         public void getValue(double[] o_factor)\r
  75         {\r
  76                 o_factor[0]=this._f0;\r
  77                 o_factor[1]=this._f1;\r
  78                 o_factor[2]=this._f2;\r
  79                 o_factor[3]=this._f3;\r
  80                 return;\r
  81         }       \r
  82         public void changeScale(double i_scale)\r
  83         {\r
  84                 this._f0=this._f0*i_scale;// newparam->dist_factor[0] =source->dist_factor[0] *scale;\r
  85                 this._f1=this._f1*i_scale;// newparam->dist_factor[1] =source->dist_factor[1] *scale;\r
  86                 this._f2=this._f2/ (i_scale * i_scale);// newparam->dist_factor[2]=source->dist_factor[2]/ (scale*scale);\r
  87                 //this.f3=this.f3;// newparam->dist_factor[3] =source->dist_factor[3];\r
  88                 return;\r
  89         }\r
  90         /**\r
  91          * int arParamIdeal2Observ( const double dist_factor[4], const double ix,const double iy,double *ox, double *oy ) 関数の代替関数\r
  92          * \r
  93          * @param i_in\r
  94          * @param o_out\r
  95          */\r
  96         public void ideal2Observ(final NyARDoublePoint2d i_in, NyARDoublePoint2d o_out)\r
  97         {\r
  98                 final double x = (i_in.x - this._f0) * this._f3;\r
  99                 final double y = (i_in.y - this._f1) * this._f3;\r
 100                 if (x == 0.0 && y == 0.0) {\r
 101                         o_out.x = this._f0;\r
 102                         o_out.y = this._f1;\r
 103                 } else {\r
 104                         final double d = 1.0 - this._f2 / 100000000.0 * (x * x + y * y);\r
 105                         o_out.x = x * d + this._f0;\r
 106                         o_out.y = y * d + this._f1;\r
 107                 }\r
 108                 return;\r
 109         }\r
 110 \r
 111         /**\r
 112          * ideal2Observをまとめて実行します。\r
 113          * @param i_in\r
 114          * @param o_out\r
 115          */\r
 116         public void ideal2ObservBatch(final NyARDoublePoint2d[] i_in, NyARDoublePoint2d[] o_out, int i_size)\r
 117         {\r
 118                 double x, y;\r
 119                 final double d0 = this._f0;\r
 120                 final double d1 = this._f1;\r
 121                 final double d3 = this._f3;\r
 122                 final double d2_w = this._f2 / 100000000.0;\r
 123                 for (int i = 0; i < i_size; i++) {\r
 124                         x = (i_in[i].x - d0) * d3;\r
 125                         y = (i_in[i].y - d1) * d3;\r
 126                         if (x == 0.0 && y == 0.0) {\r
 127                                 o_out[i].x = d0;\r
 128                                 o_out[i].y = d1;\r
 129                         } else {\r
 130                                 final double d = 1.0 - d2_w * (x * x + y * y);\r
 131                                 o_out[i].x = x * d + d0;\r
 132                                 o_out[i].y = y * d + d1;\r
 133                         }\r
 134                 }\r
 135                 return;\r
 136         }\r
 137 \r
 138         /**\r
 139          * int arParamObserv2Ideal( const double dist_factor[4], const double ox,const double oy,double *ix, double *iy );\r
 140          * \r
 141          * @param ix\r
 142          * @param iy\r
 143          * @param ix\r
 144          * @param iy\r
 145          * @return\r
 146          */\r
 147         public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point)\r
 148         {\r
 149                 double z02, z0, p, q, z, px, py, opttmp_1;\r
 150                 final double d0 = this._f0;\r
 151                 final double d1 = this._f1;\r
 152 \r
 153                 px = ix - d0;\r
 154                 py = iy - d1;\r
 155                 p = this._f2 / 100000000.0;\r
 156                 z02 = px * px + py * py;\r
 157                 q = z0 = Math.sqrt(z02);// Optimize//q = z0 = Math.sqrt(px*px+ py*py);\r
 158 \r
 159                 for (int i = 1;; i++) {\r
 160                         if (z0 != 0.0) {\r
 161                                 // Optimize opttmp_1\r
 162                                 opttmp_1 = p * z02;\r
 163                                 z = z0 - ((1.0 - opttmp_1) * z0 - q) / (1.0 - 3.0 * opttmp_1);\r
 164                                 px = px * z / z0;\r
 165                                 py = py * z / z0;\r
 166                         } else {\r
 167                                 px = 0.0;\r
 168                                 py = 0.0;\r
 169                                 break;\r
 170                         }\r
 171                         if (i == PD_LOOP) {\r
 172                                 break;\r
 173                         }\r
 174                         z02 = px * px + py * py;\r
 175                         z0 = Math.sqrt(z02);// Optimize//z0 = Math.sqrt(px*px+ py*py);\r
 176                 }\r
 177                 o_point.x = px / this._f3 + d0;\r
 178                 o_point.y = py / this._f3 + d1;\r
 179                 return;\r
 180         }\r
 181 \r
 182         /**\r
 183          * 指定範囲のobserv2Idealをまとめて実行して、結果をo_idealに格納します。\r
 184          * @param i_x_coord\r
 185          * @param i_y_coord\r
 186          * @param i_start\r
 187          *            coord開始点\r
 188          * @param i_num\r
 189          *            計算数\r
 190          * @param o_ideal\r
 191          *            出力バッファ[i_num][2]であること。\r
 192          */\r
 193         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
 194         {\r
 195                 double z02, z0, q, z, px, py, opttmp_1;\r
 196                 final double d0 = this._f0;\r
 197                 final double d1 = this._f1;\r
 198                 final double d3 = this._f3;\r
 199                 final double p = this._f2 / 100000000.0;\r
 200                 for (int j = 0; j < i_num; j++) {\r
 201 \r
 202                         px = i_x_coord[i_start + j] - d0;\r
 203                         py = i_y_coord[i_start + j] - d1;\r
 204 \r
 205                         z02 = px * px + py * py;\r
 206                         q = z0 = Math.sqrt(z02);// Optimize//q = z0 = Math.sqrt(px*px+py*py);\r
 207 \r
 208                         for (int i = 1;; i++) {\r
 209                                 if (z0 != 0.0) {\r
 210                                         // Optimize opttmp_1\r
 211                                         opttmp_1 = p * z02;\r
 212                                         z = z0 - ((1.0 - opttmp_1) * z0 - q)/ (1.0 - 3.0 * opttmp_1);\r
 213                                         px = px * z / z0;\r
 214                                         py = py * z / z0;\r
 215                                 } else {\r
 216                                         px = 0.0;\r
 217                                         py = 0.0;\r
 218                                         break;\r
 219                                 }\r
 220                                 if (i == PD_LOOP) {\r
 221                                         break;\r
 222                                 }\r
 223                                 z02 = px * px + py * py;\r
 224                                 z0 = Math.sqrt(z02);// Optimize//z0 = Math.sqrt(px*px+ py*py);\r
 225                         }\r
 226                         o_x_coord[j] = px / d3 + d0;\r
 227                         o_y_coord[j] = py / d3 + d1;\r
 228                 }\r
 229                 return;\r
 230         }       \r
 231 }\r