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
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...
void setDerivativeCutoff(scalar_type dcutoff)
void setMinCutoff(scalar_type mincutoff)
value_filter_return_type filter(scalar_type dt, const value_type x)
void setBeta(scalar_type beta)
Use angular velocity reports, if available, to predict the future orientation.
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,.
Tracker filter based on the one-Euro filter by Jan Ciger jan.ciger@reviatech.com
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 int unregister_change_handler(void *userdata, vrpn_TRACKERCHANGEHANDLER handler, vrpn_int32 sensor=vrpn_ALL_SENSORS)
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.
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_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
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,...