/// <summary>
/// align2 transform calculation, todo to seperate scale parameter in each dimension
/// </summary>
/// <param name="origin"></param>
/// <param name="target"></param>
/// <returns></returns>
public static Tuple <Matrix <double>, Vector <double>, Vector <double> > Align2(List <Vector <double> > origin, List <Vector <double> > target)
{
//convert to m x n matrix, m - dimension, n - points count
var x = DenseMatrix.OfColumnVectors(origin);
var y = DenseMatrix.OfColumnVectors(target);
//mean
var mx = x.RowSums() / x.ColumnCount;
var my = y.RowSums() / y.ColumnCount;
//centered matrix
var xc = x - DenseMatrix.OfColumnVectors(Enumerable.Repeat(mx, x.ColumnCount));
var yc = y - DenseMatrix.OfColumnVectors(Enumerable.Repeat(my, x.ColumnCount));
//scale
var sx = (xc.PointwisePower(2)).RowNorms(1) / xc.ColumnCount;
var sy = (yc.PointwisePower(2)).RowNorms(1) / yc.ColumnCount;
var Sxy = yc * xc.Transpose() / xc.ColumnCount;
var svd = Sxy.Svd();
var U = svd.U;
var D = svd.W;
var VT = svd.VT;
var rankSxy = Sxy.Rank();
var detSxy = Sxy.Determinant();
var S = DiagonalMatrix.CreateIdentity(x.RowCount);
if (rankSxy > x.ColumnCount - 1)
{
if (detSxy < 0)
{
S[x.RowCount, x.RowCount] = -1;
}
else if (rankSxy == x.RowCount - 1)
{
if (U.Determinant() * VT.Determinant() < 0)
{
S[x.RowCount, x.RowCount] = -1;
}
}
else
{
var r = DiagonalMatrix.CreateIdentity(2);
var c = DenseVector.OfArray(new[] { 1d, 1d });
var t = DenseVector.OfArray(new[] { 0d, 0d });
return(new Tuple <Matrix <double>, Vector <double>, Vector <double> >(r, c, t));
}
}
var R = U * S * VT;
var C = (D.Diagonal() * S) / sx;
var Cinv = (D.Diagonal() * S) / sy;
var T = my - C * R * mx;
return(new Tuple <Matrix <double>, Vector <double>, Vector <double> >(R, C, T));
}
public void DiagonalDenseMatrixMultiplication_IssueCP5706()
{
Matrix <double> diagonal = DiagonalMatrix.CreateIdentity(3);
Matrix <double> dense = DenseMatrix.OfArray(new double[, ] {
{ 1, 2, 3 }, { 1, 2, 3 }, { 1, 2, 3 }
});
var test = diagonal * dense;
var test2 = dense * diagonal;
}
public void CanCreateIdentity()
{
var matrix = DiagonalMatrix.CreateIdentity(5);
for (var i = 0; i < matrix.RowCount; i++)
{
for (var j = 0; j < matrix.ColumnCount; j++)
{
Assert.AreEqual(i == j ? Complex.One : Complex.Zero, matrix[i, j]);
}
}
}
public void IdentityWithWrongOrderThrowsArgumentOutOfRangeException(int order)
{
Assert.That(() => DiagonalMatrix.CreateIdentity(order), Throws.TypeOf <ArgumentOutOfRangeException>());
}
/// <summary>
/// calc transform for single scale parameter
/// </summary>
/// <param name="origin"></param>
/// <param name="target"></param>
/// <returns></returns>
public static Tuple <Matrix <double>, double, Vector <double>, List <double> > Align(List <Vector <double> > origin, List <Vector <double> > target)
{
var x = DenseMatrix.OfColumnVectors(origin);
var y = DenseMatrix.OfColumnVectors(target);
var mx = x.RowSums() / x.ColumnCount;
var my = y.RowSums() / y.ColumnCount;
var xc = x - DenseMatrix.OfColumnVectors(Enumerable.Repeat(mx, x.ColumnCount));
var yc = y - DenseMatrix.OfColumnVectors(Enumerable.Repeat(my, x.ColumnCount));
var sx = (xc.PointwisePower(2)).RowNorms(1).Sum() / xc.ColumnCount;
var sy = (yc.PointwisePower(2)).RowNorms(1).Sum() / yc.ColumnCount;
var Sxy = yc * xc.Transpose() / xc.ColumnCount;
var svd = Sxy.Svd();
var U = svd.U;
var D = svd.W;
var VT = svd.VT;
var rankSxy = Sxy.Rank();
var detSxy = Sxy.Determinant();
var S = DiagonalMatrix.CreateIdentity(x.RowCount);
if (rankSxy > x.ColumnCount - 1)
{
if (detSxy < 0)
{
S[x.RowCount, x.RowCount] = -1;
}
else if (rankSxy == x.RowCount - 1)
{
if (U.Determinant() * VT.Determinant() < 0)
{
S[x.RowCount, x.RowCount] = -1;
}
}
else
{
var r = DiagonalMatrix.CreateIdentity(2);
var c = 1;
var t = DenseVector.OfArray(new[] { 0d, 0d });
return(new Tuple <Matrix <double>, double, Vector <double>, List <double> >(r, c, t, new List <double>()
{
0d
}));
}
}
var R = U * S * VT;
var C = (D * S).Trace() / sx;
var T = my - C * R * mx;
//calculate errors
List <double> errors = new List <double>();
for (int i = 0; i < origin.Count; i++)
{
errors.Add((target[i] - (C * R * origin[i] + T)).PointwisePower(2).Norm(1));
}
return(new Tuple <Matrix <double>, double, Vector <double>, List <double> >(R, C, T, errors));
}
static void Main(string[] args)
{
Matrix a = DenseMatrix.OfArray(new double[, ] {
{ 1, 2, 3, 4 },
{ 2, 4, 4, 2 },
{ 8, 6, 4, 1 },
{ 0, 0, 0, 1 }
});
RayTracerLib.Tuple t = new RayTracerLib.Tuple(1, 2, 3);
RayTracerLib.Tuple r = new RayTracerLib.Tuple(18, 24, 33);
RayTracerLib.Tuple at = MatrixOps.MatrixXTuple(a, t);
if (at.Equals(r))
{
System.Console.WriteLine("All is good");
}
else
{
System.Console.WriteLine("not so boss");
}
Matrix a2 = DenseMatrix.OfArray(new double[, ] {
{ 1, 2, 6 },
{ -5, 8, -4 },
{ 2, 6, 4 }
});
Matrix a3 = DenseMatrix.OfArray(new double[, ] {
{ -2, -8, 3, 5 },
{ -3, 1, 7, 3 },
{ 1, 2, -9, 6 },
{ -6, 7, 7, -9 }
});
Matrix ai = DenseMatrix.OfArray(new double[, ] {
{ -5, 2, 6, -8 },
{ 1, -5, 1, 8 },
{ 7, 7, -6, -7 },
{ 1, -3, 7, 4 }
});
Matrix ia = DenseMatrix.OfArray(new double[, ] {
{ 0.21805, 0.45113, 0.24060, -0.04511 },
{ -0.80827, -1.45677, -0.44361, 0.52068 },
{ -0.07895, -0.22368, -0.05263, 0.19737 },
{ -0.52256, -0.81391, -0.30075, 0.30639 }
});
Console.WriteLine(a2.Determinant());
Console.WriteLine(a3.Determinant());
Console.WriteLine(MatrixOps.Cofactor(a3, 0, 3));
Console.WriteLine(ai.ToString());
Console.WriteLine(ai.Inverse().ToString());
Console.WriteLine(ia.ToString());
Console.WriteLine("~~~~~~~~~~~~~~~~~~~");
Matrix a4 = DenseMatrix.OfArray(new double[, ] {
{ 3, -9, 7, 3 },
{ 3, -8, 2, -9 },
{ -4, 4, 4, 1 },
{ -6, 5, -1, 1 }
});
Matrix b4 = DenseMatrix.OfArray(new double[, ] {
{ 8, 2, 2, 2 },
{ 3, -1, 7, 0 },
{ 7, 0, 5, 4 },
{ 6, -2, 0, 5 }
});
Matrix c4 = (Matrix)(a4 * b4);
Matrix d4 = (Matrix)b4.Inverse();
Matrix e4 = (Matrix)(c4 * b4.Inverse());
Console.WriteLine(c4.ToString());
Console.WriteLine(d4.ToString());
Console.WriteLine(e4.ToString());
Console.WriteLine(a4.ToString());
Console.WriteLine("~~~~~~~~~~~~~~~~~~~");
Matrix i = DiagonalMatrix.CreateIdentity(4);
Console.WriteLine(i.Inverse().ToString());
Console.WriteLine((b4 * d4).ToString());
Console.WriteLine(a4.Transpose().Inverse().ToString());
Console.WriteLine(a4.Inverse().Transpose().ToString());
Console.Write("Press Enter to finish ... ");
Console.Read();
}
public MinimizationOutput FindMinimum(IObjectiveFunction objective, Vector <double> lower_bound, Vector <double> upper_bound, Vector <double> initial_guess)
{
if (!objective.GradientSupported)
{
throw new IncompatibleObjectiveException("Gradient not supported in objective function, but required for BFGS minimization.");
}
if (!(objective is ObjectiveChecker))
{
objective = new ObjectiveChecker(objective, this.ValidateObjective, this.ValidateGradient, null);
}
// Check that dimensions match
if (lower_bound.Count != upper_bound.Count || lower_bound.Count != initial_guess.Count)
{
throw new ArgumentException("Dimensions of bounds and/or initial guess do not match.");
}
// Check that initial guess is feasible
for (int ii = 0; ii < initial_guess.Count; ++ii)
{
if (initial_guess[ii] < lower_bound[ii] || initial_guess[ii] > upper_bound[ii])
{
throw new ArgumentException("Initial guess is not in the feasible region");
}
}
IEvaluation initial_eval = objective.Evaluate(initial_guess);
// Check that we're not already done
ExitCondition current_exit_condition = this.ExitCriteriaSatisfied(initial_eval, null, lower_bound, upper_bound, 0);
if (current_exit_condition != ExitCondition.None)
{
return(new MinimizationOutput(initial_eval, 0, current_exit_condition));
}
// Set up line search algorithm
var line_searcher = new StrongWolfeLineSearch(1e-4, 0.9, Math.Max(this.ParameterTolerance, 1e-5), max_iterations: 1000);
// Declare state variables
IEvaluation candidate_point, previous_point;
double step_size;
Vector <double> gradient, step, line_search_direction, reduced_solution1, reduced_gradient, reduced_initial_point, reduced_cauchy_point, solution1;
Matrix <double> pseudo_hessian, reduced_hessian;
List <int> reduced_map;
// First step
pseudo_hessian = DiagonalMatrix.CreateIdentity(initial_guess.Count);
// Determine active set
var gradient_projection_result = QuadraticGradientProjectionSearch.search(initial_eval.Point, initial_eval.Gradient, pseudo_hessian, lower_bound, upper_bound);
var cauchy_point = gradient_projection_result.Item1;
var fixed_count = gradient_projection_result.Item2;
var is_fixed = gradient_projection_result.Item3;
var free_count = lower_bound.Count - fixed_count;
if (free_count > 0)
{
reduced_gradient = new DenseVector(free_count);
reduced_hessian = new DenseMatrix(free_count, free_count);
reduced_map = new List <int>(free_count);
reduced_initial_point = new DenseVector(free_count);
reduced_cauchy_point = new DenseVector(free_count);
CreateReducedData(initial_eval.Point, cauchy_point, is_fixed, lower_bound, upper_bound, initial_eval.Gradient, pseudo_hessian, reduced_initial_point, reduced_cauchy_point, reduced_gradient, reduced_hessian, reduced_map);
// Determine search direction and maximum step size
reduced_solution1 = reduced_initial_point + reduced_hessian.Cholesky().Solve(-reduced_gradient);
solution1 = reduced_to_full(reduced_map, reduced_solution1, cauchy_point);
}
else
{
solution1 = cauchy_point;
}
var direction_from_cauchy = solution1 - cauchy_point;
var max_step_from_cauchy_point = FindMaxStep(cauchy_point, direction_from_cauchy, lower_bound, upper_bound);
var solution2 = cauchy_point + Math.Min(max_step_from_cauchy_point, 1.0) * direction_from_cauchy;
line_search_direction = solution2 - initial_eval.Point;
var max_line_search_step = FindMaxStep(initial_eval.Point, line_search_direction, lower_bound, upper_bound);
var est_step_size = -initial_eval.Gradient * line_search_direction / (line_search_direction * pseudo_hessian * line_search_direction);
var starting_step_size = Math.Min(Math.Max(est_step_size, 1.0), max_line_search_step);
// Line search
LineSearchOutput result;
try
{
result = line_searcher.FindConformingStep(objective, initial_eval, line_search_direction, starting_step_size, upper_bound: max_line_search_step);
}
catch (Exception e)
{
throw new InnerOptimizationException("Line search failed.", e);
}
previous_point = initial_eval;
candidate_point = result.FunctionInfoAtMinimum;
gradient = candidate_point.Gradient;
step = candidate_point.Point - initial_guess;
step_size = result.FinalStep;
// Subsequent steps
int iterations;
int total_line_search_steps = result.Iterations;
int iterations_with_nontrivial_line_search = result.Iterations > 0 ? 0 : 1;
int steepest_descent_resets = 0;
for (iterations = 1; iterations < this.MaximumIterations; ++iterations)
{
// Do BFGS update
var y = candidate_point.Gradient - previous_point.Gradient;
double sy = step * y;
if (sy > 0.0) // only do update if it will create a positive definite matrix
{
double sts = step * step;
//inverse_pseudo_hessian = inverse_pseudo_hessian + ((sy + y * inverse_pseudo_hessian * y) / Math.Pow(sy, 2.0)) * step.OuterProduct(step) - ((inverse_pseudo_hessian * y.ToColumnMatrix()) * step.ToRowMatrix() + step.ToColumnMatrix() * (y.ToRowMatrix() * inverse_pseudo_hessian)) * (1.0 / sy);
var Hs = pseudo_hessian * step;
var sHs = step * pseudo_hessian * step;
pseudo_hessian = pseudo_hessian + y.OuterProduct(y) * (1.0 / sy) - Hs.OuterProduct(Hs) * (1.0 / sHs);
}
else
{
steepest_descent_resets += 1;
//pseudo_hessian = LinearAlgebra.Double.DiagonalMatrix.Identity(initial_guess.Count);
}
// Determine active set
gradient_projection_result = QuadraticGradientProjectionSearch.search(candidate_point.Point, candidate_point.Gradient, pseudo_hessian, lower_bound, upper_bound);
cauchy_point = gradient_projection_result.Item1;
fixed_count = gradient_projection_result.Item2;
is_fixed = gradient_projection_result.Item3;
free_count = lower_bound.Count - fixed_count;
if (free_count > 0)
{
reduced_gradient = new DenseVector(free_count);
reduced_hessian = new DenseMatrix(free_count, free_count);
reduced_map = new List <int>(free_count);
reduced_initial_point = new DenseVector(free_count);
reduced_cauchy_point = new DenseVector(free_count);
CreateReducedData(candidate_point.Point, cauchy_point, is_fixed, lower_bound, upper_bound, candidate_point.Gradient, pseudo_hessian, reduced_initial_point, reduced_cauchy_point, reduced_gradient, reduced_hessian, reduced_map);
// Determine search direction and maximum step size
reduced_solution1 = reduced_initial_point + reduced_hessian.Cholesky().Solve(-reduced_gradient);
solution1 = reduced_to_full(reduced_map, reduced_solution1, cauchy_point);
}
else
{
solution1 = cauchy_point;
}
direction_from_cauchy = solution1 - cauchy_point;
max_step_from_cauchy_point = FindMaxStep(cauchy_point, direction_from_cauchy, lower_bound, upper_bound);
//var cauchy_eval = objective.Evaluate(cauchy_point);
solution2 = cauchy_point + Math.Min(max_step_from_cauchy_point, 1.0) * direction_from_cauchy;
line_search_direction = solution2 - candidate_point.Point;
max_line_search_step = FindMaxStep(candidate_point.Point, line_search_direction, lower_bound, upper_bound);
//line_search_direction = solution1 - candidate_point.Point;
//max_line_search_step = FindMaxStep(candidate_point.Point, line_search_direction, lower_bound, upper_bound);
if (max_line_search_step == 0.0)
{
line_search_direction = cauchy_point - candidate_point.Point;
max_line_search_step = FindMaxStep(candidate_point.Point, line_search_direction, lower_bound, upper_bound);
}
est_step_size = -candidate_point.Gradient * line_search_direction / (line_search_direction * pseudo_hessian * line_search_direction);
starting_step_size = Math.Min(Math.Max(est_step_size, 1.0), max_line_search_step);
// Line search
try
{
result = line_searcher.FindConformingStep(objective, candidate_point, line_search_direction, starting_step_size, upper_bound: max_line_search_step);
//result = line_searcher.FindConformingStep(objective, cauchy_eval, direction_from_cauchy, Math.Min(1.0, max_step_from_cauchy_point), upper_bound: max_step_from_cauchy_point);
}
catch (Exception e)
{
throw new InnerOptimizationException("Line search failed.", e);
}
iterations_with_nontrivial_line_search += result.Iterations > 0 ? 1 : 0;
total_line_search_steps += result.Iterations;
step_size = result.FinalStep;
step = result.FunctionInfoAtMinimum.Point - candidate_point.Point;
previous_point = candidate_point;
candidate_point = result.FunctionInfoAtMinimum;
current_exit_condition = this.ExitCriteriaSatisfied(candidate_point, previous_point, lower_bound, upper_bound, iterations);
if (current_exit_condition != ExitCondition.None)
{
break;
}
}
if (iterations == this.MaximumIterations && current_exit_condition == ExitCondition.None)
{
throw new MaximumIterationsException(String.Format("Maximum iterations ({0}) reached.", this.MaximumIterations));
}
return(new MinimizationWithLineSearchOutput(candidate_point, iterations, current_exit_condition, total_line_search_steps, iterations_with_nontrivial_line_search));
}
protected override void ComputeMappingMatrix()
{
InternalMappingMatrix = DiagonalMatrix.CreateIdentity(DomainGaSpaceDimension);
}
