| PST SDK 6.0.0.0-272350a

reference.c
#ifdef WIN32
#include <windows.h>
#else
#include <unistd.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 "pstsdk_c.h"
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include <signal.h>
/*
* Helper function for clear printing of 4x4 matrices.
*/
static inline void PrintMatrix(float mat[16])
{
for (int y = 0; y < 4; ++y)
{
for (int x = 0; x < 4; ++x)
{
printf("%f \t", mat[x + y * 4]);
}
printf("\n");
}
}
/* Control variable for main loop */
static sig_atomic_t running = 1;
/* Number of data points to grab before application termination */
static const uint32_t numberOfSamplesToGrab = 1000;
/*
* Implementation of a tracker callback function.
* The OnTrackerData() callback function receives the data as soon as it becomes
* available and prints the tracking target pose to the command line.
*/
void OnTrackerData(const PstTrackerData* tracker_data, EPstErrorStatus status)
{
static uint32_t samplesGrabbed = 0;
if (samplesGrabbed++ >= numberOfSamplesToGrab)
running = 0;
// Do something with the received data.
}
/*
* Implement the exit handler to shut-down the PST Tracker connection on application termination.
*/
static void Exithandler(int sig)
{
pst_sdk_shutdown();
running = 0;
}
// Print the last error message.
void PrintLastErrorMessage()
{
char* last_error_message = NULL;
EPstErrorStatus error_status = pst_alloc_and_get_last_error_message(&last_error_message);
if (error_status != PST_ERROR_STATUS_OK)
{
last_error_message = "Failed to allocate memory error.";
}
printf("last error message: %s \n", last_error_message);
pst_free(last_error_message);
}
// Check error status and shutdown tracker upon error.
void CheckErrorCode(EPstErrorStatus status)
{
if (status != PST_ERROR_STATUS_OK)
{
PrintLastErrorMessage();
pst_sdk_shutdown();
exit(status);
}
}
// Check reference values of the arrays are equal.
bool IsEqual(float get_reference[16], float set_reference[16])
{
float eps = 1e-4f;
bool references_are_equal = true;
for (size_t i = 0; i < 16; ++i)
{
references_are_equal = references_are_equal && (fabs(get_reference[i] - set_reference[i]) < eps);
}
return references_are_equal;
}
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
PstTracker ctracker;
#ifdef WIN32
// Create an instance of the Tracker object using the default configuration path and file names.
CheckErrorCode(pst_tracker_init(&ctracker));
#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
CheckErrorCode(pst_tracker_init4(&ctracker, "", "config.cfg", "models.db", argv[1]));
#endif
char* version_string;
CheckErrorCode(pst_tracker_alloc_and_get_version_info(&ctracker, &version_string));
// Print version number of the tracker server being used.
printf("Running PST Server version %s \n", version_string);
pst_free(version_string);
// Register the OnTrackerData callback function to the tracker server.
CheckErrorCode(pst_tracker_add_tracker_data_callback(&ctracker, OnTrackerData));
// Start the tracker server.
CheckErrorCode(pst_tracker_start(&ctracker));
// Perform a system check to see if the tracker server is running OK and print the result.
printf("System check: %i \n", pst_tracker_system_check(&ctracker));
// Set the frame rate to 30 Hz.
CheckErrorCode(pst_tracker_set_framerate(&ctracker, 30.0));
// 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().
double fps;
CheckErrorCode(pst_tracker_get_framerate(&ctracker, &fps));
printf("Frame rate set to %f\n\n", fps);
printf("*******************\n\n");
// Get the transformation matrix for the current reference system.
printf("Current reference system transformation matrix:\n");
float reference[16];
CheckErrorCode(pst_tracker_get_reference(&ctracker, reference));
PrintMatrix(reference);
printf("\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 set_reference[16] = { -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 };
CheckErrorCode(pst_tracker_set_reference(&ctracker, set_reference, false));
printf("New reference system transformation matrix:\n");
float get_reference[16];
CheckErrorCode(pst_tracker_get_reference(&ctracker, get_reference));
PrintMatrix(get_reference);
// Check if the reference system was set according to the input.
if (!IsEqual(get_reference, set_reference))
{
printf("Reference not set correctly!\n");
}
else
{
printf("Reference set correctly!\n");
}
printf("\n\n*******************\n\n");
// Trying to set the reference using a non-orthonormal transformation matrix (this should fail).
float non_orthonormal_set_reference[16] = { -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 };
if(pst_tracker_set_reference(&ctracker, non_orthonormal_set_reference, false) != PST_ERROR_STATUS_OK)
{
// This should fail because the supplied matrix is non-orthonormal
printf("Reference input correctly ignored!\n");
PrintLastErrorMessage();
}
else
{
printf("Reference input incorrectly applied!\n");
}
printf("New reference system after applying non-orthonormal transformation:\n");
float non_orthonormal_get_reference[16];
CheckErrorCode(pst_tracker_get_reference(&ctracker, non_orthonormal_get_reference));
PrintMatrix(non_orthonormal_get_reference);
printf("\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 relative_set_reference[16] = { 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 };
CheckErrorCode(pst_tracker_set_reference(&ctracker, relative_set_reference, true));
printf("New reference system after applying relative transformation:\n");
float relative_get_reference[16];
CheckErrorCode(pst_tracker_get_reference(&ctracker, relative_get_reference));
PrintMatrix(relative_get_reference);
printf("\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).
CheckErrorCode(pst_tracker_set_default_reference(&ctracker));
printf("Reset default reference system:\n");
float get_default_reference[16];
CheckErrorCode(pst_tracker_get_reference(&ctracker, get_default_reference));
PrintMatrix(get_default_reference);
// Main loop, wait for auto-termination.
while (running == 1)
{
#ifdef WIN32
Sleep(100);
#else
usleep(100000);
#endif
}
// Make sure that the connection to the PST Tracker is shut down properly.
pst_tracker_destroy(&ctracker);
// Pause command line to see results.
printf("Press enter to continue...\n");
getchar();
return 0;
}