張正友雙目標定

        攝像機的標定問題是機器視覺領域的入門問題，可以分爲傳統的攝像機定標方法和攝像機自定標方法。定標的方法有很多中常見的有：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說的是不考慮畸變的投影值，這個就太簡單了）。

cvmat);

#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";
}