/**
* double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )
*
* @param i_square
* 計算対象のNyARSquareオブジェクト
* @param i_direction
* @param i_width
* @return
* @throws NyARException
*/
public void transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
{
NyARDoublePoint3d trans = this.__transMat_trans;
double err_threshold = makeErrThreshold(i_square.sqvertex);
//平行移動量計算機に、2D座標系をセット
NyARDoublePoint2d[] vertex_2d = this.__transMat_vertex_2d;
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
this._transsolver.set2dVertex(vertex_2d, 4);
//回転行列を計算
this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);
//回転後の3D座標系から、平行移動量を計算
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(平行移動量と回転行列の最適化)
o_result_conv.error = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold);
// マトリクスの保存
this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
return;
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* ARToolKitのarGetTransMatに該当します。
* @see INyARTransMat#transMatContinue
*/
public bool transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
{
NyARDoublePoint3d trans = this.__transMat_trans;
double err_threshold = makeErrThreshold(i_square.sqvertex);
NyARDoublePoint2d[] vertex_2d;
if (this._ref_dist_factor != null)
{
//歪み復元必要
vertex_2d = this.__transMat_vertex_2d;
this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
}
else
{
//歪み復元は不要
vertex_2d = i_square.sqvertex;
}
//平行移動量計算機に、2D座標系をセット
this._transsolver.set2dVertex(vertex_2d, 4);
//回転行列を計算
this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);
//回転後の3D座標系から、平行移動量を計算
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(平行移動量と回転行列の最適化)
this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result_conv);
return(true);
}
/**
* This function retrieves bitmap from the area defined by RECT(i_l,i_t,i_r,i_b) above transform matrix i_base_mat.
* ----
* この関数は、basementで示される平面のAで定義される領域から、ビットマップを読み出します。
* 例えば、8cmマーカでRECT(i_l,i_t,i_r,i_b)に-40,0,0,-40.0を指定すると、マーカの左下部分の画像を抽出します。
*
* マーカから離れた場所になるほど、また、マーカの鉛直方向から外れるほど誤差が大きくなります。
* @param i_src_imege
* 詠み出し元の画像を指定します。
* @param i_l
* 基準点からの左上の相対座標（x）を指定します。
* @param i_t
* 基準点からの左上の相対座標（y）を指定します。
* @param i_r
* 基準点からの右下の相対座標（x）を指定します。
* @param i_b
* 基準点からの右下の相対座標（y）を指定します。
* @param i_base_mat
* @return 画像の取得の成否を返す。
*/
public bool pickupImage2d(INyARRgbRaster i_src_imege, double i_l, double i_t, double i_r, double i_b, NyARTransMatResult i_base_mat)
{
double cp00, cp01, cp02, cp11, cp12;
cp00 = this._ref_perspective.m00;
cp01 = this._ref_perspective.m01;
cp02 = this._ref_perspective.m02;
cp11 = this._ref_perspective.m11;
cp12 = this._ref_perspective.m12;
//マーカと同一平面上にある矩形の4個の頂点を座標変換して、射影変換して画面上の
//頂点を計算する。
//[hX,hY,h]=[P][RT][x,y,z]
//出力先
NyARIntPoint2d[] poinsts = this._work_points;
double yt0, yt1, yt2;
double x3, y3, z3;
double m00 = i_base_mat.m00;
double m10 = i_base_mat.m10;
double m20 = i_base_mat.m20;
//yとtの要素を先に計算
yt0 = i_base_mat.m01 * i_t + i_base_mat.m03;
yt1 = i_base_mat.m11 * i_t + i_base_mat.m13;
yt2 = i_base_mat.m21 * i_t + i_base_mat.m23;
// l,t
x3 = m00 * i_l + yt0;
y3 = m10 * i_l + yt1;
z3 = m20 * i_l + yt2;
poinsts[0].x = (int)((x3 * cp00 + y3 * cp01 + z3 * cp02) / z3);
poinsts[0].y = (int)((y3 * cp11 + z3 * cp12) / z3);
// r,t
x3 = m00 * i_r + yt0;
y3 = m10 * i_r + yt1;
z3 = m20 * i_r + yt2;
poinsts[1].x = (int)((x3 * cp00 + y3 * cp01 + z3 * cp02) / z3);
poinsts[1].y = (int)((y3 * cp11 + z3 * cp12) / z3);
//yとtの要素を先に計算
yt0 = i_base_mat.m01 * i_b + i_base_mat.m03;
yt1 = i_base_mat.m11 * i_b + i_base_mat.m13;
yt2 = i_base_mat.m21 * i_b + i_base_mat.m23;
// r,b
x3 = m00 * i_r + yt0;
y3 = m10 * i_r + yt1;
z3 = m20 * i_r + yt2;
poinsts[2].x = (int)((x3 * cp00 + y3 * cp01 + z3 * cp02) / z3);
poinsts[2].y = (int)((y3 * cp11 + z3 * cp12) / z3);
// l,b
x3 = m00 * i_l + yt0;
y3 = m10 * i_l + yt1;
z3 = m20 * i_l + yt2;
poinsts[3].x = (int)((x3 * cp00 + y3 * cp01 + z3 * cp02) / z3);
poinsts[3].y = (int)((y3 * cp11 + z3 * cp12) / z3);
return this.pickFromRaster(i_src_imege, poinsts);
}
/**
* NyARTransMatResultの内容からNyARRotMatrixを復元します。
* @param i_prev_result
*/
public virtual void initRotByPrevResult(NyARTransMatResult i_prev_result)
{
this.m00 = i_prev_result.m00;
this.m01 = i_prev_result.m01;
this.m02 = i_prev_result.m02;
this.m10 = i_prev_result.m10;
this.m11 = i_prev_result.m11;
this.m12 = i_prev_result.m12;
this.m20 = i_prev_result.m20;
this.m21 = i_prev_result.m21;
this.m22 = i_prev_result.m22;
return;
}
/**
* NyARTransMatResultの内容からNyARRotMatrixを復元します。
* @param i_prev_result
*/
public virtual void initRotByPrevResult(NyARTransMatResult i_prev_result)
{
this.m00 = i_prev_result.m00;
this.m01 = i_prev_result.m01;
this.m02 = i_prev_result.m02;
this.m10 = i_prev_result.m10;
this.m11 = i_prev_result.m11;
this.m12 = i_prev_result.m12;
this.m20 = i_prev_result.m20;
this.m21 = i_prev_result.m21;
this.m22 = i_prev_result.m22;
return;
}
public void Test_arDetectMarkerLite()
{
Assembly assembly = Assembly.GetExecutingAssembly();
//AR用カメラパラメタファイルをロード
NyARParam ap = new NyARParam();
ap.loadARParam(assembly.GetManifestResourceStream(RES_CAMERA));
ap.changeScreenSize(320, 240);
//AR用のパターンコードを読み出し
NyARCode code = new NyARCode(16, 16);
Stream sr1=assembly.GetManifestResourceStream(RES_PATT);
code.loadARPatt(new StreamReader(sr1));
//試験イメージの読み出し(320x240 BGRAのRAWデータ)
StreamReader sr = new StreamReader(assembly.GetManifestResourceStream(RES_DATA));
BinaryReader bs = new BinaryReader(sr.BaseStream);
byte[] raw = bs.ReadBytes(320 * 240 * 4);
NyARRgbRaster_BGRA ra = new NyARRgbRaster_BGRA(320, 240,false);
ra.wrapBuffer(raw);
// Blank_Raster ra=new Blank_Raster(320, 240);
//１パターンのみを追跡するクラスを作成
// NyARSingleDetectMarker_Quad ar = new NyARSingleDetectMarker_Quad(ap, code, 80.0);
NyARSingleDetectMarker ar = new NyARSingleDetectMarker(ap, code, 80.0,ra.getBufferType());
NyARTransMatResult result_mat = new NyARTransMatResult();
ar.setContinueMode(false);
ar.detectMarkerLite(ra, 100);
ar.getTransmationMatrix(result_mat);
//マーカーを検出
for (int i3 = 0; i3 < 10; i3++)
{
Stopwatch sw = new Stopwatch();
sw.Start();
for (int i = 0; i < 10; i++)
{
//変換行列を取得
ar.detectMarkerLite(ra, 100);
ar.getTransmationMatrix(result_mat);
}
sw.Stop();
Debug.WriteLine(sw.ElapsedMilliseconds + "[ms]");
}
return;
}
/**
* この関数は、検出したマーカーの変換行列を計算して、o_resultへ値を返します。
* 直前に実行した{@link #detectMarkerLite}が成功していないと使えません。
* @param o_result
* 変換行列を受け取るオブジェクト。
* @
*/
public void getTransmat(NyARTransMatResult o_result)
{
// 一番一致したマーカーの位置とかその辺を計算
if (this._is_continue)
{
this._transmat.transMatContinue(this._square, this._offset, o_result, o_result);
}
else
{
this._transmat.transMat(this._square, this._offset, o_result);
}
return;
}
/// <summary>
/// Listener method called when something was detected.
/// </summary>
/// <param name="callingDetector">The detector that called the method.</param>
/// <param name="coordsX">The four x coordinates of the detected marker square.</param>
/// <param name="coordsY">The four y coordinates of the detected marker square.</param>
/// <param name="coordCount">The number of coordinates.</param>
/// <param name="coordIndices">The indices of the coordiantes in the coords array.</param>
public void onSquareDetect(NyARSquareContourDetector callingDetector, int[] coordsX, int[] coordsY, int coordCount, int[] coordIndices)
{
// Init variables
points[0].x = coordsX[coordIndices[0]];
points[0].y = coordsY[coordIndices[0]];
points[1].x = coordsX[coordIndices[1]];
points[1].y = coordsY[coordIndices[1]];
points[2].x = coordsX[coordIndices[2]];
points[2].y = coordsY[coordIndices[2]];
points[3].x = coordsX[coordIndices[3]];
points[3].y = coordsY[coordIndices[3]];
// Evaluate and find best match
if (this.colorPattern.pickFromRaster(this.Buffer, points))
{
// Find best matching marker
this.patternMatchDeviationData.setRaster(this.colorPattern);
Marker foundMarker = null;
int foundDirection = NyARMatchPattResult.DIRECTION_UNKNOWN;
double bestConfidence = 0;
foreach (var patMat in patternMatchers)
{
// Evaluate
patMat.evaluate(this.patternMatchDeviationData, evaluationResult);
// Best match?
if (evaluationResult.confidence > bestConfidence)
{
foundMarker = patMat.Marker;
foundDirection = evaluationResult.direction;
bestConfidence = evaluationResult.confidence;
}
}
// Calculate found marker square
var square = new NyARSquare();
var len = coordIndices.Length;
for (int i = 0; i < len; i++)
{
int idx = (i + len - foundDirection) % len;
this.coordinationMapper.coord2Line(coordIndices[idx], coordIndices[(idx + 1) % len], coordsX, coordsY, coordCount, square.line[i]);
}
// Calculate normal
for (int i = 0; i < len; i++)
{
NyARLinear.crossPos(square.line[i], square.line[(i + 3) % len], square.sqvertex[i]);
}
// Calculate matrix using continued mode
if (foundMarker != null)
{
var nymat = new NyARTransMatResult();
matrixCalculator.transMatContinue(square, foundMarker.RectOffset, nymat);
// Create and add result to collection
var v = square.sqvertex;
var resultSquare = new Square(v[0].x, v[0].y, v[1].x, v[1].y, v[2].x, v[2].y, v[3].x, v[3].y);
Results.Add(new DetectionResult(foundMarker, bestConfidence, nymat.ToMatrix3D(), resultSquare));
}
}
}
/**
* double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )
*
* @param i_square
* 計算対象のNyARSquareオブジェクト
* @param i_direction
* @param i_width
* @return
* @throws NyARException
*/
public void transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
{
NyARDoublePoint3d trans = this.__transMat_trans;
double err_threshold = makeErrThreshold(i_square.sqvertex);
//平行移動量計算機に、2D座標系をセット
NyARDoublePoint2d[] vertex_2d = this.__transMat_vertex_2d;
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
this._transsolver.set2dVertex(vertex_2d, 4);
//回転行列を計算
this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);
//回転後の3D座標系から、平行移動量を計算
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(平行移動量と回転行列の最適化)
o_result_conv.error = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold);
// マトリクスの保存
this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
return;
}
/**
*
* @param iw_rotmat
* @param iw_transvec
* @param i_solver
* @param i_offset_3d
* @param i_2d_vertex
* @param i_err_threshold
* @param o_result
* @return
* @
*/
private void optimize(NyARRotMatrix iw_rotmat, NyARDoublePoint3d iw_transvec, INyARTransportVectorSolver i_solver, NyARDoublePoint3d[] i_offset_3d, NyARDoublePoint2d[] i_2d_vertex, double i_err_threshold, NyARTransMatResult o_result)
{
//System.out.println("START");
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
//初期のエラー値を計算
double min_err = errRate(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d);
o_result.setValue(iw_rotmat, iw_transvec, min_err);
for (int i = 0; i < 5; i++)
{
//変換行列の最適化
this._mat_optimize.modifyMatrix(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4);
double err = errRate(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d);
//System.out.println("E:"+err);
if (min_err - err < i_err_threshold)
{
//System.out.println("BREAK");
break;
}
i_solver.solveTransportVector(vertex_3d, iw_transvec);
o_result.setValue(iw_rotmat, iw_transvec, err);
min_err = err;
}
//System.out.println("END");
return;
}
/**
* パラメータで変換行列を更新します。
*
* @param i_rot
* @param i_off
* @param i_trans
*/
public void updateMatrixValue(NyARRotMatrix i_rot, NyARDoublePoint3d i_trans, NyARTransMatResult o_result)
{
o_result.m00 = i_rot.m00;
o_result.m01 = i_rot.m01;
o_result.m02 = i_rot.m02;
o_result.m03 = i_trans.x;
o_result.m10 = i_rot.m10;
o_result.m11 = i_rot.m11;
o_result.m12 = i_rot.m12;
o_result.m13 = i_trans.y;
o_result.m20 = i_rot.m20;
o_result.m21 = i_rot.m21;
o_result.m22 = i_rot.m22;
o_result.m23 = i_trans.z;
o_result.has_value = true;
return;
}
/*
* (non-Javadoc)
* @see jp.nyatla.nyartoolkit.core.transmat.INyARTransMat#transMatContinue(jp.nyatla.nyartoolkit.core.NyARSquare, int, double, jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult)
*/
public void transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
{
NyARDoublePoint3d trans = this.__transMat_trans;
// io_result_convが初期値なら、transMatで計算する。
if (!o_result_conv.has_value)
{
this.transMat(i_square, i_offset, o_result_conv);
return;
}
//最適化計算の閾値を決定
double err_threshold = makeErrThreshold(i_square.sqvertex);
//平行移動量計算機に、2D座標系をセット
NyARDoublePoint2d[] vertex_2d = this.__transMat_vertex_2d;
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
this._transsolver.set2dVertex(vertex_2d, 4);
//回転行列を計算
this._rotmatrix.initRotByPrevResult(o_result_conv);
//回転後の3D座標系から、平行移動量を計算
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//現在のエラーレートを計算しておく
double min_err = errRate(this._rotmatrix, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
NyARDoubleMatrix33 rot = this.__rot;
//エラーレートが前回のエラー値より閾値分大きかったらアゲイン
if (min_err < o_result_conv.error + err_threshold)
{
rot.setValue(this._rotmatrix);
//最適化してみる。
for (int i = 0; i < 5; i++)
{
//変換行列の最適化
this._mat_optimize.modifyMatrix(rot, trans, i_offset.vertex, vertex_2d, 4);
double err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
//System.out.println("E:"+err);
if (min_err - err < err_threshold / 2)
{
//System.out.println("BREAK");
break;
}
this._transsolver.solveTransportVector(vertex_3d, trans);
this._rotmatrix.setValue(rot);
min_err = err;
}
this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
}
else
{
//回転行列を計算
this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);
//回転後の3D座標系から、平行移動量を計算
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(平行移動量と回転行列の最適化)
min_err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold);
this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
}
o_result_conv.error = min_err;
return;
}
/**
* パラメータで変換行列を更新します。
*
* @param i_rot
* @param i_off
* @param i_trans
*/
public void updateMatrixValue(NyARRotMatrix i_rot, NyARDoublePoint3d i_trans, NyARTransMatResult o_result)
{
o_result.m00 = i_rot.m00;
o_result.m01 = i_rot.m01;
o_result.m02 = i_rot.m02;
o_result.m03 = i_trans.x;
o_result.m10 = i_rot.m10;
o_result.m11 = i_rot.m11;
o_result.m12 = i_rot.m12;
o_result.m13 = i_trans.y;
o_result.m20 = i_rot.m20;
o_result.m21 = i_rot.m21;
o_result.m22 = i_rot.m22;
o_result.m23 = i_trans.z;
o_result.has_value = true;
return;
}
/*
* (non-Javadoc)
* @see jp.nyatla.nyartoolkit.core.transmat.INyARTransMat#transMatContinue(jp.nyatla.nyartoolkit.core.NyARSquare, int, double, jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult)
*/
public void transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
{
NyARDoublePoint3d trans = this.__transMat_trans;
// io_result_convが初期値なら、transMatで計算する。
if (!o_result_conv.has_value)
{
this.transMat(i_square, i_offset, o_result_conv);
return;
}
//最適化計算の閾値を決定
double err_threshold = makeErrThreshold(i_square.sqvertex);
//平行移動量計算機に、2D座標系をセット
NyARDoublePoint2d[] vertex_2d = this.__transMat_vertex_2d;
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
this._transsolver.set2dVertex(vertex_2d, 4);
//回転行列を計算
this._rotmatrix.initRotByPrevResult(o_result_conv);
//回転後の3D座標系から、平行移動量を計算
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//現在のエラーレートを計算しておく
double min_err = errRate(this._rotmatrix, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
NyARDoubleMatrix33 rot = this.__rot;
//エラーレートが前回のエラー値より閾値分大きかったらアゲイン
if (min_err < o_result_conv.error + err_threshold)
{
rot.setValue(this._rotmatrix);
//最適化してみる。
for (int i = 0; i < 5; i++)
{
//変換行列の最適化
this._mat_optimize.modifyMatrix(rot, trans, i_offset.vertex, vertex_2d, 4);
double err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
//System.out.println("E:"+err);
if (min_err - err < err_threshold / 2)
{
//System.out.println("BREAK");
break;
}
this._transsolver.solveTransportVector(vertex_3d, trans);
this._rotmatrix.setValue(rot);
min_err = err;
}
this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
}
else
{
//回転行列を計算
this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);
//回転後の3D座標系から、平行移動量を計算
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(平行移動量と回転行列の最適化)
min_err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold);
this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
}
o_result_conv.error = min_err;
return;
}
/**
* @deprecated
* {@link #getTransmat}
*/
public void getTransmationMatrix(NyARTransMatResult o_result)
{
this.getTransmat(o_result);
return;
}
protected abstract void onUpdateHandler(NyARSquare i_square, NyARTransMatResult result);
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* ARToolKitのarGetTransMatに該当します。
* @see INyARTransMat#transMatContinue
*/
public bool transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
{
NyARDoublePoint3d trans = this.__transMat_trans;
double err_threshold = makeErrThreshold(i_square.sqvertex);
NyARDoublePoint2d[] vertex_2d;
if (this._ref_dist_factor != null)
{
//歪み復元必要
vertex_2d = this.__transMat_vertex_2d;
this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
}
else
{
//歪み復元は不要
vertex_2d = i_square.sqvertex;
}
//平行移動量計算機に、2D座標系をセット
this._transsolver.set2dVertex(vertex_2d, 4);
//回転行列を計算
this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);
//回転後の3D座標系から、平行移動量を計算
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(平行移動量と回転行列の最適化)
this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result_conv);
return true;
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 計算に過去の履歴を使う点が、{@link #transMat}と異なります。
* @see INyARTransMat#transMatContinue
*/
public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult i_prev_result, NyARTransMatResult o_result)
{
NyARDoublePoint3d trans = this.__transMat_trans;
// io_result_convが初期値なら、transMatで計算する。
if (!i_prev_result.has_value)
{
this.transMat(i_square, i_offset, o_result);
return(true);
}
//過去のエラーレートを記録(ここれやるのは、i_prev_resultとo_resultに同じインスタンスを指定できるようにするため)
double last_error = i_prev_result.last_error;
//最適化計算の閾値を決定
double err_threshold = makeErrThreshold(i_square.sqvertex);
//平行移動量計算機に、2D座標系をセット
NyARDoublePoint2d[] vertex_2d;
if (this._ref_dist_factor != null)
{
vertex_2d = this.__transMat_vertex_2d;
this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
}
else
{
vertex_2d = i_square.sqvertex;
}
this._transsolver.set2dVertex(vertex_2d, 4);
//回転行列を計算
NyARRotMatrix rot = this._rotmatrix;
rot.initRotByPrevResult(i_prev_result);
//回転後の3D座標系から、平行移動量を計算
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//現在のエラーレートを計算
double min_err = errRate(this._rotmatrix, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
//結果をストア
o_result.setValue(rot, trans, min_err);
//エラーレートの判定
if (min_err < last_error + err_threshold)
{
// System.out.println("TR:ok");
//最適化してみる。
for (int i = 0; i < 5; i++)
{
//変換行列の最適化
this._mat_optimize.modifyMatrix(rot, trans, i_offset.vertex, vertex_2d, 4);
double err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
//System.out.println("E:"+err);
if (min_err - err < err_threshold / 2)
{
//System.out.println("BREAK");
break;
}
this._transsolver.solveTransportVector(vertex_3d, trans);
o_result.setValue(rot, trans, err);
min_err = err;
}
}
else
{
// System.out.println("TR:again");
//回転行列を計算
rot.initRotBySquare(i_square.line, i_square.sqvertex);
//回転後の3D座標系から、平行移動量を計算
rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(平行移動量と回転行列の最適化)
this.optimize(rot, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result);
}
return(true);
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 計算に過去の履歴を使う点が、{@link #transMat}と異なります。
* @see INyARTransMat#transMatContinue
*/
public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult i_prev_result, NyARTransMatResult o_result)
{
NyARDoublePoint3d trans = this.__transMat_trans;
// io_result_convが初期値なら、transMatで計算する。
if (!i_prev_result.has_value)
{
this.transMat(i_square, i_offset, o_result);
return true;
}
//過去のエラーレートを記録(ここれやるのは、i_prev_resultとo_resultに同じインスタンスを指定できるようにするため)
double last_error = i_prev_result.last_error;
//最適化計算の閾値を決定
double err_threshold = makeErrThreshold(i_square.sqvertex);
//平行移動量計算機に、2D座標系をセット
NyARDoublePoint2d[] vertex_2d;
if (this._ref_dist_factor != null)
{
vertex_2d = this.__transMat_vertex_2d;
this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4);
}
else
{
vertex_2d = i_square.sqvertex;
}
this._transsolver.set2dVertex(vertex_2d, 4);
//回転行列を計算
NyARRotMatrix rot = this._rotmatrix;
rot.initRotByPrevResult(i_prev_result);
//回転後の3D座標系から、平行移動量を計算
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//現在のエラーレートを計算
double min_err = errRate(this._rotmatrix, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
//結果をストア
o_result.setValue(rot, trans, min_err);
//エラーレートの判定
if (min_err < last_error + err_threshold)
{
// System.out.println("TR:ok");
//最適化してみる。
for (int i = 0; i < 5; i++)
{
//変換行列の最適化
this._mat_optimize.modifyMatrix(rot, trans, i_offset.vertex, vertex_2d, 4);
double err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
//System.out.println("E:"+err);
if (min_err - err < err_threshold / 2)
{
//System.out.println("BREAK");
break;
}
this._transsolver.solveTransportVector(vertex_3d, trans);
o_result.setValue(rot, trans, err);
min_err = err;
}
}
else
{
// System.out.println("TR:again");
//回転行列を計算
rot.initRotBySquare(i_square.line, i_square.sqvertex);
//回転後の3D座標系から、平行移動量を計算
rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(平行移動量と回転行列の最適化)
this.optimize(rot, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result);
}
return true;
}
/**
*
* @param iw_rotmat
* @param iw_transvec
* @param i_solver
* @param i_offset_3d
* @param i_2d_vertex
* @param i_err_threshold
* @param o_result
* @return
* @
*/
private void optimize(NyARRotMatrix iw_rotmat, NyARDoublePoint3d iw_transvec, INyARTransportVectorSolver i_solver, NyARDoublePoint3d[] i_offset_3d, NyARDoublePoint2d[] i_2d_vertex, double i_err_threshold, NyARTransMatResult o_result)
{
//System.out.println("START");
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
//初期のエラー値を計算
double min_err = errRate(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d);
o_result.setValue(iw_rotmat, iw_transvec, min_err);
for (int i = 0; i < 5; i++)
{
//変換行列の最適化
this._mat_optimize.modifyMatrix(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4);
double err = errRate(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d);
//System.out.println("E:"+err);
if (min_err - err < i_err_threshold)
{
//System.out.println("BREAK");
break;
}
i_solver.solveTransportVector(vertex_3d, iw_transvec);
o_result.setValue(iw_rotmat, iw_transvec, err);
min_err = err;
}
//System.out.println("END");
return;
}
private void UpdateMarkerTransforms()
{
// Reset all marker patterns to not found
foreach (MarkerInfo markerInfo in markerInfoList)
{
markerInfo.IsFound = false;
foreach (PatternInfo patternInfo in markerInfo.PatternInfos.Values)
patternInfo.IsFound = false;
}
int numMarkerFound = multiDetector.detectMarkerLite(raster, threshold);
NyARTransMatResult result = new NyARTransMatResult();
for (int i = 0; i < numMarkerFound; ++i)
{
int id = multiDetector.getARCodeIndex(i);
MarkerInfo markerInfo = markerInfoMap[id];
if (multiDetector.getConfidence(i) >= markerInfo.PatternInfos[id].ConfidenceThreshold)
{
try
{
markerInfo.IsFound = true;
markerInfo.PatternInfos[id].IsFound = true;
markerInfo.PatternInfos[id].Confidence = (float)multiDetector.getConfidence(i);
multiDetector.getTransmationMatrix(i, result);
markerInfo.PatternInfos[id].Transform = (new Matrix(
(float)result.m00, (float)result.m10, (float)result.m20, (float)result.m30,
(float)result.m01, (float)result.m11, (float)result.m21, (float)result.m31,
(float)result.m02, (float)result.m12, (float)result.m22, (float)result.m32,
(float)result.m03, (float)result.m13, (float)result.m23, (float)result.m33)) *
Matrix.CreateFromAxisAngle(Vector3.UnitX, MathHelper.Pi);
}
catch (NyARException exp)
{
Log.Write(exp.Message);
markerInfo.PatternInfos[id].IsFound = false;
markerInfo.PatternInfos[id].Confidence = 0;
}
}
}
// Compute the final transformation of each marker info
foreach (MarkerInfo markerInfo in markerInfoList)
{
if (markerInfo.IsFound)
{
Matrix resultMat = MatrixHelper.Empty;
List<Matrix> transforms = null;
List<float> weights = null;
switch (markerInfo.Method)
{
case ComputationMethod.Average:
int count = 0;
foreach (int id in markerInfo.PatternInfos.Keys)
{
PatternInfo patternInfo = markerInfo.PatternInfos[id];
if (patternInfo.IsFound)
{
tmpMat1 = markerInfo.RelativeTransforms[id];
Matrix.Multiply(ref tmpMat1, ref patternInfo.Transform, out tmpMat2);
Matrix.Add(ref resultMat, ref tmpMat2, out resultMat);
count++;
}
}
resultMat /= count;
break;
case ComputationMethod.BestConfidence:
float bestConfidence = 0;
foreach (int id in markerInfo.PatternInfos.Keys)
{
PatternInfo patternInfo = markerInfo.PatternInfos[id];
if (patternInfo.IsFound && patternInfo.Confidence > bestConfidence)
{
tmpMat1 = markerInfo.RelativeTransforms[id];
Matrix.Multiply(ref tmpMat1, ref patternInfo.Transform, out resultMat);
bestConfidence = patternInfo.Confidence;
}
}
break;
case ComputationMethod.WeightedAverage:
transforms = new List<Matrix>();
weights = new List<float>();
float weightSum = 0;
foreach (int id in markerInfo.PatternInfos.Keys)
{
PatternInfo patternInfo = markerInfo.PatternInfos[id];
if (patternInfo.IsFound)
{
Matrix transform = markerInfo.RelativeTransforms[id];
Matrix.Multiply(ref transform, ref patternInfo.Transform, out transform);
transforms.Add(transform);
weights.Add(patternInfo.Confidence);
weightSum += patternInfo.Confidence;
}
}
for (int i = 0; i < weights.Count; ++i)
weights[i] /= weightSum;
for (int i = 0; i < transforms.Count; ++i)
{
tmpMat1 = transforms[i];
Matrix.Multiply(ref tmpMat1, weights[i], out tmpMat2);
Matrix.Add(ref resultMat, ref tmpMat2, out resultMat);
}
break;
case ComputationMethod.Custom:
if (ComputeTransform == null)
throw new MarkerException("For ComputationMethod.Custom method, you must set " +
"the ComputeTransform property");
transforms = new List<Matrix>();
weights = new List<float>();
foreach (int id in markerInfo.PatternInfos.Keys)
{
PatternInfo patternInfo = markerInfo.PatternInfos[id];
if (patternInfo.IsFound)
{
Matrix transform = markerInfo.RelativeTransforms[id];
Matrix.Multiply(ref transform, ref patternInfo.Transform, out transform);
transforms.Add(transform);
weights.Add(patternInfo.Confidence);
}
}
ComputeTransform(transforms, weights, out resultMat);
break;
}
markerInfo.Transform = resultMat;
}
}
}