public void Optimize(Dictionary<double, double> values) { var n = _f.Functions.Count(); var xs = values.Select(v => v.Key).ToList(); var ys = values.Select(v => v.Value).ToList(); var fs = new List<List<double>>(n); for (var i = 0; i < n; i++) { fs[i] = _f.Functions[i].Evaluate(xs); } var matrix = new DenseMatrix(n, n); var vector = new DenseVector(n); for (var i = 0; i < n; i++) { for (var j = 0; j < n; j++) { matrix[i, j] = fs[i].ScalarProduct(fs[j]); } vector[i] = ys.ScalarProduct(fs[i]); } var matrixInverse = matrix.Inverse(); var result = matrixInverse * vector; for (var i = 0; i < n; i++) { _f.LinearParameters[i].Value = result[i]; } }
public FedKF(KF[] filters, DenseMatrix dcm, DenseMatrix covariances) { _filters = filters; _filteredSignals = new DenseMatrix(_filters.Length, 1); _dcm = dcm; _dcmt = dcm.Transpose(); _cInv = covariances.Inverse(); }
public void Optimize(Dictionary<double, double> values) { var ys = new List<double>(); var fs = new List<double>(); var gs = new List<double>(); var hs = new List<double>(); foreach (var value in values) { ys.Add(value.Value); fs.Add(_lppl.F(value.Key)); gs.Add(_lppl.G(value.Key)); hs.Add(_lppl.H(value.Key)); } var N = values.Count; var sfs = fs.Sum(); var sgs = gs.Sum(); var shs = hs.Sum(); var sfsfs = fs.ScalarProduct(fs); var sgsgs = gs.ScalarProduct(gs); var shshs = hs.ScalarProduct(hs); var sfsgs = fs.ScalarProduct(gs); var sfshs = fs.ScalarProduct(hs); var sgshs = gs.ScalarProduct(hs); var sys = ys.Sum(); var sysfs = ys.ScalarProduct(fs); var sysgs = ys.ScalarProduct(gs); var syshs = ys.ScalarProduct(hs); var matrixArray = new[] { N, sfs, sgs, shs, sfs, sfsfs, sfsgs, sfshs, sgs, sfsgs, sgsgs, sgshs, shs, sfshs, sgshs, shshs }; var matrix = new DenseMatrix(4, 4, matrixArray); var matrixInverse = matrix.Inverse(); var vector = new DenseVector(new[] { sys, sysfs, sysgs, syshs }); var result = matrixInverse * vector; _lppl.A = result[0]; _lppl.B = result[1]; _lppl.C1 = result[2]; _lppl.C2 = result[3]; }
public static Matrix<double> WorldToImagePoints(this Matrix<double> worldPoints, DenseMatrix cameraCalibration, DenseVector posePosition, DenseMatrix poseRotation) { return cameraCalibration.Multiply( poseRotation.Inverse() .Multiply(worldPoints) .Translate(-posePosition) .ProjectCamCenteredWorldToHomogImagePoints()); /*This version is consistent with my DPixelToWorld but different from openCV ProjectPoints * cameraCalibration.Multiply( poseRotation.Inverse() .Multiply(worldPoints.Translate(-posePosition)) .ProjectCamCenteredWorldToHomogImagePoints());*/ }
public void Optimize(Dictionary<double, double> values) { var ys = values.Select(v => v.Value).ToList(); var fs = _f.Functions.Select(f => values.Select(v => NaNToZero(f(v.Key))).ToList()).ToList(); var matrix = new DenseMatrix(_f.LinearCount, _f.LinearCount); var vector = new DenseVector(_f.LinearCount); for (var i = 0; i < _f.LinearCount; i++) { for (var j = 0; j < _f.LinearCount; j++) { matrix[i, j] = fs[i].ScalarProduct(fs[j]); } vector[i] = ys.ScalarProduct(fs[i]); } var matrixInverse = matrix.Inverse(); var result = matrixInverse * vector; for (var i = 0; i < _f.LinearCount; i++) { _f.SetA(i, result[i]); } }
/// <summary>Optimizes the specified data</summary> /// <param name="data">data</param> /// <param name="W">W</param> /// <param name="H">H</param> protected virtual void Optimize(IBooleanMatrix data, Matrix<float> W, Matrix<float> H) { var HH = new Matrix<double>(num_factors, num_factors); var HC_minus_IH = new Matrix<double>(num_factors, num_factors); var HCp = new double[num_factors]; var m = new DenseMatrix(num_factors, num_factors); // source code comments are in terms of computing the user factors // works the same with users and items exchanged // (1) create HH in O(f^2|Items|) // HH is symmetric for (int f_1 = 0; f_1 < num_factors; f_1++) for (int f_2 = 0; f_2 < num_factors; f_2++) { double d = 0; for (int i = 0; i < H.dim1; i++) d += H[i, f_1] * H[i, f_2]; HH[f_1, f_2] = d; } // (2) optimize all U // HC_minus_IH is symmetric for (int u = 0; u < W.dim1; u++) { var row = data.GetEntriesByRow(u); // create HC_minus_IH in O(f^2|S_u|) for (int f_1 = 0; f_1 < num_factors; f_1++) for (int f_2 = 0; f_2 < num_factors; f_2++) { double d = 0; foreach (int i in row) //d += H[i, f_1] * H[i, f_2] * (c_pos - 1); d += H[i, f_1] * H[i, f_2] * c_pos; HC_minus_IH[f_1, f_2] = d; } // create HCp in O(f|S_u|) for (int f = 0; f < num_factors; f++) { double d = 0; foreach (int i in row) //d += H[i, f] * c_pos; d += H[i, f] * (1 + c_pos); HCp[f] = d; } // create m = HH + HC_minus_IH + reg*I // m is symmetric // the inverse m_inv is symmetric for (int f_1 = 0; f_1 < num_factors; f_1++) for (int f_2 = 0; f_2 < num_factors; f_2++) { double d = HH[f_1, f_2] + HC_minus_IH[f_1, f_2]; if (f_1 == f_2) d += regularization; m[f_1, f_2] = d; } var m_inv = m.Inverse(); // write back optimal W for (int f = 0; f < num_factors; f++) { double d = 0; for (int f_2 = 0; f_2 < num_factors; f_2++) d += m_inv[f, f_2] * HCp[f_2]; W[u, f] = (float) d; } } }
private void optimize(DenseMatrix coefficients, DenseVector objFunValues, bool artifical) { //for calculations on the optimal solution row int cCounter, width = coefficients.ColumnCount; DenseVector cBVect = new DenseVector(basics.Count); //Sets up the b matrix DenseMatrix b = new DenseMatrix(basics.Count, 1); //basics will have values greater than coefficients.ColumnCount - 1 if there are still artificial variables //or if Nathan is bad and didn't get rid of them correctly foreach (int index in basics) { b = (DenseMatrix)b.Append(DenseVector.OfVector(coefficients.Column(index)).ToColumnMatrix()); } // removes the first column b = (DenseMatrix)b.SubMatrix(0, b.RowCount, 1, b.ColumnCount - 1); double[] cPrimes = new double[width]; double[] rhsOverPPrime; DenseMatrix[] pPrimes = new DenseMatrix[width]; DenseMatrix bInverse; int newEntering, exitingRow; bool optimal = false; if(artifical) { rhsOverPPrime = new double[numConstraints + 1]; } else { rhsOverPPrime = new double[numConstraints]; } while (!optimal) { //calculates the inverse of b for this iteration bInverse = (DenseMatrix)b.Inverse(); //updates the C vector with the most recent basic variables cCounter = 0; foreach (int index in basics) { cBVect[cCounter++] = objFunValues.At(index); } //calculates the pPrimes and cPrimes for (int i = 0; i < coefficients.ColumnCount; i++) { if (!basics.Contains(i)) { pPrimes[i] = (DenseMatrix)bInverse.Multiply((DenseMatrix)coefficients.Column(i).ToColumnMatrix()); //c' = objFunVals - cB * P'n //At(0) to turn it into a double cPrimes[i] = objFunValues.At(i) - (pPrimes[i].LeftMultiply(cBVect)).At(0); } else { pPrimes[i] = null; } } //RHS' xPrime = (DenseMatrix)bInverse.Multiply((DenseMatrix)rhsValues.ToColumnMatrix()); //Starts newEntering as the first nonbasic newEntering = -1; int iter = 0; while(newEntering == -1) { if(!basics.Contains(iter)) { newEntering = iter; } iter++; } //new entering becomes the small cPrime that corresponds to a non-basic value for (int i = 0; i < cPrimes.Length; i++) { if (cPrimes[i] < cPrimes[newEntering] && !basics.Contains(i)) { newEntering = i; } } //if the smallest cPrime is >= 0, ie they are all positive if (cPrimes[newEntering] >= 0) { optimal = true; } else { //fix me to deal with if all these values are negative exitingRow = 0; for (int i = 0; i < xPrime.RowCount; i++) { double[,] pPrime = pPrimes[newEntering].ToArray(); rhsOverPPrime[i] = xPrime.ToArray()[i, 0] / pPrime[i, 0]; if (rhsOverPPrime[i] < rhsOverPPrime[exitingRow] && rhsOverPPrime[i] > 0 ) { exitingRow = i; } } //translates from the index in the basics list to the actual row exitingRow = basics[exitingRow]; //make sure you're not being stupid here!!!! int tempIndex = basics.IndexOf(exitingRow); basics.Remove(exitingRow); basics.Insert(tempIndex, newEntering); b.SetColumn(basics.IndexOf(newEntering), coefficients.Column(newEntering)); } } }
private void CalcBaseAandC() { BaseA = new DenseMatrix(CurrBaseJ.Count); BaseC = new DenseVector(CurrBaseJ.Count); int i = 0; foreach (var j in CurrBaseJ) { BaseA.SetColumn(i, A.Column(j)); BaseC[i] = c[j]; i++; } InvBaseA = (DenseMatrix)BaseA.Inverse(); CurrNonBaseJ = new List<int>(Enumerable.Range(0, A.ColumnCount).Except(CurrBaseJ)); }
public void placeCompensatorPoles(double value) { // Check for dimensions if (A.ColumnCount != A.RowCount || A.ColumnCount <= 0) { // TODO: Throw exception } int n = A.ColumnCount; // Calculate controllability matrix Matrix<double> controllabilityMatrix = new DenseMatrix(n, n); for (int i = 0; i < n; i++) { Vector<double> vec = B.Column(0); for (int j = 0; j < i; j++) { vec = A * vec; } controllabilityMatrix.SetColumn(i, vec); } // Unity vector Matrix<double> unityVector = new DenseMatrix(1,n); for (int i = 0; i < n; i++) { // Set 1 at last index unityVector.At(0, i, (i == n-1 ? 1 : 0) ); } // Coefficients matrix Matrix<double> preparedMatrix = A.Clone(); for (int i = 0; i < n; i++) { // Substract value from diagonal preparedMatrix.At(i, i, preparedMatrix.At(i, i) - value ); } Matrix<double> coefficientsMatrix = preparedMatrix.Clone(); for (int i = 0; i < n-1; i++) { // Multiply n-1 times coefficientsMatrix = preparedMatrix * coefficientsMatrix; } // Calculate new K using Ackermann's formula K = unityVector * controllabilityMatrix.Inverse() * coefficientsMatrix; }
/// <summary> /// Run example /// </summary> /// <seealso cref="http://en.wikipedia.org/wiki/Transpose">Transpose</seealso> /// <seealso cref="http://en.wikipedia.org/wiki/Invertible_matrix">Invertible matrix</seealso> public void Run() { // Format matrix output to console var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone(); formatProvider.TextInfo.ListSeparator = " "; // Create random square matrix var matrix = new DenseMatrix(5); var rnd = new Random(1); for (var i = 0; i < matrix.RowCount; i++) { for (var j = 0; j < matrix.ColumnCount; j++) { matrix[i, j] = rnd.NextDouble(); } } Console.WriteLine(@"Initial matrix"); Console.WriteLine(matrix.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 1. Get matrix inverse var inverse = matrix.Inverse(); Console.WriteLine(@"1. Matrix inverse"); Console.WriteLine(inverse.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 2. Matrix multiplied by its inverse gives identity matrix var identity = matrix * inverse; Console.WriteLine(@"2. Matrix multiplied by its inverse"); Console.WriteLine(identity.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 3. Get matrix transpose var transpose = matrix.Transpose(); Console.WriteLine(@"3. Matrix transpose"); Console.WriteLine(transpose.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 4. Get orthogonal matrix, i.e. do QR decomposition and get matrix Q var orthogonal = matrix.QR().Q; Console.WriteLine(@"4. Orthogonal matrix"); Console.WriteLine(orthogonal.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 5. Transpose and multiply orthogonal matrix by iteslf gives identity matrix identity = orthogonal.TransposeAndMultiply(orthogonal); Console.WriteLine(@"Transpose and multiply orthogonal matrix by iteslf"); Console.WriteLine(identity.ToString("#0.00\t", formatProvider)); Console.WriteLine(); }
private double[] linearApproxym(double[] x, double[] y) { int n = x.Length; double sumOfX = 0; double sumOfXSquare = 0; double sumOfY = 0; double sumOfXY = 0; for (int i = 0; i < n; i++) { sumOfX += x[i]; sumOfXSquare += x[i] * x[i]; sumOfY += y[i]; sumOfXY += x[i] * y[i]; } var matrixX = new DenseMatrix(new double[,] { { n, sumOfX }, { sumOfX, sumOfXSquare } }); var matrixY = new DenseMatrix(2, 1, new double[] { sumOfY, sumOfXY }); var matrixP = matrixX.Inverse().Multiply(matrixY); double[] p = new double[2]; p[0] = matrixP[1, 0]; p[1] = matrixP[0, 0]; return p; }
private void CalculateCoefficients(int points) { var c = new double[points][,]; // For ever possible center given the number of points, compute ever possible coefficient for all possible orders. for (int center = 0; center < points; center++) { // Deltas matrix for center located at 'center'. var A = new DenseMatrix(points); var l = points - center - 1; for (int row = points - 1; row >= 0; row--) { A[row, 0] = 1.0; for (int col = 1; col < points; col++) { A[row, col] = A[row, col - 1] * l / col; } l -= 1; } c[center] = A.Inverse().ToArray(); // "Polish" results by rounding. var fac = SpecialFunctions.Factorial(points); for (int j = 0; j < points; j++) for (int k = 0; k < points; k++) #if PORTABLE c[center][j, k] = (Math.Round(c[center][j, k] * fac)) / fac; #else c[center][j, k] = (Math.Round(c[center][j, k] * fac, MidpointRounding.AwayFromZero)) / fac; #endif } _coefficients = c; }
//Given an observation, mu and sigma. What is N(O | mu, sigma)? public double queryGuassian(DenseVector observation, DenseVector mean, DenseMatrix covar) { double scaletemp, scale, exponent, prob; DenseMatrix v1, v2; //Temp matrices, for multiplying scaletemp = (Math.Pow(2 * Math.PI, mean.Count / 2)); scale = (Math.Sqrt(covar.Determinant())); scale *= scaletemp; scale = 1 / scale; v1 = (DenseMatrix)(observation - mean).ToRowMatrix(); v2 = (DenseMatrix)(observation - mean).ToColumnMatrix(); v2 = (DenseMatrix)(covar.Inverse()) * v2; exponent = (-0.5) * ((v1 * v2).ToArray()[0,0]); prob = scale * Math.Pow(Math.E, exponent); return prob; }
private void Optimize(int u, IBooleanMatrix data, Matrix<float> W, Matrix<float> H, Matrix<double> HH) { var row = data.GetEntriesByRow(u); // HC_minus_IH is symmetric // create HC_minus_IH in O(f^2|S_u|) var HC_minus_IH = new Matrix<double>(num_factors, num_factors); for (int f_1 = 0; f_1 < num_factors; f_1++) for (int f_2 = f_1; f_2 < num_factors; f_2++) { double d = 0; foreach (int i in row) d += H[i, f_1] * H[i, f_2]; HC_minus_IH[f_1, f_2] = d * alpha; HC_minus_IH[f_2, f_1] = d * alpha; } // create HCp in O(f|S_u|) var HCp = new double[num_factors]; for (int f = 0; f < num_factors; f++) { double d = 0; foreach (int i in row) d += H[i, f]; HCp[f] = d * (1 + alpha); } // create m = HH + HC_minus_IH + reg*I // m is symmetric // the inverse m_inv is symmetric var m = new DenseMatrix(num_factors, num_factors); for (int f_1 = 0; f_1 < num_factors; f_1++) for (int f_2 = f_1; f_2 < num_factors; f_2++) { double d = HH[f_1, f_2] + HC_minus_IH[f_1, f_2]; if (f_1 == f_2) d += regularization; m[f_1, f_2] = d; m[f_2, f_1] = d; } var m_inv = m.Inverse(); // write back optimal W for (int f = 0; f < num_factors; f++) { double d = 0; for (int f_2 = 0; f_2 < num_factors; f_2++) d += m_inv[f, f_2] * HCp[f_2]; W[u, f] = (float) d; } }
public Vector<double> Solve(DenseMatrix systemMatrix, Vector<double> freeTerms) { return systemMatrix.Inverse().Multiply(freeTerms); }
private bool Iterate() { // Step 1 var cb = new DenseVector(_task.Jb.Count()); for (int i = 0; i < _task.Jb.Count(); i++) { cb[i] = _task.c[_task.Jb[i]]; } Matrix<double> bMatrix = new DenseMatrix(_task.Jb.Count()); for (int i = 0; i < bMatrix.ColumnCount; i++) { bMatrix.SetColumn(i, _task.A.Column(_task.Jb[i])); } bMatrix = bMatrix.Inverse(); var u = cb * bMatrix; var deltas = DenseVector.Create( _task.c.Count, i => _task.Jb.Contains(i) ? double.PositiveInfinity : u * _task.A.Column(i) - _task.c[i]); //Step 2 if (deltas.ToList().TrueForAll(x => x > 0 || Math.Abs(x) < Eps)) { // STOP vector is optimal //_writer.WriteLine("Optimal plan is found: {0}", _task.xo); //_writer.WriteLine("Target function value = {0}", _task.c * _task.xo); return false; } // Step 3 int j0 = 0; for (int j = 0; j < deltas.Count; j++) { if (!_task.Jb.Contains(j) && deltas[j] < 0 && Math.Abs(deltas[j]) >= Eps) { j0 = j; break; } } var z = bMatrix * _task.A.Column(j0); if (z.ToList().TrueForAll(x => x < -Eps || Math.Abs(x) < Eps )) { // STOP target function is unlimited _writer.WriteLine("Target function is unlimited"); return false; } // Step 4 var tetta0 = double.PositiveInfinity; var s = -1; for (int i = 0; i < z.Count; i++) { if (z[i] > 0 && Math.Abs(z[i]) > Eps) { var tetta = _task.xo[_task.Jb[i]]/z[i]; if(double.IsPositiveInfinity(tetta0) || (Math.Abs(tetta0 - tetta) > Eps && tetta < tetta0)) { tetta0 = tetta; s = i; } } } // Step 5 var newXo = DenseVector.Create(_task.xo.Count, i => 0); for (int i = 0; i < _task.Jb.Count(); i++) { newXo[_task.Jb[i]] = _task.xo[_task.Jb[i]] - tetta0 * z[i]; } newXo[j0] = tetta0; _task.xo = newXo; _task.Jb[s] = j0; // Step 6 is unneccesary return true; }
public override void Match() { int r21 = 2 * WindowRadius + 1; _leftMoments = new List<Vector<double>>(); _rightMoments = new List<Vector<double>>(); // Find circural mask bounds: // for x = [-r,r] -> y = [ -sqrt(r^2 - x^2), sqrt(r^2 - x^2) ] _ybounds = new int[r21]; for(int x = -WindowRadius; x <= WindowRadius; ++x) { _ybounds[x + WindowRadius] = (int)Math.Sqrt(WindowRadius * WindowRadius - x * x); } // Find moment vectors for each feature point for(int i = 0; i < LeftFeaturePoints.Count; ++i) { IntVector2 pixel = LeftFeaturePoints[i]; if(pixel.X < WindowRadius || pixel.Y < WindowRadius || pixel.X >= LeftImage.ColumnCount - WindowRadius || pixel.Y >= LeftImage.RowCount - WindowRadius) { _leftMoments.Add(null); } else { Vector<double> mom = ComputeMomentVectorForPatch(pixel, LeftImage); if(UseCenteredMoments) mom = ComputeCenteredMomentVector(mom); if(UseScaledMoments) ScaleMomentVector(mom); Vector<double> invMom = ComputeInvariantMomentVector(mom); _leftMoments.Add(invMom); } } for(int i = 0; i < RightFeaturePoints.Count; ++i) { IntVector2 pixel = RightFeaturePoints[i]; if(pixel.X < WindowRadius || pixel.Y < WindowRadius || pixel.X >= RightImage.ColumnCount - WindowRadius || pixel.Y >= RightImage.RowCount - WindowRadius) { _rightMoments.Add(null); } else { Vector<double> mom = ComputeMomentVectorForPatch(pixel, RightImage); if(UseCenteredMoments) mom = ComputeCenteredMomentVector(mom); if(UseScaledMoments) ScaleMomentVector(mom); Vector<double> invMom = ComputeInvariantMomentVector(mom); _rightMoments.Add(invMom); } } // We need to find covariance matrix of invariants as they have // different magnitudes, so simple ||Il - Ir|| may not be best choice // E = 1/(n-1) sum( (xi-m)(xi-m)^T ) (x is column vector, m = 1/n sum(xi) // 1) Find mean int n = 0; Vector<double> meanInv = new DenseVector(_invCount); for(int i = 0; i < _leftMoments.Count; ++i) { if(_leftMoments[i] != null) { meanInv.PointwiseAddThis(_leftMoments[i]); ++n; } } for(int i = 0; i < _rightMoments.Count; ++i) { if(_rightMoments[i] != null) { meanInv.PointwiseAddThis(_rightMoments[i]); ++n; } } meanInv.MultiplyThis(1.0 / n); // 2) Find E Matrix<double> cov = new DenseMatrix(_invCount, _invCount); for(int i = 0; i < _leftMoments.Count; ++i) { if(_leftMoments[i] != null) { cov.PointwiseAddThis(CamCore.MatrixExtensions.FromVectorProduct(_leftMoments[i] - meanInv)); } } for(int i = 0; i < _rightMoments.Count; ++i) { if(_rightMoments[i] != null) { cov.PointwiseAddThis(CamCore.MatrixExtensions.FromVectorProduct(_rightMoments[i] - meanInv)); } } cov.MultiplyThis(1.0 / (n - 1)); var covInv = cov.Inverse(); // Match each point pair and find ||Il - Ir||E List<MatchedPair> costs; var matchLeft = new List<MatchedPair>(); var matchRight = new List<MatchedPair>(); for(int l = 0; l < LeftFeaturePoints.Count; ++l) { costs = new List<MatchedPair>(LeftFeaturePoints.Count); if(_leftMoments[l] != null) { for(int r = 0; r < RightFeaturePoints.Count; ++r) { if(_rightMoments[r] != null) { var d = _leftMoments[l] - _rightMoments[r]; costs.Add(new MatchedPair() { LeftPoint = new Vector2(LeftFeaturePoints[l]), RightPoint = new Vector2(RightFeaturePoints[r]), Cost = UseMahalanobis ? d * covInv * d : // cost = d^T * E^-1 * d d.DotProduct(d) }); } } costs.Sort((c1, c2) => { return c1.Cost > c2.Cost ? 1 : (c1.Cost < c2.Cost ? -1 : 0); }); // Confidence will be (c2-c1)/(c1+c2) MatchedPair match = costs[0]; match.Confidence = (costs[1].Cost - costs[0].Cost) / (costs[1].Cost + costs[0].Cost); matchLeft.Add(match); } } for(int r = 0; r < RightFeaturePoints.Count; ++r) { costs = new List<MatchedPair>(RightFeaturePoints.Count); if(_rightMoments[r] != null) { for(int l = 0; l < LeftFeaturePoints.Count; ++l) { if(_leftMoments[l] != null) { var d = _leftMoments[l] - _rightMoments[r]; costs.Add(new MatchedPair() { LeftPoint = new Vector2(LeftFeaturePoints[l]), RightPoint = new Vector2(RightFeaturePoints[r]), Cost = UseMahalanobis ? d * covInv * d : // cost = d^T * E^-1 * d d.DotProduct(d) }); } } costs.Sort((c1, c2) => { return c1.Cost > c2.Cost ? 1 : (c1.Cost < c2.Cost ? -1 : 0); }); // Confidence will be (c2-c1)/(c1+c2) MatchedPair match = costs[0]; match.Confidence = (costs[1].Cost - costs[0].Cost) / (costs[1].Cost + costs[0].Cost); matchRight.Add(match); } } Matches = new List<MatchedPair>(); foreach(var ml in matchLeft) { MatchedPair mr = matchRight.Find((m) => { return ml.LeftPoint.DistanceTo(m.LeftPoint) < 0.01; }); // We have both sides matches if(mr != null && ml.RightPoint.DistanceTo(mr.RightPoint) < 0.01) { // Cross check sucessful Matches.Add(mr); } } }
private Func<double, DenseVector> LinearGradient() { var ys = _values.Select(v => _l.Value(v.Key)).ToList(); var fs = _l.Functions.Select(f => _values.Select(v => f(v.Key)).ToList()).ToList(); var dfs = _functionsDerivatives .Select(dpf => dpf.Select(df => _values.Select(v => df.Value(v.Key)).ToList()).ToList()).ToList(); var n = _l.Functions.Count; const int nb = 3; var results = new List<Vector<double>>(); for (var b = 0; b < nb; b++) { var matrix = new DenseMatrix(n, n); var vector = new DenseVector(n); for (var i = 0; i < n; i++) { for (var j = 0; j < n; j++) { matrix[i, j] = dfs[i][b].ScalarProduct(fs[j]) + dfs[j][b].ScalarProduct(fs[i]); } vector[i] = ys.ScalarProduct(dfs[i][b]); } var matrixInverse = matrix.Inverse(); var result = matrixInverse * vector; results.Add(result); } Func<double, DenseVector> gradient = x => new DenseVector(new[] { DotProduct(results[0], _l.Functions)(x), DotProduct(results[1], _l.Functions)(x), 0//DotProduct(results[2], _l.Functions)(x) }); return gradient; }
private DenseVector NewtonMethod(ref Parser[] parsers) { double eps = 10e-3; //var helperMatrix = new DenseMatrix(equationsCount, equationsCount); var helperMatrixR = new double[equationsCount, equationsCount]; for (int i = 0; i < equationsCount; i++) { for (int j = 0; j < equationsCount; j++) { double p; double p1; try { parsers[i].Variables[variables.ElementAt(j).Key] = variables.ElementAt(j).Value + eps; p1 = parsers[i].Calculate(); parsers[i].Variables[variables.ElementAt(j).Key] = variables.ElementAt(j).Value; p = parsers[i].Calculate(); } catch(Exception exception) { ErrorAllert(exception.Message); return null; } helperMatrixR[i, j] = (p1 - p)/eps; } } var helperMatrix = new DenseMatrix(helperMatrixR); if (Math.Abs(helperMatrix.Determinant()) < eps) { ErrorAllert("Determinant of Jacobi matrix is 0. Can't calculate result."); return null; } var inverseMatrix = helperMatrix.Inverse(); var initVars = new DenseVector(equationsCount); var modVars = new Dictionary<string, double>(); for (int i = 0; i < variables.Count; i++) { initVars[i] = variables.ElementAt(i).Value; modVars[variables.ElementAt(i).Key] = variables.ElementAt(i).Value; } foreach (var parser in parsers) { parser.Variables = modVars; } bool diff = false; int itCount = 0; while (!diff && itCount < 10) { var f = new DenseVector(equationsCount); for (int j = 0; j < equationsCount; j++) { f[j] = parsers[j].Calculate(); } var result = new DenseVector((initVars - inverseMatrix*f).ToArray()); for (int i = 0; i < equationsCount; i++) { diff = diff && (Math.Abs(result[i] - initVars[i]) < eps); } initVars = result; for (int j = 0; j < equationsCount; j++) { modVars[modVars.ElementAt(j).Key] = initVars[j]; } ++itCount; } return initVars; }
private bool GomoriIteration() { // Шаг 1 _writer.WriteLine("Iteration: {0}", iterationNumber); var simplexMethod = new SimplexMethod(_writer); simplexMethod.Solve(_task); // Решаем задачу симплекс методом _writer.WriteLine("Optimal plan is found: {0}", _task.xo); _writer.WriteLine("Target function value = {0}", _task.c * _task.xo); // Шаг 2 //var artJToRemoveRow = -1; //var artJToRemoveColumn = -1; //artJToRemoveRow = -1; //artJToRemoveColumn = -1; //foreach (var artJ in _artJ) //{ // if (_task.Jb.Contains(artJ.Column)) // { // var rowToRemove = artJ.Row; // TODO probably need to rewrite row selection // var ai = _task.A.Row(rowToRemove); // Выбираем строку с искусственым ограничением // ai = -ai / ai[artJ.Column]; // var rowList = ai.ToList(); // rowList.RemoveAt(artJ.Column); // ai = DenseVector.OfEnumerable(rowList); // var aj = _task.A.Column(artJ.Column); // Выбираем столбец с искусственным ограничением // var columnList = aj.ToList(); // var bCoef = _task.b[rowToRemove] / columnList[rowToRemove]; // columnList.RemoveAt(rowToRemove); // aj = DenseVector.OfEnumerable(columnList); // var newA = DenseMatrix.Create(_task.A.RowCount - 1, _task.A.ColumnCount - 1, // (i, j) => _task.A[i < rowToRemove ? i : i + 1, j < artJ.Column ? j : j + 1]); // newA += DenseMatrix.OfMatrix(aj.ToColumnMatrix() * ai.ToRowMatrix()); // Удаляем искусственные строку // _task.A = newA; // и столбец из матрицы А // _task.b = DenseVector.Create(_task.b.Count - 1, i => i < rowToRemove ? _task.b[i] : _task.b[i + 1]); // _task.b += bCoef * aj; // _task.c = DenseVector.Create(_task.c.Count - 1, i => i < artJ.Column ? _task.c[i] : _task.c[i + 1]); // Удаляем искусственную переменную из вектора с // _task.xo = DenseVector.Create(_task.xo.Count - 1, i => i < artJ.Column ? _task.xo[i] : _task.xo[i + 1]); // Удаляем искусственную переменную из xo // _task.Jb.Remove(artJ.Column); // artJToRemoveColumn = artJ.Column; // artJToRemoveRow = artJ.Row; // break; // } //} //if (artJToRemoveRow > 0) // Удаляем искусственную переменную из базисных //{ // _artJ.RemoveAll(x => x.Row == artJToRemoveRow); // for (int i = 0; i < _artJ.Count; i++) // { // if (_artJ[i].Row > artJToRemoveRow) // { // _artJ[i].Row--; // Сдвигаем индексы базисных переменных на один // _artJ[i].Column--; // После удаления искусственной переменной // } // } // for (int i = 0; i < _task.Jb.Count; i++) // { // _task.Jb[i] = _task.Jb[i] > artJToRemoveColumn ? _task.Jb[i] - 1 : _task.Jb[i]; // } //} // Шаг 3 var falseIndex = -1; var maxFract = 0d; for (int i = 0; i < _task.xo.Count(); i++) { if (Math.Abs(Math.Round(_task.xo[i]) - _task.xo[i]) > Eps) { var fract = Math.Abs(_task.xo[i] - Math.Floor(_task.xo[i])); // Находим базисную переменную if (_task.Jb.Contains(i) && fract > Eps) // С максимальной дробной частью { // и запоминаем ее индекс if (fract > maxFract) { maxFract = fract; falseIndex = i; } } } } if (falseIndex < 0) // Если все переменные целые - решение найдено { return false; // Прерываем выполнение метода } _writer.WriteLine("Jk = {0}", falseIndex); // Шаг 4 var aB = new DenseMatrix(_task.Jb.Count()); int index = 0; foreach (var j in _task.Jb) { aB.SetColumn(index, _task.A.Column(j)); // Формируем матрицу Ab из базисных столбцов А index++; } _writer.Write("Jb: "); _task.Jb.ForEach(x => _writer.Write("{0} ", x)); _writer.WriteLine(); _writer.WriteLine("Basis matrix: {0}", aB); var y = DenseMatrix.Identity(_task.A.RowCount).Column(_task.Jb.IndexOf(falseIndex)) * aB.Inverse(); //Находим e'*Ab var newRow = new DenseVector(_task.A.ColumnCount + 1); newRow.SetSubVector(0, _task.A.ColumnCount, y * _task.A); // Находим данные для нового отсекающего ограничения _writer.WriteLine("Data for new limitation: {0}", newRow); for (int i = 0; i < newRow.Count; i++) // Формируем новое отсекающее ограничение { if (i < _task.A.ColumnCount) { if (Math.Abs(newRow[i]) < Eps) { newRow[i] = 0; } else { newRow[i] = newRow[i] > 0 ? -(newRow[i] - Math.Floor(newRow[i])) : -(Math.Ceiling(Math.Abs(newRow[i])) - Math.Abs(newRow[i])); } } else { newRow[i] = 1; } } newRow[falseIndex] = 0; _writer.WriteLine("New limitation: {0}", newRow); var newb = (y * _task.b); // Находим новый элемент вектора b newb = newb > 0 ? -(newb - Math.Floor(newb)) : -(Math.Ceiling(Math.Abs(newb)) - Math.Abs(newb)); // TODO probably need to rewrite this _writer.WriteLine("New b = {0}", newb); // Шаг 5 var newMatrix = new DenseMatrix(_task.A.RowCount + 1, _task.A.ColumnCount + 1); // Формируем новую newMatrix.SetSubMatrix(0, _task.A.RowCount, 0, _task.A.ColumnCount, _task.A); // матрицу А newMatrix.SetRow(_task.A.RowCount, newRow); newMatrix[_task.A.RowCount, _task.A.ColumnCount] = 1; var newBVector = new DenseVector(_task.b.Count + 1); // Формируем новый newBVector.SetSubVector(0, _task.b.Count, _task.b); // вектор b newBVector[_task.b.Count] = newb; var newCVector = new DenseVector(_task.c.Count + 1); // Добавляем новую newCVector.SetSubVector(0, _task.c.Count, _task.c); // компоненту вектора с var newJb = _task.Jb.ToList(); newJb.Add(newJb[newJb.Count - 1] + 1); _artJ.Add(new ArtJEntry { Column = newMatrix.ColumnCount - 1, Row = newMatrix.RowCount - 1 }); _task.A = newMatrix.Clone(); // Создаем _task.b = newBVector.Clone(); // новую задачу _task.c = newCVector.Clone(); // для следующей итерации _task.Jb = newJb; iterationNumber++; // Присваиваем новый номер итерации return true; }