OPENCV版本的單目標定示例程式碼（張正友）

【原文：http://blog.sina.com.cn/s/blog_73ef08a80100vi49.html】

攝像機的標定問題是機器視覺領域的入門問題，可以分為傳統的攝像機定標方法和攝像機自定標方法。定標的方法有很多中常見的有：Tsai（傳統）和張正友（介於傳統和自定標）等，

 

攝像機成像模型和四個座標系（通用原理）。

攝像機模型採用經典的小孔模型，如圖中Oc（光心），像面π表示的是視野平面，其到光心的距離為f（鏡頭焦距）。

四個座標系分別為：世界座標系（Ow），攝像機座標系（Oc），影象物理座標系（O1，單位mm），影象畫素座標系（O，位於視野平面的左上角，單位pix）。

空間某點P到其像點p的座標轉換過程主要是通過這四套座標系的三次轉換實現的，首先將世界座標系進行平移和轉換得到攝像機座標系，然後根據三角幾何變換得到影象物理座標系，最後根據畫素和公制單位的比率得到影象畫素座標系。（實際的應用過程是這個的逆過程，即由畫素長度獲知實際的長度）。

ps：通過攝像頭的標定，可以得到視野平面上的mm/pix解析度，對於視野平面以外的物體還是需要通過座標轉換得到視野平面上。

轉化的過程和公式參見：攝像機標定原理(關鍵是三個座標系).ppt

 

2 張正友演算法的原理

zhang法通過對一定標板在不同方向多次（三次以上）完整拍照，不需要知道定標板的運動方式。直接獲得相機的內參（參考文獻上矩陣A）和畸變係數。該標定方法精度高於自定標法，且不需要高精度的定位儀器。

ZHANG的演算法包含兩個模型：一.經典針孔模型，包含四個座標系，二畸變模型（這個來源未知）

公式三項依次表示，徑向畸變，切線畸變，薄稜鏡畸變。OPENCV中函式只能給出k1,k2,p1,p2。

還存在另外一種畸變模型，見《攝像機標定演算法庫的設計和試驗驗證》一文26 page。（也不知道出處）

ps:單從公式推導的過程來看，第一組公式等號右邊應該是U0。

Key：這個方程怎麼求？x,y 代表理想的影象座標（mm），是未知數（不太可能是已知數，xike說的是不考慮畸變的投影值，這個就太簡單了）。

*************************************************************************************
#include "cvut.h"
#include <iostream>
#include <fstream>
#include <string>

using namespace cvut;
using namespace std;

  void main() {
 ifstream fin("calibdata.txt");
 ofstream fout("caliberation_result.txt"); 
 

 cout<<"開始提取角點………………";
 int image_count=0; 
 CvSize image_size; 
 CvSize board_size = cvSize(5,7);   
 CvPoint2D32f * image_points_buf = new CvPoint2D32f[board_size.width*board_size.height];  
 Seq<CvPoint2D32f> image_points_seq; 
 
 string filename;
 while (std::getline(fin,filename))
 {
  cout<<"\n 將滑鼠焦點移到標定影象所在視窗並輸入回車進行下一幅影象的角點提取 \n";

  image_count++;
  int count;
  Image<uchar> view(filename); 
  if (image_count == 1)
  {
   
   image_size.width = view.size().width;
   image_size.height = view.size().height;
  }

  
  if (0 == cvFindChessboardCorners( view.cvimage, board_size,
            image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH ))
   
  {
   cout<<"can not find chessboard corners!\n";
   exit(1);
  }
  
  else {
   Image<uchar> view_gray(view.size(),8,1);
   
   rgb2gray(view,view_gray);
   
   
   cvFindCornerSubPix( view_gray.cvimage, image_points_buf, count, cvSize(11,11),
    cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
  
   
   image_points_seq.push_back(image_points_buf,count);
   
   
   cvDrawChessboardCorners( view.cvimage, board_size, image_points_buf, count, 1);
   view.show("calib");
  
   cvWaitKey(); 
   view.close();
  }

  
 }

 delete []image_points_buf; 
 cout<<"角點提取完成！\n"<<endl;

 
 
 cout<<"開始定標………………"<<"\n"<<endl;
 CvSize square_size = cvSize(10,10); 
 
 
 Matrix<double> object_points(1,board_size.width*board_size.height*image_count,3);
 

 Matrix<double> image_points(1,image_points_seq.cvseq->total,2);
 
 Matrix<int>    point_counts(1,image_count,1);

 Matrix<double> intrinsic_matrix(3,3,1);
 Matrix<double> distortion_coeffs(1,4,1);
 Matrix<double> rotation_vectors(1,image_count,3);
 Matrix<double> translation_vectors(1,image_count,3);

 
 int i,j,t;
 for (t=0;t<image_count;t++) {
  for (i=0;i<board_size.height;i++) {
   for (j=0;j<board_size.width;j++) {
    
    
    object_points(0,t*board_size.height*board_size.width + i*board_size.width + j,0) = i*square_size.width;
    object_points(0,t*board_size.height*board_size.width + i*board_size.width + j,1) = j*square_size.height;
    object_points(0,t*board_size.height*board_size.width + i*board_size.width + j,2) = 0;
   }
  }
 }

 
 
 char  str[10];
 itoa(image_points_seq.cvseq->total,str,10);
 cout<<str<<"\n"<<endl;

 
 for (i=0;i<image_points_seq.cvseq->total;i++)
 {
  image_points(0,i,0) = image_points_seq[i].x;
  image_points(0,i,1) = image_points_seq[i].y;
 }

 
 for (i=0;i<image_count;i++)
  point_counts(0,i) = board_size.width*board_size.height;
 
 
 cvCalibrateCamera2(object_points.cvmat,
        image_points.cvmat,
        point_counts.cvmat,
        image_size,
        intrinsic_matrix.cvmat,
        distortion_coeffs.cvmat,
        rotation_vectors.cvmat,
        translation_vectors.cvmat,
        0);
 cout<<"定標完成！\n";
 
 cout<<"標定結果顯示\n";
 cout<<"*************************************************\n";
 cout<<"相機內參intrinsic_matrix\n";
 for(int h=0;h<3;h++)
 {
    cout<<"X:"<<intrinsic_matrix(h,0,0)<<"\tY:"<<intrinsic_matrix(h,1,0)<<"\tZ:"<<intrinsic_matrix(h,2,0)<<"\n";
 }

 cout<<"\n畸變係數：distortion_coeffs\n";
 for(int ndis=0;ndis<4;ndis++)
 {
  cout<<distortion_coeffs(0,ndis,0)<<"\\";
 }
 cout<<"\n";

 cout<<"\nrotation_vectors\n";
 for(int rot=0;rot<7;rot++)
 {
   cout<<"X:"<<rotation_vectors(0,rot,0)<<"\tY:"<<rotation_vectors(0,rot,1)<<"\tZ:"<<rotation_vectors(0,rot,2)<<"\n";
 }

 cout<<"\ntranslation_vectors\n";
 for(i=0;i<7;i++)
 {
  cout<<"第"<<i+1<<"張 圖"<<"\tX:"<<translation_vectors(0,i,0)<<"\tY:"<<translation_vectors(0,i,1)<<"\tZ:"<<translation_vectors(0,i,2)<<"\n";
 }
 cout<<"***************************************************\n";
 
 cout<<"開始評價定標結果………………\n";
 double total_err = 0.0;
 double err = 0.0;
 Matrix<double> image_points2(1,point_counts(0,0,0),2);

 int temp_num  = point_counts(0,0,0); 

 cout<<"\t每幅影象的定標誤差：\n";
 
 fout<<"每幅影象的定標誤差：\n";

 for (i=0;i<image_count;i++)
 {
  
  cvProjectPoints2(object_points.get_cols(i * point_counts(0,0,0),(i+1)*point_counts(0,0,0)-1).cvmat,
      rotation_vectors.get_col(i).cvmat,
      translation_vectors.get_col(i).cvmat,
      intrinsic_matrix.cvmat,
      distortion_coeffs.cvmat,
     image_points2.cvmat,
      0,0,0,0);

  
  err = cvNorm(image_points.get_cols(i*point_counts(0,0,0),(i+1)*point_counts(0,0,0)-1).cvmat,
     image_points2.cvmat,
     CV_L1);
  total_err += err/=point_counts(0,0,0);

  cout<<"******************************************************************\n";
  cout<<"\t\t第"<<i+1<<"幅影象的平均誤差："<<err<<"畫素"<<'\n';
  fout<<"\t第"<<i+1<<"幅影象的平均誤差："<<err<<"畫素"<<'\n';
  
  cout<<"顯示image_point2\n";
  for(int ih=0;ih<7;ih++)
  {
   cout<<"X:"<<image_points2(0,ih,0)<<"\tY:"<<image_points2(0,ih,1)<<"\n";
  }
  cout<<"顯示object_Points\n";
  for(int iw=0;iw<7;iw++)
  {
   cout<<"X:"<<image_points.get_cols(i*point_counts(0,0,0),(i+1)*point_counts(0,0,0)-1)(0,iw,0)
    <<"\tY:"<<image_points.get_cols(i*point_counts(0,0,0),(i+1)*point_counts(0,0,0)-1)(0,iw,1)<<"\n";
  }
 }
 

 cout<<"\t總體平均誤差："<<total_err/image_count<<"畫素"<<'\n';
 fout<<"總體平均誤差："<<total_err/image_count<<"畫素"<<'\n'<<'\n';
 cout<<"評價完成！\n";

 
 cout<<"開始儲存定標結果………………";
 Matrix<double> rotation_vector(3,1);
 Matrix<double> rotation_matrix(3,3);
 
 fout<<"相機內引數矩陣：\n";
 fout<<intrinsic_matrix<<'\n';
 fout<<"畸變係數：\n";
 fout<<distortion_coeffs<<'\n';
 for (i=0;i<image_count;i++) {
  fout<<"第"<<i+1<<"幅影象的旋轉向量：\n";
  fout<<rotation_vectors.get_col(i);
  
  for (j=0;j<3;j++) {
   rotation_vector(j,0,0) = rotation_vectors(0,i,j);
  }
  

  cvRodrigues2(rotation_vector.cvmat,rotation_matrix.cvmat);

 
  fout<<"第"<<i+1<<"幅影象的旋轉矩陣：\n";
  fout<<rotation_matrix;
  fout<<"第"<<i+1<<"幅影象的平移向量：\n";
  fout<<translation_vectors.get_col(i)<<'\n';
 }
 cout<<"完成儲存\n";
}