static void Main(string[] args) {
List<MLArray> mlList = new List<MLArray>();
mlList.Add(CreateTestStruct());
try {
MatFileWriter mfw = new MatFileWriter("C:\\Users\\cheics\\Documents\\cc.mat", mlList, true);
} catch (Exception err) {
Console.WriteLine("shit...");
}
}
public void CreateMatlabFile()
{
double[][] data3x3 = new double[3][];
data3x3[0] = new double[] { 100.0, 101.0, 102.0 }; // first row
data3x3[1] = new double[] { 200.0, 201.0, 202.0 }; // second row
data3x3[2] = new double[] { 300.0, 301.0, 302.0 }; // third row
MLDouble mlDoubleArray = new MLDouble("Matrix_3_by_3", data3x3);
List<MLArray> mlList = new List<MLArray> {mlDoubleArray};
MatFileWriter mfw = new MatFileWriter("data.mat", mlList, false);
}
static void exportMatlabTest()
{
UAVObjectManager mgr = new UAVObjectManager();
UAVObjectsInitialize.register(mgr);
UavLogReader rd = new UavLogReader(mgr);
var data = rd.parseFile(@"C:\Users\mimmo\Desktop\xports\OP-2014-01-13_22-17-14.opl");
IEnumerable<AccelSensor> mags = data.OfType<AccelSensor>().Where(k => k.temperature.value < 37);
var gyro = data.OfType<GyroSensor>().Where(k => k.temperature.value < 37);
List<MLArray> data1 = new List<MLArray>();
foreach (var type in data.Select(k => k.GetType()).Distinct().OrderBy(k => k.Name))
{
IEnumerable<UAVObject> qq = data.Where(k => k.GetType() == type);
data1.Add(getStructure(qq, type));
}
MatFileWriter mfw = new MatFileWriter(@"C:\Users\mimmo\Desktop\xports\test2.mat", data1, true);
}
public static void tlog(string logfile)
{
List<MLArray> mlList = new List<MLArray>();
Hashtable datappl = new Hashtable();
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch
{
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
return;
}
mine.logreadmode = true;
mine.speechenabled = false;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
MAVLink.MAVLinkMessage packet = mine.readPacket();
object data = packet.data;
if (data == null)
continue;
if (data is MAVLink.mavlink_heartbeat_t)
{
if (((MAVLink.mavlink_heartbeat_t) data).type == (byte) MAVLink.MAV_TYPE.GCS)
continue;
}
Type test = data.GetType();
DateTime time = mine.lastlogread;
double matlabtime = GetMatLabSerialDate(time);
try
{
foreach (var field in test.GetFields())
{
// field.Name has the field's name.
object fieldValue = field.GetValue(data); // Get value
if (field.FieldType.IsArray)
{
}
else
{
if (!datappl.ContainsKey(field.Name + "_" + field.DeclaringType.Name))
{
datappl[field.Name + "_" + field.DeclaringType.Name] = new List<double[]>();
}
List<double[]> list =
((List<double[]>) datappl[field.Name + "_" + field.DeclaringType.Name]);
object value = fieldValue;
if (value.GetType() == typeof (Single))
{
list.Add(new double[] {matlabtime, (double) (Single) field.GetValue(data)});
}
else if (value.GetType() == typeof (short))
{
list.Add(new double[] {matlabtime, (double) (short) field.GetValue(data)});
}
else if (value.GetType() == typeof (ushort))
{
list.Add(new double[] {matlabtime, (double) (ushort) field.GetValue(data)});
}
else if (value.GetType() == typeof (byte))
{
list.Add(new double[] {matlabtime, (double) (byte) field.GetValue(data)});
}
else if (value.GetType() == typeof (sbyte))
{
list.Add(new double[] {matlabtime, (double) (sbyte) field.GetValue(data)});
}
else if (value.GetType() == typeof (Int32))
{
list.Add(new double[] {matlabtime, (double) (Int32) field.GetValue(data)});
}
else if (value.GetType() == typeof (UInt32))
{
list.Add(new double[] {matlabtime, (double) (UInt32) field.GetValue(data)});
}
else if (value.GetType() == typeof (ulong))
{
list.Add(new double[] {matlabtime, (double) (ulong) field.GetValue(data)});
}
else if (value.GetType() == typeof (long))
{
list.Add(new double[] {matlabtime, (double) (long) field.GetValue(data)});
}
else if (value.GetType() == typeof (double))
{
list.Add(new double[] {matlabtime, (double) (double) field.GetValue(data)});
}
else
{
Console.WriteLine("Unknown data type");
}
}
}
}
catch
{
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
}
foreach (string item in datappl.Keys)
{
double[][] temp = ((List<double[]>) datappl[item]).ToArray();
MLArray dbarray = new MLDouble(item.Replace(" ", "_"), temp);
mlList.Add(dbarray);
}
try
{
MatFileWriter mfw = new MatFileWriter(logfile + ".mat", mlList, true);
}
catch (Exception err)
{
MessageBox.Show("There was an error when creating the MAT-file: \n" + err.ToString(),
"MAT-File Creation Error!", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
return;
}
}
static void DoWrite(string fn, Dictionary<string, DoubleList> data, SortedDictionary<string, double> param,
List<MLArray> mlList, Hashtable seen)
{
log.Info("DoWrite start " + (GC.GetTotalMemory(false)/1024.0/1024.0));
foreach (var item in data)
{
double[][] temp = item.Value.ToArray();
MLArray dbarray = new MLDouble(item.Key, temp);
mlList.Add(dbarray);
log.Info("DoWrite " + item.Key + " " + (GC.GetTotalMemory(false)/1024.0/1024.0));
}
log.Info("DoWrite mllist " + (GC.GetTotalMemory(false)/1024.0/1024.0));
MLCell cell = new MLCell("PARM", new int[] {param.Keys.Count, 2});
int m = 0;
foreach (var item in param.Keys)
{
cell[m, 0] = new MLChar(null, item.ToString());
cell[m, 1] = new MLDouble(null, new double[] {(double) param[item]}, 1);
m++;
}
mlList.Add(cell);
MLArray seenmsg = CreateCellArray("Seen", seen.Keys.Cast<string>().ToArray());
mlList.Add(seenmsg);
try
{
log.Info("write " + fn + ".mat");
log.Info("DoWrite before" + (GC.GetTotalMemory(false)/1024.0/1024.0));
MatFileWriter mfw = new MatFileWriter(fn + ".mat", mlList, true);
log.Info("DoWrite done" + (GC.GetTotalMemory(false)/1024.0/1024.0));
}
catch (Exception err)
{
CustomMessageBox.Show("There was an error when creating the MAT-file: \n" + err.ToString(),
"MAT-File Creation Error!", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
return;
}
}
public static void log(string fn)
{
// read the file
string[] filelines = File.ReadAllLines(fn);
// store all the arrays
List<MLArray> mlList = new List<MLArray>();
// store data to putinto the arrays
Dictionary<string, List<double[]>> data = new Dictionary<string, List<double[]>>();
// store line item lengths
Hashtable len = new Hashtable();
// store whats we have seen in the log
Hashtable seen = new Hashtable();
// store the params seen
SortedDictionary<string, double> param = new SortedDictionary<string, double>();
// keep track of line no
int a = 0;
foreach (var line in filelines)
{
a++;
Console.Write(a + "\r");
string strLine = line.Replace(", ", ",");
strLine = strLine.Replace(": ", ":");
string[] items = strLine.Split(',', ':');
// process the fmt messages
if (line.StartsWith("FMT"))
{
// +1 for line no
string[] names = new string[items.Length - 5 + 1];
names[0] = "LineNo";
Array.ConstrainedCopy(items, 5, names, 1, names.Length - 1);
MLArray format = CreateCellArray(items[3] + "_label", names);
if (items[3] == "PARM")
{
}
else
{
mlList.Add(format);
}
len[items[3]] = names.Length;
} // process param messages
else if (line.StartsWith("PARM"))
{
param[items[1]] = double.Parse(items[2]);
}// everyting else is generic
else
{
// make sure the line is long enough
if (items.Length < 2)
continue;
// check we have a valid fmt message for this message type
if (!len.ContainsKey(items[0]))
continue;
// check the fmt length matchs what the log has
if (items.Length != (int)len[items[0]])
continue;
// make it as being seen
seen[items[0]] = 1;
double[] dbarray = new double[items.Length];
// set line no
dbarray[0] = a;
for (int n = 1; n < items.Length; n++)
{
try
{
dbarray[n] = double.Parse(items[n]);
}
catch { }
}
if (!data.ContainsKey(items[0]))
data[items[0]] = new List<double[]>();
data[items[0]].Add(dbarray);
}
}
foreach (var item in data)
{
double[][] temp = item.Value.ToArray();
MLArray dbarray = new MLDouble(item.Key, temp);
mlList.Add(dbarray);
}
MLCell cell = new MLCell("PARM", new int[] { param.Keys.Count, 2 });
int m = 0;
foreach (var item in param.Keys)
{
cell[m, 0] = new MLChar(null, item.ToString());
cell[m, 1] = new MLDouble(null, new double[] { (double)param[item] }, 1);
m++;
}
mlList.Add(cell);
MLArray seenmsg = CreateCellArray("Seen", seen.Keys.Cast<string>().ToArray());
mlList.Add(seenmsg);
try
{
MatFileWriter mfw = new MatFileWriter(fn + ".mat", mlList, true);
}
catch (Exception err)
{
MessageBox.Show("There was an error when creating the MAT-file: \n" + err.ToString(),
"MAT-File Creation Error!", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
return;
}
}
private void btnCreate_Click(object sender, EventArgs e)
{
List<MLArray> mlList = new List<MLArray>();
// Go through each of the options to add in the file
if (chkCell.Checked)
{
mlList.Add(CreateCellArray());
}
if (chkStruct.Checked)
{
mlList.Add(CreateStructArray());
}
if (chkChar.Checked)
{
mlList.Add(CreateCharArray());
}
if (chkSparse.Checked)
{
mlList.Add(CreateSparseArray());
}
if (chkDouble.Checked)
{
mlList.Add(CreateDoubleArray());
}
if (chkSingle.Checked)
{
mlList.Add(CreateSingleArray());
}
if (chkInt8.Checked)
{
mlList.Add(CreateInt8Array());
}
if (chkUInt8.Checked)
{
mlList.Add(CreateUInt8Array());
}
if (chkInt16.Checked)
{
mlList.Add(CreateInt16Array());
}
if (chkUInt16.Checked)
{
mlList.Add(CreateUInt16Array());
}
if (chkInt32.Checked)
{
mlList.Add(CreateInt32Array());
}
if (chkUInt32.Checked)
{
mlList.Add(CreateUIn32Array());
}
if (chkInt64.Checked)
{
mlList.Add(CreateInt64Array());
}
if (chkUInt64.Checked)
{
mlList.Add(CreateUInt64Array());
}
if (chkImagMatrix.Checked)
{
mlList.Add(CreateImaginaryArray());
}
if (mlList.Count <= 0)
{
MessageBox.Show("You must select at least one MAT-file Creation Element in order to" +
" create a MAT-file.", "No MAT-file elements selected", MessageBoxButtons.OK,
MessageBoxIcon.Exclamation);
return;
}
// Get a filename name to write the file out to
saveFileDialog.ShowDialog();
string filename = saveFileDialog.FileName;
txtOutput.Text += "Creating the MAT-file '" + filename + "'...";
try
{
MatFileWriter mfw = new MatFileWriter(filename, mlList, chkCompress.Checked);
}
catch (Exception err)
{
MessageBox.Show("There was an error when creating the MAT-file: \n" + err.ToString(),
"MAT-File Creation Error!", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
txtOutput.Text += "Failed!\n";
return;
}
txtOutput.Text += "Done!\nMAT-File created with the following data: \n";
foreach (MLArray mla in mlList)
txtOutput.Text += mla.ContentToString() + "\n";
}
public static void SaveMatFile()
{
// create a MATLAB char and double variable and add it to the structure
//MLChar scoreName = ;
//MLDouble scoreValue = new MLDouble("", new double[] { 12345 }, 1);
int maxIter = 0;
foreach(ActionData ad in ActionData.actionList)
{
if(ad.iterationNum > maxIter)
{
maxIter = ad.iterationNum;
}
}
string dateLbl = "";
dateLbl += System.DateTime.Now.Year.ToString();
dateLbl += System.DateTime.Now.Month.ToString();
dateLbl += System.DateTime.Now.Day.ToString();
dateLbl += System.DateTime.Now.ToFileTimeUtc().ToString();
int chunkSize = 50;
int effChunkSize = 50;
int chunk = 0;
int actionIndex = 0;
List<string> fieldList = new List<string> ();
fieldList.Add ("obstacle");
fieldList.Add ("goal");
fieldList.Add ("item");
fieldList.Add ("robot");
fieldList.Add ("selectedItem");
while((chunk)*chunkSize < ActionData.actionList.Count)
{
int i = 0;
int ii = 0;
actionIndex = chunk*chunkSize;
if(chunkSize + actionIndex >= ActionData.actionList.Count)
effChunkSize = ActionData.actionList.Count-actionIndex;
else
effChunkSize = chunkSize;
MLStructure structure = new MLStructure("simDat", new int[] { effChunkSize, 1 });
List<MLArray> mlList = new List<MLArray>();
MLDouble arr;
while(actionIndex < ActionData.actionList.Count)
{
ActionData ad = actionList[actionIndex] as ActionData;
structure["speed", i] = new MLDouble("", new double[] {maxIter}, 1);
structure["beforePos", i] = new MLDouble("", new double[] {ad.beforePos.x,ad.beforePos.y,ad.beforePos.z }, 1);
structure["beforeOrient", i] = new MLDouble("", new double[] {ad.beforeOrient.eulerAngles.x,ad.beforeOrient.eulerAngles.y,ad.beforeOrient.eulerAngles.z}, 1);
//Debug.Log(dateLbl+"_"+chunk+"["+i+"]" +"("+ad.beforeOrient.x+","+ad.beforeOrient.y+","+ad.beforeOrient.z+")");
structure["afterPos", i] = new MLDouble("", new double[] {ad.afterPos.x,ad.afterPos.y,ad.afterPos.z}, 1);
structure["afterOrient", i] = new MLDouble("", new double[] {ad.afterOrient.eulerAngles.x,ad.afterOrient.eulerAngles.y,ad.afterOrient.eulerAngles.z}, 1);
structure["robotType", i] = new MLChar("", ad.robotType);
structure["itemType", i] = new MLChar("", ad.itemType);
ii=0;
foreach(string fieldName in fieldList)
{
int id = ad.positionNames.IndexOf(fieldName);
Vector3 vecValue;
if(id >= 0)
vecValue = ad.positionVecs[id];
else
vecValue = new Vector3(100,100,100); // We pass an invalid value
structure[fieldName, i] = new MLDouble("", new double[] {vecValue.x,vecValue.y,vecValue.z}, 1);
ii++;
}
i++;
actionIndex++;
}
mlList.Add(structure);
MatFileWriter mfw = new MatFileWriter("playDat"+dateLbl+"_"+chunk+".mat", mlList, false);
mfw.GetType();
Debug.Log ("Saved (" + chunk + ")playDat" + dateLbl + ".mat");
chunk ++;
ActionData.actionList = new List<ActionData>();
}
}
internal void saveAsMAT()
{
SaveFileDialog saveFileDialog = new SaveFileDialog();
saveFileDialog.Filter = "MAT files (*.mat)|*.mat|All files (*.*)|*.*";
saveFileDialog.DefaultExt = "mat";
saveFileDialog.InitialDirectory = Properties.Settings.Default.saveImpedanceDirectory;
if (saveFileDialog.ShowDialog() == DialogResult.OK)
{
string filename = saveFileDialog.FileName;
string tmpinfo = new FileInfo(filename).DirectoryName;
Properties.Settings.Default.saveImpedanceDirectory = tmpinfo;
List<MLArray> mlList = new List<MLArray>();
MLStructure structure = new MLStructure("imp", new int[] { 1, 1 });
structure["f", 0] = new MLDouble("", freqs, freqs.Length);
//Only add non-null (sampled) channels
int numNonNull = 0;
List<int> goodChannels = new List<int>();
for (int i = 0; i < impedance.Length; ++i)
if (impedance[i] != null)
{
++numNonNull;
goodChannels.Add(i);
}
double[][] nonNullImpedance = new double[numNonNull][];
for (int i = 0; i < numNonNull; ++i) nonNullImpedance[i] = impedance[goodChannels[i]];
structure["z", 0] = new MLDouble("", nonNullImpedance);
mlList.Add(structure);
try
{
MatFileWriter mfw = new MatFileWriter(filename, mlList, true);
}
catch (Exception err)
{
MessageBox.Show("There was an error when creating the MAT-file: \n" + err.ToString(),
"MAT-File Creation Error!", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
}
}
}
public StereoSystem(string CalibrationMatFile, string CalibrationMatFile_Rectified)
{
MatFileReader mfr = new MatFileReader(CalibrationMatFile);
MatFileReader mfr2 = new MatFileReader(CalibrationMatFile_Rectified);
double[,] A1 = MLDoubleParser("KK_left_new", mfr2);
double[,] A2 = MLDoubleParser("KK_right_new", mfr2);
double[,] kc1 = MLDoubleParser("kc_left", mfr);
double[,] kc2 = MLDoubleParser("kc_right", mfr);
double[,] fc1 = MLDoubleParser("fc_left", mfr);
double[,] fc2 = MLDoubleParser("fc_right", mfr);
double[,] cc1 = MLDoubleParser("cc_left", mfr);
double[,] cc2 = MLDoubleParser("cc_right", mfr);
double[,] R = MLDoubleParser("R", mfr);
double[,] T = MLDoubleParser("T", mfr);
double[,] nx = MLDoubleParser("nx", mfr);
double[,] ny = MLDoubleParser("ny", mfr);
int ImageWidth = (int)nx[0, 0];
int ImageHeight = (int)ny[0, 0];
LeftCam = new CameraParameters(A1, kc1, fc1, cc1, ImageWidth, ImageHeight);
RightCam = new CameraParameters(A2, kc2, fc2, cc2, ImageWidth, ImageHeight);
TranslationVector = new Matrix<double>(T);
RotationMatrix = new Matrix<double>(R);
DisparityDepthMapMatrix = new Matrix<double>(4, 4);
ImageSize = new Size(ImageWidth, ImageHeight);
CvInvoke.cvStereoRectify(
LeftCam.CameraMatrix.Ptr,
RightCam.CameraMatrix.Ptr,
LeftCam.distCoeffs.Ptr,
RightCam.distCoeffs.Ptr,
ImageSize,
RotationMatrix.Ptr,
TranslationVector.Ptr,
LeftCam.RectificationTransform.Ptr,
RightCam.RectificationTransform.Ptr,
LeftCam.ProjectionMatrix.Ptr,
RightCam.ProjectionMatrix.Ptr,
DisparityDepthMapMatrix.Ptr,
Emgu.CV.CvEnum.STEREO_RECTIFY_TYPE.DEFAULT,
1,
Size.Empty,
ref LeftCam.ROI,
ref RightCam.ROI);
LeftCam.UndistortRectifyMap();
RightCam.UndistortRectifyMap();
var Variables = new List<MLArray>();
int[] Dims = {ImageHeight, ImageWidth};
Variables.Add(MLVariableCreator("LeftMapX", LeftCam.mapx));
Variables.Add(MLVariableCreator("RightMapX", RightCam.mapx));
Variables.Add(MLVariableCreator("LeftMapY", LeftCam.mapy));
Variables.Add(MLVariableCreator("RightMapY", RightCam.mapy));
var Writer = new MatFileWriter("ImageMap.mat", Variables, false);
DisparityMapGPU = new GpuImage<Gray, byte>(ImageHeight, ImageWidth);
DisparityMap = new Image<Gray, byte>(ImageWidth, ImageHeight);
GPU_Block_Matcher = new GpuStereoBM(NumDisparities, BlockSize);
}
private void createMatFile()
{
List<MLArray> mlList = new List<MLArray>();
mlList.Add(createStruct());
try
{
MatFileWriter mfw = new MatFileWriter(folder + @"\" + exercise +criticalC1 + criticalC2 + criticalC3 + "-" + index + ".mat", mlList, true);
//StringBuilder sb = new StringBuilder();
//sb.Appe
StreamWriter w = File.AppendText(folder + @"\" + exercise + ".txt");
w.WriteLine(criticalC1 + " " + criticalC2 + " " + criticalC3);
w.WriteLine(criticalC11 + " " + criticalC21 + " " + criticalC31);
w.WriteLine(criticalC12 + " " + criticalC22 + " " + criticalC32);
w.WriteLine(criticalC13 + " " + criticalC23 + " " + criticalC33);
w.WriteLine(criticalC14 + " " + criticalC24 + " " + criticalC34);
w.Flush();
}
catch (Exception err)
{
Console.WriteLine("shit...");
}
}
