| PST SDK 6.0.0.0-272350a

reference/reference.cs
// Copyright PS-Tech B.V. All Rights Reserved.
using System;
using System.Linq;
using System.Runtime.InteropServices;
using System.Threading;
using PSTech.Pstsdk;
namespace PsTech.Reference
{
public class Reference
{
//Registering control handler function.
[DllImport("Kernel32")]
private static extern bool SetConsoleCtrlHandler(ConsoleEventHandler handler, bool add);
private delegate bool ConsoleEventHandler(CtrlType sig);
static ConsoleEventHandler ConsoleCtrlEventHandler;
public enum CtrlType
{
CtrlCEvent = 0,
CtrlBreakEvent = 1,
CtrlCloseEvent = 2,
CtrlLogOffEvent = 5,
CtrlShutdownEvent = 6,
}
private static bool ConsoleHandler(CtrlType sig)
{
Exithandler((int)sig);
return true;
}
public static void PrintMatrix(float[] matrix)
{
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
Console.Write(matrix[j + i * 4] + "\t");
}
Console.WriteLine();
}
}
// Control variable for main loop.
static bool Running = true;
// Number of data points to grab before application termination.
static int NumberOfSamplesToGrab = 1000;
static int SamplesGrabbed = 0;
class MyTrackerListener : TrackerListener
{
public override void OnTrackerData(TrackerData data, ErrorStatus status)
{
if (SamplesGrabbed++ >= NumberOfSamplesToGrab)
{
Running = false;
}
// Do something with the received data.
}
}
static void Exithandler(int sig)
{
Tracker.Shutdown();
Running = false;
}
static int Main(string[] args)
{
// Register the exit handler with the application
ConsoleCtrlEventHandler += new ConsoleEventHandler(ConsoleHandler);
SetConsoleCtrlHandler(ConsoleCtrlEventHandler, true);
// Implement error handling of PSTech.TrackerException exceptions to prevent
// improper PST Tracker shutdown on errors.
try
{
// Create an instance of the Tracker object using the default configuration path and file names.
Tracker tracker = new Tracker();
// Print version number of the tracker server being used.
Console.Write("Running PST Server version " + tracker.GetVersionInfo() + "\n");
//Create tracker listener with callback functions for data and/or mode updates.
TrackerListener listener = new MyTrackerListener();
// Register the TrackerListener object to the tracker server.
tracker.AddTrackerListener(ref listener);
// Start the tracker server.
tracker.StartTracker();
// Perform a system check to see if the tracker server is running OK and print the result.
Console.Write("System check: " + tracker.Systemcheck() + "\n\n");
// Set the frame rate to 30 Hz.
tracker.SetFramerate(30);
// Print the new frame rate to see if it was set correctly. Note that for PST HD and Pico
// trackers the frame rate actually being set can differ from the value provided to SetFramerate().
Console.Write("Frame rate set to " + tracker.GetFramerate() + "\n\n");
Console.Write("*******************\n\n");
// Get the transformation matrix for the current reference system.
Console.Write("Current reference system transformation matrix:\n");
PrintMatrix(tracker.GetReference());
Console.Write("\n\n*******************\n\n");
// Define new reference system transformation matrix rotating the reference system by
// 90 degrees around the X-axis and 180 degrees around the Y-axis. Then translate the
// origin of the reference system by 0.1 m in the X direction, -0.5 m in the Y direction
// and 0.5 m in the Z direction.
float[] reference = { -1.0f, 0.0f, 0.0f, 0.1f,
0.0f, 0.0f, 1.0f, -0.5f,
0.0f, 1.0f, 0.0f, 0.5f,
0.0f, 0.0f, 0.0f, 1.0f };
tracker.SetReference(reference);
Console.Write("New reference system transformation matrix:\n");
PrintMatrix(tracker.GetReference());
// Check if the reference system was set according to the input.
if (tracker.GetReference().SequenceEqual(reference))
{
Console.Write("Reference set correctly!\n");
}
else
{
Console.Write("Reference not set correctly!\n");
}
Console.Write("\n\n*******************\n\n");
// Trying to set the reference using a non-orthonormal transformation matrix (this should fail).
float[] nonOrthoNormalReference = { -1.0f, 1.0f, 0.0f, 0.1f,
0.0f, 0.0f, -1.0f, -0.5f,
0.0f, -1.0f, 0.0f, 0.5f,
0.0f, 0.0f, 0.0f, 1.0f };
try
{
tracker.SetReference(nonOrthoNormalReference);
Console.Write("Reference input incorrectly applied!");
}
catch(PSTech.TrackerException e)
{
// This should fail because the supplied matrix is non-orthonormal
Console.Write("Reference input correctly ignored: " + e.Message + "\n");
}
Console.Write("New reference system after applying non-orthonormal transformation:\n");
PrintMatrix(tracker.GetReference());
Console.Write("\n\n*******************\n\n");
// Adjust the reference system by applying a relative transformation. The new reference system will have
// the X-axis pointing outward from the tracker and the Y-axis parallel to the tracker front.
float[] relativeReference = { 0.0f, -1.0f, 0.0f, 0.5f,
1.0f, 0.0f, 0.0f, 0.4f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f };
tracker.SetReference(relativeReference, true);
Console.Write("New reference system after applying relative transformation:\n");
PrintMatrix(tracker.GetReference());
Console.Write("\n\n*******************\n\n");
// Reset reference system to default (origin at 1 m form center of the PST Tracker,
// Z-axis pointing outward from the PST Tracker).
tracker.SetDefaultReference();
Console.Write("Reset default reference system:\n");
PrintMatrix(tracker.GetReference());
// Main loop, wait for auto-termination.
while (Running)
{
Thread.Sleep(100);
}
}
catch (Exception e)
{
// Catch PSTech.TrackerException exceptions and print error messages.
Console.Write(e + "\n");
}
finally
{
// Make sure that the connection to the PST Tracker is shut down properly.
Tracker.Shutdown();
}
// Pause command line to see results.
Console.Write("Press enter to continue...\n");
Console.Read();
return 0;
}
}
}