vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
vrpn_Tracker_Filter.C
Go to the documentation of this file.
1// Internal Includes
3#include "vrpn_SendTextMessageStreamProxy.h" // for operator<<, etc
4
5// Library/third-party includes
6// - none
7
8// Standard includes
9#include <iostream>
10#include <sstream> // for operator<<, basic_ostream, etc
11#include <string> // for char_traits, basic_string, etc
12#include <stddef.h> // for size_t
13#include <stdio.h> // for fprintf, NULL, stderr
14#include <string.h> // for memset
15#include <math.h>
16
17void VRPN_CALLBACK vrpn_Tracker_FilterOneEuro::handle_tracker_update(void *userdata, const vrpn_TRACKERCB info)
18{
19 // Get pointer to the object we're dealing with.
21
22 // See if this sensor is within our range. If not, we ignore it.
23 if (info.sensor >= me->d_channels) {
24 return;
25 }
26
27 // Filter the position and orientation and then report the filtered value
28 // for this channel. Keep track of the delta-time, and update our current
29 // time so we get the right one next time.
30 double dt = vrpn_TimevalDurationSeconds(info.msg_time, me->d_last_report_times[info.sensor]);
31 if (dt <= 0) { dt = 1; } // Avoid divide-by-zero in case of fluke.
32 vrpn_float64 pos[3];
33 vrpn_float64 quat[4];
34 memcpy(pos, info.pos, sizeof(pos));
35 memcpy(quat, info.quat, sizeof(quat));
36 const vrpn_float64 *filtered = me->d_filters[info.sensor].filter(dt, pos);
37 q_vec_copy(me->pos, filtered);
38 const double *q_filtered = me->d_qfilters[info.sensor].filter(dt, quat);
39 q_normalize(me->d_quat, q_filtered);
40 me->timestamp = info.msg_time;
41 me->d_sensor = info.sensor;
42 me->d_last_report_times[info.sensor] = info.msg_time;
43
44 // Send the filtered report.
45 char msgbuf[512];
46 int len = me->encode_to(msgbuf);
47 if (me->d_connection->pack_message(len, me->timestamp, me->position_m_id,
49 fprintf(stderr, "vrpn_Tracker_FilterOneEuro: cannot write message: tossing\n");
50 }
51}
52
54 const char *listen_tracker_name,
55 unsigned channels, vrpn_float64 vecMinCutoff,
56 vrpn_float64 vecBeta, vrpn_float64 vecDerivativeCutoff,
57 vrpn_float64 quatMinCutoff, vrpn_float64 quatBeta,
58 vrpn_float64 quatDerivativeCutoff)
59 : vrpn_Tracker(name, con)
60 , d_channels(channels)
61{
62 // Allocate space for the times. Fill them in with now.
63 d_last_report_times = new struct timeval[channels];
64
66
67 // Allocate space for the filters.
68 d_filters = new vrpn_OneEuroFilterVec[channels];
69 d_qfilters = new vrpn_OneEuroFilterQuat[channels];
70
71 // Fill in the parameters for each filter.
72 for (int i = 0; i < static_cast<int>(channels); ++i) {
73 d_filters[i].setMinCutoff(vecMinCutoff);
74 d_filters[i].setBeta(vecBeta);
75 d_filters[i].setDerivativeCutoff(vecDerivativeCutoff);
76
77 d_qfilters[i].setMinCutoff(quatMinCutoff);
78 d_qfilters[i].setBeta(quatBeta);
79 d_qfilters[i].setDerivativeCutoff(quatDerivativeCutoff);
80 }
81
82 // Open and set up callback handler for the tracker we're listening to.
83 // If the name starts with the '*' character, use the server
84 // connection rather than making a new one.
85 if (listen_tracker_name[0] == '*') {
86 d_listen_tracker = new vrpn_Tracker_Remote(&(listen_tracker_name[1]),
88 } else {
89 d_listen_tracker = new vrpn_Tracker_Remote(listen_tracker_name);
90 }
91 if (d_listen_tracker) d_listen_tracker->register_change_handler(this, handle_tracker_update);
92}
93
95{
96 d_listen_tracker->unregister_change_handler(this, handle_tracker_update);
97 try {
98 delete d_listen_tracker;
99 } catch (...) {
100 fprintf(stderr, "vrpn_Tracker_FilterOneEuro::~vrpn_Tracker_FilterOneEuro(): delete failed\n");
101 return;
102 }
103 try {
104 if (d_qfilters) { delete[] d_qfilters; d_qfilters = NULL; }
105 if (d_filters) { delete[] d_filters; d_filters = NULL; }
106 if (d_last_report_times) { delete[] d_last_report_times; d_last_report_times = NULL; }
107 } catch (...) {
108 fprintf(stderr, "vrpn_Tracker_FilterOneEuro::~vrpn_Tracker_FilterOneEuro(): delete failed\n");
109 return;
110 }
111}
112
114{
115 // See if we have anything new from our tracker.
116 d_listen_tracker->mainloop();
117
118 // Call the server_mainloop on our unique base class.
120}
121
123 std::string myName, vrpn_Connection *c, std::string origTrackerName,
124 vrpn_int32 numSensors, vrpn_float64 predictionTime, bool estimateVelocity)
125 : vrpn_Tracker_Server(myName.c_str(), c, numSensors)
126 , d_estimateVelocity(estimateVelocity)
127{
128 // Do the things all tracker servers need to do.
129 num_sensors = numSensors;
131
132 // Set values that don't need to be set in the list above, so that we
133 // don't worry about out-of-order warnings from the compiler in case we
134 // adjust the order in the header file in the future.
135 d_predictionTime = predictionTime;
136 d_numSensors = numSensors;
137
138 // If the name of the tracker we're using starts with a '*' character,
139 // we use our own connection to talk with it. Otherwise, we open a remote
140 // tracker with the specified name.
141 try {
142 if (origTrackerName[0] == '*') {
143 d_origTracker = new vrpn_Tracker_Remote(&(origTrackerName.c_str()[1]), c);
144 } else {
145 d_origTracker = new vrpn_Tracker_Remote(origTrackerName.c_str());
146 }
147 } catch (...) {
148 d_origTracker = NULL;
149 return;
150 }
151
152 // Initialize the rotational state of each sensor. There has not bee
153 // an orientation report, so we set its time to 0.
154 // Set up callback handlers for the pose and pose velocity messages,
155 // from which we will extract the orientation and orientation velocity
156 // information.
157 for (vrpn_int32 i = 0; i < numSensors; i++) {
158 q_type no_rotation = { 0, 0, 0, 1 };
159 RotationState rs;
160 q_copy(rs.d_rotationAmount, no_rotation);
161 rs.d_rotationInterval = 1;
163 rs.d_lastReportTime.tv_sec = 0;
164 rs.d_lastReportTime.tv_usec = 0;
165 d_rotationStates.push_back(rs);
166 }
167
168 // Register handler for all sensors, so we'll be told the ID
169 d_origTracker->register_change_handler(this, handle_tracker_report);
170 d_origTracker->register_change_handler(this, handle_tracker_velocity_report);
171}
172
174{
175 // Call the generic server mainloop routine, since this is a server
177}
178
180{
181 try {
182 delete d_origTracker;
183 } catch (...) {
184 fprintf(stderr, "vrpn_Tracker_DeadReckoning_Rotation::~vrpn_Tracker_DeadReckoning_Rotation(): delete failed\n");
185 return;
186 }
187}
188
190{
191 //========================================================================
192 // Figure out which rotation state we're supposed to use.
193 if (sensor >= d_numSensors) {
195 << "sendNewPrediction: Asked for sensor " << sensor
196 << " but I only have " << d_numSensors
197 << "sensors. Discarding.";
198 return;
199 }
201 d_rotationStates[sensor];
202
203 //========================================================================
204 // If we haven't had a tracker report yet, nothing to send.
205 if (state.d_lastReportTime.tv_sec == 0) {
206 return;
207 }
208
209 //========================================================================
210 // If we don't have permission to estimate velocity and haven't gotten it
211 // either, then we just pass along the report.
213 report_pose(sensor, state.d_lastReportTime, state.d_lastPosition,
214 state.d_lastOrientation);
215 return;
216 }
217
218 //========================================================================
219 // Estimate the future orientation based on the current angular velocity
220 // estimate and the last reported orientation. Predict it into the future
221 // the amount we've been asked to.
222
223 // Start with the previous orientation.
224 q_type newOrientation;
225 q_copy(newOrientation, state.d_lastOrientation);
226
227 // Rotate it by the amount to rotate once for every integral multiple
228 // of the rotation time we've been asked to go.
229 double remaining = d_predictionTime;
230 while (remaining > state.d_rotationInterval) {
231 q_mult(newOrientation, state.d_rotationAmount, newOrientation);
232 remaining -= state.d_rotationInterval;
233 }
234
235 // Then rotate it by the remaining fractional amount.
236 double fractionTime = remaining / state.d_rotationInterval;
237 q_type identity = { 0, 0, 0, 1 };
238 q_type fractionRotation;
239 q_slerp(fractionRotation, identity, state.d_rotationAmount, fractionTime);
240 q_mult(newOrientation, fractionRotation, newOrientation);
241
242 //========================================================================
243 // Find out the future time for which we will be predicting by adding the
244 // prediction interval to our last report time.
245 struct timeval future_time;
246 struct timeval delta;
247 delta.tv_sec = static_cast<unsigned long>(d_predictionTime);
248 double remainder = d_predictionTime - delta.tv_sec;
249 delta.tv_usec = static_cast<unsigned long>(remainder * 1e6);
250 future_time = vrpn_TimevalSum(delta, state.d_lastReportTime);
251
252 //========================================================================
253 // Pack our predicted tracker report for this future time.
254 // Use the position portion of the report unaltered.
255 if (0 != report_pose(sensor, future_time, state.d_lastPosition, newOrientation)) {
256 fprintf(stderr, "vrpn_Tracker_DeadReckoning_Rotation::sendNewPrediction(): Can't report pose\n");
257 }
258}
259
261 const vrpn_TRACKERCB info)
262{
263 // Find the pointer to the class that registered the callback and get a
264 // reference to the RotationState we're supposed to be using.
267 if (info.sensor >= me->d_numSensors) {
269 << "Received tracker message from sensor " << info.sensor
270 << " but I only have " << me->d_numSensors
271 << "sensors. Discarding.";
272 return;
273 }
275 me->d_rotationStates[info.sensor];
276
278 // If we have not received any velocity reports, then we estimate
279 // the angular velocity using the last report (if any). The new
280 // combined rotation T3 = T2 * T1, where T2 is the difference in
281 // rotation between the last time (T1) and now (T3). We want to
282 // solve for T2 (so we can keep applying it going forward). We
283 // find it by right-multiuplying both sides of the equation by
284 // T1i (inverse of T1): T3 * T1i = T2.
285 if (state.d_lastReportTime.tv_sec != 0) {
286 q_type inverted;
287 q_invert(inverted, state.d_lastOrientation);
288 q_mult(state.d_rotationAmount, info.quat, inverted);
290 info.msg_time, state.d_lastReportTime);
291 // If we get a zero or negative rotation interval, we're
292 // not going to be able to handle it, so we set things back
293 // to no rotation over a unit-time interval.
294 if (state.d_rotationInterval < 0) {
295 state.d_rotationInterval = 1;
296 q_make(state.d_rotationAmount, 0, 0, 0, 1);
297 }
298 }
299 }
300
301 // Keep track of the position, orientation and time for the next report
302 q_vec_copy(state.d_lastPosition, info.pos);
303 q_copy(state.d_lastOrientation, info.quat);
304 state.d_lastReportTime = info.msg_time;
305
306 // We have new data, so we send a new prediction.
307 me->sendNewPrediction(info.sensor);
308}
309
311 const vrpn_TRACKERVELCB info)
312{
313 // Find the pointer to the class that registered the callback and get a
314 // reference to the RotationState we're supposed to be using.
317 if (info.sensor >= me->d_numSensors) {
319 << "Received velocity message from sensor " << info.sensor
320 << " but I only have " << me->d_numSensors
321 << "sensors. Discarding.";
322 return;
323 }
325 me->d_rotationStates[info.sensor];
326
327 // Store the rotational information and indicate that we have gotten
328 // a report of the tracker velocity.
330 q_copy(state.d_rotationAmount, info.vel_quat);
331 state.d_rotationInterval = info.vel_quat_dt;
332
333 // We have new data, so we send a new prediction.
334 me->sendNewPrediction(info.sensor);
335
336 // Pass through the velocity estimate.
337 me->report_pose_velocity(info.sensor, info.msg_time, info.vel,
338 info.vel_quat, info.vel_quat_dt);
339}
340
341//===============================================================================
342// Things related to the test() function go below here.
343
344typedef struct {
345 struct timeval time;
346 vrpn_int32 sensor;
347 q_vec_type pos;
348 q_type quat;
349} POSE_INFO;
350static POSE_INFO poseResponse;
351
352typedef struct {
353 struct timeval time;
354 vrpn_int32 sensor;
355 q_vec_type pos;
356 q_type quat;
357 double quat_dt;
358} VEL_INFO;
359static VEL_INFO velResponse;
360
361// Checks whether two floating-point values are close enough
362static bool isClose(double a, double b)
363{
364 return fabs(a - b) < 1e-6;
365}
366
367// Fills in the POSE_INFO structure that is passed in with the data it
368// receives.
369static void VRPN_CALLBACK handle_test_tracker_report(void *userdata,
370 const vrpn_TRACKERCB info)
371{
372 POSE_INFO *pi = static_cast<POSE_INFO *>(userdata);
373 pi->time = info.msg_time;
374 pi->sensor = info.sensor;
375 q_vec_copy(pi->pos, info.pos);
376 q_copy(pi->quat, info.quat);
377}
378
379// Fills in the VEL_INFO structure that is passed in with the data it
380// receives.
381static void VRPN_CALLBACK handle_test_tracker_velocity_report(void *userdata,
382 const vrpn_TRACKERVELCB info)
383{
384 VEL_INFO *vi = static_cast<VEL_INFO *>(userdata);
385 vi->time = info.msg_time;
386 vi->sensor = info.sensor;
387 q_vec_copy(vi->pos, info.vel);
388 q_copy(vi->quat, info.vel_quat);
389 vi->quat_dt = info.vel_quat_dt;
390}
391
393{
394 // Construct a loopback connection to be used by all of our objects.
396
397 // Create a tracker server to be the initator and a dead-reckoning
398 // rotation tracker to use it as a base; have it predict 1 second
399 // into the future.
400 // Create a remote tracker to listen to t1 and set up its callbacks for
401 // position and velocity reports. They will fill in the static structures
402 // listed above with whatever values they receive.
403 vrpn_Tracker_Server *t0, *t1;
405 try {
406 t0 = new vrpn_Tracker_Server("Tracker0", c, 2);
407 t1 = new vrpn_Tracker_DeadReckoning_Rotation("Tracker1", c, "*Tracker0", 2, 1);
408 tr = new vrpn_Tracker_Remote("Tracker1", c);
409 } catch (...) {
410 std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test: Out of memory" << std::endl;
411 return 100;
412 }
413
414 tr->register_change_handler(&poseResponse, handle_test_tracker_report);
415 tr->register_change_handler(&velResponse, handle_test_tracker_velocity_report);
416
417 // Set up the values in the pose and velocity responses with incorrect values
418 // so that things will fail if the class does not perform as expected.
419 q_vec_set(poseResponse.pos, 1, 1, 1);
420 q_make(poseResponse.quat, 0, 0, 1, 0);
421 poseResponse.time.tv_sec = 0;
422 poseResponse.time.tv_usec = 0;
423 poseResponse.sensor = -1;
424
425 // Send a pose report from sensors 0 and 1 on the original tracker that places
426 // them at (1,1,1) and with the identity rotation. We should get a response for
427 // each of them so we should end up with the sensor-1 response matching what we
428 // sent, with no change in position or orientation.
429 q_vec_type pos = { 1, 1, 1 };
430 q_type quat = { 0, 0, 0, 1 };
431 struct timeval firstSent;
432 vrpn_gettimeofday(&firstSent, NULL);
433 t0->report_pose(0, firstSent, pos, quat);
434 t0->report_pose(1, firstSent, pos, quat);
435
436 t0->mainloop();
437 t1->mainloop();
438 c->mainloop();
439 tr->mainloop();
440
441 if ( (poseResponse.time.tv_sec != firstSent.tv_sec + 1)
442 || (poseResponse.sensor != 1)
443 || (q_vec_distance(poseResponse.pos, pos) > 1e-10)
444 || (poseResponse.quat[Q_W] != 1)
445 )
446 {
447 std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected"
448 << " initial response: pos (" << poseResponse.pos[Q_X] << ", "
449 << poseResponse.pos[Q_Y] << ", " << poseResponse.pos[Q_Z] << "), quat ("
450 << poseResponse.quat[Q_X] << ", " << poseResponse.quat[Q_Y] << ", "
451 << poseResponse.quat[Q_Z] << ", " << poseResponse.quat[Q_W] << ")"
452 << " from sensor " << poseResponse.sensor
453 << " at time " << poseResponse.time.tv_sec << ":" << poseResponse.time.tv_usec
454 << std::endl;
455 try {
456 delete tr;
457 delete t1;
458 delete t0;
459 } catch (...) {
460 std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
461 return 1;
462 }
463 c->removeReference();
464 return 1;
465 }
466
467 // Send a second tracker report for sensor 0 coming 0.4 seconds later that has
468 // translated to position (2,1,1) and rotated by 0.4 * 90 degrees around Z.
469 // This should cause a prediction for one second later than this new pose
470 // message that has rotated by very close to 1.4 * 90 degrees.
471 q_vec_type pos2 = { 2, 1, 1 };
472 q_type quat2;
473 double angle2 = 0.4 * 90 * VRPN_PI / 180.0;
474 q_from_axis_angle(quat2, 0, 0, 1, angle2);
475 struct timeval p4Second = { 0, 400000 };
476 struct timeval firstPlusP4 = vrpn_TimevalSum(firstSent, p4Second);
477 t0->report_pose(0, firstPlusP4, pos2, quat2);
478
479 t0->mainloop();
480 t1->mainloop();
481 c->mainloop();
482 tr->mainloop();
483
484 double x, y, z, angle;
485 q_to_axis_angle(&x, &y, &z, &angle, poseResponse.quat);
486 if ((poseResponse.time.tv_sec != firstPlusP4.tv_sec + 1)
487 || (poseResponse.sensor != 0)
488 || (q_vec_distance(poseResponse.pos, pos2) > 1e-10)
489 || !isClose(x, 0) || !isClose(y, 0) || !isClose(z, 1)
490 || !isClose(angle, 1.4 * 90 * VRPN_PI / 180.0)
491 )
492 {
493 std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected"
494 << " predicted pose response: pos (" << poseResponse.pos[Q_X] << ", "
495 << poseResponse.pos[Q_Y] << ", " << poseResponse.pos[Q_Z] << "), quat ("
496 << poseResponse.quat[Q_X] << ", " << poseResponse.quat[Q_Y] << ", "
497 << poseResponse.quat[Q_Z] << ", " << poseResponse.quat[Q_W] << ")"
498 << " from sensor " << poseResponse.sensor
499 << std::endl;
500 try {
501 delete tr;
502 delete t1;
503 delete t0;
504 } catch (...) {
505 std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
506 return 1;
507 }
508 c->removeReference();
509 return 2;
510 }
511
512 // Send a velocity report for sensor 1 that has has translation by (1,0,0)
513 // and rotating by 0.4 * 90 degrees per 0.4 of a second around Z.
514 // This should cause a prediction for one second later than the first
515 // report that has rotated by very close to 90 degrees. The translation
516 // should be ignored, so the position should be the original position.
517 q_vec_type vel = { 0, 0, 0 };
518 t0->report_pose_velocity(1, firstPlusP4, vel, quat2, 0.4);
519
520 t0->mainloop();
521 t1->mainloop();
522 c->mainloop();
523 tr->mainloop();
524
525 q_to_axis_angle(&x, &y, &z, &angle, poseResponse.quat);
526 if ((poseResponse.time.tv_sec != firstSent.tv_sec + 1)
527 || (poseResponse.sensor != 1)
528 || (q_vec_distance(poseResponse.pos, pos) > 1e-10)
529 || !isClose(x, 0) || !isClose(y, 0) || !isClose(z, 1)
530 || !isClose(angle, 90 * VRPN_PI / 180.0)
531 )
532 {
533 std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected"
534 << " predicted velocity response: pos (" << poseResponse.pos[Q_X] << ", "
535 << poseResponse.pos[Q_Y] << ", " << poseResponse.pos[Q_Z] << "), quat ("
536 << poseResponse.quat[Q_X] << ", " << poseResponse.quat[Q_Y] << ", "
537 << poseResponse.quat[Q_Z] << ", " << poseResponse.quat[Q_W] << ")"
538 << " from sensor " << poseResponse.sensor
539 << std::endl;
540 try {
541 delete tr;
542 delete t1;
543 delete t0;
544 } catch (...) {
545 std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
546 return 1;
547 }
548 c->removeReference();
549 return 3;
550 }
551
552 // To test the behavior of the prediction code when we're moving around more
553 // than one axis, and when we're starting from a non-identity orientation,
554 // set sensor 1 to be rotated 180 degrees around X. Then send a velocity
555 // report that will produce a rotation of 180 degrees around Z. The result
556 // should match a prediction of 180 degrees around Y (plus or minus 180, plus
557 // or minus Y axis are all equivalent).
558 struct timeval oneSecond = { 1, 0 };
559 struct timeval firstPlusOne = vrpn_TimevalSum(firstSent, oneSecond);
560 q_type quat3;
561 q_from_axis_angle(quat3, 1, 0, 0, VRPN_PI);
562 t0->report_pose(1, firstPlusOne, pos, quat3);
563 q_type quat4;
564 q_from_axis_angle(quat4, 0, 0, 1, VRPN_PI);
565 t0->report_pose_velocity(1, firstPlusOne, vel, quat4, 1.0);
566
567 t0->mainloop();
568 t1->mainloop();
569 c->mainloop();
570 tr->mainloop();
571
572 q_to_axis_angle(&x, &y, &z, &angle, poseResponse.quat);
573 if ((poseResponse.time.tv_sec != firstPlusOne.tv_sec + 1)
574 || (poseResponse.sensor != 1)
575 || (q_vec_distance(poseResponse.pos, pos) > 1e-10)
576 || !isClose(x, 0) || !isClose(fabs(y), 1) || !isClose(z, 0)
577 || !isClose(fabs(angle), VRPN_PI)
578 )
579 {
580 std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected"
581 << " predicted pose + velocity response: pos (" << poseResponse.pos[Q_X] << ", "
582 << poseResponse.pos[Q_Y] << ", " << poseResponse.pos[Q_Z] << "), quat ("
583 << poseResponse.quat[Q_X] << ", " << poseResponse.quat[Q_Y] << ", "
584 << poseResponse.quat[Q_Z] << ", " << poseResponse.quat[Q_W] << ")"
585 << " from sensor " << poseResponse.sensor
586 << std::endl;
587 try {
588 delete tr;
589 delete t1;
590 delete t0;
591 } catch (...) {
592 std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
593 return 1;
594 }
595 c->removeReference();
596 return 4;
597 }
598
599 // To test the behavior of the prediction code when we're moving around more
600 // than one axis, and when we're starting from a non-identity orientation,
601 // set sensor 0 to start out at identity. Then in one second it will be
602 // rotated 180 degrees around X. Then in another second it will be rotated
603 // additionally by 90 degrees around Z; the prediction should be another
604 // 90 degrees around Z, which should turn out to compose to +/-180 degrees
605 // around +/- Y, as in the velocity case above.
606 // To make this work, we send a sequence of three poses, one second apart,
607 // starting at the original time. We do this on sensor 0, which has never
608 // had a velocity report, so that it will be using the pose-only prediction.
609 struct timeval firstPlusTwo = vrpn_TimevalSum(firstPlusOne, oneSecond);
610 q_from_axis_angle(quat3, 1, 0, 0, VRPN_PI);
611 t0->report_pose(0, firstSent, pos, quat);
612 t0->report_pose(0, firstPlusOne, pos, quat3);
613 q_from_axis_angle(quat4, 0, 0, 1, VRPN_PI / 2);
614 q_type quat5;
615 q_mult(quat5, quat4, quat3);
616 t0->report_pose(0, firstPlusTwo, pos, quat5);
617
618 t0->mainloop();
619 t1->mainloop();
620 c->mainloop();
621 tr->mainloop();
622
623 q_to_axis_angle(&x, &y, &z, &angle, poseResponse.quat);
624 if ((poseResponse.time.tv_sec != firstPlusTwo.tv_sec + 1)
625 || (poseResponse.sensor != 0)
626 || (q_vec_distance(poseResponse.pos, pos) > 1e-10)
627 || !isClose(x, 0) || !isClose(fabs(y), 1.0) || !isClose(z, 0)
628 || !isClose(fabs(angle), VRPN_PI)
629 )
630 {
631 std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected"
632 << " predicted pose + pose response: pos (" << poseResponse.pos[Q_X] << ", "
633 << poseResponse.pos[Q_Y] << ", " << poseResponse.pos[Q_Z] << "), quat ("
634 << poseResponse.quat[Q_X] << ", " << poseResponse.quat[Q_Y] << ", "
635 << poseResponse.quat[Q_Z] << ", " << poseResponse.quat[Q_W] << ")"
636 << " from sensor " << poseResponse.sensor
637 << "; axis = (" << x << ", " << y << ", " << z << "), angle = "
638 << angle
639 << std::endl;
640 try {
641 delete tr;
642 delete t1;
643 delete t0;
644 } catch (...) {
645 std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
646 return 1;
647 }
648 c->removeReference();
649 return 5;
650 }
651
652 // Done; delete our objects and return 0 to indicate that
653 // everything worked.
654 try {
655 delete tr;
656 delete t1;
657 delete t0;
658 } catch (...) {
659 std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
660 return 1;
661 }
662 c->removeReference();
663 return 0;
664}
665
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
int send_text_message(const char *msg, struct timeval timestamp, vrpn_TEXT_SEVERITY type=vrpn_TEXT_NORMAL, vrpn_uint32 level=0)
Sends a NULL-terminated text message from the device d_sender_id.
Generic connection class not specific to the transport mechanism.
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...
virtual int mainloop(const struct timeval *timeout=NULL)=0
Call each time through program main loop to handle receiving any incoming messages and sending any pa...
value_filter_return_type filter(scalar_type dt, const value_type x)
void mainloop()
This function should be called each time through app mainloop.
std::vector< RotationState > d_rotationStates
void sendNewPrediction(vrpn_int32 sensor)
Send a prediction based on the time of the new information; date the.
vrpn_Tracker_DeadReckoning_Rotation(std::string myName, vrpn_Connection *c, std::string origTrackerName, vrpn_int32 numSensors=1, vrpn_float64 predictionTime=1.0/60.0, bool estimateVelocity=true)
static void VRPN_CALLBACK handle_tracker_report(void *userdata, const vrpn_TRACKERCB info)
Static callback handler for tracker reports and tracker velocity reports.
static void VRPN_CALLBACK handle_tracker_velocity_report(void *userdata, const vrpn_TRACKERVELCB info)
static int test(void)
Test the class to make sure it functions as it should. Returns 0 on success,.
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)
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
virtual int register_change_handler(void *userdata, vrpn_TRACKERCHANGEHANDLER handler, vrpn_int32 sensor=vrpn_ALL_SENSORS)
virtual void mainloop()
This function should be called each time through app mainloop.
vrpn_Tracker_Server(const char *name, vrpn_Connection *c, vrpn_int32 sensors=1)
virtual int report_pose(const int sensor, const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
These functions should be called to report changes in state, once per sensor.
virtual int report_pose_velocity(const int sensor, const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_float64 interval, const vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
virtual int encode_to(char *buf)
int register_server_handlers(void)
vrpn_float64 d_quat[4]
vrpn_float64 vel[3]
vrpn_Tracker(const char *name, vrpn_Connection *c=NULL, const char *tracker_cfg_file_name=NULL)
vrpn_int32 d_sensor
vrpn_float64 pos[3]
vrpn_int32 num_sensors
struct timeval timestamp
vrpn_int32 position_m_id
struct timeval msg_time
vrpn_int32 sensor
vrpn_float64 quat[4]
vrpn_float64 pos[3]
vrpn_float64 vel[3]
vrpn_float64 vel_quat_dt
struct timeval msg_time
vrpn_float64 vel_quat[4]
@ vrpn_TEXT_WARNING
#define VRPN_CALLBACK
vrpn_Connection * vrpn_create_server_connection(const char *cname, const char *local_in_logfile_name, const char *local_out_logfile_name)
Create a server connection of arbitrary type (VRPN UDP/TCP, TCP, File, Loopback, MPI).
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
vrpn_OneEuroFilter< vrpn_QuatFilterable > vrpn_OneEuroFilterQuat
vrpn_OneEuroFilter vrpn_OneEuroFilterVec
Header allowing use of a output stream-style method of sending text messages from devices.
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
timeval vrpn_TimevalSum(const timeval &tv1, const timeval &tv2)
Definition vrpn_Shared.C:54
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99
#define VRPN_PI
Definition vrpn_Shared.h:13
Header for a class of trackers that read from one tracker and apply a filter to the inputs,...