Viewer::Viewer( const QColor4ub& background_color
, double fov
, bool z_up
, bool orthographic
, boost::optional< comma::csv::options > camera_csv, boost::optional< Eigen::Vector3d > cameraposition
, boost::optional< Eigen::Vector3d > cameraorientation
, boost::property_tree::ptree* camera_config
, boost::optional< Eigen::Vector3d > scene_center
, boost::optional< double > scene_radius
, bool output_camera_position )
: qt3d::view( background_color
, fov
, z_up
, orthographic
, scene_center ? boost::optional< QVector3D >( QVector3D( scene_center->x()
, scene_center->y()
, scene_center->z() ) )
: boost::optional< QVector3D >()
, scene_radius )
, m_lookAt( false )
, m_cameraposition( cameraposition )
, m_cameraorientation( cameraorientation )
{
if( output_camera_position ) { camera_position_output_.reset( new camera_position_output( *this ) ); }
QTimer* timer = new QTimer( this );
timer->start( 40 );
connect( timer, SIGNAL( timeout() ), this, SLOT( read() ) );
if( camera_csv ) { m_cameraReader.reset( new CameraReader( *camera_csv ) ); }
if( camera_config )
{
comma::from_ptree from_ptree( *camera_config, true );
comma::visiting::apply( from_ptree ).to( *camera() );
//boost::property_tree::ptree p;
//comma::to_ptree to_ptree( p );
//comma::visiting::apply( to_ptree ).to( *camera() );
//std::cerr << "view-points: camera set to:" << std::endl;
//boost::property_tree::write_json( std::cerr, p );
}
m_cameraFixed = m_cameraposition || m_cameraReader || camera_config;
}
camera_parser::camera_parser ( const std::string& file, const std::string& path )
{
std::ifstream ifs( file.c_str() ); // todo: config stream instead?
if( !ifs.is_open() )
{
COMMA_THROW_STREAM( comma::exception, "failed to open camera config file: " << file );
}
boost::property_tree::ptree tree;
comma::property_tree::from_name_value( ifs, tree, '=', ' ' );
comma::from_ptree from_ptree( tree, path, true );
camera_parameters parameters;
comma::visiting::apply( from_ptree, parameters );
if( parameters.focal_length == "0,0" ) { std::cerr << "stereo-to-points: warning: " << path << ": focal-length set to default (\"0,0\")" << 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 == "0,0" ) { std::cerr << "stereo-to-points: warning: " << path << ": center set to default (\"0,0\")" << 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 == "0,0,0,0,0" ) { std::cerr << "stereo-to-points: warning: " << path << ": distortion set to default (\"0,0,0,0,0\")" << 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 == "0,0,0" ) { std::cerr << "stereo-to-points: warning: " << path << ": rotation set to default (\"0,0,0\")" << 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 == "0,0" ) { std::cerr << "stereo-to-points: warning: " << path << ": translation set to default (\"0,0,0\")" << 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" );
}
}
}
