public bool extractMatches(FeaturePairStack i_matche_resule, int refWidth, int refHeight)
{
// Extract the data from the features
// hough.vote((float*)&query[0], (float*)&ref[0], (int)matches.size());
int size = i_matche_resule.getLength();
if (size == 0)
{
return(false);
}
//ワークエリア
if (this._mSubBinLocations.Length < size)
{
this._mSubBinLocations = SubBinLocation.createArray(size + 10);
}
//FindHoughSimilarity
int max_dim = refWidth > refHeight ? refWidth : refHeight;//math_utils.max2(mRefImageWidth, mRefImageHeight);
if (!this.autoAdjustXYNumBins(max_dim, i_matche_resule))
{
return(false);
}
this.votemap.reset();
int num_of_subbin = this.vote(i_matche_resule, refWidth / 2, refHeight / 2, this._mSubBinLocations);
int max_hough_index = this.votemap.findMax();
if (max_hough_index < 0)
{
return(false);
}
this.FindHoughMatches(i_matche_resule, max_hough_index, kHoughBinDelta, this._mSubBinLocations, num_of_subbin);
return(true);
}
/**
* 現在の特徴点セットから、
* @param i_keymap
* @param i_result
* @return
*/
public bool kpmMatching(KeyframeMap i_keymap, NyARDoubleMatrix44 i_transmat)
{
FeaturePairStack result = new FeaturePairStack(kMaxNumFeatures);
if (!this.query(this.mQueryKeyframe, i_keymap, result))
{
return(false);
}
return(kpmUtilGetPose_binary(this._ref_cparam, result, i_transmat, this._result_param));
}
private static bool kpmUtilGetPose_binary(NyARParam i_cparam, FeaturePairStack matchData, NyARDoubleMatrix44 i_transmat, NyARTransMatResultParam i_resultparam)
{
NyARDoubleMatrix44 initMatXw2Xc = new NyARDoubleMatrix44();
// ARdouble err;
int i;
if (matchData.getLength() < 4)
{
return(false);
}
NyARDoublePoint2d[] sCoord = NyARDoublePoint2d.createArray(matchData.getLength());
NyARDoublePoint3d[] wCoord = NyARDoublePoint3d.createArray(matchData.getLength());
for (i = 0; i < matchData.getLength(); i++)
{
sCoord[i].x = matchData.getItem(i).query.x;
sCoord[i].y = matchData.getItem(i).query.y;
wCoord[i].x = matchData.getItem(i).ref_.pos3d.x;
wCoord[i].y = matchData.getItem(i).ref_.pos3d.y;
wCoord[i].z = 0.0;
}
NyARIcpPlane icp_planer = new NyARIcpPlane(i_cparam.getPerspectiveProjectionMatrix());
if (!icp_planer.icpGetInitXw2Xc_from_PlanarData(sCoord, wCoord, matchData.getLength(), initMatXw2Xc))
{
return(false);
}
/*
* printf("--- Init pose ---\n"); for( int j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) printf(" %8.3f",
* initMatXw2Xc[j][i]); printf("\n"); }
*/
NyARIcpPoint icp_point = new NyARIcpPoint(i_cparam.getPerspectiveProjectionMatrix());
icp_point.icpPoint(sCoord, wCoord, matchData.getLength(), initMatXw2Xc, i_transmat, i_resultparam);
if (i_resultparam.last_error > 10.0f)
{
return(false);
}
return(true);
}
/**
* Set the number of bins for translation based on the correspondences.
*/
private bool autoAdjustXYNumBins(int max_dim, FeaturePairStack i_point_pair)
{
int l = i_point_pair.getLength();
//prepare work area
if (this._project_dim.Length < l)
{
this._project_dim = new double[l + 10];
}
double[] projected_dim = this._project_dim;
for (int i = l - 1; i >= 0; i--)
{
// Scale is the 3rd component
FeaturePairStack.Item item = i_point_pair.getItem(i);
// Project the max_dim via the scale
double scale = SafeDivision(item.query.scale, item.ref_.scale);
projected_dim[i] = scale * max_dim;
}
// Find the median projected dim
// float median_proj_dim = FastMedian<float>(&projected_dim[0],
// (int)projected_dim.size());
double median_proj_dim = FastMedian(projected_dim, l);
// Compute the bin size a fraction of the median projected dim
double bin_size = 0.25f * median_proj_dim;
int t;
t = (int)Math.Ceiling((mMaxX - mMinX) / bin_size);
this.mNumXBins = (5 > t ? 5 : t);
t = (int)Math.Ceiling((mMaxY - mMinY) / bin_size);
this.mNumYBins = (5 > t ? 5 : t);
if (mNumXBins >= 128 || mNumYBins >= 128)
{
return(false);
}
return(true);
}
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);
}
private int vote(FeaturePairStack i_point_pair, int i_center_x, int i_center_y, SubBinLocation[] i_sub_bin_locations)
{
int size = i_point_pair.getLength();
int num_features_that_cast_vote = 0;
for (int i = 0; i < size; i++)
{
//mapCorrespondence(r,i_point_pair.getItem(i),i_center_x,i_center_y);
double rx, ry, rangle, rscale;
{
FreakFeaturePoint ins = i_point_pair.getItem(i).query;
FreakFeaturePoint ref_ = i_point_pair.getItem(i).ref_;
//angle
rangle = ins.angle - ref_.angle;
// Map angle to (-pi,pi]
if (rangle <= -PI)
{
rangle += (2 * PI);
}
else if (rangle > PI)
{
rangle -= (2 * PI);
}
double scale = SafeDivision(ins.scale, ref_.scale);
double c = (scale * Math.Cos(rangle));
double s = (scale * Math.Sin(rangle));
//scale
rscale = (double)(Math.Log(scale) * this.mScaleOneOverLogK);
//x,y
rx = c * i_center_x - s * i_center_y + (ins.x - (c * ref_.x - s * ref_.y));
ry = s * i_center_x + c * i_center_y + (ins.y - (s * ref_.x + c * ref_.y));
// Check that the vote is within range
if (rx < mMinX || rx >= mMaxX || ry < mMinY || ry >= mMaxY ||
rangle <= -PI || rangle > PI ||
rscale < mMinScale || rscale >= mMaxScale)
{
continue;
}
}
// Compute the bin location
SubBinLocation sub_bin = i_sub_bin_locations[num_features_that_cast_vote];
mapVoteToBin(sub_bin, rx, ry, rangle, rscale);
int binX = (int)Math.Floor(sub_bin.x - 0.5f);
int binY = (int)Math.Floor(sub_bin.y - 0.5f);
int binScale = (int)Math.Floor(sub_bin.scale - 0.5f);
int binAngle = ((int)Math.Floor(sub_bin.angle - 0.5f) + mNumAngleBins) % mNumAngleBins;
// Check that we can voting to all 16 bin locations
if (binX < 0 || (binX + 1) >= mNumXBins || binY < 0 ||
(binY + 1) >= mNumYBins || binScale < 0 ||
(binScale + 1) >= mNumScaleBins)
{
continue;
}
sub_bin.index = i;
num_features_that_cast_vote++;
this.votemap.vote16(binX, binY, binAngle, binScale, 1);
}
return(num_features_that_cast_vote);
}
/**
* Get only the matches that are consistent based on the hough votes.
*/
private void FindHoughMatches(FeaturePairStack in_matches, int binIndex, double binDelta, SubBinLocation[] i_sub_bin, int i_num_of_sbin)
{
/**
* Get the bins locations from an index.
*/
double ref_x, ref_y, ref_angle, ref_scale;
{
//ハッシュ値から元の値を復帰
int binX = binIndex & 0x3f;
int binY = (binIndex >> 7) & 0x3f;
int binAngle = (binIndex >> 14) & 0x3f;
int binScale = (binIndex >> 21) & 0x3f;
ref_x = binX + 0.5;
ref_y = binY + 0.5;
ref_angle = binAngle + 0.5;
ref_scale = binScale + 0.5;
}
//
int pos = 0;
for (int i = 0; i < i_num_of_sbin; i++)
{
SubBinLocation ins = i_sub_bin[i];
//getBinDistance
double d;
//x
d = Math.Abs(ins.x - ref_x);
if (d >= binDelta)
{
continue;
}
//y
d = Math.Abs(ins.y - ref_y);
if (d >= binDelta)
{
continue;
}
//scale
d = Math.Abs(ins.scale - ref_scale);
if (d >= binDelta)
{
continue;
}
// Angle
double d1 = Math.Abs(ins.angle - ref_angle);
double d2 = (double)this.mNumAngleBins - d1;
d = d1 < d2 ? d1 : d2;
if (d >= binDelta)
{
continue;
}
//idxは昇順のはずだから詰める。
int idx = ins.index;
in_matches.swap(idx, pos);
pos++;
}
in_matches.setLength(pos);
return;
}
/**
* Robustly solve for the homography given a set of correspondences.
* p=src=ref,q=dest=query
*/
public bool PreemptiveRobustHomography(HomographyMat H, FeaturePairStack matches, double i_width, double i_height)
{
RansacPointTable ps = this._ps;
// int[] tmp_i=new int[2 * num_points];
HomographyMat[] hyp = this.mHyp;/* 9*max_num_hypotheses */
CostPair[] hyp_costs = this.mHypCosts;
double scale = this.mCauchyScale;
int max_num_hypotheses = this.mMaxNumHypotheses;
int max_trials = this.mMaxTrials;
double one_over_scale2 = 1 / (scale * scale);
int num_hypotheses_remaining;
int cur_chunk_size, this_chunk_end;
int sample_size = 4;
this._homography_check.setTestWindow(i_width, i_height);
int num_points = matches.getLength();
// We need at least SAMPLE_SIZE points to sample from
if (num_points < sample_size)
{
return(false);
}
// seed = 1234;
ps.setSequential(matches.getArray(), num_points);
ps.setSeed(1234);
// Shuffle the indices
ps.shuffle(num_points);
int num_hypotheses = 0;
// Compute a set of hypotheses
for (int trial = 0; trial < max_trials; trial++)
{
// Shuffle the first SAMPLE_SIZE indices
ps.shuffle(sample_size);
// Check if the 4 points are geometrically valid
if (!ps.geometricallyConsistent4Point())
{
continue;
}
// Compute the homography
if (!this._homography_solver.solveHomography4Points(ps.indics[0], ps.indics[1], ps.indics[2], ps.indics[3], hyp[num_hypotheses]))
{
continue;
}
// Check the test points
if (!this._homography_check.geometricallyConsistent(hyp[num_hypotheses]))
{
continue;
}
num_hypotheses++;
if (num_hypotheses == max_num_hypotheses)
{
break;
}
}
// We fail if no hypotheses could be computed
if (num_hypotheses == 0)
{
return(false);
}
// Initialize the hypotheses costs
for (int i = 0; i < num_hypotheses; i++)
{
hyp_costs[i].first = 0;
hyp_costs[i].second = i;
}
num_hypotheses_remaining = num_hypotheses;
//チャンクサイズの決定
int chunk_size = this.mChunkSize;
if (chunk_size > num_points)
{
chunk_size = num_points;
}
cur_chunk_size = chunk_size;
for (int i = 0; i < num_points && num_hypotheses_remaining > 2; i += cur_chunk_size)
{
// Size of the current chunk
cur_chunk_size = (chunk_size < num_points - i) ? chunk_size : (num_points - i);
// End of the current chunk
this_chunk_end = i + cur_chunk_size;
// Score each of the remaining hypotheses
for (int j = 0; j < num_hypotheses_remaining; j++)
{
// const T* H_cur = &hyp[hyp_costs[j].second*9];
double hf = hyp[hyp_costs[j].second].cauchyProjectiveReprojectionCostSum(ps.indics, i, this_chunk_end, one_over_scale2);
hyp_costs[j].first = hf;
}
// Cut out half of the hypotheses
FastMedian(hyp_costs, num_hypotheses_remaining);
num_hypotheses_remaining = num_hypotheses_remaining >> 1;
}
// Find the best hypothesis
int min_index = hyp_costs[0].second;
double min_cost = hyp_costs[0].first;
for (int i = 1; i < num_hypotheses_remaining; i++)
{
if (hyp_costs[i].first < min_cost)
{
min_cost = hyp_costs[i].first;
min_index = hyp_costs[i].second;
}
}
// Move the best hypothesis
H.setValue(hyp[min_index]);
H.normalizeHomography();
return(true);
}
