Loading...
Searching...
No Matches
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;
}
PstStringIoStream.h
TrackerExceptions.h
PSTech::PSTException::what
virtual const char * what() const
PSTech::PSTException::full_description
virtual const char * full_description() const
PSTech::TrackerException
Definition TrackerExceptions.h:54
PSTech::Utils::PstArray
Definition PstArray.h:21
PSTech::pstsdk::Listener
Abstract listener class for receiving tracking information and tracking mode updates.
Definition pstsdk_cpp.h:38
PSTech::pstsdk::Listener::OnTrackerData
virtual void OnTrackerData(const TrackerData &data)
Callback function receiving tracking information from the tracker.
Definition pstsdk_cpp.h:49
PSTech::pstsdk::Tracker
Main PST SDK class implementing tracker communication.
Definition pstsdk_cpp.h:101
PSTech::pstsdk::Tracker::Start
void Start()
Start tracking.
PSTech::pstsdk::Tracker::GetVersionInfo
Utils::PstString GetVersionInfo() const
Get version information of the SDK.
PSTech::pstsdk::Tracker::Systemcheck
StatusMessage Systemcheck() const
Check if the tracker is running correctly.
PSTech::pstsdk::Tracker::Shutdown
static void Shutdown()
Shutdown the tracking system, stopping tracking.
PSTech::pstsdk::Tracker::GetFramerate
double GetFramerate() const
Get current frame rate.
PSTech::pstsdk::Tracker::GetReference
Utils::PstArray< float, 16 > GetReference() const
Gets the transformation matrix for the current reference system.
PSTech::pstsdk::Tracker::SetDefaultReference
void SetDefaultReference()
Reset the reference system to the default reference system.
PSTech::pstsdk::Tracker::AddTrackerListener
static void AddTrackerListener(Listener *listener)
Add a listener for receiving tracker data and tracking mode updates.
PSTech::pstsdk::Tracker::RemoveTrackerListener
static void RemoveTrackerListener(Listener *listener)
Remove a listener for receiving tracker data and tracking mode updates.
PSTech::pstsdk::Tracker::SetReference
void SetReference(const Utils::PstArray< float, 16 > &reference, bool relative=false)
Set the reference system in which tracking results are reported.
PSTech::pstsdk::Tracker::SetFramerate
void SetFramerate(double fps)
Set tracker frame rate.
pstsdk_cpp.h