自制摄像头+鱼眼镜头校正+极线校正+直接输出双目视频

来源：互联网发布：淘宝客服销售技巧编辑：程序博客网时间：2024/06/08 12:04

#include"stdafx.h"#include <iostream>  #include <cv.hpp>  #include <opencv2/highgui/highgui.hpp>#include <opencv2/imgproc/imgproc.hpp>#include <opencv2/core/core.hpp>#include"ocam_functions.cpp"using namespace cv;using namespace std;void main(){struct ocam_model ocam_left, ocam_right; // our ocam_models for the fisheye and catadioptric camerasget_ocam_model(&ocam_left, "./left_calib_results.txt");get_ocam_model(&ocam_right, "./right_calib_results.txt");int width = 1280;int height = 720;IplImage *dst_persp_left = cvCreateImage(cvSize(height, width), 8, 3);   // undistorted perspective and panoramic imageIplImage *dst_persp_right = cvCreateImage(cvSize(height, width), 8, 3);CvMat* left_mapx_persp = cvCreateMat(width, height, CV_32FC1);CvMat* left_mapy_persp = cvCreateMat(width, height, CV_32FC1);CvMat* right_mapx_persp = cvCreateMat(width, height, CV_32FC1);CvMat* right_mapy_persp = cvCreateMat(width, height, CV_32FC1);float sf = 5;//DWORD dwStart = timeGetTime();create_perspecive_undistortion_LUT(left_mapx_persp, left_mapy_persp, &ocam_left, sf);create_perspecive_undistortion_LUT(right_mapx_persp, right_mapy_persp, &ocam_right, sf);//DWORD dwEnd = timeGetTime();//cout<<"time "<<dwEnd-dwStart<<" ms"<<endl;Mat rmap[2][2];FileStorage fs("remap.yml", CV_STORAGE_READ);if (fs.isOpened()){fs["MapLx"] >> rmap[0][0];fs["MapLy"] >> rmap[0][1];fs["MapRx"] >> rmap[1][0];fs["MapRy"] >> rmap[1][1];fs.release();}elsecout << "Error: can not save the intrinsic parameters\n";VideoCapture camera(0);camera.set(CV_CAP_PROP_FRAME_WIDTH, width * 2);camera.set(CV_CAP_PROP_FRAME_HEIGHT, height);if (!camera.isOpened()) {cout << "Failed to open camera." << std::endl;return;}//Change the image to video and read frame from the cameraMat frame;Mat frame_L, frame_R;IplImage* Left;IplImage* Right;bool imsave = false;bool left_show = true;char img_name[256];int fn = 0;//cvNamedWindow("left", 0);//cvNamedWindow("right", 0);int w = 720, h = 1280;CvSize size = cvSize(2 * w, h);IplImage* src_resize = cvCreateImage(size, IPL_DEPTH_8U, 3); //创建视频 文件格式大小的图片CvVideoWriter *writer = 0;double fps = 25;writer = cvCreateVideoWriter("out4.avi", CV_FOURCC('X', 'V', 'I', 'D'), fps, cvSize(2 * w, h));    while (1) {if (!camera.read(frame)) {cout << "Failed to read frame" << std::endl;break;}frame_L = frame.rowRange(0, height).colRange(0, width);frame_R = frame.rowRange(0, height).colRange(width, width * 2);frame_L = frame_L.t();flip(frame_L, frame_L, 0);frame_R = frame_R.t();flip(frame_R, frame_R, 0);IplImage src1(frame_L);cvRemap(&src1, dst_persp_left, left_mapx_persp, left_mapy_persp, CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS, cvScalarAll(0));IplImage src2(frame_R);cvRemap(&src2, dst_persp_right, right_mapx_persp, right_mapy_persp, CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS, cvScalarAll(0));Mat saveimg1(dst_persp_left);Mat saveimg2(dst_persp_right);remap(saveimg1, frame_L, rmap[0][0], rmap[0][1], CV_INTER_LINEAR);remap(saveimg2, frame_R, rmap[1][0], rmap[1][1], CV_INTER_LINEAR);if (left_show){imshow("left", frame_L);}/*imshow("right", frame_R);*/char cc = waitKey(10);if (cc == 's'){imsave = true;}if (cc == 'q'){break;}if (imsave){destroyWindow("left");//IplImage* Left, *Right;IplImage im_temp = frame_L;            Left = cvCloneImage(&im_temp);IplImage im_temp1 = frame_L;Right = cvCloneImage(&im_temp1);cvSetImageROI(src_resize, cvRect(0, 0, w, h));cvCopy(Left, src_resize);cvResetImageROI(src_resize);cvSetImageROI(src_resize, cvRect(w, 0, w, h));cvCopy(Right, src_resize);cvResetImageROI(src_resize);cvNamedWindow("Myavi",0);cvShowImage("Myavi", src_resize);waitKey(10);cvWriteFrame(writer, src_resize); //保存图片为视频流格式  left_show = false;}}cvReleaseImage(&Left);cvReleaseImage(&Right);cvReleaseImage(&src_resize);camera.release();cvReleaseImage(&dst_persp_left);cvReleaseImage(&dst_persp_right);cvReleaseMat(&left_mapx_persp);cvReleaseMat(&left_mapy_persp);cvReleaseMat(&right_mapx_persp);cvReleaseMat(&right_mapy_persp);}

<span style="font-size:24px;color:#66ff99;">上面的源程序为读取鱼眼广角相机获取的双目视频，并且实时的采集，实时校正，最后将其保存为视频，供后续双目立体计算使用；</span>

<span style="font-size:24px;color:#66ff99;">下面贴出需要的ocam_functions.cpp文件：</span>

#include "ocam_functions.h"#define M_PI 3.1415926//------------------------------------------------------------------------------int get_ocam_model(struct ocam_model *myocam_model, char *filename){ double *pol        = myocam_model->pol; double *invpol     = myocam_model->invpol;  double *xc         = &(myocam_model->xc); double *yc         = &(myocam_model->yc);  double *c          = &(myocam_model->c); double *d          = &(myocam_model->d); double *e          = &(myocam_model->e); int    *width      = &(myocam_model->width); int    *height     = &(myocam_model->height); int *length_pol    = &(myocam_model->length_pol); int *length_invpol = &(myocam_model->length_invpol); FILE *f; char buf[CMV_MAX_BUF]; int i;  //Open file if(!(f=fopen(filename,"r"))) {   printf("File %s cannot be opened\n", filename);<span style="white-space:pre"></span>     return -1; }  //Read polynomial coefficients fgets(buf,CMV_MAX_BUF,f); fscanf(f,"\n"); fscanf(f,"%d", length_pol); for (i = 0; i < *length_pol; i++) {     fscanf(f," %lf",&pol[i]); } //Read inverse polynomial coefficients fscanf(f,"\n"); fgets(buf,CMV_MAX_BUF,f); fscanf(f,"\n"); fscanf(f,"%d", length_invpol); for (i = 0; i < *length_invpol; i++) {     fscanf(f," %lf",&invpol[i]); }  //Read center coordinates fscanf(f,"\n"); fgets(buf,CMV_MAX_BUF,f); fscanf(f,"\n"); fscanf(f,"%lf %lf\n", xc, yc); //Read affine coefficients fgets(buf,CMV_MAX_BUF,f); fscanf(f,"\n"); fscanf(f,"%lf %lf %lf\n", c,d,e); //Read image size fgets(buf,CMV_MAX_BUF,f); fscanf(f,"\n"); fscanf(f,"%d %d", height, width); fclose(f); return 0;}//------------------------------------------------------------------------------void cam2world(double point3D[3], double point2D[2], struct ocam_model *myocam_model){ double *pol    = myocam_model->pol; double xc      = (myocam_model->xc); double yc      = (myocam_model->yc);  double c       = (myocam_model->c); double d       = (myocam_model->d); double e       = (myocam_model->e); int length_pol = (myocam_model->length_pol);  double invdet  = 1/(c-d*e); // 1/det(A), where A = [c,d;e,1] as in the Matlab file double xp = invdet*(    (point2D[0] - xc) - d*(point2D[1] - yc) ); double yp = invdet*( -e*(point2D[0] - xc) + c*(point2D[1] - yc) );   double r   = sqrt(  xp*xp + yp*yp ); //distance [pixels] of  the point from the image center double zp  = pol[0]; double r_i = 1; int i;  for (i = 1; i < length_pol; i++) {   r_i *= r;   zp  += r_i*pol[i]; }  //normalize to unit norm double invnorm = 1/sqrt( xp*xp + yp*yp + zp*zp );  point3D[0] = invnorm*xp; point3D[1] = invnorm*yp;  point3D[2] = invnorm*zp;}//------------------------------------------------------------------------------void world2cam(double point2D[2], double point3D[3], struct ocam_model *myocam_model){ double *invpol     = myocam_model->invpol;  double xc          = (myocam_model->xc); double yc          = (myocam_model->yc);  double c           = (myocam_model->c); double d           = (myocam_model->d); double e           = (myocam_model->e); int    width       = (myocam_model->width); int    height      = (myocam_model->height); int length_invpol  = (myocam_model->length_invpol); double norm        = sqrt(point3D[0]*point3D[0] + point3D[1]*point3D[1]); double theta       = atan(point3D[2]/norm); double t, t_i; double rho, x, y; double invnorm; int i;    if (norm != 0)   {    invnorm = 1/norm;    t  = theta;    rho = invpol[0];    t_i = 1;    for (i = 1; i < length_invpol; i++)    {      t_i *= t;      rho += t_i*invpol[i];    }    x = point3D[0]*invnorm*rho;    y = point3D[1]*invnorm*rho;      point2D[0] = x*c + y*d + xc;    point2D[1] = x*e + y   + yc;  }  else  {    point2D[0] = xc;    point2D[1] = yc;  }}//------------------------------------------------------------------------------void create_perspecive_undistortion_LUT( CvMat *mapx, CvMat *mapy, struct ocam_model *ocam_model, float sf){     int i, j;     int width = mapx->cols; //New width     int height = mapx->rows;//New height          float *data_mapx = mapx->data.fl;     float *data_mapy = mapy->data.fl;     float Nxc = height/2.0;     float Nyc = width/2.0;     float Nz  = -width/sf;     double M[3];     double m[2];          for (i=0; i<height; i++)         for (j=0; j<width; j++)         {                M[0] = (i - Nxc);             M[1] = (j - Nyc);             M[2] = Nz;             world2cam(m, M, ocam_model);             *( data_mapx + i*width+j ) = (float) m[1];             *( data_mapy + i*width+j ) = (float) m[0];         }}//------------------------------------------------------------------------------void create_panoramic_undistortion_LUT ( CvMat *mapx, CvMat *mapy, float Rmin, float Rmax, float xc, float yc ){     int i, j;     float theta;     int width = mapx->width;     int height = mapx->height;          float *data_mapx = mapx->data.fl;     float *data_mapy = mapy->data.fl;     float rho;          for (i=0; i<height; i++)         for (j=0; j<width; j++)         {             theta = -((float)j)/width*2*M_PI; // Note, if you would like to flip the image, just inverte the sign of theta             rho   = Rmax - (Rmax-Rmin)/height*i;             *( data_mapx + i*width+j ) = yc + rho*sin(theta); //in OpenCV "x" is the             *( data_mapy + i*width+j ) = xc + rho*cos(theta);                      }}<span style="font-family: Arial, Helvetica, sans-serif;"></span>

<span style="font-size:24px;color:#3333ff;">以及上面代码需要的头文件ocam_function.h：</span>

<span style="font-family: Arial, Helvetica, sans-serif;">#include <stdlib.h></span>

#include <stdio.h>#include <float.h>#include <math.h>#include <cv.h>#include <highgui.h>#define CMV_MAX_BUF 1024#define MAX_POL_LENGTH 64struct ocam_model{  double pol[MAX_POL_LENGTH];    // the polynomial coefficients: pol[0] + x"pol[1] + x^2*pol[2] + ... + x^(N-1)*pol[N-1]  int length_pol;                // length of polynomial  double invpol[MAX_POL_LENGTH]; // the coefficients of the inverse polynomial  int length_invpol;             // length of inverse polynomial  double xc;         // row coordinate of the center  double yc;         // column coordinate of the center  double c;          // affine parameter  double d;          // affine parameter  double e;          // affine parameter  int width;         // image width  int height;        // image height};/*------------------------------------------------------------------------------ This function reads the parameters of the omnidirectional camera model from  a given TXT file------------------------------------------------------------------------------*/int get_ocam_model(struct ocam_model *myocam_model, char *filename);/*------------------------------------------------------------------------------ WORLD2CAM projects a 3D point on to the image    WORLD2CAM(POINT2D, POINT3D, OCAM_MODEL)     projects a 3D point (point3D) on to the image and returns the pixel coordinates (point2D).        POINT3D = [X;Y;Z] are the coordinates of the 3D point.    OCAM_MODEL is the model of the calibrated camera.    POINT2D = [rows;cols] are the pixel coordinates of the reprojected point        Copyright (C) 2009 DAVIDE SCARAMUZZA    Author: Davide Scaramuzza - email: davide.scaramuzza@ieee.org      NOTE: the coordinates of "point2D" and "center" are already according to the C    convention, that is, start from 0 instead than from 1.------------------------------------------------------------------------------*/void world2cam(double point2D[2], double point3D[3], struct ocam_model *myocam_model);/*------------------------------------------------------------------------------ CAM2WORLD projects a 2D point onto the unit sphere    CAM2WORLD(POINT3D, POINT2D, OCAM_MODEL)     back-projects a 2D point (point2D), in pixels coordinates,     onto the unit sphere returns the normalized coordinates point3D = [x;y;z]    where (x^2 + y^2 + z^2) = 1.        POINT3D = [X;Y;Z] are the coordinates of the 3D points, such that (x^2 + y^2 + z^2) = 1.    OCAM_MODEL is the model of the calibrated camera.    POINT2D = [rows;cols] are the pixel coordinates of the point in pixels        Copyright (C) 2009 DAVIDE SCARAMUZZA       Author: Davide Scaramuzza - email: davide.scaramuzza@ieee.org      NOTE: the coordinates of "point2D" and "center" are already according to the C    convention, that is, start from 0 instead than from 1.------------------------------------------------------------------------------*/void cam2world(double point3D[3], double point2D[2], struct ocam_model *myocam_model);/*------------------------------------------------------------------------------ Create Look Up Table for undistorting the image into a perspective image  It assumes the the final image plane is perpendicular to the camera axis------------------------------------------------------------------------------*/void create_perspecive_undistortion_LUT( CvMat *mapx, CvMat *mapy, struct ocam_model *ocam_model, float sf);/*------------------------------------------------------------------------------ Create Look Up Table for undistorting the image into a panoramic image  It computes a trasformation from cartesian to polar coordinates Therefore it does not need the calibration parameters The region to undistorted in contained between Rmin and Rmax xc, yc are the row and column coordinates of the image center------------------------------------------------------------------------------*/void create_panoramic_undistortion_LUT ( CvMat *mapx, CvMat *mapy, float Rmin, float Rmax, float xc, float yc );

1 0