图像处理3--摄像头标定

Love The Way You Lie 2022-06-18 09:11 255阅读 0赞

这几天在做摄像机标定，主要参考了opencv的例程，但是上面还是有一些Bug的，另外摄像头读取的方式改用的directshow的CameraDS类。  
一、摄像机标定基本步骤  
道具的准备：有photoshop、画图之类的公工具自制棋盘，采用A3或A4打印出来，贴到电脑旁的某个位置。  
1 新建一个模拟棋盘，标明横向和竖向的角点个数  
我绘制的棋盘角点6\*7  
2 初始化参数，4个内参数，以及畸变参数  
3 打开摄像头，获取一帧图像  
这里采用Directshow的读取方式，便于向双目的转换。  
4 寻找角点并绘出，当找出全部角点（6\*7）时，认为该图片有效，存储图片  
5每找出一张图片就进行一次标定，更新参数  
6对3、4、5循环经过20次之后，显示并存储参数  
7对最后一张图片进行校正  
  
  
做了十几组的实验，存储了每次求出的结果，用\#分割，放在data.txt中。  
有数据可知，摄像头移动范围和图片模糊程度对精度影响较大，假如摄像头移动范围较窄或者晃动的迅速，会导致结果的偏差偏大。  
  
  
正常情况下，fx，fy很接近会在490-502附近浮动。  
  
  
后来尝试，每次的初始化参数使用上一次求出的最终值，即每次程序结束时存储fx，fy，cx，cy的值（input.txt），新一次标定时，首先读取input.txt，进行初始化  
但是结果发现并非是收敛的，依然是在一定范围内浮动。  
二、配置  
将CameraDS.cp和CameraDS.h以及Directshow文件夹copy到工程文件夹下，进行opencv和directshow有关的配置  
opencv相关：  
1 菜单：Project->Settings，然后将Setting for选为All Configurations，然后选择右边的link标签，在Object/library modules附加上  
cxcore.lib cv.lib ml.lib cvaux.lib highgui.lib cvcam.lib  
directshow相关：  
1. 将CameraDS.h CameraDS.cpp以及目录DirectShow复制到你的项目中  
2. 菜单 Project->Settings->Settings for:(All configurations)->C/C++->Category(Preprocessor)->Additional include directories设置为 DirectShow/Include  
3. 菜单 Project->Settings->Settings for:(All configurations)->Link->Category(Input)->Additional library directories设置为 DirectShow/Lib  
三、运行结果 顺序 fx cx fy cy  
500.273193  
141.790436  
501.506165  
136.473099  
\#  
500.273193  
141.790436  
501.506165  
136.473099  
\#  
490.377533  
139.145706  
491.085449  
122.944351  
\#  
501.977661  
153.629349  
499.712006  
128.947861  
//以下是源程序：  
// 摄像头标定.cpp : Defines the entry point for the console application.

#include "CameraDS.h"
    #include <stdio.h>
    #include <stdlib.h>
    #include <string.h>
    #include <cv.h>
    #include <highgui.h>
    #include <cxcore.h>
    #include <cvaux.h>
    void InitCorners3D(CvMat *Corners3D, CvSize ChessBoardSize, int Nimages, float SquareSize);//初始化？？
    void makeChessBoard();//绘制棋盘
    int myFindChessboardCorners( const void* image, CvSize pattern_size,CvPoint2D32f* corners, int* corner_count=NULL,int flags=CV_CALIB_CB_ADAPTIVE_THRESH );
    inline int drawCorssMark(IplImage *dst,CvPoint pt)
    /***************************************************
    Function:        main_loop
    Description:     绘制一个十字标记                    
    Calls:          
    Called By:      
    Input:           RGB image,  pt               
    Output:         
    Return:         
    Others:          需要检查坐标是否越界 to do list
    *************************************************/
    {
    const int cross_len = 4;
    CvPoint pt1,pt2,pt3,pt4;
    pt1.x = pt.x;
    pt1.y = pt.y - cross_len;
    pt2.x = pt.x;
    pt2.y = pt.y + cross_len;
    pt3.x = pt.x - cross_len;
    pt3.y = pt.y;
    pt4.x = pt.x + cross_len;
    pt4.y = pt.y;
    cvLine(dst,pt1,pt2,CV_RGB(0,255,0),2,CV_AA, 0 );    
    cvLine(dst,pt3,pt4,CV_RGB(0,255,0),2,CV_AA, 0 );
    return 0;
    }
    /*** declarations for OpenCV */
    IplImage                 *current_frame_rgb,grid; 
    IplImage                 *current_frame_gray;
    IplImage                 *chessBoard_Img;
    int                       Thresholdness = 120;
    int image_width = 320;
    int image_height = 240;//图像长宽指定，在启动摄像头时需自己手动选择，或者在在代码中指定
    bool verbose = false; 
    const int ChessBoardSize_w = 6;
    const int ChessBoardSize_h = 7; //棋盘长宽
    // Calibration stuff
    bool   calibration_done = false;
    const CvSize   ChessBoardSize = cvSize(ChessBoardSize_w,ChessBoardSize_h);
    //float    SquareWidth = 21.6f; //实际距离 毫米单位 在A4纸上为两厘米
    float   SquareWidth = 17; //投影实际距离 毫米单位  200
    const   int NPoints = ChessBoardSize_w*ChessBoardSize_h;
    const   int NImages = 20; //Number of images to collect 
    CvPoint2D32f corners[NPoints*NImages];
    int corner_count[NImages] ={0};
    int captured_frames = 0;
    CvMat *intrinsics; //内参数
    CvMat *distortion_coeff;
    CvMat *rotation_vectors;//旋转向量
    CvMat *translation_vectors;//平移向量
    CvMat *object_points;
    CvMat *point_counts;//点计数
    CvMat *image_points;
    int find_corners_result =0 ;
    FILE * pdata;
    FILE *pinput;
    void on_mouse( int event, int x, int y, int flags, void* param )
    {
    if( event == CV_EVENT_LBUTTONDOWN )
    {
    //calibration_done = true; 
    }
    }
    int main() //为何是_tmain()是只是unicode字符集的一个开始函数。
    {
    CvFont font;
    cvInitFont( &font, CV_FONT_VECTOR0,5, 5, 0, 7, 8);
    intrinsics         = cvCreateMat(3,3,CV_32FC1);
    distortion_coeff     = cvCreateMat(1,4,CV_32FC1);
    rotation_vectors     = cvCreateMat(NImages,3,CV_32FC1);
    translation_vectors     = cvCreateMat(NImages,3,CV_32FC1);
    point_counts         = cvCreateMat(NImages,1,CV_32SC1);
    object_points     = cvCreateMat(NImages*NPoints,3,CV_32FC1);
    image_points         = cvCreateMat(NImages*NPoints,2,CV_32FC1);
    // Function to fill in the real-world points of the checkerboard
    InitCorners3D(object_points, ChessBoardSize, NImages, SquareWidth);
    CvCapture* capture = 0;
    /
    // Initialize all of the IplImage structures
    current_frame_rgb = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);
    IplImage *current_frame_rgb2 = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);
    current_frame_gray = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 1);
    chessBoard_Img   = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);  
    current_frame_rgb2->origin = chessBoard_Img->origin  = current_frame_gray->origin = current_frame_rgb->origin = 1;
    makeChessBoard();
    cvNamedWindow( "result", 0);
    cvNamedWindow( "Window 0", 0);
    cvNamedWindow( "grid", 0);
    cvMoveWindow( "grid", 100,100);
    cvSetMouseCallback( "Window 0", on_mouse, 0 );  
    cvCreateTrackbar("Thresholdness","Window 0",&Thresholdness, 255,0);
    
    int cam_count;
    //仅仅获取摄像头数目
    cam_count = CCameraDS::CameraCount();
    printf("There are %d cameras.\n", cam_count);
    //获取所有摄像头的名称
    for(int i=0; i < cam_count; i++)
    {
    char camera_name[1024];  
    int retval = CCameraDS::CameraName(i, camera_name, sizeof(camera_name) );
    if(retval >0)
    printf("Camera #%d's Name is '%s'.\n", i, camera_name);
    else
    printf("Can not get Camera #%d's name.\n", i);
    }
    if(cam_count==0)
    return -1;
    CCameraDS camera;
    //打开第一个摄像头
    //if(! camera.OpenCamera(0, true)) //弹出属性选择窗口
    if(! camera.OpenCamera(1, false, 320,240)) //不弹出属性选择窗口，用代码制定图像宽和高
    {
    fprintf(stderr, "Can not open camera.\n");
    return -1;
    }
    cvNamedWindow("camera");
    while(!calibration_done)
    {
    while (captured_frames<NImages)
    {
    //获取一帧
    current_frame_rgb = camera.QueryFrame();
    //显示
    if (!current_frame_rgb)
    {
    break;
    }
    cvShowImage("camera",current_frame_rgb);
    cvCopy(current_frame_rgb,current_frame_rgb2);
    cvCvtColor(current_frame_rgb, current_frame_gray, CV_BGR2GRAY);
    //cvThreshold(current_frame_gray,current_frame_gray,Thresholdness,255,CV_THRESH_BINARY);
    //cvThreshold(current_frame_gray,current_frame_gray,150,255,CV_THRESH_BINARY_INV);
    find_corners_result = cvFindChessboardCorners(current_frame_gray,ChessBoardSize,&corners[captured_frames*NPoints],&corner_count[captured_frames],0);
    cvDrawChessboardCorners(current_frame_rgb2, ChessBoardSize, &corners[captured_frames*NPoints], NPoints, find_corners_result);
    cvShowImage("Window 0",current_frame_rgb2);
    cvShowImage("grid",chessBoard_Img);
    if(find_corners_result==1)
    {
    cvWaitKey(2000);
    cvSaveImage("c:\\hardyinCV.jpg",current_frame_rgb2);
    captured_frames++;
    }
    //cvShowImage("result",current_frame_gray);
    
    
      //intrinsics->data.fl[0] = 256.8093262;   //fx        
       //intrinsics->data.fl[2] = 160.2826538;   //cx
    //intrinsics->data.fl[4] = 254.7511139;   //fy
    //intrinsics->data.fl[5] = 127.6264572;   //cy
    float temp1,temp2,temp3,temp4;
    pinput = fopen("input.txt","r");
       fscanf(pinput,"%f\n",&temp1);
    fscanf(pinput,"%f\n",&temp2);
     fscanf(pinput,"%f\n",&temp3);
      fscanf(pinput,"%f\n",&temp4);
    intrinsics->data.fl[0] = temp1;   //fx        
    intrinsics->data.fl[2] = temp2;   //cx
    intrinsics->data.fl[4] = temp3;   //fy
    intrinsics->data.fl[5] = temp4;   //cy
    fclose(pinput);
    
    
    
    
    intrinsics->data.fl[1] = 0;   
    intrinsics->data.fl[3] = 0;   
    intrinsics->data.fl[6] = 0;   
    intrinsics->data.fl[7] = 0;   
    intrinsics->data.fl[8] = 1;       
    distortion_coeff->data.fl[0] = -0.193740;  //k1
    distortion_coeff->data.fl[1] = -0.378588;  //k2
    distortion_coeff->data.fl[2] = 0.028980;   //p1
    distortion_coeff->data.fl[3] = 0.008136;   //p2
    cvWaitKey(40);
    find_corners_result = 0;
    }
    // if (find_corners_result !=0)
    {
    printf(" ");
    cvSetData( image_points, corners, sizeof(CvPoint2D32f));
    cvSetData( point_counts, &corner_count, sizeof(int));
    cvCalibrateCamera2( object_points,image_points,point_counts,cvSize(image_width,image_height),intrinsics,distortion_coeff,rotation_vectors,translation_vectors,0);
    // [fx 0 cx; 0 fy cy; 0 0 1].
    // cvUndistort2(current_frame_rgb,current_frame_rgb,intrinsics,distortion_coeff);
    // cvShowImage("result",current_frame_rgb);
    cvUndistort2(current_frame_rgb,current_frame_rgb2,intrinsics,distortion_coeff);
    cvShowImage("result",current_frame_rgb2);
    float intr[3][3] = {0.0};
    float dist[4] = {0.0};
    float tranv[3] = {0.0};
    float rotv[3] = {0.0};
    for ( int i = 0; i < 3; i++)
    {
    for ( int j = 0; j < 3; j++)
    {
    intr[i][j] = ((float*)(intrinsics->data.ptr + intrinsics->step*i))[j];
    }
    dist[i] = ((float*)(distortion_coeff->data.ptr))[i];
    tranv[i] = ((float*)(translation_vectors->data.ptr))[i];
    rotv[i] = ((float*)(rotation_vectors->data.ptr))[i];
    }
    dist[3] = ((float*)(distortion_coeff->data.ptr))[3];
    printf("----------------------------------------- ");
    printf("INTRINSIC MATRIX:  ");
    printf("[ %6.4f %6.4f %6.4f ]  ", intr[0][0], intr[0][1], intr[0][2]);
    printf("[ %6.4f %6.4f %6.4f ]  ", intr[1][0], intr[1][1], intr[1][2]);
    printf("[ %6.4f %6.4f %6.4f ]  ", intr[2][0], intr[2][1], intr[2][2]);
    printf("----------------------------------------- ");
    printf("DISTORTION VECTOR:  ");
    printf("[ %6.4f %6.4f %6.4f %6.4f ]  ", dist[0], dist[1], dist[2], dist[3]);
    printf("----------------------------------------- ");
    printf("ROTATION VECTOR:  ");
    printf("[ %6.4f %6.4f %6.4f ]  ", rotv[0], rotv[1], rotv[2]);
    printf("TRANSLATION VECTOR:  ");
    printf("[ %6.4f %6.4f %6.4f ]  ", tranv[0], tranv[1], tranv[2]);
    printf("----------------------------------------- ");
    
    
    
    
    
    
    
    
    pdata = fopen("data.txt","a");
    fprintf(pdata,"%f\n",intr[0][0]);
    fprintf(pdata,"%f\n",intr[0][2]);
    fprintf(pdata,"%f\n",intr[1][1]);
    fprintf(pdata,"%f\n",intr[1][2]);
    
    fprintf(pdata,"%f\n",dist[0]);
    fprintf(pdata,"%f\n",dist[1]);
    fprintf(pdata,"%f\n",dist[2]);
    fprintf(pdata,"%f\n",dist[3]);
    
    fprintf(pdata,"%f\n",rotv[0]);
    fprintf(pdata,"%f\n",rotv[1]);
    fprintf(pdata,"%f\n",rotv[2]);
    
    
    fprintf(pdata,"%f\n",tranv[0]);
    fprintf(pdata,"%f\n",tranv[1]);
    fprintf(pdata,"%f\n",tranv[2]);
    fprintf(pdata,"%f\n",tranv[3]);
    fprintf(pdata,"%s\n","#");
    fclose(pdata);
    
    
    pinput = fopen("input.txt","w");
    fprintf(pinput,"%f\n",intr[0][0]);
    fprintf(pinput,"%f\n",intr[0][2]);
    fprintf(pinput,"%f\n",intr[1][1]);
    fprintf(pinput,"%f\n",intr[1][2]);
    fclose(pinput);
    
    cvWaitKey(0);
    calibration_done = true;      
    }
    }
    camera.CloseCamera(); //可不调用此函数，CCameraDS析构时会自动关闭摄像头
    cvDestroyWindow("camera");
    exit(0);
    cvDestroyAllWindows();
    return 0;
    }
    void InitCorners3D(CvMat *Corners3D, CvSize ChessBoardSize, int NImages, float SquareSize)
    {
    int CurrentImage = 0;
    int CurrentRow = 0;
    int CurrentColumn = 0;
    int NPoints = ChessBoardSize.height*ChessBoardSize.width;
    float * temppoints = new float[NImages*NPoints*3];
    // for now, assuming we're row-scanning
    for (CurrentImage = 0 ; CurrentImage < NImages ; CurrentImage++)
    {
    for (CurrentRow = 0; CurrentRow < ChessBoardSize.height; CurrentRow++)
    {
    for (CurrentColumn = 0; CurrentColumn < ChessBoardSize.width; CurrentColumn++)
    {
    temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize.width + CurrentColumn)*3]=(float)CurrentRow*SquareSize;
    temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize.width + CurrentColumn)*3+1]=(float)CurrentColumn*SquareSize;
    temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize.width + CurrentColumn)*3+2]=0.f;
    }
    }
    }
    (*Corners3D) = cvMat(NImages*NPoints,3,CV_32FC1, temppoints);
    }
    
    
    int myFindChessboardCorners( const void* image, CvSize pattern_size,CvPoint2D32f* corners, int* corner_count,int flags )
    {
    IplImage* eig = cvCreateImage( cvGetSize(image), 32, 1 );
    IplImage* temp = cvCreateImage( cvGetSize(image), 32, 1 );
    double quality = 0.01;
    double min_distance = 5;
    int win_size =10;
    int count = pattern_size.width * pattern_size.height;
    cvGoodFeaturesToTrack( image, eig, temp, corners, &count,quality, min_distance, 0, 3, 0, 0.04 );
    cvFindCornerSubPix( image, corners, count,
    cvSize(win_size,win_size), cvSize(-1,-1),
    cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
    cvReleaseImage( &eig );
    cvReleaseImage( &temp );
    return 1;
    }
    void makeChessBoard()
    {
    CvScalar e; 
    e.val[0] =255;
    e.val[1] =255;
    e.val[2] =255;
    cvSet(chessBoard_Img,e,0);
    for(int i = 0;i<ChessBoardSize.width+1;i++)
    {
    for(int j = 0;j<ChessBoardSize.height+1;j++)
    {
    int w =(image_width)/2/(ChessBoardSize.width);
    int h = w; //(image_height)/2/(ChessBoardSize.height);
    int ii = i+1;
    int iii = ii+1;
    int jj =j+1;
    int jjj =jj+1;
    int s_x = image_width/6;          
    if((i+j)%2==1)
    {
    cvRectangle( chessBoard_Img, cvPoint(w*i+s_x,h*j+s_x),cvPoint(w*ii-1+s_x,h*jj-1+s_x), CV_RGB(0,0,0),CV_FILLED, 8, 0 );
    }
    }
    }
    }

![0_1319243450JK9O.gif][]

[0_1319243450JK9O.gif]: /images/20220618/49cb1fdf3691420fadf973a40da2af8c.png