实验设计 | 单目相机标定

1. 相机选型

实验选用的是usb免驱相机
厂商：金乾象
相机型号：S2A17
分辨率：640 x 480
帧率：120 fps
镜头：2.8mm

2. 相机驱动代码

2.1 插入相机，查看设备名 video0

ls /dev/

## 2.2 修改配置文件 camera_param.prototxt
/home/tdt/catkin_ws /src/RoboRTS/roborts_camera/config/camera_param.prototxt

2.3 打开相机

rosrun roborts_camera roborts_camera_node	//！！ 命令1

3. 相机标定代码

3.1 标定代码

/home/tdt/catkin_ws /src/RoboRTS/roborts_camera/test/cameraCalibration.cpp

#include <vector>
#include <thread>
#include <mutex>

#include<opencv2/opencv.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>

#include<image_transport/image_transport.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>

#include<stdio.h>
#include<math.h>
#include<vector>

#include <iostream>  
#include <fstream>
using namespace std;
using namespace cv;
static std::vector<std::vector<cv::Point2f>> camera_calibrate_points_sequence_; 
Mat src_img;

void MouseHandle(int event, int x, int y, int flags, void * param){
  if (event == EVENT_LBUTTONDOWN){
    *(int*)param=1;
  }else if(event == EVENT_RBUTTONDOWN){
    *(int*)param=0;
  }
}

/**
 * @brief 相机标定
 */
int CameraCalibrate(cv::Mat input)
{
  Size board_size = Size(11,8);
  static vector<Point2f> image_points_buf;  /* 缓存每幅图像上检测到的角点 */
  static bool calibrate_flag= false;
  static bool init_flag=true;

  if (input.empty())
    return 0;
  int mouse_flag=-1;
  if(init_flag==true)
  {
    namedWindow("Calibrate");
    setMouseCallback("Calibrate",MouseHandle,&mouse_flag);
    init_flag==false;
  }
  if(camera_calibrate_points_sequence_.size()<15)
  {
    if (!calibrate_flag) 
    {
    Mat show;
    input.copyTo(show);
#if 0
    putText( show, "click mouse left ",Point(100,100),0,1.5,Scalar(255,255,255),1);
    putText( show, "to find "+to_string(camera_calibrate_points_sequence_.size()+1)+"of 15 Chessboard Corners",Point(100,150),0,1.5,Scalar(255,255,255),1);
    putText( show, "Warning:Make sure whole chessboard in view",Point(100,200),0,1,Scalar(0,0,255),2);
    putText( show, "or it will crash",Point(100,250),0,1,Scalar(0,0,255),2);
#endif 
    putText( show, "click mouse left ",Point(10,10),0,0.5,Scalar(255,255,255),1);
    putText( show, "to find "+to_string(camera_calibrate_points_sequence_.size()+1)+"of 15 Chessboard Corners",Point(10,30),0,0.5,Scalar(255,255,255),1);
    putText( show, "Warning:Make sure whole chessboard in view",Point(10,50),0,0.5,Scalar(0,0,255),1);
    putText( show, "or it will crash",Point(10,70),0,0.5,Scalar(0,0,255),1);
    imshow("Calibrate", show);
    waitKey(200);
    if (mouse_flag == 1) 
    {
      Mat view_gray;
      cvtColor(input, view_gray, CV_RGB2GRAY);
      Mat show;
      input.copyTo(show);
      putText( show, "finding...",Point(input.size()/2),0,2,Scalar(0,255,0),2);
      putText( show, "warning :If it crash, Please wait 30 seconds",Point(input.size()/5),0,1,Scalar(0,0,255),1);
      imshow("Calibrate", show);
      waitKey(1);
      if (findChessboardCorners(input, board_size, image_points_buf))
      {
	find4QuadCornerSubpix(view_gray, image_points_buf, cv::Size(5, 5)); //对粗提取的角点进行精确化
	calibrate_flag= true;
	drawChessboardCorners(input, board_size, image_points_buf, false); //在图片中标记角点
#if 0
	putText(input, to_string(camera_calibrate_points_sequence_.size()+1)+"of15",Point(input.size().width/3,100),0,1.5,Scalar(255,255,255),2);
	putText(input, "whether to save these ChessboardCorners?",Point(input.size().width/3,200),0,1,Scalar(255,255,255),2);
	putText( input, "mouse left:save",Point(0,input.size().height/2),0,1.5,Scalar(0,255,0),1);
	putText( input, "mouse right:delete ",Point(input.size().width*2/3,input.size().height/2),0,1.5,Scalar(255,0,0),1);
#endif 
	putText(input, to_string(camera_calibrate_points_sequence_.size()+1)+"of15",Point(input.size().width/2,50),0,1,Scalar(0,255,0),1);
	putText(input, "whether to save these ChessboardCorners?",Point(0,70),0,0.5,Scalar(0,255,0),1);
	putText(input, "mouse left:save",Point(0,90),0,0.5,Scalar(0,255,0),1);
	putText(input, "mouse right:delete ",Point(0,110),0,0.5,Scalar(0,255,0),1);
	imshow("Calibrate", input);
      }
     }
     waitKey(1);
    }else {
      waitKey(500);
      if (mouse_flag == 1)
      {
	camera_calibrate_points_sequence_.push_back(image_points_buf);  //保存亚像素角点
	std::cout << "第" << camera_calibrate_points_sequence_.size() << "张标定图" << std::endl;
	calibrate_flag= false;
      } else if(mouse_flag==0){
	calibrate_flag= false;
      }
    }
  }else{
    destroyWindow("Calibrate");
    std::cout << "开始标定………………";
    //	棋盘三维信息
    cv::Size square_size = cv::Size(2, 2);  //!! 实际测量得到的标定板上每个棋盘格的大小厘米
    std::vector<std::vector<cv::Point3f> > object_points; //!! 保存标定板上角点的三维坐标
    cv::Mat camera_matrix = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0)); //!! 摄像机内参数矩阵
    int point_counts=board_size.width*board_size.height;  //!! 每幅图像中角点的数量
    cv::Mat dist_coeffs = cv::Mat(5, 1, CV_32FC1, cv::Scalar::all(0)); //!! 摄像机的5个畸变系数：k1,k2,p1,p2,k3 
    std::vector<cv::Mat> tvecs_mat;  /* 每幅图像的旋转向量 */
    std::vector<cv::Mat> rvecs_mat; /* 每幅图像的平移向量 */
    //	初始化标定板上角点的三维坐标
    int i, j, t;
    for (t = 0; t < camera_calibrate_points_sequence_.size(); t++)
    {
      std::vector<cv::Point3f> temp_point_set;
      for (i = 0; i < board_size.height; i++)
      {
	for (j = 0; j < board_size.width; j++)
	{
	  cv::Point3f realPoint;
	  //!!	假设标定板放在世界坐标系中z=0的平面上
	  realPoint.x = i * square_size.width;
	  realPoint.y = j * square_size.height;
	  realPoint.z = 0;
	  temp_point_set.push_back(realPoint);
	}
      }
      object_points.push_back(temp_point_set);
    }
    /* 开始标定 */
    calibrateCamera(object_points, camera_calibrate_points_sequence_, input.size(), camera_matrix, dist_coeffs, rvecs_mat, tvecs_mat, 0);
    std::cout<<camera_matrix<<std::endl;
    std::cout<<dist_coeffs<<std::endl;
    std::cout<<"标定完成！\n";
    //对标定结果进行评价
    std::cout<<"开始评价标定结果………………\n";
    double total_err = 0.0; /* 所有图像的平均误差的总和 */
    double err = 0.0; /* 每幅图像的平均误差 */
    std::vector<cv::Point2f> image_points2; /* 保存重新计算得到的投影点 */
    std::cout<<"\t每幅图像的标定误差：\n";
    for (int i=0;i<camera_calibrate_points_sequence_.size();i++)
    {
      std::vector<cv::Point3f> tempPointSet=object_points[i];
      /* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
      projectPoints(tempPointSet,rvecs_mat[i],tvecs_mat[i],camera_matrix,dist_coeffs,image_points2);
      /* 计算新的投影点和旧的投影点之间的误差*/
      std::vector<cv::Point2f> tempImagePoint = camera_calibrate_points_sequence_[i];
      cv::Mat temp_image_point_mat = cv::Mat(1,tempImagePoint.size(),CV_32FC2);
      cv::Mat image_points2mat = cv::Mat(1,image_points2.size(), CV_32FC2);
      for (int j = 0 ; j < tempImagePoint.size(); j++)
      {
	image_points2mat.at<cv::Vec2f>(0,j) = cv::Vec2f(image_points2[j].x, image_points2[j].y);
	temp_image_point_mat.at<cv::Vec2f>(0,j) = cv::Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
      }
      std::cout<<"flag"<<std::endl;
      err = norm(image_points2mat, temp_image_point_mat, cv::NORM_L2);
      total_err += err/=  point_counts;
      std::cout<<"第"<<i+1<<"幅图像的平均误差："<<err<<"像素"<<std::endl;
    }
    std::cout<< "总体平均误差：" << total_err /camera_calibrate_points_sequence_.size()<<"像素"<<std::endl;
    std::cout<<"评价完成！"<<std::endl;
    std::string file_address="cam_internal.yaml";
    cv::FileStorage fs(file_address,cv::FileStorage::APPEND);
    fs<<"matrix"<<camera_matrix;
    fs<<"dist_coeffs"<<dist_coeffs;
    fs.release();
    return 1;
  }
  return 0;
}
void imageCalllback0(const sensor_msgs::ImageConstPtr& msg)
{ 
  src_img= cv_bridge::toCvShare(msg, "bgr8")->image;
  CameraCalibrate(src_img);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_listener");
  ros::NodeHandle n;
#if 1
  cv::namedWindow("video");
#endif
  image_transport::ImageTransport it(n);
  image_transport::Subscriber sub0 = it.subscribe( "/back_camera/image_raw", 1, imageCalllback0 );
  ros::spin();
  cv::destroyWindow("video0");
  cv::destroyWindow("video1");
  return 0;
}

3.2 标定流程

1. 运行标定节点

rosrun roborts_camera cameraCalibration		//！！ 命令2

2. 根据界面提示，左击屏幕开始标定
   
   
3. 标定结果

内参矩阵：
478.8168938441036, 0, 323.1301936359724;
0, 478.7259829377906, 234.0399689180573;
0, 0, 1]

畸变系数：
[-0.4196256538026443;
0.2874980757724543;
0.0001012630024451066;
0.0001084131550102817;
-0.1624896434055047]

评估误差：
0.034138像素

4. 张氏相机标定原理

参考blog：
[1] 从零开始学习「张氏相机标定法」（一）相机成像模型
[2] 从零开始学习「张氏相机标定法」（二）单应矩阵
[3] 从零开始学习「张氏相机标定法」（三）推导求解
[4] 从零开始学习「张氏相机标定法」（四）优化算法前传
[5] 从零开始学习「张氏相机标定法」（五）优化算法正传
[6] 相机标定的基本原理与经验分享