Exemple #1
0
/** 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();
	}
}