使用Matlab、Opencv、Ros三种方法完成相机标定

一、前置知识总结

1、相机标定的意义

        在机器视觉领域，相机的标定是一个关键的环节，它决定了机器视觉系统能否有效的定位，能否有效的计算目标物。相机标定意义在于将现实世界中的三维物体与相机图像对应的二维物体映射起来，实际上就是透视投影。

2、相机标定原理

        针对针孔相机模型， 相机标定，标定的是指内参矩阵和外参矩阵，就可以确定为一的相机模型。

• 相机的内参： 主要包含 焦距，相机主点坐标，以及畸变参数即 fx fy  cx cy k1 k2 k3 p1 p2
• 相机的外参： 包含 旋转矩阵和平移矩阵 即 R  t     

 2、相机成像几何模型

• 世界坐标系：是客观三维世界的绝对坐标系，也称客观坐标系。
• 相机坐标系（光心坐标系）：以相机的光心为坐标原点， X 轴和 Y 轴分别平行于图像坐标系的 X 轴和 Y 轴，相机的光轴为 Z 轴。
• 图像坐标系：以图像平面的中心为坐标原点，X 轴和 Y 轴分别平行于图像平面的两条垂直边， 用( x , y )表示其坐标值。 图像坐标系是用物理单位（例如毫米）表示像素在图像中的位置。
• 像素坐标系：以图像平面的左上角顶点为原点，X 轴和 Y 轴分别平行于图像坐标系的 X 轴和 Y 轴，用(u , v )表示其坐标值,单位是像素。

3、相机内外参基本原理    

        相机坐标系与世界坐标系之间的坐标转换关系称为相机外参，一般由平移和旋转两部分参数组成。求取外参的过程称之为外参标定，外参标定的结果关系到如何引导机器人进行抓取。
        相机内部参数（简称内参）只与相机内部属性（如焦距、分辨率、像素尺寸、镜头畸变等)
有关。利用相机内参可以将相机坐标系中的三维空间点变换到图像平面坐标系中，经过镜头畸变等校正过程之后可进一步变换至图像像素坐标系中的二维像素点。求取这一关系的
过程称为相机内参标定。

4、相机内参矩阵推导   

4.1 相机坐标系–>图像坐标系

        真实世界中的某点会投影在相机的成像平面上，利用针孔成像原理，空间任意一点Pc与图像点p之间的关系，Pc与相机光心Oc的连线为OcPc，与像面的交点p即为空间点Pc在图像平面上的投影。

        该过程为透视投影，利用相似三角形关系可得：

        其中f为焦距，z轴方向上s为Pc点到光心的距离，一般称为比例因子。投影关系为写成矩阵形式：

        其中，s为比例因子（s不为0），f为有效焦距（光心到图像平面的距离），(x,y,z,1)T是空间点P在坐标系oxyz中的齐次坐标，(x,y,1)T是像点p在图像坐标系OXY中的齐次坐标。

        注意：此时投影点p的单位还是mm，并不是pixel，需要进一步转换到像素坐标系。

4.2、图像坐标系–>像素平面坐标系

       像素坐标系和图像坐标系都在成像平面上，只是各自的原点和度量单位不一样。图像坐标系的原点为相机光轴与成像平面的交点，通常情况下是成像平面的中点或者叫principal point。图像坐标系的单位是mm，属于物理单位，而像素坐标系的单位是pixel，我们平常描述一个像素点都是几行几列。所以这二者之间的转换如下：

dx和dy表示每一列和每一行分别代表多少mm，即1pixel=dx mm。

5、相机透镜畸变与校正

5.1、径向畸变

 光线经过透镜中心会发生弯曲，这种现象称为径向畸变，径向畸变分为枕型畸变与筒形畸变。

  径向畸变围绕像素坐标及其泰勒展开式进行矫正。     

 其中(，)为矫正之后的坐标， (x，y)代表畸变像素坐标， r 代表成像中心距离，

5.2、切向畸变

相机畸变除了会产生径向畸变之外而且会产生切向畸变，透镜与成像平面不完全平行造成切向畸变的发生。

      切向畸变的矫正公式：

6、棋盘格图片采集

        分享一个可以生成各种标定板的网站: https://calib.io/pages/camera-calibration-pattern-generatorSingle Camera Calibrator App​​​​​​https://calib.io/pages/camera-calibration-pattern-generatorSingle Camera Calibrator App​​​​​​

和自定义检测器图案。有关这些图案的细节和包含可打印图案的PDF文件，请参见校准图案。

二、使用Opencv 获得图片

import cv2 as cv
 
print(cv.__version__)
 
def videoDemo():
    capture = cv.VideoCapture(0)
    n = 0
    while True:
        ret, frame = capture.read()
        n = n + 1
        print(f"num {n} {ret}")
        cv.imshow("Video", frame)
        c = cv.waitKey(50)
        if c == 32:
            cv.destroyAllWindows()
            cv.imwrite("/home/dofbot/camera/pic/borad.png", frame)
            cv.imshow("Video", frame)
            break
 
if __name__ == '__main__':
    videoDemo()

三、Matlab 

        支持棋盘格、圆圈格在APP中选择Camera Calibrator，如下：

        点击 Add Images，导入拍照图片。标定20张左右就够了，然后角度变一下，但不需要变太大，太大了会影响标定效果。标定板最好在视场中心，且占据较大面积。 

        修改棋盘格大小为27*27mm（我的A4纸测量是这样）

         对于标准相机，菜单栏的option里选择三阶径向畸变和斜切：

        点击Calibrate，进行相机标定：
        右上角是重建平均误差，只要平均误差小于0.5，就可以认为这是相机标定的结果是可靠的。

        把相机参数导出来，点击 Export Camera Parameters。点击确定，就可以看到matlab工作区出现了相机参数。点开这个参数，就可以得到相机的各个参数： 

四、Opencv

        为了找到棋盘的图案，使⽤函数 cv2.findChessboardCorners()。还需要传⼊图案的类型⽐如说 8x8 的格⼦或 5x5 的格⼦等。在本例中我们使⽤的9×6 的格⼦。(通常情况下棋盘都是 8x8 或者7x7)。它会返回⻆点,如果得到图像的话返回值类型(Retval)就会是 True。这些⻆点会按顺序排列(从左到右，从上到下)。 在找到这些⻆点之后我们可以使⽤函数 cv2.cornerSubPix() 增加准确度。我们使⽤函数cv2.drawChessboardCorners() 绘制图案。

        在得到了这些对象点和图像点之后,我们已经准备好来做摄像机标定了。我们要使⽤的函数是cv2.calibrateCamera()。它会返回摄像机矩阵,畸变系数,旋转和变换向量等。

所有的这些步骤都被包含在下⾯的代码中了：

import cv2
import numpy as np
import glob
 
# 找棋盘格⻆点
# 设置寻找亚像素⻆点的参数，采⽤的停⽌准则是最⼤循环次数30和最⼤误差容限0.001
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)  # 阈值
#棋盘格模板规格
w = 9   # 10 - 1
h = 6   # 7 - 1
# 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为⼆维矩阵
objp = np.zeros((w*h, 3), np.float32)
objp[:, :2] = np.mgrid[0:w,0:h].T.reshape(-1, 2)
objp = objp*18.1  # 18.1 mm
 
# 储存棋盘格⻆点的世界坐标和图像坐标对
objpoints = [] # 在世界坐标系中的三维点
imgpoints = [] # 在图像平⾯的⼆维点
#加载pic⽂件夹下所有的jpg图像
images = glob.glob('D:\pic\*.png') # 拍摄的⼗⼏张棋盘图⽚所在⽬录
 
i = 0
for fname in images:
    img = cv2.imread(fname)
    # 获取画⾯中⼼点
    #获取图像的⻓宽
    h1, w1 = img.shape[0], img.shape[1]
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    u, v = img.shape[:2]
    # 找到棋盘格⻆点
    ret, corners = cv2.findChessboardCorners(gray, (w, h), None)
    # 如果找到⾜够点对，将其存储起来
    if ret == True:
        print("i:", i)
        i = i+1
        # 在原⻆点的基础上寻找亚像素⻆点
        cv2.cornerSubPix(gray,corners, (11, 11), (-1, -1), criteria)
        #追加进⼊世界三维点和平⾯⼆维点中
        objpoints.append(objp)
        imgpoints.append(corners)
        # 将⻆点在图像上显⽰
        cv2.drawChessboardCorners(img, (w, h), corners, ret)
        cv2.namedWindow('findCorners', cv2.WINDOW_NORMAL)
        cv2.resizeWindow('findCorners', 640, 480)
        cv2.imshow('findCorners', img)
        cv2.waitKey(200)
cv2.destroyAllWindows()
#%% 标定
print('正在计算')
#标定
ret, mtx, dist, rvecs, tvecs = \
    cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
 
print("ret:", ret)
print("mtx:\n", mtx)  # 内参数矩阵
print("dist畸变值:\n", dist)  # 畸变系数 distortion cofficients = (k_©,k_ª,p_©,p_ª,k_«)
print("rvecs旋转（向量）外参:\n", rvecs)  # 旋转向量 # 外参数
print("tvecs平移（向量）外参:\n", tvecs)  # 平移向量 # 外参数
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (u, v), 0, (u, v))
print('newcameramtx外参', newcameramtx)

五、Ros 

5.1 使用ROS usb_cam驱动相机

5.1.1 安装usbcam

Kinetic：

sudo apt-get install ros-kinetic-usb-cam

Melodic：

sudo apt-get install ros-melodic-usb-cam

其他版本

sudo apt-get install ros-版本名称-usb-cam

4.1.2 修改launch文件

进入目录：

roscd usb_cam
cd launch
sudo gedit usb_cam-test.launch 

目前主要修改device和width两个参数，可以使用ls /dev/video*查看系统视频设备。

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <!-- modify the video device to your device -->
    <param name="video_device" value="/dev/video0" />
    <!-- modify the size of your device -->
    <param name="image_width" value="1280" />
    <param name="image_height" value="720" />
    <param name="pixel_format" value="yuyv" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" ou$
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

c. 启动相机

roslaunch usb_cam usb_cam-test.launch

5.2 使用ROS进行相机标定

5.2.1 ros使用相机usb_cam标定内参。

• 标定完成后点击Save可以保存标定所用的图片和参数矩阵。在终端里会输出标定产生的压缩包，默认放在/tmp目录下。

5.2.2 查看摄像头设备名

ls /dev/video*

选择自己需要显示的设备，再修改launch文件，就可以使用。

查看相机接口信息

v4l2-ctl -d 0 --all

参考相机接口信息修改usb_cam-test.launch文件，文件原内容如下

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video0" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="yuyv" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

 运行修改后的launch文件即可

roslaunch usb_cam usb_cam-test.launch

打开标定窗口

一般来说正常安装ros都是包含了camera_calibration，输入下面命令检查一下

rosdep install camera_calibration #All required rosdeps installed successfully

运行前需要根据你的棋盘格修改参数

• 一个是size参数为棋盘格内角点数量比如8x9=72个格子的棋盘格，角点个数为7x8=63个，size参数就要写7x8
• 另外一个参数为square，传入的参数为棋盘格一个小格子的宽度（注意单位为m）
• image，图像话题的原始数据默认为camera:=/usb_cam

rosrun camera_calibration cameracalibrator.py --size 10x7 --square 0.015 image:=/usb_cam/image_raw camera:=/usb_cam

5.2.3 生成标定文件

标定完成后点击Calculate会稍微有点卡顿，不要担心后台正在进行标定，完成后下面的SAVE和COMMIT按钮变为可用状态，点击SAVE即可保存标定完成后的文件。

点击commit即可把标定文件存储到系统的~/.ros/camera_info/xxx.yaml目录（Ctrl+H 显示隐藏的目）

5.2.4 在ROS中使用该参数

可以在usb_cam的launch文件中增加以下参数，重新启动usb_cam节点，即可使用该标定参数。

参数值小鱼这里写的是file:///home/dev/.ros/camera_info/ost.yaml，可打开目录~/.ros/camera_info/进行查看

<param name="camera_info_url" type="string" value="file:///home/dev/.ros/camera_info/ost.yaml"/>

5.3.3 开始运行

source devel/setup.bash
roslaunch handeye-calib aruco_start_usb_cam.launch

六、 相机信息中的内参相关内容

以Intel Realsense彩色相机为例，以下是彩色相机的信息。

# rostopic echo /right_arm_camera/color/camera_info
---
header: 
  seq: 32308
  stamp: 
    secs: 1694683940
    nsecs: 530882359
  frame_id: "right_arm_camera_color_optical_frame"
height: 720
width: 1280
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [911.4010620117188, 0.0, 645.8338623046875, 0.0, 910.145263671875, 347.9687194824219, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [911.4010620117188, 0.0, 645.8338623046875, 0.0, 0.0, 910.145263671875, 347.9687194824219, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---
header: 
  seq: 32309
  stamp: 
    secs: 1694683940
    nsecs: 564413071
  frame_id: "right_arm_camera_color_optical_frame"
height: 720
width: 1280
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [911.4010620117188, 0.0, 645.8338623046875, 0.0, 910.145263671875, 347.9687194824219, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [911.4010620117188, 0.0, 645.8338623046875, 0.0, 0.0, 910.145263671875, 347.9687194824219, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---

其中包括相机的内部参数有关的参数：

当处理相机内参时，常见的三个参数是 D、K 和 P，此外相关的还有畸变模型。

6.1、D畸变系数（distortion_coefficients）：

D 是一个通常包含五个元素的数组 [k1, k2, p1, p2, k3]，用于描述相机图像中的径向和切向畸变。

• k1 和 k2 是径向畸变系数，它们用于描述镜头的弯曲形状，通常是负数。
• p1 和 p2 是切向畸变系数，它们用于描述镜头的畸变不对称性，通常是很小的值。
• k3 是一个更高阶的径向畸变系数，通常情况下很小，有时甚至可以忽略。

D: [0.0, 0.0, 0.0, 0.0, 0.0]

6.2、K相机内参矩阵（camera_matrix）：

K 是一个3x3的矩阵，它包括了相机的内部参数，用于将相机坐标系中的三维点映射到图像平面上的二维坐标。

K 的主要元素包括：

• K[0] 和 K[4] 是焦距在图像 x 和 y 轴上的分量，通常以像素为单位。
• K[2] 和 K[5] 是光心在图像 x 和 y 轴上的坐标，也以像素为单位。
• K[8] 通常是1，是一个归一化参数。
• K 包含了焦距和主点的信息。

K: [911.4010620117188, 0.0, 645.8338623046875, 0.0, 910.145263671875, 347.9687194824219, 0.0, 0.0, 1.0]

6.3、P投影矩阵（projection_matrix）：

P 是一个3x4的矩阵，包含了相机的投影参数，用于将相机坐标系中的三维点映射到图像坐标系。

P 的元素包括了内参和外参信息。

• 其中 P[0] 和 P[5] 分别对应于焦距 fx 和 fy，用于将相机坐标系的 x 和 y 坐标映射到图像坐标系上。
• P[2] 和 P[6] 对应于光心的 x 和 y 坐标 cx 和 cy。
• P[3] 和 P[7] 通常为0，用于表示没有视角的旋转或平移。
• P[10] 通常是1，是一个归一化参数。
• 投影矩阵 P包含了相机的内部参数和畸变参数。

P: [911.4010620117188, 0.0, 645.8338623046875, 0.0, 0.0, 910.145263671875, 347.9687194824219, 0.0, 0.0, 0.0, 1.0, 0.0]

6.4、distortion_model"（畸变模型）：

distortion_model 是相机标定信息中的一个字段，用于描述相机的畸变模型。
畸变模型是用来描述相机镜头产生的畸变效应的数学模型。
在计算机视觉中，畸变通常分为径向畸变和切向畸变两种主要类型，而畸变模型用于对这些畸变进行建模和校正。
Plumb Bob (钉形畸变模型):
“plumb_bob” 畸变模型是最常见的畸变模型之一，也是默认的畸变模型。它通常用于描述径向畸变（Radial Distortion）和切向畸变（Tangential Distortion）。
径向畸变主要是由于镜头形状不完全圆形引起的，导致图像中心附近的像素与图像边缘的像素之间存在畸变。这种畸变通常用多项式函数建模。
切向畸变则是由于相机镜头不完全平行于图像平面引起的，导致图像中心附近的像素存在畸变。切向畸变通常也用多项式函数建模。
“plumb_bob” 畸变模型使用径向和切向畸变系数（D）来描述这两种畸变。