23 if (info.
sensor >= me->d_channels) {
31 if (dt <= 0) { dt = 1; }
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);
49 fprintf(stderr,
"vrpn_Tracker_FilterOneEuro: cannot write message: tossing\n");
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)
60 , d_channels(channels)
63 d_last_report_times =
new struct timeval[channels];
72 for (
int i = 0; i < static_cast<int>(channels); ++i) {
78 d_qfilters[i].
setBeta(quatBeta);
85 if (listen_tracker_name[0] ==
'*') {
98 delete d_listen_tracker;
100 fprintf(stderr,
"vrpn_Tracker_FilterOneEuro::~vrpn_Tracker_FilterOneEuro(): delete failed\n");
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; }
108 fprintf(stderr,
"vrpn_Tracker_FilterOneEuro::~vrpn_Tracker_FilterOneEuro(): delete failed\n");
124 vrpn_int32 numSensors, vrpn_float64 predictionTime,
bool estimateVelocity)
126 , d_estimateVelocity(estimateVelocity)
142 if (origTrackerName[0] ==
'*') {
157 for (vrpn_int32 i = 0; i < numSensors; i++) {
158 q_type no_rotation = { 0, 0, 0, 1 };
184 fprintf(stderr,
"vrpn_Tracker_DeadReckoning_Rotation::~vrpn_Tracker_DeadReckoning_Rotation(): delete failed\n");
195 <<
"sendNewPrediction: Asked for sensor " << sensor
197 <<
"sensors. Discarding.";
224 q_type newOrientation;
237 q_type identity = { 0, 0, 0, 1 };
238 q_type fractionRotation;
240 q_mult(newOrientation, fractionRotation, newOrientation);
245 struct timeval future_time;
246 struct timeval delta;
249 delta.tv_usec =
static_cast<unsigned long>(remainder * 1e6);
256 fprintf(stderr,
"vrpn_Tracker_DeadReckoning_Rotation::sendNewPrediction(): Can't report pose\n");
269 <<
"Received tracker message from sensor " << info.
sensor
271 <<
"sensors. Discarding.";
319 <<
"Received velocity message from sensor " << info.
sensor
321 <<
"sensors. Discarding.";
350static POSE_INFO poseResponse;
359static VEL_INFO velResponse;
362static bool isClose(
double a,
double b)
364 return fabs(a - b) < 1e-6;
369static void VRPN_CALLBACK handle_test_tracker_report(
void *userdata,
372 POSE_INFO *pi =
static_cast<POSE_INFO *
>(userdata);
375 q_vec_copy(pi->pos, info.
pos);
376 q_copy(pi->quat, info.
quat);
381static void VRPN_CALLBACK handle_test_tracker_velocity_report(
void *userdata,
384 VEL_INFO *vi =
static_cast<VEL_INFO *
>(userdata);
387 q_vec_copy(vi->pos, info.
vel);
410 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test: Out of memory" << std::endl;
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;
429 q_vec_type
pos = { 1, 1, 1 };
430 q_type quat = { 0, 0, 0, 1 };
431 struct timeval firstSent;
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)
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
460 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
471 q_vec_type pos2 = { 2, 1, 1 };
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 };
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)
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
505 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
517 q_vec_type
vel = { 0, 0, 0 };
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)
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
545 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
558 struct timeval oneSecond = { 1, 0 };
561 q_from_axis_angle(quat3, 1, 0, 0,
VRPN_PI);
564 q_from_axis_angle(quat4, 0, 0, 1,
VRPN_PI);
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)
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
592 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
609 struct timeval firstPlusTwo =
vrpn_TimevalSum(firstPlusOne, oneSecond);
610 q_from_axis_angle(quat3, 1, 0, 0,
VRPN_PI);
613 q_from_axis_angle(quat4, 0, 0, 1,
VRPN_PI / 2);
615 q_mult(quat5, quat4, quat3);
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)
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 = "
645 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
659 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
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
~vrpn_Tracker_DeadReckoning_Rotation()
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)
vrpn_Tracker_Remote * d_origTracker
static int test(void)
Test the class to make sure it functions as it should. Returns 0 on success,.
vrpn_float64 d_predictionTime
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 ...
~vrpn_Tracker_FilterOneEuro()
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)
bool d_receivedAngularVelocityReport
vrpn_float64 d_lastOrientation[4]
vrpn_float64 d_rotationAmount[4]
vrpn_float64 d_lastPosition[3]
double d_rotationInterval
struct timeval d_lastReportTime
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)
#define vrpn_gettimeofday
Header for a class of trackers that read from one tracker and apply a filter to the inputs,...