vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_Filter.C
Go to the documentation of this file.
1 // Internal Includes
2 #include "vrpn_Tracker_Filter.h"
3 
4 #include "vrpn_HumanInterface.h" // for vrpn_HidInterface, etc
5 #include "vrpn_OneEuroFilter.h" // for OneEuroFilterQuat, etc
6 #include "vrpn_SendTextMessageStreamProxy.h" // for operator<<, etc
7 
8 // Library/third-party includes
9 // - none
10 
11 // Standard includes
12 #include <sstream> // for operator<<, basic_ostream, etc
13 #include <string> // for char_traits, basic_string, etc
14 #include <stddef.h> // for size_t
15 #include <stdio.h> // for fprintf, NULL, stderr
16 #include <string.h> // for memset
17 
18 void VRPN_CALLBACK vrpn_Tracker_FilterOneEuro::handle_tracker_update(void *userdata, const vrpn_TRACKERCB info)
19 {
20  // Get pointer to the object we're dealing with.
21  vrpn_Tracker_FilterOneEuro *me = static_cast<vrpn_Tracker_FilterOneEuro *>(userdata);
22 
23  // See if this sensor is within our range. If not, we ignore it.
24  if (info.sensor >= me->d_channels) {
25  return;
26  }
27 
28  // Filter the position and orientation and then report the filtered value
29  // for this channel. Keep track of the delta-time, and update our current
30  // time so we get the right one next time.
31  double dt = vrpn_TimevalDurationSeconds(info.msg_time, me->d_last_report_times[info.sensor]);
32  if (dt <= 0) { dt = 1; } // Avoid divide-by-zero in case of fluke.
33  vrpn_float64 pos[3];
34  vrpn_float64 quat[4];
35  memcpy(pos, info.pos, sizeof(pos));
36  memcpy(quat, info.quat, sizeof(quat));
37  const vrpn_float64 *filtered = me->d_filters[info.sensor].filter(dt, pos);
38  q_vec_copy(me->pos, filtered);
39  const double *q_filtered = me->d_qfilters[info.sensor].filter(dt, quat);
40  q_normalize(me->d_quat, q_filtered);
41  me->timestamp = info.msg_time;
42  me->d_sensor = info.sensor;
43  me->d_last_report_times[info.sensor] = info.msg_time;
44 
45  // Send the filtered report.
46  char msgbuf[512];
47  int len = me->encode_to(msgbuf);
48  if (me->d_connection->pack_message(len, me->timestamp, me->position_m_id,
50  fprintf(stderr, "vrpn_Tracker_FilterOneEuro: cannot write message: tossing\n");
51  }
52 }
53 
55  const char *listen_tracker_name,
56  unsigned channels, vrpn_float64 vecMinCutoff,
57  vrpn_float64 vecBeta, vrpn_float64 vecDerivativeCutoff,
58  vrpn_float64 quatMinCutoff, vrpn_float64 quatBeta,
59  vrpn_float64 quatDerivativeCutoff)
60  : vrpn_Tracker(name, con)
61  , d_channels(channels)
62 {
63  // Allocate space for the times. Fill them in with now.
64  d_last_report_times = new struct timeval[channels];
65  if (d_last_report_times == NULL) {
66  fprintf(stderr,"vrpn_Tracker_FilterOneEuro::vrpn_Tracker_FilterOneEuro(): Out of memory\n");
67  d_channels = 0;
68  return;
69  }
70 
72 
73  // Allocate space for the filters.
74  d_filters = new vrpn_OneEuroFilterVec[channels];
75  d_qfilters = new vrpn_OneEuroFilterQuat[channels];
76  if ( (d_filters == NULL) || (d_qfilters == NULL) ) {
77  fprintf(stderr,"vrpn_Tracker_FilterOneEuro::vrpn_Tracker_FilterOneEuro(): Out of memory\n");
78  d_channels = 0;
79  return;
80  }
81 
82  // Fill in the parameters for each filter.
83  for (int i = 0; i < static_cast<int>(channels); ++i) {
84  d_filters[i].setMinCutoff(vecMinCutoff);
85  d_filters[i].setBeta(vecBeta);
86  d_filters[i].setDerivativeCutoff(vecDerivativeCutoff);
87 
88  d_qfilters[i].setMinCutoff(quatMinCutoff);
89  d_qfilters[i].setBeta(quatBeta);
90  d_qfilters[i].setDerivativeCutoff(quatDerivativeCutoff);
91  }
92 
93  // Open and set up callback handler for the tracker we're listening to.
94  // If the name starts with the '*' character, use the server
95  // connection rather than making a new one.
96  if (listen_tracker_name[0] == '*') {
97  d_listen_tracker = new vrpn_Tracker_Remote(&(listen_tracker_name[1]),
98  d_connection);
99  } else {
100  d_listen_tracker = new vrpn_Tracker_Remote(listen_tracker_name);
101  }
102  d_listen_tracker->register_change_handler(this, handle_tracker_update);
103 }
104 
106 {
107  d_listen_tracker->unregister_change_handler(this, handle_tracker_update);
108  delete d_listen_tracker;
109  if (d_qfilters) { delete [] d_qfilters; d_qfilters = NULL; }
110  if (d_filters) { delete [] d_filters; d_filters = NULL; }
111  if (d_last_report_times) { delete [] d_last_report_times; d_last_report_times = NULL; }
112 }
113 
115 {
116  // See if we have anything new from our tracker.
117  d_listen_tracker->mainloop();
118 
119  // server update
121 }
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
const value_filter_return_type filter(scalar_type dt, const value_type x)
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
virtual int register_change_handler(void *userdata, vrpn_TRACKERCHANGEHANDLER handler, vrpn_int32 sensor=vrpn_ALL_SENSORS)
Header for a class of trackers that read from one tracker and apply a filter to the inputs,...
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
void setMinCutoff(scalar_type mincutoff)
Generic connection class not specific to the transport mechanism.
#define VRPN_CALLBACK
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
Definition: vrpn_Shared.C:135
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
vrpn_Connection * d_connection
Connection that this object talks to.
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
vrpn_int32 sensor
Definition: vrpn_Tracker.h:286
void setDerivativeCutoff(scalar_type dcutoff)
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
Tracker filter based on the one-Euro filter by Jan Ciger jan.ciger@reviatech.com
virtual int unregister_change_handler(void *userdata, vrpn_TRACKERCHANGEHANDLER handler, vrpn_int32 sensor=vrpn_ALL_SENSORS)
Header allowing use of a output stream-style method of sending text messages from devices.
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
Header file that completely implements a direction and orientation filter on tracking reports; it doe...
vrpn_Tracker_FilterOneEuro(const char *name, vrpn_Connection *trackercon, const char *listen_tracker_name, unsigned channels, vrpn_float64 vecMinCutoff=1.15, vrpn_float64 vecBeta=0.5, vrpn_float64 vecDerivativeCutoff=1.2, vrpn_float64 quatMinCutoff=1.5, vrpn_float64 quatBeta=0.5, vrpn_float64 quatDerivativeCutoff=1.2)
vrpn_int32 d_sender_id
Sender ID registered with the connection.
struct timeval timestamp
Definition: vrpn_Tracker.h:100
void setBeta(scalar_type beta)
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
struct timeval msg_time
Definition: vrpn_Tracker.h:285
vrpn_float64 quat[4]
Definition: vrpn_Tracker.h:288
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:287
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80