public BinaryHierarchicalNode build(FreakMatchPointSetStack features)
{
this.mNextNodeId = 0;
int[] indices = new int[features.getLength()];
for (int i = 0; i < indices.Length; i++)
{
indices[i] = (int)i;
}
return(this.build(features.getArray(), null, indices, indices.Length));
}
/**
* FsetFileデータから、page_idに一致したキーマップを生成します。
* @param i_refDataSet
* @param i_page_id
*/
public KeyframeMap(NyARNftFreakFsetFile i_refDataSet, int i_page_id)
{
NyARNftFreakFsetFile.PageInfo page_info = i_refDataSet.page_info[i_page_id];
int db_id = 0;
for (int m = 0; m < page_info.image_info.Length; m++)
{
int image_no = page_info.image_info[m].image_no;
int l = 0;
//格納予定のデータ数を数える
for (int i = 0; i < i_refDataSet.ref_point.Length; i++)
{
if (i_refDataSet.ref_point[i].refImageNo == image_no)
{
l++;
}
}
FreakMatchPointSetStack fps = new FreakMatchPointSetStack(l);
for (int i = 0; i < i_refDataSet.ref_point.Length; i++)
{
if (i_refDataSet.ref_point[i].refImageNo == image_no)
{
NyARNftFreakFsetFile.RefDataSet t = i_refDataSet.ref_point[i];
FreakMatchPointSetStack.Item fp = fps.prePush();
fp.x = t.coord2D.x;
fp.y = t.coord2D.y;
fp.angle = t.featureVec.angle;
fp.scale = t.featureVec.scale;
fp.maxima = t.featureVec.maxima > 0 ? true : false;
if (i_refDataSet.ref_point[i].featureVec.v.Length != 96)
{
throw new NyARRuntimeException();
}
fp.descripter.setValueLe(i_refDataSet.ref_point[i].featureVec.v);
fp.pos3d.x = t.coord3D.x;
fp.pos3d.y = t.coord3D.y;
fp.pos3d.z = 0;
}
}
Keyframe keyframe = new Keyframe(page_info.image_info[m].w, page_info.image_info[m].h, fps);
this.Add(db_id++, keyframe);
}
return;
}
private bool query(FreakFeaturePointStack query_keyframe, KeyframeMap i_keymap, FeaturePairStack i_result)
{
// mMatchedInliers.clear();
HomographyMat H = this._H;
InverseHomographyMat hinv = this._hinv;
hinv = new InverseHomographyMat_O1();
int num_of_query_frame = query_keyframe.getLength();
//ワークエリアの設定
if (num_of_query_frame > this._tmp_pair_stack[0].getArraySize())
{
this._tmp_pair_stack[0] = new FeaturePairStack(num_of_query_frame + 10);
this._tmp_pair_stack[1] = new FeaturePairStack(num_of_query_frame + 10);
}
int tmp_ch = 0;
int last_inliers = 0;
foreach (KeyValuePair <int, Keyframe> i in i_keymap)
{
Keyframe second = i.Value;
FreakMatchPointSetStack ref_points = second.getFeaturePointSet();
//新しいワークエリアを作る。
FeaturePairStack match_result = this._tmp_pair_stack[tmp_ch];
//ワークエリア初期化
match_result.clear();
//特徴量同士のマッチング
if (this._matcher.match(query_keyframe, second, match_result) < this.mMinNumInliers)
{
continue;
}
// Vote for a transformation based on the correspondences
if (!this.mHoughSimilarityVoting.extractMatches(match_result, second.width(), second.height()))
{
continue;
}
// Estimate the transformation between the two images
if (!this.mRobustHomography.PreemptiveRobustHomography(H, match_result, second.width(), second.height()))
{
continue;
}
//ここでHInv計算
if (!hinv.inverse(H))
{
continue;
}
// Apply some heuristics to the homography
if (!hinv.checkHomographyHeuristics(second.width(), second.height()))
{
continue;
}
// Find the inliers
this._find_inliner.extructMatches(H, match_result);
if (match_result.getLength() < mMinNumInliers)
{
continue;
}
//
// Use the estimated homography to find more inliers
match_result.clear();
if (_matcher.match(query_keyframe, ref_points, hinv, 10, match_result) < mMinNumInliers)
{
continue;
}
//
// Vote for a similarity with new matches
if (!this.mHoughSimilarityVoting.extractMatches(match_result, second.width(), second.height()))
{
continue;
}
//
// Re-estimate the homography
if (!this.mRobustHomography.PreemptiveRobustHomography(H, match_result, second.width(), second.height()))
{
continue;
}
// Apply some heuristics to the homography
if (!hinv.inverse(H))
{
continue;
}
if (!hinv.checkHomographyHeuristics(second.width(), second.height()))
{
continue;
}
//
// Check if this is the best match based on number of inliers
this._find_inliner.extructMatches(H, match_result);
//ポイント数が最小値より大きい&&最高成績ならテンポラリチャンネルを差し替える。
if (match_result.getLength() >= mMinNumInliers && match_result.getLength() > last_inliers)
{
//出力チャンネルを切り替え
tmp_ch = (tmp_ch + 1) % 2;
last_inliers = match_result.getLength();
}
}
//出力は last_inlines>0の場合に[(tmp_ch+1)%2]にある。
if (last_inliers <= 0)
{
return(false);
}
{
FeaturePairStack match_result = this._tmp_pair_stack[(tmp_ch + 1) % 2];
FeaturePairStack.Item[] dest = match_result.getArray();
for (int i = 0; i < match_result.getLength(); i++)
{
FeaturePairStack.Item t = i_result.prePush();
if (t == null)
{
System.Console.WriteLine("Push overflow!");
break;
}
t.query = dest[i].query;
t.ref_ = dest[i].ref_;
}
}
return(true);
}