/// <summary> /// Performs a simulation step using Symplectic integration with constrained dynamics. /// The constraints are treated as implicit /// </summary> private void stepSymplecticConstraints() { // TO BE COMPLETED VectorXD v = new DenseVectorXD(m_numDoFs); VectorXD f = new DenseVectorXD(m_numDoFs); MatrixXD Minv = new DenseMatrixXD(m_numDoFs); MatrixXD J = new DenseMatrixXD(m_numConstraints, m_numDoFs); MatrixXD A = new DenseMatrixXD(m_numDoFs); VectorXD B = new DenseVectorXD(m_numDoFs); VectorXD c = new DenseVectorXD(m_numConstraints); VectorXD b = new DenseVectorXD(m_numDoFs); VectorXD lamda = new DenseVectorXD(m_numConstraints); MatrixXD I = DenseMatrixXD.CreateIdentity(m_numDoFs); f.Clear(); Minv.Clear(); J.Clear(); foreach (ISimulable obj in m_objs) { obj.GetVelocity(v); obj.GetForce(f); obj.GetMassInverse(Minv); } foreach (IConstraint constraint in m_constraints) { constraint.GetForce(f); constraint.GetConstraints(c); constraint.GetConstraintJacobian(J); } foreach (ISimulable obj in m_objs) { obj.FixVector(f); obj.FixMatrix(Minv); } b = v + TimeStep * Minv * f; A = J * I * J.Transpose(); B = J * I * b + (1.0f / TimeStep) * c; lamda = A.Solve(B); v = I * (b - J.Transpose() * lamda); VectorXD x = TimeStep * v; foreach (ISimulable obj in m_objs) { obj.AdvanceIncrementalPosition(x); obj.SetVelocity(v); } }
public void Compute(DenseMatrix x1, int size) { //Setting Preparation..................................................................................................... _estimationLength = x1.Values.Length; //Creation of a Z matrix.................................................................................................. // ReSharper disable once CSharpWarnings::CS0618 var z = new DenseMatrix(_estimationLength, size, 1.0); for (var i = 0; i <= _estimationLength - 1; i++) { for (var j = 0; j <= size - 1; j++) { z[i, j] = Math.Pow(i,j); } } //Computation of Theta matrix............................................................................................. var zt = z.Transpose(); var ztz = zt * z; var theta = ztz.Inverse() * zt * x1; // ReSharper disable once CSharpWarnings::CS0618 _output = new DenseMatrix(_estimationLength, 1, 1.0); //Output creation......................................................................................................... for (var i = 0; i <= _estimationLength - 1; i++) { for (var j = 0; j <= size - 1; j++) { _output[i, 0] += theta[j,0]*Math.Pow(i,j); } } _estimationDone = true; }
public static DenseVector GetRegularizedWeights(DenseMatrix m, double lambda, DenseVector yV) { var mt = m.Transpose(); var what = (mt * m); var identity = DenseMatrix.Identity(what.ColumnCount); return (DenseVector)((what + lambda * identity).Inverse() * mt * yV); //return (DenseMatrix)(what.Inverse() * mt); }
public FedKF(KF[] filters, DenseMatrix dcm, DenseMatrix covariances) { _filters = filters; _filteredSignals = new DenseMatrix(_filters.Length, 1); _dcm = dcm; _dcmt = dcm.Transpose(); _cInv = covariances.Inverse(); }
private void HardConstraintsStep() { // Vectors and matrices to store data VectorXD C0 = new DenseVectorXD(m_numcs); VectorXD f = new DenseVectorXD(m_numdofs); VectorXD v = new DenseVectorXD(m_numdofs); MatrixXD M = new DenseMatrixXD(m_numdofs, m_numdofs); MatrixXD J = new DenseMatrixXD(m_numcs, m_numdofs); MatrixXD System = new DenseMatrixXD(m_numcs + m_numdofs, m_numcs + m_numdofs); VectorXD Independent = new DenseVectorXD(m_numdofs + m_numcs); // Compute forces and retrieve the rigid bodies data foreach (ISimulable i in m_objs) { i.clearForcesAndMatrices(); i.addForcesAndMatrices(); i.getForceVector(f); i.getVelocityVector(v); i.getMassMatrix(M); } // Compute and retrieve restrictions and Jacobians foreach (Constraint c in m_constraints) { c.getConstraintVector(C0); c.getConstraintJacobian(J); } // Set up left-side system System.SetSubMatrix(0, m_numdofs, 0, m_numdofs, M); System.SetSubMatrix(m_numdofs, m_numcs, 0, m_numdofs, J); System.SetSubMatrix(0, m_numdofs, m_numdofs, m_numcs, J.Transpose()); System.SetSubMatrix(m_numdofs, m_numcs, m_numdofs, m_numcs, DenseMatrixXD.Create(m_numcs, m_numcs, 0.0)); // Set up right-side system VectorXD b = M * v + TimeStep * f; VectorXD AtC0 = (-1.0f / TimeStep) * C0; Independent.SetSubVector(0, m_numdofs, b); Independent.SetSubVector(m_numdofs, m_numcs, AtC0); // Solve system VectorXD newVelocities = System.Solve(Independent); // Update bodies foreach (ISimulable i in m_objs) { i.setVelocityVector(newVelocities); i.advancePosition(); } }
/// <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(); }
public void Compute(DenseMatrix x1, DenseMatrix y1, int estimationLength) { var random = new Random(); //this._y1 = Y1; //Setting preparation..................................................................................................... _estimationError = 0.0; _estimationStatusPercentage = 0; if (estimationLength == 0) _estimationLength = x1.Values.Length - _modelArmax.StartingPoint; else _estimationLength = estimationLength; //I Step: Y(L)............................................................................................................ // ReSharper disable once CSharpWarnings::CS0618 var yl = new DenseMatrix(_estimationLength, 1, 0.0); for (var i = 0; i <= _estimationLength - 1; i++) { yl[i, 0] = y1[i + _modelArmax.StartingPoint, 0]; } _statusString = "Iterative LS: Step I: DONE"; //II Step: V0............................................................................................................. // ReSharper disable once CSharpWarnings::CS0618 _modelArmax.V0 = new DenseMatrix(x1.Values.Length, 1, 1.0); // for (var i = 0; i <= x1.Values.Length - 1; i++) { _modelArmax.V0[i, 0] = random.Next(-100, 100); } _statusString = "Iterative LS: Step II: DONE"; //Definition of temporary matrixes // ReSharper disable once CSharpWarnings::CS0618 var thetaK1 = new DenseMatrix(_modelArmax.NaParameter + _modelArmax.NbParameter + _modelArmax.NdParameter, 1, 0.0); // ReSharper disable once CSharpWarnings::CS0618 var fiKl = new DenseMatrix(_modelArmax.NaParameter + _modelArmax.NbParameter + _modelArmax.NdParameter, _estimationLength, 0.0); // ReSharper disable once CSharpWarnings::CS0618 var vkT = new DenseMatrix(_estimationLength + (_numberOfIterations + 1) * _modelArmax.StartingPoint, 1, 1.0); // ReSharper disable once CSharpWarnings::CS0618 var thetaDiff = new DenseMatrix(_modelArmax.NaParameter + _modelArmax.NbParameter + _modelArmax.NdParameter, 1, 0.0); //Predefinition of results // ReSharper disable once CSharpWarnings::CS0618 _modelArmax.Theta = new DenseMatrix(_modelArmax.NaParameter + _modelArmax.NbParameter + _modelArmax.NdParameter, 1, 0.0); // ReSharper disable once CSharpWarnings::CS0618 _modelArmax.Yk = new DenseMatrix(x1.Values.Length, 1, 0.0); for (var k = 1; k <= _numberOfIterations; k++) { //III Step: Fi_k(_estimationLength)................................................................................... for (var t = _modelArmax.StartingPoint; t <= _modelArmax.StartingPoint + _estimationLength - 1; t++) { var fik = _fiCalculator.CalculateFi_k(_modelArmax.NaParameter, _modelArmax.NbParameter, _modelArmax.NdParameter, _modelArmax.NkParameter, t, _modelArmax.ModelShift, x1, y1, _modelArmax.V0); for (var i = 0; i <= _modelArmax.NaParameter + _modelArmax.NbParameter + _modelArmax.NdParameter - 1; i++) { fiKl[i, t - _modelArmax.StartingPoint] = fik[i, 0]; } } _statusString = "Iteration " + k + " LS: Step III: DONE"; //IV Step: Theta(k)................................................................................................... var fiKLt = fiKl.Transpose(); _statusString = "Iteration " + k +" LS: Step IV: 1/5 DONE"; var fiFiT = MultiplicatorCuda.Multiply(fiKl, (DenseMatrix)fiKLt); _statusString = "Iteration " + k + " LS: Step IV: 2/5 DONE"; var fiFiTi = fiFiT.Inverse(); _statusString = "Iteration " + k + " LS: Step IV: 3/5 DONE"; var fiFiTiFiKl = MultiplicatorCuda.Multiply((DenseMatrix)fiFiTi, fiKl); _statusString = "Iteration " + k + " LS: Step IV: 4/5 DONE"; var thetaK = fiFiTiFiKl * yl; _statusString = "Iteration " + k + " LS: Step IV: 5/5 DONE"; //V Step: Vk_t........................................................................................................ for (var i = 0; i <=_estimationLength - 1; i++) { var fikt = _fiCalculator.CalculateFi_k(_modelArmax.NaParameter, _modelArmax.NbParameter, _modelArmax.NdParameter, _modelArmax.NkParameter, i + _modelArmax.StartingPoint, _modelArmax.ModelShift, x1, y1, _modelArmax.V0); var thetaKy1 = thetaK.Transpose() * fikt; vkT[i, 0] = y1[i + _modelArmax.StartingPoint, 0] - thetaKy1[0, 0]; } _statusString = "Iteration " + k + " LS: Step V: DONE"; //VI Step: Average Error.............................................................................................. for (var i = 0; i <= _modelArmax.NaParameter + _modelArmax.NbParameter + _modelArmax.NdParameter - 1; i++) { thetaDiff[i, 0] = Math.Abs(thetaK[i, 0] - thetaK1[i, 0]); } _estimationError = 0.0; for (var i = 0; i <= _modelArmax.NaParameter + _modelArmax.NbParameter + _modelArmax.NdParameter - 1; i++) { _estimationError = _estimationError + thetaDiff[i, 0]; } _estimationDifference = 0.0; for (var i = 0; i <= _estimationLength - 1; i++) { _estimationDifference += vkT[i, 0]; } _estimationDifference = _estimationDifference / _estimationLength; _estimationError = _estimationError / (_modelArmax.NaParameter + _modelArmax.NbParameter + _modelArmax.NdParameter); _statusString = "Iteration " + k + " LS: Step VI: Error: " + _estimationError + @"; Diff: " + _estimationDifference; //VII Step: Decision.................................................................................................. if (Double.IsNaN(_estimationError)) k = _numberOfIterations; else { _modelArmax.V0 = vkT; for (var i = 0; i <= _modelArmax.NaParameter + _modelArmax.NbParameter + _modelArmax.NdParameter - 1; i++) { thetaK1[i, 0] = thetaK[i, 0]; _estimationStatusPercentage = k * 100 / _numberOfIterations; } if (_estimationError < _acceptableError) k = _numberOfIterations; } _statusString = "Iteration " + k + " LS: Step VII: DONE"; } //END: Vk................................................................................................................. if (_estimationLength - _modelArmax.StartingPoint < 10000) { _modelArmax.Offset = _estimationLength - _modelArmax.StartingPoint; } _modelArmax.CreateStartMatrixX(x1); _modelArmax.CreateStartMatrixY(y1); _modelArmax.Theta = thetaK1; _modelArmax.Model(x1, true); _statusString = "Iterative LS: Estimation: DONE"; _estimationDone = true; }
/// <summary> /// Adaptive Cross Approximation (ACA) matrix compression /// the result is stored in U and V matrices like U*V /// </summary> /// <param name="acaThres">Relative error threshold to stop adding rows and columns in ACA iteration</param> /// <param name="m">Row indices of Z submatrix to compress</param> /// <param name="n">Column indices of Z submatrix to compress</param> /// <param name="U">to store result</param> /// <param name="V">to store result</param> /// <returns>pair with matrix U and V</returns> public static Tuple<Matrix, Matrix> Aca(double acaThres, List<int> m, List<int> n, Matrix U, Matrix V) { int M = m.Count; int N = n.Count; int Min = Math.Min(M, N); U = new DenseMatrix(Min, Min); V = new DenseMatrix(Min, Min); //if Z is a vector, there is nothing to compress if (M == 1 || N == 1) { U = UserImpedance(m, n); V = new DenseMatrix(1, 1); V[0, 0] = 1.0; return new Tuple<Matrix,Matrix>(U,V); } //Indices of columns picked up from Z //Vector J = new DenseVector(N); //List<int> J = new List<int>(N); List<int> J = new List<int>(new int [N]); //int[] J = new int[N]; //Indices of rows picked up from Z //Vector I = new DenseVector(M); List<int> I = new List<int>(new int [M]); //int[] I = new int[M]; //Row indices to search for maximum in R //Vector i = new DenseVector(M); List<int> i = new List<int>(); //int[] i = new int[M]; //Column indices to search for maximum in R //Vector j = new DenseVector(N); List<int> j = new List<int>(); //int[] j = new int[N]; for (int k = 1; k < M; k++) { i.Add(k); } for (int k = 0; k < N; k++) { j.Add(k); } //Initialization //Initialize the 1st row index I(1) = 1 I[0] = 0; //Initialize the 1st row of the approximate error matrix List<int> m0 = new List<int>(); m0.Add(m[I[0]]); Matrix Rik = UserImpedance(m0, n); //Find the 1st column index J(0) double max = -1.0; int col = 0; foreach (int ind in j) { if (Math.Abs(Rik[0, ind]) > max) { max = Math.Abs(Rik[0, ind]); col = ind; } } //J[0] = j[col]; J[0] = col; j.Remove(J[0]); //First row of V V = new DenseMatrix(1, Rik.ColumnCount); V.SetRow(0, Rik.Row(0).Divide(Rik[0, J[0]])); //Initialize the 1st column of the approximate error matrix List<int> n0 = new List<int>(); n0.Add(n[J[0]]); Matrix Rjk = UserImpedance(m, n0); //First column of U U = new DenseMatrix(Rjk.RowCount, 1); U.SetColumn(0, Rjk.Column(0)); // Norm of (approximate) Z, to test error double d1 = U.L2Norm(); double d2 = V.L2Norm(); double normZ = d1 * d1 * d2 * d2; //Find 2nd row index I(2) int row = 0; max = -1.0; foreach (int ind in i) { if (Math.Abs(Rjk[ind, 0]) > max) { max = Math.Abs(Rjk[ind, 0]); row = ind; } } //I[1] = i[row]; I[1] = row; i.Remove(I[1]); //Iteration for (int k = 1; k < Math.Min(M, N); k++) { //Update (Ik)th row of the approximate error matrix: List<int> t1 = new List<int>(); t1.Add(m[I[k]]); Rik = (Matrix)(UserImpedance(t1, n) - U.SubMatrix(I[k], 1, 0, U.ColumnCount).Multiply(V)); //CHECKED OK all code before works fine //Find kth column index Jk max = -1.0; col = 0; foreach (int ind in j) { if (Math.Abs(Rik[0, ind]) > max) { max = Math.Abs(Rik[0, ind]); col = ind; } } J[k] = col; j.Remove(J[k]); //Terminate if R(I(k),J(k)) == 0 if (Rik[0, J[k]] == 0) { break; } //Set k-th row of V equal to normalized error Matrix Vk = (Matrix)Rik.Divide(Rik[0, J[k]]); //Update (Jk)th column of the approximate error matrix List<int> n1 = new List<int>(); n1.Add(n[J[k]]); Rjk = (Matrix)(UserImpedance(m, n1) - U.Multiply(V.SubMatrix(0, V.RowCount, J[k], 1))); // Set k-th column of U equal to updated error Matrix Uk = Rjk; //Norm of approximate Z //Matrix s = (Matrix)(U.Transpose().Multiply(Uk)).Multiply((Vk.Multiply(V.Transpose())).Transpose()); //Matrix s = (Matrix)((U.Transpose()).Multiply(Uk)).Multiply(((Vk.Multiply(V.Transpose())).Transpose())); Matrix a = (Matrix)U.Transpose().Multiply(Uk); Matrix b = (Matrix)Vk.Multiply(V.Transpose()).Transpose(); Matrix s = (Matrix)a.PointwiseMultiply(b); double sum = 0; for (int i1 = 0; i1 < s.RowCount; i1++) { for (int j1 = 0; j1 < s.ColumnCount; j1++) { sum += s[i1, j1]; } } d1 = Uk.L2Norm(); d2 = Vk.L2Norm(); normZ += 2 * sum + d1 * d1 * d2 * d2; //Update U and V //U.SetColumn(U.ColumnCount - 1, Uk.Column(0)); //V.SetRow(V.RowCount - 1, Vk.Row(0)); U = AddColumn(U, (Vector)Uk.Column(0)); //U.SetColumn(k, Uk.Column(0)); V = AddRow(V, (Vector)Vk.Row(0)); //V.SetRow(k, Vk.Row(0)); if (d1 * d2 <= acaThres * Math.Sqrt(normZ)) { break; } if (k == Math.Min(N, M) - 1) { break; } max = -1; row = 0; foreach (int ind in i) { if (Math.Abs(Rjk[ind, 0]) > max) { max = Math.Abs(Rjk[ind, 0]); row = ind; } } I[k + 1] = row; //i = removeIndex(i,I[k+1]); i.Remove(I[k + 1]); } return new Tuple<Matrix, Matrix>(U, V); }
/// <summary> /// Samples the distribution. /// </summary> /// <param name="rnd">The random number generator to use.</param> /// <param name="degreesOfFreedom">The degrees of freedom (n) for the Wishart distribution.</param> /// <param name="scale">The scale matrix (V) for the Wishart distribution.</param> /// <param name="chol">The cholesky decomposition to use.</param> /// <returns>a random number from the distribution.</returns> static Matrix<double> DoSample(System.Random rnd, double degreesOfFreedom, Matrix<double> scale, Cholesky<double> chol) { var count = scale.RowCount; // First generate a lower triangular matrix with Sqrt(Chi-Squares) on the diagonal // and normal distributed variables in the lower triangle. var a = new DenseMatrix(count, count); for (var d = 0; d < count; d++) { a.At(d, d, Math.Sqrt(Gamma.Sample(rnd, (degreesOfFreedom - d)/2.0, 0.5))); } for (var i = 1; i < count; i++) { for (var j = 0; j < i; j++) { a.At(i, j, Normal.Sample(rnd, 0.0, 1.0)); } } var factor = chol.Factor; return factor*a*a.Transpose()*factor.Transpose(); }
private double[] getCoeffs(bool includeLinear) { double[] coeffs = null; try { DenseMatrix X = new DenseMatrix(_calibrationData.Count, 3); DenseVector Y = new DenseVector(_calibrationData.Count); int i = 0; foreach (Spot _spot in _calibrationData) { X.At(i, 0, includeLinear ? _spot.s : 0); X.At(i, 1, _spot.s * _spot.s); X.At(i, 2, _spot.s * _spot.s * _spot.s); Y.At(i, _spot.p); i++; } var XT = X.Transpose(); var A = (XT * X).Inverse() * XT * Y; coeffs = A.ToArray(); } catch (Exception ex) { coeffs = null; } return coeffs; }
private void HardConstraintsStep() { // Apuntes miki + apuntes clase // Step de Fuertes: // Tenemos que conseguir una matriz enorme formada por // // | A J^t | // | J 0 | // A es la matriz de masas-> 6 * cada componente como en soft // J es la matriz jacobiana que rellenamos en Constraint.getConstraintJacobian // J se forma 3 * numero de constraints, 6* num rigidbodies // J^t se forma 6* num rigidbodies, 3 * numero de constraints // Recordemos que cada rigidbody tiene 6 numdof // C0 = 3 * numero de constraints<- Es similar a C en soft MatrixXD massMatrix = DenseMatrixXD.CreateIdentity(m_objs.Count * 6); MatrixXD jacobian = new DenseMatrixXD(3 * Constraints.Count, 6 * m_objs.Count); VectorXD C0 = new DenseVectorXD(3 * m_constraints.Count); // paso 1: necesitamos settear la jacobiana y C0 para cada restricción foreach (Constraint c in m_constraints) { c.getConstraintVector(C0); c.getConstraintJacobian(jacobian); } // las fuerzas son 1 x 6 * m_objs.Count, igual que las velocidades. // Parece que se mete el torque y demás. 3 * objetos no funciona VectorXD total_force = new DenseVectorXD(6 * m_objs.Count); VectorXD total_velocities = new DenseVectorXD(6 * m_objs.Count); // PARA CADA RIGIDBODY // Paso 2: Limpiamos las fuerzas y matrices anteriores para evitar sumas de error // Paso 3: Establecemos la matriz de masas (Pondremos la inercia como inercia 0 porque si no es null->RigidBody.cs // Paso 4: Metemos valores en las velocidades anteriores, ¿Por qué? Porque Al final haremos un A * v = b // como en las prácticas anteriores y b es un vector formado por b y -1 * C0 / TimeStep // siendo b la fórmula (matriz_de_masas * velocidades) + (TimeStep * fuerzas); // Paso 5: Añadimos fuerzas y matrices (que las hemos limpiado antes) // Paso 6: Metemos las nuevas fuerzas (getForceVector) en el vector de fuerzas foreach (RigidBody obj in m_objs) { obj.clearForcesAndMatrices(); obj.getMassMatrix(massMatrix); obj.getVelocityVector(total_velocities); obj.addForcesAndMatrices(); obj.getForceVector(total_force); } // Creamos la super matriz que hemos dicho con la de masas, jacobianas y ceros // Al crear una MtrixXD se crea con ceros. MatrixXD jacobianT = jacobian.Transpose(); MatrixXD ceroMatrix = new DenseMatrixXD(3 * m_constraints.Count, 3 * m_constraints.Count); DenseMatrixXD megaMatrix = new DenseMatrixXD(jacobian.RowCount + massMatrix.RowCount, jacobianT.ColumnCount + massMatrix.ColumnCount); // la matriz de masas es 0,0 a (m_objs.Count * 6) // Una vez acaba esa, va la jacobiana traspuesta // Debajo de la misma manera va la jacobiana // y en la esquina abajo derecha va la de ceros megaMatrix.SetSubMatrix(0, 0, massMatrix); megaMatrix.SetSubMatrix(0, 0 + massMatrix.ColumnCount, jacobianT); megaMatrix.SetSubMatrix(0 + massMatrix.RowCount, 0, jacobian); megaMatrix.SetSubMatrix(0 + massMatrix.RowCount, 0 + massMatrix.ColumnCount, ceroMatrix); // M * v = M * v0 + timestep * fuerzas // V0 es la velocidad del step anterior // en A * v = b, b lo dividiremos como b y b2 // y los concatenamos VectorXD b = (massMatrix * total_velocities) + (TimeStep * total_force); VectorXD b2 = -1 * C0 / TimeStep; // b total - ambos bs VectorXD realB = new DenseVectorXD(b.Count + b2.Count); realB.SetSubVector(0, b.Count, b); realB.SetSubVector(b.Count, b2.Count, b2); // V se forma por velocidades y un vector de lamdas, que debe ser del tamaño de C0 ya que // b2 es escalar * c0 VectorXD lamdas = new DenseVectorXD(C0.Count); // conjunto de velocidades formada por las velocidades y las lamdas VectorXD megaV = new DenseVectorXD(total_velocities.Count + lamdas.Count); megaV.SetSubVector(0, total_velocities.Count, total_velocities); megaV.SetSubVector(total_velocities.Count, lamdas.Count, lamdas); // Resolvemos el sistema megaV = megaMatrix.Solve(realB); // nueva velocidad VectorXD newVelocities = megaV.SubVector(0, total_velocities.Count); // Establecemos las nuevas posiciones y velocidades foreach (RigidBody obj in m_objs) { obj.setVelocityVector(newVelocities); obj.advancePosition(); } }
public static DenseMatrix PseudoInverse(DenseMatrix m) { var mt = m.Transpose(); var what = (mt * m); return (DenseMatrix)(what.Inverse() * mt); }
public void ComputeProjectiveMatrices(int imgWidth, int imgHeight) { int ih = imgHeight, iw = imgWidth; // Minimise sum(wi - wc)/sum(wc), wi are projective weight of point pi, wc is weight of center point // We have pc - center point, P = [pi - pc] (3 rows x N cols matrix) // sum(wi - wc)/sum(wc) = w^T * P * P^T * w / (w^T * pc * pc^T * w), where w = [ w1, w2, 1 ] is third row of projective matrix Matrix<double> PPt = new DenseMatrix(3, 3); PPt[0, 0] = iw * ih * (iw * iw - 1) / 12.0; PPt[1, 1] = iw * ih * (ih * ih - 1) / 12.0; int iw1 = iw - 1; int ih1 = ih - 1; Matrix<double> pcpct = new DenseMatrix(3, 3); pcpct[0, 0] = iw1 * iw1 * 0.25; pcpct[0, 1] = iw1 * ih1 * 0.25; pcpct[0, 2] = iw1 * 0.5; pcpct[1, 0] = iw1 * ih1 * 0.25; pcpct[1, 1] = ih1 * ih1 * 0.25; pcpct[1, 2] = ih1 * 0.5; pcpct[2, 0] = iw1 * 0.5; pcpct[2, 1] = ih1 * 0.5; pcpct[2, 2] = 1.0; Matrix<double> F = FundamentalMatrix; // As w = [e]x * z, w' = Fz, for some z = [z1 z2 0] // Then for both images minimsed sum is equal: // zt * [e]x*P*Pt*[e]x * z / (zt * [e]x*pc*pct*[e]x * z) + zt * F*P'*Pt'*F * z / (zt * F*pc'*pct'*F * z) // Let A = [e]x*P*Pt*[e]x and B = [e]x*pc*pct*[e]x // Only upper 2x2 submatrix is used as z3 = 0 _A_L = (EpiCrossLeft.Transpose() * PPt * EpiCrossLeft).SubMatrix(0, 2, 0, 2); _A_R = (FundamentalMatrix.Transpose() * PPt * FundamentalMatrix).SubMatrix(0, 2, 0, 2); _B_L = (EpiCrossLeft.Transpose() * pcpct * EpiCrossLeft).SubMatrix(0, 2, 0, 2); _B_R = (FundamentalMatrix.Transpose() * pcpct * FundamentalMatrix).SubMatrix(0, 2, 0, 2); // Finally we need to such a z to minimise (z^T * A * z) / (z^T * B * z) + (z^T * A' * z) / (z^T * B' * z) // // We need to find some initial approximation of z // First lets find z that minimise error in one image, which is eqivalent to maximising : // e = (z^T * B * z) / (z^T * A * z) // Decompose A to A = D^T * D ( as A is symmetric positive definite ) : // d11 = sqrt(a11 - a21^2/a22), d12 = 0, d21 = a21/sqrt(a22), d22 = sqrt(a22) // D^-1 is defined as 1/(d11*d22) * [ d22, 0; -d21, d11 ] // // We have : // e = (z^T * B * z) / (z^T * D^T * D * z) // Let y = D*z, y^T = z^T * D^T => z = D^-1 * y, z^T = y^T * (D^T)^-1 // e = (y^T * (D^T)^-1 * B * D^-1 * y) / (y^T * y) // As y defined up to scale factor ( same as z ), then let ||y|| = 1 // e is maximised then if y is an eigenvector of (D^T)^-1 * B * D^-1 associated with the largest eigenvalue Matrix<double> D1_L = new DenseMatrix(2, 2); double d22 = Math.Sqrt(_A_L[1, 1]); double d21 = _A_L[1, 0] / d22; double d11 = Math.Sqrt(_A_L[0, 0] - d21 * d21); double scale = 1.0 / (d11 * d22); D1_L[0, 0] = d22 * scale; D1_L[0, 1] = 0.0; D1_L[1, 0] = -d21 * scale; D1_L[1, 1] = d11 * scale; Matrix<double> DtBD_L = D1_L.Transpose() * _B_L * D1_L; var evd_L = DtBD_L.Evd(); var y_L = evd_L.EigenVectors.Row(1); var z_L = D1_L * y_L; Matrix<double> D1_R = new DenseMatrix(2, 2); d22 = Math.Sqrt(_A_R[1, 1]); d21 = _A_R[1, 0] / d22; d11 = Math.Sqrt(_A_R[0, 0] - d21 * d21); scale = 1.0 / (d11 * d22); D1_R[0, 0] = d22 * scale; D1_R[0, 1] = 0.0; D1_R[1, 0] = -d21 * scale; D1_R[1, 1] = d11 * scale; Matrix<double> DtBD_R = D1_R.Transpose() * _B_R * D1_R; var evd_R = DtBD_R.Evd(); var y_R = evd_R.EigenVectors.Row(1); var z_R = D1_R * y_R; // Set initial estimate as vector between vectors minimising errors in each image // (its a close one, so may serve as final result if error is small enough) var z_init = (z_L / z_L.L2Norm() + z_R / z_R.L2Norm()) / 2.0; // Scale z, so that z2 = 1 var z = new DenseVector(3); z[0] = z_L[0] / z_L[1]; z[1] = 1.0; z[2] = 0.0; double error = ComputeProjectionError(z[0]); // Iteratively minimise : // zt*A_L*z/(zt*B_L*z) + zt*A_R*z/(zt*B_R*z) // z = [ z1 1 0 ] // So only one parameter z1 OneVariableMinimisation miniAlg = new OneVariableMinimisation() { DoComputeDerivativesNumerically = true, NumericalDerivativeStep = 1e-3, MaximumIterations = 100, InitialParameter = z[0], Function = ComputeProjectionError, // Derivative_1st = ComputeProjectionError_Derivative_1st, // Derivative_2nd = ComputeProjectionError_Derivative_2nd }; miniAlg.Process(); if(error > miniAlg.MinimalValue) z[0] = miniAlg.MinimalParameter; // w = [e]x * z, w' = Fz _Hp_L = DenseMatrix.CreateIdentity(3); _Hp_R = DenseMatrix.CreateIdentity(3); var w_L = EpiCrossLeft * z; var w_R = FundamentalMatrix * z; _Hp_L[2, 0] = w_L[0] / w_L[2]; _Hp_L[2, 1] = w_L[1] / w_L[2]; _Hp_R[2, 0] = w_R[0] / w_R[2]; _Hp_R[2, 1] = w_R[1] / w_R[2]; }