/** 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 CommandDispatcher::flushQueue(bool discard) {
LOG4CPP_DEBUG(log, "CommandDispatcher::flushQueue(bool discard=%d)", discard);
if ( discard ) {
LOG4CPP_WARN(log, "Flushing Command Queue: Discarding all commands");
while ( !d_queuedCommands.empty() && !d_suspended ) {
d_queuedCommands.pop();
}
} else {
LOG4CPP_INFO(log, "Flushing Command Queue: Notifying all commands");
while ( !d_queuedCommands.empty() && !d_suspended ) {
d_handler->notify( d_queuedCommands.front() );
d_queuedCommands.pop();
}
}
return OK;
} //namespace comsys
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;
}
}
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;
}
FileWriterCommandHandler::~FileWriterCommandHandler() {
LOG4CPP_INFO(log, "Stopping FileWriterCommandHandler: no more Commands will by dumped");
// Closing the logfile
d_fwappender->close();
}
PollEventGenerator::~PollEventGenerator() {
LOG4CPP_INFO(log, "Terminating the PollEventGenerator events generation thread... ");
// Thread::terminate() should always be called at the start of any
// destructor of a class derived from Thread to assure the remaining
// part of the destructor is called without the thread still executing.
this->terminate();
threadStopNotify();
}
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;
}
void compute( Measurement::Timestamp t )
{
typename MatType::value_type mat = *(m_inPortA.get());
typename VecType::value_type vec = *(m_inPortB.get());
typename ResType::value_type res;
try {
res = mat * vec;
}
catch (void*) {
LOG4CPP_INFO( logger, "division by zero during dehomogenization" );
return;
}
m_outPort.send( ResType ( t, res ) );
}
void Undistortion::initMap( int width, int height, int origin )
{
// skip if already initialized with same values
if ( m_pMapX && m_pMapX->width == width && m_pMapX->height == height )
return;
LOG4CPP_INFO( logger, "Creating undistortion map" );
LOG4CPP_DEBUG( logger, "coeffs=" << m_coeffs );
LOG4CPP_DEBUG( logger, "intrinsic=" << m_intrinsics );
// copy ublas to OpenCV parameters
CvMat* pCvCoeffs = cvCreateMat( 1, 8, CV_32FC1 );
for ( unsigned i = 0; i < 8; i++ )
pCvCoeffs->data.fl[ i ] = static_cast< float >( m_coeffs( i ) );
CvMat* pCvIntrinsics = cvCreateMat( 3, 3, CV_32FC1 );
for ( unsigned i = 0; i < 3; i++ )
for ( unsigned j = 0; j < 3; j++ )
cvmSet( pCvIntrinsics, i, j, m_intrinsics( i, j ) );
// compensate for left-handed OpenCV coordinate frame
for ( unsigned i = 0; i < 3; i++ )
cvmSet( pCvIntrinsics, i, 2, cvmGet( pCvIntrinsics, i, 2 ) * -1 );
// compensate if origin==0
if ( !origin )
{
cvmSet( pCvIntrinsics, 1, 2, height - 1 - cvmGet( pCvIntrinsics, 1, 2 ) );
cvmSet( pCvCoeffs, 0, 2, cvmGet( pCvCoeffs, 0, 2 ) * -1 );
}
// initialize the distortion map
// create map images
m_pMapX.reset( new Image( width, height, 1, IPL_DEPTH_32F ) );
m_pMapY.reset( new Image( width, height, 1, IPL_DEPTH_32F ) );
cvInitUndistortMap( pCvIntrinsics, pCvCoeffs, *m_pMapX, *m_pMapY );
LOG4CPP_TRACE( logger, "origin=" << origin );
Math::Vector< double, 2 > startPixel( *reinterpret_cast< float* >( m_pMapX->imageData ), *reinterpret_cast< float* >( m_pMapY->imageData ) );
LOG4CPP_DEBUG( logger, "first pixel (0, 0) mapped from " << startPixel );
// release data
cvReleaseMat( &pCvCoeffs );
cvReleaseMat( &pCvIntrinsics );
}
exitCode QueryRegistry::unregisterQuery(t_queryName const & query, bool release) {
t_queryRegistry::iterator queryIt;
t_queribleRegistry::iterator queribleIt;
t_queribleRegistry::iterator firstDescr;
t_pQdesc pDesc;
LOG4CPP_DEBUG(log, "QueryRegistry::unregisterQuery(query=%s, release=%s)", query.c_str(), release ? "YES" : "NO" );
queryIt = d_queryRegistry.find(query);
// If the query was NOT registered
if ( (queryIt == d_queryRegistry.end())) {
LOG4CPP_WARN(log, "Trying to remove a query [%s] NOT registered", query.c_str());
return QR_QUERY_NOT_EXIST;
}
// looking for a query descriptor
queribleIt = firstDescr = d_queribleRegistry.find(queryIt->second);
while ( queribleIt != d_queribleRegistry.end() ) {
if ( queribleIt->first != firstDescr->first) {
queribleIt = d_queribleRegistry.end();
break;
}
pDesc = queribleIt->second;
if ( pDesc->name == query ) {
break;
}
queribleIt++;
}
// Eventually releasing the query desciption
if (queribleIt != d_queribleRegistry.end()) {
if (release) {
delete (queribleIt->second);
}
d_queribleRegistry.erase(queribleIt);
}
d_queryRegistry.erase(queryIt);
LOG4CPP_INFO(log, "Unregistered query [%s] exported by Querible [%s]", query.c_str(), (queryIt->second)->name().c_str());
return OK;
}
exitCode CommandDispatcher::dispatch(Command * command, bool clean)
throw (exceptions::OutOfMemoryException) {
LOG4CPP_DEBUG(log, "CommandDispatcher::dispatch(Command * command)");
// If disabled...
if ( d_suspended ) {
// Queuing command for handler notify
LOG4CPP_INFO(log, "Disaptcher suspended; queuing new Command for delayed dispatching");
queueCommand(command);
return DIS_SUSPENDED;
}
// Dispatching command immediatly
d_handler->notify(command);
if ( clean ) {
delete command;
}
return OK;
}
Math::ErrorPose CovarianceEstimation< Measurement::Pose, Measurement::ErrorPose >::incrementalEstimate( Math::Pose& poseNew )
{
ublas::vector_range< typename Math::Vector< double >::base_type > posMean( mean, ublas::range( 0, 3 ) );
ublas::vector_range< typename Math::Vector< double >::base_type > rotMean( mean, ublas::range( 3, 7 ) );
LOG4CPP_TRACE ( logger, "Update pose event: " << poseNew );
// The order is tx, ty, tz, qx, qy, qz, qw.
Math::Vector< double > poseNewVec( 7 );
poseNew.toVector( poseNewVec );
ublas::vector_range< typename Math::Vector< double >::base_type > posNew( poseNewVec, ublas::range( 0, 3 ) );
ublas::vector_range< typename Math::Vector< double >::base_type > rotNew( poseNewVec, ublas::range( 3, 7 ) );
// Take care of quaternion ambiguity
if ( ublas::inner_prod( rotNew, rotMean ) < 0 )
rotNew *= -1;
// Update running mean value
mean = ( ( ((double)m_counter - 1) / (double)m_counter ) * mean ) + ( ( 1 / (double)m_counter ) * poseNewVec );
// Running outer product of pose random variable (not yet normalized by number of measurements)
outProd = outProd + ublas::outer_prod( poseNewVec, poseNewVec );
/*
* Use inverted mean value to transform the additive 7x7
* covariance to the 6x6 multiplicative format The conversion is
* conducted according to the following formulas:
*
* q_m = q_0 * ( q_id + q_e )
*
* where q_id is the identity quaternion and q_e is a quaternion
* with expectation ((0,0,0),0) and a covariance covering only the
* imaginary part. Together ( q_id + q_e ) represent a small
* quaternion ((e_rx, e_ry, e_rz), 1). If mean and covariance of
* the quaternion are estimated according to the usual formulas,
* however, one gets the following instead:
*
* q_m = q_0 + q'_e
*
* Together with the first formula, this yields
*
* q_0 * ( q_id + q_e ) = q_0 + q'_e
* ( q_id + q_e ) = ~q_0 * q_0 + ~q_0 * q'_e
* q_e = q_id + ~q_0 * q'_e - q_id
* q_e = ~q_0 * q'_e
*
* Thus, one has to rotate the distribution by ~q_0. The variance
* of the real part can then be discarded, it should be ~0.
*/
Math::Vector< double, 7 > invMean;
(~(Math::Pose::fromVector( mean ) ) ).toVector( invMean );
Math::ErrorVector< double, 7 > ev ( invMean, outProd / ( (double)m_counter ) - ublas::outer_prod ( mean, mean ) );
Math::ErrorPose invEp = Math::ErrorPose::fromAdditiveErrorVector( ev );
// We created the error pose from the inverted mean value above, to obtain the transformed 6x6 covariance
// Now, we recreate the error pose with the computed mean value.
Math::ErrorPose ep( Math::Pose::fromVector( mean ), invEp.covariance() );
LOG4CPP_TRACE( logger, "Running (empirical) mean / covariance: " << std::endl << ep );
// For debug purposes, compute positional and angular error...
Math::Matrix< double, 6, 6 > covar = ep.covariance();
double posRms = sqrt ( covar (0,0) + covar (1,1) + covar (2,2) );
LOG4CPP_INFO( logger, "RMS positional error [mm]: " << posRms );
Math::Vector< double, 3 > axis;
axis (0) = sqrt ( covar (3,3) );
axis (1) = sqrt ( covar (4,4) );
axis (2) = sqrt ( covar (5,5) );
double norm = norm_2 (axis);
double phi = asin ( norm ) * 2;
phi = phi * 180 / boost::math::constants::pi<double>();
LOG4CPP_INFO( logger, "Standard deviation of rotational error [deg]: " << phi );
return ep;
}
exitCode FileWriterCommandHandler::notify() {
LOG4CPP_INFO(log, "\t --- Notify received [%d]", d_cnotify++ );
return OK;
}
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
}
