reference.cs

This example can be found in examples\csharp\reference\reference.cs. When the PST SDK has been installed through the PST Software Suite installer the examples can be found in the Development folder.

This example shows how to use the PST SDK to adjust the PST Tracker reference system. The reference system defines the Cartesian coordinate system in which tracking results are reported. The example shows how to set the reference system by supplying a 4x4 homogeneous transformation matrix. It also shows how to check if the reference system was set successfully.

When compiling and running this example, please make sure that the required dependencies can be found by the executable (e.g. by copying the Redist directory into the build directory. When the PST SDK has been installed through the PST Software Suite installer the Redist folder can be found in the Development folder.).

// 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(); // Cleans up native resources only; does NOT remove listeners
            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.
                // Wrap the Tracker in a using statement to ensure Dispose() is called automatically
                using (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(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");
            }
 
            // Pause command line to see results.
            Console.Write("Press enter to continue...\n");
            Console.Read();
 
            return 0;
        }
    }
}