private void SquareOfSquaresPage_Loaded(object sender, Windows.UI.Xaml.RoutedEventArgs e)
{
var random = new Random((int)DateTime.Now.Ticks);
foreach (var square in Squares.Squares)
{
var segment = new CircleSegment()
{
Height = square.ActualHeight,
Width = square.ActualWidth,
CenterPointX = square.ActualHeight / 2,
CenterPointY = square.ActualWidth / 2,
StartAngle = random.Next(360),
SweepAngle = random.Next(360),
StrokeThickness = random.Next(25),
IsStrokeRounded = random.Next(100) > 50,
IsPie = random.Next(100) > 75,
IsClosed = random.Next(100) > 75,
Stroke = square.RandomColor()
};
segment.Radius = Math.Max((square.ActualHeight / 2) - segment.StrokeThickness, 0);
square.Content = segment;
}
}
public void TestContains1()
{
var segment = new CircleSegment
{
Circle =
{
Center = new Point(0, 0),
Radius = 5
},
StartAngle = Math.PI / 2,
EndAngle = Math.PI
};
segment.Contains(segment.Circle.Center).Should().BeTrue("because a circle segment should contain its center");
segment.Contains(segment.StartPoint).Should().BeTrue("because a circle segment should contain its corners");
segment.Contains(segment.EndPoint).Should().BeTrue("because a circle segment should contain its corners");
segment.Contains(new Point(-2, -2)).Should().BeTrue("because the point lies in the center of the circle segment");
segment.Contains(new Point(-3, -3)).Should().BeTrue("because the point lies in the center of the circle segment");
segment.Contains(new Point(6, 0)).Should().BeFalse();
segment.Contains(new Point(-6, 0)).Should().BeFalse();
segment.Contains(new Point(0, 6)).Should().BeFalse();
segment.Contains(new Point(0, -6)).Should().BeFalse();
segment.Contains(new Point(2, 2)).Should().BeFalse("because the point lies in the wrong slice of the circle");
segment.Contains(new Point(-2, 2)).Should().BeFalse("because the point lies in the wrong slice of the circle");
segment.Contains(new Point(2, -2)).Should().BeFalse("because the point lies in the wrong slice of the circle");
}
public void find_circle(Mat img_mat, double threadhold)
{
Cv2.Blur(img_mat, img_mat, new Size(5, 5));
CircleSegment[] circle_seg = new CircleSegment[100];
circle_seg = Cv2.HoughCircles(img_mat, HoughMethods.Gradient, 1, 100, 200, 40, 20);
for (int i = 0; i < circle_seg.Length; i++)
{
bool bfound = false;
foreach (Polygon_T s in polygon_array)
{
if (circle_seg[i].Center.X > 0 && circle_seg[i].Center.Y > 0)
{
double dist = Math.Sqrt((circle_seg[i].Center.X - s.center_pos.X) *
(circle_seg[i].Center.X - s.center_pos.X) +
(circle_seg[i].Center.Y - s.center_pos.Y) *
(circle_seg[i].Center.Y - s.center_pos.Y));
if (dist < threadhold)
{
bfound = true;
}
}
}
if (bfound == false)
{
Polygon_T p = new Polygon_T();
p.center_pos = circle_seg[i].Center;
p.gt = GraphicType.GT_Sphere;
polygon_array.Add(p);
Cv2.Circle(img_mat, circle_seg[i].Center, 5, Scalar.Red, 2);
Cv2.Circle(img_mat, (int)circle_seg[i].Center.X, (int)circle_seg[i].Center.Y, (int)circle_seg[i].Radius, Scalar.Red, 2);
}
}
}
// returns the normalized vector from the compass center to the blue dot
public Point2f GetOrientation()
{
try
{
Bitmap compassArea = Crop(screen.bitmap, 600, 550, 900, 1050);
CircleSegment crosshair = FindCircle(compassArea);
var r = crosshair.Radius + 12;
var c = crosshair.Center;
Rectangle compassRect = new Rectangle((int)(c.X - r), (int)(c.Y - r), (int)(r * 2), (int)(r * 2));
Bitmap croppedCompass = Crop(compassArea, compassRect);
// work out where the target indicator is
pictureBox2.Image = croppedCompass;
Point2f result = FindTarget2(croppedCompass);
history.Insert(0, result);
if (history.Count > 5)
{
history.RemoveAt(5);
}
lastGoodOrientation = result;
lastGoodOrientationTimestamp = DateTime.UtcNow;
return(result);
}
catch
{
history.Clear();
throw;
}
}
private Point2f ComputeVector(CircleSegment c, OpenCvSharp.Point target, bool forward)
{
double x = (target.X - c.Center.X);
double y = (target.Y - c.Center.Y);
var maxRadius = Math.Max(c.Radius, Math.Sqrt(x * x + y * y));
x /= maxRadius;
y /= maxRadius;
// could return ship pitch and roll here ...
/*
* var rollangle = Math.Atan2(x, -y); // wrong order on purpose so that up is 0 degrees roll.
* var pitchangle = Math.Asin(Math.Sqrt(x*x + y*y) / maxRadius);
* if (!forward)
* pitchangle = Math.PI - pitchangle;
* return new PointF((float)pitchangle, (float)rollangle);
*/
// but x/y is actually easier to handle since we are only doing a crude alignment, and not computing angular velocities or anything
if (!forward)
{
y = (y > 0) ? 2 - y : -2 - y; // if target is behind, add lots of pitch offset so that exactly wrong direction is 2/-2.
}
return(new Point2f((float)x, (float)y));
}
private static Rect FindValuePosition(CircleSegment shape, System.Windows.Size size)
{
Vector offset = -(Vector)size / 2;
double angle = shape.Angle;
int numMaxIterations = Math.Max(1, (int)Math.Log(shape.Circle.Radius, 2));
double minimum = 0.5;
double maximum = 1;
double current = minimum;
double fittinRadius = current;
for (int i = 0; i < numMaxIterations; ++i)
{
Rect rect = FindRectangle(shape.Circle, current, angle, offset, size);
if (shape.Contains(rect))
{
maximum = current;
fittinRadius = current;
}
else
{
minimum = current;
}
if (maximum == minimum)
{
break;
}
current = minimum + (maximum - minimum) / 2;
}
return(FindRectangle(shape.Circle, fittinRadius, angle, offset, size));
}
private double GetObstacleCollisionDistance(bool forward, double curvature, double remainingDistance, IList <Polygon> obstacles)
{
double radius = 1 / Math.Abs(curvature);
Coordinates centerPoint;
if (forward)
{
centerPoint = new Coordinates(0, radius);
}
else
{
centerPoint = new Coordinates(0, -radius);
}
CircleSegment rearSegment, frontSegment;
double frontRadius = Math.Sqrt(radius * radius + TahoeParams.FL * TahoeParams.FL);
Coordinates frontStartPoint = new Coordinates(TahoeParams.FL, 0);
rearSegment = new CircleSegment(radius, centerPoint, Coordinates.Zero, remainingDistance + 2, true);
frontSegment = new CircleSegment(frontRadius, centerPoint, frontStartPoint, remainingDistance + 2, true);
Services.Dataset.ItemAs <CircleSegment>("parking path").Add(rearSegment, Services.RelativePose.CurrentTimestamp);
return(Math.Min(TestObstacleCollisionCircle(rearSegment, obstacles), TestObstacleCollisionCircle(frontSegment, obstacles)));
}
public void GenerateCircleSegments()
{
float unitAngle = (float)(Math.PI / nSlice);
float unitScale = (1 - planetCore.transform.localScale.x) / nLayer;
int order = nLayer * nSlice + 1; // Careful : planetCore orderInLayer must be greater than this one
colorBlocks = new Color[nSlice, nLayer]; // Generate 2D array of colors
segmentsOrdered = new CircleSegment[nSlice, nLayer]; // Generate 2D array of segments
Color[,] colorMapping = CreateInitialMapping(heightFilling, probabilityBlackLastLayer, fillingMode); // Generate initial mapping
for (int i = 0; i < nLayer; i++)
{
for (int j = 0; j < nSlice; j++)
{
GameObject segment = Instantiate(circleSegmentPrefab, transform);
SpriteRenderer renderer = segment.GetComponent <SpriteRenderer>();
Material segmentMaterial = renderer.material;
CircleSegment circleSegment = segment.GetComponent <CircleSegment>();
// Size
segment.transform.localScale =
planetCore.transform.localScale + (i + 1) * new Vector3(unitScale, unitScale, unitScale);
// Layer
renderer.sortingOrder = order;
// Shader for creating an arc
segmentMaterial.SetFloat("_Angle", unitAngle);
// Setting the arc at the right position
float rotate = -2 * j * unitAngle - (float)unitAngle;
segment.transform.rotation = Quaternion.Euler(0f, 0f, rotate * Mathf.Rad2Deg);
// Setting the collider
// CircleSegment circleSegment = segment.GetComponent<CircleSegment>();
float angle = j * 2 * unitAngle;
circleSegment.Initialize(
0.5f * (planetCore.transform.localScale.x + i * unitScale) / (segment.transform.localScale.x), // innerRadius
0.5f * (planetCore.transform.localScale.x + (i + 1) * unitScale) / (segment.transform.localScale.x), // outerRadius
angle + rotate + Mathf.PI / 2, // startAngle
angle + rotate + Mathf.PI / 2 + 2 * unitAngle, // endAngle
j, // slice
i //layer
);
// Color
// renderer.color = segmentColors[order % segmentColors.Length]; // Replace by a better chosen color
circleSegment.ChangeColor(colorMapping[j, i], false);
colorBlocks[j, i] = renderer.color; // Update color
order -= 1;
// Save segment in array
segmentsOrdered[j, i] = circleSegment;
}
}
}
public static CircleSegment CreateCircleSegment(Drawing drawing, IList <IFigure> foundDependencies)
{
var result = new CircleSegment()
{
Drawing = drawing, Dependencies = foundDependencies
};
return(result);
}
public void Render(IGraphics g, WorldTransform wt)
{
ArcVotingResults results = this.results;
if (results == null)
{
return;
}
// get the current vehicle position/heading
float vehicleHeading = (float)Services.VehicleStateService.Heading;
PointF vehiclePos = Utility.ToPointF(Services.VehicleStateService.Location);
g.GoToVehicleCoordinates(vehiclePos, vehicleHeading + (float)Math.PI / 2.0f);
IPen p = g.CreatePen();
p.Color = color;
p.Width = nomPixelWidth / wt.Scale;
foreach (ArcResults result in results.arcResults)
{
// generate the arc
if (Math.Abs(result.curvature) < 1e-4)
{
g.DrawLine(p, new PointF(0, 0), new PointF(dist, 0));
}
else
{
double curvature = result.curvature;
bool leftTurn = curvature > 0;
double radius = Math.Abs(1 / curvature);
double frontRadius = Math.Sqrt(TahoeParams.FL * TahoeParams.FL + radius * radius);
CircleSegment rearSegment;
if (leftTurn)
{
Coordinates center = new Coordinates(0, radius);
rearSegment = new CircleSegment(radius, center, Coordinates.Zero, dist, true);
}
else
{
Coordinates center = new Coordinates(0, -radius);
rearSegment = new CircleSegment(radius, center, Coordinates.Zero, dist, false);
}
PointF[] points = Utility.ToPointF(rearSegment.ToPoints(10));
g.DrawLines(p, points);
}
}
g.ComeBackFromVehicleCoordinates();
p.Dispose();
}
/// <summary>
/// Try to match (part of) a large green circle on the screen.
/// </summary>
public CircleSegment FindCorona()
{
// see the Experiments for how this works
Bitmap cropped = CompassSensor.Crop(screen.bitmap,
screen.bitmap.Width * 1 / 3,
screen.bitmap.Height * 1 / 3,
screen.bitmap.Width * 2 / 3,
screen.bitmap.Height * 2 / 3);
Mat screenwhole = BitmapConverter.ToMat(cropped);
Point2f ShipPointerOffset = new Point2f(0, 0);
try
{
ShipPointerOffset = FindShipPointer(IsolateYellow(screenwhole));
}
catch (Exception)
{
// If we can't find the ship pointer (it's hard to see it against the sun) then use the middle of the screen.
}
// erase the vivid areas, otherwise the blur subtraction turns yellow near red to green
Mat brightHSV = screenwhole.CvtColor(ColorConversionCodes.BGR2HSV);
Mat darkAreasMask = brightHSV.InRange(InputArray.Create(new int[] { 0, 0, 0 }), InputArray.Create(new int[] { 180, 255, 180 }));
Mat darkAreas = new Mat();
screenwhole.CopyTo(darkAreas, darkAreasMask);
Mat screenblur = darkAreas - darkAreas.Blur(new OpenCvSharp.Size(10, 10));
Mat sourceHSV = screenblur.CvtColor(ColorConversionCodes.BGR2HSV);
Mat mask = sourceHSV.InRange(InputArray.Create(new int[] { 35, 204, 20 }), InputArray.Create(new int[] { 90, 255, 255 }));
Mat sourceHSVFiltered = new Mat();
sourceHSV.CopyTo(sourceHSVFiltered, mask);
Mat sourceGrey = sourceHSVFiltered.Split()[2].InRange(32, 256);
LineSegmentPoint[] result = sourceGrey.HoughLinesP(1, 3.1415 / 180, 5, 10, 2);
List <Point2d> points = new List <Point2d>();
foreach (var line in result)
{
points.Add(line.P1);
points.Add(line.P2);
}
if (points.Count < 8)
{
throw new ArgumentException("Not enough points in corona circle");
}
CircleSegment c = ComputeCircle(points);
sourceGrey.Line(c.Center, ShipPointerOffset, 255);
c.Center -= ShipPointerOffset; // adjust for camera movement by taking ship pointer offset
sourceGrey.Circle(c.Center, (int)c.Radius, 255);
debugWindow.Image = BitmapConverter.ToBitmap(sourceGrey);
return(c);
}
private CircleSegment FindSegment(float mX, float mY)
{
PointF center = GetCenter(RenderTarget.Screen);
mX = (mX - center.X) / fZoom;
mY = (mY - center.Y) / fZoom;
CircleSegment result = fModel.FindSegment(mX, mY);
return(result);
}
public void Ctor()
{
var segment = new CircleSegment();
segment.Circle.Center.Should().Be(new Point(0, 0));
segment.Circle.Radius.Should().Be(0);
segment.StartAngle.Should().Be(0);
segment.EndAngle.Should().Be(0);
segment.StartPoint.Should().Be(new Point(0, 0));
segment.EndPoint.Should().Be(new Point(0, 0));
}
public void TestContains4()
{
var segment = new CircleSegment
{
Circle =
{
Radius = 1
},
};
segment.Contains(new Point(0.5, 0.5)).Should().BeFalse();
}
private void pictureMain_Paint(object sender, PaintEventArgs e)
{
if (scale > 0)
{
e.Graphics.ScaleTransform(1 / (float)scale, 1 / (float)scale);
}
// overlay shapes
foreach (ListViewItem it in listOverlay.CheckedItems)
{
int sn = int.Parse(it.Text);
object o = overlayShapes[sn];
switch (o.GetType().Name)
{
case "LineSegmentPoint":
LineSegmentPoint line = (LineSegmentPoint)o;
e.Graphics.DrawLine(shapeColors[sn % shapeColors.Length], line.P1.X, line.P1.Y, line.P2.X, line.P2.Y);
break;
case "CircleSegment":
CircleSegment circle = (CircleSegment)o;
e.Graphics.DrawEllipse(
shapeColors[sn % shapeColors.Length],
circle.Center.X - circle.Radius,
circle.Center.Y - circle.Radius,
circle.Radius * 2,
circle.Radius * 2);
break;
case "Point[]":
// polygon contour
System.Drawing.Point[] pts = (System.Drawing.Point[])o;
if (pts.Length > 2)
{
e.Graphics.DrawPolygon(shapeColors[sn % shapeColors.Length], (System.Drawing.Point[])o);
}
break;
case "Rectangle":
e.Graphics.DrawRectangle(shapeColors[sn % shapeColors.Length], (Rectangle)o);
break;
default:
break;
}
}
// ROI display
if (selectROI.X >= 0 || selectROI.Y >= 0 || selectROI.Width > 0 || selectROI.Height > 0)
{
e.Graphics.DrawRectangle(roiPen, selectROI.X, selectROI.Y, selectROI.Width, selectROI.Height);
}
}
public static void MatchCorona()
{
Bitmap screen = new Bitmap("Screenshot_0028.bmp");
Bitmap cropped = CompassSensor.Crop(screen, screen.Width * 1 / 3, screen.Height * 1 / 3, screen.Width * 2 / 3, screen.Height * 2 / 3);
Mat screenwhole = BitmapConverter.ToMat(cropped);
// erase the vivid areas, otherwise the blur subtraction turns yellow near red to green
Mat brightHSV = screenwhole.CvtColor(ColorConversionCodes.BGR2HSV);
Mat darkAreasMask = brightHSV.InRange(InputArray.Create(new int[] { 0, 0, 0 }), InputArray.Create(new int[] { 180, 255, 180 }));
Mat darkAreas = new Mat();
screenwhole.CopyTo(darkAreas, darkAreasMask);
Mat screenblur = darkAreas - darkAreas.Blur(new OpenCvSharp.Size(10, 10));
Window w3 = new Window(screenblur);
//screenblur.SaveImage("sharplines.png");
Mat sourceHSV = screenblur.CvtColor(ColorConversionCodes.BGR2HSV);
/* Paint.Net uses HSV [0..360], [0..100], [0..100].
* OpenCV uses H: 0 - 180, S: 0 - 255, V: 0 - 255
* Paint.NET colors:
* 73 100 18 brightest part of green edge
* 72 98 9 very dark green
* suggested range [70..180], [80..100], [8..100] (paint.net)
* suggested range [35..90], [204..255], [20..255] (openCV)
* */
Mat mask = sourceHSV.InRange(InputArray.Create(new int[] { 35, 204, 20 }), InputArray.Create(new int[] { 90, 255, 255 }));
Mat sourceHSVFiltered = new Mat();
sourceHSV.CopyTo(sourceHSVFiltered, mask);
Window w5 = new Window("yellowfilter", sourceHSVFiltered.CvtColor(ColorConversionCodes.HSV2BGR));
Mat sourceGrey = sourceHSVFiltered.Split()[2].InRange(32, 256); // Value channel is pretty good as a greyscale conversion
Window w6 = new Window("yellowFilterValue", sourceGrey);
LineSegmentPoint[] result = sourceGrey.HoughLinesP(1, 3.1415 / 180, 5, 10, 2);
List <Point2d> points = new List <Point2d>();
foreach (var line in result)
{
points.Add(line.P1);
points.Add(line.P2);
darkAreas.Line(line.P1, line.P2, new Scalar(255, 0, 255));
}
CircleSegment c = CruiseSensor.ComputeCircle(points);
darkAreas.Circle(c.Center, (int)c.Radius, new Scalar(255, 255, 0));
Window w9 = new Window("final", darkAreas);
}
public void TestContains3()
{
var segment = new CircleSegment
{
Circle =
{
Center = new Point(300, 300),
Radius = 200
},
StartAngle = 0,
EndAngle = Math.PI / 2
};
segment.Contains(new Point(250, 350)).Should().BeTrue();
}
protected override void OnMouseMove(MouseEventArgs e)
{
switch (fMouseCaptured)
{
case MouseCaptured.mcNone:
{
CircleSegment selected = FindSegment(e.Location);
string hint = "";
if (!Equals(fModel.Selected, selected))
{
fModel.Selected = selected;
if (selected != null && selected.IRec != null)
{
string name = GKUtils.GetNameString(selected.IRec, true, false);
hint = /*selected.Gen.ToString() + ", " + */ name;
}
Invalidate();
}
if (!Equals(fHint, hint))
{
fHint = hint;
if (!string.IsNullOrEmpty(hint))
{
//fToolTip.Show(hint, this, mpt.X, mpt.Y, 3000);
ToolTip = hint;
}
}
}
break;
case MouseCaptured.mcDrag:
{
Point pt = new Point(e.Location);
AdjustScroll(-(pt.X - fMouseCaptureX), -(pt.Y - fMouseCaptureY));
fMouseCaptureX = pt.X;
fMouseCaptureY = pt.Y;
}
break;
}
e.Handled = true;
base.OnMouseMove(e);
}
public void TestContains2()
{
var segment = new CircleSegment
{
Circle =
{
Center = new Point(0, 0),
Radius = 5
},
StartAngle = Math.PI,
EndAngle = Math.PI * 1.5
};
segment.Contains(new Rect(0, 0, 0, 0)).Should().BeTrue();
segment.Contains(new Rect(0, -1.1, 1, 1)).Should().BeTrue();
}
// For moving around the position
void Start()
{
xRotationSpeed = Random.Range(-0.03f, 0.03f);
yRotationSpeed = Random.Range(-0.03f, 0.03f);
zRotationSpeed = Random.Range(-0.03f, 0.03f);
transform.eulerAngles = new Vector3(Random.Range(-90f, 90f), Random.Range(-90f, 90f), Random.Range(-90f, 90f));
_startRotation = transform.eulerAngles;
CirclePointBuffer = new ComputeBuffer(numberOfCircularPoints, Marshal.SizeOf(typeof(CirclePointData)));
CircleSegmentBuffer = new ComputeBuffer(numberOfCircularPoints, Marshal.SizeOf(typeof(CircleSegment)));
CircleSegmentRenderBuffer = new ComputeBuffer(numberOfCircularPoints, Marshal.SizeOf(typeof(CircleSegment)));
var pData = new CirclePointData[numberOfCircularPoints];
var cData = new CircleSegment[numberOfCircularPoints];
for (int i = 0; i < pData.Length; i++)
{
//Vector3 velocity = new Vector3(0, 0, Random.Range(-15f, 15f));
Vector3 center = new Vector3(0, 0, 0);
float theta = ((float)i / pData.Length) * Mathf.PI * 2;
float xPos = Mathf.Cos(theta) * radius;
float yPos = 0;
float zPos = Mathf.Sin(theta) * radius;
//float zPos = 0;
Vector3 position = new Vector3(xPos, yPos, zPos);
//Vector3 velocity = position - center;
Vector3 velocity = Vector3.zero;
velocity = velocity.normalized;
pData[i].Velocity = velocity;
pData[i].Position = position;
}
for (int i = 0; i < cData.Length - 1; i++)
{
Vector3 start = pData[i].Position;
Vector3 end = pData[i + 1].Position;
cData[i].Start = start;
cData[i].End = end;
}
cData[cData.Length - 1].Start = pData[cData.Length - 1].Position;
cData[cData.Length - 1].End = pData[0].Position;
CirclePointBuffer.SetData(pData);
CircleSegmentBuffer.SetData(cData);
CircleSegmentRenderBuffer.SetData(cData);
pData = null;
cData = null;
lineRenderMat = new Material(LineRender);
lineRenderMat.hideFlags = HideFlags.HideAndDontSave;
}
public void PushCircleSegment(CircleSegment circle, CarTimestamp timestamp, string name, bool relative)
{
try {
if (relative)
{
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert();
timestamp = absTransform.Timestamp;
circle = circle.Transform(absTransform);
}
Services.Dataset.ItemAs <CircleSegment>(name).Add(circle, timestamp);
}
catch (Exception ex) {
OperationalLayer.Tracing.OperationalTrace.WriteWarning("could not send circle data to ui: {0}", ex.Message);
}
}
protected override void OnMouseUp(MouseEventArgs e)
{
if (fMouseCaptured == MouseCaptured.mcDrag)
{
fMouseCaptured = MouseCaptured.mcNone;
Cursor = Cursors.Default;
}
else if (e.Button == MouseButtons.Left)
{
CircleSegment selected = FindSegment(e.X, e.Y);
if (selected != null && selected.IRec != null)
{
RootPerson = selected.IRec;
}
}
base.OnMouseUp(e);
}
/// <summary>
/// Use template matching to find the small blue circle. HoughCircles didn't work because the dot/circle is too small.
/// </summary>
/// <param name="croppedCompass"></param>
/// <returns>The normalized vector from the center of the compass to the blue dot (y value is in [-2..-1]U[1..2] if dot is an empty circle)</returns>
public Point2f FindTarget2(Bitmap croppedCompass)
{
Mat source = BitmapConverter.ToMat(croppedCompass);
Mat[] channels = source.Split();
Mat blues2 = channels[0];
Mat clean = blues2.EmptyClone();
clean.SetTo(0);
blues2.CopyTo(clean, blues2.InRange(128, 255));
double minval, maxval_closed, maxval_open;
OpenCvSharp.Point minloc, maxloc_closed, maxloc_open;
Mat result_closed = clean.EmptyClone();
Cv2.MatchTemplate(clean, template_closed, result_closed, TemplateMatchModes.CCoeffNormed);
Cv2.MinMaxLoc(result_closed, out minval, out maxval_closed, out minloc, out maxloc_closed);
Mat result_open = clean.EmptyClone();
Cv2.MatchTemplate(clean, template_open, result_open, TemplateMatchModes.CCoeffNormed);
Cv2.MinMaxLoc(result_open, out minval, out maxval_open, out minloc, out maxloc_open);
Graphics g = Graphics.FromImage(croppedCompass);
const float match_threshold = 0.7f;
if (maxval_open > maxval_closed && maxval_open > match_threshold)
{
CircleSegment c = FindCircle(croppedCompass);
g.DrawLine(new Pen(Color.Green, 2), c.Center.X, c.Center.Y, maxloc_open.X + template_open.Width / 2, maxloc_open.Y + template_open.Height / 2);
return(ComputeVector(c, new OpenCvSharp.Point(maxloc_open.X + template_open.Size().Width / 2, maxloc_open.Y + template_open.Size().Height / 2), false));
}
else if (maxval_closed > match_threshold)
{
CircleSegment c = FindCircle(croppedCompass);
g.DrawLine(new Pen(Color.Green, 1), c.Center.X, c.Center.Y, maxloc_closed.X + template_closed.Width / 2, maxloc_closed.Y + template_closed.Height / 2);
return(ComputeVector(c, new OpenCvSharp.Point(maxloc_closed.X + template_closed.Size().Width / 2, maxloc_closed.Y + template_closed.Size().Height / 2), true));
}
else
{
throw new ArgumentException("Could not find target");
}
}
protected override void OnMouseUp(MouseEventArgs e)
{
if (fMouseCaptured == MouseCaptured.mcDrag)
{
fMouseCaptured = MouseCaptured.mcNone;
Cursor = Cursors.Default;
}
else if (e.Buttons == MouseButtons.Primary)
{
CircleSegment selected = FindSegment(e.Location);
if (selected != null && selected.IRec != null)
{
RootPerson = selected.IRec;
}
}
e.Handled = true;
base.OnMouseUp(e);
}
private static double GetObstacleClearanceCircle(CircleSegment segment, IList <Polygon> obstacles)
{
double minDist = double.MaxValue;
foreach (Polygon obs in obstacles)
{
foreach (Coordinates pt in obs)
{
Coordinates closestPt = segment.GetClosestPoint(pt);
double dist = closestPt.DistanceTo(pt);
if (dist < minDist)
{
minDist = dist;
}
}
}
return(minDist);
}
private double GetObstacleCollisionDistance(IList <Obstacle> obstacles)
{
if (movingOrder == null || movingOrder.DestState == null)
{
return(100);
}
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
bool counterClockwise = false;
if ((movingOrder.Forward && movingOrder.TurningRadius > 0) || (!movingOrder.Forward && movingOrder.TurningRadius < 0))
{
counterClockwise = true;
}
else
{
counterClockwise = false;
}
CircleSegment rearSegment, frontSegment;
double frontRadius = Math.Sqrt(movingOrder.TurningRadius * movingOrder.TurningRadius + TahoeParams.FL * TahoeParams.FL);
Coordinates frontStartPoint = pose.xy + Coordinates.FromAngle(pose.heading) * TahoeParams.FL;
Coordinates frontEndPoint = movingOrder.DestState.Loc + movingOrder.DestState.Heading.Normalize(TahoeParams.FL);
if (counterClockwise)
{
rearSegment = new CircleSegment(Math.Abs(movingOrder.TurningRadius), movingOrder.CenterPoint, pose.xy, movingOrder.DestState.Loc, true);
frontSegment = new CircleSegment(frontRadius, movingOrder.CenterPoint, frontStartPoint, frontEndPoint, true);
}
else
{
rearSegment = new CircleSegment(Math.Abs(movingOrder.TurningRadius), movingOrder.CenterPoint, pose.xy, movingOrder.DestState.Loc, false);
frontSegment = new CircleSegment(frontRadius, movingOrder.CenterPoint, frontStartPoint, frontEndPoint, false);
}
Services.Dataset.ItemAs <CircleSegment>("parking path").Add(rearSegment, curTimestamp);
return(Math.Min(TestObstacleCollisionCircle(rearSegment, obstacles), TestObstacleCollisionCircle(frontSegment, obstacles)));
}
private void mnu_Vision_HoughCircle_Click(object sender, EventArgs e)
{
dlgHoughCircle dlg = new dlgHoughCircle();
runVisionCommand(dlg, true);
CircleSegment[] res = dlg.getCircles();
overlayShapes.Clear();
if (res != null && res.Length > 0)
{
for (int i = 0; i < res.Length; i++)
{
CircleSegment l = res[i];
if (selectROI.Width > 0)
{
l.Center.X += selectROI.X;
l.Center.Y += selectROI.Y;
}
overlayShapes[i] = l;
}
}
refreshOverlayListview();
}
private static void TestObstacleCollision(double curvature, double dist, IList <Polygon> obstacles, out double collisionDist, out double clearanceDist)
{
if (Math.Abs(curvature) < 1e-10)
{
// process as a straight line
// determine end point of circle -- s = rθ, θ = sk (s = arc len, r = radius, θ = angle, k = curvature (1/r)
// this will always be very very near straight, so just process as straight ahead
LineSegment rearAxleSegment = new LineSegment(new Coordinates(0, 0), new Coordinates(dist, 0));
LineSegment frontAxleSegment = new LineSegment(new Coordinates(TahoeParams.FL, 0), new Coordinates(dist + TahoeParams.FL, 0));
collisionDist = Math.Min(TestObstacleCollisionStraight(rearAxleSegment, obstacles), TestObstacleCollisionStraight(frontAxleSegment, obstacles));
clearanceDist = Math.Min(GetObstacleClearanceLine(rearAxleSegment, obstacles), GetObstacleClearanceLine(frontAxleSegment, obstacles));
}
else
{
// build out the circle formed by the rear and front axle
bool leftTurn = curvature > 0;
double radius = Math.Abs(1 / curvature);
double frontRadius = Math.Sqrt(TahoeParams.FL * TahoeParams.FL + radius * radius);
CircleSegment rearSegment, frontSegment;
if (leftTurn)
{
Coordinates center = new Coordinates(0, radius);
rearSegment = new CircleSegment(radius, center, Coordinates.Zero, dist, true);
frontSegment = new CircleSegment(frontRadius, center, new Coordinates(TahoeParams.FL, 0), dist, true);
}
else
{
Coordinates center = new Coordinates(0, -radius);
rearSegment = new CircleSegment(radius, center, Coordinates.Zero, dist, false);
frontSegment = new CircleSegment(frontRadius, center, new Coordinates(TahoeParams.FL, 0), dist, false);
}
collisionDist = Math.Min(TestObstacleCollisionCircle(rearSegment, obstacles), TestObstacleCollisionCircle(frontSegment, obstacles));
clearanceDist = Math.Min(GetObstacleClearanceCircle(rearSegment, obstacles), GetObstacleClearanceCircle(frontSegment, obstacles));
}
}
private void refreshOverlayListview()
{
listOverlay.Items.Clear();
foreach (int key in overlayShapes.Keys)
{
ListViewItem it = listOverlay.Items.Add(key.ToString());
it.Checked = true;
it.BackColor = shapeColors[key % shapeColors.Length].Color;
object o = overlayShapes[key];
switch (o.GetType().Name)
{
case "LineSegmentPoint":
it.SubItems.Add("直线");
LineSegmentPoint line = (LineSegmentPoint)o;
it.SubItems.Add(line.P1.DistanceTo(line.P2).ToString("0.00"));
break;
case "CircleSegment":
it.SubItems.Add("圆");
CircleSegment circle = (CircleSegment)o;
it.SubItems.Add(circle.Radius.ToString("0"));
break;
case "Point[]":
it.SubItems.Add("轮廓");
it.SubItems.Add("");
break;
case "Rectangle":
it.SubItems.Add("矩形");
it.SubItems.Add("");
break;
default:
break;
}
}
}
private double TestObstacleCollisionCircle(CircleSegment segment, IList <Obstacle> obstacles)
{
double minDist = double.MaxValue;
foreach (Obstacle obs in obstacles)
{
Coordinates[] pts;
if (obs.AvoidancePolygon.Intersect(segment, out pts))
{
for (int i = 0; i < pts.Length; i++)
{
// get the distance from the start
double dist = segment.DistFromStart(pts[i]);
if (dist < minDist)
{
minDist = dist;
}
}
}
}
return(minDist - 0.5);
}
public void PushCircleSegment(CircleSegment circle, CarTimestamp timestamp, string name, bool relative)
{
try {
if (relative) {
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert();
timestamp = absTransform.Timestamp;
circle = circle.Transform(absTransform);
}
Services.Dataset.ItemAs<CircleSegment>(name).Add(circle, timestamp);
}
catch (Exception ex) {
OperationalLayer.Tracing.OperationalTrace.WriteWarning("could not send circle data to ui: {0}", ex.Message);
}
}
private double GetObstacleCollisionDistance(IList<Obstacle> obstacles)
{
if (movingOrder == null || movingOrder.DestState == null) return 100;
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
bool counterClockwise = false;
if ((movingOrder.Forward && movingOrder.TurningRadius > 0) || (!movingOrder.Forward && movingOrder.TurningRadius < 0)) {
counterClockwise = true;
}
else {
counterClockwise = false;
}
CircleSegment rearSegment, frontSegment;
double frontRadius = Math.Sqrt(movingOrder.TurningRadius*movingOrder.TurningRadius + TahoeParams.FL*TahoeParams.FL);
Coordinates frontStartPoint = pose.xy + Coordinates.FromAngle(pose.heading)*TahoeParams.FL;
Coordinates frontEndPoint = movingOrder.DestState.Loc + movingOrder.DestState.Heading.Normalize(TahoeParams.FL);
if (counterClockwise) {
rearSegment = new CircleSegment(Math.Abs(movingOrder.TurningRadius), movingOrder.CenterPoint, pose.xy, movingOrder.DestState.Loc, true);
frontSegment = new CircleSegment(frontRadius, movingOrder.CenterPoint, frontStartPoint, frontEndPoint, true);
}
else {
rearSegment = new CircleSegment(Math.Abs(movingOrder.TurningRadius), movingOrder.CenterPoint, pose.xy, movingOrder.DestState.Loc, false);
frontSegment = new CircleSegment(frontRadius, movingOrder.CenterPoint, frontStartPoint, frontEndPoint, false);
}
Services.Dataset.ItemAs<CircleSegment>("parking path").Add(rearSegment, curTimestamp);
return Math.Min(TestObstacleCollisionCircle(rearSegment, obstacles), TestObstacleCollisionCircle(frontSegment, obstacles));
}
private static double TestObstacleCollisionCircle(CircleSegment segment, IList<Polygon> obstacles)
{
double minDist = double.MaxValue;
foreach (Polygon obs in obstacles) {
Coordinates[] pts;
if (obs.Intersect(segment, out pts)) {
for (int i = 0; i < pts.Length; i++) {
// get the distance from the start
double dist = segment.DistFromStart(pts[i]);
if (dist < minDist) {
minDist = dist;
}
}
}
}
return minDist;
}
private static void TestObstacleCollision(double curvature, double dist, IList<Polygon> obstacles, out double collisionDist, out double clearanceDist)
{
if (Math.Abs(curvature) < 1e-10) {
// process as a straight line
// determine end point of circle -- s = rθ, θ = sk (s = arc len, r = radius, θ = angle, k = curvature (1/r)
// this will always be very very near straight, so just process as straight ahead
LineSegment rearAxleSegment = new LineSegment(new Coordinates(0, 0), new Coordinates(dist, 0));
LineSegment frontAxleSegment = new LineSegment(new Coordinates(TahoeParams.FL, 0), new Coordinates(dist+TahoeParams.FL,0));
collisionDist = Math.Min(TestObstacleCollisionStraight(rearAxleSegment, obstacles), TestObstacleCollisionStraight(frontAxleSegment, obstacles));
clearanceDist = Math.Min(GetObstacleClearanceLine(rearAxleSegment, obstacles), GetObstacleClearanceLine(frontAxleSegment, obstacles));
}
else {
// build out the circle formed by the rear and front axle
bool leftTurn = curvature > 0;
double radius = Math.Abs(1/curvature);
double frontRadius = Math.Sqrt(TahoeParams.FL*TahoeParams.FL + radius*radius);
CircleSegment rearSegment, frontSegment;
if (leftTurn) {
Coordinates center = new Coordinates(0, radius);
rearSegment = new CircleSegment(radius, center, Coordinates.Zero, dist, true);
frontSegment = new CircleSegment(frontRadius, center, new Coordinates(TahoeParams.FL, 0), dist, true);
}
else {
Coordinates center = new Coordinates(0, -radius);
rearSegment = new CircleSegment(radius, center, Coordinates.Zero, dist, false);
frontSegment = new CircleSegment(frontRadius, center, new Coordinates(TahoeParams.FL, 0), dist, false);
}
collisionDist = Math.Min(TestObstacleCollisionCircle(rearSegment, obstacles), TestObstacleCollisionCircle(frontSegment, obstacles));
clearanceDist = Math.Min(GetObstacleClearanceCircle(rearSegment, obstacles), GetObstacleClearanceCircle(frontSegment, obstacles));
}
}
private double GetObstacleCollisionDistance(bool forward, double curvature, double remainingDistance, IList<Polygon> obstacles)
{
double radius = 1/Math.Abs(curvature);
Coordinates centerPoint;
if (forward) {
centerPoint = new Coordinates(0, radius);
}
else {
centerPoint = new Coordinates(0, -radius);
}
CircleSegment rearSegment, frontSegment;
double frontRadius = Math.Sqrt(radius*radius + TahoeParams.FL*TahoeParams.FL);
Coordinates frontStartPoint = new Coordinates(TahoeParams.FL, 0);
rearSegment = new CircleSegment(radius, centerPoint, Coordinates.Zero, remainingDistance+2, true);
frontSegment = new CircleSegment(frontRadius, centerPoint, frontStartPoint, remainingDistance+2, true);
Services.Dataset.ItemAs<CircleSegment>("parking path").Add(rearSegment, Services.RelativePose.CurrentTimestamp);
return Math.Min(TestObstacleCollisionCircle(rearSegment, obstacles), TestObstacleCollisionCircle(frontSegment, obstacles));
}
private static double GetObstacleClearanceCircle(CircleSegment segment, IList<Polygon> obstacles)
{
double minDist = double.MaxValue;
foreach (Polygon obs in obstacles) {
foreach (Coordinates pt in obs) {
Coordinates closestPt = segment.GetClosestPoint(pt);
double dist = closestPt.DistanceTo(pt);
if (dist < minDist)
minDist = dist;
}
}
return minDist;
}
