/// <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];
}