/// <summary> /// Find laser-plane through RANSAC /// </summary> /// <param name="context">Context</param> /// <param name="plane">Found plane</param> /// <returns>Success</returns> public bool FindLaserPlane(Bundle bundle) { BundleBookmarks b = new BundleBookmarks(bundle); List <System.Drawing.PointF> laser_pixels = b.LaserPixel; List <Ray> eye_rays = b.EyeRays; System.Drawing.Rectangle roi = b.ROI; List <Ray> rays; Console.WriteLine("value {0}", _only_out_of_roi); if (_only_out_of_roi) { rays = new List <Ray>(); for (int i = 0; i < laser_pixels.Count; ++i) { if (!roi.Contains(laser_pixels[i])) { rays.Add(eye_rays[i]); } } } else { rays = eye_rays; } if (rays.Count == 0) { return(false); } Vector[] isect; double[] ts; int[] plane_ids; IList <Plane> reference_planes = b.ReferencePlanes; Core.Intersection.FindEyeRayPlaneIntersections( rays.ToArray(), reference_planes.ToArray(), out ts, out isect, out plane_ids); Ransac <PlaneModel> ransac = new Ransac <PlaneModel>(isect); int min_consensus = (int)Math.Max(rays.Count * _min_consensus_precent, b.Image.Width * 0.05); _constraint.Bundle = bundle; Ransac <PlaneModel> .Hypothesis h = ransac.Run(_max_iterations, _plane_accurracy, min_consensus, _constraint); if (h != null) { b.LaserPlane = h.Model.Plane; return(true); } else { return(false); } }
/// <summary> /// Deals with a singe hypothesis /// </summary> /// <param name="h">Hypothesis</param> /// <param name="max_distance">Maximum distance threshold to qualify sample as inlier</param> /// <param name="constraints">Additional constraints</param> private void BuildHypothesis(Ransac <T> .Hypothesis h, double max_distance, IRansacModelConstraint constraints) { // Initial fit T model = h.Model; if (!model.Build(this.ChooseRandom(model.RequiredSamples))) { return; } if (constraints != null && !constraints.Test(model)) { return; } // Model parameters estimated, determine consensus set this.DetermineConsensusSet(h, max_distance); // Refit model using regression method and consensus set if (h.ConsensusIds.Count >= model.RequiredSamples) { model.Fit(h.ConsensusSet); h.ConsensusIds.Clear(); this.DetermineConsensusSet(h, max_distance); } }
private void DetermineConsensusSet(Ransac <T> .Hypothesis h, double max_distance) { T model = h.Model; for (int i = 0; i < _samples.Length; ++i) { double dist = model.DistanceTo(_samples[i]); if (dist <= max_distance) { h.ConsensusIds.Add(i); } } }
/// <summary> /// Run Ransac /// </summary> /// <param name="max_iter">Maximum number of iterations to generate hypothesis</param> /// <param name="max_distance">Maximum distance threshold to qualify sample as inlier</param> public Hypothesis Run(int max_iter, double max_distance, int min_consensus_size, IRansacModelConstraint constraints) { Hypothesis final = null; int i = 0; while (i < max_iter && final == null) { Hypothesis h = new Ransac <T> .Hypothesis(_samples); this.BuildHypothesis(h, max_distance, constraints); if (h.ConsensusIds.Count >= min_consensus_size) { final = h; } ++i; } return(final); }