vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Tracker_WiimoteHead.C
Go to the documentation of this file.
1
15/*
16 Copyright Iowa State University 2009-2010
17 Distributed under the Boost Software License, Version 1.0.
18 (See accompanying comment below or copy at
19 http://www.boost.org/LICENSE_1_0.txt)
20
21 Boost Software License - Version 1.0 - August 17th, 2003
22
23 Permission is hereby granted, free of charge, to any person or organization
24 obtaining a copy of the software and accompanying documentation covered by
25 this license (the "Software") to use, reproduce, display, distribute,
26 execute, and transmit the Software, and to prepare derivative works of the
27 Software, and to permit third-parties to whom the Software is furnished to
28 do so, all subject to the following:
29
30 The copyright notices in the Software and this entire statement, including
31 the above license grant, this restriction and the following disclaimer,
32 must be included in all copies of the Software, in whole or in part, and
33 all derivative works of the Software, unless such copies or derivative
34 works are solely in the form of machine-executable object code generated by
35 a source language processor.
36
37 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
38 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
39 FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
40 SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
41 FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
42 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
43 DEALINGS IN THE SOFTWARE.
44*/
45
46
47
48// Local Includes
49#include "quat.h" // for q_xyz_quat_type, q_vec_copy, etc
50#include "vrpn_Connection.h" // for vrpn_Connection, etc
52#include "vrpn_Types.h" // for vrpn_float64
53
54// Standard includes
55#include <math.h> // for tan, atan2, sqrt
56#include <stdio.h> // for NULL, fprintf, stderr
57#include <algorithm> // for swap
58#include <iostream> // for operator<<, basic_ostream, etc
59
60#undef VERBOSE
61
64inline static bool wm_isnan(const double x) {
65 return (x != x);
66}
67
70const double two = 2;
71
72// Some stats source: http://wiibrew.org/wiki/Wiimote#IR_Camera
73const double xResSensor = 1024.0, yResSensor = 768.0;
74
77const double fovX = Q_DEG_TO_RAD(43.0), fovY = Q_DEG_TO_RAD(32.00);
78//const double fovX = Q_DEG_TO_RAD(45.0), fovY = (fovX / xResSensor) * yResSensor;
79
80const double radPerPx = fovX / xResSensor;
81const double cvtDistToAngle = radPerPx / two;
83
86#define MAKE_IDENTITY_QUAT(dest) \
87 dest[0] = dest[1] = dest[2] = 0; dest[3] = 1
88
91#define MAKE_NULL_VEC(dest) \
92 dest[0] = dest[1] = dest[2] = 0
93
94
96 vrpn_Connection* trackercon,
97 const char* wiimote,
98 float update_rate,
99 float led_spacing) :
100 vrpn_Tracker(name, trackercon),
101 d_name(wiimote),
102 d_update_interval(update_rate ? (1.0 / update_rate) : 1.0 / 60.0),
103 d_blobDistance(led_spacing),
104 d_flipState(FLIP_UNKNOWN),
105 d_points(0),
106 d_ana(NULL),
107 d_contact(false),
108 d_lock(false),
109 d_updated(false),
110 d_gravDirty(true) {
111 // If the name is NULL, we're done.
112 if (wiimote == NULL) {
113 d_name = NULL;
114 fprintf(stderr, "vrpn_Tracker_WiimoteHead: "
115 "Can't start without a valid specified Wiimote device!");
116 return;
117 }
118
120
121 //--------------------------------------------------------------------
122 // Whenever we get a connection, set a flag so we make sure to send an
123 // update. Set up a handler to do this.
125 handle_connection, this);
126
127 //--------------------------------------------------------------------
128 // Set the current matrix to identity, the current timestamp to "now",
129 // the current matrix to identity in case we never hear from the Wiimote.
130 // Also, set the updated flag to send a single report
131 reset();
132
133 // put a little z translation as a saner default
134 d_currentPose.xyz[2] = 1;
135
136 // Set up our initial "default" pose to make sure everything is
137 // safely initialized before our first report.
139}
140
142
143 // If the analog pointer is NULL, we're done.
144 if (d_ana == NULL) {
145 return;
146 }
147
148 // Turn off the callback handler
150 (void)ret;
151
152 // Delete the analog device.
153 try {
154 delete d_ana;
155 } catch (...) {
156 fprintf(stderr, "vrpn_Tracker_WiimoteHead::~vrpn_Tracker_WiimoteHead(): delete failed\n");
157 return;
158 }
159 d_ana = NULL;
160}
161
167 _reset_pose();
169}
170
172 if (d_ana) {
173 // Turn off the callback handler and delete old analog
174 // if we already have an analog source
176 try {
177 delete d_ana;
178 } catch (...) {
179 fprintf(stderr, "vrpn_Tracker_WiimoteHead::setup_wiimote(): delete failed\n");
180 return;
181 }
182 d_ana = NULL;
183 }
184
185 // Open the analog device and point the remote at it.
186 // If the name starts with the '*' character, use the server
187 // connection rather than making a new one.
188 try {
189 if (d_name[0] == '*') {
191 } else {
193 }
194 } catch (...) { d_ana = NULL; }
195
196 if (d_ana == NULL) {
197 fprintf(stderr, "vrpn_Tracker_WiimoteHead: "
198 "Can't open Analog %s\n", d_name);
199 return;
200 }
201
202 // register callback
204 if (ret == -1) {
205 fprintf(stderr, "vrpn_Tracker_WiimoteHead: "
206 "Can't setup change handler on Analog %s\n", d_name);
207 try {
208 delete d_ana;
209 } catch (...) {
210 fprintf(stderr, "vrpn_Tracker_WiimoteHead::setup_wiimote(): delete failed\n");
211 return;
212 }
213 d_ana = NULL;
214 return;
215 }
216
217 // will notify when we catch the first report.
218 d_contact = false;
219}
220
222 struct timeval now;
223
224 // Call generic server mainloop, since we are a server
226
227 // Mainloop() the wiimote to get fresh values
228 if (d_ana != NULL) {
229 d_ana->mainloop();
230 }
231
232 // See if we have new data, or if it has been too long since our last
233 // report. Send a new report in either case.
234 vrpn_gettimeofday(&now, NULL);
235 double interval = vrpn_TimevalDurationSeconds(now, d_prevtime);
236
237 if (_should_report(interval)) {
238 // Figure out the new matrix based on the current values and
239 // the length of the interval since the last report
240 update_pose();
241
242 report();
243 }
244}
245
247 q_xyz_quat_type newPose;
248
249 // Start at the identity pose
250 MAKE_NULL_VEC(newPose.xyz);
251 MAKE_IDENTITY_QUAT(newPose.quat);
252
253 // If our gravity vector has changed and it's not 0,
254 // we need to update our gravity correction transform.
255 if (d_gravDirty && _have_gravity()) {
257 }
258
259 // Update pose estimate
260 _update_2_LED_pose(newPose);
261
262 if (d_lock) {
263 // Gravity correction
264 if (_have_gravity()) {
265 q_xyz_quat_compose(&d_currentPose, &d_currentPose, &d_gravityXform);
266 }
267
268 if (d_flipState == FLIP_UNKNOWN) {
270 if (d_flipState == FLIP_180) {
271 return; // must throw away first update after setting flip to 180
272 }
273 }
274
275 // Copy final pose into the tracker position and quaternion structures.
277 }
278}
279
281 // pack and deliver tracker report;
282 if (d_connection) {
283 char msgbuf[1000];
284 int len = encode_to(msgbuf);
286 position_m_id, d_sender_id, msgbuf,
288 fprintf(stderr, "vrpn_Tracker_WiimoteHead: "
289 "cannot write message: tossing\n");
290 }
291 } else {
292 fprintf(stderr, "vrpn_Tracker_WiimoteHead: "
293 "No valid connection\n");
294 }
295
296 // We just sent a report, so reset the time
298 d_updated = false;
299}
300
301// static
304
305 if (!wh) {
306 return;
307 }
308 if (!wh->d_contact) {
309#ifdef VERBOSE
310 fprintf(stderr, "vrpn_Tracker_WiimoteHead: "
311 "got first report from Wiimote!\n");
312#endif
313 }
314
315 int i, firstchan;
316 // Grab all the blobs
317 for (i = 0; i < 4; i++) {
318 firstchan = i * 3 + 4;
319 // -1 should signal a missing blob, but experimentally
320 // we sometimes get 0 instead
321 if (info.channel[firstchan] > 0
322 && info.channel[firstchan + 1] > 0
323 && info.channel[firstchan + 2] > 0) {
324 wh->d_vX[i] = info.channel[firstchan];
325 wh->d_vY[i] = info.channel[firstchan + 1];
326 wh->d_vSize[i] = info.channel[firstchan + 2];
327 wh->d_points = i + 1;
328 } else {
329 break;
330 }
331 }
332
333 wh->d_contact = true;
334 wh->d_updated = true;
335
336 bool newgrav = false;
337
338 // Grab gravity
339 for (i = 0; i < 3; i++) {
340 if (info.channel[1 + i] != wh->d_vGrav[i]) {
341 newgrav = true;
342 break;
343 }
344 }
345
346 if (newgrav) {
347 if (!wh->d_gravDirty) {
348 // only slide back the previous gravity if we actually used it once.
349 q_vec_copy(wh->d_vGravAntepenultimate, wh->d_vGravPenultimate);
350 q_vec_copy(wh->d_vGravPenultimate, wh->d_vGrav);
351 }
352 for (i = 0; i < 3; i++) {
353 wh->d_vGrav[i] = info.channel[1 + i];
354 }
355 wh->d_gravDirty = true;
356 }
357
358 // Store the time of the report into the tracker's timestamp field.
359 wh->vrpn_Tracker::timestamp = info.msg_time;
360}
361
362// static
365
366 // Indicate that we should send a report with whatever we have.
367 wh->d_updated = true;
368
369 // Always return 0 here, because nonzero return means that the input data
370 // was garbage, not that there was an error. If we return nonzero from a
371 // vrpn_Connection handler, it shuts down the connection.
372 return 0;
373}
374
376 // Moving average of last three gravity vectors
378 q_vec_type movingAvg = Q_NULL_VECTOR;
379
380 q_vec_copy(movingAvg, d_vGrav);
381 q_vec_add(movingAvg, movingAvg, d_vGravPenultimate);
382 q_vec_add(movingAvg, movingAvg, d_vGravAntepenultimate);
383 q_vec_scale(movingAvg, 0.33333, movingAvg);
384
385 // reset gravity transform
388
389 q_vec_type regulargravity = Q_NULL_VECTOR;
390 regulargravity[2] = 1;
391
392 q_from_two_vecs(d_gravityXform.quat, movingAvg, regulargravity);
393 d_gravDirty = false;
394}
395
396void vrpn_Tracker_WiimoteHead::_update_2_LED_pose(q_xyz_quat_type & newPose) {
397 if (d_points != 2) {
398 // we simply stop updating our pos+orientation if we lost LED's
399 // TODO: right now if we don't have exactly 2 points we lose the lock
400 d_lock = false;
402 return;
403 }
404
405 // TODO right now only handling the 2-LED glasses
406
407 d_lock = true;
408 double rx, ry, rz;
409 rx = ry = rz = 0;
410
411 double X0, X1, Y0, Y1;
412
413 X0 = d_vX[0];
414 X1 = d_vX[1];
415 Y0 = d_vY[0];
416 Y1 = d_vY[1];
417
418 if (d_flipState == FLIP_180) {
425 std::swap(X0, X1);
426 std::swap(Y0, Y1);
427 }
428
429 const double dx = X0 - X1;
430 const double dy = Y0 - Y1;
431 const double dist = sqrt(dx * dx + dy * dy);
432 const double angle = dist * cvtDistToAngle;
433 // Note that this is an approximation, since we don't know the
434 // distance/horizontal position. (I think...)
435
436 const double headDist = (d_blobDistance / 2.0) / tan(angle);
437
438 // Translate the distance along z axis, and tilt the head
439 newPose.xyz[2] = headDist; // translate along Z
440 rz = atan2(dy, dx); // rotate around Z
441
442 // Find the sensor pixel of the line of sight - directly between
443 // the LEDs
444 const double avgX = (X0 + X1) / 2.0;
445 const double avgY = (Y0 + Y1) / 2.0;
446
451 if (wm_isnan(avgX)) {
452 std::cerr << "NaN detected in avgX: X0 = " << X0 << ", X1 = " << X1 << std::endl;
453 return;
454 }
455
456 if (wm_isnan(avgY)) {
457 std::cerr << "NaN detected in avgY: Y0 = " << Y0 << ", Y1 = " << Y1 << std::endl;
458 return;
459 }
460
461 // b is the virtual depth in the sensor from a point to the full sensor
462 // used for finding similar triangles to calculate x/y translation
463 const double bHoriz = xResSensor / 2 / tan(fovX / 2);
464 const double bVert = yResSensor / 2 / tan(fovY / 2);
465
466 // World head displacement (X and Y) from a centered origin at
467 // the calculated distance from the sensor
468 newPose.xyz[0] = headDist * (avgX - xResSensor / 2) / bHoriz;
469 newPose.xyz[1] = headDist * (avgY - yResSensor / 2) / bVert;
470
471 // set the quat. part of our pose with rotation angles
472 q_from_euler(newPose.quat, rz, ry, rx);
473
474 // Apply the new pose
475 q_vec_copy(d_currentPose.xyz, newPose.xyz);
476 q_copy(d_currentPose.quat, newPose.quat);
477}
478
480 if (d_flipState != FLIP_UNKNOWN) {
481 return;
482 }
483
484 q_vec_type upVec = {0, 1, 0};
485
486 q_xform(upVec, d_currentPose.quat, upVec);
487 if (upVec[1] < 0) {
488 // We are upside down - we will need to rotate 180 about the sensor Z
489 // Must recalculate now.
490#ifdef VERBOSE
491 fprintf(stderr, "vrpn_Tracker_WiimoteHead: d_flipState = FLIP_180\n");
492#endif
494 update_pose();
495 } else {
496 // OK, we are fine - there is a positive Y component to our up vector
497#ifdef VERBOSE
498 fprintf(stderr, "vrpn_Tracker_WiimoteHead: d_flipState = FLIP_NORMAL\n");
499#endif
501 }
502}
503
505 q_vec_copy(pos, d_currentPose.xyz); // set position;
506 q_copy(d_quat, d_currentPose.quat); // set orientation
507}
508
509
513
517
518 // Default earth gravity is (0, 1, 0)
520
521 d_gravDirty = true;
522}
523
525 d_vX[0] = d_vX[1] = d_vX[2] = d_vX[3] = -1;
526 d_vY[0] = d_vY[1] = d_vY[2] = d_vY[3] = -1;
527 d_vSize[0] = d_vSize[1] = d_vSize[2] = d_vSize[3] = -1;
528 d_points = 0;
529
530
532 d_lock = false;
533}
534
536 // Reset to the identity pose
539
541
542 // Set the updated flag to send a report
543 d_updated = true;
545 d_lock = false;
546
547 // Convert the matrix into quaternion notation and copy into the
548 // tracker pos and quat elements.
550}
551
552bool vrpn_Tracker_WiimoteHead::_should_report(double elapsedInterval) const {
553 // If we've gotten new wiimote reports since our last report, return true.
554 if (d_updated) {
555 return true;
556 }
557
558 // If it's been more than our max interval, send an update anyway
559 if (elapsedInterval >= d_update_interval) {
560 return true;
561 }
562
563 // Not time has elapsed, and nothing has changed, so return false.
564 return false;
565}
566
568 return (d_vGrav[0] != 0 || d_vGrav[1] != 1 || d_vGrav[2] != 0);
569}
virtual int unregister_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
Definition: vrpn_Analog.h:197
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_Analog.C:327
virtual int register_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
Definition: vrpn_Analog.h:192
int register_autodeleted_handler(vrpn_int32 type, vrpn_MESSAGEHANDLER handler, void *userdata, vrpn_int32 sender=vrpn_ANY_SENDER)
Registers a handler with the connection, and remembers to delete at destruction.
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...
Generic connection class not specific to the transport mechanism.
virtual vrpn_int32 register_message_type(const char *name)
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...
Provides a tracker device given data from a Wii Remote and LED glasses.
void _reset_points()
reset cached points, point count, and flip state,
static void VRPN_CALLBACK VRPN_CALLBACK handle_analog_update(void *userdata, const vrpn_ANALOGCB info)
Callback triggered when our data source issues an update.
void _update_gravity_moving_avg()
based on cached gravity data, use a moving average to update the tracker's stored gravity transform.
virtual ~vrpn_Tracker_WiimoteHead()
destructor
q_xyz_quat_type d_currentPose
Current pose estimate.
void report()
Pack and send tracker report.
vrpn_Analog_Remote * d_ana
Source of analog data, traditionally vrpn_WiiMote Must present analog channels in this order:
struct timeval d_prevtime
Time of last tracker report issued.
void _update_2_LED_pose(q_xyz_quat_type &newPose)
Create tracker-relative pose estimate based on sensor location of 2 tracked points.
FlipState d_flipState
Whether we need to flip the order of the tracked points before calculating a pose.
const double d_blobDistance
distance between LEDs on glasses, in meters
vrpn_Tracker_WiimoteHead(const char *name, vrpn_Connection *trackercon, const char *wiimote, float update_rate, float led_spacing=0.145)
constructor
q_xyz_quat_type d_gravityXform
Gravity correction transformation.
bool d_lock
Flag: Does the tracking algorithm report a lock?
virtual void mainloop()
VRPN mainloop function.
void _reset_gravity()
reset gravity transform and cached gravity vectors
void _update_flip_state()
If flip state is unknown, set flip state appropriately.
bool _should_report(double elapsedInterval) const
return true if we have new data or max time elapsed
static int VRPN_CALLBACK VRPN_CALLBACK handle_connection(void *, vrpn_HANDLERPARAM)
Callback triggered when a new client connects to the tracker.
bool d_updated
Flag: Have we received updated Wiimote data since last report?
bool d_gravDirty
Flag: Have we received updated gravity data since last gravity update?
virtual void reset()
reset pose, gravity transform, and cached points and gravity
bool d_contact
Flag: Have we received the first message from the Wiimote?
const char * d_name
Tracker device name.
bool _have_gravity() const
return true if our gravity values look like real data
void _reset_pose()
reset current pose, last report time, and tracker pose
const double d_update_interval
maximum time between updates, in seconds
void update_pose()
function to drive the full pose update process
void setup_wiimote()
set up connection to wiimote-like analog device
void _convert_pose_to_tracker()
Set the vrpn_Tracker position and rotation to that indicated by our d_currentPose;.
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:552
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80
struct timeval msg_time
Definition: vrpn_Analog.h:169
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:171
This structure is what is passed to a vrpn_Connection message callback.
const char * vrpn_got_connection
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
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:144
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:99
const double yResSensor
const double radPerPx
const double two
const double fovX
Field of view experimentally determined at Iowa State University March 2010.
#define MAKE_IDENTITY_QUAT(dest)
Utility function to set a quat equal to the identity rotation.
const double fovY
const double cvtDistToAngle
const double xResSensor
#define MAKE_NULL_VEC(dest)
Utility function to set a 3-vector equal to the zero vector.
vrpn_Tracker interface provided by processing Wii Remote data for head tracking.