3D vision (2): Calibration and correction of monocular camera

3D vision (2): Calibration and correction of monocular camera

The camera maps coordinate points in the three-dimensional world (in meters) to a two-dimensional image plane (in pixels). This process can be described by a pinhole camera model and a lens distortion model. These two models can project the external 3D points to the internal imaging plane of the camera to form the internal parameters of the camera.

---

1. Camera model

Assuming that there is a robot car, the robot car coordinate system can be established with the rear wheel of the car as the origin of the 3D coordinates. The robot car carries a monocular camera, and the camera coordinate system can be established with the optical center of the camera as the origin of the 3D coordinates. Now that the 3D coordinates of an object in the robotic vehicle coordinate system are known, how to calculate its 2D index position of the pixel on the imaging image?

1. From the robot car coordinate system to the camera coordinate system

Step 1: Use the external parameter matrix to perform 3D coordinate transformation. Note that the rotation matrix is ​​R, the translation vector is t, and the 3D coordinates of the object in the robot car coordinate system are Pw, and the 3D coordinates in the camera coordinate system are P = (X, Y, Z), then:

P = RPw + t = TPw

Here the coordinate units of Pw and P are both meters.

2. Camera coordinate system to normalized plane coordinate system

Step 2: Use the pinhole camera model to transform the 3D camera coordinates P = (X, Y, Z) into 2D normalized plane coordinates (x, y, 1).

Using the principle of similar triangles, we can get: z/f = x/X = y/Y.
Generally, we take z = 1, and the normalized coordinates after mapping are (x, y, z) = (X/f, Y/f, 1), where the units of x and y are meters.

The normalized coordinates can be regarded as a point on the plane at z=1 in front of the camera, and this z=1 plane is also called the normalized plane. The normalized plane is multiplied by the internal reference matrix to the left to obtain the pixel coordinates, so we can regard the pixel coordinates (u, v) as the result of quantitative measurement of points on the normalized plane.

It can also be seen from this model that if the camera coordinates are multiplied by any non-zero constant at the same time, the normalized coordinates are the same, which means that the depth of the point is lost during the projection process, so it cannot be obtained in monocular vision The depth value of the pixel.

3. Normalized plane coordinate distortion

Step 3: Distort the normalized coordinates. In order to obtain a better imaging effect, sometimes we will add a lens in front of the camera. The addition of lenses will have new effects on the propagation of light during imaging. One is that the shape of the lens itself has an impact on the light propagation; the other is that the lens and the imaging plane cannot be completely parallel during the mechanical assembly process, which will also change the position of the light when it passes through the lens and is projected onto the imaging plane.

The distortion caused by the shape of the lens is called radial distortion. In the pinhole model, a straight line projected onto the pixel plane is still a straight line, but in the actual shooting process, a straight line in the real environment often becomes a curve in the picture, and the closer to the edge of the image, this phenomenon more obvious. Distortion is mainly divided into two categories: barrel distortion and pincushion distortion. The magnification of barrel distortion images decreases with increasing distance from the optical axis, while the magnification of pincushion distortion images increases with increasing distance from the optical axis.

The distortion caused by the inability to make the lens and the imaging plane strictly parallel during the camera assembly process becomes tangential distortion.

Note that the normalized plane coordinates are (x, y), the polar coordinates are (r, theta), and the distortion coordinates are (x_distorted, y_distorted). The conversion relationship between them can be described by polynomials:

Radial distortion:

Tangential distortion:

Combining the above two distortions, the distortion coordinates are obtained:

here the units of x_distorted and y_distorted are meters.

4. Normalize the plane coordinate system to the pixel coordinate system

Step 4: Project the distorted coordinates (x_distorted, y_distorted) to the pixel plane to get the position of the point on the image.

Between the pixel coordinates and the normalized plane coordinates, there is a difference of one scaling and one translation of the origin. We set the pixel coordinates to be scaled by fx times on the u axis, and fy times on the v axis, and at the same time the origin is translated (cx. cy), then the distorted coordinates (x_distorted, y_distorted) and pixel coordinates (u, v) The relationship is:

u = fx * x_distorted + cx
v = fy * y_distorted + cy

Sometimes we do not consider the distortion model, and can directly translate and scale the normalized coordinates x and y to obtain pixel coordinates u and v. Here, the units of x_distorted, y_distorted, x, and y are meters, the units of u, v are pixels, the units of fx, fy are pixels/meter, and the units of cx, cy are pixels.

2. Monocular camera calibration

It is generally believed that the internal reference matrix of the camera is fixed after leaving the factory and will not change during use. Some camera manufacturers will tell you the internal parameters of the camera, but sometimes we need to manually determine the internal parameters of the camera, which is the so-called calibration. In addition, if you feel that the calibration process is too cumbersome and the accuracy is not high, you can use the following method to approximate the internal parameter matrix and the distortion coefficient vector:

Note that the image size is (h, w) = (size[0], size[1]), for the internal reference matrix K= [[fx, 0, cx], [0, fy, cy], [0, 0, 1 ], can approximate fx = fy = size[1], cx = size[1]/2, cy = size[0]/2. For the distortion coefficient vector D, D=zeros(1, 5) can be approximated.

"Zhang's calibration" is a single-plane checkerboard camera calibration method proposed by Professor Zhang Zhengyou in 1998. Zhang's calibration method has been widely used as a toolbox or packaged function. The original text is "A Flexible New Technique for Camera Calibration" . The method mentioned in this article provides great convenience for camera calibration and has high accuracy. From then on, no special calibration object is needed for calibration, only a printed checkerboard is needed.

Zhang's calibration is to use a printed checkerboard, and then mark each corner point with its pixel coordinates in the pixel coordinate system and the coordinates in the world coordinate system. The H matrix can be solved by more than 4 groups of points. value. But in order to reduce the error and have stronger robustness, we generally take many photos and select a large number of corner points for calibration.

We assume that the calibration chessboard is located on the zw=0 plane in the world coordinates, then the simplified formula can be obtained:

Define the H matrix as:
Then the original equation can be transformed into:

With the help of the OpenCV checkerboard point detection function, we can get the observed values ​​of u and v. Since the checkerboards are arranged in a certain order, the corresponding indexes can be assigned as their 3D coordinate points. Although there are size differences with the real world coordinates, this will only affect the calculation results of the external parameter matrix, not the internal parameters. Matrix solution. In this way, we get the corresponding observation values ​​of (u, v, 1) and (xw.yw, 1), and the H matrix can be solved by solving the linear equation system. Then, through the special properties of the rotation matrix and the internal reference matrix, the internal reference matrix K, the rotation matrix R and the translation vector t can be restored from the H matrix.

The specific calibration process is as follows:
step1: Prepare a checkerboard picture and fix it on the wall.
step2: Take a series of checkerboard photos from different angles and store them in a folder.
step3: For each captured checkerboard picture, detect the feature points (u, v, 1 ) of all checkerboard grids in the picture.
step4: For each captured chessboard picture, assign the corresponding index to their 3D coordinate points (xw.yw, 1).
step5: Use the cv::calibrateCamera function to calibrate and solve the parameter optimization problem.
step6: Use the cv::undistort function to correct the original image.

3. Experimental results

Take a series of photos of the checkerboard from different angles, as shown in the figure:
Perform checkerboard interior point detection on each picture:

The parameter results obtained by calibration are:

The original image and the corrected image are shown in the figure below. It can be seen that the distortion has been largely eliminated.

4. Source code

Monocular camera calibration:

#include <opencv2/opencv.hpp>
// opencv.hpp中己经包含了OpenCV各模块的头文件，如高层GUI图形用户界面模块头文件highgui.hpp、图像处理模块头文件imgproc.hpp、2D特征模块头文件features2d.hpp等。
// 所以我们在编写应用程序时，原则上仅写上一句 #include <opencv2/opencv.hpp> 即可，这样可以精简优化代码
#include <opencv2/calib3d/calib3d.hpp>
// calib3d模块主要是相机校准和三维重建相关的内容：基本的多视角几何算法，单个立体摄像头标定，物体姿态估计，立体相似性算法，3D信息的重建等。
#include <opencv2/highgui/highgui.hpp>
// highgui模块，高层GUI图形用户界面，包含媒体的I/O输入输出、视频捕捉、图像和视频的编码解码、图形交互界面的接口等内容
#include <opencv2/imgproc/imgproc.hpp>
// imgproc模块，图像处理模块，包含：线性和非线性的图像滤波、图像的几何变换、特征检测等

#include <iostream>
#include<unistd.h> 
// unistd.h是用于linux/unix系统的调用，相当于windows下的windows.h，包含了许多UNIX系统服务的函数原型，例如read函数、write函数、sleep函数。
#include <chrono>
// chrono是C++11新加入的方便时间日期操作的标准库，它既是相应的头文件名称，也是std命名空间下的一个子命名空间，所有时间日期相关定义均在std::chrono命名空间下。
// 通过这个新的标准库，可以非常方便进行时间日期相关操作。 


using namespace std;


// 定义棋盘格维度，{6,4}代表行内点数为6，列内点数为4
int CHECKERBOARD[2]{
    
    6,4}; 

int main()
{
    
    
  
  // objpoints中每个元素都是一个小vector，每个小vector存储的每个元素都是opencv的cv::Point3f数据结构
  // n * 54 * 3 * 1
  std::vector<std::vector<cv::Point3f> > objpoints;

  // imgpoints中每个元素都是一个小vector，每个小vector存储的每个元素都是opencv的cv::Point2f数据结构
  // n * 54 * 2 * 1
  std::vector<std::vector<cv::Point2f> > imgpoints;

  // objp: 54 * 3 * 1, 记录单张棋盘格，54个内点的3d位置索引
  // 指定棋盘格坐标点时，按照先从上到下，后从左到右的顺序记录。每一行棋盘格的记录方式：(y索引, x索引， 0）
  std::vector<cv::Point3f> objp;
  //  [0, 0, 0;
  //  1, 0, 0;
  //  2, 0, 0;
  //  3, 0, 0;
  //  ... ...
  //  2, 8, 0;
  //  3, 8, 0;
  //  4, 8, 0;
  //  5, 8, 0]
  
  for(int i{
    
    0}; i<CHECKERBOARD[1]; i++)
  {
    
    
    for(int j{
    
    0}; j<CHECKERBOARD[0]; j++)
      objp.push_back(cv::Point3f(j,i,0));
  }


  // images_path，存储所有棋盘格图片的存储路径
  std::vector<cv::String> images_path;
  std::string path = "../images2/*.jpg";
  cv::glob(path, images_path);
  std::string saved_path;

  cv::Mat frame, gray;
  
  // corner_pts，记录检测到的棋盘格54个内点的2D像素坐标 
  std::vector<cv::Point2f> corner_pts;
  // success，用于判断是否成功检测到棋盘格
  bool success;

  // 开始计时
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  
  for(int i{
    
    0}; i<images_path.size(); i++)
  {
    
     
    chrono::steady_clock::time_point t11 = chrono::steady_clock::now();
    
    // 图像大小 640 x 480
    frame = cv::imread(images_path[i]);
    std::cout << images_path[i] << std::endl;
    cv::cvtColor(frame,gray, cv::COLOR_BGR2GRAY);

    // OpenCV函数寻找棋盘格
    success = cv::findChessboardCorners(gray,cv::Size(CHECKERBOARD[0],CHECKERBOARD[1]), corner_pts, cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);

    if(success)
    {
    
    
      cv::TermCriteria criteria(cv::TermCriteria::EPS | cv::TermCriteria::MAX_ITER, 30, 0.001);

      // 进一步refine检测到的网格内点的坐标精度
      // 这里cornerSubPix函数直接在原有corner_pts基础上进行覆盖，不会多创建一个新的变量再赋值
      cv::cornerSubPix(gray, corner_pts, cv::Size(11,11), cv::Size(-1,-1), criteria);

      // 作图，棋盘格检测结果
      cv::drawChessboardCorners(frame, cv::Size(CHECKERBOARD[0],CHECKERBOARD[1]), corner_pts, success);

      objpoints.push_back(objp);
      imgpoints.push_back(corner_pts);
    }
    
//     cv::imshow("Image", frame);
//     cv::waitKey(10);
    
    saved_path = "../images1_demo/" + std::to_string(i) + ".jpg";
    cv::imwrite(saved_path, frame);
    
    chrono::steady_clock::time_point t22 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t22 - t11);
    cout << "每一张图片处理耗时: " << time_used.count() << " 秒. " << endl;

  }

  cv::destroyAllWindows();
  
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used1 = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "整体耗时: " << time_used1.count() << " 秒. " << endl;

  
  // 内参矩阵、畸变系数、旋转矩阵R、平移向量T
  cv::Mat cameraMatrix, distCoeffs, R, T;

  chrono::steady_clock::time_point t111 = chrono::steady_clock::now();
  
  // 这里注意参数顺序，必须先cols后rows
  cv::calibrateCamera(objpoints, imgpoints, cv::Size(gray.cols,gray.rows), cameraMatrix, distCoeffs, R, T);
  
  chrono::steady_clock::time_point t222 = chrono::steady_clock::now();
  chrono::duration<double> time_used_cali = chrono::duration_cast<chrono::duration<double>>(t222 - t111);
  cout << "矫正耗时: " << time_used_cali.count() << " 秒. " << endl;

  
  std::cout << "cameraMatrix : " << cameraMatrix << std::endl;
  std::cout << "distCoeffs : " << distCoeffs << std::endl;
//   std::cout << "Rotation vector : " << R << std::endl;
//   std::cout << "Translation vector : " << T << std::endl;

  return 0;
}



// 对于相机内参矩阵：[[fx, 0, cx], [0, fy, cy], [0, 0, 1]
//  一般都可近似 fx = fy = size[1], cx = size[1]/2, cy = size[0]/2

// images2文件夹，内参标定结果：
// cameraMatrix : [845.5595871866724, 0, 1324.600361657917;
// 0, 850.5931334946969, 729.9380327446599;
// 0, 0, 1]
// distCoeffs : [-0.1129616696736557, 0.01545728211105597, -0.001661835061769386, -0.0001092622724212072, -0.001159949110844942]

Monocular camera correction:

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <stdio.h>
#include <iostream>

using namespace std;


int main()
{
    
    
  // images_path，存储所有棋盘格图片的存储路径
  std::vector<cv::String> images_path;
  std::string path = "../images2/*.jpg";
  cv::glob(path, images_path);
  
  // 根据计算得到的内参、畸变系数，对畸变图片进行矫正
  cv::Mat image;
  image = cv::imread(images_path[0]);
  cv::Mat dst, map1, map2, new_camera_matrix;
  cv::Size imageSize(cv::Size(image.cols, image.rows));
  
  // 内参矩阵
  float K[3][3] = {
    
    845.5595871866724, 0, 1324.600361657917, 0, 850.5931334946969, 729.9380327446599, 0, 0, 1};    // float类型
  cv::Mat cameraMatrix = cv::Mat(3, 3, CV_32FC1, K);    

  // 畸变系数 
  float d[1][5] = {
    
    -0.1129616696736557, 0.01545728211105597, -0.001661835061769386, -0.0001092622724212072, -0.001159949110844942};   // float类型 
  cv::Mat distCoeffs = cv::Mat(1, 5, CV_32FC1, d);   
   
  // 将内参矩阵和畸变系数进行融合，得到新的矫正参数矩阵
  // 最后一个参数需要注意：最后一个参数默认是false，也就是相机光心不在默认的图像中心位置，可能导致去除畸变后的图像边缘仍存在畸变，因此需要改成true
  new_camera_matrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 0.01, imageSize, 0, true);
    
  for(int i{
    
    0}; i<images_path.size(); i++)
  {
    
      
        
    image = cv::imread(images_path[i]); 

    // 第1种方法：OpenCV undistort函数，转换图像以补偿径向和切向镜头失真
    cv::undistort(image, dst, new_camera_matrix, distCoeffs, new_camera_matrix);

    // 第2种方法：OpenCV remap函数，计算联合不失真和整流变换，并以重映射的映射形式表示结果
    //   cv::initUndistortRectifyMap(cameraMatrix, distCoeffs, cv::Mat(),cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs,   imageSize, 1, imageSize, 0),imageSize, CV_16SC2, map1, map2);
    // 
    //   cv::remap(image, dst, map1, map2, cv::INTER_LINEAR);
     
    cv::Mat resize_dst;
    resize(dst, resize_dst, cv::Size(256*2, 144*2), 0, 0, cv::INTER_LINEAR);

    cv::imshow("undistorted image", resize_dst);
    cv::waitKey(0);  
    
    std::string saved_path = "../images2_undist/" + std::to_string(i) + ".jpg";
    cv::imwrite(saved_path, dst);
    
  }

  return 0;
}

5. Related Links

If the code doesn't work, or you want to use the dataset I made directly, you can download the project link:
https://blog.csdn.net/Twilight737