static void Main(string[] args)
{
Transform3D[] trns = new Transform3D[4];
MathNet.Numerics.LinearAlgebra.Matrix <double> identity = MathNet.Numerics.LinearAlgebra.Matrix <double> .Build.Dense(3, 3);
identity[0, 0] = 1;
identity[1, 1] = 1;
identity[2, 2] = 1;
MathNet.Numerics.LinearAlgebra.Vector <double> v1 = MathNet.Numerics.LinearAlgebra.Vector <double> .Build.Dense(3);
v1[0] = 0;
MathNet.Numerics.LinearAlgebra.Vector <double> v2 = MathNet.Numerics.LinearAlgebra.Vector <double> .Build.Dense(3);
v2[0] = 9;
MathNet.Numerics.LinearAlgebra.Vector <double> v3 = MathNet.Numerics.LinearAlgebra.Vector <double> .Build.Dense(3);
v3[0] = 10;
MathNet.Numerics.LinearAlgebra.Vector <double> v4 = MathNet.Numerics.LinearAlgebra.Vector <double> .Build.Dense(3);
v4[0] = 11;
trns[0] = new Transform3D(identity, v1);
trns[1] = new Transform3D(identity, v2);
trns[2] = new Transform3D(identity, v3);
trns[3] = new Transform3D(identity, v4);
Candidate.initSums(42, 3.14, 2.72);
Density d = new Density(); // finder, we need an instance for certain complicated reason
Transform3D solution = d.Find(trns);
Console.WriteLine(solution);
}
public static MathNet.Numerics.LinearAlgebra.Matrix <T> pow <T>(MathNet.Numerics.LinearAlgebra.Matrix <T> M, int n)
where T : struct, System.IEquatable <T>, System.IFormattable
{
if (M.RowCount != M.ColumnCount)
{
throw new System.ArgumentException("It's imposible to take non-square matrix to power!");
}
var M2 = M.SubMatrix(0, M.RowCount, 0, M.ColumnCount);
if (n == 0)
{
for (var j = 0; j < M.RowCount; j++)
{
for (var k = 0; k < M.ColumnCount; k++)
{
if (j == k)
{
M2[j, k] = MathNet.Numerics.LinearAlgebra.Matrix <T> .One;
}
else
{
M2[j, k] = MathNet.Numerics.LinearAlgebra.Matrix <T> .Zero;
}
}
}
return(M2);
}
if (n < 0)
{
M2 = M2.Inverse();
}
//for (int j = 1; j < Math.Abs(n); j++)
// M = M * M;
return(M2.Power(System.Math.Abs(n)));
}
public void TestConvertParsleyToEmgu()
{
MathNet.Numerics.LinearAlgebra.Vector v = new MathNet.Numerics.LinearAlgebra.Vector(new double[] { 1.0f, 2.0f, 3.0f });
Emgu.CV.Structure.MCvPoint3D32f f = v.ToEmguF();
Assert.AreEqual(1.0, f.x);
Assert.AreEqual(2.0, f.y);
Assert.AreEqual(3.0, f.z);
Emgu.CV.Structure.MCvPoint3D64f d = v.ToEmgu();
Assert.AreEqual(1.0, d.x);
Assert.AreEqual(2.0, d.y);
Assert.AreEqual(3.0, d.z);
double[,] data = new double[2, 3] {
{ 1.0, 2.0, 3.0 }, { 4.0, 5.0, 6.0 }
};
MathNet.Numerics.LinearAlgebra.Matrix m = MathNet.Numerics.LinearAlgebra.Matrix.Create(data);
Emgu.CV.Matrix <double> m2 = m.ToEmgu();
Assert.AreEqual(data[0, 0], m2[0, 0]);
Assert.AreEqual(data[0, 1], m2[0, 1]);
Assert.AreEqual(data[0, 2], m2[0, 2]);
Assert.AreEqual(data[1, 0], m2[1, 0]);
Assert.AreEqual(data[1, 1], m2[1, 1]);
Assert.AreEqual(data[1, 2], m2[1, 2]);
}
public static MathNet.Numerics.LinearAlgebra.Matrix <T> inv <T>(MathNet.Numerics.LinearAlgebra.Matrix <T> M)
where T : struct, System.IEquatable <T>, System.IFormattable
{
//if (M.RowCount != M.ColumnCount)
//throw new DimensionMismatchException("It's imposible to calculate inverse matrix of non-square matrix!");
return(M.Inverse());
}
public static MathNet.Numerics.LinearAlgebra.Vector <T> solve <T>(MathNet.Numerics.LinearAlgebra.Matrix <T> M)
where T : struct, System.IEquatable <T>, System.IFormattable
{
var result = M.SubMatrix(0, M.RowCount, 0, M.ColumnCount - 1).Solve(M.Column(M.ColumnCount - 1));
return(result);
}
public void AddTranslation(float x, float y)
{
var m = MathNet.Numerics.LinearAlgebra.Matrix <float> .Build.DenseIdentity(3);
m[0, 2] = x;
m[1, 2] = y;
matrix = matrix * m;
}
public void AddScale(float xscale, float yscale)
{
var m = MathNet.Numerics.LinearAlgebra.Matrix <float> .Build.DenseIdentity(3);
m[0, 0] = xscale;
m[1, 1] = yscale;
matrix = matrix * m;
}
/// <summary>
/// Private constructor for transformations
/// </summary>
/// <param name="matrix">Matrix</param>
private Matrix3x3(MathNet.Numerics.LinearAlgebra.Matrix <float> matrix)
{
if (matrix.RowCount != 3 || matrix.ColumnCount != 3)
{
throw new System.ArgumentException("Matrix is not square 3x3");
}
_values = matrix;
}
private double[] getMatrixRow(MathNet.Numerics.LinearAlgebra.Matrix m, int idx)
{
double[] res = new double[m.ColumnCount];
for (int i = 0; i < m.ColumnCount; ++i)
{
res[i] = m[idx, i];
}
return(res);
}
private static MathNet.Numerics.LinearAlgebra.Vector <double> Multiply2(MathNet.Numerics.LinearAlgebra.Matrix <double> matrix, MathNet.Numerics.LinearAlgebra.Vector <double> rightSide)
{
MathNet.Numerics.LinearAlgebra.VectorBuilder <double> build = MathNet.Numerics.LinearAlgebra.Vector <double> .Build;
MathNet.Numerics.LinearAlgebra.Vector <double> ret = build.SameAs(matrix, rightSide, matrix.RowCount);
Transformer.DoMultiply(matrix, rightSide, ret);
return(ret);
}
public static Matrix4D ToMatrix4D(this MathNet.Numerics.LinearAlgebra.Matrix <double> value)
{
return(new Matrix4D
(
value[0, 0], value[0, 1], value[0, 2], value[0, 3],
value[1, 0], value[1, 1], value[1, 2], value[1, 3],
value[2, 0], value[2, 1], value[2, 2], value[2, 3],
value[3, 0], value[3, 1], value[3, 2], value[3, 3]
));
}
private static QuantityBase MultiplyDoubleMatrix(QuantityBase left, QuantityBase right)
{
Quantity <double> leftQ = (Quantity <double>)left;
Quantity <MathNet.Numerics.LinearAlgebra.Matrix <double> > rightQ = (Quantity <MathNet.Numerics.LinearAlgebra.Matrix <double> >)right;
IUnit unit = MultiplyUnits(left, right);
MathNet.Numerics.LinearAlgebra.Matrix <double> matrix = leftQ.Value * rightQ.Value;
return(new Quantity <MathNet.Numerics.LinearAlgebra.Matrix <double> >(matrix, unit));
}
private void creatMatrixForLSI()//func for LSI
{
createWordsMap();
createDocsMap();
int rows = m_wordsMap.Count;
int cols = m_docsMap.Count;
double[,] A = new double[rows, cols];
double[,] q = new double[rows, 1];
foreach (string word in m_wordsMap.Keys) //create A matrix
{
long ptr = m_Dictionary[word];
string rec = findPostingRec(@"C:\Users\Daniel\Documents\Visual Studio 2015\Projects\IR\IR\bin\Debug\t\PostingF.txt", ptr);
List <string> docsOfword = new List <string>();
string[] sRec = rec.Split(' ');
for (int i = 0; i < sRec.Length; i++)
{
if (sRec[i].EndsWith("#"))
{
docsOfword.Add(sRec[i].Substring(0, sRec[i].Length - 1));
}
}
int Arow = m_wordsMap[word];
foreach (string doc in docsOfword)
{
int Acol = m_docsMap[doc];
A[Arow, Acol] = 1;
}
q[Arow, 0] = 1; //set q vector
}
var Amatrix = DenseMatrix.OfArray(A);
var svd = Amatrix.Svd(true);
var D_T = svd.VT;
var T = svd.U;
var S = svd.W;
int k = 1;
var D_Tk = D_T.SubMatrix(0, k, 0, D_T.ColumnCount);
var Tk = T.SubMatrix(0, T.RowCount, 0, k);
var Sk = S.SubMatrix(0, k, 0, k);
var Sk_inver = Sk.Inverse();
m_Dk_T = D_Tk;
m_Sk_invers = Sk_inver;
m_Tk = Tk;
var Qmatrix = DenseMatrix.OfArray(q);
var q_T = Qmatrix.Transpose();
var qMatrix = q_T * m_Tk * m_Sk_invers;
m_q = qMatrix.Row(0);
}
protected override int InitData(int iT)
{
A = GenMatrix(rnd, iT);
B = GenMatrix(rnd, iT);
xA = ConvertM(A);
xB = ConvertM(B);
mA = MathNet.Numerics.LinearAlgebra.Matrix <double> .Build.SparseOfArray(A);
mB = MathNet.Numerics.LinearAlgebra.Matrix <double> .Build.SparseOfArray(B);
return(1);
}
/// <summary>
/// Convert MathNet.Numerics.LinearAlgebra.Matrix to Emgu.CV.Matrix
/// </summary>
/// <param name="m">MathNet.Numerics.LinearAlgebra.Matrix</param>
/// <returns>Emgu.CV.Matrix<double></returns>
public static Emgu.CV.Matrix <double> ToEmgu(this MathNet.Numerics.LinearAlgebra.Matrix m)
{
Emgu.CV.Matrix <double> res = new Emgu.CV.Matrix <double>(m.RowCount, m.ColumnCount);
for (int r = 0; r < m.RowCount; ++r)
{
for (int c = 0; c < m.ColumnCount; ++c)
{
res[r, c] = m[r, c];
}
}
return(res);
}
/// <summary>
/// Raises this square matrix to a positive integer exponent and places the result into the result matrix
/// </summary>
/// <param name="exponent">Positive exponent</param>
/// <returns>Matrix raised to a power of</returns>
public Matrix3x3 Power(int exponent)
{
if (exponent < 0)
{
throw new System.ArgumentException("The exponent cannot be a negative number");
}
MathNet.Numerics.LinearAlgebra.Matrix <float> resultValues =
MathNet.Numerics.LinearAlgebra.Matrix <float> .Build.Dense(3, 3);
_values.Power(exponent, resultValues);
return(new Matrix3x3(resultValues));
}
static void Main(string[] args)
{
//MachineLearningHelper.Configuration.Delimiter = "\t";
MachineLearningHelper.FetchRecords <Record>("sample.csv").CorrelationMatrix().ToConsole(true);
double[][] data = MachineLearningHelper.FetchRecords <Record>("sample.csv");
MathNet.Numerics.LinearAlgebra.Matrix <double> matrix = data.CorrelationMatrix();
matrix.ToConsole();
ReadKey();
}
/// <summary>
/// Transforms the given 3D vector point using the given transformation matrix.
/// </summary>
/// <param name="TMatrix"> The 4x4 transformation matrix containing rotational and translational matrix. </param>
/// <param name="w"> Determines the scaling parameter for the translation: 0... neglect translation; 1... include translation. </param>
/// <param name="PointVector"> The IEnumerable object of type Vector (e.g. List, array,... of 3D point vectors). </param>
/// <returns> Transformed Points as IEnumerable of type Vector. </returns>
public static IEnumerable<MathNet.Numerics.LinearAlgebra.Vector> TransformVectorToVector(MathNet.Numerics.LinearAlgebra.Matrix TMatrix, double w,
IEnumerable<MathNet.Numerics.LinearAlgebra.Vector> PointVector)
{
foreach(MathNet.Numerics.LinearAlgebra.Vector v in PointVector)
{
//convert vector to a column matrix
MathNet.Numerics.LinearAlgebra.Matrix colMatrix = new MathNet.Numerics.LinearAlgebra.Matrix(4, 1);
colMatrix[3, 0] = w;
colMatrix.SetMatrix(0, 2, 0, 0, v.ToColumnMatrix());
//perform transformation of the point - column vector is extracted after the multiplication
yield return (TMatrix.Multiply(colMatrix)).GetColumnVector(0).ToNonHomogeneous();
}
}
private static Complex[,] ToCompMas(MathNet.Numerics.LinearAlgebra.Matrix <System.Numerics.Complex> t)
{
System.Numerics.Complex[,] mas = t.ToArray();
Complex[,] res = new Complex[mas.GetLength(0), mas.GetLength(1)];
for (int i = 0; i < mas.GetLength(0); i++)
{
for (int j = 0; j < mas.GetLength(1); j++)
{
res[i, j] = new Complex(mas[i, j].Real, mas[i, j].Imaginary);
}
}
return(res);
}
/// <summary>
/// Transforms the given 3D vector point using the given transformation matrix.
/// </summary>
/// <param name="TMatrix"> The 4x4 transformation matrix containing rotational and translational matrix. </param>
/// <param name="w"> Determines the scaling parameter for the translation: 0... neglect translation; 1... include translation. </param>
/// <param name="PointVector"> The IEnumerable object of type Vector (e.g. List, array,... of 3D point vectors). </param>
/// <returns> Transformed Points as IEnumerable of type Vector. </returns>
public static IEnumerable <MathNet.Numerics.LinearAlgebra.Vector> TransformVectorToVector(MathNet.Numerics.LinearAlgebra.Matrix TMatrix, double w,
IEnumerable <MathNet.Numerics.LinearAlgebra.Vector> PointVector)
{
foreach (MathNet.Numerics.LinearAlgebra.Vector v in PointVector)
{
//convert vector to a column matrix
MathNet.Numerics.LinearAlgebra.Matrix colMatrix = new MathNet.Numerics.LinearAlgebra.Matrix(4, 1);
colMatrix[3, 0] = w;
colMatrix.SetMatrix(0, 2, 0, 0, v.ToColumnMatrix());
//perform transformation of the point - column vector is extracted after the multiplication
yield return((TMatrix.Multiply(colMatrix)).GetColumnVector(0).ToNonHomogeneous());
}
}
private static MathNet.Numerics.LinearAlgebra.Matrix <double> DoMultiply(MathNet.Numerics.LinearAlgebra.Matrix <double> matrix, MathNet.Numerics.LinearAlgebra.Vector <double> rightSide, MathNet.Numerics.LinearAlgebra.Vector <double> result)
{
for (var i = 0; i < matrix.RowCount; i++)
{
double s = 0.0;
for (var j = 0; j < 008; j++)
{
double ddd = matrix.Storage.At(i, j);
s += ddd * rightSide[j];
}
result[i] = s;
}
return(matrix);
}
/// <summary>
/// Transforms the given 3D vector point using the given transformation matrix.
/// </summary>
/// <param name="TMatrix"> The 4x4 transformation matrix containing rotational and translational matrix. </param>
/// <param name="w"> Determines the scaling parameter for the translation: 0... neglect translation; 1... include translation.</param>
/// <param name="PointVector"> The IEnumerable object of type Vector (e.g. List, array,... of 3D point vectors). </param>
/// <returns> Transformed Points as IEnumerable of Emgu type MCvPoint3D32f. </returns>
public static IEnumerable<Emgu.CV.Structure.MCvPoint3D32f> TransformVectorToEmgu(MathNet.Numerics.LinearAlgebra.Matrix TMatrix, double w,
IEnumerable<MathNet.Numerics.LinearAlgebra.Vector> PointVector)
{
foreach (MathNet.Numerics.LinearAlgebra.Vector v in PointVector)
{
//convert vector to a column matrix
MathNet.Numerics.LinearAlgebra.Matrix colMatrix = new MathNet.Numerics.LinearAlgebra.Matrix(4, 1);
colMatrix[3, 0] = w;
colMatrix.SetMatrix(0, 2, 0, 0, v.ToColumnMatrix());
//perform transformation of the point - column vector is extracted after the multiplication
//the resulting vector is converted to the Emgu type MCvPoint3D32f using ToEmguF().
yield return (TMatrix.Multiply(colMatrix)).GetColumnVector(0).ToNonHomogeneous().ToEmguF();
}
}
private void button5_Click(object sender, EventArgs e)
{
MathNet.Numerics.LinearAlgebra.Matrix <double> aMatrix =
MathNet.Numerics.LinearAlgebra.Matrix <double> .Build.Dense(2, 2);
aMatrix[0, 0] = 0.2;
aMatrix[0, 1] = 0.3;
aMatrix[1, 0] = 0.8;
aMatrix[1, 1] = 0.7;
double[] expTriProb = new double[2] {
0.7, 0.3
};
SolverCLP.objFuncDel fAndcDel = new ObjectiveFunctions(aMatrix, expTriProb).objectFuncQHStyle;
var s2 = SolverCLP.solver(fAndcDel);
}
/// <summary>
/// Transforms the given 3D vector point using the given transformation matrix.
/// </summary>
/// <param name="TMatrix"> The 4x4 transformation matrix containing rotational and translational matrix. </param>
/// <param name="w"> Determines the scaling parameter for the translation: 0... neglect translation; 1... include translation.</param>
/// <param name="PointVector"> The IEnumerable object of type Vector (e.g. List, array,... of 3D point vectors). </param>
/// <returns> Transformed Points as IEnumerable of Emgu type MCvPoint3D32f. </returns>
public static IEnumerable <Emgu.CV.Structure.MCvPoint3D32f> TransformVectorToEmgu(MathNet.Numerics.LinearAlgebra.Matrix TMatrix, double w,
IEnumerable <MathNet.Numerics.LinearAlgebra.Vector> PointVector)
{
foreach (MathNet.Numerics.LinearAlgebra.Vector v in PointVector)
{
//convert vector to a column matrix
MathNet.Numerics.LinearAlgebra.Matrix colMatrix = new MathNet.Numerics.LinearAlgebra.Matrix(4, 1);
colMatrix[3, 0] = w;
colMatrix.SetMatrix(0, 2, 0, 0, v.ToColumnMatrix());
//perform transformation of the point - column vector is extracted after the multiplication
//the resulting vector is converted to the Emgu type MCvPoint3D32f using ToEmguF().
yield return((TMatrix.Multiply(colMatrix)).GetColumnVector(0).ToNonHomogeneous().ToEmguF());
}
}
/// <summary>
/// Based on the given the current rigid body list, it Updates the avaliable joints.
/// When done an event is fired (JointListAvaliable) containing the updated JointList
/// </summary>
/// <param name="trackableList">The trackable list</param>
public static void UpdateJoints(List <Trackable> trackableList)
{
if (clearCoRData)
{
ClearCoRData();
}
int jointCount = jointList.Count;
int trackableCount = trackableList.Count;
int[] trackableIndex = new int[maxTrackableID];
// Build the trackableIndex (add 1 to i (otherwise you need to initialise the intex to a value other than 0 which wasts cycles))
for (int i = 0; i < trackableCount; i++)
{
trackableIndex[trackableList[i].ID] = i + 1;
}
// Go through the trackableIndex, if the value in the index equal to the trackable1 and trackable2 isn't equal to 0 then they exist
for (int j = 0; j < jointCount; j++)
{
if (trackableIndex[jointList[j].Trackable1] != 0 && trackableIndex[jointList[j].Trackable2] != 0)
{
jointList[j].Exists = true;
// Take 1 off of the value in the Index to make up for it being added when being built
jointList[j].Yaw = Math.Round(trackableList[trackableIndex[jointList[j].Trackable2] - 1].Yaw - trackableList[trackableIndex[jointList[j].Trackable1] - 1].Yaw + jointList[j].YawOffset, 1);
jointList[j].Pitch = Math.Round(trackableList[trackableIndex[jointList[j].Trackable2] - 1].Pitch - trackableList[trackableIndex[jointList[j].Trackable1] - 1].Pitch + jointList[j].PitchOffset, 1);
jointList[j].Roll = Math.Round(trackableList[trackableIndex[jointList[j].Trackable2] - 1].Roll - trackableList[trackableIndex[jointList[j].Trackable1] - 1].Roll + jointList[j].RollOffset, 1);
jointList[j].TimeStamp = trackableList[trackableIndex[jointList[j].Trackable2] - 1].TimeStamp;
// START Added for MEng Project
MathNet.Numerics.LinearAlgebra.Matrix CoR = jointCoRList[j].FindCoR(trackableList[trackableIndex[jointList[j].Trackable1] - 1], trackableList[trackableIndex[jointList[j].Trackable2] - 1]);
jointList[j].xCoordinate = (int)CoR[0, 0];
jointList[j].yCoordinate = (int)CoR[1, 0];
jointList[j].zCoordinate = (int)CoR[2, 0];
// END Added for MEng Project
}
else
{
jointList[j].Exists = false;
}
}
OnJointListAvaliable(jointList);
}
public void AddRotation(float angleInDegrees)
{
float r = (float)(angleInDegrees * Math.PI / 180.0F);
var m = MathNet.Numerics.LinearAlgebra.Matrix <float> .Build.Dense(3, 3);
m[0, 0] = (float)Math.Cos(r);
m[0, 1] = (float)-Math.Sin(r);
m[0, 2] = 0;
m[1, 0] = (float)Math.Sin(r);
m[1, 1] = (float)Math.Cos(r);
m[1, 2] = 0;
m[2, 0] = 0;
m[2, 1] = 0;
m[2, 2] = 1;
matrix = matrix * m;
}
// [Fact]
// public void Test2()
// {
// var m1 = GetTestMatrix();
// var m2 = GetTestMatrixOld();
// var v1 = m1.Image(new Vector3(1, 2, 3));
// var v2 = m2.Image(Vector.CreateVector3(1, 2, 3));
// Console.WriteLine(v1);
// Console.WriteLine(v2);
// AssertVectorEqual(v1, v2);
// }
public void AssertMatrixEqual(Matrix4x4 m1, MathNet.Numerics.LinearAlgebra.Matrix <double> m2)
{
Assert.Equal(m1.M11, m2[0, 0]);
Assert.Equal(m1.M12, m2[0, 1]);
Assert.Equal(m1.M13, m2[0, 2]);
Assert.Equal(m1.M14, m2[0, 3]);
Assert.Equal(m1.M21, m2[1, 0]);
Assert.Equal(m1.M22, m2[1, 1]);
Assert.Equal(m1.M23, m2[1, 2]);
Assert.Equal(m1.M24, m2[1, 3]);
Assert.Equal(m1.M31, m2[2, 0]);
Assert.Equal(m1.M32, m2[2, 1]);
Assert.Equal(m1.M33, m2[2, 2]);
Assert.Equal(m1.M34, m2[2, 3]);
Assert.Equal(m1.M41, m2[3, 0]);
Assert.Equal(m1.M42, m2[3, 1]);
Assert.Equal(m1.M43, m2[3, 2]);
Assert.Equal(m1.M44, m2[3, 3]);
}
private static MathNet.Numerics.LinearAlgebra.Matrix <double> PseudoInverse2(MathNet.Numerics.LinearAlgebra.Double.Matrix matrix)
{
MathNet.Numerics.LinearAlgebra.Factorization.Svd <double> svd = MathNet.Numerics.LinearAlgebra.Double.Factorization.UserSvd.Create(matrix, true);
MathNet.Numerics.LinearAlgebra.Matrix <double> w = svd.W;
MathNet.Numerics.LinearAlgebra.Vector <double> s = svd.S;
double tolerance = 008 * svd.L2Norm * System.Math.Pow(2, -53);
for (int i = 0; i < s.Count; i++)
{
s[i] = s[i] < tolerance ? 0 : 1 / s[i];
}
for (var i = 0; i < 008; i++)
{
double value = s.At(i);
w.Storage.At(i, i, value);
}
MathNet.Numerics.LinearAlgebra.Matrix <double> ddd = svd.U * w * svd.VT;
return(ddd.Transpose());
}
public static Vector2 trilaterate2DLinear(List <BleNode> nodes, List <double> distances)
{
if (nodes.Count == 3 && distances.Count == 3)
{
MathNet.Numerics.LinearAlgebra.Vector <double> vA = MathNet.Numerics.LinearAlgebra.Vector <double> .Build.Dense(new double[] { nodes[0].X, nodes[0].Y });
double[] b = { 0, 0 };
b[0] = 0.5 * (Math.Pow(distances[0], 2) - Math.Pow(distances[1], 2) + Math.Pow(getDistance(nodes[1], nodes[0]), 2));
b[1] = 0.5 * (Math.Pow(distances[0], 2) - Math.Pow(distances[2], 2) + Math.Pow(getDistance(nodes[2], nodes[0]), 2));
MathNet.Numerics.LinearAlgebra.Vector <double> vb = MathNet.Numerics.LinearAlgebra.Vector <double> .Build.Dense(b);
double[,] A = { { nodes[1].X - nodes[0].X, nodes[1].Y - nodes[0].Y }, { nodes[2].X - nodes[0].X, nodes[2].Y - nodes[0].Y } };
MathNet.Numerics.LinearAlgebra.Matrix <double> mA = MathNet.Numerics.LinearAlgebra.Matrix <double> .Build.DenseOfArray(A);
MathNet.Numerics.LinearAlgebra.Matrix <double> mAT = mA.Transpose();
MathNet.Numerics.LinearAlgebra.Vector <double> x = MathNet.Numerics.LinearAlgebra.Vector <double> .Build.Dense(2);
double det = mA.Multiply(mAT).Determinant();
if (det > 0.1)
{
x = (mA.Transpose() * mA).Inverse() * (mA.Transpose() * vb);
}
else
{
x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
}
x.Add(vA);
double[] coordinates = x.ToArray();
Vector2 vector = new Vector2((float)coordinates[0], (float)coordinates[1]);
return(vector);
}
return(new Vector2());
}
internal Transform3D toTransform3D()
{
MathNet.Numerics.LinearAlgebra.Matrix <double> r = MathNet.Numerics.LinearAlgebra.Matrix <double> .Build.Dense(3, 3);
MathNet.Numerics.LinearAlgebra.Vector <double> tr = MathNet.Numerics.LinearAlgebra.Vector <double> .Build.Dense(3);
r[0, 0] = rot[0];
r[0, 1] = rot[1];
r[0, 2] = rot[2];
r[1, 0] = rot[3];
r[1, 1] = rot[4];
r[1, 2] = rot[5];
r[2, 0] = rot[6];
r[2, 1] = rot[7];
r[2, 2] = rot[8];
var Rt0 = r * t0;
tr[0] = t[0] - Rt0[0];
tr[1] = t[1] - Rt0[1];
tr[2] = t[2] - Rt0[2];
return(new Transform3D(r, tr));
}
private Pose2D getQmin(List <Correlation <Vector2> > assoc)
{
MathNet.Numerics.LinearAlgebra.Matrix A = new MathNet.Numerics.LinearAlgebra.Matrix(3, 3, 0);
MathNet.Numerics.LinearAlgebra.Matrix b = new MathNet.Numerics.LinearAlgebra.Matrix(3, 1, 0);
int n_assoc = assoc.Count;
for (int k = 0; k < n_assoc; k++)
{
Vector2 pi = assoc[k].Point1;
Vector2 ci = assoc[k].Point2;
double ki = pi.X * pi.X + pi.Y * pi.Y + LMET * LMET;
double cxpxPcypy = ci.X * pi.X + ci.Y * pi.Y;
double cxpyMcypx = ci.X * pi.Y - ci.Y * pi.X;
A[0, 0] = A[0, 0] + 1.0 - pi.Y * pi.Y / ki;
A[0, 1] = A[0, 1] + pi.X * pi.Y / ki;
A[0, 2] = A[0, 2] - ci.Y + pi.Y / ki * cxpxPcypy;
A[1, 1] = A[1, 1] + 1.0 - pi.X * pi.X / ki;
A[1, 2] = A[1, 2] + ci.X - pi.X / ki * cxpxPcypy;
A[2, 2] = A[2, 2] + ci.X * ci.X + ci.Y * ci.Y - cxpxPcypy * cxpxPcypy / ki;
b[0, 0] = b[0, 0] + ci.X - pi.X - pi.Y / ki * cxpyMcypx;
b[1, 0] = b[1, 0] + ci.Y - pi.Y + pi.X / ki * cxpyMcypx;
b[2, 0] = b[2, 0] + (cxpxPcypy / ki - 1.0) * cxpyMcypx;
}
// % Complete the A-matrix by assigning the symmetric portions of it
A[1, 0] = A[0, 1];
A[2, 0] = A[0, 2];
A[2, 1] = A[1, 2];
MathNet.Numerics.LinearAlgebra.Matrix Q_Min = -1 * (A.Inverse() * b);
return(new Pose2D(Q_Min[0, 0], Q_Min[1, 0], Q_Min[2, 0]));
}
/// <summary>
/// Обратная матрица через метод из библиотеки MathNet. Когда мой метод работает хорошо, этот в половине случаев работает ещё лучше. Но когда у меня плохо, у этого лучше только на доли процентов
/// </summary>
/// <returns></returns>
public CSqMatrix InvertByMathNet()
{
System.Numerics.Complex[,] mas = new System.Numerics.Complex[this.ColCount, this.ColCount];
for (int i = 0; i < this.ColCount; i++)
{
for (int j = 0; j < this.ColCount; j++)
{
mas[i, j] = new System.Numerics.Complex(this[i, j].Re, this[i, j].Im);
}
}
MathNet.Numerics.LinearAlgebra.Matrix <System.Numerics.Complex> matrix = MathNet.Numerics.LinearAlgebra.Complex.DenseMatrix.OfArray(mas);
mas = matrix.Inverse().ToArray();
Complex[,] res = new Complex[this.ColCount, this.ColCount];
for (int i = 0; i < this.ColCount; i++)
{
for (int j = 0; j < this.ColCount; j++)
{
res[i, j] = new Complex(mas[i, j].Real, mas[i, j].Imaginary);
}
}
return(new CSqMatrix(res));
}
/// <summary>Optimizes the specified data</summary>
/// <param name="data">data</param>
/// <param name="inverse_data">data</param>
/// <param name="W">W</param>
/// <param name="H">H</param>
void Optimize(IBooleanMatrix data, IBooleanMatrix inverse_data, Matrix<double> W, Matrix<double> 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 MathNet.Numerics.LinearAlgebra.Matrix(num_factors, num_factors);
MathNet.Numerics.LinearAlgebra.Matrix m_inv;
// TODO speed up using more parts of that library
// TODO using properties gives a 3-5% performance penalty
// 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);
// prepare KDD Cup specific weighting
int num_user_items = row.Count;
int user_positive_weight_sum = 0;
foreach (int i in row)
user_positive_weight_sum += inverse_data.NumEntriesByRow(i);
double neg_weight_normalization = (double) (num_user_items * (1 + CPos)) / (Feedback.Count - user_positive_weight_sum);
// TODO precompute
// TODO check whether this is correct
// 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] * CPos;
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;
for (int i = 0; i < inverse_data.NumberOfRows; i++)
if (row.Contains(i))
d += H[i, f] * (1 + CPos);
else
d += H[i, f] * inverse_data.NumEntriesByRow(i) * neg_weight_normalization;
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;
}
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] = d;
}
}
}
public static MathNet.Numerics.LinearAlgebra.Matrix ToMathNet( this Matrix m)
{
double[][] mdata = MathNet.Numerics.LinearAlgebra.Matrix.CreateMatrixData(3, 3);
mdata[0][0] = m.M11;
mdata[0][1] = m.M12;
mdata[0][2] = m.M13;
mdata[1][0] = m.M21;
mdata[1][1] = m.M22;
mdata[1][2] = m.M23;
mdata[2][0] = m.M31;
mdata[2][1] = m.M32;
mdata[2][2] = m.M33;
MathNet.Numerics.LinearAlgebra.Matrix mnet = new MathNet.Numerics.LinearAlgebra.Matrix(mdata);
return mnet;
}
/// <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<double> W, Matrix<double> 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 MathNet.Numerics.LinearAlgebra.Matrix(num_factors, num_factors);
MathNet.Numerics.LinearAlgebra.Matrix m_inv;
// TODO speed up using more parts of that library
// 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;
}
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] = d;
}
}
}
