private Task DoStartCalibrationAprilGrid(object o)
{
return(Task.Factory.StartNew(() =>
{
Parent.SyncContext.Post(f =>
{
CustomDialog customDialog;
customDialog = new CustomDialog()
{
Title = "Select extrinsic calibration file"
};
var dataContext = new ReplaySelectDialogModel(OnReplaySelectDialogClose(customDialog));
Parent.ReplayViewModel.Refresh();
dataContext.FilesForReplay.AddRange(Parent.ReplayViewModel.FilesForReplay.Where(x => !x.Item1.IsRemote));
customDialog.Content = new ReplaySelectDialog
{
DataContext = dataContext
};
Parent.DialogCoordinator.ShowMetroDialogAsync(Parent, customDialog);
}, null);
}));
}
private Task DoRunCalibration(object o)
{
return(Task.Factory.StartNew(() =>
{
bool response = o is string && !string.IsNullOrEmpty(o as string) && (o as string).Equals("response");
Parent.SyncContext.Post(c =>
{
CustomDialog customDialog;
if (response)
{
customDialog = new CustomDialog()
{
Title = "Select response calibration file"
};
}
else
{
customDialog = new CustomDialog()
{
Title = "Select vignette calibration file"
};
}
var dataContext = new ReplaySelectDialogModel(OnReplaySelectDialogClose(response, customDialog));
Parent.ReplayViewModel.Refresh();
dataContext.FilesForReplay.AddRange(Parent.ReplayViewModel.FilesForReplay.Where(x => !x.Item1.IsRemote));
customDialog.Content = new ReplaySelectDialog {
DataContext = dataContext
};
Parent.DialogCoordinator.ShowMetroDialogAsync(Parent, customDialog);
}, null);
}));
}
private Action <ReplaySelectDialogModel> OnReplaySelectDialogClose(CustomDialog customDialog)
{
return(obj =>
{
Parent.SyncContext.Send(d =>
{
Parent.DialogCoordinator.HideMetroDialogAsync(Parent, customDialog);
}, null);
Parent.SyncContext.Post(d =>
{
Task.Factory.StartNew(async() =>
{
ReplaySelectDialogModel replaySelectDialogModel = obj as ReplaySelectDialogModel;
if (replaySelectDialogModel.SelectedFile != null)
{
RemoteDataStore remoteDataStore = new RemoteDataStore(Parent.SettingContainer.Settings.ConnectionSettings.SelectedConnection.IpAddress, Parent.SettingContainer.Settings.ConnectionSettings.SelectedConnection.Username, Parent.SettingContainer.Settings.ConnectionSettings.SelectedConnection.Password);
string guid = Guid.NewGuid().ToString();
string localFile = "";
string remoteFile = "";
string remoteFolder = string.Format(@"/var/tmp/firefly/{0}", guid);
string expactString = string.Format("{0}@{1}:.{{0,}}[$]", Parent.SettingContainer.Settings.ConnectionSettings.SelectedConnection.Username, Parent.SettingContainer.Settings.ConnectionSettings.SelectedConnection.Hostname);
Parent.SyncContext.Send(c =>
{
localFile = replaySelectDialogModel.SelectedFile.FullPath;
remoteFile = string.Format(@"/var/tmp/firefly/{0}/{1}", guid, Path.GetFileName(localFile));
}, null);
var controller = await Parent.DialogCoordinator.ShowProgressAsync(Parent, "Please wait...", "Calculating calibration parameter now!", settings: Parent.MetroDialogSettings);
controller.SetIndeterminate();
controller.SetCancelable(false);
remoteDataStore.ExecuteCommands(new List <string>()
{
string.Format("mkdir -p {0}", remoteFolder)
}, expactString);
remoteDataStore.UploadFile(remoteFile, localFile);
remoteDataStore.UploadContentToFile(string.Format(@"{0}/target.yaml", remoteFolder), YamlTranslator.ConvertToYaml(new CalibrationTarget()
{
TargetType = CalibrationTargetType.Aprilgrid,
TagSize = Parent.SettingContainer.Settings.CalibrationSettings.AprilGridCalibration.TagSize,
TagSpacing = Parent.SettingContainer.Settings.CalibrationSettings.AprilGridCalibration.TagSpacingFactor,
TagCols = Parent.SettingContainer.Settings.CalibrationSettings.AprilGridCalibration.TagsX,
TagRows = Parent.SettingContainer.Settings.CalibrationSettings.AprilGridCalibration.TagsY
}));
remoteDataStore.ExecuteCommands(new List <string>()
{
string.Format(@"cd {0}", remoteFolder),
string.Format(@"unzip {0} -d {1}", Path.GetFileName(localFile), Path.GetFileNameWithoutExtension(localFile)),
@"source ~/kalibr_workspace/devel/setup.bash",
string.Format(@"kalibr_bagcreater --folder {0} --output-bag {0}.bag", Path.GetFileNameWithoutExtension(localFile)),
string.Format(@"kalibr_calibrate_cameras --bag {0}.bag --topics /cam0/image_raw --models pinhole-equi --target target.yaml --dont-show-report", Path.GetFileNameWithoutExtension(localFile)),
string.Format("pdftoppm report-cam-{0}.pdf result -png", Path.GetFileNameWithoutExtension(localFile))
}, expactString);
CameraChain cameraChain = YamlTranslator.ConvertFromYaml <CameraChain>(remoteDataStore.DownloadFileToMemory(string.Format("{0}/camchain-{1}.yaml", remoteFolder, Path.GetFileNameWithoutExtension(localFile))));
string outputPath = Path.Combine(Path.GetTempPath(), "firefly", guid);
if (!Directory.Exists(outputPath))
{
Directory.CreateDirectory(outputPath);
}
foreach (string file in remoteDataStore.GetAllFileNames(remoteFolder))
{
if (!file.Contains(".bag") && !file.Contains(".ffc"))
{
remoteDataStore.DownloadFile(string.Format("{0}/{1}", remoteFolder, file), Path.Combine(outputPath, file));
}
}
ShowResults(outputPath, cameraChain);
remoteDataStore.ExecuteCommands(new List <string>()
{
string.Format(@"rm -r {0}", remoteFolder)
}, expactString);
await controller.CloseAsync();
}
});
}, null);
});
}
private Task DoLoadFromFile(object o)
{
return(Task.Factory.StartNew(() =>
{
Parent.SyncContext.Post(c =>
{
CustomDialog customDialog = new CustomDialog()
{
Title = "Select calibration file"
};
var dataContext = new ReplaySelectDialogModel(obj =>
{
Parent.SyncContext.Post(d =>
{
Task.Factory.StartNew(async() =>
{
ReplaySelectDialogModel replaySelectDialogModel = obj as ReplaySelectDialogModel;
if (replaySelectDialogModel.SelectedFile != null)
{
MetroDialogSettings settings = new MetroDialogSettings()
{
AnimateShow = false,
AnimateHide = false
};
var controller = await Parent.DialogCoordinator.ShowProgressAsync(Parent, "Please wait...", "Loading calibration images!", settings: Parent.MetroDialogSettings);
controller.SetCancelable(false);
controller.SetIndeterminate();
string file = null;
string outputPath = null;
Parent.SyncContext.Send(async d2 =>
{
file = replaySelectDialogModel.SelectedFile.FullPath;
outputPath = Path.Combine(Path.GetTempPath(), "firefly", Guid.NewGuid().ToString());
}, null);
RawDataReader reader = new RawDataReader(file, RawReaderMode.Camera0);
reader.Open();
MemoryImageExporter exporter = new MemoryImageExporter();
exporter.AddFromReader(reader, delegate(double percent)
{
double value = percent;
value = value > 1 ? 1 : value;
controller.SetProgress(value);
});
reader.Close();
Parent.SyncContext.Post(d2 =>
{
int i = 0;
int count = exporter.Images.Count;
foreach (Mat m in exporter.Images)
{
//double value = 0.5 + i / count * 0.5;
//value = value > 1 ? 1 : value;
//controller.SetProgress(value);
if (i++ % 20 == 0)
{
int squaresX = Parent.SettingContainer.Settings.CalibrationSettings.ChArucoCalibrationSettings.SquaresX;
int squaresY = Parent.SettingContainer.Settings.CalibrationSettings.ChArucoCalibrationSettings.SquaresY;
float squareLength = Parent.SettingContainer.Settings.CalibrationSettings.ChArucoCalibrationSettings.SquareLength;
float markerLength = Parent.SettingContainer.Settings.CalibrationSettings.ChArucoCalibrationSettings.MarkerLength;
PredefinedDictionaryName dictionary = Parent.SettingContainer.Settings.CalibrationSettings.ChArucoCalibrationSettings.Dictionary;
ChArUcoImageContainer cauic = new ChArUcoImageContainer(squaresX, squaresY, squareLength, markerLength, dictionary);
CvImageContainer container = new CvImageContainer();
container.CvImage = m;
cauic.OriginalImage = container;
Images.Add(cauic);
}
}
controller.CloseAsync();
}, null);
}
}, TaskCreationOptions.LongRunning);
Parent.DialogCoordinator.HideMetroDialogAsync(Parent, customDialog);
}, null);
});
Parent.ReplayViewModel.Refresh();
dataContext.FilesForReplay.AddRange(Parent.ReplayViewModel.FilesForReplay.Where(x => !x.Item1.IsRemote));
customDialog.Content = new ReplaySelectDialog {
DataContext = dataContext
};
Parent.DialogCoordinator.ShowMetroDialogAsync(Parent, customDialog);
}, null);
}));
}
private Action <ReplaySelectDialogModel> OnReplaySelectDialogClose(bool response, CustomDialog customDialog)
{
return(obj =>
{
Parent.SyncContext.Send(d =>
{
Parent.DialogCoordinator.HideMetroDialogAsync(Parent, customDialog);
}, null);
Parent.SyncContext.Post(d =>
{
Task.Factory.StartNew(async() =>
{
ReplaySelectDialogModel replaySelectDialogModel = obj as ReplaySelectDialogModel;
if (replaySelectDialogModel.SelectedFile != null)
{
MetroDialogSettings settings = new MetroDialogSettings()
{
AnimateShow = false,
AnimateHide = false
};
var controller = await Parent.DialogCoordinator.ShowProgressAsync(Parent, "Please wait...", "Photometric calibration!", settings: Parent.MetroDialogSettings);
controller.SetCancelable(false);
controller.SetIndeterminate();
string file = null;
string outputPath = null;
double fxO = 0.0;
double fyO = 0.0;
double cxO = 0.0;
double cyO = 0.0;
double fxN = 0.0;
double fyN = 0.0;
double cxN = 0.0;
double cyN = 0.0;
double k1 = 0.0;
double k2 = 0.0;
double k3 = 0.0;
double k4 = 0.0;
int width = 0;
int height = 0;
List <double> responseValues = new List <double>();
Parent.SyncContext.Send(d2 =>
{
file = replaySelectDialogModel.SelectedFile.FullPath;
outputPath = Path.Combine(Path.GetTempPath(), "firefly", Guid.NewGuid().ToString());
fxO = Parent.CameraViewModel.OrginalCameraMatrix.GetValue(0, 0);
fyO = Parent.CameraViewModel.OrginalCameraMatrix.GetValue(1, 1);
cxO = Parent.CameraViewModel.OrginalCameraMatrix.GetValue(0, 2);
cyO = Parent.CameraViewModel.OrginalCameraMatrix.GetValue(1, 2);
fxN = Parent.CameraViewModel.NewCameraMatrix.GetValue(0, 0);
fyN = Parent.CameraViewModel.NewCameraMatrix.GetValue(1, 1);
cxN = Parent.CameraViewModel.NewCameraMatrix.GetValue(0, 2);
cyN = Parent.CameraViewModel.NewCameraMatrix.GetValue(1, 2);
k1 = Parent.CameraViewModel.DistortionCoefficients.GetValue(0, 0);
k2 = Parent.CameraViewModel.DistortionCoefficients.GetValue(0, 1);
k3 = Parent.CameraViewModel.DistortionCoefficients.GetValue(0, 2);
k4 = Parent.CameraViewModel.DistortionCoefficients.GetValue(0, 3);
if (!response)
{
responseValues.AddRange(ResponseValues.Select(f => f.Y));
}
width = Parent.CameraViewModel.ImageWidth;
height = Parent.CameraViewModel.ImageHeight;
}, null);
RawDataReader reader = new RawDataReader(file, RawReaderMode.Camera0);
reader.Open();
PhotometricCalibratrionExporter exporter = new PhotometricCalibratrionExporter(fxO, fyO, cxO, cyO, fxN, fyN, cxN, cyN, width, height, k1, k2, k3, k4, outputPath, response, responseValues);
exporter.Open();
exporter.AddFromReader(reader, delegate(double percent)
{
double value = percent * 0.33;
value = value > 1 ? 1 : value;
controller.SetProgress(value);
});
exporter.Close();
reader.Close();
Process p = new Process();
p.StartInfo.RedirectStandardOutput = true;
p.StartInfo.UseShellExecute = false;
p.StartInfo.WorkingDirectory = outputPath;
p.StartInfo.CreateNoWindow = true;
p.EnableRaisingEvents = true;
string options = "";
if (response)
{
p.StartInfo.FileName = Path.Combine(Path.GetDirectoryName(Assembly.GetExecutingAssembly().Location), "Tools", "responseCalib.exe");
}
else
{
p.StartInfo.FileName = Path.Combine(Path.GetDirectoryName(Assembly.GetExecutingAssembly().Location), "Tools", "vignetteCalib.exe");
options = "facW=7 facH=7";
}
p.StartInfo.Arguments = string.Format("{0}\\ -noGUI -showPercent {1}", outputPath, options);
p.OutputDataReceived += new DataReceivedEventHandler((s, e) =>
{
Debug.WriteLine(e.Data);
if (!string.IsNullOrEmpty(e.Data))
{
foreach (string line in e.Data.Split('\n'))
{
if (line.StartsWith("percent: "))
{
double percent = double.Parse(line.Replace("percent: ", ""), CultureInfo.InvariantCulture);
double value = 0.33 + percent * 0.66;
value = value > 1 ? 1 : value;
controller.SetProgress(value);
}
}
}
}
);
p.Exited += new EventHandler((s, e) =>
{
Parent.SyncContext.Post(async x =>
{
if (response)
{
ParseResponseResult(outputPath);
}
else
{
ParseVignetteResult(outputPath);
}
Directory.Delete(outputPath, true);
await controller.CloseAsync();
}, null);
});
p.Start();
p.BeginOutputReadLine();
}
}, TaskCreationOptions.LongRunning);
}, null);
});
}
private Action <ReplaySelectDialogModel> OnReplaySelectDialogClose(CustomDialog customDialog)
{
return(obj =>
{
Parent.SyncContext.Send(d =>
{
Parent.DialogCoordinator.HideMetroDialogAsync(Parent, customDialog);
}, null);
Parent.SyncContext.Post(d =>
{
Task.Factory.StartNew(async() =>
{
ReplaySelectDialogModel replaySelectDialogModel = obj as ReplaySelectDialogModel;
if (replaySelectDialogModel.SelectedFile != null)
{
RemoteDataStore remoteDataStore = new RemoteDataStore(Parent.SettingContainer.Settings.ConnectionSettings.SelectedConnection.IpAddress, Parent.SettingContainer.Settings.ConnectionSettings.SelectedConnection.Username, Parent.SettingContainer.Settings.ConnectionSettings.SelectedConnection.Password);
string guid = Guid.NewGuid().ToString();
string localFile = "";
string remoteFile = "";
string remoteFolder = string.Format(@"/var/tmp/firefly/{0}", guid);
string expactString = string.Format("{0}@{1}:.{{0,}}[$]", Parent.SettingContainer.Settings.ConnectionSettings.SelectedConnection.Username, Parent.SettingContainer.Settings.ConnectionSettings.SelectedConnection.Hostname);
string imuModel = Imu.ConvertImuModelToString(Parent.SettingContainer.Settings.CalibrationSettings.ImuCalibration.ImuModel);
Parent.SyncContext.Send(c =>
{
localFile = replaySelectDialogModel.SelectedFile.FullPath;
remoteFile = string.Format(@"/var/tmp/firefly/{0}/{1}", guid, Path.GetFileName(localFile));
}, null);
var controller = await Parent.DialogCoordinator.ShowProgressAsync(Parent, "Please wait...", "Calculating calibration parameter now!", settings: Parent.MetroDialogSettings);
controller.SetIndeterminate();
controller.SetCancelable(false);
remoteDataStore.ExecuteCommands(new List <string>()
{
string.Format("mkdir -p {0}", remoteFolder)
}, expactString);
remoteDataStore.UploadFile(remoteFile, localFile);
remoteDataStore.UploadContentToFile(string.Format(@"{0}/target.yaml", remoteFolder), YamlTranslator.ConvertToYaml(new CalibrationTarget()
{
TargetType = CalibrationTargetType.Aprilgrid,
TagSize = Parent.SettingContainer.Settings.CalibrationSettings.AprilGridCalibration.TagSize,
TagSpacing = Parent.SettingContainer.Settings.CalibrationSettings.AprilGridCalibration.TagSpacingFactor,
TagCols = Parent.SettingContainer.Settings.CalibrationSettings.AprilGridCalibration.TagsX,
TagRows = Parent.SettingContainer.Settings.CalibrationSettings.AprilGridCalibration.TagsY
}));
remoteDataStore.UploadContentToFile(string.Format(@"{0}/imu.yaml", remoteFolder), YamlTranslator.ConvertToYaml(new Imu()
{
RosTopic = "/imu0",
UpdateRate = Parent.SettingContainer.Settings.ImuSettings.UpdateRate,
AccelerometerNoiseDensity = Parent.SettingContainer.Settings.CalibrationSettings.ImuCalibration.AccelerometerNoiseDensity * Parent.SettingContainer.Settings.CalibrationSettings.ImuCalibration.AccelerometerNoiseDensitySafetyScale,
AccelerometerRandomWalk = Parent.SettingContainer.Settings.CalibrationSettings.ImuCalibration.AccelerometerRandomWalk * Parent.SettingContainer.Settings.CalibrationSettings.ImuCalibration.AccelerometerRandomWalkSafetyScale,
GyroscopeNoiseDensity = Parent.SettingContainer.Settings.CalibrationSettings.ImuCalibration.GyroscopeNoiseDensity * Parent.SettingContainer.Settings.CalibrationSettings.ImuCalibration.GyroscopeNoiseDensitySafetyScale,
GyroscopeRandomWalk = Parent.SettingContainer.Settings.CalibrationSettings.ImuCalibration.GyroscopeRandomWalk * Parent.SettingContainer.Settings.CalibrationSettings.ImuCalibration.GyroscopeRandomWalkSafetyScale
}));
remoteDataStore.UploadContentToFile(string.Format(@"{0}/cam.yaml", remoteFolder), YamlTranslator.ConvertToYaml(new CameraChain()
{
Cam0 = new Data.Storage.Model.Camera()
{
CameraModel = CameraModel.Pinhole,
DistortionModel = DistortionModel.Equidistant,
DistortionCoefficients = Parent.SettingContainer.Settings.CalibrationSettings.IntrinsicCalibrationSettings.DistCoeffs.ToArray(),
Fx = Parent.SettingContainer.Settings.CalibrationSettings.IntrinsicCalibrationSettings.Fx,
Fy = Parent.SettingContainer.Settings.CalibrationSettings.IntrinsicCalibrationSettings.Fy,
Cx = Parent.SettingContainer.Settings.CalibrationSettings.IntrinsicCalibrationSettings.Cx,
Cy = Parent.SettingContainer.Settings.CalibrationSettings.IntrinsicCalibrationSettings.Cy,
RosTopic = "/cam0/image_raw",
Height = Parent.SettingContainer.Settings.CameraSettings.Height,
Width = Parent.SettingContainer.Settings.CameraSettings.Width
}
}));
string options = string.Format(CultureInfo.InvariantCulture, "--dont-show-report --reprojection-sigma {0} {1}", Parent.SettingContainer.Settings.CalibrationSettings.ExtrinsicCalibrationSettings.ReprojectionSigma, Parent.SettingContainer.Settings.CalibrationSettings.ExtrinsicCalibrationSettings.TimeCalibration ? "--time-calibration" : "");
remoteDataStore.ExecuteCommands(new List <string>()
{
string.Format(@"cd {0}", remoteFolder),
string.Format(@"unzip {0} -d {1}", Path.GetFileName(localFile), Path.GetFileNameWithoutExtension(localFile)),
@"source ~/kalibr_workspace/devel/setup.bash",
string.Format(@"kalibr_bagcreater --folder {0} --output-bag {0}.bag", Path.GetFileNameWithoutExtension(localFile)),
string.Format(@"kalibr_calibrate_imu_camera --bag {0}.bag --cams cam.yaml --imu imu.yaml --imu-models {1} --target target.yaml {2}", Path.GetFileNameWithoutExtension(localFile), imuModel, options),
string.Format("pdftoppm report-imucam-{0}.pdf result -png", Path.GetFileNameWithoutExtension(localFile))
}, expactString);
CameraChain cameraChain = YamlTranslator.ConvertFromYaml <CameraChain>(remoteDataStore.DownloadFileToMemory(string.Format("{0}/camchain-imucam-{1}.yaml", remoteFolder, Path.GetFileNameWithoutExtension(localFile))));
ImuCain imuChain = YamlTranslator.ConvertFromYaml <ImuCain>(remoteDataStore.DownloadFileToMemory(string.Format("{0}/imu-{1}.yaml", remoteFolder, Path.GetFileNameWithoutExtension(localFile))));
List <string> availablePlots = new List <string>();
foreach (string file in remoteDataStore.GetAllFileNames(remoteFolder))
{
if (file.Contains(".csv"))
{
availablePlots.Add(file.Replace(string.Format("data-imucam-{0}.", Path.GetFileNameWithoutExtension(localFile)), "").Replace(".csv", ""));
}
}
Dictionary <string, string> plotDataImu = new Dictionary <string, string>();
foreach (string plot in availablePlots)
{
plotDataImu.Add(plot, remoteDataStore.DownloadFileToMemory(string.Format("{0}/data-imucam-{1}.{2}.csv", remoteFolder, Path.GetFileNameWithoutExtension(localFile), plot)));
}
string outputPath = Path.Combine(Path.GetTempPath(), "firefly", guid);
if (!Directory.Exists(outputPath))
{
Directory.CreateDirectory(outputPath);
}
foreach (string file in remoteDataStore.GetAllFileNames(remoteFolder))
{
if (!file.Contains(".bag") && !file.Contains(".ffc"))
{
remoteDataStore.DownloadFile(string.Format("{0}/{1}", remoteFolder, file), Path.Combine(outputPath, file));
}
}
CsvToMatlabWritter csvToMatlabWritter = new CsvToMatlabWritter(Path.Combine(outputPath, "plot-data.mat"));
csvToMatlabWritter.Write(plotDataImu, "ExtrinsicCalibrationData");
csvToMatlabWritter.Save();
ShowResults(outputPath, cameraChain, imuChain);
remoteDataStore.ExecuteCommands(new List <string>()
{
string.Format(@"rm -r {0}", remoteFolder)
}, expactString);
await controller.CloseAsync();
}
});
}, null);
});
}
