/**
* 画素の一致度を返す。NULLピクセルが無いことを前提に比較
* @param i_buf
* @param i_stride
* @param mtemp
* @param sx
* @param sy
* @return
* @throws NyARException
*/
private static int ar2GetBestMatchingSubFineFull(int[] i_buf, int i_stride, NyARTemplatePatchImage mtemp, int sx, int sy)
{
int[] tmp_buf = mtemp.img;
int sum2 = 0;
int sum1 = 0;
int sum3 = 0;
int t_ptr = 0;
int s_ptr = ((sy - (mtemp.yts * NyARTemplatePatchImage.AR2_TEMP_SCALE)) * i_stride + sx - (mtemp.xts * NyARTemplatePatchImage.AR2_TEMP_SCALE));
int padding = NyARTemplatePatchImage.AR2_TEMP_SCALE * (i_stride - mtemp.xsize);
for (int j = mtemp.ysize - 1; j >= 0; j--)
{
int i = mtemp.xsize - 1;
for (; i >= 4; i -= 4)
{
int sn1 = i_buf[s_ptr]; // w = *(p2+0);// + *(p2+1) + *(p2+2);
int sn2 = i_buf[s_ptr + NyARTemplatePatchImage.AR2_TEMP_SCALE]; // w = *(p2+0);// + *(p2+1) + *(p2+2);
int sn3 = i_buf[s_ptr + NyARTemplatePatchImage.AR2_TEMP_SCALE * 2]; // w = *(p2+0);// + *(p2+1) + *(p2+2);
int sn4 = i_buf[s_ptr + NyARTemplatePatchImage.AR2_TEMP_SCALE * 3]; // w = *(p2+0);// + *(p2+1) + *(p2+2);
sum2 += sn1 * sn1 + sn2 * sn2 + sn3 * sn3 + sn4 * sn4;
sum1 += sn1 + sn2 + sn3 + sn4;
sum3 += tmp_buf[t_ptr] * sn1 + tmp_buf[t_ptr + 1] * sn2 + tmp_buf[t_ptr + 2] * sn3 + tmp_buf[t_ptr + 3] * sn4;
s_ptr += NyARTemplatePatchImage.AR2_TEMP_SCALE * 4;
t_ptr += 4;
}
for (; i >= 0; i--)
{
int sn = i_buf[s_ptr];// w = *(p2+0);// + *(p2+1) + *(p2+2);
sum2 += sn * sn;
sum1 += sn;
sum3 += tmp_buf[t_ptr] * sn;
s_ptr += NyARTemplatePatchImage.AR2_TEMP_SCALE;
t_ptr++;
}
s_ptr += padding;
}
sum3 -= sum1 * mtemp.sum_of_img / mtemp.valid_pixels;
int vlen = sum2 - sum1 * sum1 / mtemp.valid_pixels;
if (vlen == 0)
{
return(0);
}
else
{
return(sum3 * 100 / mtemp.vlen * 100 / (int)Math.Sqrt(vlen));
}
}
/**
*
* @param mtemp
* @param sx
* @param sy
* @param 評価点。エラーの場合0
* @return
* @throws NyARException
*/
private static int ar2GetBestMatchingSubFine(INyARGrayscaleRaster i_raster, NyARTemplatePatchImage mtemp, int sx, int sy)
{
System.Console.WriteLine("This function is not tested! Check accury of result before using.");
int[] tmp_buf = mtemp.img;
int sum2 = 0;
int sum1 = 0;
int sum3 = 0;
int t_ptr = 0;
for (int j = mtemp.ysize - 1; j >= 0; j--)
{
int i = mtemp.xsize - 1;
for (; i >= 0; i--)
{
int tn = tmp_buf[t_ptr];
if (tn != NyARTemplatePatchImage.AR2_TEMPLATE_NULL_PIXEL)
{
int sn = i_raster.getPixel(
(sx + (i - mtemp.xts) * NyARTemplatePatchImage.AR2_TEMP_SCALE),
(sy + (j - mtemp.yts) * NyARTemplatePatchImage.AR2_TEMP_SCALE));
sum2 += sn * sn;
sum1 += sn;
sum3 += tn * sn;
}
t_ptr++;
}
}
sum3 -= sum1 * mtemp.sum_of_img / mtemp.valid_pixels;
int vlen = sum2 - sum1 * sum1 / mtemp.valid_pixels;
if (vlen == 0)
{
return(0);
}
else
{
return(sum3 * 100 / mtemp.vlen * 100 / (int)Math.Sqrt(vlen));
}
}
/**
* N個の基準点から、最もテンプレートに一致した座標を返却する。
* 検索範囲は、{@link #setSearchArea}で与えたpx,pyについて、xn+i_px>=xn>=xn-i_px,yn+i_py>=yn>=yn-i_pyの矩形範囲。
* i_pointsそれぞれについて検索する。
* @param i_template
* 探索範囲。単三区店を中心に、
* @param ry
* @param i_points
* 検索する座標セット。(近い場所の場合に、同一条件の探索をキャンセルできる？)
* @param o_obs_point
* 観察座標系での一致点。returnが0の場合は無効。
* @return
* 一致率(値範囲調査中)
* 0の場合は一致せず。
* @throws NyARException
*/
public double ar2GetBestMatching(NyARTemplatePatchImage i_template, NyARIntPoint2d[] i_points, int i_number_of_point,
NyARDoublePoint2d o_obs_point)
{
//最大テンプレートサイズの制限
Debug.Assert(i_template.xsize * i_template.ysize < 100 * 100);
//NULLピクセルを持つテンプレートか判定する。
bool is_full_template = i_template.xsize * i_template.ysize == i_template.valid_pixels;
NyARIntSize s = this._i_ref_raster.getSize();
int yts = i_template.yts;
int xts = i_template.xts;
int[] sbuf = (int[])this._i_ref_raster.getBuffer();
//パッチの探索
int ret = 1;
int sw = this._search_area.x;
int sh = this._search_area.y;
//パッチエリアの初期化
for (int ii = i_number_of_point - 1; ii >= 0; ii--)
{
if (i_points[ii].y < 0)
{
break;
}
// 検索するパッチ中心を決定
int px = (i_points[ii].x / (SKIP_INTERVAL + 1)) * (SKIP_INTERVAL + 1) + (SKIP_INTERVAL + 1) / 2;
int py = (i_points[ii].y / (SKIP_INTERVAL + 1)) * (SKIP_INTERVAL + 1) + (SKIP_INTERVAL + 1) / 2;
//検索範囲を画面内に制限
int search_left = px - sw;
if (search_left < 0)
{
search_left = 0;
}
int search_right = px + sw;
// if( ex >= xsize ) ex = xsize-1;
if (search_right >= s.w)
{
search_right = s.w - 1;
}
int search_top = py - sh;
if (search_top < 0)
{
search_top = 0;
}
int search_bottom = py + sh;
if (search_bottom >= s.h)
{
search_bottom = s.h - 1;
}
//利用するパッチエリアの初期化
initWorkArea(search_left, search_top, search_right, search_bottom);
}
MatchingCandidateList ml = this.__ml;
ml.init();
for (int ii = i_number_of_point - 1; ii >= 0; ii--)
{
if (i_points[ii].x < 0)
{
// if( ret ){
if (ret != 0)
{
return(-1);
}
else
{
break;
}
}
int px = (i_points[ii].x / (SKIP_INTERVAL + 1)) * (SKIP_INTERVAL + 1) + (SKIP_INTERVAL + 1) / 2;
int py = (i_points[ii].y / (SKIP_INTERVAL + 1)) * (SKIP_INTERVAL + 1) + (SKIP_INTERVAL + 1) / 2;
for (int j = py - sh; j <= py + sh; j += SKIP_INTERVAL + 1)
{
if (j - yts * NyARTemplatePatchImage.AR2_TEMP_SCALE < 0)
{
continue;
}
// if( j + yts2*AR2_TEMP_SCALE >= ysize ){
if (j + yts * NyARTemplatePatchImage.AR2_TEMP_SCALE >= s.h)
{
break;
}
for (int i = px - sw; i <= px + sw; i += SKIP_INTERVAL + 1)
{
if (i - xts * NyARTemplatePatchImage.AR2_TEMP_SCALE < 0)
{
continue;
}
// if( i + mtemp.xts2*AR2_TEMP_SCALE >= xsize ){
if (i + xts * NyARTemplatePatchImage.AR2_TEMP_SCALE >= s.w)
{
break;
}
// 既に検出済のエリア？
if (this._mbuf[i + j * s.w] != 0)
{
// mfImage[j*xsize+i] ){
continue;
}
this._mbuf[i + j * s.w] = 1;//ii番目のパッチで検索済みをマーク
int wval = is_full_template ? ar2GetBestMatchingSubFineFull(sbuf, s.w, i_template, i, j) : ar2GetBestMatchingSubFine(sbuf, s.w, i_template, i, j);
if (wval <= 0)
{
continue;
}
//ログへ追加
ml.tryToAdd(i, j, wval);
ret = 0;
}
}
}
double ret_sim = 0;
//一番スコアの良いパッチを得る
int wval2 = 0;
ret = -1;
for (int l = ml.num_of_item - 1; l >= 0; l--)
{
for (int j = ml.items[l].y - SKIP_INTERVAL; j <= ml.items[l].y + SKIP_INTERVAL; j++)
{
if (j - i_template.yts * NyARTemplatePatchImage.AR2_TEMP_SCALE < 0)
{
continue;
}
// if( j+mtemp.yts2*AR2_TEMP_SCALE >= ysize ){
if (j + i_template.yts * NyARTemplatePatchImage.AR2_TEMP_SCALE >= s.h)
{
break;
}
for (int i = ml.items[l].x - SKIP_INTERVAL; i <= ml.items[l].x + SKIP_INTERVAL; i++)
{
if (i - xts * NyARTemplatePatchImage.AR2_TEMP_SCALE < 0)
{
continue;
}
// if( i+mtemp.xts2*AR2_TEMP_SCALE >= xsize ){
if (i + xts * NyARTemplatePatchImage.AR2_TEMP_SCALE >= s.w)
{
break;
}
int wval = is_full_template ? ar2GetBestMatchingSubFineFull(sbuf, s.w, i_template, i, j) : ar2GetBestMatchingSubFine(sbuf, s.w, i_template, i, j);
if (wval <= 0)
{
continue;
}
if (wval > wval2)
{
o_obs_point.x = i;
o_obs_point.y = j;
wval2 = wval;
ret_sim = (double)wval / 10000;
ret = 0;
}
}
}
}
return(ret_sim);
}
/**
* i_rasiterの画像から、i_surfaceにマッチするパターンを検出して、その理想座標と３次元座標セットを返す。
* 検出した頂点セットは、o_pos2dとo_pos3dへ最大i_num個出力する。
* @param i_raster
* 現在の画像
* @param i_surface
* 検出すべきサーフェイスセット
* @param i_trans
* 現在の姿勢変換行列
* @param o_pos2d
* 出力パラメータ。画面上の理想点。
* オブジェクトの配列を指定すること。
* @param o_pos3d
* 出力パラメータ。三次元サーフェイス座標。
* オブジェクトの配列を指定すること。
* @param i_num
* 返却数。この数値は、コンストラクタに与えた最大数以下である必要がある。o_pos2dとo_pos3dは、この数値より大きい配列でなければならない。
* @return
* 検出した頂点セットの数。
* @throws NyARException
*/
public int tracking(INyARGrayscaleRaster i_raster, NyARSurfaceDataSet i_surface, NyARDoubleMatrix44 i_trans, NyARDoublePoint2d[] o_pos2d, NyARDoublePoint3d[] o_pos3d, int i_num)
{
//テンプレートドライバの更新
INyARTemplateMatchingDriver tmd;
if (this._last_raster != i_raster)
{
tmd = this._last_driver = new NyARTemplateMatchingDriver_INT1D(i_raster, 12, 12);
this._last_raster = i_raster;
}
else
{
tmd = this._last_driver;
}
//射影変換行列の計算とログへの追加
NyARSurfaceTransMatrixSet tlog = this._ctrans_log.preAdd();
tlog.setValue(this._ref_cparam.getPerspectiveProjectionMatrix(), i_trans);
//可視な候補を選択する。(一時リスト)
this._feature_selector.extractVisibleFeatures(i_surface.fset, tlog, this._candidate, this._candidate2);
PatchImagePositions pcpoints = this.__pcpoints;
//load screen size.
NyARIntSize s = this._ref_cparam.getScreenSize();
//頂点選択クラス類の初期化
NyARSurfaceFeatureIndexSelector index_selecter = this.__index_selecter;
NyARSurfaceFeaturesPtr selected_features = this.__selected_features;
selected_features.clear();
//最大返却数の決定
int max_feature = i_num > this.__selected_features.getArraySize() ? this.__selected_features.getArraySize() : i_num;
int num = 0;
NyARSurfaceFeatures current_candidate = this._candidate;
for (int i = max_feature - 1; i >= 0; i--)
{
//高精度を優先して探索。なければ低精度に切り替える。切替は1度だけ。出力は座標集合。
int k = index_selecter.ar2SelectTemplate(current_candidate, this._prev_selected_features, selected_features, s);
if (k < 0)
{
if (current_candidate == this._candidate2)
{
break;
}
current_candidate = this._candidate2;
//未選択なら終了
k = index_selecter.ar2SelectTemplate(current_candidate, this._prev_selected_features, selected_features, s);
if (k < 0)
{
break;
}
}
//候補kを確保
NyARSurfaceFeatureItem cai = current_candidate.getItem(k);
//可視な点について、トラッキングするためのパッチ画像を生成
NyARTemplatePatchImage template_ = this.__template_patch;
template_.makeFromReferenceImage((int)(cai.x + 0.5), (int)(cai.y + 0.5), tlog.ctrans, this._ref_cparam.getDistortionFactor(), i_surface.iset.items[cai.scale]);
//パッチ画像の内容をチェック？
if (template_.vlen * template_.vlen >= (template_.xsize) * (template_.ysize) * AR2_DEFALUT_TRACKING_SD_THRESH * AR2_DEFALUT_TRACKING_SD_THRESH)
{
//射影変換行列ログから候補点を作る。
int number_of_point = pcpoints.makeCandidatePos(cai, this._ctrans_log);
//画像からテンプレートを検索
double sim = tmd.ar2GetBestMatching(template_, pcpoints.pos, number_of_point, o_pos2d[num]);
//類似値が一定以上なら、保存
if (sim > this.simThresh)
{
if (selected_features.push(cai) == null)
{
break;//最大値に達したら終わり
}
this._ref_cparam.getDistortionFactor().observ2Ideal(o_pos2d[num], o_pos2d[num]);
o_pos3d[num].x = cai.ref_feature.mx;
o_pos3d[num].y = cai.ref_feature.my;
o_pos3d[num].z = 0;
//選択した得量を記録
num++;
}
}
//選択された候補を取り外す。
current_candidate.remove(k);
}
// 過去ログへ記録
this._prev_selected_features.clear();
for (int i = 0; i < selected_features.getLength(); i++)
{
this._prev_selected_features.push(selected_features.getItem(i).ref_feature);
}
return(num);
}
