10/08/2013

OpenCV 2.46 Calibration example source code (using calibrateCamera function)

This is advanced from "http://feelmare.blogspot.kr/2011/08/camera-calibration-using-pattern-image.html"

When you run the calibration example source code, some information will ask you.
First question is to ask number of width corner points.
Second question is to ask number of height corner points.
Third question is to ask number of pattern boards.


Because I use this chess board pattern, the answers is as follows


Of course, you have to prepare the captured images of chess pattern.

The source code detect corner points and calibration will be performed.
This function 'findChessboardCorners' is used to detection corners.
And 'calibrateCamera' function is used to get calibration parameters.

This is calibration example source code.


////
#include < stdio.h>
#include < opencv2\opencv.hpp>
//#include < opencv2\features2d\features2d.hpp>
//#include < opencv2\nonfree\nonfree.hpp>
//#include < opencv2\nonfree\features2d.hpp>

#include < vector>

#ifdef _DEBUG
#pragma comment(lib, "opencv_core246d.lib") 
#pragma comment(lib, "opencv_imgproc246d.lib")   //MAT processing
//#pragma comment(lib, "opencv_objdetect246d.lib") //HOG
//#pragma comment(lib, "opencv_gpu246d.lib")
//#pragma comment(lib, "opencv_features2d246d.lib")
#pragma comment(lib, "opencv_highgui246d.lib")
//#pragma comment(lib, "opencv_ml246d.lib")
//#pragma comment(lib, "opencv_stitching246d.lib")
//#pragma comment(lib, "opencv_superres246d.lib")
#pragma comment(lib, "opencv_calib3d246d.lib")
//#pragma comment(lib, "opencv_nonfree246d.lib")
//#pragma comment(lib, "opencv_flann246d.lib")

#else
#pragma comment(lib, "opencv_core246.lib")
#pragma comment(lib, "opencv_imgproc246.lib")
//#pragma comment(lib, "opencv_objdetect246.lib")
//#pragma comment(lib, "opencv_gpu246.lib")
//#pragma comment(lib, "opencv_features2d246.lib")
#pragma comment(lib, "opencv_highgui246.lib")
//#pragma comment(lib, "opencv_ml246.lib")
//#pragma comment(lib, "opencv_stitching246.lib")
//#pragma comment(lib, "opencv_superres246.lib")
#pragma comment(lib, "opencv_calib3d246.lib")
//#pragma comment(lib, "opencv_nonfree246.lib")
//#pragma comment(lib, "opencv_flann246.lib")

#endif

using namespace std;
using namespace cv;

struct cornerInformation{
 float x;
 float y;
 float x3;
 float y3;
 float z3;
};

void fprintMatrix(Mat matrix, string name);
void fprintfVectorMat(vector< Mat> matrix, string name);
void fprintf2Point( vector< vector< Point2f> > Points, string name);
void fprintf3Point( vector< vector< Point3f> > Points, string name);

void main()
{
 //////////////////////////////////////////////////////////////////////////////////////////////////
 //Set input params..
 int board_w, board_h;
 int n_boards;
 float measure=25;
 Size imageSize;

 vector< vector< Point2f> > imagePoints;
 vector< vector< Point3f> > objectPoints;

 board_w=6;
 board_h=7;
 n_boards=18;

 printf("How many cross points of width direction? \n" ); 
 scanf("%d", &board_w);
 printf("How many cross points of Height direction? \n" );
 scanf("%d", &board_h);

 printf("How many board? (board will be read by this namimg-> ./pattern/p1.jpg, ./pattern/p2.jpg...\n");
 scanf("%d", &n_boards);

 printf("What mm ?\n");
 scanf("%f", &measure);
 
 printf("w=%d h=%d n=%d %lfmm\n", board_w, board_h, n_boards, measure);
 //////////////////////////////////////////////////////////////////////////////////////////////////
 
 //////////////////////////////////////////////////////////////////////////////////////////////////
 //image load
 //extraction image point and object point
 char str[100];
 for(int i=0; i< n_boards; ++i)
 {
  //image load
  sprintf(str,"./pattern/p%d.jpg", i+1 );
  printf("%s\n", str);
  Mat img = imread(str);
  imageSize = Size(img.cols, img.rows);
  Mat gray;
  cvtColor(img, gray, CV_RGB2GRAY);
  vector< Point2f> corners;  

  //find chessboard corners
  bool sCorner = findChessboardCorners(gray, Size(board_w, board_h), corners);

  //if find corner success, then
  if(sCorner)
  {
   //corner point refine
   cornerSubPix(gray, corners, Size(11,11), Size(-1,-1), TermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1));
   //draw corner
   drawChessboardCorners(img, Size(board_w, board_h), corners, sCorner);
   if(corners.size() == board_w*board_h)
   {
    vector< Point2f> v_tImgPT;
    vector< Point3f> v_tObjPT;
    //save 2d coordenate and world coordinate
    for(int j=0; j< corners.size(); ++j)
    {
     Point2f tImgPT;
     Point3f tObjPT;

     tImgPT.x = corners[j].x;
     tImgPT.y = corners[j].y;

     tObjPT.x = j%board_w*measure;
     tObjPT.y = j/board_w*measure;
     tObjPT.z = 0;

     v_tImgPT.push_back(tImgPT);
     v_tObjPT.push_back(tObjPT);     
    }
    imagePoints.push_back(v_tImgPT);
    objectPoints.push_back(v_tObjPT);
   }
  }
  sprintf(str,"Detected%d.jpg",i+1);
  imwrite(str,img);
  imshow("pattern",img);
  cvWaitKey(10);
 }
 //////////////////////////////////////////////////////////////////////////////////////////////////


 //////////////////////////////////////////////////////////////////////////////////////////////////
 //claibration part
 vector< Mat> rvecs, tvecs;
 Mat intrinsic_Matrix(3,3, CV_64F);
 Mat distortion_coeffs(8,1, CV_64F);
 
 calibrateCamera(objectPoints, imagePoints, imageSize, intrinsic_Matrix, distortion_coeffs, rvecs, tvecs);

 
 for(int i=0; i< distortion_coeffs.rows; ++i)
 {
  for(int j=0; j< distortion_coeffs.cols; ++j)
  {
   printf("%lf ", distortion_coeffs.at< double>(i,j)); //cvmGet(matrix,i,j));
  }
  printf("\n");
 }
 printf("\n");
 //////////////////////////////////////////////////////////////////////////////////////////////////
 

 //////////////////////////////////////////////////////////////////////////////////////////////////
 //save part
 fprintMatrix(intrinsic_Matrix, "intrinsic.txt");
 fprintMatrix(distortion_coeffs, "distortion_coeffs.txt");

 fprintfVectorMat(rvecs, "rotation.txt");
 fprintfVectorMat(tvecs, "translation.txt");

 fprintf3Point(objectPoints, "objectpt.txt");
 fprintf2Point(imagePoints, "imagept.txt");

 FILE* fp=fopen("ptSize.txt","w");
 fprintf(fp,"%d %d\n", board_w, board_h);
 fclose(fp);
 //////////////////////////////////////////////////////////////////////////////////////////////////
}

void fprintf3Point( vector< vector< Point3f> > Points, string name)
{
 FILE * fp;
 fp = fopen(name.c_str() ,"w");
 for(int i=0; i< Points.size(); ++i)
 {
  for(int j=0; j< Points[i].size(); ++j)
  {
   fprintf(fp,"%lf %lf %lf\n", Points[i][j].x, Points[i][j].y, Points[i][j].z);
  }
  fprintf(fp,"\n");
 }
 fclose(fp);
}


void fprintf2Point( vector< vector< Point2f> > Points, string name)
{
 FILE * fp;
 fp = fopen(name.c_str() ,"w");
 for(int i=0; i< Points.size(); ++i)
 {
  for(int j=0; j< Points[i].size(); ++j)
  {
   fprintf(fp,"%lf %lf\n", Points[i][j].x, Points[i][j].y);
  }
  fprintf(fp,"\n");
 }
 fclose(fp);
}


void fprintfVectorMat(vector< Mat> matrix, string name)
{
 FILE * fp;
 fp = fopen(name.c_str() ,"w");
 int i,j; 
 printf("%s size %d, %d\n",name.c_str(),matrix.size(), matrix[0].cols, matrix[0].rows);
 for(i=0; i< matrix.size(); ++i)
 {
  for(int j=0; j< matrix[i].rows; ++j)  
  {
   for(int k=0; k< matrix[i].cols; ++k)
   {
    fprintf(fp,"%lf ", matrix[i].at<  double >(j,k)); 
   }
   fprintf(fp,"\n");
  }
  fprintf(fp,"\n");
 }

 
 fclose(fp);
}

void fprintMatrix(Mat matrix, string name)
{
 FILE * fp;
 fp = fopen(name.c_str() ,"w");
 int i,j; 
 printf("%s size %d %d\n",name.c_str(), matrix.cols, matrix.rows);
 for(i=0; i< matrix.rows; ++i)
 {
  for(j=0; j< matrix.cols; ++j)
  {
   fprintf(fp,"%lf ", matrix.at<  double >(i,j)); 
  }
  fprintf(fp,"\n");
 }
 
 fclose(fp);
}

////


After calibration, the source code save ->
distortion_coeffs.txt
intrinsic.txt
rotation.txt
translatioin.txt
imagept.txt
objectpt.txt

and

The result images of corner detected.














This is matlab source code.
To confirm the result of calibration, I draw 2D image coordinate point to the 3D space.

m=[R|t]M or m=[R|-Rc]M
m is camera origin axis based coordinate.
M is world origin axis based coordinate.
In the -Rc, c is translate vector based on world origin axis.

pattern axis based
The equation is like this
R'(m-t)=M
In the equation, m is camera line coordinate for drawing.
M is camera coordinate based on pattern axis.


camera axis based, pattern position in 3D
The equation is like this
m=R*M+t or m=[R|t]M
In the equation, M is pattern coordinate for example -> [0 0 0; 10, 0 0; 0 10 0; 10 10 0] or 
R is rotation 3x3 matrix, t is 3x1 translation matrix.
After calibration, we can get each R,t of pattern boards.
m is pattern 3D coordinate based on camera origin axis. 


The main m file is Sapce2D3D.m in matlab files.

////
clc;

close all;
%clear all;

%ImgPT=load('imagept.txt');
%ObjPT=load('objectpt.txt');
k=load('intrinsic.txt');R=load('rotation.txt');T=load('translation.txt');%S=load('ptSize.txt');

%intrinsic and extrinsic matrix
R = reshape(R,3,length(R)/3);
T = reshape(T,3,length(T)/3);
boardXYZ=[0 0 0 1; 175 0 0 1; 175 200 0 1; 0 200 0 1; 0 0 0 1]';

figure(1);
hold on; xlabel('x'); ylabel('y'); zlabel('z'); grid on; axis equal;


cmap = hsv( length(R) );
for i=1:length(R)
    Convert3D_2{i} = ( [rodrigues(R(:,i)) T(:,i)] * boardXYZ )';
    plot3( Convert3D_2{i}(1:5,1), Convert3D_2{i}(1:5,2), Convert3D_2{i}(1:5,3),'-', 'color', cmap(i,:) );  
    text( Convert3D_2{i}(1,1), Convert3D_2{i}(1,2),Convert3D_2{i}(1,3),num2str(i),'color', cmap(i,:) );
end

%camera draw
camView(5,eye(3), zeros(3,1),[1 0 0],'o');

%draw axis
drawAxis(5);

figure(2)
hold on; xlabel('x'); ylabel('y'); zlabel('z'); grid on; axis equal;
plot3( boardXYZ(1,:), boardXYZ(2,:), boardXYZ(3,:),'-', 'color', cmap(i,:) );  

for i=1:length(R)
    camView(10, rodrigues(R(:,i))', -T(:,i), cmap(i,:),num2str(i));  
end

////

You can download calibration source code and matlab code in here.