Loading...
Searching...
No Matches
reference.c

This example can be found in `examples\c\reference\reference.c`.

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.).

#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(const 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(const float get_reference[16], const 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;
}
pst_tracker_get_framerate
EPstErrorStatus pst_tracker_get_framerate(const PstTracker *ctracker, double *fps)
Get current frame rate.
pst_tracker_init
EPstErrorStatus pst_tracker_init(PstTracker *ctracker)
This function initializes the tracker object.
pst_alloc_and_get_last_error_message
EPstErrorStatus pst_alloc_and_get_last_error_message(char **cstring)
This function returns the last error message that was recorded by the system.
pst_tracker_init4
EPstErrorStatus pst_tracker_init4(PstTracker *ctracker, const char path[], const char config_file[], const char db_file[], const char grabber_name[])
This function initializes the tracker object.
pst_tracker_destroy
void pst_tracker_destroy(PstTracker *ctracker)
pst_sdk_shutdown
void pst_sdk_shutdown()
Shutdown the tracking system, stopping tracking.
pst_tracker_set_default_reference
EPstErrorStatus pst_tracker_set_default_reference(PstTracker *ctracker)
Reset the reference system to the default reference system.
pst_tracker_alloc_and_get_version_info
EPstErrorStatus pst_tracker_alloc_and_get_version_info(const PstTracker *ctracker, char **version_string)
Get version information of the SDK.
pst_free
void pst_free(void *data)
Free data allocated by the PST SDK.
pst_tracker_start
EPstErrorStatus pst_tracker_start(PstTracker *ctracker)
Start tracking.
pst_tracker_set_framerate
EPstErrorStatus pst_tracker_set_framerate(PstTracker *ctracker, double fps)
Set tracker frame rate.
pst_tracker_get_reference
EPstErrorStatus pst_tracker_get_reference(const PstTracker *ctracker, float creference[16])
Gets the transformation matrix for the current reference system.
pst_tracker_add_tracker_data_callback
EPstErrorStatus pst_tracker_add_tracker_data_callback(PstTracker *ctracker, void(*on_tracker_data)(const PstTrackerData *, EPstErrorStatus))
Add a callback function for tracker data.
pst_tracker_system_check
EPstStatusMessage pst_tracker_system_check(const PstTracker *ctracker)
Check if the tracker is running correctly.
EPstErrorStatus
EPstErrorStatus
Tracker error messages enum class.
Definition pstsdk_c.h:44
pst_tracker_set_reference
EPstErrorStatus pst_tracker_set_reference(PstTracker *ctracker, const float creference[16], bool relative)
Set the reference system in which tracking results are reported.
PST_ERROR_STATUS_OK
@ PST_ERROR_STATUS_OK
Definition pstsdk_c.h:45
pstsdk_c.h
PstTrackerData
Tracking information retrieved from tracker.
Definition pstsdk_c.h:248
PstTracker
Main PST SDK struct for tracker communication.
Definition pstsdk_c.h:264