PST SDK  5.2.0.0-0eac0f6
reference.py
1 """Reference example of the PST SDK
2 
3 This example shows how to use the PST SDK to adjust the PST Tracker reference system.
4 The reference system defines the Cartesian coordinate system in which tracking results
5 are reported. The example shows how to set the reference system by supplying a 4x4
6 homogeneous transformation matrix. It also shows how to check if the reference system
7 was set successfully.
8 
9 Copyright PS-Tech B.V. All Rights Reserved.
10 """
11 import context
12 import time
13 import sys
14 import pstech.pstsdk.tracker as pst
15 import pstech.pstsdk.errors as psterrors
16 
17 #global variables for example
18 running = True
19 max_samples = 1000
20 samples = 0
21 
22 """Helper function for printing """
23 def print_matrix(matrix):
24  for y in range(4):
25  for x in range(4):
26  print(str(matrix[x + y * 4]), end="\t")
27  print("")
28  print("\n")
29 
30 # Control variable for main loop
31 running = True
32 
33 # Number of data points to grab before application termination
34 max_samples = 100
35 
36 # Global number of samples
37 samples = 0
38 
39 """Implementation of a tracker callback function
40 
41 Implementation of a tracker callback function. The callback_function
42 receives the data as soon as it becomes available.
43 
44 Args:
45  tracker_data: Object containing tracking information retrieved from tracker
46  status_message: Status message reported by the tracker.
47 
48 See Also:
49  pstech.pstdk.trackerdata.TrackerData
50  pstech.pstsdk.errors.EStatusMessage
51 """
52 def callback_function(tracker_data, status_message):
53  global samples
54  global running
55 
56  if samples >= max_samples:
57  running = False
58 
59  samples += 1
60  # Do something here with the received data
61 
62 """Helper function to register the exit handler with the application"""
63 def register_exit_handler():
64  if sys.platform.startswith("linux"):
65  import signal
66  signal.signal(signal.SIGTERM, exit_handler)
67  signal.signal(signal.SIGHUP, exit_handler)
68  signal.signal(signal.SIGQUIT, exit_handler)
69  signal.signal(signal.SIGINT, exit_handler)
70  elif sys.platform.startswith("win"):
71  import win32api
72  win32api.SetConsoleCtrlHandler(exit_handler, True)
73 
74 """Implement the exit handler to shut-down the PST Tracker connection on application termination."""
75 def exit_handler(*args):
76  global running
77  pst.Tracker.shutdown()
78  running = False
79  return True
80 
81 def main():
82  if(len(sys.argv) < 2):
83  print("\nConfiguration Error: A camera configuration file needs to be specified. This file can be found in the Redist folder of your installation. "
84  "See the documentation of the Python bindings for more information.")
85  exit(0)
86 
87  # Register exit_handler for proper shutdown
88  register_exit_handler()
89 
90  try:
91  # Use Context Manager to prevent improper Tracker shutdown on errors.
92  # Create an instance of the Tracker object using the default configuration path and file names.
93  with pst.Tracker("", "","", sys.argv[1]) as tracker:
94  # Print version number of the tracker server being used.
95  print("Running PST Server version " + tracker.get_version_info())
96 
97  # Register the listener object to the tracker server.
98  tracker.add_tracker_listener(callback_function)
99 
100  # Start the tracker server.
101  tracker.start()
102 
103  # Perform a system check to see if the tracker server is running OK and print the result.
104  print("System check: " + str(tracker.system_check()))
105 
106  # Set the frame rate to 30 Hz.
107  tracker.set_framerate(30)
108 
109  # Print the new frame rate to see if it was set correctly. Note that for PST HD and Pico
110  # trackers the frame rate actually being set can differ from the value provided to Tracker.set_framerate().
111  print("Frame rate set to: " + str(tracker.get_framerate()))
112  print("***************************\n")
113 
114  # Get the transformation matrix for the current reference system.
115  print("Current reference system transformation matrix:")
116  print_matrix(tracker.get_reference())
117 
118  # Define new reference system transformation matrix rotating the reference system by
119  # 90 degrees around the X-axis and 180 degrees around the Y-axis. Then translate the
120  # origin of the reference system by 0.1 m in the X direction, -0.5 m in the Y direction
121  # and 0.5 m in the Z direction.
122  reference = [-1.0, 0.0, 0.0, 0.1,
123  0.0, 0.0, 1.0,-0.5,
124  0.0, 1.0, 0.0, 0.5,
125  0.0, 0.0, 0.0, 1.0]
126 
127  tracker.set_reference(reference)
128  print("New reference system transformation matrix:")
129  print_matrix(tracker.get_reference())
130 
131 
132  # Trying to set the reference using a non-orthonormal transformation matrix (this should fail).
133  non_orthonormal_reference = [-1.0, 1.0, 0.0, 0.1,
134  0.0, 0.0,-1.0,-0.5,
135  0.0,-1.0, 0.0, 0.5,
136  0.0, 0.0, 0.0, 1.0
137  ]
138  tracker.set_reference(non_orthonormal_reference)
139  print("New reference system after applying non-orthonormal transformation:")
140  print_matrix(tracker.get_reference())
141 
142  # Adjust the reference system by applying a relative transformation. The new reference system will have
143  # the X-axis pointing outward from the tracker and the Y-axis parallel to the tracker front.
144  relative_reference = [ 0.0,-1.0, 0.0, 0.5,
145  1.0, 0.0, 0.0, 0.4,
146  0.0, 0.0, 1.0, 0.0,
147  0.0, 0.0, 0.0, 1.0
148  ]
149  tracker.set_reference(relative_reference, True)
150  print("New reference system after applying relative transformation:")
151  print_matrix(tracker.get_reference())
152  print("***************************\n")
153 
154  # Reset reference system to default (origin at 1 m from center of the PST Tracker,
155  # Z-axis pointing outward from the PST Tracker).
156  tracker.set_default_reference()
157  print("Reset default reference system:")
158  print_matrix(tracker.get_reference())
159 
160  while running:
161  time.sleep(0.1)
162 
163  except psterrors.TrackerError as err:
164  # Catch TrackerError and print error messages.
165  print(err.message)
166 
167 if __name__ == "__main__":
168  main()