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" ); } } }