[backup]NyARToolkit for Java

NyARTransMatのエラー判定閾値計算の自動化

diff --git a/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat.java b/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat.java
index 0ce8d52..b155345 100644 (file)
--- a/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat.java
+++ b/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat.java
@@ -45,12 +45,7 @@ import jp.nyatla.nyartoolkit.core.types.matrix.*;
  * \r
  */\r
 public class NyARTransMat implements INyARTransMat\r
-{\r
-       private final static double FIT_DIFF_THRESHOLD_CONT_OPT = 1.0;\r
-       private final static double FIT_DIFF_THRESHOLD_CONT = 0.1;\r
-       private final static double FIT_DIFF_THRESHOLD_OPT = 1.0;\r
-       \r
-\r
+{      \r
        private final NyARDoublePoint2d _center=new NyARDoublePoint2d(0,0);\r
        private final NyARTransOffset _offset=new NyARTransOffset();\r
        private NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
@@ -120,6 +115,23 @@ public class NyARTransMat implements INyARTransMat
        private final NyARDoublePoint3d[] __transMat_vertex_3d = NyARDoublePoint3d.createArray(4);\r
        private final NyARLinear[] __transMat_linear_ref=new NyARLinear[4];\r
        private final NyARDoublePoint3d __transMat_trans=new NyARDoublePoint3d();\r
+       \r
+       /**\r
+        * 頂点情報を元に、エラー閾値を計算します。\r
+        * @param i_vertex\r
+        */\r
+       private double makeErrThreshold(NyARDoublePoint2d[] i_vertex)\r
+       {\r
+               double a,b,l1,l2;\r
+               a=i_vertex[0].x-i_vertex[2].x;\r
+               b=i_vertex[0].y-i_vertex[2].y;\r
+               l1=a*a+b*b;\r
+               a=i_vertex[1].x-i_vertex[3].x;\r
+               b=i_vertex[1].y-i_vertex[3].y;\r
+               l2=a*a+b*b;\r
+               return (Math.sqrt(l1>l2?l1:l2))/200;\r
+       }\r
+       \r
        /**\r
         * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )\r
         * \r
@@ -136,6 +148,7 @@ public class NyARTransMat implements INyARTransMat
                final NyARLinear[] linear_ref=__transMat_linear_ref;\r
                final NyARDoublePoint3d trans=this.__transMat_trans;\r
                \r
+               double err_threshold=makeErrThreshold(i_square.sqvertex);\r
                //計算用に頂点情報を初期化（順番調整）\r
                initVertexOrder(i_square, i_direction, sqvertex_ref,linear_ref);\r
                \r
@@ -156,7 +169,7 @@ public class NyARTransMat implements INyARTransMat
                this._transsolver.solveTransportVector(vertex_3d,trans);\r
                \r
                //計算結果の最適化(平行移動量と回転行列の最適化)\r
-               o_result_conv.error=this.optimize(this._rotmatrix, trans, this._transsolver,this._offset.vertex, vertex_2d);\r
+               o_result_conv.error=this.optimize(this._rotmatrix, trans, this._transsolver,this._offset.vertex, vertex_2d,err_threshold);\r
                \r
                // マトリクスの保存\r
                this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,o_result_conv);\r
@@ -178,7 +191,9 @@ public class NyARTransMat implements INyARTransMat
                        this.transMat(i_square, i_direction, i_width, o_result_conv);\r
                        return;\r
                }\r
-\r
+               \r
+               //最適化計算の閾値を決定\r
+               double err_threshold=makeErrThreshold(i_square.sqvertex);\r
                //計算用に頂点情報を初期化（順番調整）\r
                initVertexOrder(i_square, i_direction, sqvertex_ref,linear_ref);\r
 \r
@@ -203,7 +218,7 @@ public class NyARTransMat implements INyARTransMat
                double min_err=errRate(this._rotmatrix,trans, this._offset.vertex, vertex_2d,4,vertex_3d);\r
                NyARDoubleMatrix33 rot=this.__rot;\r
                //エラーレートが前回のエラー値より閾値分大きかったらアゲイン\r
-               if(min_err<o_result_conv.error+FIT_DIFF_THRESHOLD_CONT){\r
+               if(min_err<o_result_conv.error+err_threshold){\r
                        rot.setValue(this._rotmatrix);\r
                        //最適化してみる。\r
                        for (int i = 0;i<5; i++) {\r
@@ -211,7 +226,7 @@ public class NyARTransMat implements INyARTransMat
                                this._mat_optimize.modifyMatrix(rot, trans, this._offset.vertex, vertex_2d, 4);\r
                                double err=errRate(rot,trans,this._offset.vertex, vertex_2d,4,vertex_3d);\r
                                //System.out.println("E:"+err);\r
-                               if(min_err-err<FIT_DIFF_THRESHOLD_CONT_OPT){\r
+                               if(min_err-err<err_threshold/2){\r
                                        //System.out.println("BREAK");\r
                                        break;\r
                                }\r
@@ -229,14 +244,14 @@ public class NyARTransMat implements INyARTransMat
                        this._transsolver.solveTransportVector(vertex_3d,trans);\r
                        \r
                        //計算結果の最適化(平行移動量と回転行列の最適化)\r
-                       min_err=this.optimize(this._rotmatrix, trans, this._transsolver,this._offset.vertex, vertex_2d);\r
+                       min_err=this.optimize(this._rotmatrix, trans, this._transsolver,this._offset.vertex, vertex_2d,err_threshold);\r
                        this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,o_result_conv);\r
                }\r
                o_result_conv.error=min_err;\r
                return;\r
        }\r
        private NyARDoubleMatrix33 __rot=new NyARDoubleMatrix33();\r
-       private double optimize(NyARRotMatrix io_rotmat,NyARDoublePoint3d io_transvec,INyARTransportVectorSolver i_solver,NyARDoublePoint3d[] i_offset_3d,NyARDoublePoint2d[] i_2d_vertex) throws NyARException\r
+       private double optimize(NyARRotMatrix io_rotmat,NyARDoublePoint3d io_transvec,INyARTransportVectorSolver i_solver,NyARDoublePoint3d[] i_offset_3d,NyARDoublePoint2d[] i_2d_vertex,double i_err_threshold) throws NyARException\r
        {\r
                //System.out.println("START");\r
                NyARDoublePoint3d[] vertex_3d=this.__transMat_vertex_3d;\r
@@ -249,7 +264,7 @@ public class NyARTransMat implements INyARTransMat
                        this._mat_optimize.modifyMatrix(rot, io_transvec, i_offset_3d, i_2d_vertex, 4);\r
                        double err=errRate(rot,io_transvec, i_offset_3d, i_2d_vertex,4,vertex_3d);\r
                        //System.out.println("E:"+err);\r
-                       if(min_err-err<FIT_DIFF_THRESHOLD_OPT){\r
+                       if(min_err-err<i_err_threshold){\r
                                //System.out.println("BREAK");\r
                                break;\r
                        }\r