xpath::element::element( const std::string& n )
{
std::vector< std::string > v = split( n, '[' );
switch( v.size() )
{
case 1:
name = v[0];
break;
case 2:
if( v[0] == "" ) { COMMA_THROW_STREAM( comma::exception, "got non-empty index " << v[1] << " in empty element" ); }
if( v[1].length() < 2 || v[1][ v[1].size() - 1 ] != ']' ) { COMMA_THROW_STREAM( comma::exception, "invalid index in element \"" << n << "\"" ); }
name = v[0];
index = boost::lexical_cast< std::size_t >( v[1].substr( 0, v[1].size() - 1 ) );
break;
default:
COMMA_THROW_STREAM( comma::exception, "invalid element \"" << n << "\"" );
}
}
std::pair< comma::uint32, comma::uint32 > to_ntp_time( boost::posix_time::ptime t )
{
if( t < ntp_base ) { COMMA_THROW_STREAM( comma::exception, "cannot convert to ntp time: " << t << ", which is less than NTP time base " << ntp_base ); }
comma::int32 s = ( t - epoch_time ).total_seconds(); // 32 bit signed int in boost and posix
comma::int32 m = t.time_of_day().total_microseconds() % 1000000;
if( t >= epoch_time || m == 0 )
{
return std::pair< comma::uint32, comma::uint32 >( static_cast< comma::uint32 >( ntp_diff + s ), static_cast< comma::uint32 >( m / ntp_microsec_coeff ) );
}
else
{
return std::pair< comma::uint32, comma::uint32 >( static_cast< comma::uint32 >( ntp_diff + s - 1 ), static_cast< comma::uint32 >( m / ntp_microsec_coeff ) );
}
}
xpath::element::element( const std::string& n, boost::optional< std::size_t > idx )
: name( n )
, index( idx )
{
if( index && name == "" ) { COMMA_THROW_STREAM( comma::exception, "got non-empty index in empty element" ); }
}
camera_parser::camera_parser ( const std::string& file, const std::string& path )
{
camera_parameters parameters("","","","","");
comma::read< camera_parameters >( parameters, file, path );
if( parameters.focal_length.empty() )
{
parameters.focal_length = zero_focal_length;
std::cerr << "stereo-to-points: warning: " << path << ": focal-length set to default (\"" << zero_focal_length << "\")" << std::endl;
}
std::vector< std::string > v = comma::split( parameters.focal_length, ',' );
if ( v.size() != 2 ) COMMA_THROW_STREAM( comma::exception, " unexpected input for " << path << " focal-length : " << parameters.focal_length );
double fX = boost::lexical_cast< double >( v[0] );
double fY = boost::lexical_cast< double >( v[1] );
// std::cerr << parameters.focal_length << " ( " << fX << "," << fY << " ) " << std::endl;
if( parameters.center.empty() )
{
parameters.center = zero_center;
std::cerr << "stereo-to-points: warning: " << path << ": center set to default (\"" << zero_center << "\")" << std::endl;
}
v = comma::split( parameters.center, ',' );
if ( v.size() != 2 ) COMMA_THROW_STREAM( comma::exception, " unexpected input for " << path << " center : " << parameters.center );
double cX = boost::lexical_cast< double >( v[0] );
double cY = boost::lexical_cast< double >( v[1] );
// std::cerr << parameters.center << " ( " << cX << "," << cY << " ) " << std::endl;
if( parameters.distortion.empty() )
{
parameters.distortion = zero_distortion;
std::cerr << "stereo-to-points: warning: " << path << ": distortion set to default (\"" << zero_distortion << "\")" << std::endl;
}
v = comma::split( parameters.distortion, ',' );
if ( v.size() != 5 ) COMMA_THROW_STREAM( comma::exception, " unexpected input for " << path << " distortion : " << parameters.distortion );
double k1 = boost::lexical_cast< double >( v[0] );
double k2 = boost::lexical_cast< double >( v[1] );
double p1 = boost::lexical_cast< double >( v[2] );
double p2 = boost::lexical_cast< double >( v[3] );
double k3 = boost::lexical_cast< double >( v[4] );
// std::cerr << parameters.distortion << " ( " << k1 << "," << k2 << "," << p1 << "," << p2 << "," << k3 << " ) " << std::endl;
if( parameters.rotation.empty() )
{
parameters.rotation = zero_rotation;
std::cerr << "stereo-to-points: warning: " << path << ": rotation set to default (\"" << zero_rotation << "\")" << std::endl;
}
v = comma::split( parameters.rotation, ',' );
if ( v.size() != 3 ) COMMA_THROW_STREAM( comma::exception, " unexpected input for " << path << " rotation : " << parameters.rotation );
double roll = boost::lexical_cast< double >( v[0] );
double pitch = boost::lexical_cast< double >( v[1] );
double yaw = boost::lexical_cast< double >( v[2] );
// std::cerr << parameters.rotation << " ( " << roll << "," << pitch << "," << yaw << " ) " << std::endl;
if( parameters.translation.empty() )
{
parameters.translation = zero_translation;
std::cerr << "stereo-to-points: warning: " << path << ": translation set to default (\"" << zero_translation << "\")" << std::endl;
}
v = comma::split( parameters.translation, ',' );
if ( v.size() != 3 ) COMMA_THROW_STREAM( comma::exception, " unexpected input for " << path << " translation : " << parameters.translation );
double tX = boost::lexical_cast< double >( v[0] );
double tY = boost::lexical_cast< double >( v[1] );
double tZ = boost::lexical_cast< double >( v[2] );
// std::cerr << parameters.translation << " ( " << tX << "," << tY << "," << tZ << " ) " << std::endl;
m_camera << fX, 0, cX,
0, fY, cY,
0, 0, 1;
m_distortion << k1,k2,p1,p2,k3;
snark::rotation_matrix rotation( Eigen::Vector3d( roll, pitch, yaw ) );
m_rotation = rotation.rotation();
m_translation << tX, tY, tZ;
if( !parameters.map.empty() )
{
v = comma::split( parameters.size, ',' );
if ( v.size() != 2 ) COMMA_THROW_STREAM( comma::exception, " unexpected input for " << path << " map size : " << parameters.size );
unsigned int width = boost::lexical_cast< unsigned int >( v[0] );
unsigned int height = boost::lexical_cast< unsigned int >( v[1] );
std::ifstream stream( parameters.map.c_str(), std::ios::binary );
if( !stream )
{
COMMA_THROW( comma::exception, "failed to open undistort map in \"" << parameters.map << "\"" );
}
std::size_t size = width * height * 4;
std::vector< char > buffer( size );
stream.read( &buffer[0], size );
if( stream.gcount() < 0 || std::size_t( stream.gcount() ) != size )
{
COMMA_THROW( comma::exception, "failed to read \"" << parameters.map << "\"" << " stream.gcount():" <<stream.gcount() );
}
m_map_x = cv::Mat( height, width, CV_32FC1, &buffer[0] ).clone();
stream.read( &buffer[0], size );
if( stream.gcount() < 0 || std::size_t( stream.gcount() ) != size )
{
COMMA_THROW( comma::exception, "failed to read \"" << parameters.map << "\"" );
}
m_map_y = cv::Mat( height, width, CV_32FC1, &buffer[0] ).clone();
stream.peek();
if( !stream.eof() )
{
COMMA_THROW( comma::exception, "expected " << ( size * 2 ) << " bytes in \"" << parameters.map << "\", got more" );
}
}
}
