Exemplo n.º 1
0
        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;
        }
Exemplo n.º 2
0
        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);
        }
Exemplo n.º 3
0
        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);
        }
Exemplo n.º 4
0
        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)));
            }
        }
Exemplo n.º 5
0
        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)));
            //}
        }