/** retrieves a pose for the given timestamp */ bool SimpleApplicationPullSinkPosePrivate::getPose( SimplePose & pose, unsigned long long int timestamp ) { if ( pSink == NULL ) return false; try { LOG4CPP_INFO( logger, "Trying to pull pose in SimpleApplicationPullSinkPosePrivate::getPose() for timestamp: " << timestamp ); // Retrieve measurement for current timestamp Ubitrack::Measurement::Pose p = pSink->get(timestamp); LOG4CPP_INFO( logger, "Sucessfully pulled pose in SimpleApplicationPullSinkPosePrivate::getPose(): " << p ); // Convert measurement pose.tx = p->translation()( 0 ); pose.ty = p->translation()( 1 ); pose.tz = p->translation()( 2 ); pose.rx = p->rotation().x(); pose.ry = p->rotation().y(); pose.rz = p->rotation().z(); pose.rw = p->rotation().w(); pose.timestamp = p.time(); } catch ( const Ubitrack::Util::Exception& e ) { LOG4CPP_ERROR( logger, "Caught exception in SimpleApplicationPullSinkPosePrivate::getPose(): " << e ); setError( e.what() ); return false; } return true; }
/** * callback from Pose port * @param pose current transformation */ void CameraPose::poseIn( const Ubitrack::Measurement::Pose& pose, int redraw ) { m_lastUpdateTime = pose.time(); Ubitrack::Math::Pose invpose = ~(*pose); Ubitrack::Math::Matrix< double, 4, 4 > m( invpose.rotation(), invpose.translation() ); double* tmp = m.content(); boost::mutex::scoped_lock l( m_poseLock ); for ( int i = 0; i < 16; i++ ) m_pose[i] = tmp[i]; // the camera pose has changed, so redraw the world if (redraw) { LOG4CPP_DEBUG( logger, "CameraPose: calling invalidate()" ); m_pModule->invalidate(); } }