opencv学习记录3-相机标定与姿态解算

相机标定与姿态解算

1.相关概念学习

1.1相机模型

确定空间物体表面某点的三维几何位置与其在图像中对应点之间的相互关系，必须建立相机成像的几何模型

1.2四个重要坐标系：

相机的几何模型，用来反映环境信息到图像信息之间的映射关系

• 世界坐标系

• 相机坐标系

• 图像成像坐标系

• 图像像素坐标系

整个成像过程分为两次映射过程

1.世界坐标系->相机坐标系

2.相机坐标系->像素坐标系

描述第一次映射模型的参数叫做 相机外参 ，描述第二次映射模型的参数叫做相机内参，它分为 内参矩阵和 畸变参数矩阵

1.3内参矩阵

描述了相机坐标系到像素坐标系之间的坐标转换

• 水平缩放因子fx，垂直缩放因子fy: 物体投影到成像平面时水平、竖直的 放缩倍数 不同，坐标系转换时需要乘以 缩放因子

• 水平平移因子Cx，垂直平移因子Cy: 从成像坐标系转换到像素坐标系需要带上 平移项

等号左边是像素坐标系，右边是内参矩阵和相机坐标系

1.4镜头畸变

产生原因：

• 透镜形状：由于透镜形状造成的畸变称为 径向畸变 ，径向畸变分为 桶形畸变 和 枕形畸变 ，径向畸变有 越远离光心，畸变越大的特征

• 桶形畸变：过分偏折

• 枕形畸变：偏折不够

• 安装误差：相机的组装过程中由于不能使透镜和成像面严格平行造成的畸变称为 切向畸变

1.5畸变的修正

径向畸变：

创建一个映射函数F(r)来描述畸变发生前后像素点的对应关系，其中 r为像素点离图像中点的距离

切向畸变：

将两个修正综合一下得到：

完整的【原像素点->去畸变后像素点】的函数对应：

畸变参数矩阵：

1.6如何求相机内参？

张正有棋盘标定法

一个标定板：

1.7姿态解算

由于实物的坐标从相机坐标系投影到成像坐标系时，z轴方向信息全部丢失（纵深信息）

重获纵深信息——PNP解算

其中

为所求的坐标变换，其中 R为旋转3×3矩阵，t为平移1×3矩阵

即：

以上 12 个参数，说明最少用 6 对 3D-2D 匹配点 就可以实现对于矩阵的求解 ——> 直接线性变换（ DLT ）

如何更简便地求解？—— 用三个参数即可描述这种旋转变换： （欧拉角）

yaw角(绕z-轴转角)、pitch角(绕y-轴转角)、roll角(绕x-轴转角)

所以最少需要 3 对 3D-2D 匹配点 就可以实现求解。通常使用 4 对点，另一对点用于验证、减小误差

综上：给出 4 对 3D-2D 匹配点对 ，经过PNP解算后就可以获得一个 旋转矩阵R 和一个 平移矩阵T，这两个矩阵描述了世界坐标系与相机坐标系之间的转换方式——即描述了物体的 相对位姿

1.8旋转矩阵到欧拉角

把旋转矩阵转换成三个欧拉角：

得到

分别为 roll角(绕x-轴转角)，pitch角(绕y-轴转角)，yaw角(绕z-轴转角)

ps：（之后补充学习）
1.solvePnP解算出的是旋转向量，需要经过罗德里斯公式映射后才能变成旋转矩阵
2.要用卡尔曼滤波预测的方法来跟踪对面车辆的移动状态并进行预测

1.9总结

1. 已知多对 3D-2D匹配点对 的世界坐标系坐标以及像素坐标系坐标

2. 已知相机内参，即映射过程

3. 利用PNP解算还原丢失的纵深信息

4. 所有信息重新代回抽象模型，反解映射函式，求得相对姿态

2.实现一次相机标定

运用opencv自带函数

2.1代码

注：

常用cv2.findChessCorners来确定是否找到了角点，再输入物点，用cv2.cornerSubPix，寻找亚像素坐标，这样更精准

import numpy as np
import cv
#glob ：返回所有匹配的文件路径列表
import glob

#EPS表示迭代次数达到最大次数时停止
#MAX_ITER表示角点位置变化的最小值已经达到最小时停止迭代
#两个条件都要满足
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30 , 0.001)

#以z轴为 0 取平面上的点
#6*8矩阵来储存角点

objp = np.zeros((6*8, 3), np.float32)
objp[:, :2] = np.mgrid[0:8, 0:6].T.reshape(-1, 2)

#objpoints 来储存世界坐标系下的三维点坐标
#imgpoints 来储存像素坐标系下的二维点坐标
objpoints = []
imgpoints = []

#选出所有拍摄的图片
images = glob.glob('C:/Users/DELL/Desktop/camera/*.JPG')

#转为黑白图片，提高效率
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    
#找到棋盘格上的角点(返回到像素坐标系)
#ret来判断是否找到了角点
ret, corners = cv2.findChessboardCorners(gray, (8, 6), None)
if ret == True :
    #输入物点，（11，11）表示窗口大小，（-1，-1）表示忽略掉的细微结构
    #这样会更精确
    objpoints.append(objp)
    corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1, -1), criteria)
    imgpoints.append(corners2)
    
#画出找到的点(corners2->img,即亚像素坐标系到图像坐标系)
img = cv2.drawChessboardCorners(img, (8, 6), corners2, ret)
cv2.namedWindow('img', 0)
cv2.imshow('img', img)
while cv2.waitKey(100) != 27:
    if cv2.getWindowProperty('img', cv2.WND_PROP_VISIBLE) <= 0:
        break
cv2.destroyAllWindows()

#直接调用calibrateCamera即可完成求解矩阵等操作
#mrx：相机内参数
#dist：畸变参数
#rvecs：旋转向量
#tveces：平移向量
ret,mrx,dist,rvecs,tveces = cv2.calibrateCamera(objpoints,imgpoints, (8, 6),
None, None)

print(mrx)

结果：

测得相机内参矩阵：

[[ 4.70909596e+03 0.00000000e+00 7.80898770e+01]
[ 0.00000000e+00 2.71572676e+03 -5.91631123e+01]
[ 0.00000000e+00 0.00000000e+00 1.00000000e+00]]

畸变参数：

[ 0.11052443 -0.04990666 -0.03609475 -0.00387042 0.0139093 ]

2.2相机内参矩阵各参数含义说明

• f：焦距

• dx：像素x方向宽度，单位毫米

• f/dx：使用像素来描述x轴方向焦距的长度

• f/dy：使用像素来描述y轴方向焦距的长度

• u0,v0：主点的实际位置，单位是像素

3.实现一次去畸变

3.1所给定数据如下

内参：

[[ 458.654 0.000 367.215]
[ 0.000 457.296 248.375]
[ 0.000 0.000 1.000 ]]

畸变参数：

[ -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0]

原图:

3.2具体操作

import numpy as np
import cv2

#运用mat函数创建矩阵，用来存放畸变参数和内参
#mrx：相机内参数
#dist：畸变参数
mrx = np.mat([[458.654, 0.0, 367.215],[0.0, 457.296, 248.375], [0.0, 0.0, 1.0]])
dist = np.mat([ -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0])

#读取图像
img = cv2.imread('C:/Users/DELL/Desktop/distorted.png')
#返回该图片的大小和维度
h, w = img.shape[:2]
#用cv2.getOptimalNewCameraMatrix得到新的不畸变的相机参数
#roi：尺寸
newcameramtx,roi = cv2.getOptimalNewCameraMatrix(mrx, dist, (w, h), 1, (w, h))

#用cv2.undistort去畸变
dst = cv2.undistort(img, mrx, dist, None, newcameramtx)
x,y,w,h = roi
dst = dst[y:y+h, x:x+w]
cv2.imwrite('C:/Users/DELL/Desktop/1.png',dst)

去畸变后结果：

3.实现姿态解算

solvepnp通过2D点和3D点求解相机的位姿 (R,t)，在opencv3中常用的方法是 epnp， DLS，迭代法

3.1二维码角点检测

import cv2
import numpy as np

# 读取图像
src = cv2.imread( 'C:/Users/DELL/Desktop/test.png')

# 灰度转换，提高效率
gray = cv2.cvtColor(src, cv2.COLOR_BGR2GRAY)
# 检测
qrcoder = cv2.QRCodeDetector()
# 解码
#points：得到的二维码四个点的坐标信息
codeinfo, points, straight_qrcode = qrcoder.detectAndDecode(gray)
result = np.copy(src)
# 描绘轮廓
cv2.drawContours(result, [np.int32(points)], 0, (0, 0, 255), 2)

cv2.namedWindow('result', 0)
cv2.imshow("result", result)
cv2.waitKey(0)
cv2.destroyAllWindows()

结果：

3.2姿态解算

# 使用opencv中的函数solvepnp
import cv2
import numpy as np

mrx = np.mat([[ 4.70909596e+03 , 0.00000000e+00, 7.80898770e+01],[
0.00000000e+00 , 2.71572676e+03 ,-5.91631123e+01], [ 0.00000000e+00 ,
0.00000000e+00 , 1.00000000e+00]])
dist = np.mat([ 0.11052443, -0.04990666 ,-0.03609475, -0.00387042 , 0.0139093 ])

#objpoints：世界坐标系的坐标（假设一个
tag_size_half = 0.025
objPoints = np.mat([[-tag_size_half, -tag_size_half, 0],
                    [tag_size_half, -tag_size_half, 0],
                    [tag_size_half, tag_size_half, 0],
                    [-tag_size_half, tag_size_half, 0]], dtype=np.float64)
#3.1中获得的二维码四个点的2d坐标信息
imgPoints = np.mat([[ 687, 484 ],[1520, 437 ],[1476.684 , 1558.8041],[ 647 ,
1411 ]])

# cv2.Rodrigues()：进行旋转矩阵和旋转向量之间的相互转化
#rvecs：相机坐标系中物体的旋转
#tvec：相机坐标系中物体的平移
retval,rvec,tvec = cv2.solvePnP(objPoints, imgPoints, mrx, dist)

print(during1)
print( rvec, tvec)

结果：

[[-0.37706436]
[ 1.25688629]
[-0.13645112]]

[[0.02891978]
[0.05420701]
[0.14332578]]

（参考资料csdn，知乎）

四种方法对比：

1. 迭代法：只能用四个共面的点来求解

2. P3P：可以使用任意 4 个特征点求解，不要求共面

3. EPNP：只要特征点数量大于 3 就可求出正解

4. BA（光束法平差）：这种方法将相机位姿和三维点位置放在一起进行重投影误差最小化的优化

（ BA 非线性？）

精确度： BA最精确

用时比较： P3P方法用时最短

经查询，p3p在使用时仍有一些问题：

1. P3P值利用3个点的信息，当给定的匹配点多于3组时，难以利用更多的信息。
2. 如果3D点或2D点受到噪声影响，或者存在误匹配，则算法失效。

EPnP的思路和P3P差不多，相对P3P来说，EPnP利用更多的信息，用迭代的方式对相机位姿进行优化，以尽可能消除噪声的影响。

ps:

P3P使用时最后还需要一对点用于验证。