/// <summary>
/// Perform a deep copy of another general vehicle,
/// scaling the covariance matrices and setting a new detection probability.
/// </summary>
/// <param name="that">Copied vehicle.</param>
/// <param name="motioncovmultiplier">Scalar multiplier for the motion covariance matrix.</param>
/// <param name="measurecovmultiplier">Scalar multiplier for the measurement covariance matrix.</param>
/// <param name="pdetection">Probability of detection.</param>
/// <param name="clutter">Clutter density.</param>
/// <param name="copytrajectory">If true, the vehicle historic trajectory is copied. Relatively heavy operation.</param>
public KinectTrackVehicle(KinectVehicle that, double motioncovmultiplier, double measurecovmultiplier, double pdetection, double clutter, bool copytrajectory = false)
: base(that, motioncovmultiplier, measurecovmultiplier, pdetection, clutter, copytrajectory)
{
// Since FilmArea is a property (and Rectangle a struct)
// Inflate won't work (will occur on the temporary return value)
// so a local copy is needed
var newfilm = this.Measurer.FilmArea;
newfilm.Inflate(-that.Border, -that.Border);
this.Measurer = new KinectMeasurer(this.Measurer.VisionFocal,
newfilm,
this.Measurer.RangeClip,
that.ResX, that.ResY, that.Border,
() => that.DepthFrame);
}
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>
/// 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();
}
