void MonoDvsCalibration::saveCalibration()
{
if (setCameraInfo())
ROS_INFO("Calibration saved successfully.");
else
ROS_ERROR("Error while saving calibration");
}
void reconfig( Config& newconfig, uint32_t level )
{
reconfiguring = true;
boost::mutex::scoped_lock lock( mutex );
// Resolve frame ID using tf_prefix parameter:
if( newconfig.frame_id == "" )
{
newconfig.frame_id = "camera";
}
std::string tf_prefix = tf::getPrefixParam( nh );
ROS_DEBUG_STREAM( "tf_prefix = " << tf_prefix );
newconfig.frame_id = tf::resolve( tf_prefix, newconfig.frame_id );
setCameraInfo( *camera_info_manager[LEFT] , config.camera_info_url_left , newconfig.camera_info_url_left );
setCameraInfo( *camera_info_manager[RIGHT], config.camera_info_url_right, newconfig.camera_info_url_right );
for( size_t i = 0; i < NUM_CAMERAS; ++i )
{
newconfig.frame_width = setProperty( camera[i], CV_CAP_PROP_FRAME_WIDTH , newconfig.frame_width );
newconfig.frame_height = setProperty( camera[i], CV_CAP_PROP_FRAME_HEIGHT, newconfig.frame_height );
//newconfig.fps = setProperty( camera[i], CV_CAP_PROP_FPS , newconfig.fps );
newconfig.brightness = setProperty( camera[i], CV_CAP_PROP_BRIGHTNESS , newconfig.brightness );
newconfig.contrast = setProperty( camera[i], CV_CAP_PROP_CONTRAST , newconfig.contrast );
newconfig.saturation = setProperty( camera[i], CV_CAP_PROP_SATURATION , newconfig.saturation );
newconfig.hue = setProperty( camera[i], CV_CAP_PROP_HUE , newconfig.hue );
newconfig.gain = setProperty( camera[i], CV_CAP_PROP_GAIN , newconfig.gain );
newconfig.exposure = setProperty( camera[i], CV_CAP_PROP_EXPOSURE , newconfig.exposure );
//setFOURCC( camera[i], newconfig.fourcc );
frame[i]->header.frame_id = newconfig.frame_id;
}
if( fps != newconfig.fps )
{
fps = newconfig.fps;
timer.setPeriod( ros::Duration( 1. / fps ) );
}
config = newconfig;
reconfiguring = false;
}
void CUDARenderer::draw(const glm::mat4 &View, const glm::mat4 &Projection)
{
setCameraInfo(camera.cam);
gpuErrchk(cudaGraphicsMapResources(1, &cudaSurfRes));
{
cudaArray *viewCudaArray;
gpuErrchk(cudaGraphicsSubResourceGetMappedArray(&viewCudaArray, cudaSurfRes, 0, 0));
renderToTexture(dimBlock, dimGrid, d_scene, viewCudaArray);
}
gpuErrchk(cudaGraphicsUnmapResources(1, &cudaSurfRes));
glDrawArrays(GL_TRIANGLES, 0, 6);
}
void Scene::keyPressEvent(QKeyEvent *event)
{
switch (event->key()) {
case Qt::Key_Up:
m_triangle->sety0(m_triangle->m_y0+step);
m_shphere->sety0(m_shphere->m_y0+step);
break;
case Qt::Key_Left:
m_triangle->setx0(m_triangle->m_x0-step);
m_shphere->setx0(m_shphere->m_x0-step);
break;
case Qt::Key_Right:
m_triangle->setx0(m_triangle->m_x0+step);
m_shphere->setx0(m_shphere->m_x0+step);
break;
case Qt::Key_Down:
m_triangle->sety0(m_triangle->m_y0-step);
m_shphere->sety0(m_shphere->m_y0-step);
break;
case Qt::Key_W:
m_triangle->setz0(m_triangle->m_z0-step); // - от нас
m_shphere->setz0(m_shphere->m_z0-step);
break;
case Qt::Key_S:
m_triangle->setz0(m_triangle->m_z0+step); // + к нам
m_shphere->setz0(m_shphere->m_z0+step);
break;
case Qt::Key_A:
--m_angle;
if (m_angle<0) m_angle=359;
break;
case Qt::Key_D:
++m_angle;
if (m_angle>=360) m_angle=0;
break;
case Qt::Key_I: // уменьшаем угол перспективы камеры min=10 // TODO ещё по идее надо отодвигать камеру, чтобы пирамида охватывала сцену
if (cameraFocusAngle>10) --cameraFocusAngle;
break;
case Qt::Key_O: // увеличиваем угол перспективы камеры max=120 // TODO приближать камеру
if (cameraFocusAngle<120) ++cameraFocusAngle;
break;
case Qt::Key_P:
cameraFocusAngle=90;
break;
default:
break;
}
setCameraInfo();
setFigureInfo();
update();
}
void Scene::wheelEvent(QWheelEvent *event)
{ // шаг колеса обычно 120 едениц, 1 еденица это 1/8 градуса, значит 1 шаг = 15 градусам.
// мы будем считать в еденицах (некоторые драйвера мыши дают точность больше, т.е. меньше 120 за такт)
if (!event->isAccepted()) {
//camEyePos += event->delta();
/*if (camEyePos < -8 * 120)
camEyePos = -8 * 120;
if (camEyePos > 10 * 120)
camEyePos = 10 * 120;//*/
//event->accept();
}
// move to new position by step 120/10000 пока только по оси Z (-delta - значит крутим на себя)
cameraEye=cameraEye+QVector3D(0.0f,0.0f,((float)event->angleDelta().y()/10000));
cameraCenter=cameraCenter+QVector3D(0.0f,0.0f,((float)event->angleDelta().y()/10000));
setCameraInfo();
}
void MultiProjector::initFromCameraInfo(BaseCameraInfo* _camera_info){
MultiCameraInfo* mci = dynamic_cast<MultiCameraInfo*>(_camera_info);
if (! mci) {
throw std::runtime_error("Multi Projector needs a Multi CameraInfo");
}
_projectors.clear();
_projectors.resize(mci->cameraInfos().size());
for (size_t i = 0; i<mci->cameraInfos().size(); i++){
PinholeCameraInfo * pci =dynamic_cast<PinholeCameraInfo*>(mci->cameraInfos()[i]);
if (pci){
PinholeProjector* pp= new PinholeProjector;
_projectors[i] = pp;
pp->setMaxDistance(_max_distance);
pp->setMinDistance(_min_distance);
pp->setIncidenceAngle(_incidence_angle);
} else
throw std::runtime_error("unknown projector type");
}
setCameraInfo(mci);
}
void Scene::initializeGL() {
QOpenGLFunctions_3_0 *f = QOpenGLContext::currentContext()->versionFunctions<QOpenGLFunctions_3_0>();
if ( !f ) {
qWarning( "Could not obtain OpenGL versions object"" );
exit( 1 );
}
f->initializeOpenGLFunctions();
//
qDebug() << QString((const char*)f->glGetString(GL_VERSION)) << "\n" <<
QString((const char*)f->glGetString(GL_VENDOR))<< "\n" <<
QString((const char*)f->glGetString(GL_RENDERER));//<< "\n" << glGetString(GL_EXTENTIONS);
qDebug() << "Version format= " << QOpenGLContext::currentContext()->format().majorVersion() <<
" " << QOpenGLContext::currentContext()->format().minorVersion();
// очищаем поле
//glClearColor(0.1f,0.1f,0.2f,1.0f); // тёмно-синенький
f->glClearColor(0.1f,0.1f,0.2f,1.0f); //
//создаём объект - треугольник (создаём один, а нарисовать можем много таких-же)
//m_triangle=new Triangle(&m_program, m_vertexAttr, m_colorAttr, m_texAttr, m_texUniform);
m_triangle=new Triangle();
// инициализируем массив координат для сетевой сферы
spherepoints=new pointsofsphere(300);
// тест - создаём простую сферу на шейдерах
m_shphere=new shphere();
if (perspective)
m_shphere->setPerspective();
else
m_shphere->setOrthogonal();
setCameraInfo(); // формируем и посылаем текст для отображения параметров в главном окне
setFigureInfo(); //
}
void MonoDvsCalibration::saveCalibration()
{
setCameraInfo();
}
bool StereoNode::setCameraInfoR(sensor_msgs::SetCameraInfo::Request& req, sensor_msgs::SetCameraInfo::Response& rsp)
{
return setCameraInfo(req, rsp, r_cam_, r_msg_camera_info_);
}
void StereoDvsCalibration::saveCalibration()
{
setCameraInfo();
}
bool ColladaConverter::convert()
{
unsigned i;
//succesfully loaded file, now convert data
if (m_dom->getAsset() && m_dom->getAsset()->getUnit())
{
domAsset::domUnitRef unit = m_dom->getAsset()->getUnit();
domFloat meter = unit->getMeter();
printf("asset unit meter=%f\n",meter);
// m_unitMeterScaling = meter;
}
if ( m_dom->getAsset() && m_dom->getAsset()->getUp_axis() )
{
domAsset::domUp_axis * up = m_dom->getAsset()->getUp_axis();
switch( up->getValue() )
{
case UPAXISTYPE_X_UP:
printf(" X is Up Data and Hiearchies must be converted!\n" );
printf(" Conversion to X axis Up isn't currently supported!\n" );
printf(" COLLADA_RT defaulting to Y Up \n" );
setGravity(CrtVec3f(-10,0,0));
setCameraInfo(btVector3(1,0,0),1);
break;
case UPAXISTYPE_Y_UP:
printf(" Y Axis is Up for this file \n" );
printf(" COLLADA_RT set to Y Up \n" );
setGravity(CrtVec3f(0,-10,0));
setCameraInfo(btVector3(0,1,0),0);
break;
case UPAXISTYPE_Z_UP:
printf(" Z Axis is Up for this file \n" );
printf(" All Geometry and Hiearchies must be converted!\n" );
setGravity(CrtVec3f(0,0,-10));
break;
default:
break;
}
}
//we don't handle visual objects, physics objects are rered as such
for (unsigned int s=0;s<m_dom->getLibrary_visual_scenes_array().getCount();s++)
{
domLibrary_visual_scenesRef scenesRef = m_dom->getLibrary_visual_scenes_array()[s];
for (unsigned int i=0;i<scenesRef->getVisual_scene_array().getCount();i++)
{
domVisual_sceneRef sceneRef = scenesRef->getVisual_scene_array()[i];
for (unsigned int n=0;n<sceneRef->getNode_array().getCount();n++)
{
domNodeRef nodeRef = sceneRef->getNode_array()[n];
nodeRef->getRotate_array();
nodeRef->getTranslate_array();
nodeRef->getScale_array();
}
}
}
// Load all the geometry libraries
for ( i = 0; i < m_dom->getLibrary_geometries_array().getCount(); i++)
{
domLibrary_geometriesRef libgeom = m_dom->getLibrary_geometries_array()[i];
printf(" CrtScene::Reading Geometry Library \n" );
for ( unsigned int i = 0; i < libgeom->getGeometry_array().getCount(); i++)
{
//ReadGeometry( );
domGeometryRef lib = libgeom->getGeometry_array()[i];
domMesh *meshElement = lib->getMesh();
if (meshElement)
{
// Find out how many groups we need to allocate space for
int numTriangleGroups = (int)meshElement->getTriangles_array().getCount();
int numPolygonGroups = (int)meshElement->getPolygons_array().getCount();
int totalGroups = numTriangleGroups + numPolygonGroups;
if (totalGroups == 0)
{
printf("No Triangles or Polygons found int Geometry %s \n", lib->getId() );
} else
{
//printf("Found mesh geometry (%s): numTriangleGroups:%i numPolygonGroups:%i\n",lib->getId(),numTriangleGroups,numPolygonGroups);
}
}
domConvex_mesh *convexMeshElement = lib->getConvex_mesh();
if (convexMeshElement)
{
printf("found convexmesh element\n");
// Find out how many groups we need to allocate space for
int numTriangleGroups = (int)convexMeshElement->getTriangles_array().getCount();
int numPolygonGroups = (int)convexMeshElement->getPolygons_array().getCount();
int totalGroups = numTriangleGroups + numPolygonGroups;
if (totalGroups == 0)
{
printf("No Triangles or Polygons found in ConvexMesh Geometry %s \n", lib->getId() );
}else
{
printf("Found convexmesh geometry: numTriangleGroups:%i numPolygonGroups:%i\n",numTriangleGroups,numPolygonGroups);
}
}//fi
}//for each geometry
}//for all geometry libraries
//m_dom->getLibrary_physics_models_array()
for ( i = 0; i < m_dom->getLibrary_physics_scenes_array().getCount(); i++)
{
domLibrary_physics_scenesRef physicsScenesRef = m_dom->getLibrary_physics_scenes_array()[i];
for (unsigned int s=0;s<physicsScenesRef->getPhysics_scene_array().getCount();s++)
{
domPhysics_sceneRef physicsSceneRef = physicsScenesRef->getPhysics_scene_array()[s];
if (physicsSceneRef->getTechnique_common())
{
if (physicsSceneRef->getTechnique_common()->getGravity())
{
const domFloat3 grav = physicsSceneRef->getTechnique_common()->getGravity()->getValue();
printf("gravity set to %f,%f,%f\n",grav.get(0),grav.get(1),grav.get(2));
setGravity(CrtVec3f((float)grav.get(0),(float)grav.get(1),(float)grav.get(2)));
}
}
for (unsigned int ps=0;ps<physicsSceneRef->getInstance_physics_model_array().getCount();ps++)
{
domInstance_physics_modelRef instance_physicsModelRef = physicsSceneRef->getInstance_physics_model_array()[ps];
daeElementRef ref = instance_physicsModelRef->getUrl().getElement();
domPhysics_modelRef model = *(domPhysics_modelRef*)&ref;
unsigned int p,r;
for ( p=0;p<model->getInstance_physics_model_array().getCount();p++)
{
domInstance_physics_modelRef instancePhysicsModelRef = model->getInstance_physics_model_array()[p];
daeElementRef ref = instancePhysicsModelRef->getUrl().getElement();
domPhysics_modelRef model = *(domPhysics_modelRef*)&ref;
//todo: group some shared functionality in following 2 'blocks'.
for (r=0;r<instancePhysicsModelRef->getInstance_rigid_body_array().getCount();r++)
{
domInstance_rigid_bodyRef instRigidbodyRef = instancePhysicsModelRef->getInstance_rigid_body_array()[r];
btScalar mass = 1.f;
bool isDynamics = true;
btCollisionShape* colShape = 0;
btCompoundShape* compoundShape = 0;
xsNCName bodyName = instRigidbodyRef->getBody();
domInstance_rigid_body::domTechnique_commonRef techniqueRef = instRigidbodyRef->getTechnique_common();
if (techniqueRef)
{
if (techniqueRef->getMass())
{
mass = (btScalar)techniqueRef->getMass()->getValue();
}
if (techniqueRef->getDynamic())
{
isDynamics = techniqueRef->getDynamic()->getValue();
}
}
if (bodyName && model)
{
//try to find the rigid body
for (unsigned int r=0;r<model->getRigid_body_array().getCount();r++)
{
domRigid_bodyRef rigidBodyRef = model->getRigid_body_array()[r];
if (rigidBodyRef->getSid() && !strcmp(rigidBodyRef->getSid(),bodyName))
{
btRigidBodyOutput output;
output.m_colShape = colShape;
output.m_compoundShape = compoundShape;
output.m_mass = 1.f;
output.m_isDynamics = true;
btRigidBodyInput rbInput;
rbInput.m_rigidBodyRef2 = rigidBodyRef;
rbInput.m_instanceRigidBodyRef = instRigidbodyRef;
ConvertRigidBodyRef( rbInput , output );
mass = output.m_mass;
isDynamics = output.m_isDynamics;
colShape = output.m_colShape;
compoundShape = output.m_compoundShape;
}
}
//////////////////////
}
if (compoundShape)
colShape = compoundShape;
if (colShape)
{
btRigidBodyInput input;
input.m_instanceRigidBodyRef = instRigidbodyRef;
input.m_rigidBodyRef2 = 0;
input.m_bodyName = (char*)bodyName;
PreparePhysicsObject(input, isDynamics,mass,colShape);
}
}
}
for (r=0;r<instance_physicsModelRef->getInstance_rigid_body_array().getCount();r++)
{
domInstance_rigid_bodyRef instRigidbodyRef = instance_physicsModelRef->getInstance_rigid_body_array()[r];
btScalar mass = 1.f;
bool isDynamics = true;
btCollisionShape* colShape = 0;
btCompoundShape* compoundShape = 0;
xsNCName bodyName = instRigidbodyRef->getBody();
domInstance_rigid_body::domTechnique_commonRef techniqueRef = instRigidbodyRef->getTechnique_common();
if (techniqueRef)
{
if (techniqueRef->getMass())
{
mass = (btScalar)techniqueRef->getMass()->getValue();
}
if (techniqueRef->getDynamic())
{
isDynamics = techniqueRef->getDynamic()->getValue();
}
}
if (bodyName && model)
{
//try to find the rigid body
for (unsigned int r=0;r<model->getRigid_body_array().getCount();r++)
{
domRigid_bodyRef rigidBodyRef = model->getRigid_body_array()[r];
if (rigidBodyRef->getSid() && !strcmp(rigidBodyRef->getSid(),bodyName))
{
btRigidBodyOutput output;
output.m_colShape = colShape;
output.m_compoundShape = compoundShape;
output.m_mass = 1.f;
output.m_isDynamics = true;
btRigidBodyInput rbInput;
rbInput.m_rigidBodyRef2 = rigidBodyRef;
rbInput.m_instanceRigidBodyRef = instRigidbodyRef;
ConvertRigidBodyRef( rbInput , output );
mass = output.m_mass;
isDynamics = output.m_isDynamics;
colShape = output.m_colShape;
compoundShape = output.m_compoundShape;
}
}
//////////////////////
}
if (compoundShape)
colShape = compoundShape;
if (colShape)
{
btRigidBodyInput input;
input.m_instanceRigidBodyRef = instRigidbodyRef;
input.m_rigidBodyRef2 = 0;
input.m_bodyName = (char*)bodyName;
PreparePhysicsObject(input, isDynamics,mass,colShape);
}
} //for each instance_rigid_body
} //for each physics model
//handle constraints
for (unsigned int ma=0;ma<physicsSceneRef->getInstance_physics_model_array().getCount();ma++)
{
domInstance_physics_modelRef instance_physicsModelRef = physicsSceneRef->getInstance_physics_model_array()[ma];
daeElementRef ref = instance_physicsModelRef->getUrl().getElement();
domPhysics_modelRef model = *(domPhysics_modelRef*)&ref;
{
ConstraintInput cInput;
cInput.m_instance_physicsModelRef = instance_physicsModelRef;
cInput.m_model = model;
prepareConstraints(cInput);
}
//also don't forget the model's 'instance_physics_models!
for ( unsigned int p=0;p<model->getInstance_physics_model_array().getCount();p++)
{
domInstance_physics_modelRef instancePhysicsModelRef = model->getInstance_physics_model_array()[p];
daeElementRef ref = instancePhysicsModelRef->getUrl().getElement();
domPhysics_modelRef model = *(domPhysics_modelRef*)&ref;
ConstraintInput cInput;
cInput.m_instance_physicsModelRef = instancePhysicsModelRef;
cInput.m_model = model;
prepareConstraints(cInput);
}
} //2nd time, for each physics model
}
}
return true;
}
