private void CameraZm_NewVideoSample(object sender, WPFMediaKit.DirectShow.MediaPlayers.VideoSampleArgs e)
{
Dispatcher.Invoke(new Action(delegate()
{
System.Drawing.Bitmap oNewFrame = e.VideoFrame;
//byte[] oFrameMomeryStream = this.ConvertBitmapToBytes(oNewFrame);
//IntPtr ptr = oNewFrame.GetHbitmap();
this.MyArRaster.wrapBuffer(oNewFrame);
if (this.MySingleDetectMarker.detectMarkerLite(this.MyArRaster, 127))
{
this.CvMainZm.Children.Clear();
NyARDoubleMatrix44 oResultMat = new NyARDoubleMatrix44();
this.MySingleDetectMarker.getTransmationMatrix(oResultMat);
NyARSquare square = this.MySingleDetectMarker.refSquare();
Polygon oPolygon = new Polygon()
{
SnapsToDevicePixels = true,
Fill = new SolidColorBrush(Colors.Violet),
Opacity = 0.8,
Stroke = new SolidColorBrush(Colors.Red)
};
oPolygon.Points = new PointCollection(new Point[] {
new Point(square.sqvertex[0].x, 600 - square.sqvertex[0].y),
new Point(square.sqvertex[1].x, 600 - square.sqvertex[1].y),
new Point(square.sqvertex[2].x, 600 - square.sqvertex[2].y),
new Point(square.sqvertex[3].x, 600 - square.sqvertex[3].y)
});
this.CvMainZm.Children.Add(oPolygon);
}
}));
}
/**
* double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )
*
* @param i_square
* 計算対象のNyARSquareオブジェクト
* @param i_width
* @return
* @throws NyARException
*/
public void transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
{
NyARDoublePoint3d trans = this.__transMat_trans;
//平行移動量計算機に、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);
//回転行列を計算
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);
//計算結果の最適化(平行移動量と回転行列の最適化)
double err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d);
// マトリクスの保存
o_result_conv.setValue(this._rotmatrix, trans, err);
return;
}
/**
* Make tansform matrix by ICP algorism.
* i_prev_result parameter is not effective. should set 0.
*/
public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 i_prev_result, double i_prev_err,
NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param)
{
if(this._icpp.icpPoint(i_square.sqvertex,i_offset.vertex,4,o_result,o_result,o_param)){
return true;
}
return false;
}
/**オブジェクトのステータスを更新し、必要に応じて自己コールバック関数を駆動します。
*/
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);
}
/**
* Make tansform matrix by ICP algorism.
*/
public bool transMat(NyARSquare i_square, NyARRectOffset i_offset,NyARDoubleMatrix44 i_result,NyARTransMatResultParam o_param)
{
if(this._icpc.icpGetInitXw2Xc_from_PlanarData(i_square.sqvertex,i_offset.vertex,4,i_result)){
if(this._icpp.icpPoint(i_square.sqvertex,i_offset.vertex,4,i_result,i_result,o_param)){
return true;
}
}
return false;
}
/**
* Make tansform matrix by ICP algorism.
* i_prev_result parameter is not effective. should set 0.
*/
public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 i_prev_result, double i_prev_err,
NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param)
{
if (this._icpp.icpPoint(i_square.sqvertex, i_offset.vertex, 4, o_result, o_result, o_param))
{
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();
}
}
}
/**
* Make tansform matrix by ICP algorism.
*/
public bool transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 i_result, NyARTransMatResultParam o_param)
{
if (this._icpc.icpGetInitXw2Xc_from_PlanarData(i_square.sqvertex, i_offset.vertex, 4, i_result))
{
if (this._icpp.icpPoint(i_square.sqvertex, i_offset.vertex, 4, i_result, i_result, o_param))
{
return(true);
}
}
return(false);
}
/** オブジェクトのステータスを更新し、必要に応じてハンドル関数を駆動します。
* 戻り値は、「実際にマーカを発見する事ができたか」です。クラスの状態とは異なります。
*/
private bool updateStatus(NyARSquare i_square, int i_code_index)
{
NyARTransMatResult result = this.__NyARSquare_result;
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, result);
// OnUpdate
this.onUpdateHandler(i_square, 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の再認識
// イベント生成
// 変換行列を作成
this._transmat.transMatContinue(i_square, this._offset, result);
// OnUpdate
this.onUpdateHandler(i_square, result);
this._lost_delay_count = 0;
return(true);
}
else
{// 異なるコードの認識→今はサポートしない。
throw new NyARException();
}
}
}
/**
* 内部関数です。
* この関数は、thisの二次元矩形情報プロパティを更新します。
* @param i_coord
* @param i_vertex_index
* @
*/
protected internal void updateSquareInfo(NyARIntCoordinates i_coord, int[] i_vertex_index)
{
NyARMatchPattResult mr = this.__detectMarkerLite_mr;
//輪郭座標から頂点リストに変換
NyARIntPoint2d[] vertex = this.__ref_vertex; //C言語ならポインタ扱いで実装
vertex[0] = i_coord.items[i_vertex_index[0]];
vertex[1] = i_coord.items[i_vertex_index[1]];
vertex[2] = i_coord.items[i_vertex_index[2]];
vertex[3] = i_coord.items[i_vertex_index[3]];
//画像を取得
if (!this._inst_patt.pickFromRaster(this._last_input_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_coord, sq.line[i]);
}
//ちょっと、ひっくり返してみようか。
for (int i = 0; i < 4; i++)
{
//直線同士の交点計算
if (!sq.line[i].crossPos(sq.line[(i + 3) % 4], sq.sqvertex[i]))
{
throw new NyARException();//ここのエラー復帰するならダブルバッファにすればOK
}
}
}
/*
* (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;
}
//平行移動量計算機に、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 err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d);
// マトリクスの保存
this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
// エラー値が許容範囲でなければTransMatをやり直し
if (err > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR)
{
// rotationを矩形情報で初期化
this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);
//回転行列の平行移動量の計算
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(this._rotmatrix,trans)
double err2 = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d);
//エラー値が低かったら値を差換え
if (err2 < err)
{
// 良い値が取れたら、差換え
this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
}
err = err2;
}
//エラー値保存
o_result_conv.error = err;
return;
}
/**
* 矩形が見付かるたびに呼び出されます。
* 発見した矩形のパターンを検査して、方位を考慮した頂点データを確保します。
*/
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
}
}
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 計算に過去の履歴を使う点が、{@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;
//平行移動量計算機に、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);
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
//回転行列を計算
this._rotmatrix.initRotByPrevResult(i_prev_result);
//回転後の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);
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans);
// エラー値が許容範囲でなければTransMatをやり直し
if (err > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR)
{
return(false);
}
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans);
//エラー値保存
if (o_param != null)
{
o_param.last_error = err;
}
return(true);
}
/**
* 頂点データを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]);
}
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 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;
//平行移動量計算機に、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);
//回転行列を計算
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);
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans);
if (o_param != null)
{
o_param.last_error = err;
}
return(true);
}
/**
* double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )
*
* @param i_square
* 計算対象のNyARSquareオブジェクト
* @param i_width
* @return
* @throws NyARException
*/
public void transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult o_result_conv)
{
NyARDoublePoint3d trans = this.__transMat_trans;
//平行移動量計算機に、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);
// マトリクスの保存
this.updateMatrixValue(this._rotmatrix, trans, o_result_conv);
return;
}
/**
* 矩形が見付かるたびに呼び出されます。
* 発見した矩形のパターンを検査して、方位を考慮した頂点データを確保します。
*/
public void detectMarkerCallback(NyARIntCoordinates i_coord, int[] i_vertex_index)
{
NyARMatchPattResult mr = this.__detectMarkerLite_mr;
//輪郭座標から頂点リストに変換
NyARIntPoint2d[] vertex = this.__ref_vertex;
vertex[0] = i_coord.items[i_vertex_index[0]];
vertex[1] = i_coord.items[i_vertex_index[1]];
vertex[2] = i_coord.items[i_vertex_index[2]];
vertex[3] = i_coord.items[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_coord, sq.line[i]);
}
for (int i = 0; i < 4; i++)
{
//直線同士の交点計算
if (!sq.line[i].crossPos(sq.line[(i + 3) % 4], sq.sqvertex[i]))
{
throw new NyARException();//ここのエラー復帰するならダブルバッファにすればOK
}
}
}
void CaptureListener.OnBuffer(CaptureDevice i_sender, double i_sample_time, IntPtr i_buffer, int i_buffer_len)
{
// calculate size of the frame bitmap
int w = i_sender.video_width;
int h = i_sender.video_height;
int s = w * (i_sender.video_bit_count / 8); // stride
AForge.Imaging.Filters.FiltersSequence seq = new AForge.Imaging.Filters.FiltersSequence();
seq.Add(new AForge.Imaging.Filters.Grayscale(0.2125, 0.7154, 0.0721));
seq.Add(new AForge.Imaging.Filters.Threshold(127));
seq.Add(new AForge.Imaging.Filters.GrayscaleToRGB());
AForge.Imaging.UnmanagedImage srcImg = new AForge.Imaging.UnmanagedImage(i_buffer, w, h, s, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
AForge.Imaging.UnmanagedImage outputImg = seq.Apply(srcImg);
byte[] destArr = new byte[outputImg.Stride * outputImg.Height];
System.Runtime.InteropServices.Marshal.Copy(outputImg.ImageData, destArr, 0, outputImg.Stride * outputImg.Height);
this.m_raster.wrapBuffer(destArr);
try
{
int detectedMkrs = this.m_ar.detectMarkerLite(this.m_raster, m_threshold);
NyARSquare square = null;
if (detectedMkrs > 0)
{
NyARTransMatResult transMat = new NyARTransMatResult();
NyARDoublePoint2d[] points = m_ar.getCorners(0); // RichF added this method
square = new NyARSquare();
square.sqvertex = points;
}
Dispatcher.BeginInvoke(new Action(delegate()
{
TransformedBitmap b = new TransformedBitmap();
b.BeginInit();
b.Source = BitmapSource.Create(w, h, dpiX, dpiY, PixelFormats.Bgr32, BitmapPalettes.WebPalette, i_buffer, i_buffer_len, s);
b.SetValue(TransformedBitmap.TransformProperty, new ScaleTransform(-1, -1));
b.EndInit();
image1.SetValue(Image.SourceProperty, b);
if (square != null)
{
recognizedTag.Points = new PointCollection(new Point[] {
new Point(cameraResX - square.sqvertex[0].x, cameraResY - square.sqvertex[0].y),
new Point(cameraResX - square.sqvertex[1].x, cameraResY - square.sqvertex[1].y),
new Point(cameraResX - square.sqvertex[2].x, cameraResY - square.sqvertex[2].y),
new Point(cameraResX - square.sqvertex[3].x, cameraResY - square.sqvertex[3].y)
});
recognizedTag.Visibility = System.Windows.Visibility.Visible;
}
else
{
recognizedTag.Visibility = System.Windows.Visibility.Hidden;
}
}), null);
}
catch
{
}
}
/**
* アプリケーションフレームワークのハンドラ（マーカ更新）
*/
protected override void onUpdateHandler(NyARSquare i_square, NyARDoubleMatrix44 result)
{
this.transmat = result;
}
/** * 自己コールバック関数です。 * 継承したクラスで、マーカ更新時の処理を実装してください。 * 引数の値の有効期間は、関数が終了するまでです。 * @param i_square * 現在のマーカ検出位置です。 * @param o_result * 現在の姿勢変換行列です。 */ protected abstract void onUpdateHandler(NyARSquare i_square, NyARDoubleMatrix44 o_result);
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 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;
//平行移動量計算機に、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);
//回転行列を計算
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);
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans);
if (o_param != null)
{
o_param.last_error = err;
}
return true;
}
/**
* 矩形が見付かるたびに呼び出されます。
* 発見した矩形のパターンを検査して、方位を考慮した頂点データを確保します。
*/
public void detectMarkerCallback(NyARIntCoordinates i_coord, int[] i_vertex_index)
{
if (this._match_patt == null)
{
return;
}
//輪郭座標から頂点リストに変換
NyARIntPoint2d[] vertex = this.__ref_vertex;
vertex[0] = i_coord.items[i_vertex_index[0]];
vertex[1] = i_coord.items[i_vertex_index[1]];
vertex[2] = i_coord.items[i_vertex_index[2]];
vertex[3] = i_coord.items[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_coord, sq.line[i]);
}
for (int i = 0; i < 4; i++)
{
//直線同士の交点計算
if (!sq.line[i].crossPos(sq.line[(i + 3) % 4], sq.sqvertex[i]))
{
throw new NyARException();//ここのエラー復帰するならダブルバッファにすればOK
}
}
}
/**
* この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。
* 計算に過去の履歴を使う点が、{@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;
//平行移動量計算機に、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);
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
//回転行列を計算
this._rotmatrix.initRotByPrevResult(i_prev_result);
//回転後の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);
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans);
// エラー値が許容範囲でなければTransMatをやり直し
if (err > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR)
{
return false;
}
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans);
//エラー値保存
if (o_param != null)
{
o_param.last_error = err;
}
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;//みつかった。
}
protected abstract void onUpdateHandler(NyARSquare i_square, NyARTransMatResult 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, NyARTransMatResult i_prev_result, NyARTransMatResult o_result)
{
NyARDoublePoint3d trans = this.__transMat_trans;
// i_prev_resultが初期値なら、transMatで計算する。
if (!i_prev_result.has_value)
{
this.transMat(i_square, i_offset, o_result);
return true;
}
//平行移動量計算機に、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);
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
//回転行列を計算
this._rotmatrix.initRotByPrevResult(i_prev_result);
//回転後の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);
// マトリクスの保存
o_result.setValue(this._rotmatrix, trans, err);
// エラー値が許容範囲でなければTransMatをやり直し
if (err > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR)
{
// rotationを矩形情報で初期化
this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex);
//回転行列の平行移動量の計算
this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4);
this._transsolver.solveTransportVector(vertex_3d, trans);
//計算結果の最適化(this._rotmatrix,trans)
double err2 = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d);
//エラー値が低かったら値を差換え
if (err2 < err)
{
// 良い値が取れたら、差換え
o_result.setValue(this._rotmatrix, trans, err2);
}
err = err2;
}
//エラー値保存
return true;
}
/// <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));
}
}
}