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