Пример #1
0
        public VPoint(VShape shape, VPoint camera)
        {
            ID = shape.ID;
            healthy = true;

            if (shape._isSquare)
                tag = "Square";
            if (shape._isTriangle)
                tag = "Triangle";

            Point camXY = shape.FindCenter();

            //Right so this is where things get ugly. Camera X and Y are divided by the total to get a percentage.
            //50% is subtracted to make the coordinant a percentage of total width relative to middle.
            //This is multiplied by the Camera constant (viewing angle) times the height of the 

            double xToCam = ((double)camXY.X / 480 - 0.5) * (EveDef.CameraWidthOverZ * 1.00000 * (camera.z + EveDef.WristToEnd_Z - EveDef.WristToCam_Z));
            double yToCam = ((double)camXY.Y / 640 - 0.5) * (EveDef.CameraWidthOverZ * 1.33333 * (camera.z + EveDef.WristToEnd_Z - EveDef.WristToCam_Z));
            
            double rToCam = Geometry.XYtoR(xToCam, yToCam);
            double thetaToCam = Geometry.XYtoTheta(xToCam, yToCam);

            //these are the distance from the center of camera to the object in correctly oriented mm
            int dX = (int)Math.Round(Geometry.PolarToX(thetaToCam + camera.Eve.A, rToCam));
            int dY = -(int)Math.Round(Geometry.PolarToY(thetaToCam + camera.Eve.A, rToCam));

            //the real camera position x and y, plus the dX and dY to the object, minus the extra x and y to get to the end-effector from the camera
            x = camera.x + dX - (int)Math.Round(Geometry.PolarToX(camera.Eve.A + Math.PI / 2, EveDef.WristToCam_L));
            y = camera.y + dY - (int)Math.Round(Geometry.PolarToY(camera.Eve.A + Math.PI / 2, EveDef.WristToCam_L));

            y = (int)(1.11*y - 30);
  
            if(y > 250 && y < 500)
            {
                z = (int)-((y - 250) / 30);
            }
            else
                z = 0;

            TransformXYZtoEVE();
            TransformXYZtoMARTY();

            Eve.D = Math.PI / 2; //NEEDS MORE
            Marty.D = 0;
        }
Пример #2
0
 public bool IsAnalogous(VShape other)
 {
     return (Distance(FindCenter(), other.FindCenter()) < 32 && _shape.Length == other._shape.Length);
 }
Пример #3
0
        public int FindShapes()
        {
            source = source.Resize(480, 640, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR);

            Random rnd = new Random();

            working = source.Convert<Gray, Byte>();

            working._SmoothGaussian(3);
            working = working.Canny(100, 60);

            Bgr red = new Bgr(0, 0, 255);
            Bgr green = new Bgr(0, 255, 0);
            Bgr blue = new Bgr(255, 0, 0);


            using (MemStorage storage = new MemStorage()) //allocate storage for contour approximation
                for (Contour<DrawingPoint> contours = working.FindContours(); contours != null; contours = contours.HNext)
                {
                    if (contours.Area > 1000)
                    {
                        
                        VShape newShape = new VShape(contours.ApproxPoly(contours.Perimeter * 0.04, storage).ToArray(), new Bgr(rnd.Next(32, 255), rnd.Next(32, 255), rnd.Next(32, 255)));

                        if (newShape._isTriangle || newShape._isSquare)
                        {
                            bool IsAdded = false;

                            //MessageBox.Show("Found something");

                            for (int i = 0; i < shapes.Count; i++)
                            {
                                if (shapes[i].IsAnalogous(newShape))
                                {
                                    newShape.ID = shapes[i].ID;
                                    shapes[i] = newShape;
                                    //shapes.Add(newShape);
                                    IsAdded = true;
                                    //break;
                                }
                            }

                            if (!IsAdded)
                            {
                                shapes.Add(newShape);
                            }
                        }
                    }
                }

            List<VShape> damnedShapes = new List<VShape>();

            foreach (VShape shape in shapes)
            {
                if (shape.health == 0)
                {
                    damnedShapes.Add(shape);
                    continue;
                }
                else
                    shape.health -= 1;

                //post.Draw(new CircleF(shape.FindCenter(), shape.health * (float)shape.scale / 50), shape.ID, 0);
                //////frame.Draw("          Inches away: " + Math.Round((1750/(shape.scale+7))-1.0,2).ToString() + ">" + shape.scale.ToString(), ref newFont, shape.FindCenter(), new Bgr(255, 64, 128));
                //post.Draw("       Center X: " + shape.FindCenter().X.ToString(), ref newFont, new Point(shape.FindCenter().X, shape.FindCenter().Y - 8), new Bgr(255, 64, 128));
                //post.Draw("       Center Y: " + shape.FindCenter().Y.ToString(), ref newFont, new Point(shape.FindCenter().X, shape.FindCenter().Y + 8), new Bgr(255, 64, 128));

                post.FillConvexPoly(shape._shape, shape.ID);
                //post.Draw(new CircleF(new PointF(20, 20), 10), new Bgr(128,128,128), 0);

            }

            foreach (VShape damnedShape in damnedShapes)
            {
                shapes.Remove(damnedShape);
            }


            return shapes.Count;

        }