/// <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);
}
}
public bool FindLaserLine(Bundle bundle) {
BundleBookmarks b = new BundleBookmarks(bundle);
Emgu.CV.Image<Bgr, byte> image = b.Image;
int laser_color = (int)b.LaserColor;
using (Emgu.CV.Image<Gray, byte> channel = image[laser_color]) {
List<System.Drawing.PointF> pixels = ExtractPoints(channel);
b.LaserPixel = pixels;
return pixels.Count > 0;
}
}
public bool FindLaserLine(Bundle bundle)
{
BundleBookmarks b = new BundleBookmarks(bundle);
Emgu.CV.Image <Bgr, byte> image = b.Image;
int laser_color = (int)b.LaserColor;
using (Emgu.CV.Image <Gray, byte> channel = image[laser_color]) {
List <System.Drawing.PointF> pixels = ExtractPoints(channel);
b.LaserPixel = pixels;
return(pixels.Count > 0);
}
}
/// <summary>
/// Filter laser-plane if angle between laser-plane and camera view direction is too small.
/// </summary>
/// <param name="context">Context</param>
/// <param name="filtered_plane">Laser-plane as given in context</param>
/// <returns>True if angle between camera and laser is greater-equal to minimum angle specified, false otherwise</returns>
public bool FilterLaserPlane(Bundle bundle)
{
BundleBookmarks b = new BundleBookmarks(bundle);
return(Math.Abs(Vector.ScalarProduct(_camera_view_direction, b.LaserPlane.Normal)) >= _angle_proj_cos);
}
/// <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;
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>
/// Process image
/// </summary>
/// <param name="s">Setup</param>
/// <param name="image">Image</param>
/// <param name="points">Found points</param>
/// <param name="pixels">Corresponding pixels for each point</param>
/// <returns>True if successful, false otherwise</returns>
public bool Process(
Setup s, Emgu.CV.Image<Bgr, byte> image,
out List<Vector> points, out List<System.Drawing.Point> pixels)
{
pixels = null;
points = null;
// 1. Update values needed by algorithms
Bundle b = new Bundle();
BundleBookmarks bb = new BundleBookmarks(b);
bb.ROI = _roi;
bb.ReferencePlanes = s.ReferenceBody.ReferencePlanes;
bb.Image = image;
bb.LaserColor = s.Laser.Color;
// 2. Extract laser line
if (!_line_algorithm.FindLaserLine(bb.Bundle)) return false;
// 3. Filter laser points
if (!_line_filter.FilterLaserLine(bb.Bundle)) return false;
if (bb.LaserPixel.Count < 3) return false;
// 4. Detect laser plane
List<Ray> eye_rays = new List<Ray>(Core.Ray.EyeRays(s.Camera.Intrinsics, bb.LaserPixel.ToArray()));
bb.EyeRays = eye_rays;
if (!_plane_algorithm.FindLaserPlane(bb.Bundle)) return false;
// 5. Filter laser plane
if (!_plane_filter.FilterLaserPlane(bb.Bundle)) return false;
// 6. Extract relevant points in ROI
pixels = new List<System.Drawing.Point>();
points = new List<Vector>();
List<System.Drawing.PointF> laser_pixel = bb.LaserPixel;
Plane laser_plane = bb.LaserPlane;
IList<Plane> reference_planes = s.ReferenceBody.AllPlanes;
for (int i = 0; i < laser_pixel.Count; ++i) {
// Round to nearest pixel
System.Drawing.Point p = laser_pixel[i].ToNearestPoint();
double t;
if (_roi.Contains(p)) {
Core.Ray r = eye_rays[i];
if (Core.Intersection.RayPlane(r, laser_plane, out t)) {
if (this.PointInsideOfReferenceVolume(reference_planes, r, t))
{
System.Drawing.Point in_roi = Core.IndexHelper.MakeRelative(p, _roi);
pixels.Add(p);
_point_accum.Accumulate(in_roi, r, t);
points.Add(_point_accum.Extract(in_roi));
}
}
}
}
s.Positioner.TransformPoints(ref points);
return points.Count > 0;
}
/// <summary>
/// Process image
/// </summary>
/// <param name="s">Setup</param>
/// <param name="image">Image</param>
/// <param name="points">Found points</param>
/// <param name="pixels">Corresponding pixels for each point</param>
/// <returns>True if successful, false otherwise</returns>
public bool Process(
Setup s, Emgu.CV.Image <Bgr, byte> image,
out List <Vector> points, out List <System.Drawing.Point> pixels)
{
pixels = null;
points = null;
// 1. Update values needed by algorithms
Bundle b = new Bundle();
BundleBookmarks bb = new BundleBookmarks(b);
bb.ROI = _roi;
bb.ReferencePlanes = s.ReferenceBody.ReferencePlanes;
bb.Image = image;
bb.LaserColor = s.Laser.Color;
// 2. Extract laser line
if (!_line_algorithm.FindLaserLine(bb.Bundle))
{
return(false);
}
// 3. Filter laser points
if (!_line_filter.FilterLaserLine(bb.Bundle))
{
return(false);
}
if (bb.LaserPixel.Count < 3)
{
return(false);
}
// 4. Detect laser plane
List <Ray> eye_rays = new List <Ray>(Core.Ray.EyeRays(s.Camera.Intrinsics, bb.LaserPixel.ToArray()));
bb.EyeRays = eye_rays;
if (!_plane_algorithm.FindLaserPlane(bb.Bundle))
{
return(false);
}
// 5. Filter laser plane
if (!_plane_filter.FilterLaserPlane(bb.Bundle))
{
return(false);
}
// 6. Extract relevant points in ROI
pixels = new List <System.Drawing.Point>();
points = new List <Vector>();
List <System.Drawing.PointF> laser_pixel = bb.LaserPixel;
Plane laser_plane = bb.LaserPlane;
IList <Plane> reference_planes = s.ReferenceBody.AllPlanes;
for (int i = 0; i < laser_pixel.Count; ++i)
{
// Round to nearest pixel
System.Drawing.Point p = laser_pixel[i].ToNearestPoint();
double t;
if (_roi.Contains(p))
{
Core.Ray r = eye_rays[i];
if (Core.Intersection.RayPlane(r, laser_plane, out t))
{
if (this.PointInsideOfReferenceVolume(reference_planes, r, t))
{
System.Drawing.Point in_roi = Core.IndexHelper.MakeRelative(p, _roi);
pixels.Add(p);
_point_accum.Accumulate(in_roi, r, t);
points.Add(_point_accum.Extract(in_roi));
}
}
}
}
s.Positioner.TransformPoints(ref points);
return(points.Count > 0);
}
/// <summary>
/// Filter laser-plane if angle between laser-plane and camera view direction is too small.
/// </summary>
/// <param name="context">Context</param>
/// <param name="filtered_plane">Laser-plane as given in context</param>
/// <returns>True if angle between camera and laser is greater-equal to minimum angle specified, false otherwise</returns>
public bool FilterLaserPlane(Bundle bundle) {
BundleBookmarks b = new BundleBookmarks(bundle);
return Math.Abs(Vector.ScalarProduct(_camera_view_direction, b.LaserPlane.Normal)) >= _angle_proj_cos;
}