public Vector3 ImageToWorld(double u, double v, double w, bool transposeRot, bool negateRot, bool invertRot, bool invertZ, float rotAngleX, float rotAngleY, float rotAngleZ)
{
double xprime = (u - ((float)(ImageWidth / 2))) / FocalLength;
double yprime = (v - ((float)(ImageHeight / 2))) / FocalLength;
//double xprime = (u) / FocalLength;
//double yprime = (v) / FocalLength;
if (invertZ)
{
w = -w;
}
double x = xprime * w;
double y = yprime * w;
double z = w;
Double[,] homoXYZ = new Double[3, 1];
homoXYZ[0, 0] = x;
homoXYZ[1, 0] = y;
homoXYZ[2, 0] = z;
Double[,] TranslationMatrix = new Double[3, 1];
TranslationMatrix[0, 0] = PositionVector.X;
TranslationMatrix[1, 0] = PositionVector.Y;
TranslationMatrix[2, 0] = PositionVector.Z;
Double[,] aRotationMatrix = new Double[3, 3];
aRotationMatrix[0, 0] = RotationMatrix.M11;
aRotationMatrix[0, 1] = RotationMatrix.M12;
aRotationMatrix[0, 2] = RotationMatrix.M13;
aRotationMatrix[1, 0] = RotationMatrix.M21;
aRotationMatrix[1, 1] = RotationMatrix.M22;
aRotationMatrix[1, 2] = RotationMatrix.M23;
aRotationMatrix[2, 0] = RotationMatrix.M31;
aRotationMatrix[2, 1] = RotationMatrix.M32;
aRotationMatrix[2, 2] = RotationMatrix.M33;
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseHomoXYZ = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(homoXYZ);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseTranslationMatrix = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(TranslationMatrix);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseRotationMatrix = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(aRotationMatrix);
if (transposeRot)
{
denseRotationMatrix = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Transpose();
}
if (invertRot)
{
denseRotationMatrix = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Inverse();
}
denseRotationMatrix = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Inverse();
if (negateRot)
{
denseRotationMatrix = -denseRotationMatrix;
}
//Double[,] TranslationMatrix = new Double[3, 1];
//TranslationMatrix[0, 0] = TranslationVector.X;
//TranslationMatrix[1, 0] = TranslationVector.Y;
//TranslationMatrix[2, 0] = TranslationVector.Z;
//denseRotationMatrix = denseRotationMatrix.Transpose() *
//MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseWorldCoordinates = InverseRotationMatrix * (denseHomoXYZ - TranslationMatrix);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseWorldCoordinates = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)(denseRotationMatrix) * (denseHomoXYZ - denseTranslationMatrix);
float worldX = (float)denseWorldCoordinates[0, 0];
float worldY = (float)denseWorldCoordinates[1, 0];
float worldZ = (float)denseWorldCoordinates[2, 0];
//Microsoft.Xna.Framework.Matrix rotX;
//Microsoft.Xna.Framework.Matrix rotY;
//Microsoft.Xna.Framework.Matrix rotZ;
//Vector3 origin = new Vector3(0, 0, 0);
//Microsoft.Xna.Framework.Matrix.CreateRotationX((float)rotAngleX, out rotX);
//Microsoft.Xna.Framework.Matrix.CreateRotationY((float)rotAngleY, out rotY);
//Microsoft.Xna.Framework.Matrix.CreateRotationZ((float)rotAngleZ, out rotZ);
//Vector3 tempV = new Vector3((float)worldX, (float)worldY, (float)worldZ);
//tempV = Vector3.Transform(tempV, rotX);
//tempV = Vector3.Transform(tempV, rotY);
//tempV = Vector3.Transform(tempV, rotZ);
//worldX = tempV.X;
//worldY = tempV.Y;
//worldZ = tempV.Z;
Vector3 WorldCoodrinates = new Vector3(worldX, worldY, worldZ);
return(WorldCoodrinates);
}
public Matrix Inverse()
{
if (!IsMatrixQuadratic())
{
throw new Exception("Für eine Inverse Matrix muss diese quadratisch sein!");
}
//Math.net Library
MathNetLAGen.Matrix<double> netMatrix = new MathNetLA.Double.DenseMatrix(ToArray());
var inversedMatrix = new Matrix(netMatrix.Inverse().ToArray());
return inversedMatrix;
}
private void button4_Click(object sender, EventArgs e)
{
Action fileProc = () =>
{
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
MathNet.Numerics.Control.LinearAlgebraProvider = new MathNet.Numerics.Algorithms.LinearAlgebra.Mkl.MklLinearAlgebraProvider();
MathNet.Numerics.Control.NumberOfParallelWorkerThreads = Environment.ProcessorCount;
addText("Обробка файлу вимірювань...\r\n");
double[][] SGG_data = null;
if (System.IO.File.Exists("sgg_data.bin"))
{
SGG_data = IOFunc.binLoad_SGG_data("sgg_data.bin");
}
else
{
SGG_data = Data.IOFunc.read_SGG_data(SphericalHarmonicAnalyze.Properties.Settings.Default.SGG_measures, new setProgressDel(addVal));
IOFunc.binwrite_SGG_data("sgg_data.bin", SGG_data);
}
addText("Дані вимірювань оброблено: {0} шт.\r\n", SGG_data.Length); Thread.Sleep(500);
ReferenceSystem elipsoid = new ReferenceSystem(ReferenceSystem.Default.TideFree);
elipsoid.gridParameters.cellSize = SphericalHarmonicAnalyze.Properties.Settings.Default.GridCellSize;
elipsoid.gridParameters.coLatitudeBounds = SphericalHarmonicAnalyze.Properties.Settings.Default.minCoLatitude;
elipsoid.maxDegree = SphericalHarmonicAnalyze.Properties.Settings.Default.modelMaxOrder;
int greedColumnsCount, greedRowsCount;
List <double[]> greed = MathFunc.generateGrid(elipsoid.gridParameters.cellSize, out greedColumnsCount, out greedRowsCount, elipsoid.gridParameters.coLatitudeBounds, 180 - elipsoid.gridParameters.coLatitudeBounds);
addText("Сітку згенеровано: {0} комірок \r\n", greed.Count);
double avgR = MathFunc.getAvgRadius(SGG_data);
List <int>[] map = MathFunc.getMappingOfPoints(elipsoid, SGG_data, greed.ToArray(), greedRowsCount, greedColumnsCount, avgR); sw.Stop(); addText("Точки віднесено до комірок сітки за: {0}.\r\n", sw.Elapsed.ToString());
addText("Кількість клітинок сітки всього: {0}\r\n", greed.Count);
int res1 = 0; foreach (var item in map)
{
res1 += item.Count;
}
addText("Використано вимірів: {0}\r\nСер радіус: {1}\r\n", res1, avgR);
test.checkMap(SGG_data, map, greed, elipsoid);
List <int>[] newMap = null;
MathFunc.checkGreed(ref greed, map, out newMap);
addText("Кількість клітинок сітки, в яких присутні дані вимірювань: {0}\r\n", greed.Count);
map = newMap; newMap = null;
IOFunc.writeGreedToCsvFileWithMeasureCount(greed, map, "greed_new_map.txt");
double[] avgRadius; sw.Restart();
double[] regularisedValues = MathFunc.regularization(SGG_data, greed.ToArray(), map, out avgRadius); sw.Stop(); addText("Регуляризація (на основі сферичної відстані) виконана за: {0}.\r\n", sw.Elapsed.ToString());
IOFunc.writeGreedToCsvFileWithMeasureS(greed, regularisedValues, "greed_regular_grad.txt");
avgRadius[0] = Math.Round(avgRadius[0]);
elipsoid.satelliteSphere = avgRadius[0];
addText("Середній радіус: {0,10:0.000}.\r\nМінімальний радіус: {1,10:0.0000}\r\nМаксимальний радіус:{2,10:0.0000}\r\n", avgRadius[0], avgRadius[1], avgRadius[2]);
SGG_data = null; map = null;
int[][] t_nm = MathFunc.get_nm(elipsoid.maxDegree);
sw.Restart();
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dm = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(greed.Count, (MathFunc.getArraySize(elipsoid.maxDegree) - 3) * 2 - (elipsoid.maxDegree - 1));
sw.Stop(); addText("Пам'ять для матриці коефіцієнтів виділено за: {0}.\r\n", sw.Elapsed.ToString());
sw.Restart();
int progress = 0;
//Обчислення елементів матриці
var p = Parallel.For(0, dm.RowCount, (i) =>
{
double[] line = MathFunc.getCoefMatrixLineKoop(elipsoid, elipsoid.maxDegree, t_nm, elipsoid.satelliteSphere, greed[i][0], greed[i][1]);
lock (dm)
{
dm.SetRow(i, line);
}
progress++;
if (progress / 100D == Math.Round(progress / 100D))
{
addVal(progress, dm.RowCount, "Визначено");
}
});
if (!p.IsCompleted)
{
throw new Exception("Parallel.For");
}
;
IOFunc.writeMatrixToMatLabFile(dm, @"matlab\A.mat", "A");
sw.Stop();
richTextBox1.Invoke(new setProgressDel(addVal), new object[] { 0, dm.RowCount, "" });
addText("Матриця {0} на {1} ({2}MB) згенерована за: {3,10}\r\n", dm.RowCount, dm.ColumnCount, dm.ColumnCount * dm.RowCount * 8 / 1000000, sw.Elapsed.ToString() /* + "\r\nЗапис у файл...\r\n"*/);
if (true)
{
GravityModel gm08 = new GravityModel(elipsoid.maxDegree);
gm08.loadFromFile("GO_CONS_EGM_GCF_2.gfc", new setProgressDel(addVal));
MathNet.Numerics.LinearAlgebra.Double.DenseVector dmL = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm08.getGradientForGrid(elipsoid, greed));//regularisedValues);
MathNet.Numerics.LinearAlgebra.Double.DenseVector dmL2;
GravityModel gm = new GravityModel(elipsoid.maxDegree);
if (radioButton1.Checked)
{
sw.Restart();
gm.loadFromFile(SphericalHarmonicAnalyze.Properties.Settings.Default.inGravityModel, new setProgressDel(addVal));
sw.Stop(); addText("Вихідна модель завантажена за: {0}.\r\n", sw.Elapsed.ToString());
sw.Restart();
dmL2 = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm.getGradientForGrid(elipsoid, greed));
sw.Stop(); addText("Градієнти за вихідною моделлю обчислені для сітки за: {0}.\r\n", sw.Elapsed.ToString());
}
else
{
sw.Restart();
gm = GravityModel.getNormalModel(elipsoid, elipsoid.maxDegree);
dmL2 = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm.getGradientForGrid(elipsoid, greed));
sw.Stop(); addText("Нормальні градієнти обчислені для сітки за: {0}.\r\n", sw.Elapsed.ToString());
}
dmL = dmL - dmL2;
dmL2 = null;
IOFunc.writeMatrixToMatLabFile(dmL.ToColumnMatrix(), @"matlab\L.mat", "L");
sw.Restart();
MathNet.Numerics.LinearAlgebra.Double.DenseVector dmLNormal = null;
dmLNormal = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)dm.TransposeThisAndMultiply(dmL);
dmL = null;
IOFunc.writeMatrixToMatLabFile(dmLNormal.ToColumnMatrix(), @"matlab\LNorm.mat", "LNorm");
sw.Stop(); addText("Стовпчик вільних членів обчислений за: {0}.\r\n", sw.Elapsed.ToString());
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dmANorm = null;
sw.Restart();
dmANorm = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)dm.TransposeThisAndMultiply(dm); dm = null;
sw.Stop(); addText("Нормальна матриця коефіціэнтів обчислена за: {0}.\r\n", sw.Elapsed.ToString());
IOFunc.writeMatrixToMatLabFile(dmANorm, @"matlab\ANorm.mat", "ANorm");
//dmLNormal = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)dmLNormal.Multiply(5e-8);
var x = dmANorm.Inverse();
var res = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)x.Multiply(dmLNormal);
IOFunc.writeModeVectorlToTxtFile(res, elipsoid, @"matlab\_out.AL");
addText(@"Результат за методом A\L знайдено...");
x = null;
GravityModel gm_R = new GravityModel(gm);
gm_R.addDeltaCoef(res.ToArray()); res = null;
double[] h = GravityModel.getGeoidHeight(elipsoid, gm_R, greed);
double[] dg = GravityModel.getAnomaly(elipsoid, gm_R, greed);
IOFunc.writeGeoidHeightsAndAnomalysToTxt(greed, h, dg, elipsoid, @"output\result_AL.txt");
IOFunc.writeGravityModelToTxtFile(gm_R, @"output\model_AL.gcf");
sw.Restart();
addText(dmANorm.Rank().ToString() + "\r\n");
dmANorm = null;
dmLNormal = null;
sw.Stop(); addText("Невідомі знайдено за: {0}.\r\n", sw.Elapsed.ToString());
}
};
if (System.IO.File.Exists(SphericalHarmonicAnalyze.Properties.Settings.Default.inGravityModel))
{
tabControl1.SelectedTab = tabControl1.TabPages[1];
this.UseWaitCursor = true;
ts = new CancellationTokenSource();
ct = ts.Token;
tsk = Task.Factory.StartNew(fileProc, ct);
var setCur = Task.Factory.StartNew(() => { tsk.Wait(); this.UseWaitCursor = false; addText("Обчислення завершені!"); });
richTextBox1.SaveFile(@"output\zvit.rtf");
}
}
