示例#1
0
        /*
         * Input:
         *  Data file name
         * Output:
         *  none
         *
         *  Parse input data and loops over scans, performing scan matching
         */
        public static void RunScanMatch(String inpF)
        {
            //Call Init
            Init(inpF);
            //Initialise Optimisation routine
            var obj    = ObjectiveFunction.GradientHessian(Cost);
            var solver = new ConjugateGradientMinimizer(1e0, 30); //(1e-5, 100, false);

            Vector <double> x_init = Vector <double> .Build.DenseOfArray(new double[] { 0.0, 0.0, 0.0 });

            Vector <double> Xopt = Vector <double> .Build.DenseOfArray(new double[] { 0.0, 0.0, 0.0 });

            //Initialise reference point cloud, expressed in map coordinates. rPNn
            var rBNn = Vector <double> .Build.Dense(2);

            rBNn[0] = Pose[0][0];
            rBNn[1] = Pose[0][1];
            var Rnb = SO2.EulerRotation(Pose[0][2]);

            rEBb = GetPointCloudFromRange(Range[0]);
            var rPNn_new = Matrix <double> .Build.DenseOfMatrix(rEBb);

            for (int j = 0; j < rPNn_new.ColumnCount; j++)
            {
                rPNn_new.SetColumn(j, rBNn.Add(Rnb.Multiply(rEBb.Column(j))));
            }

            rPNn = rPNn_new;

            //Loop through data, setting up and running optimisation routine each time.
            for (int i = 1; i < Range.Count(); i++)
            {
                //Initialise independent point cloud, expressed in body coordinates.rPBb
                rEBb = GetPointCloudFromRange(Range[i]);
                //Set up initial conditions
                x_init.SetValues(Pose[i]);

                //Solve
                var result = solver.FindMinimum(obj, x_init);
                Xopt = result.MinimizingPoint;
                rBNn = Vector <double> .Build.Dense(2);

                rBNn[0] = Xopt[0];
                rBNn[1] = Xopt[1];
                Rnb     = SO2.EulerRotation(Xopt[2]);

                //Append to PointCloud
                rPNn_new = Matrix <double> .Build.DenseOfMatrix(rEBb);

                for (int j = 0; j < rPNn_new.ColumnCount; j++)
                {
                    rPNn_new.SetColumn(j, rBNn.Add(Rnb.Multiply(rEBb.Column(j))));
                }
                rPNn = rPNn.Append(rPNn_new);
            }
        }
示例#2
0
        /*
         * Cost Function:
         * Input:
         *  v = (x,y,psi)
         * Output:
         *  cost     f (double)
         *  gradient g (vector)
         *  hessian  h (matrix)
         */
        public static Tuple <double, Vector <double>, Matrix <double> > Cost(Vector <double> v)
        {
            double f      = 0;
            var    NewVec = Vector <double> .Build;
            var    NewMat = Matrix <double> .Build;

            var g = NewVec.Dense(3);
            var h = NewMat.Dense(3, 3);



            var rBNn = v.SubVector(0, 2);
            var rEBn = SO2.EulerRotation(v.At(2)).Multiply(rEBb);
            var dRnb = SO2.Derivative(v.At(2));
            var rENn = NewMat.Dense(2, rEBn.ColumnCount);

            for (int i = 0; i < rENn.ColumnCount; i++)
            {
                rENn.SetColumn(i, rBNn.Add(rEBn.Column(i)));
            }

            double[] Je_tmp = { 1, 0, 0, 1, 0, 0 };
            var      Je     = NewMat.DenseOfColumnMajor(2, 3, Je_tmp);

            //Get point cloud data.
            for (int i = 0; i < rEBb.ColumnCount; i++)
            {
                //TODO: inner forloop can be vectorised.
                for (int j = 0; j < rPNn.ColumnCount; j++)
                {
                    //Calculate error
                    var e = rENn.Column(i).Subtract(rPNn.Column(j));
                    //Calculate residual
                    double r = Math.Exp(-0.25 / (Sigma * Sigma) * e.DotProduct(e));

                    //Calculate Jacobian of the exponent
                    Je.SetColumn(2, dRnb.Multiply(rEBb.Column(i)));
                    //Calculate Jacobian of the residual
                    var J = Je.TransposeThisAndMultiply(e).Multiply(-0.5 / (Sigma * Sigma) * r);

                    //Calculate cost
                    f -= r * r;
                    //Calculate the exact gradient and appproximate hessian
                    g.Subtract(J.Multiply(r), g);     // J'*r
                    h.Subtract(J.OuterProduct(J), h); //J'*J (Gauss-Newton approximation)
                }
            }
            //h = Matrix<double>.Build.DenseIdentity(3);
            return(Tuple.Create(0.5 * f / rPNn.ColumnCount, g.Divide((double)rPNn.ColumnCount), h.Divide((double)rPNn.ColumnCount)));
        }