/* 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);
}
