示例#1
0
        /*Check that the returned gradient is equal to the numerical gradient*/
        public void GradientMatchNumerical()
        {
            //
            var v = Vector <double> .Build.Dense(3);

            //
            ScanMatching.rEBb = Matrix <double> .Build.Random(2, 50);

            ScanMatching.rPNn = Matrix <double> .Build.Random(2, 15);

            var hp = new HelperFunctions();
            Func <double[], double> f = x => hp.ExtractCost(x);

            var derivEst   = new NumericalJacobian(5, 2);
            var gradExpect = derivEst.Evaluate(f, v.ToArray());

            (var ftmp, var gradActual, var Htmp) = ScanMatching.Cost(v);



            // Not sure how to do assert array equal with precision.
            for (int i = 0; i < gradExpect.Length; i++)
            {
                Assert.Equal(gradExpect[i], gradActual.ToArray()[i], 5);
            }
        }
示例#2
0
        /*The following scenarios are contrived such that the point cloud locations are mirrored and should produce the same cost*/
        public void EqualCostScenarios()
        {
            // Initialise point clouds
            double[] x1 = { 0, 0, 0, 1 };
            double[] x2 = { 0, -0.5, 0, 0.5 };

            Matrix <double> PointCloud1 = Matrix <double> .Build.DenseOfColumnMajor(2, 2, x1);

            Matrix <double> PointCloud2 = Matrix <double> .Build.DenseOfColumnMajor(2, 2, x2);

            ScanMatching.rEBb = PointCloud2;
            ScanMatching.rPNn = PointCloud1;

            // Generate two pose vectors (x,y,psi)
            double[]        vv1 = { -0.25, 0.5, -0.1 };
            Vector <double> v1  = Vector <double> .Build.Dense(vv1);

            double[]        vv2 = { 0.25, 0.5, 0.1 };
            Vector <double> v2  = Vector <double> .Build.DenseOfArray(vv2);

            (double expected, var G1, var H1) = ScanMatching.Cost(v2);
            (double actual, var G2, var H2)   = ScanMatching.Cost(v1);

            /*Precision is given as an integer defining the number of decimal places,
             * Normal practice would be to select precision based on machine precision
             * E.g., sqrt(eps) where eps is machine precision, sqrt(eps) = 8.5e-8 on my machine.
             * Therefore, 8 decimal places will suffice
             */
            Assert.Equal(expected, actual, 8);
        }
示例#3
0
        public void CloserPointsImproveCost()
        {
            // Initialise point clouds
            double[] x1 = { 0, 1 };
            double[] x2 = { 0, 2 };

            Matrix <double> PointCloud1 = Matrix <double> .Build.DenseOfColumnMajor(2, 1, x1);

            Matrix <double> PointCloud2 = Matrix <double> .Build.DenseOfColumnMajor(2, 1, x2);

            ScanMatching.rEBb = PointCloud1;
            ScanMatching.rPNn = PointCloud2;

            // Generate two pose vectors (x,y,psi)
            Vector <double> v1 = Vector <double> .Build.Dense(3);//This should yield a higher cost than

            double[]        vv = { 0, 1, 0 };
            Vector <double> v2 = Vector <double> .Build.DenseOfArray(vv);//this one

            (double CostExpectLow, var G1, var H1)  = ScanMatching.Cost(v2);
            (double CostExpectHigh, var G2, var H2) = ScanMatching.Cost(v1);

            // Since the points lie on top of one another when pose = {0 1 0} that should yield a lower cost


            Assert.True(CostExpectLow < CostExpectHigh, "Expected closer points to yield a lower cost, something is fundamentally wrong with the cost function");
        }
示例#4
0
        public double ExtractCost(double[] v)
        {
            var vv = Vector <double> .Build.DenseOfArray(v);

            (var f, _, _) = ScanMatching.Cost(vv);

            return(f);
        }
    public MainWindow() : base(Gtk.WindowType.Toplevel)
    {
        Build();

        var plotView = new PlotView();

        this.Add(plotView);
        plotView.ShowAll();

        // Choose file
        Gtk.FileChooserDialog fcd = new Gtk.FileChooserDialog("Open File", null, Gtk.FileChooserAction.Open);
        fcd.AddButton(Gtk.Stock.Cancel, Gtk.ResponseType.Cancel);
        fcd.AddButton(Gtk.Stock.Open, Gtk.ResponseType.Ok);
        fcd.DefaultResponse = Gtk.ResponseType.Ok;
        fcd.SelectMultiple  = false;
        Gtk.ResponseType response = (Gtk.ResponseType)fcd.Run();

        String DataFile = fcd.Filename;

        if (response == ResponseType.Ok)
        {
            // Run scan Matching
            ScanMatching.RunScanMatch(DataFile);
            Console.WriteLine($"{DataFile} selected");
        }
        else
        {
            Console.WriteLine("no file selected");
        }

        fcd.Destroy();

        // Run Plot
        var model = new PlotModel {
            Title = "Scan Matching"
        };
        var scatterSeries = new ScatterSeries {
            MarkerType = MarkerType.Circle
        };
        var r = new Random(314);

        for (int i = 0; i < D_Scan_Matching_GUI.ScanMatching.rPNn.ColumnCount; i++)
        {
            var x          = D_Scan_Matching_GUI.ScanMatching.rPNn[0, i];
            var y          = D_Scan_Matching_GUI.ScanMatching.rPNn[1, i];
            var size       = 5;
            var colorValue = 1500;
            scatterSeries.Points.Add(new ScatterPoint(x, y, size, colorValue));
        }

        model.Series.Add(scatterSeries);
        model.Axes.Add(new LinearColorAxis {
            Position = AxisPosition.Right, Palette = OxyPalettes.Jet(200)
        });
        plotView.Model = model;
    }
示例#6
0
        public Tuple <double, Vector <double> > ExtractCostAndGrad(Vector <double> v)
        {
            (var f, var g, _) = ScanMatching.Cost(v);

            return(Tuple.Create(f, g));
        }