/* gets the calibration of the camera from a file */ void myprogeo::load_cam(char *fich_in,int cam, int w, int h) { //FILE *entrada; //int i; if (strlen(fich_in) ==0 ){ std::cout << w << ", " << h << std::endl; this->cameras[cam].fdistx=515; this->cameras[cam].fdisty=515; this->cameras[cam].u0=h/2; this->cameras[cam].v0=w/2; this->cameras[cam].position.X=0; this->cameras[cam].position.Y=0; this->cameras[cam].position.Z=0; this->cameras[cam].foa.X=0; this->cameras[cam].foa.Y=1; this->cameras[cam].foa.Z=0; this->cameras[cam].skew=0; this->cameras[cam].roll=0; update_camera_matrix(&cameras[cam]); } else{ xmlReader(&(this->cameras[cam]), fich_in); update_camera_matrix(&cameras[cam]); } display_camerainfo(cameras[cam]); }
CameraConf::CameraConf(RoboCompJointMotor::JointMotorPrx jprx) { this->jprx = jprx; /*Set Extrinsecs camera parameters*/ this->camera.position.X=0.0; this->camera.position.Y=0.0; this->camera.position.Z=0.0; this->camera.position.H=1.0; this->camera.foa.X=0.0; this->camera.foa.Y=0.0; this->camera.foa.Z=0.0; this->camera.foa.H=1.0; this->camera.roll=0.0; /*Set Intrinsecs camera parameters*/ this->camera.u0=120; this->camera.v0=160; this->camera.fdistx=320.0; this->camera.fdisty=320.0; this->camera.skew=0.0; this->camera.rows = 240; this->camera.columns = 320; update_camera_matrix(&(this->camera)); }
void Controller::drawWorld(cv::Mat image, int cam) { cv::Point p, q; HPoint3D p1, p2; int n=0; /*Update camera with current values*/ update_camera_matrix(&(this->cameras[cam])); for(vector<HPoint3D>::iterator it = this->lines.begin(); it != this->lines.end(); it++) { if(n%2==0) { p1 = (*it); } else { p2 = (*it); this->drawLine(image, p1, p2, cam); } n++; } /*Draw central point*/ if(this->drawCenter) { p.x = cWidth/2; p.y = 0; q.x = cWidth/2; q.y = cHeight-1; cv::line(image, p, q, cv::Scalar(255,0,0), 1, CV_AA, 0); p.x = 0; p.y = cHeight/2; q.x = cWidth-1; q.y = cHeight/2; cv::line(image, p, q, cv::Scalar(255,0,0), 1, CV_AA, 0); } }
// UPDATE CAMERA (ANGLE, SPEEDS, POSITION, MATRIX, FRUSTRUM) inline void update_camera(CAMERA *cam) { update_camera_ang(cam); update_camera_speeds(cam); update_camera_pos(cam); update_camera_matrix(cam); update_camera_frustrum(cam); }
void Module_DLT::init(Ice::PropertiesPtr prop) { /*Default values for camera parameters*/ this->camera.position.X = 0.; this->camera.position.Y = 0.; this->camera.position.Z = 0.; this->camera.position.H = 1.; this->camera.foa.X = 0.; this->camera.foa.Y = 0.; this->camera.foa.Z = 0.; this->camera.foa.H = 1.; this->camera.fdistx = 300.0; this->camera.fdisty = 300.0; this->camera.v0 = IMAGE_WIDTH/2; this->camera.u0 = IMAGE_HEIGHT/2; this->camera.roll = 0.0; this->camera.columns = IMAGE_WIDTH; this->camera.rows = IMAGE_HEIGHT; this->camera.skew = 0.0; update_camera_matrix(&(this->camera)); // init K = gsl_matrix_alloc(3,3); R = gsl_matrix_alloc(3,3); X = gsl_vector_alloc(3); }
void Module_DLT::on_calibrate_button_clicked() { if (this->getCounterPointsImage() == NUM_POINTSS){ std::cout << "Clic calibrate: " << std::endl; this->solve_equation_system(); drawKRTMatrix(this->K, this->R, this->X, k, r, x); /* for (int i=0; i<9; i++) { std::cout << this->solution_matrix[i] << std::endl; std::stringstream labelString; labelString << solution_matrix[i]; k[i]->set_label(labelString.str()); } */ }else{ std::cout << "Tienes que selecionar todos los puntos" << std::endl; } std::cout << "Información invertida de la cámara" << std::endl; display_camerainfo(this->camera); /*Update camera with current values*/ update_camera_matrix(&(this->camera)); display_camerainfo(this->camera); reverse_update_camera_matrix(&(this->camera)); display_camerainfo(this->camera); }
void Controller::init(Ice::PropertiesPtr prop, int nCameras) { int i=0; FILE *worldconfig; /* initializes the 3D world */ worldconfig=fopen(this->world.c_str(),"r"); if(worldconfig==NULL){ cerr << "Calibrator: World configuration configuration file " << this->world << " does not exits" << endl; }else{ do{ i=load_world_line(worldconfig); } while(i!=EOF); fclose(worldconfig); } this->load_world(); for (int cam=0; cam< nCameras; cam++){ /*Default values for camera parameters*/ this->cameras[cam].position.X = 0.; this->cameras[cam].position.Y = 0.; this->cameras[cam].position.Z = 0.; this->cameras[cam].position.H = 1.; this->cameras[cam].foa.X = 0.; this->cameras[cam].foa.Y = 0.; this->cameras[cam].foa.Z = 0.; this->cameras[cam].foa.H = 1.; this->cameras[cam].fdistx = 300.0; this->cameras[cam].fdisty = 300.0; this->cameras[cam].v0 = cWidth/2; this->cameras[cam].u0 = cHeight/2; this->cameras[cam].roll = 0.0; this->cameras[cam].columns = cWidth; this->cameras[cam].rows = cHeight; this->cameras[cam].skew = 0.0; std::ostringstream strInd; strInd << "rgbdManualCalibrator.Camera." << cam << ".Calibration"; std::string camera = prop->getProperty(strInd.str()); xmlReader(&(this->cameras[cam]), camera.c_str()); //old calibration /*Ice::PropertyDict pd = prop->getPropertiesForPrefix("rgbdManualCalibrator.Config"); this->load_camera_config(pd, cam);*/ //newOne update_camera_matrix(&(this->cameras[cam])); } this->lastx=0.; this->lasty=0.; this->lastz=0.; /*Load parameters from file*/ /* for the undo operation be ready from the very beginning */ /*save_cam(cameraOUTfile);*/ }
void CameraConf::calcCameraPos() { RoboCompJointMotor::MotorStateMap motorsstate; gsl_matrix * init, * motor_pan, * RT_pan; gsl_matrix * neck, * RT_neck, * motor_tilt, *RT_tilt; gsl_matrix * cam_right, * RT_cam_right; gsl_matrix * foa_rel, * foa; float pan_value, tilt_value; init = gsl_matrix_alloc(4,4); motor_pan = gsl_matrix_alloc(4,4); RT_pan = gsl_matrix_alloc(4,4); neck = gsl_matrix_alloc(4,4); RT_neck = gsl_matrix_alloc(4,4); motor_tilt = gsl_matrix_alloc(4,4); RT_tilt = gsl_matrix_alloc(4,4); cam_right = gsl_matrix_alloc(4,4); RT_cam_right = gsl_matrix_alloc(4,4); foa_rel = gsl_matrix_alloc(4,1); foa = gsl_matrix_alloc(4,1); /*Get motors state*/ jprx->getAllMotorState(motorsstate); pan_value = motorsstate["neck"].pos; tilt_value = -motorsstate["tilt"].pos; /*Initial matrix*/ gsl_matrix_set(init,0,0,1.0); gsl_matrix_set(init,0,1,0.0); gsl_matrix_set(init,0,2,0.0); gsl_matrix_set(init,0,3,INIT_X); //Profundidad gsl_matrix_set(init,1,0,0.0); gsl_matrix_set(init,1,1,1.0); gsl_matrix_set(init,1,2,0.0); gsl_matrix_set(init,1,3,INIT_Y); //Lateral (izq positivo) gsl_matrix_set(init,2,0,0.0); gsl_matrix_set(init,2,1,0.0); gsl_matrix_set(init,2,2,1.0); gsl_matrix_set(init,2,3,INIT_Z); //Altura gsl_matrix_set(init,3,0,0.0); gsl_matrix_set(init,3,1,0.0); gsl_matrix_set(init,3,2,0.0); gsl_matrix_set(init,3,3,1.0); /*Pan motor turn in Z axis*/ gsl_matrix_set(motor_pan,0,0,cos(pan_value)); gsl_matrix_set(motor_pan,0,1,-sin(pan_value)); gsl_matrix_set(motor_pan,0,2,0.0); gsl_matrix_set(motor_pan,0,3,0.0); //Profundidad gsl_matrix_set(motor_pan,1,0,sin(pan_value)); gsl_matrix_set(motor_pan,1,1,cos(pan_value)); gsl_matrix_set(motor_pan,1,2,0.0); gsl_matrix_set(motor_pan,1,3,0.0); //Lateral (izq positivo) gsl_matrix_set(motor_pan,2,0,0.0); gsl_matrix_set(motor_pan,2,1,0.0); gsl_matrix_set(motor_pan,2,2,1.0); gsl_matrix_set(motor_pan,2,3,0.0); //Altura gsl_matrix_set(motor_pan,3,0,0.0); gsl_matrix_set(motor_pan,3,1,0.0); gsl_matrix_set(motor_pan,3,2,0.0); gsl_matrix_set(motor_pan,3,3,1.0); gsl_linalg_matmult(init, motor_pan, RT_pan); /*Neck length*/ gsl_matrix_set(neck,0,0,1.0); gsl_matrix_set(neck,0,1,0.0); gsl_matrix_set(neck,0,2,0.0); gsl_matrix_set(neck,0,3,0.0); //Profundidad gsl_matrix_set(neck,1,0,0.0); gsl_matrix_set(neck,1,1,1.0); gsl_matrix_set(neck,1,2,0.0); gsl_matrix_set(neck,1,3,0.0); //Lateral (izq positivo) gsl_matrix_set(neck,2,0,0.0); gsl_matrix_set(neck,2,1,0.0); gsl_matrix_set(neck,2,2,1.0); gsl_matrix_set(neck,2,3,NECK_LENGTH); //Altura gsl_matrix_set(neck,3,0,0.0); gsl_matrix_set(neck,3,1,0.0); gsl_matrix_set(neck,3,2,0.0); gsl_matrix_set(neck,3,3,1.0); gsl_linalg_matmult(RT_pan, neck, RT_neck); /*Tilt motor turn in Y axis*/ gsl_matrix_set(motor_tilt,0,0,cos(tilt_value)); gsl_matrix_set(motor_tilt,0,1,0.0); gsl_matrix_set(motor_tilt,0,2,sin(tilt_value)); gsl_matrix_set(motor_tilt,0,3,0.0); //Profundidad gsl_matrix_set(motor_tilt,1,0,0.0); gsl_matrix_set(motor_tilt,1,1,1.0); gsl_matrix_set(motor_tilt,1,2,0.0); gsl_matrix_set(motor_tilt,1,3,0.0); //Lateral (izq positivo) gsl_matrix_set(motor_tilt,2,0,-sin(tilt_value)); gsl_matrix_set(motor_tilt,2,1,0.0); gsl_matrix_set(motor_tilt,2,2,cos(tilt_value)); gsl_matrix_set(motor_tilt,2,3,0.0); //Altura gsl_matrix_set(motor_tilt,3,0,0.0); gsl_matrix_set(motor_tilt,3,1,0.0); gsl_matrix_set(motor_tilt,3,2,0.0); gsl_matrix_set(motor_tilt,3,3,1.0); gsl_linalg_matmult(RT_neck, motor_tilt, RT_tilt); /*Camera right position*/ gsl_matrix_set(cam_right,0,0,1.0); gsl_matrix_set(cam_right,0,1,0.0); gsl_matrix_set(cam_right,0,2,0.0); gsl_matrix_set(cam_right,0,3,CAM_RIGHT_X); //Profundidad gsl_matrix_set(cam_right,1,0,0.0); gsl_matrix_set(cam_right,1,1,1.0); gsl_matrix_set(cam_right,1,2,0.0); gsl_matrix_set(cam_right,1,3,CAM_RIGHT_Y); //Lateral (izq positivo) gsl_matrix_set(cam_right,2,0,0.0); gsl_matrix_set(cam_right,2,1,0.0); gsl_matrix_set(cam_right,2,2,1.0); gsl_matrix_set(cam_right,2,3,CAM_RIGHT_Z); //Altura gsl_matrix_set(cam_right,3,0,0.0); gsl_matrix_set(cam_right,3,1,0.0); gsl_matrix_set(cam_right,3,2,0.0); gsl_matrix_set(cam_right,3,3,1.0); gsl_linalg_matmult(RT_tilt, cam_right, RT_cam_right); /*Calc foa (1 meter in front of the camera*/ gsl_matrix_set(foa_rel,0,0,1.0); gsl_matrix_set(foa_rel,1,0,0.0); gsl_matrix_set(foa_rel,2,0,0.0); gsl_matrix_set(foa_rel,3,0,1.0); gsl_linalg_matmult(RT_cam_right,foa_rel,foa); /*Update extrinsecs camera parameters*/ this->camera.position.X=(float)gsl_matrix_get(RT_cam_right,0,3)*1000; this->camera.position.Y=(float)gsl_matrix_get(RT_cam_right,1,3)*1000; this->camera.position.Z=(float)gsl_matrix_get(RT_cam_right,2,3)*1000; this->camera.position.H=1.0; this->camera.foa.X=(float)gsl_matrix_get(foa,0,0)*1000; this->camera.foa.Y=(float)gsl_matrix_get(foa,1,0)*1000; this->camera.foa.Z=(float)gsl_matrix_get(foa,2,0)*1000; this->camera.foa.H=1.0; this->camera.roll=0.0; update_camera_matrix(&(this->camera)); gsl_matrix_free(init); gsl_matrix_free(motor_pan); gsl_matrix_free(RT_pan); gsl_matrix_free(neck); gsl_matrix_free(RT_neck); gsl_matrix_free(motor_tilt); gsl_matrix_free(RT_tilt); gsl_matrix_free(cam_right); gsl_matrix_free(RT_cam_right); gsl_matrix_free(foa_rel); gsl_matrix_free(foa); }