相机位姿估计

前言

这部分内容博主也不是很熟悉，写下这篇博文想记录下自己当时的求解过程，也想让看到的朋友一起讨论，看看我做的对不对。下面我就开始喽。

旋转角度

欧拉角

首先介绍一个欧拉角的概念。
任何一个旋转都可以表示为依次绕着三个旋转轴旋转三个角度的组合。这三个角称为欧拉角。对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表示，如下图（蓝色是起始坐标系，红色是旋转之后的坐标系）：

相机位姿求解

如果场景的三维结构已知，利用多个控制点在三维场景中的坐标及其在图像中的透视投影坐标即可求解出相机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R，该类求解方法统称为N点透视位姿求解（Perspective-N-Point，PNP问题）。
在这里我选用的是SolvePnPRansac，SolvePnPRansac是PnP位姿估计鲁棒算法的一种，我没有学习具体的算法过程，直接对比了一下其与SolvePnP之间的区别，就选择了SolvePnPRansac，哈哈哈，原谅我的不严谨。一下是OpenCV里对这个函数的说明：

bool cv::solvePnPRansac(InputArray    objectPoints,
						InputArray    ,
						InputArray    cameraMatrix,
						InputArray    distCoeffs,
						OutputArray   rvec,
						OutputArray   tvec,
						bool  	      useExtrinsicGuess = false,
						int  	      iterationsCount = 100,
						float  	      reprojectionError = 8.0,
						double  	  confidence = 0.99,
						OutputArray   inliers = noArray(),
						int  	      flags = SOLVEPNP_ITERATIVE 
					   ) 		
参数：
In_objectPoints ：参考点在世界坐标系下的点集；
in_imagePoints：参考点在相机像平面的坐标；
in_cameraMatrix ：相机内参；
in_distCoeffs ：相机畸变系数；
out_rvec：旋转矩阵；
out_tvec：平移向量；
in_useExtrinsicGuess：如果求解PnP使用迭代算法，初始值可以使用猜测的初始值（true），也可以使用解析求解的结果作为初始值（false）；
in_iterationsCoun：Ransac算法的迭代次数，这只是初始值，根据估计外点的概率，可以进一步缩小迭代次数；（此值函数内部是会不断改变的），所以一开始可以赋一个大的值；
in_reprojectionErrr： Ransac筛选内点和外点的距离阈值，这个根据估计内点的概率和每个点的均方差（假设误差按照高斯分布）可以计算出此阈值；
in_confidence ：此值与计算采样（迭代）次数有关。此值代表从n个样本中取s个点，N次采样可以使s个点全为内点的概率；
out_inliers ：返回内点的序列。为矩阵形式；
in_ flags ：最小子集的计算模型；
           SOLVEPNP_ITERATIVE(此方案，最小模型用的EPNP，内点选出之后用了一个迭代)；
           SOLVE_P3P(P3P只用在最小模型上，内点选出之后用了一个EPNP) ；  
           SOLVE_AP3P(AP3P只用在最小模型上，内点选出之后用了一个EPNP)；
           SOLVE_EPnP(最小模型上&内点选出之后都采用了EPNP)。
返回值：
成功返回true，失败返回false；

旋转矩阵和旋转向量之间的转换

在刚才的相机位姿求解和上两篇博文中详解张正友相机标定原理（一）、详解张正友相机标定原理（二）中，有提到求解相机相对于图像的像素坐标系的旋转向量（注意：很多地方说成是旋转矩阵，其实那个函数求解出来的结果是一个向量，大家可以自行去验证，使用旋转向量的形式来表示旋转关系的），这里我们要先将其转换成旋转矩阵的形式。
旋转向量与旋转矩阵的相互转换可以用罗德里格斯公式来解决，罗得里格斯公式的推导过程网上很容易找得到。罗得里格斯公式：

OpenCV里自带了罗德里格斯函数，可以直接调用，Rodrigues()，可以实现旋转向量和旋转矩阵的相互转换（数学家真厉害），输入旋转矩阵/向量，输出旋转向量/矩阵。函数比较简单就不单独列出，最后会贴全部程序。

旋转矩阵和欧拉角之间的转换

上一部分求解得到了旋转矩阵，下边将旋转矩阵转换成欧拉角的形式。欧拉角和旋转矩阵之间的关系如下：

平移量求解

再上一个步骤中已经求得了绕X轴、Y轴、Z轴的旋转角度，在相机位姿求解时求解出了平移向量。我做的小例子中相机是用的笔记本电脑的摄像头，因此相机的位置是固定不动的，棋盘格标定板的位置是变动的。认为设置的是棋盘格角点的位置，因此求得的是相机相对于棋盘格的位置信息。将上一个步骤中求得的三个方向上的欧拉角以及位姿求解时得到的平移向量，分别一次带入绕X轴、Y轴、Z轴的旋转的式子，要不断更新（x,y,z)的值，循环代入，即可得到平移量。

最后可参考相机位姿估计的文章，写的很详细，我从中借鉴了很多东西。实在太困，写的不清楚，想起来再补充吧。

代码

#旋转矩阵转换成欧拉角，求解相机在世界坐标系中的旋转角度
def rotationMatrixToEulerAngles(rvecs):
    R = np.zeros((3, 3), dtype=np.float64)
    cv2.Rodrigues(rvecs, R)
    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
    singular = sy < 1e-6
    if  not singular:
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0
    X = x*180.0/3.141592653589793
    Y = y*180.0/3.141592653589793
    Z = z*180.0/3.141592653589793
    return X,Y,Z
    
# 经过三次旋转求解相机在世界坐标系中的坐标
def RotateByZ(Cx, Cy, Z):
    rz = Z*math.pi/180.0
    outX = math.cos(rz)*Cx - math.sin(rz)*Cy
    outY = math.sin(rz)*Cx + math.cos(rz)*Cy
    return outX, outY
def RotateByY(Cx, Cz, Y):
    ry = Y*math.pi/180.0
    outZ = math.cos(ry)*Cz - math.sin(ry)*Cx
    outX = math.sin(ry)*Cz + math.cos(ry)*Cx
    return outX, outZ
def RotateByX(Cy, Cz, X):
    rx = X*math.pi/180.0
    outY = math.cos(rx)*Cy - math.sin(rx)*Cz
    outZ = math.sin(rx)*Cy + math.cos(rx)*Cz
    return outY, outZ
    
cap = cv2.VideoCapture(0) #读入视频文件 
c=1
cap = cv2.VideoCapture(0)
width = int(cap.get(3)) # 读取摄像头分辨率参数
height = int(cap.get(4))
img = np.zeros((width,height,3),dtype=np.uint8) # 创建图像模板  
if cap.isOpened(): #判断是否正常打开 
  rval ,img = cap.read() 
else: 
  rval = False  
timeF = 150 #视频帧计数间隔频率 ，可根据需求自行调节判断时间间隔  
while rval:  #循环读取视频帧 
  rval, img = cap.read() 
  cv2.imshow('img',img)
  if(c%timeF == 0): #每隔timeF帧进行操作 
      gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)    
      ret0, corners = cv2.findChessboardCorners(gray, (6,4), flags=3)        
      if ret0 == True:            
            criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)            
            objp = np.zeros((6*4,3),np.float32)             
            objp[:,:2] = np.mgrid[0:18:3,0:12:3].T.reshape(-1,2)                        
            objpoints = [] 
            imgpoints = []         
            objpoints.append(objp)           
            corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)            
            if corners2:                
                imgpoints.append(corners2)
            else:
                imgpoints.append(corners)            
            img = cv2.drawChessboardCorners(img,(6,4),corners2,ret)        
            ret, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)
            print("相机的旋转：")
            R = rotationMatrixToEulerAngles(rvecs)
            print("请绕X轴逆时针旋转",R[0],"°")
            print("请绕Y轴逆时针旋转",R[1],"°")
            print("请绕Z轴逆时针旋转",R[2],"°")                
            # 相机坐标系下值
            x = tvecs[0]
            y = tvecs[1]
            z = tvecs[2]        
            (x, y) = RotateByZ(x, y, -1.0*R[2])
            (x, z) = RotateByY(x, z, -1.0*R[1])
            (y, z) = RotateByX(y, z, -1.0*R[0])
            Cx = x*-1
            Cx = Cx - 7.5
            Cy = y*-1
            Cy = Cy - 7.5
            Cz = z*-1
            # 输出相机位置
            print("相机的平移：")
            print("请沿X轴负方向平移",Cx[0],"cm")
            print("请沿Y轴负方向平移",Cy[0],"cm")
            print("请沿Z轴负方向平移",Cz[0],"cm")     
  c = c + 1
  cv2.waitKey(1)
cap.release()