reference.cpp

 
#ifdef WIN32
#include <windows.h>
#else
#include <csignal>
#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 <iostream>
#include <thread>
#include <chrono>
#include <atomic>
 
#include "pstsdk_cpp.h"
#include "TrackerExceptions.h"
#include "PstStringIoStream.h"
 
/*
 * Helper function for clear printing of 4x4 matrices.
 */
static inline void PrintMatrix(const PSTech::Utils::PstArray<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 std::atomic<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(const PSTech::pstsdk::TrackerData& td) override
    {
        static uint32_t samplesGrabbed = 0;
        if (samplesGrabbed++ >= numberOfSamplesToGrab)
            running = false;
 
        // Do something with the received data.
    }
 
public:
    virtual ~MyListener() override
    {
        // Unregistering listener before destruction to prevent unwanted behavior
        PSTech::pstsdk::Tracker::RemoveTrackerListener(this);
    }
} listener;
 
/*
 * Implement the exit handler to shut-down the PST Tracker connection on application termination.
 */
static void Exithandler(int sig)
{
    PSTech::pstsdk::Tracker::Shutdown();
    running = false;
}
 
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.
#ifdef WIN32
        PSTech::pstsdk::Tracker pst;
#else
        // On Linux, specify the type of grabber that needs to be used as the last parameter: 
        // "basler_ace" for PST HD or "basler_dart" for PST Pico
        PSTech::pstsdk::Tracker pst("","config.cfg","models.db",argv[1]);
#endif
        // 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.
        PSTech::pstsdk::Tracker::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.
        PSTech::Utils::PstArray<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 (this should fail).
        PSTech::Utils::PstArray<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 };
        try
        {
            pst.SetReference(nonOrthonormalReference);
            std::cout << "Reference input incorrectly applied!\n";
        }
        catch(PSTech::TrackerException& e)
        {
            // This should fail because the supplied matrix is non-orthonormal
            std::cout << "Reference input correctly ignored: " << e.what() << "\n";
        }
        std::cout << "New reference system after applying non-orthonormal transformation:\n";
        PrintMatrix(pst.GetReference());
 
        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.
        PSTech::Utils::PstArray<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)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(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.
    PSTech::pstsdk::Tracker::Shutdown();
    
    // Pause command line to see results.
    std::cout << "Press enter to continue...\n";
    std::cin.get();
 
    return 0;
}