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