public double cauchyProjectiveReprojectionCost(FeaturePairStack.Item i_ptr, double i_one_over_scale2)
{
NyARDoublePoint2d ref_ = i_ptr.ref_;
double w = this.m20 * ref_.x + this.m21 * ref_.y + this.m22;
double vx = ((this.m00 * ref_.x + this.m01 * ref_.y + this.m02) / w) - i_ptr.query.x; //XP
double vy = ((this.m10 * ref_.x + this.m11 * ref_.y + this.m12) / w) - i_ptr.query.y; //YP
double T = Math.Log(1 + (vx * vx + vy * vy) * i_one_over_scale2);
return(T);
}
/**
* Shuffle the elements of an array.
*/
public void shuffle(int i_num_of_shuffle)
{
FeaturePairStack.Item[] b = this.indics;
int seed = this.seed;
int n = this._num;
for (int i = 0; i < i_num_of_shuffle; i++)
{
seed = (214013 * seed + 2531011);
int k = ((seed >> 16) & 0x7FFF) % n;
FeaturePairStack.Item t = b[i];
b[i] = b[k];
b[k] = t;
}
this.seed = seed;
}
/**
* 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);
}
/**
* SolveHomography4Points
* Solve for the homography given 4 point correspondences.
*/
virtual public bool solveHomography4Points(FeaturePairStack.Item p1, FeaturePairStack.Item p2, FeaturePairStack.Item p3, FeaturePairStack.Item p4, HomographyMat H)
{
// T s, sp;
// T t[2], tp[2];
NyARDoublePoint2d x1p = new NyARDoublePoint2d(), x2p = new NyARDoublePoint2d(), x3p = new NyARDoublePoint2d(), x4p = new NyARDoublePoint2d();
NyARDoublePoint2d xp1p = new NyARDoublePoint2d(), xp2p = new NyARDoublePoint2d(), xp3p = new NyARDoublePoint2d(), xp4p = new NyARDoublePoint2d();
double[] ts = new double[3];
double[] tps = new double[3];
//
// Condition the points
//
if (!condition4Points2d(x1p, x2p, x3p, x4p, ts, p1.ref_, p2.ref_, p3.ref_, p4.ref_))
{
return(false);
}
if (!condition4Points2d(xp1p, xp2p, xp3p, xp4p, tps, p1.query, p2.query, p3.query, p4.query))
{
return(false);
}
//
// Solve for the homography
//
if (!this.solveHomography4PointsInhomogenous(H, x1p, x2p, x3p, x4p, xp1p, xp2p, xp3p, xp4p))
{
return(false);
}
//
// Denomalize the computed homography
//
H.denormalizeHomography(ts, tps);
return(true);
}
/**
* Check the geometric consistency between four correspondences.
* Ransacテーブルの4点についてHomography4PointsGeometricallyConsistent
*/
public bool geometricallyConsistent4Point()
{
FeaturePairStack.Item p1 = this.indics[0];
FeaturePairStack.Item p2 = this.indics[1];
FeaturePairStack.Item p3 = this.indics[2];
FeaturePairStack.Item p4 = this.indics[3];
if (((Geometry.LinePointSide(p1.ref_, p2.ref_, p3.ref_) > 0) ^ (Geometry.LinePointSide(p1.query, p2.query, p3.query) > 0)) == true)
{
return(false);
}
if (((Geometry.LinePointSide(p2.ref_, p3.ref_, p4.ref_) > 0) ^ (Geometry.LinePointSide(p2.query, p3.query, p4.query) > 0)) == true)
{
return(false);
}
if (((Geometry.LinePointSide(p3.ref_, p4.ref_, p1.ref_) > 0) ^ (Geometry.LinePointSide(p3.query, p4.query, p1.query) > 0)) == true)
{
return(false);
}
if (((Geometry.LinePointSide(p4.ref_, p1.ref_, p2.ref_) > 0) ^ (Geometry.LinePointSide(p4.query, p1.query, p2.query) > 0)) == true)
{
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);
}
/**
* SolveHomography4Points
* Solve for the homography given 4 point correspondences.
*/
override public bool solveHomography4Points(FeaturePairStack.Item p1, FeaturePairStack.Item p2, FeaturePairStack.Item p3, FeaturePairStack.Item p4, HomographyMat H)
{
NyARDoublePoint2d[] a1 = this._a1;
NyARDoublePoint2d[] a2 = this._a2;
a1[0] = p1.ref_; a1[1] = p2.ref_; a1[2] = p3.ref_; a1[3] = p4.ref_;
a2[0] = p1.query; a2[1] = p2.query; a2[2] = p3.query; a2[3] = p4.query;
double mus1_0 = 0;
double mus1_1 = 0;
double mus2_0 = 0;
double mus2_1 = 0;
double s1, s2;
{
/**
* Condition four 2D points such that the mean is zero and the standard
* deviation is sqrt(2).
* condition4Points2d+AddHomographyPointContraint function
*/
//condition4Points2d(a1,a2,this._mat_A);
for (int i = 0; i < 4; i++)
{
mus1_0 += a1[i].x;
mus1_1 += a1[i].y;
mus2_0 += a2[i].x;
mus2_1 += a2[i].y;
}
mus1_0 /= 4;
mus1_1 /= 4;
mus2_0 /= 4;
mus2_1 /= 4;
double dw1 = 0;
double dw2 = 0;
for (int i = 0; i < 4; i++)
{
double[] l;
double X1 = a1[i].x - mus1_0;
double Y1 = a1[i].y - mus1_1;
double X2 = a2[i].x - mus2_0;
double Y2 = a2[i].y - mus2_1;
dw2 += Math.Sqrt(X2 * X2 + Y2 * Y2);
dw1 += Math.Sqrt(X1 * X1 + Y1 * Y1);
l = this._mat_A[i * 2];
l[0] = -X1;
l[1] = -Y1;
l[6] = X2 * X1;
l[7] = X2 * Y1;
l[8] = X2;
l = this._mat_A[i * 2 + 1];
l[3] = -X1;
l[4] = -Y1;
l[6] = Y2 * X1;
l[7] = Y2 * Y1;
l[8] = Y2;
}
if (dw1 * dw2 == 0)
{
return(false);
}
s1 = (4 / dw1) * SQRT2;
s2 = (4 / dw2) * SQRT2;
// double ss=s1*s2;
for (int i = 0; i < 4; i++)
{
double[] l;
l = this._mat_A[i * 2];
l[6] = (-l[0] * s1) * (l[8] * s2); //emulation calculation order.
l[7] = (-l[1] * s1) * (l[8] * s2); //emulation calculation order.
// l[6]*=ss;
// l[7]*=ss;
l[0] *= s1;
l[1] *= s1;
l[2] = -1;
l[3] = l[4] = l[5] = 0;
l[8] *= s2;
l = this._mat_A[i * 2 + 1];
l[6] = (-l[3] * s1) * (l[8] * s2); //emulation calculation order.
l[7] = (-l[4] * s1) * (l[8] * s2); //emulation calculation order.
// l[6]*=ss;
// l[7]*=ss;
l[0] = l[1] = l[2] = 0;
l[3] *= s1;
l[4] *= s1;
l[5] = -1;
l[8] *= s2;
}
}
if (!this.solveHomography4PointsInhomogenous(H))
{
return(false);
}
//
// Denomalize the computed homography
//
{ //H.denormalizeHomography(ts, tps);
double stx = s1 * mus1_0;
double sty = s1 * mus1_1;
double apc = (H.m20 * mus2_0) + (H.m00 / s2);
double bpd = (H.m21 * mus2_0) + (H.m01 / s2);
H.m00 = s1 * apc;
H.m01 = s1 * bpd;
H.m02 = H.m22 * mus2_0 + H.m02 / s2 - stx * apc - sty * bpd;
double epg = (H.m20 * mus2_1) + (H.m10 / s2);
double fph = (H.m21 * mus2_1) + (H.m11 / s2);
H.m10 = s1 * epg;
H.m11 = s1 * fph;
H.m12 = H.m22 * mus2_1 + H.m12 / s2 - stx * epg - sty * fph;
H.m20 = H.m20 * s1;
H.m21 = H.m21 * s1;
H.m22 = H.m22 - H.m20 * mus1_0 - H.m21 * mus1_1;
}
return(true);
}