/** 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;
}
exitCode QueryRegistry::registerQuery(Querible * querible, t_pQdesc queryDescriptor) {
t_queryRegistry::iterator queryIt;
std::string const & query = (queryDescriptor->name);
LOG4CPP_DEBUG(log, "QueryRegistry::registerQuery(query=%s, querible=%p)", query.c_str(), querible);
if ( !querible ) {
LOG4CPP_ERROR(log, "Unable to register a query [%s] without an associated querible", query.c_str());
return QR_MISSING_QUERIBLE;
}
// Avoid query name duplication
queryIt = d_queryRegistry.find(query);
if ( (queryIt != d_queryRegistry.end()) ) {
LOG4CPP_WARN(log, "Query already registered [%s]", query.c_str());
return QR_QUERY_DUPLICATE;
}
d_queryRegistry.insert(t_queryRegistryEntry(query, querible));
d_queribleRegistry.insert(t_queribleRegistryEntry(querible, queryDescriptor));
LOG4CPP_INFO(log, "Registered new query [%s] exported by Querible [%s]", query.c_str(), querible->name().c_str());
return OK;
}
void SimpleFacade::sendUtqlToServerString( const char* buffer ) throw()
{
try
{
m_pPrivate->sendUtqlToServerString( buffer );
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception in SimpleFacade::sendUtqlToServerString: " << e );
setError( e.what() );
}
}
void SimpleFacade::connectToServer( const char* sAddress ) throw()
{
try
{
m_pPrivate->connectToServer( sAddress );
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception in SimpleFacade::connectToServer: " << e );
setError( e.what() );
}
}
void SimpleFacade::stopDataflow() throw()
{
try
{
// stop the event queue
m_pPrivate->stopDataflow();
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception in SimpleFacade::stopDataflow: " << e );
setError( e.what() );
}
}
SimplePoseReceiver* SimpleFacade::getPushSourcePose( const char* sComponentName ) throw()
{
try
{
return m_pPrivate->componentByName< Components::ApplicationPushSourcePose >( sComponentName ).get();
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception in SimpleFacade::getPushSourcePose( " << sComponentName <<" ): " << e );
setError( e.what() );
return 0;
}
}
bool SimpleFacade::loadDataflow( const char* sDfSrg ) throw()
{
try
{
m_pPrivate->loadDataflow( sDfSrg );
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception in SimpleFacade::loadDataflow: " << e );
setError( e.what() );
return false;
}
return true;
}
SimpleFacade::SimpleFacade( const char* sComponentPath ) throw()
: m_pPrivate( 0 )
, m_sError( 0 )
{
try
{
m_pPrivate = new SimpleFacadePrivate( sComponentPath );
LOG4CPP_TRACE( logger, "SimpleFacadePrivate created successfully" );
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception constructing SimpleFacade: " << e );
setError( e.what() );
}
}
bool SimpleFacade::setStringCallback( const char* sCallbackName, SimpleStringReceiver* pCallback ) throw()
{
try
{
m_pPrivate->componentByName< Components::ApplicationPushSinkBase >( sCallbackName )->setStringCallback( pCallback );
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception in SimpleFacade::setStringCallback( " << sCallbackName <<" ): " << e );
setError( e.what() );
return false;
}
return true;
}
bool SimpleFacade::set3DErrorPositionListCallback( const char* sCallbackName, SimpleErrorPositionList3DReceiver* pCallback ) throw()
{
try
{
m_pPrivate->setCallback< Measurement::ErrorPositionList >( sCallbackName, boost::bind( &convert3DErrorPositionListCallback, pCallback, _1 ) );
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception in SimpleFacade::set3DErrorPositionListCallback( " << sCallbackName <<" ): " << e );
setError( e.what() );
return false;
}
return true;
}
SimplePositionList3DReceiver* SimpleFacade::getPushSourcePositionList3D( const char* sComponentName ) throw()
{
try
{
SimplePositionList3DReceiver * pr = m_pPrivate->componentByName< SimplePositionList3DReceiver >( sComponentName ).get();
LOG4CPP_DEBUG( logger, "Successfully retrieved component " << sComponentName << " in SimpleFacade::getPushSourcePositionList3D " );
return pr;
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception in SimpleFacade::getPushSourcePositionList3D( " << sComponentName <<" ): " << e );
setError( e.what() );
return 0;
}
}
void Undistortion::initParams( const std::string& intrinsicMatrixFile, const std::string& distortionFile )
{
// read intrinsics
if ( !intrinsicMatrixFile.empty() )
{
Measurement::Matrix3x3 measMat;
measMat.reset( new Math::Matrix< double, 3, 3 >() );
Util::readCalibFile( intrinsicMatrixFile, measMat );
m_intrinsics = *measMat;
LOG4CPP_DEBUG(logger, "Loaded calibration file : " << m_intrinsics);
}
else
{
m_intrinsics( 0, 0 ) = 400;
m_intrinsics( 0, 1 ) = 0;
m_intrinsics( 0, 2 ) = -160;
m_intrinsics( 1, 0 ) = 0;
m_intrinsics( 1, 1 ) = 400;
m_intrinsics( 1, 2 ) = -120;
m_intrinsics( 2, 0 ) = 0;
m_intrinsics( 2, 1 ) = 0;
m_intrinsics( 2, 2 ) = -1;
}
// read coefficients
if ( !distortionFile.empty() )
{
// first try new, eight element distortion file
Measurement::Vector8D measVec;
measVec.reset( new Math::Vector< double, 8 >() );
try {
Util::readCalibFile( distortionFile, measVec );
m_coeffs = *measVec;
} catch ( Ubitrack::Util::Exception ) {
LOG4CPP_ERROR( logger, "Cannot read new image distortion model. Trying old format." );
// try old format, this time without exception handling
Measurement::Vector4D measVec4D;
measVec4D.reset( new Math::Vector< double, 4 >() );
Util::readCalibFile( distortionFile, measVec4D );
m_coeffs = Math::Vector< double, 8 >::zeros();
boost::numeric::ublas::subrange(m_coeffs, 0, 4 ) = *measVec4D;
}
}
else
m_coeffs = Math::Vector< double, 8 >::zeros();
}
exitCode CommandDispatcher::dispatch(bool clean)
throw (exceptions::InitializationException,
exceptions::OutOfMemoryException) {
LOG4CPP_DEBUG(log, "CommandDispatcher::dispatch()");
if ( !d_command ) {
LOG4CPP_ERROR(log, "Unable to dispatch Default command: NOT yet defined!");
return CS_DISPATCH_FAILURE;
}
LOG4CPP_INFO(log, "Default command disaptching");
dispatch(d_command, clean);
return OK;
}
bool SimpleFacade::loadDataflowString( const char* sDataflow ) throw()
{
try
{
std::istringstream ss( sDataflow );
m_pPrivate->loadDataflow( ss );
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception in SimpleFacade::loadDataflowString: " << e );
setError( e.what() );
return false;
}
return true;
}
SimplePosition3DReceiver* SimpleFacade::getPushSourcePosition3D( const char* sComponentName ) throw()
{
try
{
LOG4CPP_INFO( logger, "Trying to get SimpleFacade::getPushSourcePosition3D for "<<sComponentName);
SimplePosition3DReceiver* result = m_pPrivate->componentByName< Components::ApplicationPushSourcePosition >( sComponentName ).get();
LOG4CPP_INFO( logger, "Getting SimpleFacade::getPushSourcePosition3D "<<result);
return result;
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception in SimpleFacade::getPushSourcePosition3D( " << sComponentName <<" ): " << e );
setError( e.what() );
return 0;
}
}
bool SimpleFacade::setPosition2DCallback( const char* sCallbackName, SimplePosition2DReceiver* pCallback ) throw()
{
try
{
//m_pPrivate->componentByName< Components::ApplicationPushSinkBase >( sCallbackName )->setPosition2DCallback( pCallback );
m_pPrivate->setCallback< Measurement::Position2D >( sCallbackName, boost::bind( &convertPosition2DCallback, pCallback, _1 ) );
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception in SimpleFacade::setPosition2DCallback( " << sCallbackName <<" ): " << e );
setError( e.what() );
return false;
}
return true;
}
void Transparency::draw( Measurement::Timestamp& t, int parity )
{
//added win32-guard by CW (16.7.2012): gl-functions seem to be unavailable on windows systems
#ifndef WIN32
#ifdef HAVE_GLEW
LOG4CPP_DEBUG( logger, "Transparency::draw(), set alpha to " << alpha << " for timestamp " << t );
// Enable global transparency for the virtual scene. This affects
// all render components except the BackgroundVideo component.
glBlendColor ( 0.0, 0.0, 0.0, alpha );
glBlendFunc ( GL_CONSTANT_ALPHA, GL_ONE_MINUS_CONSTANT_ALPHA );
#else
LOG4CPP_ERROR( logger, "Transparency::draw() has no effect since 'glew' is not installed" );
#endif
#endif
}
void ImageFileRecorder::saveImage( const ImageMeasurement &img )
{
char fileName[256];
LOG4CPP_DEBUG( logger, "saveImage(): ");
std::ostream os( &filebuffer );
std::sprintf ( fileName, "%s%.5u%s", m_prefix.c_str(), m_counter++, m_suffix.c_str() );
boost::filesystem::path path = m_imgDir;
path /= fileName;
LOG4CPP_DEBUG( logger, "saveImage(): Saving image to file " << path );
if( cvSaveImage( path.string().c_str(), *img ) == 0 )
LOG4CPP_ERROR( logger, "Error saving image " << path );
os << (unsigned long long)( img.time() / 1000000.) << " " << path.filename() << std::endl;
}
SimpleApplicationPullSinkErrorPosition3D * SimpleFacade::getPullSinkErrorPosition3D( const char* sComponentName ) throw()
{
Ubitrack::Components::ApplicationPullSinkErrorPosition * pSink;
try
{
pSink = m_pPrivate->componentByName< Components::ApplicationPullSinkErrorPosition >( sComponentName ).get();
}
catch ( const Ubitrack::Util::Exception& e )
{
LOG4CPP_ERROR( logger, "Caught exception in SimpleFacade::getPullSinkErrorPosition3D( " << sComponentName <<" ): " << e );
setError( e.what() );
return 0;
}
// Create proxy object and return it
SimpleApplicationPullSinkErrorPosition3D * pSinkPriv = new SimpleApplicationPullSinkErrorPosition3DPrivate( pSink );
return pSinkPriv;
}
void triggerIn( const Measurement::Button& e )
{
if ( *e == m_inButton )
{
// lock the file to prevent other threads from writing simultaneously
boost::mutex::scoped_lock l( m_mutex );
if ( m_bStopped )
{
LOG4CPP_DEBUG( logger, getName() << " Received trigger event with timestamp " << e.time() << ". Invoke computation by sending first sync signal..." );
m_outPortSync.send( Measurement::Button( e.time(), m_button ) );
m_bStopped = false;
// Reset internal state
m_counter = 0;
mean = Math::Vector< double, 7 >::zeros();
outProd = Math::Matrix< double, 7, 7 >::zeros();
}
else
{
LOG4CPP_ERROR( logger, getName() << " received trigger signal while computation was already running. Ignored." );
}
}
}
FileWriterCommandHandler::FileWriterCommandHandler(const std::string & fileName, std::string const & logName, bool append) :
comsys::CommandHandler(logName),
Device(Device::CH_FILEWRITER, fileName, logName),
d_cnotify(0),
log(Device::log) {
LOG4CPP_DEBUG(log, "FileWriterCommandHandler(const std::string &, bool)");
// Creating a simple plain layout for maximun output string control
d_fwlayout = new log4cpp::PatternLayout();
try {
d_fwlayout->setConversionPattern(std::string("%m"));
} catch (log4cpp::ConfigureFailure cf) {
LOG4CPP_FATAL(log, "Invalid conversion pattern");
}
// Opening the file for Commands dumped
d_fwappender = new log4cpp::FileAppender ("FileWriterXMLDumper", fileName, append);
d_fwappender->setLayout (d_fwlayout);
// Initializing a new Category for the FileWriter
d_fwcategory = &log.getInstance("FileWriterXMLDumper");
try {
d_fwcategory->setPriority (log4cpp::Priority::INFO);
// Avoiding msg duplication on root category
d_fwcategory->setAdditivity(false);
} catch (std::invalid_argument) {
LOG4CPP_ERROR(log, "Invalid priority level");
}
d_fwcategory->removeAllAppenders ();
d_fwcategory->addAppender(*d_fwappender);
// Registering device into the DeviceDB
dbReg();
}
/*
* Update Texture - requires valid OpenGL Context
*/
void TextureUpdate::updateTexture(const Measurement::ImageMeasurement& image) {
#ifdef HAVE_OPENCV
// access OCL Manager and initialize if needed
Vision::OpenCLManager& oclManager = Vision::OpenCLManager::singleton();
if (!image) {
// LOG4CPP_WARN ??
return;
}
// if OpenCL is enabled and image is on GPU, then use OCL codepath
bool image_isOnGPU = oclManager.isInitialized() & image->isOnGPU();
if ( m_bTextureInitialized )
{
// check if received image fits into the allocated texture
// find out texture format
int umatConvertCode = -1;
GLenum glFormat = GL_LUMINANCE;
GLenum glDatatype = GL_UNSIGNED_BYTE;
int numOfChannels = 1;
Image::ImageFormatProperties fmtSrc, fmtDst;
image->getFormatProperties(fmtSrc);
image->getFormatProperties(fmtDst);
getImageFormat(fmtSrc, fmtDst, image_isOnGPU, umatConvertCode, glFormat, glDatatype);
if (image_isOnGPU) {
#ifdef HAVE_OPENCL
glBindTexture( GL_TEXTURE_2D, m_texture );
// @todo this probably causes unwanted delay - .. except when executed on gpu ...
if (umatConvertCode != -1) {
cv::cvtColor(image->uMat(), m_convertedImage, umatConvertCode );
} else {
m_convertedImage = image->uMat();
}
cv::ocl::finish();
glFinish();
cl_command_queue commandQueue = oclManager.getCommandQueue();
cl_int err;
clFinish(commandQueue);
err = clEnqueueAcquireGLObjects(commandQueue, 1, &(m_clImage), 0, NULL, NULL);
if(err != CL_SUCCESS)
{
LOG4CPP_ERROR( logger, "error at clEnqueueAcquireGLObjects:" << getOpenCLErrorString(err) );
}
cl_mem clBuffer = (cl_mem) m_convertedImage.handle(cv::ACCESS_READ);
cl_command_queue cv_ocl_queue = (cl_command_queue)cv::ocl::Queue::getDefault().ptr();
size_t offset = 0;
size_t dst_origin[3] = {0, 0, 0};
size_t region[3] = {static_cast<size_t>(m_convertedImage.cols), static_cast<size_t>(m_convertedImage.rows), 1};
err = clEnqueueCopyBufferToImage(cv_ocl_queue, clBuffer, m_clImage, offset, dst_origin, region, 0, NULL, NULL);
if (err != CL_SUCCESS)
{
LOG4CPP_ERROR( logger, "error at clEnqueueCopyBufferToImage:" << getOpenCLErrorString(err) );
}
err = clEnqueueReleaseGLObjects(commandQueue, 1, &m_clImage, 0, NULL, NULL);
if(err != CL_SUCCESS)
{
LOG4CPP_ERROR( logger, "error at clEnqueueReleaseGLObjects:" << getOpenCLErrorString(err) );
}
cv::ocl::finish();
#else // HAVE_OPENCL
LOG4CPP_ERROR( logger, "Image isOnGPU but OpenCL is disabled!!");
#endif // HAVE_OPENCL
} else {
// load image from CPU buffer into texture
glBindTexture( GL_TEXTURE_2D, m_texture );
glTexSubImage2D( GL_TEXTURE_2D, 0, 0, 0, image->width(), image->height(),
glFormat, glDatatype, image->Mat().data );
}
}
#endif // HAVE_OPENCV
}
void TextureUpdate::initializeTexture(const Measurement::ImageMeasurement& image, const GLuint tex_id) {
#ifdef HAVE_OPENCV
if (!image) {
return;
}
if ( !m_bTextureInitialized )
{
// access OCL Manager and initialize if needed
Vision::OpenCLManager& oclManager = Vision::OpenCLManager::singleton();
// if OpenCL is enabled and image is on GPU, then use OCL codepath
bool image_isOnGPU = oclManager.isEnabled() & image->isOnGPU();
// find out texture format
int umatConvertCode = -1;
GLenum glFormat = GL_LUMINANCE;
GLenum glDatatype = GL_UNSIGNED_BYTE;
int numOfChannels = 1;
Image::ImageFormatProperties fmtSrc, fmtDst;
image->getFormatProperties(fmtSrc);
image->getFormatProperties(fmtDst);
getImageFormat(fmtSrc, fmtDst, image_isOnGPU, umatConvertCode, glFormat, glDatatype);
// generate power-of-two sizes
m_pow2Width = 1;
while ( m_pow2Width < (unsigned)image->width() )
m_pow2Width <<= 1;
m_pow2Height = 1;
while ( m_pow2Height < (unsigned)image->height() )
m_pow2Height <<= 1;
m_texture = tex_id;
glBindTexture( GL_TEXTURE_2D, m_texture );
// define texture parameters
glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR );
glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL );
// load empty texture image (defines texture size)
glTexImage2D( GL_TEXTURE_2D, 0, fmtDst.channels, m_pow2Width, m_pow2Height, 0, glFormat, glDatatype, 0 );
LOG4CPP_DEBUG( logger, "glTexImage2D( width=" << m_pow2Width << ", height=" << m_pow2Height << " ): " << glGetError() );
LOG4CPP_INFO( logger, "initalized texture ( " << glFormat << " ) OnGPU: " << image_isOnGPU);
if (oclManager.isInitialized()) {
#ifdef HAVE_OPENCL
//Get an image Object from the OpenGL texture
cl_int err;
// windows specific or opencl version specific ??
#ifdef WIN32
m_clImage = clCreateFromGLTexture2D( oclManager.getContext(), CL_MEM_WRITE_ONLY, GL_TEXTURE_2D, 0, m_texture, &err);
#else
m_clImage = clCreateFromGLTexture( oclManager.getContext(), CL_MEM_WRITE_ONLY, GL_TEXTURE_2D, 0, m_texture, &err);
#endif
if (err != CL_SUCCESS)
{
LOG4CPP_ERROR( logger, "error at clCreateFromGLTexture2D:" << getOpenCLErrorString(err) );
}
#endif
}
m_bTextureInitialized = true;
}
#endif // HAVE_OPENCV
}
