public void OptimiserFindMinimum() { double[] x1 = { 0, 1, 2, 1, 0, -1, 2, 0 }; double[] x2 = { 0, 0.5, 2, 0.5, 0, -1.5, 2, -0.5 }; ScanMatching.rEBb = Matrix <double> .Build.DenseOfColumnMajor(2, 1, x2); ScanMatching.rPNn = Matrix <double> .Build.DenseOfColumnMajor(2, 1, x1); var obj = ObjectiveFunction.GradientHessian(ScanMatching.Cost); var solver = new ConjugateGradientMinimizer(1e-12, 10000); //(1e-5, 100, false); Vector <double> x_init = Vector <double> .Build.DenseOfArray(new double[] { 0.01, 0.45, 0.00 }); var result = solver.FindMinimum(obj, x_init); double[] Expected = { 0, 0.5, 0 }; var Actual = result.MinimizingPoint; // Not sure how to do assert array equal with precision. for (int i = 0; i < Expected.Length; i++) { Assert.Equal(Expected[i], Actual[i], 2); } //Assert.Equal(result.ReasonForExit.ToString(), "foo"); //Assert.Equal(Expected, Actual.ToArray()); }
/// <summary> /// Find vector x that minimizes the function f(x) using the Newton algorithm. /// For more options and diagnostics consider to use <see cref="NewtonMinimizer"/> directly. /// </summary> public static Vector <double> OfFunctionGradientHessian(Func <Vector <double>, Tuple <double, Vector <double>, Matrix <double> > > functionGradientHessian, Vector <double> initialGuess, double gradientTolerance = 1e-8, int maxIterations = 1000) { var objective = ObjectiveFunction.GradientHessian(functionGradientHessian); var result = NewtonMinimizer.Minimum(objective, initialGuess, gradientTolerance, maxIterations); return(result.MinimizingPoint); }
public void FindMinimum_Rosenbrock_Easy() { var obj = ObjectiveFunction.GradientHessian(RosenbrockFunction.Value, RosenbrockFunction.Gradient, RosenbrockFunction.Hessian); var solver = new NewtonMinimizer(1e-5, 1000); var result = solver.FindMinimum(obj, new DenseVector(new[] { 1.2, 1.2 })); Assert.That(Math.Abs(result.MinimizingPoint[0] - 1.0), Is.LessThan(1e-3)); Assert.That(Math.Abs(result.MinimizingPoint[1] - 1.0), Is.LessThan(1e-3)); }
public void FindMinimum_Rosenbrock_Hard() { var obj = ObjectiveFunction.GradientHessian(point => Tuple.Create(RosenbrockFunction.Value(point), RosenbrockFunction.Gradient(point), RosenbrockFunction.Hessian(point))); var solver = new NewtonMinimizer(1e-5, 1000); var result = solver.FindMinimum(obj, new DenseVector(new[] { -1.2, 1.0 })); Assert.That(Math.Abs(result.MinimizingPoint[0] - 1.0), Is.LessThan(1e-3)); Assert.That(Math.Abs(result.MinimizingPoint[1] - 1.0), Is.LessThan(1e-3)); }
/* * 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); } }