From 671474f0468d28486fc9e217f9b2696a7a0655bd Mon Sep 17 00:00:00 2001
From: nyatla <nyatla@7cac0a50-4618-4814-88d0-24b83990f816>
Date: Thu, 12 Nov 2009 03:56:57 +0000
Subject: [PATCH] =?utf8?q?[backup]NyARToolkit=20for=20Java=20NyARTransMat?=
 =?utf8?q?=E3=81=AEtransmatContinue=E6=A9=9F=E8=83=BD=E3=81=AE=E8=AA=BF?=
 =?utf8?q?=E6=95=B4=E3=80=82=20=E3=82=B8=E3=83=83=E3=82=BF=E3=81=8C?=
 =?utf8?q?=E5=87=BA=E3=81=AB=E3=81=8F=E3=81=8F=E3=81=AA=E3=81=A3=E3=81=9F?=
 =?utf8?q?=EF=BC=88=E3=81=AF=E3=81=9A=EF=BC=89?=
MIME-Version: 1.0
Content-Type: text/plain; charset=utf8
Content-Transfer-Encoding: 8bit

---
 src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat.java | 15 +++++++++------
 .../nyartoolkit/core/transmat/NyARTransMatResult.java     |  4 ++++
 .../nyartoolkit/core/transmat/NyARTransMat_ARToolKit.java |  5 ++++-
 .../core/transmat/rotmatrix/NyARRotVector.java            |  6 +++---
 4 files changed, 20 insertions(+), 10 deletions(-)

diff --git a/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat.java b/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat.java
index ed7aa13..a094d15 100644
--- a/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat.java
+++ b/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat.java
@@ -47,7 +47,9 @@ import jp.nyatla.nyartoolkit.core.types.matrix.*;
 public class NyARTransMat implements INyARTransMat
 {
 	private final static double FIT_DIFF_THRESHOLD = 1.0;
-	private final static double FIT_DIFF_THRESHOLD_CONT = 1.0;
+	private final static double FIT_DIFF_THRESHOLD_CONT = 0.1;
+	private final static double FIT_DIFF_THRESHOLD_OPT = 0.01;
+	
 
 	private final NyARDoublePoint2d _center=new NyARDoublePoint2d(0,0);
 	private final NyARTransOffset _offset=new NyARTransOffset();
@@ -154,7 +156,7 @@ public class NyARTransMat implements INyARTransMat
 		this._transsolver.solveTransportVector(vertex_3d,trans);
 		
 		//計算結果の最適化(平行移動量と回転行列の最適化)
-		this.optimize(this._rotmatrix, trans, this._transsolver,this._offset.vertex, vertex_2d);
+		o_result_conv.error=this.optimize(this._rotmatrix, trans, this._transsolver,this._offset.vertex, vertex_2d);
 		
 		// マトリクスの保存
 		this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,o_result_conv);
@@ -200,8 +202,8 @@ public class NyARTransMat implements INyARTransMat
 		//現在のエラーレートを計算しておく
 		double min_err=errRate(this._rotmatrix,trans, this._offset.vertex, vertex_2d,4,vertex_3d);
 		NyARDoubleMatrix33 rot=this.__rot;
-		//エラーレートが閾値超えてたらアゲイン
-		if(min_err<FIT_DIFF_THRESHOLD_CONT){
+		//エラーレートが前回のエラー値より閾値分大きかったらアゲイン
+		if(min_err<o_result_conv.error+FIT_DIFF_THRESHOLD_CONT){
 			rot.setValue(this._rotmatrix);
 			//最適化してみる。
 			for (int i = 0;i<5; i++) {
@@ -227,9 +229,10 @@ public class NyARTransMat implements INyARTransMat
 			this._transsolver.solveTransportVector(vertex_3d,trans);
 			
 			//計算結果の最適化(平行移動量と回転行列の最適化)
-			this.optimize(this._rotmatrix, trans, this._transsolver,this._offset.vertex, vertex_2d);
+			min_err=this.optimize(this._rotmatrix, trans, this._transsolver,this._offset.vertex, vertex_2d);
 			this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,o_result_conv);
 		}
+		o_result_conv.error=min_err;
 		return;
 	}
 	private NyARDoubleMatrix33 __rot=new NyARDoubleMatrix33();
@@ -246,7 +249,7 @@ public class NyARTransMat implements INyARTransMat
 			this._mat_optimize.modifyMatrix(rot, io_transvec, i_offset_3d, i_2d_vertex, 4);
 			double err=errRate(rot,io_transvec, i_offset_3d, i_2d_vertex,4,vertex_3d);
 			//System.out.println("E:"+err);
-			if(min_err-err<FIT_DIFF_THRESHOLD){
+			if(min_err-err<FIT_DIFF_THRESHOLD_OPT){
 				//System.out.println("BREAK");
 				break;
 			}
diff --git a/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMatResult.java b/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMatResult.java
index 25b6ef7..4181d95 100644
--- a/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMatResult.java
+++ b/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMatResult.java
@@ -40,6 +40,10 @@ import jp.nyatla.nyartoolkit.core.types.matrix.*;
  */
 public class NyARTransMatResult extends NyARDoubleMatrix34
 {
+	/**
+	 * エラーレート。この値はINyARTransMatの派生クラスが使います。
+	 */
+	public double error;	
 	public boolean has_value = false;
 	/**
 	 * この関数は、0-PIの間で値を返します。
diff --git a/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_ARToolKit.java b/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_ARToolKit.java
index 2f47ea1..d14d8b0 100644
--- a/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_ARToolKit.java
+++ b/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_ARToolKit.java
@@ -153,7 +153,7 @@ public class NyARTransMat_ARToolKit implements INyARTransMat
 		this._transsolver.solveTransportVector(vertex_3d,trans);
 		
 		//計算結果の最適化(平行移動量と回転行列の最適化)
-		this.optimize(this._rotmatrix, trans, this._transsolver,this._offset.vertex, vertex_2d);
+		o_result_conv.error=this.optimize(this._rotmatrix, trans, this._transsolver,this._offset.vertex, vertex_2d);
 		
 		// マトリクスの保存
 		this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,o_result_conv);
@@ -216,7 +216,10 @@ public class NyARTransMat_ARToolKit implements INyARTransMat
 				// 良い値が取れたら、差換え
 				this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,o_result_conv);
 			}
+			err=err2;
 		}
+		//エラー値保存
+		o_result_conv.error=err;
 		return;
 	}
 	private double optimize(NyARRotMatrix_ARToolKit io_rotmat,NyARDoublePoint3d io_transvec,INyARTransportVectorSolver i_solver,NyARDoublePoint3d[] i_offset_3d,NyARDoublePoint2d[] i_2d_vertex) throws NyARException
diff --git a/src/jp/nyatla/nyartoolkit/core/transmat/rotmatrix/NyARRotVector.java b/src/jp/nyatla/nyartoolkit/core/transmat/rotmatrix/NyARRotVector.java
index 2274485..b4310eb 100644
--- a/src/jp/nyatla/nyartoolkit/core/transmat/rotmatrix/NyARRotVector.java
+++ b/src/jp/nyatla/nyartoolkit/core/transmat/rotmatrix/NyARRotVector.java
@@ -82,9 +82,9 @@ public class NyARRotVector
 	{
 		//1行目
 		final NyARPerspectiveProjectionMatrix cmat= this._projection_mat_ref;
-		final double w1 = i_linear1.run * i_linear2.rise - i_linear2.run * i_linear1.rise;
-		final double w2 = i_linear1.rise * i_linear2.intercept - i_linear2.rise * i_linear1.intercept;
-		final double w3 = i_linear1.intercept * i_linear2.run - i_linear2.intercept * i_linear1.run;
+		final double w1 = i_linear1.dy * i_linear2.dx - i_linear2.dy * i_linear1.dx;
+		final double w2 = i_linear1.dx * i_linear2.c - i_linear2.dx * i_linear1.c;
+		final double w3 = i_linear1.c * i_linear2.dy - i_linear2.c * i_linear1.dy;
 
 		final double m0 = w1 * (cmat.m01 * cmat.m12 - cmat.m02 * cmat.m11) + w2 * cmat.m11 - w3 * cmat.m01;//w1 * (cpara[0 * 4 + 1] * cpara[1 * 4 + 2] - cpara[0 * 4 + 2] * cpara[1 * 4 + 1]) + w2 * cpara[1 * 4 + 1] - w3 * cpara[0 * 4 + 1];
 		final double m1 = -w1 * cmat.m00 * cmat.m12 + w3 * cmat.m00;//-w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0];
-- 
2.11.0