public static UInt64 GetUInt64(this MatFileReader reader, string name)
{
var marray = reader.GetMLArray(name);
if (!marray.IsUInt64)
{
throw new InvalidCastException("data is not of type UInt64");
}
var n = marray.Size;
var darray = (MLUInt64)marray;
return(darray.GetReal(0));
}
public static int GetInt(this MatFileReader reader, string name)
{
var marray = reader.GetMLArray(name);
if (!marray.IsInt32)
{
throw new InvalidCastException("data is not of type Int32");
}
var n = marray.Size;
var darray = (MLInt32)marray;
return(darray.GetReal(0));
}
public static double GetDouble(this MatFileReader reader, string name)
{
var marray = reader.GetMLArray(name);
if (!marray.IsDouble)
{
throw new InvalidCastException("data is not of type double");
}
var n = marray.Size;
var darray = (MLDouble)marray;
return(darray.GetReal(0));
}
public void ImportMatCurve(string path)
{
var mfr = new MatFileReader(path);
var h = (mfr.GetMLArray("H") as MLDouble)?.GetArray();
var q = (mfr.GetMLArray("Q") as MLDouble)?.GetArray();
var hOpt = (mfr.GetMLArray("H_Opt") as MLDouble)?.GetArray();
var qOpt = (mfr.GetMLArray("Q_Opt") as MLDouble)?.GetArray();
var curve = new List <PumpPerformancePoint>();
if (qOpt != null && hOpt != null && qOpt[0].Count() == hOpt[0].Count())
{
for (int i = 0; i < qOpt[0].Count(); i++)
{
curve.Add(new PumpPerformancePoint(hOpt[0][i], qOpt[0][i]));
}
}
else if (hOpt != null && q != null && hOpt[0].Count() == q[0].Count())
{
for (int i = 0; i < q.Count(); i++)
{
curve.Add(new PumpPerformancePoint(hOpt[0][i], q[0][i]));
}
}
else if (h != null && q != null && h[0].Count() == q[0].Count())
{
for (int i = 0; i < q[0].Count(); i++)
{
curve.Add(new PumpPerformancePoint(h[0][i], q[0][i]));
}
}
else
{
throw new InvalidDataException("Keine gültigen Daten in MAT-File vorhanden.");
}
PerformanceCurve = curve.ToArray();
}
public static uint[] GetUInt32Array(this MatFileReader reader, string name)
{
var marray = reader.GetMLArray(name);
if (!marray.IsUInt32)
{
throw new InvalidCastException("data is not of type UInt32");
}
var n = marray.Size;
var darray = (MLUInt32)marray;
var data = new uint[n];
for (var i = 0; i < n; ++i)
{
data[i] = darray.GetReal(i);
}
return(data);
}
public static double[] GetDoubleArray(this MatFileReader reader, string name)
{
var marray = reader.GetMLArray(name);
if (!marray.IsDouble)
{
throw new InvalidCastException("data is not of type double");
}
var n = marray.Size;
var darray = (MLDouble)marray;
var data = new double[n];
for (var i = 0; i < n; ++i)
{
data[i] = darray.GetReal(i);
}
return(data);
}
public static int[,] GetInt32Array2D(this MatFileReader reader, string name)
{
var marray = reader.GetMLArray(name);
if (!marray.IsInt32)
{
throw new InvalidCastException("data is not of type Int32");
}
var m = marray.M;
var n = marray.N;
var darray = (MLInt32)marray;
var data = new int[m, n];
for (var i = 0; i < m; ++i)
{
for (var j = 0; j < n; ++j)
{
data[i, j] = darray.GetReal(i, j);
}
}
return(data);
}
public static float[] GetSingleArray(this MatFileReader reader, string name)
{
var marray = reader.GetMLArray(name);
if (!marray.IsSingle)
{
throw new InvalidCastException("data is not of type float");
}
var m = marray.M;
var n = marray.N;
var l = m * n;
var darray = (MLSingle)marray;
var data = new float[l];
for (var i = 0; i < m; ++i)
{
for (var j = 0; j < n; ++j)
{
data[i * n + j] = darray.GetReal(i, j);
}
}
return(data);
}
internal ArrayReader(string fileName)
{
try
{
MatFileReader mfr = new MatFileReader(fileName);
A = ((MLDouble)(mfr.GetMLArray("A"))).GetArray();
PP = ((MLDouble)(mfr.GetMLArray("PP"))).GetArray();
double[][] Q = ((MLDouble)(mfr.GetMLArray("q"))).GetArray();
q = new double[Q.Length];
for (int i = 0; i < Q.Length; i++)
{
q[i] = Q[i][0];
}
double[][] P = ((MLDouble)mfr.GetMLArray("p")).GetArray();
p = new double[P.Length];
for (int i = 0; i < P.Length; i++)
{
p[i] = P[i][0];
}
double[][] V = ((MLDouble)mfr.GetMLArray("v")).GetArray();
v = new double[V.Length];
for (int i = 0; i < V.Length; i++)
{
v[i] = V[i][0];
}
double[][] WW = ((MLDouble)mfr.GetMLArray("w")).GetArray();
w = new double[WW.Length];
for (int i = 0; i < WW.Length; i++)
{
w[i] = WW[i][0];
}
}
catch (System.IO.IOException)
{
throw new MatlabIOException("Cannot read file");
}
}
static void Main(string[] args)
{
csmatio.io.MatFileReader mfr = new MatFileReader(@"E:\Users\Administrator\Source\Repos\TinyEKF\TinyEKFDLLTest\TinyEKFDLLTest\big-circle-ekf-dll.mat");
var x = (mfr.GetMLArray("x") as MLDouble).GetArray().ToList();
var y = (mfr.GetMLArray("y") as MLDouble).GetArray().ToList();
var orth_theta = (mfr.GetMLArray("orth_theta") as MLDouble).GetArray().ToList();
var delta_odom = (mfr.GetMLArray("delta_odom") as MLDouble).GetArray().ToList();
int length = mfr.Data[0].Size;
List <double []> z = new List <double []>(length);
List <double []> x_fusion = new List <double[]>(length);
for (int i = 0; i < length; i++)
{
//var temp = x[i][0];
double[] temp = new[] { x[i][0], y[i][0], orth_theta[i][0] };
z.Add(temp);
}
x_fusion.Add(new[] { z[0][0], z[0][1], z[0][2] });
TinyEKFWrapper.ekf_t ekf = new TinyEKFWrapper.ekf_t();
IntPtr ekfIntPtr = Marshal.AllocHGlobal(Marshal.SizeOf(ekf));
try
{
Marshal.StructureToPtr(ekf, ekfIntPtr, false);
TinyEKFWrapper.EKFInit(ekfIntPtr, 3, 3);
TinyEKFWrapper.Init(ekfIntPtr, z[0]);
//var a = (TinyEKFWrapper.ekf_t)Marshal.PtrToStructure(ekfIntPtr, ekf.GetType());
for (int i = 1; i < length; i++)
{
TinyEKFWrapper.Model(ekfIntPtr, delta_odom[i][0]);
//a = (TinyEKFWrapper.ekf_t)Marshal.PtrToStructure(ekfIntPtr, ekf.GetType());
TinyEKFWrapper.EKFStep(ekfIntPtr, z[i]);
//a = (TinyEKFWrapper.ekf_t)Marshal.PtrToStructure(ekfIntPtr, ekf.GetType());
var temp = (TinyEKFWrapper.ekf_t)Marshal.PtrToStructure(ekfIntPtr, ekf.GetType());
x_fusion.Add(new[] { temp.x[0], temp.x[1], temp.x[2] });
}
}
finally
{
Marshal.DestroyStructure(ekfIntPtr, ekf.GetType());
}
StreamWriter file = new StreamWriter(@"E:\Document\EyooProject\2.Vanke\odom\b-circle-dll.dat");
foreach (var data in x_fusion)
{
foreach (var d in data)
{
file.Write(d.ToString() + ",");
}
file.WriteLine();
}
file.Close();
// TinyEKFWrapper.Print();
Console.WriteLine("the last postion [{0}\t{1}\t{2}]", x_fusion[length - 1][0], x_fusion[length - 1][1], x_fusion[length - 1][2]);
Console.ReadLine();
}
