// Update is called once per frame
public FFData FeedForward(double[] _inputFeatures)
{
FFData data = new FFData();
data.input = _inputFeatures;
//Add bias unit to input array
data.a1 = DenseMatrix.Create(1, 1, 1).Append(DenseVector.OfArray(_inputFeatures).ToRowMatrix());
//Multiple input array (a1) by weights for each node in hidden layer to produce number at each node
data.z2 = data.a1.Multiply(theta1.Transpose());
//Run logistic function on results to squash
data.a2 = MatrixSigmoid(data.z2);
//Add bias unit to results of hidden layer
data.a2 = DenseMatrix.Create(1, 1, 1).Append(data.a2);
//Multiply results of hidden layer with weights for output node to produce final result
data.z3 = data.a2.Multiply(theta2.Transpose());
//Squash final result
data.a3 = MatrixSigmoid(data.z3);
//As we are using only 1 output, result is a 1x1 matrix
return(data);
}
public static Matrix <double> WorldToObserver(double[] eye, double[] target, double[] up)
{
/*
* Возвращает матрицу трансформации, с помощью которой
* все объекты сцены получат новые координаты в пространстве,
* центром которого является позиция камеры.
*
| XAxis.x XAxis.y XAxis.z -(XAxis*eye) |
| YAxis.x YAxis.y YAxis.z -(YAxis*eye) |
| ZAxis.x ZAxis.y ZAxis.z -(ZAxis*eye) |
| 0 0 0 1 |
|
| eye - позиция камеры в мировом пространстве
| target - позиция цели, на которую направлена камера
| up - вектор, направленный вертикально вверх с точки зрения камеры
*/
Vector <double> v_eye = DenseVector.OfArray(eye);
Vector <double> z_axis = DenseVector.OfArray(NormalizeArray(SubstractArrays(eye, target)));
Vector <double> x_axis = DenseVector.OfArray(NormalizeArray(Arrays3CrossProduct(up, z_axis.ToArray())));
Vector <double> y_axis = DenseVector.OfArray(up);
return(TransformMatrix(
new Point(x_axis[0], y_axis[0], z_axis[0]),
new Point(x_axis[1], y_axis[1], z_axis[1]),
new Point(x_axis[2], y_axis[2], z_axis[2]),
new Point(-x_axis.DotProduct(v_eye), -y_axis.DotProduct(v_eye), -z_axis.DotProduct(v_eye))
));
}
/// <summary>
/// 対象物の位置をロボット座標系で取得します。
/// </summary>
/// <param name="blob"></param>
/// <returns></returns>
public static (double x, double y) GetTargetRobotCoordinate(SettingsObj obj, ConnectedComponents.Blob blob)
{
var srcList = new List <DenseVector>(4);
var dstList = new List <DenseVector>(4);
// 各座標系の4点をリストに詰める
srcList.Add(DenseVector.OfArray(new double[] { obj.TopLeftArPoseX, obj.TopLeftArPoseY }));
srcList.Add(DenseVector.OfArray(new double[] { obj.TopRightArPoseX, obj.TopRightArPoseY }));
srcList.Add(DenseVector.OfArray(new double[] { obj.BottomRightArPoseX, obj.BottomRightArPoseY }));
srcList.Add(DenseVector.OfArray(new double[] { obj.BottomLeftArPoseX, obj.BottomLeftArPoseY }));
dstList.Add((HomographyHelper.CreateVector2(obj.TopLeftDobotPoseX, obj.TopLeftDobotPoseY)));
dstList.Add(HomographyHelper.CreateVector2(obj.TopRightDobotPoseX, obj.TopRightDobotPoseY));
dstList.Add(HomographyHelper.CreateVector2(obj.BottomRightDobotPoseX, obj.BottomRightDobotPoseY));
dstList.Add(HomographyHelper.CreateVector2(obj.BottomLeftDobotPoseX, obj.BottomLeftDobotPoseY));
// 画像座標系での値
Console.WriteLine($"homography_IMAGE:imgX:{blob.Left + blob.Width / 2}, imgY:{blob.Top + blob.Height / 2}");
// 射影変換行列を求めて
var h**o = HomographyHelper.FindHomography(srcList, dstList);
// 入力平面から出力平面上の座標に変換
var ret = h**o.Translate(blob.Left + blob.Width / 2, blob.Top + blob.Height / 2);
// ロボット座標系での値
Console.WriteLine($"homography_ROBOT:boxX:{ret.dstX}, boxY:{ret.dstY}");
return(ret);
}
public float ErrorAtPoint(Vector3 v)
{
Vector <double> u = DenseVector.OfArray(new double[] { v.x, v.y, v.z, 1 });
Vector <double> Qu = quadric * u;
return((float)u.DotProduct(Qu));
}
void InitQuadrics()
{
Matrix <double>[] faceQuadrics = new Matrix <double> [heMesh.Faces.Length];
// Compute quadrics for each face
for (int i = 0; i < faceQuadrics.Length; i++)
{
Face f = heMesh.Faces[i];
if (f.IsBoundary)
{
continue;
}
Vector3 normal = f.Normal;
Vector3 point = f.anyHalfEdge.tailVertex.position;
float d = -Vector3.Dot(normal, point);
DenseVector v = DenseVector.OfArray(new double[] { normal.x, normal.y, normal.z, d });
faceQuadrics[i] = v.OuterProduct(v);
}
// Compute quadrics for each vertex by adding face quadrics
vertQuadrics = new ErrorQuadric[heMesh.Vertices.Length];
for (int i = 0; i < vertQuadrics.Length; i++)
{
vertQuadrics[i] = new ErrorQuadric();
Vertex v = heMesh.Vertices[i];
HalfEdge he = v.anyHalfEdge;
HalfEdge start = he;
do
{
Face f = he.face;
if (!f.IsBoundary)
{
vertQuadrics[i].AddQuadric(faceQuadrics[f.Index]);
}
he = he.flip.next;
}while (he != start);
}
edgeCosts = new float[heMesh.Edges.Length];
// Compute initial error values for each edge
for (int i = 0; i < heMesh.Edges.Length; i++)
{
ErrorQuadric Qsum = new ErrorQuadric();
int v1 = heMesh.Edges[i].anyHalfEdge.tailVertex.Index;
int v2 = heMesh.Edges[i].anyHalfEdge.headVertex.Index;
Qsum.AddQuadric(vertQuadrics[v1]);
Qsum.AddQuadric(vertQuadrics[v2]);
Vector3 opt = Qsum.OptimalPoint(heMesh.Vertices[v1].position, heMesh.Vertices[v2].position);
edgeCosts[i] = Qsum.ErrorAtPoint(opt);
}
}
public double[] simulateHidden(double[] data)
{
Vector <double> data_vector;
Matrix <double> hidden_activations, hidden_probs;
double[] tmp_data = new double[data.Length + 1];
tmp_data[0] = 1;
for (int i = 1; i < tmp_data.Length; i++)
{
tmp_data[i] = data[i - 1];
}
data_vector = DenseVector.OfArray(tmp_data);
hidden_activations = data_vector.ToRowMatrix().Multiply(weights);
hidden_probs = DenseMatrix.OfArray(logisticMatrix(hidden_activations));
double[] tmp_states = new double[hidden_probs.ColumnCount];
for (int i = 1; i < hidden_probs.ColumnCount; i++)
{
tmp_states[i] = hidden_probs.At(0, i) > 0.5 ? 1 : 0;
}
tmp_states[0] = 1;
return(tmp_states);
}
public ISpace <double> CreateSingleSampleSpaceFromSample(ISample <double> sample)
{
var vector = DenseVector.OfArray(sample.Storage);
var space = vector.ToRowMatrix();
return(new MdnnDoubleSpace(space.ToArray()));
}
/// <summary>
/// WARNING MIGHT NOT BE THREAD SAFE
/// </summary>
/// <param name="normals"></param>
/// <param name="posses"></param>
/// <param name="preferredPosition"></param>
/// <returns></returns>
public static Vector <float> CalculateCubeQEF(Vector3[] normals, Vector3[] posses, int numIntersections, Vector3 preferredPosition)
{
List <Vector3> normals1 = new List <Vector3>(normals.Take(numIntersections));
List <Vector3> posses1 = new List <Vector3>(posses.Take(numIntersections));
Vector3 preferredPosition1 = preferredPosition;
ABuffer[0, 0] = normals1[0].X;
ABuffer[0, 1] = normals1[0].Y;
ABuffer[0, 2] = normals1[0].Z;
ABuffer[1, 0] = normals1[1].X;
ABuffer[1, 1] = normals1[1].Y;
ABuffer[1, 2] = normals1[1].Z;
ABuffer[2, 0] = normals1[2].X;
ABuffer[2, 1] = normals1[2].Y;
ABuffer[2, 2] = normals1[2].Z;
var A = DenseMatrix.OfRowArrays(normals1.Select(e => new[] { e.X, e.Y, e.Z }).ToArray());
//var A = DenseMatrix.OfArray(ABuffer); //NOT USED UNSURE IF IT IS BUGGED
BBuffer[0] = Vector3.Dot(normals1[0], posses1[0] - preferredPosition1);
BBuffer[1] = Vector3.Dot(normals1[1], posses1[1] - preferredPosition1);
BBuffer[2] = Vector3.Dot(normals1[2], posses1[2] - preferredPosition1);
var b = DenseVector.OfArray(normals1.Zip(posses1.Select(p => p - preferredPosition1), Vector3.Dot).ToArray());
//var b = DenseVector.OfArray(BBuffer); //NOT USED UNSURE IF IT IS BUGGED
//TODO: think about the fact that x-p is not normalized with respect to the normal vector
var leastsquares = CalculateQEF(A, b);
return(leastsquares + DenseVector.OfArray(new[] { preferredPosition1.X, preferredPosition1.Y, preferredPosition1.Z }));
}
public static double[] Transform3(double[] src, double[,] align)
{
var scale = align[3, 0];
var r = DenseMatrix.OfArray(new double[,] { { align[0, 0], align[0, 1], align[0, 2] }, { align[1, 0], align[1, 1], align[1, 2] }, { align[2, 0], align[2, 1], align[2, 2] } });
var t = DenseVector.OfArray(new double[] { align[0, 3], align[1, 3], align[2, 3] });
return (scale * r * DenseVector.OfArray(src.Take(3).ToArray()) + t).ToArray();
}
/// <summary>
/// 寻找距离点p最近的边
/// </summary>
/// <param name="verticesList"></param>
/// <param name="p"></param>
/// <returns></returns>
public static Point[] FindEdgeNearestPoint(IList <Point> verticesList, Point p)
{
if (verticesList.Count < 2)
{
throw new Exception("No Enough Points In VerticesList");
}
double minD = double.MaxValue;
Point pA = null;
Point pB = null;
for (int i = 0; i < verticesList.Count - 1; ++i)
{
Point pStart = verticesList[i];
Point pEnd = verticesList[(i != verticesList.Count - 1) ? (i + 1) : 0];
// 线方程
DenseMatrix ma = DenseMatrix.OfArray(new[, ] {
{ pStart.X, 1 }, { pEnd.X, 1 }
});
DenseVector vb = DenseVector.OfArray(new[] { pStart.Y, pEnd.Y });
var result = ma.LU().Solve(vb);
double k = result[0];
double b = result[1];
double d = Math.Abs(k * p.X - p.Y + b) / Math.Sqrt(k * k + 1);
if (d < minD)
{
minD = d;
pA = pStart;
pB = pEnd;
}
}
return(new[] { pA, pB });
}
public void XorTest()
{
NeuralNetwork network = new NeuralNetwork(2, 1, 1, 1);
float[,] xorInput = new float[4, 2]
{
{ 0, 0 },
{ 0, 1 },
{ 1, 0 },
{ 1, 1 }
};
float[,] xorOutput = new float[4, 1]
{
{ 1 },
{ 0 },
{ 0 },
{ 1 }
};
SparseMatrix X = new SparseMatrix(SparseCompressedRowMatrixStorage <float> .OfArray(xorInput));
SparseMatrix Y = new SparseMatrix(SparseCompressedRowMatrixStorage <float> .OfArray(xorOutput));
network.LearnNetwork(X, Y, 5000, 0.001f, 0);
network.Inputs = DenseVector.OfArray(new float[2] {
0, 0
});
network.ForwardPropagation();
var test = network.GetAswer();
Assert.IsNotNull(test);
Assert.AreEqual(Math.Round(test[0]), 1);
}
public void solveForX()
{
solveForY();
for (int i = n - 1; i >= 0; i--)
{
double sum = 0;
for (int j = i + 1; j < n; j++)
{
sum += A[i, j] * X[j];
}
X[i] = (Y[i] - sum);
}
Console.WriteLine("Solutia X: ");
for (int i = 0; i < n; i++)
{
Console.WriteLine(X[i]);
}
Console.WriteLine("Xlib: ");
Matrix <double> libAinit = DenseMatrix.OfArray(Ainit);
Vector <double> libBinit = DenseVector.OfArray(Binit);
Vector <double> libX = libAinit.Solve(libBinit);
for (int i = 0; i < n; i++)
{
Console.WriteLine(libX[i]);
}
}
static void Main(string[] args)
{
double[] xInitial = new double[17] {
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1
};
double[] xLower = new double[17];
xLower.Populate(-5);
double[] xUpper = new double[17];
xUpper.Populate(5);
Matrix <Double> featureValues = DenseMatrix.OfArray(readFileIntoArray("testData"));
Vector <Double> trueValues = DenseVector.OfArray(readResultFileIntoArray("saveResult"));
var solution = NelderMeadSolver.Solve(x => MyCostFunctionMethod(x, featureValues, trueValues), xInitial, xLower, xUpper);
Console.WriteLine(solution.Result);
Console.WriteLine("solution = {0}", solution.GetSolutionValue(0));
for (int i = 0; i < 17; i++)
{
Console.WriteLine("x = {0}", solution.GetValue(i + 1));
}
Console.ReadKey();
}
protected override void CalculateEquivalentNodalForcesVector()
{
if (load == null)
{
EquivalentNodalForcesVector = new DenseVector(12);
return;
}
double[] sv = load.startValue;
double[] ev = load.endValue;
double[] loads = { ((1.0 / 3.0) * sv[0] + (1.0 / 6.0) * ev[0]) * L,
((7.0 / 20.0) * sv[1] + (3.0 / 20.0) * ev[1]) * L,
((7.0 / 20.0) * sv[2] + (3.0 / 20.0) * ev[2]) * L,
0,
((1.0 / 20.0) * sv[2] + (1.0 / 30.0) * ev[2]) * L * L,
((1.0 / 20.0) * sv[1] + (1.0 / 30.0) * ev[1]) * L * L,
((1.0 / 6.0) * sv[0] + (1.0 / 3.0) * ev[0]) * L,
((3.0 / 20.0) * sv[1] + (7.0 / 20.0) * ev[1]) * L,
((3.0 / 20.0) * sv[2] + (7.0 / 20.0) * ev[2]) * L,
0,
(-(1.0 / 30.0) * sv[2] - (1.0 / 20.0) * ev[2]) * L * L,
(-(1.0 / 30.0) * sv[1] - (1.0 / 20.0) * ev[1]) * L * L };
EquivalentNodalForcesVector = DenseVector.OfArray(loads);
startNode.EquivalentForces.Add(EquivalentNodalForcesVector.SubVector(0, 6), startNode.EquivalentForces);
endNode.EquivalentForces.Add(EquivalentNodalForcesVector.SubVector(6, 6), endNode.EquivalentForces);
}
public override Matrix <double> CalculateModel(List <AffineRegionsPair> pairs)
{
int[] x, y, u, v;
x = new int[3];
y = new int[3];
u = new int[3];
v = new int[3];
for (int i = 0; i < 3; i++)
{
(x[i], y[i], u[i], v[i]) = pairs[i].GetPairsCoordinates();
}
Matrix <double> basePairsMatrix = DenseMatrix.OfArray(new double[, ] {
{ x[0], y[0], 1, 0, 0, 0 },
{ x[1], y[1], 1, 0, 0, 0 },
{ x[2], y[2], 1, 0, 0, 0 },
{ 0, 0, 0, x[0], y[0], 1 },
{ 0, 0, 0, x[1], y[1], 1 },
{ 0, 0, 0, x[2], y[2], 1 }
});
Vector <double> transformedVector = DenseVector.OfArray(new double[] {
u[0],
u[1],
u[2],
v[0],
v[1],
v[2]
});
Matrix <double> resultMatrix = basePairsMatrix.Inverse() * transformedVector.ToColumnMatrix();
return(GetAffineMatrixOfVector(resultMatrix.Column(0)));
}
//###########################
//CONVERTERS
internal Dictionary <JointType, Point3D> ConvertToRefernceFrame(Dictionary <JointType, Point3D> bodyPointsDict)
{
Dictionary <JointType, Point3D> convertedBodyPointsDict = new Dictionary <JointType, Point3D>();
Point3D convertedPoint = new Point3D();
Vector <double> point = DenseVector.OfArray(new double[3]);
foreach (JointType joint in bodyPointsDict.Keys)
{
point[0] = bodyPointsDict[joint].X;
point[1] = bodyPointsDict[joint].Y;
point[2] = bodyPointsDict[joint].Z;
if (registrationHelper.RegistrationCompleteBool)
{
point = point * this.rotationMatrix + this.translationVector;
}
convertedPoint.X = point[0];
convertedPoint.Y = point[1];
convertedPoint.Z = point[2];
convertedBodyPointsDict.Add(joint, convertedPoint);
}
Point3D head1 = bodyPointsDict[JointType.Head];
Point3D head2 = convertedBodyPointsDict[JointType.Head];
//if (registrationHelper.RegistrationCompleteBool)
// Console.WriteLine( "bpHEAD " + head1.X + " " + head2.X );
return(convertedBodyPointsDict);
}
private void reset()
{
if (_featureMap.EliteMap.Count == 0)
{
_mean = LA.Vector <double> .Build.Dense(_numParams);
}
else
{
_mean = DenseVector.OfArray(_featureMap.GetRandomElite().ParamVector);
}
_direction = LA.Vector <double> .Build.Dense(_featureMap.NumFeatures);
for (int i = 0; i < _featureMap.NumFeatures; i++)
{
_direction[i] = Sampler.gaussian() * _featureMap.GetFeatureScalar(i);
}
_mutationPower = _params.MutationPower;
_pc = LA.Vector <double> .Build.Dense(_numParams);
_ps = LA.Vector <double> .Build.Dense(_numParams);
_C = new DecompMatrix(_numParams);
_individualsEvaluated = 0;
}
internal List <ColorSpacePoint> ConvertCSPoints(List <ColorSpacePoint> csPoints)
{
List <ColorSpacePoint> convertedPoints = new List <ColorSpacePoint>();
ColorSpacePoint convertedPoint = new ColorSpacePoint();
Vector <double> vPoint = DenseVector.OfArray(new double[3]);
foreach (ColorSpacePoint point in csPoints)
{
vPoint[0] = point.X;
vPoint[1] = point.Y;
if (registrationHelper.RegistrationCompleteBool)
{
vPoint = vPoint * this.rotationMatrix + this.translationVector;
}
convertedPoint.X = (float)vPoint[0];
convertedPoint.Y = (float)vPoint[1];
convertedPoints.Add(convertedPoint);
}
ColorSpacePoint head1 = csPoints[1];
ColorSpacePoint head2 = convertedPoints[1];
//if( registrationHelper.RegistrationCompleteBool )
// Console.WriteLine("csHEAD " + head1.X + " " + head2.X);
return(convertedPoints);
}
public Matrix <double> insert(Matrix <double> data, int index, int number, int axis)
{
int size;
if (axis == 0)
{
size = data.RowCount;
}
else
{
size = data.ColumnCount;
}
double[] for_vector = new double[size];
for (int i = 0; i < size; i++)
{
for_vector[i] = number;
}
if (axis == 0)
{
data = data.InsertColumn(index, DenseVector.OfArray(for_vector));
}
else
{
data = data.InsertRow(0, DenseVector.OfArray(for_vector));
}
return(data);
}
void InitializeTestValues()
{
testAnchorPositions["DW51A7"] = DenseVector.OfArray(new double[] { 1342, 2393, 1676 });
testAnchorPositions["DW8428"] = DenseVector.OfArray(new double[] { 1019, 1777, 2792 });
testAnchorPositions["DW8E23"] = DenseVector.OfArray(new double[] { -1309, -515, 1160 });
testAnchorPositions["DW092D"] = DenseVector.OfArray(new double[] { 1854, -578, 1026 });
}
public static Tuple <double[, ], double> AffineAlign(double[] sx, double[] sy, double[] tx, double[] ty)
{
List <Vector <double> > src = new List <Vector <double> >();
for (int i = 0; i < sx.Length; i++)
{
src.Add(DenseVector.OfArray(new[] { sx[i], sy[i], 1 }));
}
var a = DenseMatrix.OfRowVectors(src);
var vec1 = a.TransposeThisAndMultiply(a).Inverse().Multiply(a.Transpose()).Multiply(DenseVector.OfArray(tx));
var vec2 = a.TransposeThisAndMultiply(a).Inverse().Multiply(a.Transpose()).Multiply(DenseVector.OfArray(ty));
var affine = DenseMatrix.OfRowVectors(vec1, vec2);
//calculate errors
List <double> errors = new List <double>();
for (int i = 0; i < src.Count; i++)
{
errors.Add((affine * src[i] - DenseVector.OfArray(new[] { tx[i], ty[i] })).PointwisePower(2).Norm(1));
}
return(new Tuple <double[, ], double>(new double[, ]
{
{ affine[0, 0], affine[0, 1], 0, affine[0, 2] },
{ affine[1, 0], affine[1, 1], 0, affine[1, 2] },
{ 0, 0, 1, 0 },
{ 0, 0, 0, 1 },
},
errors.Max()));
}
public static LabelWithConfidence LabelWithConfidence(NeuralNet model, NNInstrumentation instr, double[] datum, bool crop)
{
Vector <double> datum_v = DenseVector.OfArray(datum);
if (crop)
{
datum_v = model.CropMaybe(datum_v);
}
double[] outs = model.EvaluateNNConcretePostCrop(datum_v, instr);
// Console.WriteLine("Outs = {0}", DenseVector.OfArray(outs));
Tuple <double, int> max = UMath.Max(outs);
Tuple <double, int> secmax = UMath.MaxExcluding(max.Item2, outs);
UMath.SoftMax(outs);
var result = new LabelWithConfidence
{
datum = datum,
actualLabel = max.Item2,
secBestLabel = secmax.Item2,
softMaxValue = outs[max.Item2],
diffFromSecondBest = Math.Abs(outs[max.Item2] - outs[secmax.Item2])
};
return(result);
}
public static string Align2Test(double[] sx, double[] sy, double[] tx, double[] ty)
{
List <Vector <double> > src = new List <Vector <double> >();
for (int i = 0; i < sx.Length; i++)
{
src.Add(DenseVector.OfArray(new[] { sx[i], sy[i] }));
}
List <Vector <double> > tar = new List <Vector <double> >();
for (int i = 0; i < tx.Length; i++)
{
tar.Add(DenseVector.OfArray(new[] { tx[i], ty[i] }));
}
var ret = Align2(src, tar);
StringBuilder sb = new StringBuilder();
sb.AppendLine(ret.Item1.ToMatrixString());
sb.AppendLine(ret.Item2.ToString("F6"));
sb.AppendLine(ret.Item3.ToVectorString());
return(sb.ToString());
}
public void TestSimplexMethod()
{
Matrix <double> matrix = DenseMatrix.OfArray(new double[, ] {
{ 1, 0, 2, -1 }, { 0, 1, 4, 2 }
});
Vector <double> simplexFunction = DenseVector.OfArray(new double[] { 1, -4, 3, 4 });
Vector <double> constraints = DenseVector.OfArray(new double[] { 2, 6 });
BasisFinder basisFinder = new BasisFinder(matrix, simplexFunction, constraints);
int[] basis = basisFinder.GetBasis();
Console.WriteLine($"Basis: {string.Join(", ", basis)}");
Simplex simplex = new Simplex(basisFinder, basis);
simplex.FindAnswer();
Console.WriteLine($"Matrix: {simplex.matrix.ToString()}");
Console.WriteLine($"Constraints: {constraints.ToString()}");
Console.WriteLine($"Simplex function: {simplexFunction.ToString()}");
double answer = simplex.GetAnswer();
Console.WriteLine($"Answer: {answer}");
double[,] matrixResult = { { 1, 0.5, 4, 0 }, { 0, 0.5, 2, 1 } };
Assert.IsTrue(answer == 17.0 && matrixResult.IsEqualsValues(simplex.matrix));
}
public PhysicBall()
{
Location = CreateVector.Dense <float>(2);
Speed = CreateVector.Dense <float>(2);
Acceleration = DenseVector.OfArray(new float[] { 1, 1 });
DeltaLocation = CreateVector.Dense <float>(2);
}
/// <summary>
/// TOA解算算法
/// </summary>
/// <param name="anchor_num">基站数量</param>
/// <param name="anchor_position">基站坐标阵列</param>
/// <param name="distance_vector">标签到各个基站的距离矢量</param>
/// <returns>最终点坐标X,Y</returns>
public static double[] toa(int anchor_num, double[,] anchor_position, double[] distance_vector)
{
int row = anchor_position.GetLength(0);
int col = anchor_position.GetLength(1);
Matrix <double> model = DenseMatrix.OfArray(anchor_position);
var A = model;
Vector <double> distance_model = DenseVector.OfArray(distance_vector);
double[] oneVector = Enumerable.Repeat <double>(1, anchor_num).ToArray();
Vector <double> colInser = DenseVector.OfArray(oneVector);
A = -2 * A;
A = A.InsertColumn(2, colInser); //插入一列
var temp5 = (Matrix.op_DotMultiply(model, model) * -1);
var temp6 = Vector.op_DotMultiply(distance_model, distance_model);
var temp7 = temp6 + temp5.Column(0) + temp5.Column(1);
var temp_left = A.Transpose() * (A);
var temp_right = A.Transpose() * temp7;
var theta = temp_left.QR().Solve(temp_right);
if (theta[2] - theta[1] * theta[1] < 0)
{
Console.WriteLine(" data error!!!");
theta[0] = 0;
theta[1] = 0;
}
else
{
theta[0] = Math.Abs(Math.Sqrt(theta[2] - theta[1] * theta[1]));
}
var estimate_tag_position = new double[] { theta[0], theta[1] };
return(estimate_tag_position);
}
public override Vector <double> EvaluateConcrete(Vector <double> input)
{
// If we have a meanImage ...
if (meanImage_ != null)
{
return((input - DenseVector.OfArray(meanImage_)) * scale_);
}
// If we have a meanChannel ...
if (meanChannel_ != null && meanChannel_.Count() > 0)
{
Vector <double> cur = input;
for (int channel = 0; channel < InputCoordinates.ChannelCount; channel++)
{
for (int r = 0; r < InputCoordinates.RowCount; r++)
{
for (int c = 0; c < InputCoordinates.ColumnCount; c++)
{
int index = InputCoordinates.GetIndex(channel, r, c);
cur[index] = (input[index] - meanChannel_[channel]) * scale_;
}
}
}
return(cur);
}
// If we are only doing scaling ...
return(input * (float)scale_);
}
static void Main(string[] args)
{
Matrix <double> Aj = DenseMatrix.OfArray(new double[, ] {
{ 10, -1, 2, 0 },
{ -1, 11, -1, 3 },
{ 2, -1, 10, -1 },
{ 0, 3, -1, 8 }
});
Vector <double> Bj = DenseVector.OfArray(new double[] { 6, 25, -11, 15 });
Matrix <double> As = DenseMatrix.OfArray(new double[, ] {
{ 5, 2, -4 },
{ 2, 1, -2 },
{ -4, -2, 5, }
});
Vector <double> Bs = DenseVector.OfArray(new double[] { 1, 2, 3 });
SleSolver sleSolver = new SleSolver();
var A = sleSolver.generateDiagonallyDominantMatrix(4);
Console.WriteLine(A);
Console.WriteLine(Aj * sleSolver.JacobiMethod(Aj, Bj, 1e-16));
Console.WriteLine(Aj * sleSolver.SeidelMethod(Aj, Bj, 1e-16));
Console.WriteLine(As * sleSolver.JacobiMethod(As, Bs, 1e-16));
Console.WriteLine(As * sleSolver.SeidelMethod(As, Bs, 1e-16));
}
/* example for
* f1 = 3 * x - cos y - 0.9 = 0
* f2 = sin( x - 0.6 ) - y - 1.6 = 0
* Starting point (0;-1)
* K1 = 1 K2 = 0.5 H = 0.01
*/
static void diff_One()
{
Console.WriteLine("**************** Diff One Method ****************\n\n");
count = 0;
x_old = 0;
y_old = -1;
double k1 = 1;
double k2 = 0.5;
double h = 0.01;
Vector <double> vector_old = DenseVector.OfArray(new double[]
{
x_old, y_old
});
Vector <double> vector_new;
while (true)
{
count++;
vector_new = vector_old - h * vectorOf_Nabla_f1_f2(vector_old[0], vector_old[1]);
if (count % 100 == 0)
{
consoleWrite(vector_new[0], vector_new[1], count);
}
if (Math.Abs(vector_old.Sum() - vector_new.Sum()) < 0.000001 || count == 10000)
{
consoleWrite(vector_new[0], vector_new[1], count);
break;
}
vector_old = vector_new;
}
}
public void GenerateImpulseResponse_CheckSignal()
{
var sampleRate = 16000;
var dftLength = 1024;
var delaySampleCount = 30;
var time = (double)delaySampleCount / sampleRate;
var distance = AcousticConstants.SoundSpeed * time;
var roomSize = DenseVector.OfArray(new double[] { distance, distance, distance });
var distanceAttenuation = new DistanceAttenuation(distance => 0.9);
var reflectionAttenuation = new ReflectionAttenuation(frequency => 0.7);
var room = new Room(roomSize, distanceAttenuation, reflectionAttenuation, 1);
var soundSource = new SoundSource(distance / 2, distance / 2, distance / 2);
var microphone = new Microphone(distance / 2, distance / 2, distance / 2);
var response = MirrorMethod.GenerateImpulseResponse(room, soundSource, microphone, sampleRate, dftLength);
Assert.AreEqual(dftLength / 2, response.Length);
for (var t = 0; t < response.Length; t++)
{
if (t == 0)
{
Assert.AreEqual(0.9, response[t], 1.0E-6);
}
else if (t == delaySampleCount)
{
Assert.AreEqual(6 * 0.9 * 0.7, response[t], 1.0E-6);
}
}
}
