/**
* 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 ) );
}
/** 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() );
}
/** 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 );
}
/** 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 );
}
/** 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() );
}
/** Method that computes the result for the smoothing. */
void compute( Measurement::Timestamp t )
{
if( !m_init ){
m_mean = *( m_inPort.get() );
m_init = true;
}
else
m_mean = ( m_alpha * ( *( m_inPort.get() ) ) ) + ( ( 1. - m_alpha ) * m_mean );
LOG4CPP_TRACE( logger, "exponential smoothing: " << m_mean );
m_outPort.send( EventType( t, m_mean ) );
}
/** Method that computes the result. */
void compute( Measurement::Timestamp t )
{
LOG4CPP_TRACE( logger, "desired list size: " << m_size );
// Retrieve and store measurement
m_list.push_back( *m_inPort.get() );
LOG4CPP_TRACE( logger, "current size: " << m_list.size() );
if ( m_list.size() < m_size )
{
if ( isPortPush ( "Input" ) )
{
LOG4CPP_TRACE( logger, "push input port, wait for more measurements..." );
return;
}
LOG4CPP_TRACE( logger, "pull inport port, retrieve missing measurements..." );
while ( m_list.size() < m_size )
{
LOG4CPP_TRACE( logger, "items in list: " << m_list.size() << ", pulling next measurement" );
m_inPort.pull( t );
m_list.push_back( *m_inPort.get() );
};
}
LOG4CPP_TRACE( logger, "desired list size reached" );
m_outPort.send( Measurement::Measurement< typename std::vector< typename EventType::value_type > >( t, m_list ) );
m_list.clear();
}
void PoseErrorVisualization::receiveError( const Ubitrack::Measurement::ErrorPose& error )
{
LOG4CPP_DEBUG( logger, "Received error pose" );
LOG4CPP_TRACE( logger, *error );
boost::mutex::scoped_lock l( m_poseLock );
// rotate the position covariance into the target coordinate frame
Matrix< double, 3, 3 > j( ~error->rotation() );
Matrix< double, 3, 3 > tmp( ublas::prod( j, ublas::subrange( error->covariance(), 0, 3, 0, 3 ) ) );
Matrix< double, 3, 3 > posError( ublas::prod( tmp, ublas::trans( j ) ) );
LOG4CPP_TRACE( logger, "Position error: " << std::endl << posError );
m_posEllipsoid.setCovariance( posError );
// x rotation error
rotErrorJacobian( j, m_rotXEllipsoid.position() );
noalias( tmp ) = ublas::prod( j, ublas::subrange( error->covariance(), 3, 6, 3, 6 ) );
noalias( posError ) = ublas::prod( tmp, ublas::trans( j ) );
LOG4CPP_TRACE( logger, "X rotation error: " << std::endl << posError );
m_rotXEllipsoid.setCovariance( posError );
// y rotation error
rotErrorJacobian( j, m_rotYEllipsoid.position() );
noalias( tmp ) = ublas::prod( j, ublas::subrange( error->covariance(), 3, 6, 3, 6 ) );
noalias( posError ) = ublas::prod( tmp, ublas::trans( j ) );
LOG4CPP_TRACE( logger, "Y rotation error: " << std::endl << posError );
m_rotYEllipsoid.setCovariance( posError );
// z rotation error
rotErrorJacobian( j, m_rotZEllipsoid.position() );
noalias( tmp ) = ublas::prod( j, ublas::subrange( error->covariance(), 3, 6, 3, 6 ) );
noalias( posError ) = ublas::prod( tmp, ublas::trans( j ) );
LOG4CPP_TRACE( logger, "Z rotation error: " << std::endl << posError );
m_rotZEllipsoid.setCovariance( posError );
}
/** render the object */
void Projection::draw( Measurement::Timestamp& time, int parity )
{
// use projection in stereo mode only if correct eye
if ( ( m_stereoEye == stereoEyeRight && parity == 1 ) || ( m_stereoEye == stereoEyeLeft && parity == 0 ) )
return;
if ( m_pPull && m_pPull->isConnected())
inputIn( m_pPull->get(time), 0);
LOG4CPP_TRACE( logger, "Updating projection matrix to:" << std::endl << m_projection );
glMatrixMode( GL_PROJECTION );
glLoadMatrixd( m_projection.content() );
glMatrixMode( GL_MODELVIEW );
}
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" );
}
}
/**
* UTQL component constructor.
*
* @param sName Unique name of the component.
* @param subgraph UTQL subgraph
*/
RingBuffer( const std::string& sName, boost::shared_ptr< Graph::UTQLSubgraph > cfg )
: Dataflow::TriggerComponent( sName, cfg )
, m_inPort( "Input", *this )
, m_outPort( "Output", *this )
, m_size( 0 ) // number of elements
, m_position_buffer( 0 )
{
m_list.clear();
if( cfg->m_DataflowAttributes.hasAttribute( "size" ) )
{
cfg->m_DataflowAttributes.getAttributeData( "size", m_size );
m_list.reserve( m_size );
LOG4CPP_TRACE( logger, "desired list size: " << m_size );
}
}
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() );
}
}
Math::ErrorVector< double, 3 > CovarianceEstimation< Measurement::Position, Measurement::ErrorPosition >::incrementalEstimate( Math::Vector< double, 3 >& posNew )
{
ublas::vector_range< typename Math::Vector< double >::base_type > posMean ( mean, ublas::range( 0, 3 ) );
ublas::matrix_range< typename Math::Matrix< double, 0, 0 >::base_type > outProd3 ( outProd, ublas::range ( 0, 3 ), ublas::range ( 0, 3 ) );
// Running mean value of position random variable
posMean = ( ( ((double)m_counter - 1) / (double)m_counter ) * posMean ) + ( ( 1 / (double)m_counter ) * posNew );
// Running outer product of position random variable (not yet normalized by number of measurements)
outProd3 = outProd3 + ublas::outer_prod( posNew, posNew );
// Compute covariance matrix
if ( m_counter == 1 )
{
LOG4CPP_TRACE( logger, "Not enough data to compute covariance matrix" );
return Math::ErrorVector< double, 3 >();
}
Math::ErrorVector< double, 3 > ev ( posMean, outProd3 / ((double)m_counter) - ublas::outer_prod ( posMean, posMean ) );
LOG4CPP_TRACE( logger, "Running (empirical) mean / covariance: " << std::endl << ev );
return ev;
}
/**
* UTQL component constructor.
*
* @param sName Unique name of the component.
* @param subgraph UTQL subgraph
*/
WindowedAggregator( const std::string& sName, boost::shared_ptr< Graph::UTQLSubgraph > cfg )
: Dataflow::TriggerComponent( sName, cfg )
, m_inPort( "Input", *this )
, m_outPort( "Output", *this )
, m_time( 0.0 ) // duration in nanoseconds
{
m_mList.clear();
m_tList.clear();
if( cfg->m_DataflowAttributes.hasAttribute( "time" ) )
{
cfg->m_DataflowAttributes.getAttributeData( "time", m_time );
LOG4CPP_TRACE( logger, "desired list duration[ms] : " << m_time );
m_time *= 1e+06; //ms to ns
}
}
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 );
}
/** Method that computes the result. */
void compute( Measurement::Timestamp t )
{
//add newest elements to queues
m_mList.push_back( *m_inPort.get() );
m_tList.push_back( t );
//delete oldest elements from queues
while( ( m_tList.front() + m_time ) < t )
{
m_mList.pop_front();
m_tList.pop_front();
}
//copy all elements from queue to vector
std::vector< typename EventType::value_type > output;
output.reserve( m_mList.size() );
output.assign( m_mList.begin(), m_mList.end() );
LOG4CPP_TRACE( logger, "items in queue: " << m_mList.size() );
m_outPort.send( Measurement::Measurement< typename std::vector< typename EventType::value_type > >( t, output ) );
}
/** Method that computes the result. */
void compute( Measurement::Timestamp t )
{
if( m_list.size() < m_list.capacity() )
{
m_list.push_back( *m_inPort.get() );
if( m_list.size() < m_list.capacity())
{
LOG4CPP_TRACE( logger, "Ring Buffer not yet full, reached " << m_list.size() << " of " << m_list.capacity() << " measurements." );
UBITRACK_THROW( "Ring buffer not full. need to add more measurements." );
return;
}
m_outPort.send( Measurement::Measurement< typename std::vector< typename EventType::value_type > >( t, m_list ) );
return;
}
m_list[ m_position_buffer++ ] = *m_inPort.get();
m_position_buffer = m_position_buffer % m_size;
m_outPort.send( Measurement::Measurement< typename std::vector< typename EventType::value_type > >( t, m_list ) );
}
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;
}
