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