override protected void onReservRequest(int i_start, int i_end, object[] i_buffer)
{
for (int i = i_start; i < i_end; i++)
{
i_buffer[i] = new NyARSquare();
}
}
/**
* この関数は、頂点同士の距離から、頂点のシフト量（回転量）を返します。
* よく似た２つの矩形の頂点同士の、頂点の対応を取るために使用します。
* @param i_square
* 比較対象の矩形
* @return
* シフト量を数値で返します。
* シフト量はthis-i_squareです。1の場合、this.sqvertex[0]とi_square.sqvertex[1]が対応点になる(shift量1)であることを示します。
*/
public int checkVertexShiftValue(NyARSquare i_square)
{
NyARDoublePoint2d[] a = this.sqvertex;
NyARDoublePoint2d[] b = i_square.sqvertex;
//3-0番目
int min_dist = int.MaxValue;
int min_index = 0;
int xd, yd;
for (int i = 3; i >= 0; i--)
{
int d = 0;
for (int i2 = 3; i2 >= 0; i2--)
{
xd = (int)(a[i2].x - b[(i2 + i) % 4].x);
yd = (int)(a[i2].y - b[(i2 + i) % 4].y);
d += xd * xd + yd * yd;
}
if (min_dist > d)
{
min_dist = d;
min_index = i;
}
}
return(min_index);
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 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);
}
/**
* この関数は、頂点同士の距離から、頂点のシフト量（回転量）を返します。
* よく似た２つの矩形の頂点同士の、頂点の対応を取るために使用します。
* @param i_square
* 比較対象の矩形
* @return
* シフト量を数値で返します。
* シフト量はthis-i_squareです。1の場合、this.sqvertex[0]とi_square.sqvertex[1]が対応点になる(shift量1)であることを示します。
*/
public int checkVertexShiftValue(NyARSquare i_square)
{
NyARDoublePoint2d[] a = this.sqvertex;
NyARDoublePoint2d[] b = i_square.sqvertex;
//3-0番目
int min_dist = int.MaxValue;
int min_index = 0;
int xd, yd;
for (int i = 3; i >= 0; i--)
{
int d = 0;
for (int i2 = 3; i2 >= 0; i2--)
{
xd = (int)(a[i2].x - b[(i2 + i) % 4].x);
yd = (int)(a[i2].y - b[(i2 + i) % 4].y);
d += xd * xd + yd * yd;
}
if (min_dist > d)
{
min_dist = d;
min_index = i;
}
}
return min_index;
}
/**
* 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;
}
override protected void onReservRequest(int i_start, int i_end, object[] i_buffer)
{
for (int i = i_start; i < i_end; i++)
{
i_buffer[i] = new NyARSquare();
}
}
/**
* arGetLine(int x_coord[], int y_coord[], int coord_num,int vertex[], double line[4][3], double v[4][2]) arGetLine2(int x_coord[], int y_coord[], int
* coord_num,int vertex[], double line[4][3], double v[4][2], double *dist_factor) の２関数の合成品です。 マーカーのvertex,lineを計算して、結果をo_squareに保管します。
* Optimize:STEP[424->391]
*
* @param i_cparam
* @return
* @throws NyARException
*/
private bool getSquareLine(int[] i_mkvertex, int[] i_xcoord, int[] i_ycoord, NyARSquare o_square)
{
NyARLinear[] l_line = o_square.line;
NyARVec ev = this.__getSquareLine_ev; // matrixPCAの戻り値を受け取る
NyARVec mean = this.__getSquareLine_mean; // matrixPCAの戻り値を受け取る
double[] mean_array = mean.getArray();
NyARCameraDistortionFactor dist_factor = this._dist_factor_ref;
NyARMat input = this.__getSquareLine_input; // 次処理で初期化される。
NyARMat evec = this.__getSquareLine_evec; // アウトパラメータを受け取るから初期化不要//new NyARMat(2,2);
double[][] evec_array = evec.getArray();
double w1;
int st, ed, n, i;
NyARLinear l_line_i, l_line_2;
for (i = 0; i < 4; i++)
{
w1 = (double)(i_mkvertex[i + 1] - i_mkvertex[i] + 1) * 0.05 + 0.5;
st = (int)(i_mkvertex[i] + w1);
ed = (int)(i_mkvertex[i + 1] - w1);
n = ed - st + 1;
if (n < 2)
{
// nが2以下でmatrix.PCAを計算することはできないので、エラー
return(false);
}
// pcaの準備
input.realloc(n, 2);
// バッチ取得
dist_factor.observ2IdealBatch(i_xcoord, i_ycoord, st, n, input.getArray());
// 主成分分析
input.matrixPCA(evec, ev, mean);
l_line_i = l_line[i];
l_line_i.run = evec_array[0][1]; // line[i][0] = evec->m[1];
l_line_i.rise = -evec_array[0][0]; // line[i][1] = -evec->m[0];
l_line_i.intercept = -(l_line_i.run * mean_array[0] + l_line_i.rise * mean_array[1]); // line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]);
}
NyARDoublePoint2d[] l_sqvertex = o_square.sqvertex;
NyARIntPoint[] l_imvertex = o_square.imvertex;
for (i = 0; i < 4; i++)
{
l_line_i = l_line[i];
l_line_2 = l_line[(i + 3) % 4];
w1 = l_line_2.run * l_line_i.rise - l_line_i.run * l_line_2.rise;
if (w1 == 0.0)
{
return(false);
}
l_sqvertex[i].x = (l_line_2.rise * l_line_i.intercept - l_line_i.rise * l_line_2.intercept) / w1;
l_sqvertex[i].y = (l_line_i.run * l_line_2.intercept - l_line_2.run * l_line_i.intercept) / w1;
// 頂点インデクスから頂点座標を得て保存
l_imvertex[i].x = i_xcoord[i_mkvertex[i]];
l_imvertex[i].y = i_ycoord[i_mkvertex[i]];
}
return(true);
}
protected abstract void onUpdateHandler(NyARSquare i_square, NyARTransMatResult result);
/**
* 頂点データをNyARSquareにセットする関数です。
* 初期位置セットには使わないこと。
* @param i_vx
* @param i_s
*/
private void setSquare(NyARDoublePoint2d[] i_vx,NyARSquare i_s)
{
NyARLinear l=this.__tmp_l;
//線分を平滑化。（ノイズが多いソースを使う時は線分の平滑化。ほんとは使いたくない。）
for(int i=3;i>=0;i--){
i_s.sqvertex[i].setValue(i_vx[i]);
l.makeLinearWithNormalize(i_vx[i], i_vx[(i+1)%4]);
i_s.line[i].a=i_s.line[i].a*0.6+l.a*0.4;
i_s.line[i].b=i_s.line[i].b*0.6+l.b*0.4;
i_s.line[i].c=i_s.line[i].c*0.6+l.c*0.4;
}
for(int i=3;i>=0;i--){
i_s.line[i].crossPos(i_s.line[(i+3)%4],i_s.sqvertex[i]);
}
}
/**
* アプリケーションフレームワークのハンドラ（マーカ更新）
*/
protected override void onUpdateHandler(NyARSquare i_square, NyARDoubleMatrix44 result)
{
this.transmat = result;
}
/**
* アプリケーションフレームワークのハンドラ（マーカ更新）
*/
protected override void onUpdateHandler(NyARSquare i_square, NyARDoubleMatrix44 result)
{
NyARD3dUtil.toD3dCameraView(result, 1f, ref this.transmat);
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 計算に過去の履歴を使う点が、{@link #transMat}と異なります。
* @see INyARTransMat#transMatContinue
*/
public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 i_prev_result, double i_prev_err, NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param)
{
NyARDoublePoint3d trans = this.__transMat_trans;
//最適化計算の閾値を決定
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(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
//エラーレートの判定
if (min_err < i_prev_err + err_threshold)
{
//save initial result
o_result.setValue(rot, trans);
// 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);
min_err = err;
}
//継続計算成功
if (o_param != null)
{
o_param.last_error = min_err;
}
return(true);
}
//継続計算失敗
return(false);
}
/** オブジェクトのステータスを更新し、必要に応じて自己コールバック関数を駆動します。
* 戻り値は、「実際にマーカを発見する事ができたか」を示す真偽値です。クラスの状態とは異なります。
*/
private bool updateStatus(NyARSquare i_square, int i_code_index)
{
if (this._current_arcode_index < 0)
{// 未認識中
if (i_code_index < 0)
{// 未認識から未認識の遷移
// なにもしないよーん。
return false;
}
else
{// 未認識から認識の遷移
this._current_arcode_index = i_code_index;
// イベント生成
// OnEnter
this.onEnterHandler(i_code_index);
// 変換行列を作成
this._transmat.transMat(i_square, this._offset, this._transmat_result, this._last_result_param);
// OnUpdate
this.onUpdateHandler(i_square, this._transmat_result);
this._lost_delay_count = 0;
return true;
}
}
else
{// 認識中
if (i_code_index < 0)
{// 認識から未認識の遷移
this._lost_delay_count++;
if (this._lost_delay < this._lost_delay_count)
{
// OnLeave
this._current_arcode_index = -1;
this.onLeaveHandler();
}
return false;
}
else if (i_code_index == this._current_arcode_index)
{// 同じARCodeの再認識
// イベント生成
// 変換行列を作成
if (!this._transmat.transMatContinue(i_square, this._offset, this._transmat_result, this._last_result_param.last_error, this._transmat_result, this._last_result_param))
{
this._transmat.transMat(i_square, this._offset, this._transmat_result, this._last_result_param);
}
// OnUpdate
this.onUpdateHandler(i_square, this._transmat_result);
this._lost_delay_count = 0;
return true;
}
else
{// 異なるコードの認識→今はサポートしない。
throw new NyARException();
}
}
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 計算に過去の履歴を使う点が、{@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);
}
/**
* ARMarkerInfo2 *arDetectMarker2( ARInt16 *limage, int label_num, int *label_ref,int *warea, double *wpos, int *wclip,int area_max, int area_min, double
* factor, int *marker_num ) 関数の代替品 ラベリング情報からマーカー一覧を作成してo_marker_listを更新します。 関数はo_marker_listに重なりを除外したマーカーリストを作成します。
*
* @param i_raster
* 解析する２値ラスタイメージを指定します。
* @param o_square_stack
* 抽出した正方形候補を格納するリスト
* @throws NyARException
*/
public void detectMarker(NyARBinRaster i_raster, NyARSquareStack o_square_stack)
{
INyARLabeling labeling_proc = this._labeling;
NyARLabelingImage limage = this._limage;
// 初期化
// マーカーホルダをリセット
o_square_stack.clear();
// ラベリング
labeling_proc.labeling(i_raster);
// ラベル数が0ならここまで
int label_num = limage.getLabelStack().getLength();
if (label_num < 1)
{
return;
}
NyARLabelingLabelStack stack = limage.getLabelStack();
NyARLabelingLabel[] labels = stack.getArray();
// ラベルを大きい順に整列
stack.sortByArea();
// デカいラベルを読み飛ばし
int i;
for (i = 0; i < label_num; i++)
{
// 検査対象内のラベルサイズになるまで無視
if (labels[i].area <= AR_AREA_MAX)
{
break;
}
}
int xsize = this._width;
int ysize = this._height;
int[] xcoord = this._xcoord;
int[] ycoord = this._ycoord;
int coord_max = this._max_coord;
int[] mkvertex = this.__detectMarker_mkvertex;
OverlapChecker overlap = this._overlap_checker;
int coord_num;
int label_area;
NyARLabelingLabel label_pt;
//重なりチェッカの最大数を設定
overlap.reset(label_num);
int vertex1;
for (; i < label_num; i++)
{
label_pt = labels[i];
label_area = label_pt.area;
// 検査対象サイズよりも小さくなったら終了
if (label_area < AR_AREA_MIN)
{
break;
}
// クリップ領域が画面の枠に接していれば除外
if (label_pt.clip_l == 1 || label_pt.clip_r == xsize - 2)
{// if(wclip[i*4+0] == 1 || wclip[i*4+1] ==xsize-2){
continue;
}
if (label_pt.clip_t == 1 || label_pt.clip_b == ysize - 2)
{// if( wclip[i*4+2] == 1 || wclip[i*4+3] ==ysize-2){
continue;
}
// 既に検出された矩形との重なりを確認
if (!overlap.check(label_pt))
{
// 重なっているようだ。
continue;
}
// 輪郭を取得
coord_num = limage.getContour(i, coord_max, xcoord, ycoord);
if (coord_num == coord_max)
{
// 輪郭が大きすぎる。
continue;
}
//頂点候補のインデクスを取得
vertex1 = scanVertex(xcoord, ycoord, coord_num);
// 頂点候補(vertex1)を先頭に並べなおした配列を作成する。
normalizeCoord(xcoord, ycoord, vertex1, coord_num);
// 領域を準備する。
NyARSquare square_ptr = o_square_stack.prePush();
// 頂点情報を取得
if (!getSquareVertex(xcoord, ycoord, vertex1, coord_num, label_area, mkvertex))
{
o_square_stack.pop();// 頂点の取得が出来なかったので破棄
continue;
}
if (!getSquareLine(mkvertex, xcoord, ycoord, square_ptr))
{
// 矩形が成立しなかった。
o_square_stack.pop();
continue;
}
// 検出済の矩形の属したラベルを重なりチェックに追加する。
overlap.push(label_pt);
}
return;
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 計算に過去の履歴を使う点が、{@link #transMat}と異なります。
* @see INyARTransMat#transMatContinue
*/
public bool transMatContinue(NyARSquare i_square,NyARRectOffset i_offset, NyARDoubleMatrix44 i_prev_result,double i_prev_err,NyARDoubleMatrix44 o_result,NyARTransMatResultParam o_param)
{
NyARDoublePoint3d trans = this.__transMat_trans;
//最適化計算の閾値を決定
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(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d);
//エラーレートの判定
if(min_err<i_prev_err+err_threshold){
//save initial result
o_result.setValue(rot,trans);
// 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);
min_err = err;
}
//継続計算成功
if (o_param != null)
{
o_param.last_error = min_err;
}
return true;
}
//継続計算失敗
return false;
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* ARToolKitのarGetTransMatに該当します。
* @see INyARTransMat#transMatContinue
*/
public bool transMat(NyARSquare i_square,NyARRectOffset i_offset, NyARDoubleMatrix44 o_result,NyARTransMatResultParam o_param)
{
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);
//回転行列を計算
if (!this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex))
{
return false;
}
//回転後の3D座標系から、平行移動量を計算
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(平行移動量と回転行列の最適化)
double err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result);
//必要なら計算パラメータを返却
if (o_param != null)
{
o_param.last_error = err;
}
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;
}
/*
* (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;
}
/**オブジェクトのステータスを更新し、必要に応じて自己コールバック関数を駆動します。
*/
private bool updateStatus(NyARSquare i_square, INyIdMarkerData i_marker_data)
{
bool is_id_found = false;
if (!this._is_active)
{// 未認識中
if (i_marker_data == null)
{// 未認識から未認識の遷移
// なにもしないよーん。
this._is_active = false;
}
else
{// 未認識から認識の遷移
this._data_current.copyFrom(i_marker_data);
// イベント生成
// OnEnter
this.onEnterHandler(this._data_current);
// 変換行列を作成
this._transmat.transMat(i_square, this._offset, this._transmat_result, this._last_result_param);
// OnUpdate
this.onUpdateHandler(i_square, this._transmat_result);
this._lost_delay_count = 0;
this._is_active = true;
is_id_found = true;
}
}
else
{// 認識中
if (i_marker_data == null)
{
// 認識から未認識の遷移
this._lost_delay_count++;
if (this._lost_delay < this._lost_delay_count)
{
// OnLeave
this.onLeaveHandler();
this._is_active = false;
}
}
else if (this._data_current.isEqual(i_marker_data))
{
//同じidの再認識
if (!this._transmat.transMatContinue(i_square, this._offset, this._transmat_result, this._last_result_param.last_error, this._transmat_result, this._last_result_param))
{
this._transmat.transMat(i_square, this._offset, this._transmat_result, this._last_result_param);
}
// OnUpdate
this.onUpdateHandler(i_square, this._transmat_result);
this._lost_delay_count = 0;
is_id_found = true;
}
else
{// 異なるコードの認識→今はサポートしない。
throw new NyARException();
}
}
return is_id_found;
}
/// <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));
}
}
}
/** * 自己コールバック関数です。 * 継承したクラスで、マーカ更新時の処理を実装してください。 * 引数の値の有効期間は、関数が終了するまでです。 * @param i_square * 現在のマーカ検出位置です。 * @param o_result * 現在の姿勢変換行列です。 */ protected abstract void onUpdateHandler(NyARSquare i_square, NyARDoubleMatrix44 o_result);
/**
* 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;
}
/**
* arGetLine(int x_coord[], int y_coord[], int coord_num,int vertex[], double line[4][3], double v[4][2]) arGetLine2(int x_coord[], int y_coord[], int
* coord_num,int vertex[], double line[4][3], double v[4][2], double *dist_factor) の２関数の合成品です。 マーカーのvertex,lineを計算して、結果をo_squareに保管します。
* Optimize:STEP[424->391]
*
* @param i_cparam
* @return
* @throws NyARException
*/
private bool getSquareLine(int[] i_mkvertex, int[] i_xcoord, int[] i_ycoord, NyARSquare o_square)
{
NyARLinear[] l_line = o_square.line;
NyARVec ev = this.__getSquareLine_ev; // matrixPCAの戻り値を受け取る
NyARVec mean = this.__getSquareLine_mean;// matrixPCAの戻り値を受け取る
double[] mean_array = mean.getArray();
NyARCameraDistortionFactor dist_factor = this._dist_factor_ref;
NyARMat input = this.__getSquareLine_input;// 次処理で初期化される。
NyARMat evec = this.__getSquareLine_evec;// アウトパラメータを受け取るから初期化不要//new NyARMat(2,2);
double[][] evec_array = evec.getArray();
double w1;
int st, ed, n, i;
NyARLinear l_line_i, l_line_2;
for (i = 0; i < 4; i++)
{
w1 = (double)(i_mkvertex[i + 1] - i_mkvertex[i] + 1) * 0.05 + 0.5;
st = (int)(i_mkvertex[i] + w1);
ed = (int)(i_mkvertex[i + 1] - w1);
n = ed - st + 1;
if (n < 2)
{
// nが2以下でmatrix.PCAを計算することはできないので、エラー
return false;
}
// pcaの準備
input.realloc(n, 2);
// バッチ取得
dist_factor.observ2IdealBatch(i_xcoord, i_ycoord, st, n, input.getArray());
// 主成分分析
input.matrixPCA(evec, ev, mean);
l_line_i = l_line[i];
l_line_i.run = evec_array[0][1];// line[i][0] = evec->m[1];
l_line_i.rise = -evec_array[0][0];// line[i][1] = -evec->m[0];
l_line_i.intercept = -(l_line_i.run * mean_array[0] + l_line_i.rise * mean_array[1]);// line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]);
}
NyARDoublePoint2d[] l_sqvertex = o_square.sqvertex;
NyARIntPoint[] l_imvertex = o_square.imvertex;
for (i = 0; i < 4; i++)
{
l_line_i = l_line[i];
l_line_2 = l_line[(i + 3) % 4];
w1 = l_line_2.run * l_line_i.rise - l_line_i.run * l_line_2.rise;
if (w1 == 0.0)
{
return false;
}
l_sqvertex[i].x = (l_line_2.rise * l_line_i.intercept - l_line_i.rise * l_line_2.intercept) / w1;
l_sqvertex[i].y = (l_line_i.run * l_line_2.intercept - l_line_2.run * l_line_i.intercept) / w1;
// 頂点インデクスから頂点座標を得て保存
l_imvertex[i].x = i_xcoord[i_mkvertex[i]];
l_imvertex[i].y = i_ycoord[i_mkvertex[i]];
}
return true;
}
/*
* (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;
}