void CameraVehicle_MotorSpeedChanged(object sender, MotorSpeedChangedEventArgs e)
{
backgroundImage = null;
Dispatcher.Invoke(() =>
{
currentTimeTextBlock.Text = e.Tick.ToString();
leftWheelSpeedTextBlock.Text = e.LeftSpeed.ToString("f2");
rightWheelSpeedTextBlock.Text = e.RightSpeed.ToString("f2");
});
if (saveToFileCheckBox.IsChecked.GetValueOrDefault(false) && saveToFileCheckBox.IsEnabled)
{
actionList.Add(e);
}
if (e.LeftSpeed == 0 && e.RightSpeed == 0 && lastMotorEvent != null)
{
if (lastMotorEvent.LeftSpeed == lastMotorEvent.RightSpeed)
{
double distance = (e.Tick - lastMotorEvent.Tick) / 1000 * lastMotorEvent.LeftSpeed;
//vehicle.Forward(distance);
MapWindow w = GetMapWindow();
w.startingVehicleState = vehicle.State;
w.Draw();
}
}
lastMotorEvent = e;
}
private static MapWindow GetMapWindow()
{
foreach (var window in App.Current.Windows)
{
MapWindow w = window as MapWindow;
if (w != null)
{
return(w);
}
}
MapWindow w1 = new MapWindow();
w1.Show();
return(w1);
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
connectionComboBox.Items.Add("USB");
foreach (string s in SerialPort.GetPortNames())
{
connectionComboBox.Items.Add(s);
}
var x = from n in NetworkInterface.GetAllNetworkInterfaces()
from add in n.GetIPProperties().UnicastAddresses
where n.OperationalStatus == OperationalStatus.Up &&
(n.NetworkInterfaceType == NetworkInterfaceType.Wireless80211 || n.NetworkInterfaceType == NetworkInterfaceType.Ethernet) &&
add.Address.AddressFamily == AddressFamily.InterNetwork
select add.Address.ToString();
interfaceComboBox.Items.Add("Select file");
foreach (var ip in x)
{
interfaceComboBox.Items.Add(ip);
}
leftSteeringMaxTextBlock.Text = rightSteeringMaxTextBlock.Text = vehicle.State.TurnMaxDegree.ToString("f2");
timer = new DispatcherTimer();
timer.Interval = TimeSpan.FromMilliseconds(100);
timer.Tick += timer_Tick;
timer.Start();
MapWindow w = GetMapWindow();
w.map = new Map(300, 300);
//w.carRectangle.Width = vehicle.State.Width;
//w.carRectangle.Height = vehicle.State.Length;
//w.carRectangle.SetValue(Canvas.LeftProperty, vehicle.State.Center.X - vehicle.State.Width / 2);
//w.carRectangle.SetValue(Canvas.TopProperty, vehicle.State.Center.Y + vehicle.State.Length / 2);
//backgroundImage = new Image<Gray, byte>(@"C:\Users\loveright\Documents\Visual Studio 2013\Projects\乐高\电脑端\bin\Debug\images\backgroundImage.bmp");
//var img = new Image<Bgr, byte>(@"C:\Users\loveright\Documents\Visual Studio 2013\Projects\乐高\电脑端\bin\Debug\images\1425366576428.jpg");
//AddToMap(backgroundImage, img);
}
private void AnalyzeFrame(object sender, DoWorkEventArgs e)
{
Image <Bgr, byte> img = ((ImageAvailableEventArgs)e.Argument).Image;
if (backgroundImage == null)
{
backgroundImage = new Image <Gray, byte>(img.Size);
}
//long t = Environment.TickCount;
Image <Gray, byte> b = GetBackground(img);
Dispatcher.BeginInvoke(new Action(() => CvInvoke.cvShowImage("Background", img.Copy(b))));
//Debug.WriteLine("GetBackground: " + (Environment.TickCount - t));
//t = Environment.TickCount;
//CvInvoke.cvShowImage("Masked", img);
Contour <PointInt> contours = b.FindContours();
Contour <PointInt> biggestContour = contours;
while (contours != null)
{
if (contours.Area > biggestContour.Area)
{
biggestContour = contours;
}
contours = contours.HNext;
}
//Debug.WriteLine("contours: " + (Environment.TickCount - t));
//t = Environment.TickCount;
bool voteFull = false;
Image <Gray, byte> tempImage = new Image <Gray, byte>(backgroundImage.Size);
tempImage.Draw(biggestContour, new Gray(10), -1);
backgroundImage = backgroundImage.Add(tempImage);
PointInt pUseless = PointInt.Empty;
voteFull = !backgroundImage.CheckRange(0, 250, ref pUseless);
if (voteFull)
{
collectedBackgroundCount++;
Dispatcher.Invoke(() =>
{
backgroundImage = backgroundImage - new Gray(130);
//w.carRectangle.Width = this.vehicle.State.Width;
//w.carRectangle.Height = this.vehicle.State.Length;
//w.carRectangle.SetValue(Canvas.LeftProperty, this.vehicle.State.Center.X - this.vehicle.State.Width / 2);
//w.carRectangle.SetValue(Canvas.TopProperty, this.vehicle.State.Center.Y + this.vehicle.State.Length / 2);
backgroundImage._Dilate(1);
//backgroundImage.ROI = new Rectangle(0, backgroundImage.Height - 1, backgroundImage.Width, 1);
//backgroundImage._Or(new Image<Gray, byte>(backgroundImage.Width, 1, new Gray(255)));
//backgroundImage.ROI = Rectangle.Empty;
CvInvoke.cvShowImage("Background-Draw to map",
backgroundImage.ThresholdBinary(new Gray(0), new Gray(255)));
AddToMap(backgroundImage.ThresholdBinary(new Gray(0), new Gray(255)), img);
MapWindow w = GetMapWindow();
w.Draw();
CameraVehicle vehicle = this.vehicle as CameraVehicle;
//if (vehicle != null)
//{
// if (collectedBackgroundCount < 3)
// vehicle.SetSpeed(4.05, 5000);
//}
getBackgroundCheckBox.IsChecked = false;
CvInvoke.cvShowImage("VotedBackground", backgroundImage);
});
}
else
{
Dispatcher.BeginInvoke(new Action(() => CvInvoke.cvShowImage("VotedBackground", backgroundImage)));
}
}
private void AddToMap(Image <Gray, byte> backgroundImage, Image <Bgr, byte> img)
{
backgroundImage.Save("images/backgroundImage.bmp");
MapWindow w = GetMapWindow();
//因为已经是二值图像了，所以头两个参数的值不影响。
LineSegment2D[] lines = backgroundImage.HoughLines(50, 50, 1, Math.PI / 180, 15, 3, 25)[0];
Graphics g = Graphics.FromImage(img.Bitmap);
foreach (LineSegment2D line in lines)
{
//在“天上”的东西不考虑。
if (line.P1.Y < backgroundImage.Height / 2 || line.P2.Y < backgroundImage.Height / 2)
{
continue;
}
//太远的东西不准确，不考虑。
if (line.P1.Y < 240 || line.P2.Y < 240)
{
continue;
}
double distanceInPixel = line.P1.Y - backgroundImage.Height / 2.0;
double distanceInCM1 = GetRealVerticalDistaneFromPixelDistance(distanceInPixel);
double hd1 = GetRealHorizontalDistanceFromPixelDistance(distanceInCM1, backgroundImage.Width / 2.0 - line.P1.X);
distanceInPixel = line.P2.Y - backgroundImage.Height / 2.0;
double distanceInCM2 = GetRealVerticalDistaneFromPixelDistance(distanceInPixel);
double hd2 = GetRealHorizontalDistanceFromPixelDistance(distanceInCM2, backgroundImage.Width / 2.0 - line.P2.X);
double k = Math.Abs((hd1 - hd2) / (distanceInCM1 - distanceInCM2));
Font font = new Font("Arial", 12);
if (k > 1.19) //看线是不是水平的
{
double x1 = vehicle.State.Center.X + vehicle.CameraPosition.X - hd1;
double x2 = vehicle.State.Center.X + vehicle.CameraPosition.X - hd2;
//if ((x1 < 30 && x2 < 30 && distanceInCM1 > 30 && distanceInCM1 < 110) == false)
//{
w.map.Fixtures.Add(new System.Windows.Media.RectangleGeometry(new Rect(
Math.Min(x1, x2),
vehicle.State.Center.Y - vehicle.CameraPosition.Y - (distanceInCM1 + distanceInCM2) / 2,
Math.Abs(x2 - x1),
1)));
//}
img.Draw(line, new Bgr(255, 0, 0), 1);
float y = (line.P1.Y + line.P2.Y) / 2f;
g.DrawString("!" /*+ ((distanceInCM1 + distanceInCM2) / 2).ToString("f2")*/ + ",k=" + k.ToString("f2"), font, System.Drawing.Brushes.Red,
new PointF((line.P1.X + line.P2.X) / 2, y));
}
else if (k < 0.12) //线虽然在图像上是斜的，可能现实中是垂直的。
{
g.DrawString("<>" /*+ hd1.ToString("f2")*/ + ",k=" + k.ToString("f2"), font, System.Drawing.Brushes.Red,
(new PointF((line.P1.X + line.P2.X) / 2, (line.P1.Y + line.P2.Y) / 2)));
img.Draw(line, new Bgr(255, 0, 0), 1);
double y1 = vehicle.State.Center.Y - vehicle.CameraPosition.Y - distanceInCM1;
double y2 = vehicle.State.Center.Y - vehicle.CameraPosition.Y - distanceInCM2;
//if ((hd1 < 30 && y1 > 30 && y1 < 110 && y2 > 30 && y2 < 110) == false)
//{
w.map.Fixtures.Add(new System.Windows.Media.RectangleGeometry(new Rect(
vehicle.State.Center.X + vehicle.CameraPosition.X - (hd2 + hd1) / 2,
Math.Min(y1, y2), 1, Math.Abs(y2 - y1))));
//}
//Line lineShape = new Line();
//lineShape.X1 = lineShape.X2 = vehicle.State.Center.X + vehicle.CameraPosition.X - (hd2 + hd1) / 2;
//lineShape.Y1 = vehicle.State.Center.Y - vehicle.CameraPosition.Y - distanceInCM1;
//lineShape.Y2 = vehicle.State.Center.Y - vehicle.CameraPosition.Y - distanceInCM2;
//lineShape.Stroke = Brushes.Red;
//canvas.Children.Add(lineShape);
}
else
{
img.Draw(line, new Bgr(0, 255, 0), 1);
g.DrawString("k=" + k.ToString("f2"), font, System.Drawing.Brushes.Red, (new PointF((line.P1.X + line.P2.X) / 2, (line.P1.Y + line.P2.Y) / 2)));
}
}
g.Dispose();
CvInvoke.cvShowImage("Line", img);
//if (collectedBackgroundCount == 1)
//{
//w.map.Fixtures.Add(new RectangleGeometry(new Rect(0, 176, 18, 1)));
//w.map.Fixtures.Add(new RectangleGeometry(new Rect(18, 150, 1, 26)));
//w.map.Fixtures.Add(new RectangleGeometry(new Rect(0, 150, 18, 0.5)));
//}
//else if (collectedBackgroundCount == 2)
//{
// w.map.Fixtures.Add(new RectangleGeometry(new Rect(0, 120, 16.5, 1)));
// w.map.Fixtures.Add(new RectangleGeometry(new Rect(16.5, 96.4, 1, 23.6)));
//}
}
