/* * File: StereoKitti.cpp * Author: carlos * * Created on 6 de junio de 2013, 18:13 */ #include "StereoKitti.h" StereoKitti::StereoKitti() { } StereoKitti::StereoKitti(const StereoKitti& orig) { } StereoKitti::~StereoKitti() { } int StereoKitti::readCalibrationFile(string calibFile) { int ret = 0; char data[500] = {0}; float dataFl[20] = {0}; int idCam = 0; char matName[100]; FILE *fd = fopen(calibFile.data(), "r"); string mats[] = {"S_", "K_", "D_", "R_", "T_", "S_rect_", "R_rect_", "P_rect_"}; int matId = 0; if (fd != NULL) { while (fscanf(fd, "%s", data) > 0) { sprintf(matName, "%s%02d:", mats[matId].data(), idCam); printf("***%s***\n", data); if (strcmp(matName, data) == 0) { switch (matId) { case 0: fscanf(fd, "%f %f", &dataFl[0], &dataFl[1]); this->m_frameWidth = (int) dataFl[0]; this->m_frameHeight = (int) dataFl[1]; break; case 1: fscanf(fd, "%f %f %f %f %f %f %f %f %f", &dataFl[0],&dataFl[1], &dataFl[2],&dataFl[3],&dataFl[4],&dataFl[5],&dataFl[6], &dataFl[7],&dataFl[8]); // calib_params->camera[0].matrix[0] = dataFl[0]; // calib_params->camera[0].matrix[1] = dataFl[1]; // calib_params->camera[0].matrix[2] = dataFl[2]; // calib_params->camera[0].matrix[3] = dataFl[3]; // calib_params->camera[0].matrix[4] = dataFl[4]; // calib_params->camera[0].matrix[5] = dataFl[5]; // calib_params->camera[0].matrix[6] = dataFl[6]; // calib_params->camera[0].matrix[7] = dataFl[7]; // calib_params->camera[0].matrix[8] = dataFl[8]; break; } matId++; } } printf("Calibration File Loaded.\n"); } else { // printf("Calibration File Not Found. %s\n", fileName.data()); ret = -1; } return ret; //% Escribe en fichero: "calib_estereo_param.dat" % //% - fxl, fyl, u0l, v0l (floats) % //% - fxr, fyr, u0r, v0r (floats) % //% - matriz fundamental [9] (floats) % //% - coeficientes distorsion left [4] (floats) % //% - coeficientes distorsion right [4] (floats) % //% - matriz rotaci�n est�reo [9] (floats) % //% - matriz traslaci�n est�reo [3] (floats) % //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // calib_params->camera[0].matrix[0] = data[0]; // calib_params->camera[0].matrix[1] = 0; // calib_params->camera[0].matrix[2] = data[2]; // calib_params->camera[0].matrix[3] = 0; // calib_params->camera[0].matrix[4] = data[1]; // calib_params->camera[0].matrix[5] = data[3]; // calib_params->camera[0].matrix[6] = 0; // calib_params->camera[0].matrix[7] = 0; // calib_params->camera[0].matrix[8] = 1; // // calib_params->camera[1].matrix[0] = data[4]; // calib_params->camera[1].matrix[1] = 0; // calib_params->camera[1].matrix[2] = data[6]; // calib_params->camera[1].matrix[3] = 0; // calib_params->camera[1].matrix[4] = data[5]; // calib_params->camera[1].matrix[5] = data[7]; // calib_params->camera[1].matrix[6] = 0; // calib_params->camera[1].matrix[7] = 0; // calib_params->camera[1].matrix[8] = 1; // // calib_params->fund_matrix[0] = data[8]; // calib_params->fund_matrix[1] = data[9]; // calib_params->fund_matrix[2] = data[10]; // calib_params->fund_matrix[3] = data[11]; // calib_params->fund_matrix[4] = data[12]; // calib_params->fund_matrix[5] = data[13]; // calib_params->fund_matrix[6] = data[14]; // calib_params->fund_matrix[7] = data[15]; // calib_params->fund_matrix[8] = data[16]; // // calib_params->camera[0].distortion[0] = data[17]; // calib_params->camera[0].distortion[1] = data[18]; // calib_params->camera[0].distortion[2] = data[19]; // calib_params->camera[0].distortion[3] = data[20]; // // calib_params->camera[1].distortion[0] = data[21]; // calib_params->camera[1].distortion[1] = data[22]; // calib_params->camera[1].distortion[2] = data[23]; // calib_params->camera[1].distortion[3] = data[24]; // // calib_params->rot_matrix[0] = data[25]; // calib_params->rot_matrix[1] = data[26]; // calib_params->rot_matrix[2] = data[27]; // calib_params->rot_matrix[3] = data[28]; // calib_params->rot_matrix[4] = data[29]; // calib_params->rot_matrix[5] = data[30]; // calib_params->rot_matrix[6] = data[31]; // calib_params->rot_matrix[7] = data[32]; // calib_params->rot_matrix[8] = data[33]; // // calib_params->trans_vector[0] = data[34]; // calib_params->trans_vector[1] = data[35]; // calib_params->trans_vector[2] = data[36]; // // calib_params->camera[0].img_size[0] = 640; // calib_params->camera[0].img_size[1] = 480; // // calib_params->camera[1].img_size[0] = 640; // calib_params->camera[1].img_size[1] = 480; // // printf("Stereo Calibration File Read. OK\n"); // return ret; } int StereoKitti::getPoint_3dImage(int u, int v, StereoKitti::t_Point* pt) { StereoKitti::t_Point point; int ret = -1; uchar* rgb_ptr = m_images_stereo.undist_left_image.ptr(v); double* xyz_ptr = m_images_stereo.recons3D.ptr(v); uchar vd = m_images_stereo.vdisp_image.at(v, u); if (vd != 0) { ret = 0; // Get RGB information. uchar pr, pg, pb; pb = rgb_ptr[m_images_stereo.undist_left_image.channels() * u]; pg = rgb_ptr[m_images_stereo.undist_left_image.channels() * u + 1]; pr = rgb_ptr[m_images_stereo.undist_left_image.channels() * u + 2]; double px, py, pz, pw; // px = xyz_ptr[m_images_stereo.recons3D.channels() * u]; // py = xyz_ptr[m_images_stereo.recons3D.channels() * u + 1]; // pz = xyz_ptr[m_images_stereo.recons3D.channels() * u + 2]; px = m_images_stereo.recons3D.at(v, u)[0]; py = m_images_stereo.recons3D.at(v, u)[1]; pz = m_images_stereo.recons3D.at(v, u)[2]; // Insert information into point cloud structure. point.x = -px / 1000.0; point.y = py / 1000.0; point.z = pz / 1000.0; // point.x = -px; // point.y = py; // point.z = pz; double xx, yy, zz; xx = -point.z; yy = -point.x; zz = point.y; point.x = xx; point.y = yy; point.z = zz; //sumar altura camara point.r = pr; point.g = pg; point.b = pb; // point.r = 255; // point.g = 255; // point.b = 255; point.u = u; point.v = v; // printf("(u,v)(%d,%d) = %.02lf %.02lf %.02lf (%d %d %d)\n",u,v,xx,yy,zz,pr,pg,pb); *pt = point; } return ret; }