相机标定

前言

• 使用OpenCV
• 参考,靠谱:
  相机标定之张正友标定法数学原理详解（含python源码） - 知乎 (zhihu.com)
  link.zhihu.com/?target=htt…

辨析

• 是否需要告诉算法,实际的棋盘格的尺寸(这点很关键,很多博客只是弄个与棋盘格内角点对应的1长度(这个量的单位其实是m))

学习要点

• glob这个库来索引指定目录下全部文件
• yaml文件的写入,读取
• calibrateCamera(,, gray_img.shape[::-1], None, None)
  注意第三参数,是(cols, rows),即(宽, 高), 只能用gray_img.shape[::-1].gray_img.shape是(rows, cols)
• 这个改造成标定灰度图的,作者原文可以处理标定真彩色

代码

• python

  """
  dahua 相机
  120张样本的内参结果
  internal matrix:
   [[1.67718566e+03 0.00000000e+00 1.30574747e+03]
   [0.00000000e+00 1.66563629e+03 1.02422126e+03]
   [0.00000000e+00 0.00000000e+00 1.00000000e+00]]
  distortion cofficients:
   [[-0.37982045  0.20745996  0.00073598  0.0021865  -0.07472313]]
  Average Error of Reproject:  0.05167174586350024
  
  """
  import os
  import numpy as np
  import cv2
  import glob
  
  # 校正函数
  def dedistortion( img_dir, img_type, save_dir, inter_corner_shape, mat_inter, coff_dis):
      w,h = inter_corner_shape
      images = glob.glob(img_dir + os.sep + '**.' + img_type)
      for fname in images:
          img_name = fname.split(os.sep)[-1]
          img = cv2.imread(fname)
          newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mat_inter,coff_dis,(w,h),0,(w,h)) # 自由比例参数
          dst = cv2.undistort(img, mat_inter, coff_dis, None, newcameramtx)
          # clip the image
          # x,y,w,h = roi
          # dst = dst[y:y+h, x:x+w]
          cv2.imwrite(save_dir + os.sep + img_name, dst)
      print('Dedistorted images have been saved to: %s successfully.' %save_dir)
  
  # 内参保存成 yaml
  def save_cam_param(cameraMatrix, distCoeffs, file_path):
     file_yaml = file_path + "cam_param.yaml"
     fs = cv2.FileStorage(file_yaml, cv2.FileStorage_WRITE)
     fs.write("cameraMatrix", cameraMatrix)
     fs.write("distCoeffs", distCoeffs)
     fs.release()
  
  def calib(inter_corner_shape, size_per_grid, img_dir, img_type, cam_param_path):
      # criteria: only for subpix calibration, which is not used here.
      criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
      w, h = inter_corner_shape
      # cp_int: corner point in int form, save the coordinate of corner points in world sapce in 'int' form
      # like (0,0,0), (1,0,0), (2,0,0) ....,(10,7,0).
      cp_int = np.zeros((w * h, 3), np.float32)
      cp_int[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)
      # cp_world: corner point in world space, save the coordinate of corner points in world space.
      cp_world = cp_int * size_per_grid
  
      obj_points = []  # the points in world space
      img_points = []  # the points in image space (relevant to obj_points)
      images = glob.glob(img_dir + os.sep + '**.' + img_type)
      for fname in images:
          # img = cv2.imread(fname)
          # gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
          gray_img = cv2.imread(fname, cv2.IMREAD_GRAYSCALE)
          # find the corners, cp_img: corner points in pixel space.
          ret, cp_img = cv2.findChessboardCorners(gray_img, (w, h), None)
          # if ret is True, save.
          if ret == True:
              cv2.cornerSubPix(gray_img,cp_img,(11,11),(-1,-1),criteria)
              obj_points.append(cp_world)
              img_points.append(cp_img)
              # view the corners
              # cv2.drawChessboardCorners(img, (w, h), cp_img, ret)
              # cv2.imshow('FoundCorners', img)
  
              # 不用绘出检测的角点,提高性能
              # cv2.drawChessboardCorners(gray_img, (w, h), cp_img, ret)
              # cv2.imshow('FoundCorners', gray_img)
              # cv2.waitKey(1)
      # cv2.destroyAllWindows()
      # calibrate the camera
      # ret, mat_inter, coff_dis, v_rot, v_trans = cv2.calibrateCamera(obj_points, img_points, gray_img.shape[::-1], None,
  
      # print("gray_img.shape[::-1]: ", gray_img.shape[::-1])
      # return
      ret, mat_inter, coff_dis, v_rot, v_trans = cv2.calibrateCamera(obj_points, img_points, gray_img.shape[::-1], None, None)
      print(("ret:"), ret)
      print(("internal matrix:\n"), mat_inter)
      # in the form of (k_1,k_2,p_1,p_2,k_3)
      print(("distortion cofficients:\n"), coff_dis)
      print(("rotation vectors:\n"), v_rot)
      print(("translation vectors:\n"), v_trans)
  
      # 保存相机内参
      save_cam_param(mat_inter, coff_dis, cam_param_path)
  
      # calculate the error of reproject
      total_error = 0
      for i in range(len(obj_points)):
          img_points_repro, _ = cv2.projectPoints(obj_points[i], v_rot[i], v_trans[i], mat_inter, coff_dis)
          error = cv2.norm(img_points[i], img_points_repro, cv2.NORM_L2) / len(img_points_repro)
          total_error += error
      print(("Average Error of Reproject: "), total_error / len(obj_points))
  
      return mat_inter, coff_dis
  
  
  if __name__ == '__main__':
      inter_corner_shape = (12, 8)
      size_per_grid = 0.02
      # img_dir = ".\pic\RGB_camera_calib_img"
      # img_type = "png"
      img_dir = "..\pic_sample"
      img_type = "bmp"
      cam_param_path = "F:\Project\Calibrate_Cam\cam_param\"
      # 求取相机参数,主要关注内参
      calib(inter_corner_shape, size_per_grid, img_dir, img_type, cam_param_path)
  
      # 校正
      # 读取相机内参
      fs2 = cv2.FileStorage('../cam_param/cam_param.yaml', cv2.FileStorage_READ)
      cameraMatrix = fs2.getNode('cameraMatrix').mat()
      distCoeffs = fs2.getNode('distCoeffs').mat()
      fs2.release()
      # 校正图片
      inter_corner_shape = (12, 8)
      img_type = "bmp"
  
      distortion_path = "F:\Project\Calibrate_Cam\pic_distortion"
      correct_path = "F:\Project\Calibrate_Cam\pic_correct"
      dedistortion(distortion_path, img_type, correct_path, inter_corner_shape, cameraMatrix, distCoeffs)
  

• C++

  //calibration.cpp
  #include <iostream>
  #include <sstream>
  #include <time.h>
  #include <stdio.h>
  #include <fstream>
  
  #include <opencv2/core/core.hpp>
  #include <opencv2/imgproc/imgproc.hpp>
  #include <opencv2/calib3d/calib3d.hpp>
  #include <opencv2/highgui/highgui.hpp>
  
  using namespace cv;
  using namespace std;
  #define calibration
  
  int main()
  {
  #ifdef calibration
  
          ifstream fin("right_img.txt");             /* 标定所用图像文件的路径 */
          ofstream fout("caliberation_result_right.txt");  /* 保存标定结果的文件 */
  
          // 读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
          int image_count = 0;  /* 图像数量 */
          Size image_size;      /* 图像的尺寸 */
          Size board_size = Size(11,8);             /* 标定板上每行、列的角点数 */
          vector<Point2f> image_points_buf;         /* 缓存每幅图像上检测到的角点 */
          vector<vector<Point2f>> image_points_seq; /* 保存检测到的所有角点 */
          string filename;      // 图片名
          vector<string> filenames;
  
          while (getline(fin, filename))
          {
                  ++image_count;
                  Mat imageInput = imread(filename);
                  filenames.push_back(filename);
  
                  // 读入第一张图片时获取图片大小
                  if (image_count == 1)
                  {
                          image_size.width = imageInput.cols;
                          image_size.height = imageInput.rows;
                  }
  
                  /* 提取角点 */
                  if (0 == findChessboardCorners(imageInput, board_size, image_points_buf))
                  {
                          //cout << "can not find chessboard corners!\n";  // 找不到角点
                          cout << "**" << filename << "** can not find chessboard corners!\n";
                          exit(1);
                  }
                  else
                  {
                          Mat view_gray;
                          cvtColor(imageInput, view_gray, CV_RGB2GRAY);  // 转灰度图
  
                          /* 亚像素精确化 */
                          // image_points_buf 初始的角点坐标向量，同时作为亚像素坐标位置的输出
                          // Size(5,5) 搜索窗口大小
                          // （-1，-1）表示没有死区
                          // TermCriteria 角点的迭代过程的终止条件, 可以为迭代次数和角点精度两者的组合
                          cornerSubPix(view_gray, image_points_buf, Size(5, 5), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
  
                          image_points_seq.push_back(image_points_buf);  // 保存亚像素角点
  
                          /* 在图像上显示角点位置 */
                          drawChessboardCorners(view_gray, board_size, image_points_buf, false); // 用于在图片中标记角点
  
                          imshow("Camera Calibration", view_gray);       // 显示图片
  
                          waitKey(500); //暂停0.5S      
                  }
          }
          int CornerNum = board_size.width * board_size.height;  // 每张图片上总的角点数
  
          //-------------以下是摄像机标定------------------
  
          /*棋盘三维信息*/
          Size square_size = Size(60, 60);         /* 实际测量得到的标定板上每个棋盘格的大小 */
          vector<vector<Point3f>> object_points;   /* 保存标定板上角点的三维坐标 */
  
          /*内外参数*/
          Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 摄像机内参数矩阵 */
          vector<int> point_counts;   // 每幅图像中角点的数量
          Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));       /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
          vector<Mat> tvecsMat;      /* 每幅图像的旋转向量 */
          vector<Mat> rvecsMat;      /* 每幅图像的平移向量 */
  
          /* 初始化标定板上角点的三维坐标 */
          int i, j, t;
          for (t = 0; t<image_count; t++)
          {
                  vector<Point3f> tempPointSet;
                  for (i = 0; i<board_size.height; i++)
                  {
                          for (j = 0; j<board_size.width; j++)
                          {
                                  Point3f realPoint;
  
                                  /* 假设标定板放在世界坐标系中z=0的平面上 */
                                  realPoint.x = i * square_size.width;
                                  realPoint.y = j * square_size.height;
                                  realPoint.z = 0;
                                  tempPointSet.push_back(realPoint);
                          }
                  }
                  object_points.push_back(tempPointSet);
          }
  
          /* 初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板 */
          for (i = 0; i<image_count; i++)
          {
                  point_counts.push_back(board_size.width * board_size.height);
          }
  
          /* 开始标定 */
          // object_points 世界坐标系中的角点的三维坐标
          // image_points_seq 每一个内角点对应的图像坐标点
          // image_size 图像的像素尺寸大小
          // cameraMatrix 输出，内参矩阵
          // distCoeffs 输出，畸变系数
          // rvecsMat 输出，旋转向量
          // tvecsMat 输出，位移向量
          // 0 标定时所采用的算法
          calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);
  
          //------------------------标定完成------------------------------------
  
          // -------------------对标定结果进行评价------------------------------
  
          double total_err = 0.0;         /* 所有图像的平均误差的总和 */
          double err = 0.0;               /* 每幅图像的平均误差 */
          vector<Point2f> image_points2;  /* 保存重新计算得到的投影点 */
          fout << "每幅图像的标定误差：\n";
  
          for (i = 0; i<image_count; i++)
          {
                  vector<Point3f> tempPointSet = object_points[i];
  
                  /* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
                  projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);
  
                  /* 计算新的投影点和旧的投影点之间的误差*/
                  vector<Point2f> tempImagePoint = image_points_seq[i];
                  Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
                  Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);
  
                  for (int j = 0; j < tempImagePoint.size(); j++)
                  {
                          image_points2Mat.at<Vec2f>(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
                          tempImagePointMat.at<Vec2f>(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
                  }
                  err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
                  total_err += err /= point_counts[i];
                  fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
          }
          fout << "总体平均误差：" << total_err / image_count << "像素" << endl << endl;
  
          //-------------------------评价完成---------------------------------------------
  
          //-----------------------保存定标结果------------------------------------------- 
          Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 保存每幅图像的旋转矩阵 */
          fout << "相机内参数矩阵：" << endl;
          fout << cameraMatrix << endl << endl;
          fout << "畸变系数：\n";
          fout << distCoeffs << endl << endl << endl;
          for (int i = 0; i<image_count; i++)
          {
                  fout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
                  fout << tvecsMat[i] << endl;
  
                  /* 将旋转向量转换为相对应的旋转矩阵 */
                  Rodrigues(tvecsMat[i], rotation_matrix);
                  fout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
                  fout << rotation_matrix << endl;
                  fout << "第" << i + 1 << "幅图像的平移向量：" << endl;
                  fout << rvecsMat[i] << endl << endl;
          }
          fout << endl;
  
          //--------------------标定结果保存结束-------------------------------
  
          //----------------------显示定标结果--------------------------------
  
          Mat mapx = Mat(image_size, CV_32FC1);
          Mat mapy = Mat(image_size, CV_32FC1);
          Mat R = Mat::eye(3, 3, CV_32F);
          string imageFileName;
          std::stringstream StrStm;
          for (int i = 0; i != image_count; i++)
          {
                  initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
                  Mat imageSource = imread(filenames[i]);
                  Mat newimage = imageSource.clone();
                  remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
                  StrStm.clear();
                  imageFileName.clear();
                  StrStm << i + 1;
                  StrStm >> imageFileName;
                  imageFileName += "_d.jpg";
                  imwrite(imageFileName, newimage);
          }
  
          fin.close();
          fout.close();
  
  #else 
                  /// 读取一副图片，不改变图片本身的颜色类型（该读取方式为DOS运行模式）
                  Mat src = imread("F:\\lane_line_detection\\left_img\\1.jpg");
                  Mat distortion = src.clone();
                  Mat camera_matrix = Mat(3, 3, CV_32FC1);
                  Mat distortion_coefficients;
  
  
                  //导入相机内参和畸变系数矩阵
                  FileStorage file_storage("F:\\lane_line_detection\\left_img\\Intrinsic.xml", FileStorage::READ);
                  file_storage["CameraMatrix"] >> camera_matrix;
                  file_storage["Dist"] >> distortion_coefficients;
                  file_storage.release();
  
                  //矫正
                  cv::undistort(src, distortion, camera_matrix, distortion_coefficients);
  
                  cv::imshow("img", src);
                  cv::imshow("undistort", distortion);
                  cv::imwrite("undistort.jpg", distortion);
  
                  cv::waitKey(0);
  #endif // DEBUG
          return 0;
  }