/** 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();
}
}