public void copyFrom(NyARLinear i_source)
{
this.dx = i_source.dx;
this.dy = i_source.dy;
this.c = i_source.c;
return;
}
public void copyFrom(NyARLinear i_source)
{
this.dx = i_source.dx;
this.dy = i_source.dy;
this.c = i_source.c;
return;
}
public double c;//切片
public static NyARLinear[] createArray(int i_number)
{
NyARLinear[] ret = new NyARLinear[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARLinear();
}
return ret;
}
public double c; //切片
public static NyARLinear[] createArray(int i_number)
{
NyARLinear[] ret = new NyARLinear[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARLinear();
}
return(ret);
}
/**
* 2直線の交点を計算します。
* @param l_line_i
* @param l_line_2
* @param o_point
* @return
*/
public static bool crossPos(NyARLinear l_line_i, NyARLinear l_line_2, NyARDoublePoint2d o_point)
{
double w1 = l_line_2.dy * l_line_i.dx - l_line_i.dy * l_line_2.dx;
if (w1 == 0.0)
{
return false;
}
o_point.x = (l_line_2.dx * l_line_i.c - l_line_i.dx * l_line_2.c) / w1;
o_point.y = (l_line_i.dy * l_line_2.c - l_line_2.dy * l_line_i.c) / w1;
return true;
}
/**
* 2直線の交点を計算します。
* @param l_line_i
* @param l_line_2
* @param o_point
* @return
*/
public static bool crossPos(NyARLinear l_line_i, NyARLinear l_line_2, NyARDoublePoint2d o_point)
{
double w1 = l_line_2.dy * l_line_i.dx - l_line_i.dy * l_line_2.dx;
if (w1 == 0.0)
{
return(false);
}
o_point.x = (l_line_2.dx * l_line_i.c - l_line_i.dx * l_line_2.c) / w1;
o_point.y = (l_line_i.dy * l_line_2.c - l_line_2.dy * l_line_i.c) / w1;
return(true);
}
/**
* 矩形が見付かるたびに呼び出されます。
* 発見した矩形のパターンを検査して、方位を考慮した頂点データを確保します。
*/
public void onSquareDetect(NyARSquareContourDetector i_sender, int[] i_coordx, int[] i_coordy, int i_coor_num, int[] i_vertex_index)
{
NyARMatchPattResult mr = this.__detectMarkerLite_mr;
//輪郭座標から頂点リストに変換
NyARIntPoint2d[] vertex = this.__tmp_vertex;
vertex[0].x = i_coordx[i_vertex_index[0]];
vertex[0].y = i_coordy[i_vertex_index[0]];
vertex[1].x = i_coordx[i_vertex_index[1]];
vertex[1].y = i_coordy[i_vertex_index[1]];
vertex[2].x = i_coordx[i_vertex_index[2]];
vertex[2].y = i_coordy[i_vertex_index[2]];
vertex[3].x = i_coordx[i_vertex_index[3]];
vertex[3].y = i_coordy[i_vertex_index[3]];
//画像を取得
if (!this._inst_patt.pickFromRaster(this._ref_raster, vertex))
{
return;
}
//取得パターンをカラー差分データに変換して評価する。
this._deviation_data.setRaster(this._inst_patt);
if (!this._match_patt.evaluate(this._deviation_data, mr))
{
return;
}
//現在の一致率より低ければ終了
if (this.confidence > mr.confidence)
{
return;
}
//一致率の高い矩形があれば、方位を考慮して頂点情報を作成
NyARSquare sq = this.square;
this.confidence = mr.confidence;
//directionを考慮して、squareを更新する。
for (int i = 0; i < 4; i++)
{
int idx = (i + 4 - mr.direction) % 4;
this._coordline.coord2Line(i_vertex_index[idx], i_vertex_index[(idx + 1) % 4], i_coordx, i_coordy, i_coor_num, sq.line[i]);
}
for (int i = 0; i < 4; i++)
{
//直線同士の交点計算
if (!NyARLinear.crossPos(sq.line[i], sq.line[(i + 3) % 4], sq.sqvertex[i]))
{
throw new NyARException();//ここのエラー復帰するならダブルバッファにすればOK
}
}
}
/**
* 頂点データを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]);
}
}
/**
* この関数は、輪郭点集合からay+bx+c=0の直線式を計算します。
* @param i_st
* 直線計算の対象とする、輪郭点の開始インデックス
* @param i_ed
* 直線計算の対象とする、輪郭点の終了インデックス
* @param i_coord
* 輪郭点集合のオブジェクト。
* @param o_line
* 直線式を受け取るオブジェクト
* @return
* 直線式の計算に成功すると、trueを返します。
* @
*/
public bool coord2Line(int i_st, int i_ed, NyARIntCoordinates i_coord, NyARLinear o_line)
{
//頂点を取得
int n, st, ed;
double w1;
int cood_num = i_coord.length;
//探索区間の決定
if (i_ed >= i_st)
{
//頂点[i]から頂点[i+1]までの輪郭が、1区間にあるとき
w1 = (double)(i_ed - i_st + 1) * 0.05 + 0.5;
//探索区間の決定
st = (int)(i_st + w1);
ed = (int)(i_ed - w1);
}
else
{
//頂点[i]から頂点[i+1]までの輪郭が、2区間に分かれているとき
w1 = (double)((i_ed + cood_num - i_st + 1) % cood_num) * 0.05 + 0.5;
//探索区間の決定
st = ((int)(i_st + w1)) % cood_num;
ed = ((int)(i_ed + cood_num - w1)) % cood_num;
}
//探索区間数を確認
if (st <= ed)
{
//探索区間は1区間
n = ed - st + 1;
if (this._dist_factor != null)
{
this._dist_factor.observ2IdealBatch(i_coord.items, st, n, this._xpos, this._ypos, 0);
}
}
else
{
//探索区間は2区間
n = ed + 1 + cood_num - st;
if (this._dist_factor != null)
{
this._dist_factor.observ2IdealBatch(i_coord.items, st, cood_num - st, this._xpos, this._ypos, 0);
this._dist_factor.observ2IdealBatch(i_coord.items, 0, ed + 1, this._xpos, this._ypos, cood_num - st);
}
}
//要素数の確認
if (n < 2)
{
// nが2以下でmatrix.PCAを計算することはできないので、エラー
return(false);
}
//主成分分析する。
NyARDoubleMatrix22 evec = this.__getSquareLine_evec;
double[] mean = this.__getSquareLine_mean;
this._pca.pca(this._xpos, this._ypos, n, evec, this.__getSquareLine_ev, mean);
o_line.a = evec.m01; // line[i][0] = evec->m[1];
o_line.b = -evec.m00; // line[i][1] = -evec->m[0];
o_line.c = -(o_line.a * mean[0] + o_line.b * mean[1]); // line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]);
return(true);
}
/**
* 矩形が見付かるたびに呼び出されます。
* 発見した矩形のパターンを検査して、方位を考慮した頂点データを確保します。
*/
public void onSquareDetect(NyARSquareContourDetector i_sender, int[] i_coordx, int[] i_coordy, int i_coor_num, int[] i_vertex_index)
{
//既に発見済なら終了
if (this.marker_data != null)
{
return;
}
//輪郭座標から頂点リストに変換
NyARIntPoint2d[] vertex = this.__tmp_vertex;
vertex[0].x = i_coordx[i_vertex_index[0]];
vertex[0].y = i_coordy[i_vertex_index[0]];
vertex[1].x = i_coordx[i_vertex_index[1]];
vertex[1].y = i_coordy[i_vertex_index[1]];
vertex[2].x = i_coordx[i_vertex_index[2]];
vertex[2].y = i_coordy[i_vertex_index[2]];
vertex[3].x = i_coordx[i_vertex_index[3]];
vertex[3].y = i_coordy[i_vertex_index[3]];
NyIdMarkerParam param = this._marker_param;
NyIdMarkerPattern patt_data = this._marker_data;
// 評価基準になるパターンをイメージから切り出す
if (!this._id_pickup.pickFromRaster(this._ref_raster, vertex, patt_data, param))
{
return;
}
//エンコード
if (!this._encoder.encode(patt_data, this._data_temp))
{
return;
}
//継続認識要求されている？
if (this._prev_data == null)
{
//継続認識要求なし
this._current_data.copyFrom(this._data_temp);
}
else
{
//継続認識要求あり
if (!this._prev_data.isEqual((this._data_temp)))
{
return;//認識請求のあったIDと違う。
}
}
//新しく認識、または継続認識中に更新があったときだけ、Square情報を更新する。
//ココから先はこの条件でしか実行されない。
NyARSquare sq = this.square;
//directionを考慮して、squareを更新する。
for (int i = 0; i < 4; i++)
{
int idx = (i + 4 - param.direction) % 4;
this._coordline.coord2Line(i_vertex_index[idx], i_vertex_index[(idx + 1) % 4], i_coordx, i_coordy, i_coor_num, sq.line[i]);
}
for (int i = 0; i < 4; i++)
{
//直線同士の交点計算
if (!NyARLinear.crossPos(sq.line[i], sq.line[(i + 3) % 4], sq.sqvertex[i]))
{
throw new NyARException();//ここのエラー復帰するならダブルバッファにすればOK
}
}
this.threshold = param.threshold;
this.marker_data = this._current_data;//みつかった。
}
/**
* この関数は、輪郭点集合からay+bx+c=0の直線式を計算します。
* @param i_st
* 直線計算の対象とする、輪郭点の開始インデックス
* @param i_ed
* 直線計算の対象とする、輪郭点の終了インデックス
* @param i_coord
* 輪郭点集合のオブジェクト。
* @param o_line
* 直線式を受け取るオブジェクト
* @return
* 直線式の計算に成功すると、trueを返します。
* @
*/
public bool coord2Line(int i_st, int i_ed, NyARIntCoordinates i_coord, NyARLinear o_line)
{
//頂点を取得
int n, st, ed;
double w1;
int cood_num = i_coord.length;
//探索区間の決定
if (i_ed >= i_st)
{
//頂点[i]から頂点[i+1]までの輪郭が、1区間にあるとき
w1 = (double)(i_ed - i_st + 1) * 0.05 + 0.5;
//探索区間の決定
st = (int)(i_st + w1);
ed = (int)(i_ed - w1);
}
else
{
//頂点[i]から頂点[i+1]までの輪郭が、2区間に分かれているとき
w1 = (double)((i_ed + cood_num - i_st + 1) % cood_num) * 0.05 + 0.5;
//探索区間の決定
st = ((int)(i_st + w1)) % cood_num;
ed = ((int)(i_ed + cood_num - w1)) % cood_num;
}
//探索区間数を確認
if (st <= ed)
{
//探索区間は1区間
n = ed - st + 1;
if (this._dist_factor != null)
{
this._dist_factor.observ2IdealBatch(i_coord.items, st, n, this._xpos, this._ypos, 0);
}
}
else
{
//探索区間は2区間
n = ed + 1 + cood_num - st;
if (this._dist_factor != null)
{
this._dist_factor.observ2IdealBatch(i_coord.items, st, cood_num - st, this._xpos, this._ypos, 0);
this._dist_factor.observ2IdealBatch(i_coord.items, 0, ed + 1, this._xpos, this._ypos, cood_num - st);
}
}
//要素数の確認
if (n < 2)
{
// nが2以下でmatrix.PCAを計算することはできないので、エラー
return false;
}
//主成分分析する。
NyARDoubleMatrix22 evec = this.__getSquareLine_evec;
double[] mean = this.__getSquareLine_mean;
this._pca.pca(this._xpos, this._ypos, n, evec, this.__getSquareLine_ev, mean);
o_line.a = evec.m01;// line[i][0] = evec->m[1];
o_line.b = -evec.m00;// line[i][1] = -evec->m[0];
o_line.c = -(o_line.a * mean[0] + o_line.b * mean[1]);// line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]);
return true;
}
/**
* 矩形が見付かるたびに呼び出されます。
* 発見した矩形のパターンを検査して、方位を考慮した頂点データを確保します。
*/
public void onSquareDetect(NyARSquareContourDetector i_sender, int[] i_coordx, int[] i_coordy, int i_coor_num, int[] i_vertex_index)
{
if (this._match_patt == null)
{
return;
}
//輪郭座標から頂点リストに変換
NyARIntPoint2d[] vertex = this.__tmp_vertex;
vertex[0].x = i_coordx[i_vertex_index[0]];
vertex[0].y = i_coordy[i_vertex_index[0]];
vertex[1].x = i_coordx[i_vertex_index[1]];
vertex[1].y = i_coordy[i_vertex_index[1]];
vertex[2].x = i_coordx[i_vertex_index[2]];
vertex[2].y = i_coordy[i_vertex_index[2]];
vertex[3].x = i_coordx[i_vertex_index[3]];
vertex[3].y = i_coordy[i_vertex_index[3]];
//画像を取得
if (!this._inst_patt.pickFromRaster(this._ref_raster, vertex))
{
return;//取得失敗
}
//取得パターンをカラー差分データに変換して評価する。
this._deviation_data.setRaster(this._inst_patt);
//code_index,dir,c1にデータを得る。
NyARMatchPattResult mr = this.__detectMarkerLite_mr;
int lcode_index = 0;
int dir = 0;
double c1 = 0;
for (int i = 0; i < this._match_patt.Length; i++)
{
this._match_patt[i].evaluate(this._deviation_data, mr);
double c2 = mr.confidence;
if (c1 < c2)
{
lcode_index = i;
c1 = c2;
dir = mr.direction;
}
}
//認識処理
if (this._target_id == -1)
{ // マーカ未認識
//現在は未認識
if (c1 < this.cf_threshold_new)
{
return;
}
if (this.confidence > c1)
{
// 一致度が低い。
return;
}
//認識しているマーカIDを保存
this.code_index = lcode_index;
}
else
{
//現在はマーカ認識中
// 現在のマーカを認識したか？
if (lcode_index != this._target_id)
{
// 認識中のマーカではないので無視
return;
}
//認識中の閾値より大きいか？
if (c1 < this.cf_threshold_exist)
{
return;
}
//現在の候補よりも一致度は大きいか？
if (this.confidence > c1)
{
return;
}
this.code_index = this._target_id;
}
//新しく認識、または継続認識中に更新があったときだけ、Square情報を更新する。
//ココから先はこの条件でしか実行されない。
//一致率の高い矩形があれば、方位を考慮して頂点情報を作成
this.confidence = c1;
NyARSquare sq = this.square;
//directionを考慮して、squareを更新する。
for (int i = 0; i < 4; i++)
{
int idx = (i + 4 - dir) % 4;
this._coordline.coord2Line(i_vertex_index[idx], i_vertex_index[(idx + 1) % 4], i_coordx, i_coordy, i_coor_num, sq.line[i]);
}
for (int i = 0; i < 4; i++)
{
//直線同士の交点計算
if (!NyARLinear.crossPos(sq.line[i], sq.line[(i + 3) % 4], sq.sqvertex[i]))
{
throw new NyARException();//ここのエラー復帰するならダブルバッファにすればOK
}
}
}
/// <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));
}
}
}
/**
* 矩形が見付かるたびに呼び出されます。
* 発見した矩形のパターンを検査して、方位を考慮した頂点データを確保します。
*/
public void onSquareDetect(NyARSquareContourDetector i_sender, int[] i_coordx, int[] i_coordy, int i_coor_num, int[] i_vertex_index)
{
NyARMatchPattResult mr = this.__detectMarkerLite_mr;
//輪郭座標から頂点リストに変換
NyARIntPoint2d[] vertex = this.__tmp_vertex;
vertex[0].x = i_coordx[i_vertex_index[0]];
vertex[0].y = i_coordy[i_vertex_index[0]];
vertex[1].x = i_coordx[i_vertex_index[1]];
vertex[1].y = i_coordy[i_vertex_index[1]];
vertex[2].x = i_coordx[i_vertex_index[2]];
vertex[2].y = i_coordy[i_vertex_index[2]];
vertex[3].x = i_coordx[i_vertex_index[3]];
vertex[3].y = i_coordy[i_vertex_index[3]];
//画像を取得
if (!this._inst_patt.pickFromRaster(this._ref_raster, vertex))
{
return;
}
//取得パターンをカラー差分データに変換して評価する。
this._deviation_data.setRaster(this._inst_patt);
//最も一致するパターンを割り当てる。
int square_index, direction;
double confidence;
this._match_patt[0].evaluate(this._deviation_data, mr);
square_index = 0;
direction = mr.direction;
confidence = mr.confidence;
//2番目以降
for (int i = 1; i < this._match_patt.Length; i++)
{
this._match_patt[i].evaluate(this._deviation_data, mr);
if (confidence > mr.confidence)
{
continue;
}
// もっと一致するマーカーがあったぽい
square_index = i;
direction = mr.direction;
confidence = mr.confidence;
}
//最も一致したマーカ情報を、この矩形の情報として記録する。
NyARDetectMarkerResult result = this.result_stack.prePush();
result.arcode_id = square_index;
result.confidence = confidence;
NyARSquare sq = result.square;
//directionを考慮して、squareを更新する。
for (int i = 0; i < 4; i++)
{
int idx = (i + 4 - direction) % 4;
this._coordline.coord2Line(i_vertex_index[idx], i_vertex_index[(idx + 1) % 4], i_coordx, i_coordy, i_coor_num, sq.line[i]);
}
for (int i = 0; i < 4; i++)
{
//直線同士の交点計算
if (!NyARLinear.crossPos(sq.line[i], sq.line[(i + 3) % 4], sq.sqvertex[i]))
{
throw new NyARException();//ここのエラー復帰するならダブルバッファにすればOK
}
}
}
/**
* 配列の前方に、似たベクトルを集めます。似たベクトルの判定基準は、2線の定義点における直線の法線上での距離の二乗和です。
* ベクトルの統合と位置情報の計算には、加重平均を用います。
* @param i_vector
* 編集するオブジェクトを指定します。
*/
public void MargeResembleCoords(VecLinearCoordinates i_vector)
{
VecLinearCoordinates.VecLinearCoordinatePoint[] items = i_vector.items;
NyARLinear l1 = this._l1;
NyARLinear l2 = this._l2;
NyARDoublePoint2d p = this._p;
for (int i = i_vector.length - 1; i >= 0; i--)
{
VecLinearCoordinates.VecLinearCoordinatePoint target1 = items[i];
if (target1.scalar == 0)
{
continue;
}
double rdx = target1.dx;
double rdy = target1.dy;
double rx = target1.x;
double ry = target1.y;
l1.setVector(target1);
double s_tmp = target1.scalar;
target1.dx *= s_tmp;
target1.dy *= s_tmp;
target1.x *= s_tmp;
target1.y *= s_tmp;
for (int i2 = i - 1; i2 >= 0; i2--)
{
VecLinearCoordinates.VecLinearCoordinatePoint target2 = items[i2];
if (target2.scalar == 0)
{
continue;
}
if (target2.getVecCos(rdx, rdy) >= _SQ_ANG_TH)
{
// それぞれの代表点から法線を引いて、相手の直線との交点を計算する。
l2.setVector(target2);
l1.normalLineCrossPos(rx, ry, l2, p);
double wx, wy;
double l = 0;
// 交点間の距離の合計を計算。lに2*dist^2を得る。
wx = (p.x - rx);
wy = (p.y - ry);
l += wx * wx + wy * wy;
l2.normalLineCrossPos(target2.x, target2.y, l2, p);
wx = (p.x - target2.x);
wy = (p.y - target2.y);
l += wx * wx + wy * wy;
// 距離が一定値以下なら、マージ
if (l > _SQ_DIFF_DOT_TH)
{
continue;
}
// 似たようなベクトル発見したら、後方のアイテムに値を統合。
s_tmp = target2.scalar;
target1.x += target2.x * s_tmp;
target1.y += target2.y * s_tmp;
target1.dx += target2.dx * s_tmp;
target1.dy += target2.dy * s_tmp;
target1.scalar += s_tmp;
//要らない子を無効化しておく。
target2.scalar = 0;
}
}
}
//前方詰め
i_vector.removeZeroDistItem();
//加重平均解除なう(x,y位置のみ)
for (int i = 0; i < i_vector.length; i++)
{
VecLinearCoordinates.VecLinearCoordinatePoint ptr = items[i];
double d = 1 / ptr.scalar;
ptr.x *= d;
ptr.y *= d;
ptr.dx *= d;
ptr.dy *= d;
}
}
