public static ExtrinsicCameraParameters Calibrate(PointF[] locals, MCvPoint3D32f[] globals, Size pattern)
        {
            PointF[] pl = new PointF[]
            {
                locals[0 + (pattern.Height -1) * pattern.Width],
                locals[0 + 0 * pattern.Width],
                locals[(pattern.Width-1) + (pattern.Height -1) * pattern.Width],
            };

            MCvPoint3D32f[] pg = new MCvPoint3D32f[]
            {
                globals[0 + (pattern.Height -1) * pattern.Width],
                globals[0 + 0 * pattern.Width],
                globals[(pattern.Width-1) + (pattern.Height -1) * pattern.Width],
            };

            //f2 = (p1 - p0).Normalize(1);
            //f1 = (p2 - p0).Normalize(1);
            //f1 = (f1 - (f1.DotProduct(f2) * f2)).Normalize(1);
            //f3 = new DenseVector(new double[] { f1[1] * f2[2] - f1[2] * f2[1], f1[2] * f2[0] - f1[0] * f2[2], f1[0] * f2[1] - f1[1] * f2[0] });
            //f3 = f3.Normalize(1);

            var plv = pl.Select(row => new DenseVector(new double[] { row.X, row.Y })).ToArray();

            return null;
        }
Esempio n. 2
0
        public PolyRect(PointF[] points)
        {
            var rect = PolygonMath.GetBoundingBox(points);
            this.SetBounds(rect.X, rect.Y, rect.Width, rect.Height);

            // PolygonMath.IsUnrotated can tell us that the points follow a
            // particular pattern, but in order to represent 90-degree rotations
            // and flips, we want to consider them to be *non-rectangle* points.
            // Therefore, we use a much more strict definition: there can only
            // be 4 points, and they must be in the canonical order.
            var right = this.x + this.width;
            var bottom = this.y + this.height;

            this.rect = points.Length == 4 &&
                        points[0].X == this.x && points[0].Y == this.y &&
                        points[1].X == right && points[1].Y == this.y &&
                        points[2].X == right && points[2].Y == bottom &&
                        points[3].X == this.x && points[3].Y == bottom;

            this.points = points.Select(p => new float[] { p.X, p.Y }).ToArray();
        }
Esempio n. 3
0
        protected override void OnPaint(PaintEventArgs e)
        {
            if (fitToSize)
            {
                // Measure size (this is with border)
                var w = e.ClipRectangle.Width;
                var h = e.ClipRectangle.Height;

                var dw = targetW - w;
                var dh = targetH - h;

                this.Size = new Size(this.Width + dw, this.Height + dh);
                fitToSize = false;
            }

            var g = e.Graphics;
            g.FillRectangle(Brushes.Black, e.ClipRectangle);

            if (data == null)
            {
                g.DrawString("NO ETS2 DATA MINER ACTIVE", new Font("Arial", 24.0f), Brushes.White, 5, 5);
                return;
            }

            // Render map
            float scale = 150; // p-p
            dlMap.RenderMap(e.ClipRectangle, g, true, ref scale);

            // ME:
            g.DrawEllipse(new Pen(meColor, 5.0f), targetW/2 - 3, targetH/2 - 3, 6, 6);

            var meHeading = Math.PI - data.MyTelemetry.Physics.RotationX*-2*Math.PI;
            var meHeadingRadius = 25;
            g.DrawLine(new Pen(meColor, 3.0f), targetW/2, targetH/2,
                targetW/2 + (float) Math.Sin(meHeading)*meHeadingRadius,
                targetH/2 + (float) Math.Cos(meHeading)*meHeadingRadius);

            var centerX = data.MyTelemetry.Physics.CoordinateX;
            var centerY = data.MyTelemetry.Physics.CoordinateY;
            var centerZ = data.MyTelemetry.Physics.CoordinateZ;

            var steeringAngle = 35.0/180*Math.PI*data.MyTelemetry.Controls.GameSteer;
            var wheelBase = 4;
            var saTan = (float) Math.Tan(Math.Abs(steeringAngle));
            var steerRadius = wheelBase/saTan;
            var steerCircumfere = steerRadius*2*Math.PI;
            var steerRadiusSc = steerRadius/scale*targetW;
            var offx = Math.Sin(meHeading);

            if (false && float.IsNaN(steerRadiusSc) == false && float.IsInfinity(steerRadiusSc) == false &&
                Math.Abs(steerRadius) < 30000)
            {

                if (steeringAngle < 0) // left
                {
                    var corrAngle = meHeading + Math.PI/2;
                    g.DrawArc(new Pen(Brushes.Aqua, 2.0f),
                        targetW/2 - steerRadiusSc/2 - (float) Math.Sin(corrAngle)*steerRadiusSc/2,
                        targetH/2 - steerRadiusSc/2 - (float) Math.Cos(corrAngle)*steerRadiusSc/2,
                        steerRadiusSc, steerRadiusSc,
                        (float) (meHeading/Math.PI*180 + 90), 360);
                }
                else
                {
                    var corrAngle = meHeading + Math.PI/2;
                    g.DrawArc(new Pen(Brushes.Aqua, 2.0f),
                        targetW/2 - steerRadiusSc/2 + (float) Math.Sin(corrAngle)*steerRadiusSc/2,
                        targetH/2 - steerRadiusSc/2 + (float) Math.Cos(corrAngle)*steerRadiusSc/2,
                        steerRadiusSc, steerRadiusSc,
                        (float) (meHeading/Math.PI*180 + 90), 360);
                }
                /*g.DrawEllipse(new Pen(Brushes.Turquoise, 2.0f),
                    targetW / 2 - (float)Math.Sin(meHeading) * steerRadiusSc,
                    targetH / 2 - (float)Math.Cos(meHeading) * steerRadiusSc,
                    steerRadiusSc, steerRadiusSc);*/
            }

            // Track this vehicle along curve:
            var mX = data.MyTelemetry.Physics.CoordinateX;
            var mY = data.MyTelemetry.Physics.CoordinateZ;

            var px = 0.0f;
            var py = 0.0f;

            var step = 0.05f;
            var heading = meHeading + Math.PI/2;

            foreach (var car in data.Cars)
            {
                car.Tracked = false;
                car.Tick();
            }

            var pwr = Main.Drivetrain.CalculatePower(data.MyTelemetry.Drivetrain.EngineRpm, data.MyTelemetry.Controls.GameThrottle);
            var scanDistance = 2.0f + Math.Pow(1 - Math.Abs(data.MyTelemetry.Controls.GameSteer), 64)*3.5f;
            for (float ts = 0.0f; ts < scanDistance && !data.Cars.Any(x => x.Tracked); ts += step)
            {
                // Interpolate the steer radius
                var ds = data.MyTelemetry.Drivetrain.Speed*ts;
                var spd = Math.Max(10, data.MyTelemetry.Drivetrain.Speed); // always scan at minimum of 36kmh
                var da = spd/steerCircumfere*2*Math.PI*step;

                var dix = Math.Sin(heading) - Math.Sin(heading + da);
                var diy = Math.Cos(heading) - Math.Cos(heading + da);

                if (steeringAngle < 0)
                    heading -= da;
                else
                    heading += da;

                px += (float) dix*steerRadius/2;
                py += (float) diy*steerRadius/2;

                // Rotated polygon
                var carL = 12.0f;
                var carW = 2.5f;
                var hg = -heading; //
                PointF[] poly = new PointF[]
                {
                    new PointF(mX + px + carL/2*(float) Math.Cos(hg) - carW/2*(float) Math.Sin(hg),
                        mY + py + carL/2*(float) Math.Sin(hg) + carW/2*(float) Math.Cos(hg)),
                    new PointF(mX + px - carL/2*(float) Math.Cos(hg) - carW/2*(float) Math.Sin(hg),
                        mY + py - carL/2*(float) Math.Sin(hg) + carW/2*(float) Math.Cos(hg)),
                    new PointF(mX + px - carL/2*(float) Math.Cos(hg) + carW/2*(float) Math.Sin(hg),
                        mY + py - carL/2*(float) Math.Sin(hg) - carW/2*(float) Math.Cos(hg)),
                    new PointF(mX + px + carL/2*(float) Math.Cos(hg) + carW/2*(float) Math.Sin(hg),
                        mY + py + carL/2*(float) Math.Sin(hg) - carW/2*(float) Math.Cos(hg)),
                };
                foreach (var car in data.Cars)
                {
                    if (car.Valid && IsPolygonsIntersecting(car.Box, poly))
                    {
                        car.Tracked = true;
                        break;
                    }
                }

                var drx = targetW/2 + px/scale*targetW;
                var dry = targetH/2 + py/scale*targetH;

                var polyToDraw =
                    poly.Select(
                        x => new PointF(targetW/2 + (x.X - mX)/2/scale*targetW, targetH/2 + (x.Y - mY)/2/scale*targetH))
                        .ToArray();
                g.FillPolygon(Brushes.Tomato, polyToDraw);
                //g.DrawLine(new Pen(Color.DarkSalmon, 5.0f), drx, dry, drx + 1, dry);
            }

            g.FillRectangle(new SolidBrush(Color.FromArgb(25, 25, 25)), 0, 0, targetW, 32);
            var trafficColor = Brushes.BlueViolet;
            foreach (var car in data.Cars)
            {
                if (!car.Valid)
                    continue;
                var x = targetW/2 + (car.X - centerX)/2/scale*targetW;
                var y = targetH/2 + (car.Z - centerZ)/2/scale*targetH;

                var sz = 10*scale;
                if (sz > 10) sz = 10;
                var of = sz/2;

                var dx = car.X - centerX;
                var dy = car.Z - centerZ;
                var dv = data.MyTelemetry.Drivetrain.Speed - car.Speed; // m/s
                var distance = (float) Math.Sqrt(dx*dx + dy*dy) - 12;
                if (distance < 0.1f) distance = 0.1f;
                var tti = dv < 0 ? -1.0f : distance/dv;

                var tc = trafficColor;
                if (car.Tracked)
                {
                    g.DrawString("Track #" + car.ID + " SPD " + Math.Round(car.Speed*3.6) +
                                 "km/h (d=" + Math.Round(car.Speed*3.6 - data.MyTelemetry.Drivetrain.SpeedKmh,1) +
                                 "); distance " + Math.Round(distance, 1) + "m TTI " + ((tti==-1.0f)?"never":Math.Round(tti, 2) + "s"),
                        new Font("Arial", 11.0f, FontStyle.Bold),
                        Brushes.White,
                        5, 5);
                    car.Distance = distance;
                    car.TTI = tti;
                    TrackedCar = car;

                    tc = Brushes.Turquoise;
                    if (tti < 10 && tti > 0)
                        tc = Brushes.CadetBlue;
                    if (tti < 5 && tti > 0)
                        tc = Brushes.DarkSalmon;
                    if (tti < 2 && tti > 0)
                        tc = Brushes.HotPink;
                }

                if (car.Box == null)
                    continue;
                var polyToDraw =
                    car.Box.Select(
                        d =>
                            new PointF(targetW/2 + (d.X - centerX)/2/scale*targetW,
                                targetH/2 + (d.Y - centerZ)/2/scale*targetH)).ToArray();
                g.FillPolygon(tc, polyToDraw);
                //g.DrawEllipse(new Pen(tc, sz), x - of,  y - of, sz, sz);

                /*if (tti == -1337.0f)
                    g.DrawString(":-)", new Font("Arial", 7.0f), Brushes.White, x-8,y-4);
                else
                    g.DrawString(Math.Round(tti,1).ToString(), new Font("Arial", 7.0f), Brushes.White, x-8,y-4);*/
            }
            g.FillRectangle(new SolidBrush(Color.FromArgb(25, 25, 25)), 0, targetH - 32, targetW, targetH);
            g.DrawString("Looking " + Math.Round(scanDistance, 2) + "s ahead SPD " + Math.Round(data.MyTelemetry.Drivetrain.SpeedKmh) + "kmh PWR " + Math.Round(pwr) + "hp THR " +
                Math.Round(data.MyTelemetry.Controls.GameThrottle * 100, 1) + "% BKR " + Math.Round(data.MyTelemetry.Controls.GameBrake * 100, 2) + "%",
                new Font("Arial", 11.0f, FontStyle.Bold), Brushes.White,
                2, targetH - 25);
            if (data.Cars.Any(x => x.Tracked) == false)
            {
                g.DrawString("No car being tracked (followed)",
                    new Font("Arial", 12.0f, FontStyle.Bold),
                    Brushes.White,
                    5, 5);
                TrackedCar = null;
            }
        }
Esempio n. 4
0
 public PointF[] OffsetPoints(PointF[] Points)
 {
     return Points.Select(p => PointF.Subtract(p, Offset)).ToArray();
 }
Esempio n. 5
0
        public static PointF[] PhineTune(Projector projector, Camera camera, PointF[] cameraCorners, PointF[] rough, int subdiv)
        {
            var color = Color.Green;
            projector.DrawBackground(Color.Black);
            var nolight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
            projector.DrawBackground(color);
            var fulllight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
            Func<int, bool, Image<Gray, byte>[]> takePics = (step, vertical) =>
            {
                Image<Gray, byte>[] pics = new Image<Gray, byte>[3];
                projector.DrawBackground(Color.Black);
                camera.TakePicture(5).Dispose();
                projector.DrawPhaseMod(step, (float)(-2.0f * Math.PI / 3.0f), vertical, color);
                pics[0] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
                projector.DrawBackground(Color.Black);
                camera.TakePicture(5).Dispose();
                projector.DrawPhaseMod(step, 0f, vertical, color);
                pics[1] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
                projector.DrawBackground(Color.Black);
                camera.TakePicture(5).Dispose();
                projector.DrawPhaseMod(step, (float)(2.0f * Math.PI / 3.0f), vertical, color);
                pics[2] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
                return pics;
            };
            Func<Image<Gray, byte>[], double[]> itop = (pics) =>
            {
                return cameraCorners.Select(c =>
                {
                    var mini = nolight[(int)c.Y, (int)c.X].Intensity;
                    var maxi = fulllight[(int)c.Y, (int)c.X].Intensity;
                    var i1 = pics[0][(int)c.Y, (int)c.X].Intensity;
                    i1 = (i1 - mini) / (maxi - mini);
                    var i2 = pics[1][(int)c.Y, (int)c.X].Intensity;
                    i2 = (i2 - mini) / (maxi - mini);
                    var i3 = pics[2][(int)c.Y, (int)c.X].Intensity;
                    i3 = (i3 - mini) / (maxi - mini);
                    return PhaseModulation.IntensityToPhase(i1, i2, i3);
                }).ToArray();
            };

            int w = projector.Size.Width;
            int h = projector.Size.Height;
            var xphs = itop(takePics(subdiv, true));
            var yphs = itop(takePics(subdiv, false));

            var ids = new int[cameraCorners.Length];
            int idx = 0;
            ids = ids.Select(i => idx++).ToArray();

            return ids.Select(i =>
                {
                    var v = rough[i];
                    var denom = (double)subdiv;
                    var xph = xphs[i] * (w / denom);
                    var yph = yphs[i] * (h / denom);
                    var phsx = Math.Floor(v.X / denom) * denom;
                    var phsy = Math.Floor(v.Y / denom) * denom;
                    double apx = v.X / w;
                    double phx = xphs[i];

                    apx = Math.Floor(apx / (1.0 / denom)) * (1.0 / denom) + (phx < 1 ? phx / denom : 0);
                    apx = Math.Round(apx, 5) * w;

                    double apy = v.Y / h;
                    double phy = yphs[i];

                    apy = Math.Floor(apy / (1.0 / denom)) * (1.0 / denom) + (phy < 1 ? phy / denom : 0);
                    apy = Math.Round(apy, 5) * h;

                    return new PointF((float)(apx), (float)(apy));

                }).ToArray();
        }
Esempio n. 6
0
        public static PointF[] PhaseCalib(Projector projector, Camera camera, PointF[] cameraCorners, int steps = 7)
        {
            var color = Color.Green;
            projector.DrawBackground(Color.Black);
            var nolight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];

            projector.DrawBackground(color);
            var fulllight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
            Func<int, bool, Image<Gray, byte>[]> takePics = (step, vertical) =>
            {
                Image<Gray, byte>[] pics = new Image<Gray, byte>[3];
                projector.DrawBackground(Color.Black);
                camera.TakePicture(5).Dispose();
                projector.DrawPhaseMod(step, (float)(-2.0f * Math.PI / 3.0f), vertical, color);
                pics[0] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
                projector.DrawBackground(Color.Black);
                camera.TakePicture(5).Dispose();
                projector.DrawPhaseMod(step, 0f, vertical, color);
                pics[1] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
                projector.DrawBackground(Color.Black);
                camera.TakePicture(5).Dispose();
                projector.DrawPhaseMod(step, (float)(2.0f * Math.PI / 3.0f), vertical, color);
                pics[2] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
                return pics;
            };
            Func<Image<Gray, byte>[], double[]> itop = (pics) =>
            {
                return cameraCorners.Select(c =>
                {
                    var mini = nolight[(int)c.Y, (int)c.X].Intensity;
                    var maxi = fulllight[(int)c.Y, (int)c.X].Intensity;
                    var i1 = pics[0][(int)c.Y, (int)c.X].Intensity;
                    i1 = (i1 - mini) / (i1 - maxi);
                    var i2 = pics[1][(int)c.Y, (int)c.X].Intensity;
                    i2 = (i2 - mini) / (i2 - maxi);
                    var i3 = pics[2][(int)c.Y, (int)c.X].Intensity;
                    i3 = (i3 - mini) / (i3 - maxi);
                    return PhaseModulation.IntensityToPhase(i1, i2, i3);
                }).ToArray();
            };

            int w = projector.Size.Width;
            int h = projector.Size.Height;
            int subdiv = 1;
            int[] xs = new int[cameraCorners.Length];
            int[] ys = new int[cameraCorners.Length];
            int[] ids = Range.OfInts(cameraCorners.Length).ToArray();
            for (int i = 0; i < steps; i++)
            {
                var xpics = takePics(subdiv, true);
                var xphs = itop(xpics);
                var xh = ids.Select(id => xphs[id] > 0.5).ToArray();

                var qd = QuickDraw.Start(xpics[1].Bitmap);
                var thrash = ids.Select(id =>
                {
                    qd.Color(xh[id] ? Color.White : Color.Gray);
                    qd.DrawPoint(cameraCorners[id].X, cameraCorners[id].Y, 5);
                    return id;
                }).ToArray();
                qd.Finish();
                DebugWindow.DrawBitmap(xpics[1].Bitmap);

                var ypics = takePics(subdiv, false);
                var yphs = itop(ypics);
                var yh = ids.Select(id => yphs[id] > 0.5).ToArray();

                qd = QuickDraw.Start(ypics[1].Bitmap);
                thrash = ids.Select(id =>
                {
                    qd.Color(yh[id] ? Color.White : Color.Gray);
                    qd.DrawPoint(cameraCorners[id].X, cameraCorners[id].Y, 5);
                    return id;
                }).ToArray();
                qd.Finish();
                DebugWindow.DrawBitmap(ypics[1].Bitmap);

                xs = ids.Select(id => (xs[id] << 1) | (xh[id] ? 1 : 0)).ToArray();
                ys = ids.Select(id => (ys[id] << 1) | (yh[id] ? 1 : 0)).ToArray();

                subdiv = subdiv << 1;
            }
            var fxs = ids.Select(id => ((double)xs[id] / (double)subdiv) * w).ToArray();
            var fys = ids.Select(id => ((double)ys[id] / (double)subdiv) * h).ToArray();
            return fxs.Zip(fys, (x,y) => new PointF((float)x,(float)y)).ToArray();
        }
Esempio n. 7
0
 public static Func<PointF[], PointF[]> FindHomography(PointF[] ccs, PointF[] pcs)
 {
     var hg = Emgu.CV.CameraCalibration.FindHomography(ccs, pcs, HOMOGRAPHY_METHOD.DEFAULT, 2);
     var t = ccs.Select(c => new PointF(c.X, c.Y)).ToArray();
     hg.ProjectPoints(t);
     var tes = pcs.Zip(t, (a, b) => (b.X - a.X) * (b.X - a.X) + (b.Y - a.Y) * (b.Y - a.Y)).ToArray();
     var sum = tes.Sum();
     return (ps) =>
     {
         var psc = ps.Select(p => new PointF(p.X, p.Y)).ToArray();
         hg.ProjectPoints(psc);
         return psc;
     };
 }
Esempio n. 8
0
        public static CalibrationResult CalibrateProjector(Projector projector, Camera camera, Size pattern, CalibrationResult cameraCalib, PointF[][] cacalibdata, Size cameraPattern, float checkerboardSize)
        {
            List<PointF[]> cameraCorners = new List<PointF[]>();
            List<PointF[]> projectorCorners = new List<PointF[]>();
            var cpattern = new Size(pattern.Width - 1, pattern.Height - 1);
            int steps = 15;
            double rotx = 0, roty = 0, rotz = 0;
            for (int i = 0; i < steps; i++)
            {
                var di = (double)i / (double)steps;
                rotx = 0;
                roty = Math.Sin(di * Math.PI / 2) * 0.8;
                rotz = Math.Sin(di * Math.PI / 2) * 0.6;
                var pcs = projector.DrawCheckerboard(pattern,
                    rotx,
                    roty,
                    rotz, 0.5);
                var img = camera.TakePicture(3);
                var ccs = GetCameraCorners(img, cpattern, false);
                if (ccs != null)
                {

                    projectorCorners.Add(pcs);
                    cameraCorners.Add(ccs);
                    var withCorners = camera.TakePicture(0);
                    if (DebugWindow != null)
                    {
                        QuickDraw.Start(withCorners)
                            .Color(Color.White)
                            .DrawPoint(ccs, 5)
                            .Finish();
                        DebugWindow.DrawBitmap(withCorners);
                    }
                }
            }
            //for (int i = 0; i < steps; i++)
            //{
            //    var di = (double)i / (double)steps;
            //    rotx += 0.04;
            //    roty *= 0.90;
            //    var pcs = projector.DrawCheckerboard(pattern,
            //        rotx,
            //        roty,
            //        rotz, 0.7);
            //    var ccs = GetCameraCorners(camera, cpattern, false);
            //    if (ccs != null)
            //    {

            //        projectorCorners.Add(pcs);
            //        cameraCorners.Add(ccs);
            //        var withCorners = camera.TakePicture(0);
            //        if (DebugWindow != null)
            //        {
            //            QuickDraw.Start(withCorners)
            //                .Color(Color.White)
            //                .DrawPoint(ccs, 5)
            //                .Finish();
            //            DebugWindow.DrawBitmap(withCorners);
            //        }
            //    }
            //}

            var hm = CreateHomography(cameraCorners.ToArray(), projectorCorners.ToArray());
            var globals = GenerateCheckerBoard(cameraPattern, checkerboardSize, 0);
            var globalCorners = cacalibdata.Select(row => globals).ToArray();
            var proj = cacalibdata.Select(cs => cs.Select(c => new PointF(c.X, c.Y)).ToArray()).ToArray();
            foreach (var p in proj)
                hm.ProjectPoints(p);
            IntrinsicCameraParameters projIntrin = new IntrinsicCameraParameters();
            ExtrinsicCameraParameters[] projExtrins;
            Emgu.CV.CameraCalibration.CalibrateCamera(globalCorners, proj, projector.Size, projIntrin, Emgu.CV.CvEnum.CALIB_TYPE.CV_CALIB_RATIONAL_MODEL, out projExtrins);

            return new CalibrationResult() { Intrinsic = projIntrin, Extrinsic = projExtrins.First() };
        }
Esempio n. 9
0
        public static CalibrationResult CalibrateProjector(Projector projector, PointF[][] cacalibdata, PointF[][] cameraCorners, PointF[][] projectorCorners, Size cameraPattern, float checkerboardSize)
        {
            var hm = CreateHomography(cameraCorners, projectorCorners);
            var globals = GenerateCheckerBoard(cameraPattern, checkerboardSize, 0);
            var globalCorners = cacalibdata.Select(row => globals).ToArray();
            var proj = cacalibdata.Select(cs => cs.Select(c => new PointF(c.X, c.Y)).ToArray()).ToArray();
            foreach (var p in proj)
                hm.ProjectPoints(p);
            IntrinsicCameraParameters projIntrin = new IntrinsicCameraParameters();
            ExtrinsicCameraParameters[] projExtrins;
            Emgu.CV.CameraCalibration.CalibrateCamera(globalCorners, proj, projector.Size, projIntrin, Emgu.CV.CvEnum.CALIB_TYPE.CV_CALIB_ZERO_TANGENT_DIST | CALIB_TYPE.CV_CALIB_FIX_K3, out projExtrins);

            return new CalibrationResult() { Intrinsic = projIntrin, Extrinsic = projExtrins.First() };
        }
Esempio n. 10
0
 public static CalibrationResult CalibrateCamera(PointF[][] cameraCorners, Size cameraSize, Size pattern, float checkerBoardSize)
 {
     var globals = GenerateCheckerBoard(pattern, checkerBoardSize, 0);
     var globalCorners = cameraCorners.Select(row => globals).ToArray();
     var intrinsic = new IntrinsicCameraParameters();
     ExtrinsicCameraParameters[] cameraExtrinsicsArray;
     Emgu.CV.CameraCalibration.CalibrateCamera(globalCorners, cameraCorners, cameraSize, intrinsic, CALIB_TYPE.CV_CALIB_RATIONAL_MODEL, out cameraExtrinsicsArray);
     var extrinsic = cameraExtrinsicsArray.First();
     return new CalibrationResult() { Intrinsic = intrinsic, Extrinsic = extrinsic };
 }
Esempio n. 11
0
        public static int RenderMap(Rectangle clip, Graphics g, bool dedicated, ref float scale)
        {
            var ets2Tel = (Main.Data.Active == null)
                ? default(Ets2Telemetry)
                : ((Ets2DataMiner)Main.Data.Active).MyTelemetry;

            // Search the map
            var map = FrmMain.Ets2Map;

            g.FillRectangle(map.Loading ? Brushes.DarkOrange : Brushes.Black, clip);
            g.SmoothingMode = mapScale < 1000 ? SmoothingMode.AntiAlias : SmoothingMode.HighSpeed;
            g.InterpolationMode = InterpolationMode.NearestNeighbor;
            g.PixelOffsetMode = PixelOffsetMode.None;

            float tx = 0.0f;
            float ty = 0.0f;
            float baseScale = 0.0f;

            if (Main.Data.Active == null || (dedicated && locationOverride))
            {
                tx = location.X;
                ty = location.Y;
                baseScale = mapScale;
            }
            else
            {
                var d = GetLivePoint();
                tx = d.Item1.X;
                ty = d.Item1.Y;
                baseScale = d.Item2;
            }

            var totalX = 0.0f;
            var totalY = 0.0f;

            // Input scale value is max scale
            var maxScale = scale;
            if (baseScale > maxScale)
                baseScale = scale;
            scale = baseScale;

            if (clip.Width > clip.Height)
            {
                totalX = baseScale;
                totalY = (int)(baseScale * (float)clip.Height / clip.Width);
            }
            else
            {
                totalY = baseScale;
                totalX = (int)(baseScale * (float)clip.Width / clip.Height);
            }

            var startX = clip.X + tx - totalX;
            var endX = clip.X + tx + totalX;
            var startY = clip.Y + ty - totalY;
            var endY = clip.Y + ty + totalY;

            var scaleX = clip.Width / (endX - startX);
            var scaleY = clip.Height / (endY - startY);

            if (float.IsInfinity(scaleX) || float.IsNaN(scaleX))
                scaleX = clip.Width;
            if (float.IsInfinity(scaleY) || float.IsNaN(scaleY))
                scaleY = clip.Height;

            var nodesNearby =
                map.Nodes.Values.Where(
                    x => x.X >= startX - 1500 && x.X <= endX + 1500 && x.Z >= startY -1500 && x.Z <= endY + 1500);
            var itemsNearby = nodesNearby.SelectMany(x => x.GetItems()).Where(x => x.HideUI == false).ToList();

            var roads = itemsNearby.Where(x => x.Type == Ets2ItemType.Road);
            var prefabs = itemsNearby.Where(x => x.Type == Ets2ItemType.Prefab);

            var gpsPen = new Pen(Brushes.MediumPurple, 22*scaleX);
            var localPen = new Pen(Brushes.Orange, 7.5f * scaleX);
            var prefabLane = new Pen(Brushes.Yellow, 3 * scaleX);
            var expressPen = new Pen(Brushes.Yellow, 19 * scaleX);
            var highwayPen = new Pen(Brushes.Red, 22*scaleX);

            List<ulong> nodesPassed = new List<ulong>();
            List<List<PointF>> roadPoints = new List<List<PointF>>();
            var nodesToFollow = prefabs.SelectMany(x => x.NodesList.Values).Distinct();

            // Gather all prefabs, and issue a drawing command
            foreach (var node in nodesToFollow)
            {
                if (node == null)
                    continue;

                bool isHighway = false;
                bool isExpress = false;
                bool isLocal = false;

                // Nodes from prefab are always like:
                // Prefab = Forward
                // Road=backward
                var road = node.ForwardItem != null && node.ForwardItem.Type == Ets2ItemType.Prefab
                    ? node.BackwardItem
                    : node.ForwardItem;
                var roadStart = road;
                isHighway = road == null || road.RoadLook == null ? false : road.RoadLook.IsHighway;
                isExpress = road == null || road.RoadLook == null ? false : road.RoadLook.IsExpress;
                isLocal = road == null || road.RoadLook == null ? false : road.RoadLook.IsLocal;
                var fw = node.ForwardItem != null && node.ForwardItem.Type == Ets2ItemType.Road;

                if (road == null)
                {
                    // DEAD END
                    //Console.WriteLine("Dead-end from prefab..");
                    continue;
                }

                var roadChain = new List<Ets2Item>();

                // Start drawing at start road
                if (fw)
                {
                    do
                    {
                        roadChain.Add(road);
                        road = road.EndNode == null ? null : road.EndNode.ForwardItem;
                    } while (road != null && road.Type == Ets2ItemType.Road);
                }
                else
                {
                    do
                    {
                        roadChain.Add(road);
                        road = road.StartNode == null ? null : road.StartNode.BackwardItem;
                    } while (road != null && road.Type == Ets2ItemType.Road);
                }

                if (!fw)
                    roadChain.Reverse();

                foreach (var n in roadChain.Where(x => x.HideUI == false))
                {
                    n.GenerateRoadPolygon(64);
                }
                var pen = isHighway ? highwayPen : isExpress ? expressPen : localPen;

                var roadPoly =
                    roadChain.Where(x => x.HideUI == false)
                        .SelectMany(x => x.RoadPolygons)
                        .Select(x => new PointF((x.X - startX)*scaleX, (x.Y - startY)*scaleY));

                if (roadPoly.Any())
                {
                    g.DrawLines(pen, roadPoly.ToArray());
                }
            }

            // Draw GPS routes if any
            if (route != null && route.Segments != null && route.Segments.Any())
            {
                foreach (var seg in route.Segments)
                {
                    if (seg.Solutions.Any())
                    {
                        foreach (var opt in seg.Solutions)
                        {
                            var pt = opt.Points.Select(x => new PointF((x.X - startX) * scaleX, (x.Z - startY) * scaleY));

                            g.DrawLines(new Pen(Color.SpringGreen, 5.0f), pt.ToArray());
                        }
                    }
                    else
                    {
                        foreach (var opt in seg.Options)
                        {
                            var pt = opt.Points.Select(x => new PointF((x.X - startX) * scaleX, (x.Z - startY) * scaleY));

                            g.DrawLines(new Pen(Color.LightSkyBlue, 5.0f), pt.ToArray());
                        }
                    }
                }
            }

            if (LaneAssistance.hook != null)
            {
                var d = new PointF((LaneAssistance.hook.X - startX) * scaleX, (LaneAssistance.hook.Z - startY) * scaleY);
                g.FillEllipse(Brushes.GreenYellow, d.X - 5, d.Y - 5, 10, 10);
                var d2 = new PointF(d.X + (float)Math.Sin(LaneAssistance.yawRoad) * 25, d.Y + (float)Math.Cos(LaneAssistance.yawRoad) * 25);
                g.DrawLine(new Pen(Color.GreenYellow, 3.0f), d, d2);
            }
            if (LaneAssistance.lookPoint != null)
            {
                var d = new PointF((LaneAssistance.lookPoint.X - startX) * scaleX, (LaneAssistance.lookPoint.Y - startY) * scaleY);
                g.FillEllipse(Brushes.Pink, d.X - 5, d.Y - 5, 10, 10);
            }
            // Cities?
            var cityFont = new Font("Arial", 10.0f);
            foreach (var cities in itemsNearby.Where(x => x.Type == Ets2ItemType.City && x.StartNode!=null))
            {
                var centerX = cities.StartNode.X;
                var centerY = cities.StartNode.Z;

                var mapX = (centerX - startX)*scaleX;
                var mapY = (centerY - startY)*scaleY;
                //
                g.DrawString(cities.City, cityFont, Brushes.White, mapX,mapY);
            }

            // Draw all prefab curves
            foreach (var prefabItem in prefabs.Where(x => x.Prefab != null && x.HideUI == false).Distinct())
            {
                if (prefabItem.Prefab.Company != null)
                {
                    var nx = prefabItem.NodesList.FirstOrDefault().Value.X;
                    var ny = prefabItem.NodesList.FirstOrDefault().Value.Z;

                    var companyRect = new PointF[]
                    {
                        new PointF(nx - prefabItem.Prefab.Company.MinX, ny - prefabItem.Prefab.Company.MinY),
                        new PointF(nx - prefabItem.Prefab.Company.MinX, ny + prefabItem.Prefab.Company.MaxY),
                        new PointF(nx + prefabItem.Prefab.Company.MaxX, ny + prefabItem.Prefab.Company.MaxY),
                        new PointF(nx + prefabItem.Prefab.Company.MaxX, ny - prefabItem.Prefab.Company.MinY),
                    };

                    var offsetPoly = companyRect.Select(x => new PointF((x.X - startX) * scaleX, (x.Y - startY) * scaleY)).ToArray();

                    //g.FillPolygon(Brushes.Orange, offsetPoly);
                }
                else
                {
                    // Then it's likely a road prefab.
                    //prefab.Origin = 0;
                    // TODO: find origin
                    var originNode = prefabItem.NodesList.FirstOrDefault().Value;
                    if (originNode != null)
                    {
                        foreach (
                            var poly in
                                prefabItem.Prefab.GeneratePolygonCurves(originNode, prefabItem.Origin))
                        {
                            var offsetPoly = poly.Select(x => new PointF((x.X - startX) * scaleX, (x.Y - startY) * scaleY)).ToArray();

                            var p = new Pen(prefabLane.Color, 1.0f);
                            g.DrawLines(p, offsetPoly);
                        }
                    }
                }
            }

            var rotation = ets2Tel == null ? 0 : ets2Tel.Physics.RotationX*2*Math.PI;

            //g.FillEllipse(Brushes.Turquoise, scaleX*totalX-5, scaleY*totalY-5,10,10);
            g.DrawLine(new Pen(Brushes.Cyan, 5.0f), scaleX*totalX + (float) Math.Sin(rotation)*localPen.Width*2,
                scaleY*totalY + (float) Math.Cos(rotation)*localPen.Width*2, scaleX*totalX, scaleY*totalY);

            if (dedicated && ProcessDoubleClick)
            {
                ProcessDoubleClick = false;

                // Calculate the coordinate
                var clkX = 2 * (-0.5f + DoubleClickPoint.X / (float)clip.Width) * totalX + tx;
                var clkY = 2 * (-0.5f + DoubleClickPoint.Y / (float)clip.Height) * totalY + ty;

                var currentLocation = GetLivePoint();
                // Navigate to this point
                route = map.NavigateTo(currentLocation.Item1, new PointF(clkX, clkY));
            }

            // At 50 speed: 20fps
            // At 100 speed: 2fps
            var fps = 15.0f;
            if (ets2Tel != null && ets2Tel.Drivetrain.SpeedKmh > 50)
                fps -= (ets2Tel.Drivetrain.SpeedKmh - 50) * 0.43f; // 2.777spd = -1fps
            if (fps < 2) fps = 2;

            var interval = 1000.0f/fps;

            return (int) interval;
        }
Esempio n. 12
0
 public OctavePlot(PointF[] points)
 {
     dataX = Embrace(String.Join(",", points.Select(d => d.X.ToString(cul))));
     dataY = Embrace(String.Join(",", points.Select(d => d.Y.ToString(cul))));
 }