/** integrates an absolute measurement. */
void receiveAbsolute( const Measurement::Rotation& m )
{
LOG4CPP_DEBUG( logger, "Received absolute measurement: " << m );
LOG4CPP_TRACE( logger, "state before: " << m_kf.getState() );
m_kf.addRotationMeasurement( m );
LOG4CPP_DEBUG( logger, "computed state: " << m_kf.getState() );
}
/** integrates an absolute measurement. */
void receiveVelocity( const Measurement::RotationVelocity& m )
{
LOG4CPP_DEBUG( logger, "Received velocity measurement: " << m );
LOG4CPP_TRACE( logger, "state before: " << m_kf.getState() );
m_kf.addVelocityMeasurement( m );
LOG4CPP_DEBUG( logger, "computed state: " << m_kf.getState() );
}
exitCode CommandDispatcher::setDefaultCommand(Command * command) {
LOG4CPP_DEBUG(log, "CommandDispatcher::setDefaultCommand(Command * command)");
if ( d_command != command ) {
LOG4CPP_WARN(log, "Associated default command updated");
LOG4CPP_DEBUG(log, "Previous command overwritten but NOT released: ensure to release any provious command's memory!");
d_command = command;
}
return OK;
}
void globFiles( const std::string& directory, const std::string & patternString, std::list< boost::filesystem::path >& files, bool globDirectories)
{
boost::filesystem::path testPath( directory );
if ( boost::filesystem::is_directory( testPath ) && boost::filesystem::exists( testPath ) )
{
boost::regex ext( patternString.c_str() );
// iterate directory
boost::filesystem::directory_iterator dirEnd;
for ( boost::filesystem::directory_iterator it( testPath ); it != dirEnd; it++ )
{
#ifdef BOOST_FILESYSTEM_I18N
boost::filesystem::path p( it->path() );
#else
boost::filesystem::path p( *it );
#endif
// check for files with suitable extension
if ( boost::filesystem::exists( p ) && ! boost::filesystem::is_directory( p ) && boost::regex_match( p.filename().string(), ext ) )
{
LOG4CPP_TRACE( logger, "Adding file " << p << " to list" );
files.push_back( p );
}
// glob directories, if desired
else if ( globDirectories && boost::filesystem::exists( p ) && boost::filesystem::is_directory( p ) )
{
files.push_back( p );
}
}
// sort, since directory iteration is not ordered on some file systems
files.sort();
LOG4CPP_DEBUG( logger, "Sorted list of files" );
for ( std::list< boost::filesystem::path >::iterator iter = files.begin(); iter != files.end(); iter ++ )
{
LOG4CPP_DEBUG( logger, *iter );
}
}
else if ( boost::filesystem::exists( testPath ) ) {
files.push_back( testPath );
}
else {
UBITRACK_THROW( "Invalid path specified" );
}
if ( files.size() == 0 ) {
UBITRACK_THROW( "No suitable files found at the specified location" );
}
}
void PollEventGenerator::run (void) {
threadStartNotify("POLL");
while (true) {
LOG4CPP_DEBUG(log, "Next poll within %d ms", d_pollTime);
sleep(d_pollTime);
LOG4CPP_DEBUG(log, "Polling");
notify(false);
}
threadStopNotify();
}
void Cross2D::crossPositionIn( const Ubitrack::Measurement::Position2D& pos )
{
LOG4CPP_DEBUG( logger, "received cross position " << pos );
boost::mutex::scoped_lock l( m_positionLock );
m_crossPosition = pos;
m_pModule->invalidate( this );
}
/** Method that returns a predicted measurement. */
Measurement::Rotation sendOut( Measurement::Timestamp t )
{
LOG4CPP_DEBUG( logger, "Computing rotation for t=" << t );
LOG4CPP_TRACE( logger, "state: " << m_kf.getState() );
return m_kf.predict( t );
}
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;
}
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
/**
* Handler method for input of perturbed measurement data. All
* measurements are collected and will contribute to the finally
* generated distribution which will be computed and pushed onward
* as soon as the configured amount of trials is reached.
*
* It is not necessary to trigger the compoent before via the
* {@code TriggerInput} input and noisy data may be pushed
* asynchronously. However, each push will result in an event to
* be issued on the {@code Sync} output.
*/
void dataIn( const EventType& e )
{
LOG4CPP_TRACE( logger, getName() << " Received perturbed measurement with timestamp " << e.time() );
if ( m_bStopped )
{
LOG4CPP_TRACE( logger, getName() << " Covariance estimation has not been triggered yet, ignore measurement" );
return;
}
m_counter ++;
LOG4CPP_TRACE( logger, getName() << " Current counter: " << m_counter << ", go on until: " << m_size );
// Compute incremental covariance
typename ResultType::value_type estimate = incrementalEstimate ( *e );
// Check list size
if ( m_counter == m_size )
{
// push onward final result
LOG4CPP_DEBUG( logger, getName() << " Terminate and push final result" );
m_outPortDist.send( ResultType ( e.time(), estimate ) );
boost::mutex::scoped_lock l( m_mutex );
m_bStopped = true;
return;
}
// If not reached, send next trigger event
LOG4CPP_TRACE( logger, getName() << " Triggering computation..." );
m_outPortSync.send( Measurement::Button( Measurement::now(), m_button ) );
}
timeout_t PollEventGenerator::pollTime() const {
LOG4CPP_DEBUG(log, "PollEventGenerator::pollTime()");
return d_pollTime;
}
CommandDispatcher::CommandDispatcher(Handler * handler, bool suspended):
EventDispatcher(handler, suspended, "cd"),
d_command(0) {
LOG4CPP_DEBUG(log, "CommandDispatcher::CommandDispatcher(Handler * handler, bool suspended)");
}
/** Method that returns a predicted measurement. */
Measurement::ErrorPose sendOut( Measurement::Timestamp t )
{
LOG4CPP_DEBUG( logger, "Computing pose for t=" << t );
LOG4CPP_TRACE( logger, "state: " << m_pKF->getState() << std::endl << m_pKF->getCovariance() );
return m_pKF->predictPose( t );
}
/**
* UTQL component constructor.
*
* @param sName Unique name of the component.
* @param subgraph UTQL subgraph
*/
CovarianceEstimation( const std::string& sName, boost::shared_ptr< Graph::UTQLSubgraph > subgraph )
: Dataflow::Component( sName )
, m_inPortPerturbed( "PerturbedInput", *this, boost::bind( &CovarianceEstimation::dataIn, this, _1 ) )
, m_inPortTrigger( "TriggerInput", *this, boost::bind( &CovarianceEstimation::triggerIn, this, _1 ) )
, m_outPortSync( "Sync", *this )
, m_outPortDist ( "Distribution", *this )
, m_bStopped( true )
, m_counter ( 0 )
, m_size( 100 )
, m_button( ' ' )
, m_inButton( ' ' )
{
LOG4CPP_DEBUG( logger, "Setup CovarianceEstimation component" );
// read configuration for amount of samples to use
subgraph->m_DataflowAttributes.getAttributeData( "size", m_size );
// read button key
std::string button( " " );
std::string inButton( " " );
if ( subgraph->m_DataflowAttributes.hasAttribute( "button" ) )
button = subgraph->m_DataflowAttributes.getAttributeString( "button" );
if ( subgraph->m_DataflowAttributes.hasAttribute( "inButton" ) )
inButton = subgraph->m_DataflowAttributes.getAttributeString( "inButton" );
m_button = Math::Scalar< int >( button[ 0 ] );
m_inButton = Math::Scalar< int >( inButton[ 0 ] );
mean = Math::Vector< double, 7 >::zeros();
outProd = Math::Matrix< double, 7, 7 >::zeros( );
}
exitCode CommandDispatcher::suspend() {
LOG4CPP_DEBUG(log, "CommandDispatcher::suspend()");
d_suspended = true;
return OK;
}
QueryRegistry::~QueryRegistry() {
LOG4CPP_DEBUG(log, "QueryRegistry::~QueryRegistry()");
// flushing the registry <map>
d_queryRegistry.clear();
}
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 DeviceGPIO::ttyLock(unsigned short port) {
if (port==TTYMUX_SINGLE)
return OK;
// Acquiring mux lock to satefly use the port
LOG4CPP_DEBUG(log, "Waiting for MUX1 lock...");
d_ttyLock.enterMutex();
LOG4CPP_DEBUG(log, "TTY mux lock acquired by port [%c]", 'A'-1+port);
// Switching MUX to te required port
ttySelect(port);
return OK;
}
exitCode PollEventGenerator::setPollTime(timeout_t p_pollTime, bool reset) {
LOG4CPP_DEBUG(log, "PollEventGenerator::setPollTime(timeout_t, bool");
d_pollTime = p_pollTime;
return OK;
}
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;
}
Transparency::Transparency( const std::string& name, boost::shared_ptr< Graph::UTQLSubgraph > subgraph,
const VirtualObjectKey& componentKey, VirtualCamera* pModule )
: VirtualObject( name, subgraph, componentKey, pModule )
{
LOG4CPP_DEBUG( logger, "Transparency::Transparency(), initialize alpha to " << alpha );
alpha = 0.5;
m_pPush.reset( new PushConsumer< Ubitrack::Measurement::Distance >( "Input", *this, boost::bind( &Transparency::alphaIn, this, _1 ) ) );
}
void PoseErrorVisualization::draw3DContent( Measurement::Timestamp& t, int )
{
LOG4CPP_DEBUG( logger, "Drawing ellipsoids" );
// save old state
GLboolean oldCullMode;
glGetBooleanv( GL_CULL_FACE, &oldCullMode );
GLboolean oldLineSmooth = glIsEnabled( GL_LINE_SMOOTH );
GLfloat oldLineWidth;
glGetFloatv( GL_LINE_WIDTH, &oldLineWidth );
// set new state
glEnable( GL_CULL_FACE );
glLineWidth( 1 );
glEnable( GL_LINE_SMOOTH );
// position error
glColor3f( 0.8f, 0.8f, 0.0f );
m_posEllipsoid.draw();
// x-axis rotation error
glColor3f( 0.8f, 0.0f, 0.0f );
m_rotXEllipsoid.draw();
glBegin( GL_LINES );
glVertex3d( 0, 0, 0 );
glVertex3d( m_rotXEllipsoid.position()( 0 ), m_rotXEllipsoid.position()( 1 ), m_rotXEllipsoid.position()( 2 ) );
glEnd();
// y-axis rotation error
glColor3f( 0.0f, 0.8f, 0.0f );
m_rotYEllipsoid.draw();
glBegin( GL_LINES );
glVertex3d( 0, 0, 0 );
glVertex3d( m_rotYEllipsoid.position()( 0 ), m_rotYEllipsoid.position()( 1 ), m_rotYEllipsoid.position()( 2 ) );
glEnd();
// z-axis rotation error
glColor3f( 0.0f, 0.0f, 0.8f );
m_rotZEllipsoid.draw();
glBegin( GL_LINES );
glVertex3d( 0, 0, 0 );
glVertex3d( m_rotZEllipsoid.position()( 0 ), m_rotZEllipsoid.position()( 1 ), m_rotZEllipsoid.position()( 2 ) );
glEnd();
// restore old state
if ( !oldCullMode )
glDisable( GL_CULL_FACE );
if ( !oldLineSmooth )
glDisable( GL_LINE_SMOOTH );
glLineWidth( oldLineWidth );
}
QueryRegistry * QueryRegistry::getInstance() {
if ( !d_instance ) {
d_instance = new QueryRegistry();
}
LOG4CPP_DEBUG(d_instance->log, "QueryRegistry::getInstance()");
return d_instance;
}
exitCode DeviceGPIO::ttyUnLock(unsigned short port) {
if (port==TTYMUX_SINGLE)
return OK;
d_ttyLock.leaveMutex();
LOG4CPP_DEBUG(log, "TTY mux lock released by port [%c]", 'A'-1+port);
return OK;
}
ImageFileRecorder::ImageFileRecorder( const std::string& sName, boost::shared_ptr< Graph::UTQLSubgraph > subgraph )
: Dataflow::Component( sName )
, m_bStop( false )
, m_prefix( "img" )
, m_suffix( "png" )
, m_imgDir( "" )
, m_logfile( "frames.txt" )
, m_counter( 0 )
, m_inPort( "Input", *this, boost::bind( &ImageFileRecorder::saveImage, this, _1 ) )
{
LOG4CPP_DEBUG( logger, "ImageFileRecorder(): Configuring..." );
if ( subgraph -> m_DataflowAttributes.hasAttribute( "file" ) ) {
m_logfile = subgraph->m_DataflowAttributes.getAttributeString( "file" );
}
else
{
m_logfile = "frames.txt";
}
if ( subgraph -> m_DataflowAttributes.hasAttribute( "prefix" ) )
m_prefix = subgraph->m_DataflowAttributes.getAttributeString( "prefix" );
if ( subgraph -> m_DataflowAttributes.hasAttribute( "suffix" ) )
m_suffix = "." + subgraph->m_DataflowAttributes.getAttributeString( "suffix" );
boost::filesystem::path path = "";
if ( subgraph -> m_DataflowAttributes.hasAttribute( "path" ) )
path = subgraph->m_DataflowAttributes.getAttributeString( "path" );
m_imgDir /= path;
if ( ! boost::filesystem::is_directory( m_imgDir ) )
UBITRACK_THROW( "Invalid path specified for images." );
boost::filesystem::path testPath = path;
testPath /= m_prefix;
testPath /= "XXXXX";
testPath /= m_suffix;
LOG4CPP_DEBUG( logger, "ImageFileRecorder(): Image path pattern: " << testPath );
LOG4CPP_DEBUG( logger, "ImageFileRecorder(): Timestamp file: " << m_logfile );
}
void Color2Grayscale::pushImage(const ImageMeasurement& m )
{
IplImage* img = *m;
if(img->nChannels == 1)
{
LOG4CPP_DEBUG( logger, "Got grayscale image: pushing unmodified" );
m_outPort.send( m );
// TODO: why should we clone the image instead of just pushing the measurement?
//boost::shared_ptr< Image > pImage(new Image(cvCloneImage(img)));
//m_outPort.send( ImageMeasurement( m.time(), pImage ) );
}
else
{
LOG4CPP_DEBUG( logger, "Got color image: pushing converted" );
boost::shared_ptr< Image > pImage( new Image( img->width, img->height, 1 ) );
pImage->origin = img->origin; //inserted by CW to have correct origin
cvConvertImage(*m,*pImage);
m_outPort.send( ImageMeasurement( m.time(), pImage ) );
}
}
void Transparency::alphaIn( const Ubitrack::Measurement::Distance &a )
{
if ( *a < 0.0 )
alpha = 0.0;
else if ( *a > 1.0 )
alpha = 1.0;
else
alpha = *a;
LOG4CPP_DEBUG( logger, "Transparency::alphaIn() " << a );
}
/** integrates an inverse rotation velocity measurement. */
void receiveInverseRotationVelocity( const Measurement::RotationVelocity& m )
{
LOG4CPP_DEBUG( logger, "Received inverse rotation velocity measurement: " << m );
LOG4CPP_TRACE( logger, "state before: " << m_pKF->getState() << std::endl << m_pKF->getCovariance() );
m_pKF->addInverseRotationVelocityMeasurement( m );
checkSend( m.time() );
LOG4CPP_TRACE( logger, "state after: " << m_pKF->getState() << std::endl << m_pKF->getCovariance() );
}
DeviceGPIO::DeviceGPIO(std::string const & logName) :
Device(Device::DEVICE_GPIO, 0, logName),
d_config(Configurator::getInstance()),
log(Device::log) {
LOG4CPP_DEBUG(log, "DeviceGPIO(const std::string &, bool)");
// Registering device into the DeviceDB
dbReg();
}
exitCode FileWriterCommandHandler::notify(comsys::Command * command)
throw (exceptions::IllegalCommandException) {
std::string xmlCommand;
LOG4CPP_DEBUG(log, "FileWriterCommandHandler::notify(Command * command)");
command->xmlDump(xmlCommand);
d_fwcategory->info("%s", xmlCommand.c_str());
return OK;
}
