/* * 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); } }
/* * 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))); }