reference.cpp

This example can be found in Development\reference\reference.cpp.

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.

#ifdef WIN32
#include <windows.h>
#endif

/*
 * Define handler functions required to ensure a clean shutdown of the PST Tracker when the
 * application is terminated.
 */

static void Exithandler(int sig);

#ifdef WIN32
BOOL WINAPI ConsoleHandler(DWORD CEvent)
{
    Exithandler(CEvent);
    return TRUE;
}
#endif

/* End of handler functions */

#include <memory>
#include <iostream>

#include "pst-sdk.h"
#include "TrackerExceptions.h"

/*
 * Helper function for clear printing of 4x4 matrices.
 */
static inline void PrintMatrix(const std::array<float, 16>& mat)
{    
    for (int y = 0; y < 4; ++y)
    {
        for (int x = 0; x < 4; ++x)
        {
            std::cout << mat[x + y * 4] << "\t";
        }
        std::cout << "\n";
    }
}

/* Control variable for main loop */
static bool running = true;

/* Number of data points to grab before application termination */
static const uint32_t numberOfSamplesToGrab = 1000;

/*
 * Implementation of the PSTech::pstsdk::Listener class to receive tracking data.
 * The OnTrackerData() callback function receives the data as soon as it becomes 
 * available.
 */
class MyListener : public PSTech::pstsdk::Listener
{    
    virtual void OnTrackerData(PSTech::pstsdk::TrackerData& td)
    {
        static uint32_t samplesGrabbed = 0;
        if (samplesGrabbed++ >= numberOfSamplesToGrab)
            running = false;

        // Do something with the received data.
    }
} listener;

/*
 * Create a shared pointer for storing the PSTech::pstsdk::Tracker object instance.
 * By making use of a shared pointer, proper destruction of the PST Tracker connection
 * is ensured in case the application is shut down.
 */
std::shared_ptr<PSTech::pstsdk::Tracker> pst;

/*
 * Implement the exit handler to shut-down the PST Tracker connection on application terrmination.
 */
static void Exithandler(int sig)
{
    pst->Shutdown();
}

int main(int argc, char *argv[])
{
    // Register the exit handler with the application
#ifdef WIN32
    SetConsoleCtrlHandler((PHANDLER_ROUTINE)ConsoleHandler, TRUE);
#else
    signal(SIGTERM, Exithandler);
    signal(SIGKILL, Exithandler);
    signal(SIGQUIT, Exithandler);
    signal(SIGINT, Exithandler);
#endif

    // 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.
        pst = std::make_shared<PSTech::pstsdk::Tracker>();

        // Print version number of the tracker server being used.
        std::cout << "Running PST Server version " << pst->GetVersionInfo() << "\n";

        // Register the listener object to the tracker server.
        pst->AddTrackerListener(&listener);
        
        // Start the tracker server.
        pst->Start();
        
        // Perform a system check to see if the tracker server is running OK and print the result.
        std::cout << "System check: " << (int)pst->Systemcheck() << "\n";
        
        // Set the frame rate to 30 Hz.
        pst->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().
        std::cout << "Frame rate set to " << pst->GetFramerate() << "\n\n";
        std::cout << "*******************\n\n";

        // Get the transformation matrix for the current reference system.
        std::cout << "Current reference system transformation matrix:\n";
        PrintMatrix(pst->GetReference());

        std::cout << "\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.
        std::array<float, 16> 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 };
        pst->SetReference(reference);
        std::cout << "New reference system transformation matrix:\n";
        PrintMatrix(pst->GetReference());

        // Check if the reference system was set according to the input.
        if (pst->GetReference() != reference)
            std::cout << "Reference not set correctly!\n";
        else
            std::cout << "Reference set correctly!\n";

        std::cout << "\n\n*******************\n\n";

        // Trying to set the reference using a non-orthonormal transformation matrix.
        std::array<float, 16> 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 };
        pst->SetReference(nonOrthonormalReference);
        std::cout << "New reference system after applying non-orthonormal transformation:\n";
        PrintMatrix(pst->GetReference());

        // Check to see if the reference was set, which shouldn't be the case.
        if (pst->GetReference() != nonOrthonormalReference)
            std::cout << "Reference not set correctly!\n";
        else
            std::cout << "Reference set correctly!\n";

        std::cout << "\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.
        std::array<float, 16> 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 };
        pst->SetReference(relativeReference, true);
        std::cout << "New reference system after applying relative transformation:\n";
        PrintMatrix(pst->GetReference());
        
        std::cout << "\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).
        pst->SetDefaultReference();
        std::cout << "Reset default reference system:\n";
        PrintMatrix(pst->GetReference());

        // Main loop, wait for auto-termination.
        while (running)
        {
            Sleep(100);
        }
    }
    catch (PSTech::TrackerException &e)
    {
        // Catch PSTech::TrackerException exceptions and print error messages.
        std::cout << e.full_description() << "\n";
    }

    // Make sure that the connection to the PST Tracker is shut down properly.
    pst->Shutdown();
    
    // Pause command line to see results.
    std::cout << "Press enter to continue...\n";
    std::cin.get();

    return 0;
}