lib/src/jp/nyatla/nyartoolkit/core/transmat/solver/NyARTransportVectorSolver.java

   1 /* \r
   2  * PROJECT: NyARToolkit(Extension)\r
   3  * --------------------------------------------------------------------------------\r
   4  *\r
   5  * The NyARToolkit is Java edition ARToolKit class library.\r
   6  * Copyright (C)2008-2009 Ryo Iizuka\r
   7  *\r
   8  * This program is free software: you can redistribute it and/or modify\r
   9  * it under the terms of the GNU General Public License as published by\r
  10  * the Free Software Foundation, either version 3 of the License, or\r
  11  * (at your option) any later version.\r
  12  * \r
  13  * This program is distributed in the hope that it will be useful,\r
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  16  * GNU General Public License for more details.\r
  17  *\r
  18  * You should have received a copy of the GNU General Public License\r
  19  * along with this program.  If not, see <http://www.gnu.org/licenses/>.\r
  20  * \r
  21  * For further information please contact.\r
  22  *      http://nyatla.jp/nyatoolkit/\r
  23  *      <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>\r
  24  * \r
  25  */\r
  26 package jp.nyatla.nyartoolkit.core.transmat.solver;\r
  27 \r
  28 import jp.nyatla.nyartoolkit.NyARException;\r
  29 import jp.nyatla.nyartoolkit.core.param.NyARPerspectiveProjectionMatrix;\r
  30 import jp.nyatla.nyartoolkit.core.types.*;\r
  31 \r
  32 /**\r
  33  * このクラスは、ARToolKitと同じアルゴリズムを、異なる演算手順で処理して、並進ベクトルを求めます。\r
  34  * アルゴリズムは、ARToolKit 拡張現実プログラミング入門 の、P207のものです。\r
  35  * <p>計算手順\r
  36  * <pre>\r
  37  * [A]*[T]=bを、[A]T*[A]*[T]=[A]T*[b]にする。\r
  38  * set2dVertexで[A]T*[A]=[M]を計算して、Aの3列目の情報だけ保存しておく。\r
  39  * getTransportVectorで[M]*[T]=[A]T*[b]を連立方程式で解いて、[T]を得る。\r
  40  * </pre>\r
  41  * </p>\r
  42  */\r
  43 public class NyARTransportVectorSolver implements INyARTransportVectorSolver\r
  44 {\r
  45         private double[] _cx;\r
  46         private double[] _cy;   \r
  47         private final NyARPerspectiveProjectionMatrix _projection_mat;\r
  48         private int _nmber_of_vertex;\r
  49         /**\r
  50          * コンストラクタです。\r
  51          * 射影変換オブジェクトの参照値と、取り扱う頂点の最大数を指定して、インスタンスを生成します。\r
  52          * @param i_projection_mat_ref\r
  53          * 射影変換オブジェクトの参照値です。\r
  54          * @param i_max_vertex\r
  55          * 取り扱う頂点の最大数。\r
  56          */\r
  57         public NyARTransportVectorSolver(NyARPerspectiveProjectionMatrix i_projection_mat_ref,int i_max_vertex)\r
  58         {\r
  59                 this._projection_mat=i_projection_mat_ref;\r
  60                 this._cx=new double[i_max_vertex];\r
  61                 this._cy=new double[i_max_vertex];      \r
  62                 return;\r
  63         }\r
  64         private double _a00,_a01_10,_a02_20,_a11,_a12_21,_a22;\r
  65         /**\r
  66          * この関数は、射影変換後の２次元頂点座標をセットします。\r
  67          * i_number_of_vertexは、コンストラクタで指定した最大数以下である必要があります。\r
  68          */\r
  69         public void set2dVertex(NyARDoublePoint2d[] i_ref_vertex_2d,int i_number_of_vertex) throws NyARException\r
  70         {\r
  71                 //3x2nと2n*3の行列から、最小二乗法計算するために3x3マトリクスを作る。               \r
  72                 //行列[A]の3列目のキャッシュ\r
  73                 final double[] cx=this._cx;\r
  74                 final double[] cy=this._cy;\r
  75                 \r
  76                 double m22;\r
  77                 double p00=this._projection_mat.m00;\r
  78                 double p01=this._projection_mat.m01;\r
  79                 double p11=this._projection_mat.m11;\r
  80                 double p12=this._projection_mat.m12;\r
  81                 double p02=this._projection_mat.m02;\r
  82                 double w1,w2,w3,w4;\r
  83                 \r
  84                 this._a00=i_number_of_vertex*p00*p00;\r
  85                 this._a01_10=i_number_of_vertex*p00*p01;\r
  86                 this._a11=i_number_of_vertex*(p01*p01+p11*p11);\r
  87                 \r
  88                 //[A]T*[A]の計算\r
  89                 m22=0;\r
  90                 w1=w2=0;\r
  91                 for(int i=0;i<i_number_of_vertex;i++){\r
  92                         //座標を保存しておく。\r
  93                         w3=p02-(cx[i]=i_ref_vertex_2d[i].x);\r
  94                         w4=p12-(cy[i]=i_ref_vertex_2d[i].y);\r
  95                         w1+=w3;\r
  96                         w2+=w4;\r
  97                         m22+=w3*w3+w4*w4;\r
  98                 }\r
  99                 this._a02_20=w1*p00;\r
 100                 this._a12_21=p01*w1+p11*w2;\r
 101                 this._a22=m22;\r
 102 \r
 103                 this._nmber_of_vertex=i_number_of_vertex;\r
 104                 return;\r
 105         }\r
 106         \r
 107         /**\r
 108          * 画面座標群と3次元座標群から、平行移動量を計算します。\r
 109          * 2d座標系は、直前に実行した{@link #set2dVertex}のものを使用します。\r
 110          */\r
 111         public void solveTransportVector(NyARDoublePoint3d[] i_vertex3d,NyARDoublePoint3d o_transfer) throws NyARException\r
 112         {\r
 113                 final int number_of_vertex=this._nmber_of_vertex;\r
 114                 final double p00=this._projection_mat.m00;\r
 115                 final double p01=this._projection_mat.m01;\r
 116                 final double p02=this._projection_mat.m02;\r
 117                 final double p11=this._projection_mat.m11;\r
 118                 final double p12=this._projection_mat.m12;\r
 119                 //行列[A]の3列目のキャッシュ\r
 120                 final double[] cx=this._cx;\r
 121                 final double[] cy=this._cy;                     \r
 122                 \r
 123                 //回転行列を元座標の頂点群に適応\r
 124                 //[A]T*[b]を計算\r
 125                 double b1=0,b2=0,b3=0;\r
 126                 for(int i=0;i<number_of_vertex;i++)\r
 127                 {\r
 128                         double w1=i_vertex3d[i].z*cx[i]-p00*i_vertex3d[i].x-p01*i_vertex3d[i].y-p02*i_vertex3d[i].z;\r
 129                         double w2=i_vertex3d[i].z*cy[i]-p11*i_vertex3d[i].y-p12*i_vertex3d[i].z;\r
 130                         b1+=w1;\r
 131                         b2+=w2;\r
 132                         b3+=cx[i]*w1+cy[i]*w2;\r
 133                 }\r
 134                 //[A]T*[b]を計算\r
 135                 b3=p02*b1+p12*b2-b3;//順番変えたらダメよ\r
 136                 b2=p01*b1+p11*b2;\r
 137                 b1=p00*b1;\r
 138                 //([A]T*[A])*[T]=[A]T*[b]を方程式で解く。\r
 139                 //a01とa10を0と仮定しても良いんじゃないかな？\r
 140                 double a00=this._a00;\r
 141                 double a01=this._a01_10;\r
 142                 double a02=this._a02_20;\r
 143                 double a11=this._a11;\r
 144                 double a12=this._a12_21;\r
 145                 double a22=this._a22;\r
 146                 \r
 147                 double t1=a22*b2-a12*b3;\r
 148                 double t2=a12*b2-a11*b3;\r
 149                 double t3=a01*b3-a02*b2;\r
 150                 double t4=a12*a12-a11*a22;\r
 151                 double t5=a02*a12-a01*a22;\r
 152                 double t6=a02*a11-a01*a12;\r
 153                 double det=a00*t4-a01*t5 + a02*t6;\r
 154                 o_transfer.x= (a01*t1 - a02*t2 +b1*t4)/det;\r
 155                 o_transfer.y=-(a00*t1 + a02*t3 +b1*t5)/det;\r
 156                 o_transfer.z= (a00*t2 + a01*t3 +b1*t6)/det;\r
 157                 \r
 158         \r
 159                 return;\r
 160         }\r
 161 }