/// <summary>
/// Primary entry point.
/// </summary>
/// <param name="args">Command line arguments.</param>
//[STAThread]
public static void Main(string[] args)
{
string recfile = "data.zip";
string scenefile = "";
string commandfile = "";
string configfile = "";
int particlecount = 1;
double reftime = 0;
bool onlymapping = false;
bool realtime = false;
bool viewer = false;
bool filterhistory = false;
bool screenshotmode = false;
bool headless = false;
bool noterminate = false;
bool showhelp = false;
VehicleType input = VehicleType.Simulation;
NavigationAlgorithm algorithm = NavigationAlgorithm.PHD;
OptionSet options = new OptionSet {
{ "f|scene=", "Scene description file. Simulated, recorded or device id.", f => scenefile = f },
{ "r|recfile=", "Recording file. Saves State and events for reviewing.", r => recfile = r },
{ "c|command=", "Auto-command file (simulates user input).", c => commandfile = c },
{ "g|config=", "Configuration file. Contains global constants.", g => configfile = g },
{ "a|algorithm=", "SLAM solver algorithm ('odometry', 'phd', 'loopy' or 'isam2').", a => algorithm = (a == "isam2") ? NavigationAlgorithm.ISAM2 : (a == "odometry") ? NavigationAlgorithm.Odometry : (a == "loopy") ? NavigationAlgorithm.LoopyPHD : NavigationAlgorithm.PHD },
{ "p|particles=", "Number of particles used for the RB-PHD.", (int p) => particlecount = p },
{ "y|onlymap", "Only do mapping, assuming known localization.", y => onlymapping = y != null },
{ "i|input=", "Vehicle input stream: 'kinect', 'simulation' or 'record'.", i => input = (i == "kinect") ? VehicleType.Kinect : (i == "record") ? VehicleType.Record : VehicleType.Simulation },
{ "R|realtime", "Process the system in realtime, instead of a fixed step.", R => realtime = R != null },
{ "v|view", "View a precorded session.", v => viewer = v != null },
{ "H|history=", "Trajectory history mode: either 'filter' or 'smooth'.", h => filterhistory = (h == "filter") },
{ "t|reftime=", "Reference time.", (double t) => reftime = t },
{ "s|screenshot", "Screenshot mode: just take the screenshots in the tags.", s => screenshotmode = s != null },
{ "x|headless", "Run headless, i.e. with no GUI.", x => headless = x != null },
{ "q|noterminate", "Skip simulation termination due command depletion o similars.", q => noterminate = q != null },
{ "h|help", "Show this message and exit.", h => showhelp = h != null }
};
try {
options.Parse(args);
}
catch (OptionException e) {
Console.Write("monorfs: ");
Console.WriteLine(e.Message);
Console.WriteLine("Try monorfs --help for more information.");
Environment.Exit(1);
}
if (showhelp)
{
ShowHelp(options);
return;
}
TimeSpan time = DateTime.Now.ToUniversalTime() - new DateTime(2010, 1, 1, 0, 0, 0, DateTimeKind.Utc);
Util.SeedGenerators((int)time.TotalSeconds);
if (!KinectVehicle.Initialize())
{
KinectVehicle.Shutdown();
Environment.Exit(2);
}
// Reading configuration from cfg file specified in argument
// or (if none was specified) from the internal config file of the recording file
if (!string.IsNullOrEmpty(configfile))
{
try {
Config.FromFile(configfile);
}
catch (FileNotFoundException e) {
Console.WriteLine("Error: Configuration file '" + e.FileName + "' not found.");
Environment.Exit(3);
}
}
else if (input == VehicleType.Record)
{
try {
Config.FromRecordFile(recfile);
}
catch (IOException) {
Console.WriteLine("Couldn't read config data from record file. Using default configuration.");
}
}
else
{
Console.WriteLine("Using default configuration.");
}
try {
switch (Config.Model)
{
case DynamicsModel.Linear1D:
Run <Linear1DMeasurer, LinearPose1D, LinearMeasurement1D>(
recfile, scenefile, commandfile,
particlecount, onlymapping, realtime, viewer,
filterhistory, reftime, screenshotmode, headless, noterminate,
input, algorithm);
break;
case DynamicsModel.Linear2D:
Run <Linear2DMeasurer, LinearPose2D, LinearMeasurement2D>(
recfile, scenefile, commandfile,
particlecount, onlymapping, realtime, viewer,
filterhistory, reftime, screenshotmode, headless, noterminate,
input, algorithm);
break;
case DynamicsModel.PRM3D:
default:
if (input == VehicleType.Kinect)
{
Run <KinectMeasurer, Pose3D, PixelRangeMeasurement>(
recfile, scenefile, commandfile,
particlecount, onlymapping, realtime, viewer,
filterhistory, reftime, screenshotmode, headless, noterminate,
input, algorithm);
}
else
{
Run <PRM3DMeasurer, Pose3D, PixelRangeMeasurement>(
recfile, scenefile, commandfile,
particlecount, onlymapping, realtime, viewer,
filterhistory, reftime, screenshotmode, headless, noterminate,
input, algorithm);
}
break;
}
}
catch (FileNotFoundException e) {
Console.WriteLine("Error: File '" + e.FileName + "' not found.");
Environment.Exit(4);
}
KinectVehicle.Shutdown();
signalthread.Abort();
}
FromFiles(string scenefile, string commandfile = "", int particlecount = 5,
VehicleType input = VehicleType.Simulation,
NavigationAlgorithm algorithm = NavigationAlgorithm.PHD,
bool onlymapping = false, bool realtime = false,
bool sidebar = true, bool noterminate = false)
{
Vehicle <MeasurerT, PoseT, MeasurementT> explorer;
Navigator <MeasurerT, PoseT, MeasurementT> navigator;
List <double[]> commands;
// batch methods extra information
TimedState estimate = null;
TimedArray odometry = null;
TimedMeasurements measurements = null;
try {
switch (input)
{
case VehicleType.Kinect:
if (typeof(MeasurementT) == typeof(PixelRangeMeasurement) &&
typeof(PoseT) == typeof(Pose3D) &&
typeof(MeasurerT) == typeof(KinectMeasurer))
{
explorer = KinectVehicle.FromSensor(scenefile, sidebar) as
Vehicle <MeasurerT, PoseT, MeasurementT>;
}
else
{
throw new ArgumentException("KinectVehicle can only handle 3D Pose + PixelRange");
}
break;
case VehicleType.Record:
if (algorithm == NavigationAlgorithm.LoopyPHD)
{
Console.WriteLine("reading Loopy PHD initialization data from file");
explorer = RecordVehicle <MeasurerT, PoseT, MeasurementT> .
FromFile(scenefile, true, out estimate, out odometry, out measurements);
}
else
{
explorer = RecordVehicle <MeasurerT, PoseT, MeasurementT> .
FromFile(scenefile, false, out estimate, out odometry, out measurements);
}
break;
case VehicleType.Simulation:
default:
explorer = SimulatedVehicle <MeasurerT, PoseT, MeasurementT> .
FromFile(File.ReadAllText(scenefile));
break;
}
}
catch (IOException) {
Console.WriteLine("Couldn't open vehicle file/device: " + scenefile);
explorer = new SimulatedVehicle <MeasurerT, PoseT, MeasurementT>();
}
try {
if (!string.IsNullOrEmpty(commandfile))
{
commands = FP.CommandsFromDescriptor(File.ReadAllLines(commandfile));
}
else
{
commands = new List <double[]>();
}
}
catch (FileNotFoundException) {
Console.WriteLine("command file not found: " + commandfile);
commands = new List <double[]>();
}
switch (algorithm)
{
case NavigationAlgorithm.Odometry:
navigator = new OdometryNavigator <MeasurerT, PoseT, MeasurementT>(explorer);
break;
case NavigationAlgorithm.ISAM2:
navigator = new ISAM2Navigator <MeasurerT, PoseT, MeasurementT>(explorer, onlymapping);
break;
case NavigationAlgorithm.LoopyPHD:
Console.WriteLine("initializing Loopy PHD");
if (estimate != null)
{
navigator = new LoopyPHDNavigator <MeasurerT, PoseT, MeasurementT>(explorer, estimate,
odometry, measurements);
}
else
{
navigator = new LoopyPHDNavigator <MeasurerT, PoseT, MeasurementT>(explorer, commands,
new PHDNavigator <MeasurerT, PoseT, MeasurementT>(explorer, particlecount, onlymapping));
}
explorer = new FakeVehicle <MeasurerT, PoseT, MeasurementT>(explorer, true);
Console.WriteLine("Loopy PHD initialized");
break;
case NavigationAlgorithm.PHD:
default:
navigator = new PHDNavigator <MeasurerT, PoseT, MeasurementT>(explorer, particlecount, onlymapping);
break;
}
string title = "monorfs - simulating " + scenefile + " [" + algorithm + "]";
return(new Simulation <MeasurerT, PoseT, MeasurementT>(title, explorer, navigator, commands, realtime, noterminate));
}
/// <summary>
/// Run the simulation or viewer.
/// </summary>
public static void Run <MeasurerT, PoseT, MeasurementT>(
string recfile, string scenefile, string commandfile,
int particlecount, bool onlymapping, bool realtime, bool viewer,
bool filterhistory, double reftime, bool screenshotmode, bool headless, bool noterminate,
VehicleType input, NavigationAlgorithm algorithm)
where PoseT : IPose <PoseT>, new()
where MeasurementT : IMeasurement <MeasurementT>, new()
where MeasurerT : IMeasurer <MeasurerT, PoseT, MeasurementT>, new()
{
if (viewer)
{
string tmpdir;
using (var sim = Viewer <MeasurerT, PoseT, MeasurementT> .
FromFiles(recfile, filterhistory, reftime, screenshotmode, out tmpdir)) {
sim.ScreenshotPrefix = recfile + "-screenshot-";
setSignalHandler(sim);
if (sim.Estimate.Count > 0)
{
sim.Run();
if (sim.TagChanged)
{
string datadir = Path.Combine(tmpdir, "data");
string bkpfile = recfile + ".bk";
Console.WriteLine(" -- tags were modified, rewriting");
File.WriteAllText(Path.Combine(datadir, "tags.out"), sim.SerializedTags);
Console.WriteLine(" -- old file was moved to " + bkpfile);
if (File.Exists(bkpfile))
{
File.Delete(bkpfile);
}
File.Move(recfile, bkpfile);
Console.WriteLine(" -- compressing");
ZipFile.CreateFromDirectory(datadir, recfile);
}
Directory.Delete(tmpdir, true);
}
else
{
Console.WriteLine("Nothing to show! Make sure there are appropriate frames " +
"in the specified file. This can happen if screenshot mode is " +
"used and there are no screenshot tags.");
}
}
}
else
{
using (var sim = Simulation <MeasurerT, PoseT, MeasurementT> .
FromFiles(scenefile, commandfile, particlecount, input,
algorithm, onlymapping, realtime, !headless, noterminate)) {
sim.ScreenshotPrefix = recfile + "-screenshot-";
sim.CheckpointFile = recfile;
setSignalHandler(sim);
if (headless)
{
Stopwatch timer = new Stopwatch();
Console.WriteLine("running headless");
timer.Start();
sim.RunHeadless();
timer.Stop();
Console.WriteLine("finished running (" + timer.Elapsed.TotalSeconds.ToString("F4") + " s)");
}
else
{
sim.Run();
}
sim.SaveToFile(recfile);
}
}
}
