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); }