Please be sure you understand the method before reading the next. You can find the concept of method by searching in the internet or just click here to download the paper. This post only care about the implementation!

1. Preparation
It is easy to do the following:
  - Set up a C/C++ compiler (Visual Studio, Eclipse CDT, DEV C++ ...)
  - Download the latest OpenCV library.
  - Download a math library, I suggest you to use Engen library. You can find it here.
  - Integrate all libray into your compiler.

2. Data collection
We use camera which need to be calibrated to capture the image contained 2 small balls. Each ball presents the point. 

B and C lie on a line and are observable points which can be detected by using the Hough transformation.
A is an unobservable point and it's position is calculated according to the position of B and C.
We will capture N (N = 100 for example) images and find N positions of B and C. These positions then will be written in a data file for the purpose of calibrating below.

Code for collecting data:

std::vector<Vec3f> circles;

cv::HoughCircles(src, circles, CV_HOUGH_GRADIENT, 1, 

                           src_gray.rows/8, 200, 100, 0, 0 ); 

//We just desire to detect 2 circles. 

if(circles.size() != 2) continue;

cv::Point B(cvRound(circles[0][0], cvRound(circles[0][1]);

cv::Point C(cvRound(circles[1][0], cvRound(circles[1][1]); 

//Store these points in a file 

point_dist(B, A) > point_dist(C, A) ?

data_file << B : data_file << C;

data_file << "\n";

                }
          } 
     

data_file.close();

}

3. Calculate camera intrinsics.

std::vector<double> camera_intrinsic()

{

using namespace Eigen; 

  std::vector<double> results;  double u0, v0, anpha, beta, gamma;double Za;double L = 2.0;double la = 0.6;double lb = 0.4;double temp1, temp2;int i = 0, j = 0;  MatrixXd V(N,6);  MatrixXd Q(N,2);  VectorXd l(N);  VectorXd s(2);  double xb, yb, xc, yc;  Vector3d a(3), b(3), c(3), h(3);  VectorXd x;  VectorXd e = VectorXd::Ones(N);  double b11, b12, c11, c12;  std::ifstream file("data.txt");  while(file >> xb >> yb >> xc >> yc) {    Q.row(j) << yc - yb, xb - xc;    l(j) = xb*(yc - yb) + yb*(xb - xc);    j++;  }  file.close();  file.open("data.txt");  s = (Q.transpose()*Q).inverse()*Q.transpose()*l;  a<<s[0], s[1], 1;  if(file.is_open()){    while(file >> b11 >> b12 >> c11 >> c12) {      b<<b11, b12, 1;   c<<c11, c12, 1;      temp1 = (a.cross(c)).dot(b.cross(c));      temp2 = (b.cross(c)).dot(b.cross(c));      h = a + ((la*temp1)/(lb*temp2))*b;      V.row(i) << h(0)*h(0), 2*h(0)*h(1), h(1)*h(1),

                  2*h(0)*h(2), 2*h(1)*h(2), h(2)*h(2);      i++;    }  }x = (L*L)*((((V.transpose()*V).inverse())*V.transpose())*e);v0 = (x(1)*x(3) - x(0)*x(4))/(x(0)*x(2) - x(1)*x(1));Za = sqrt(x(5) - (x(3)*x(3) + v0*(x(1)*x(3) - x(0)*x(4)))/x(0));anpha = Za/sqrt(x(0));beta = Za*sqrt(fabs(x(0)/(x(0)*x(2) - x(1)*x(1))));gamma = -(x(1)*anpha*anpha*beta)/(Za*Za);u0 = v0*gamma/beta - x(3)*anpha*anpha/(Za*Za);results.push_back(u0); 

results.push_back(v0); 

results.push_back(anpha);

results.push_back(beta); 

results.push_back(gamma);return results;

} 

4. Calculate camera extrinsics

Chose 4 points according to your own real cordinate system to calculate the extrinsics of camera. It is simple, --->> no code for this section :)​