三维坐标 偏转_三维坐标系旋转

在做单目三维位姿估计(即估计目标物相对相机的姿态或相机相对目标物的姿态)时会用到solvepnp函数，

函数原型为：cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, flags]]]]) → retval, rvec, tvec

参数解释objectPoints：世界坐标系中的3D点坐标，单位mm

imagePoints：图像坐标系中点的坐标，单位像素

cameraMatrix：相机内参矩阵

distCoeffs：畸变系数

rvec：旋转矩阵

tvec：平移矩阵

useExtrinsicGuess：是否输出平移矩阵和旋转矩阵，默认为false

flags：SOLVEPNP _ITERATIVE、SOLVEPNP _P3P、SOLVEPNP _EPNP、SOLVEPNP _DLS、SOLVEPNP _UPNP

内参矩阵和畸变系数都是要通过标定得到的，这个不细讲，opencv官方提供了有标定例子(或者参考我的这篇文章：用matlab标定获取相机内参矩阵和畸变系数)。函数输出的是旋转矩阵rvec和tvec。

本文就来说说得到了这个旋转矩阵rvec后，如何得知目标物实际的角度呢~

旋转矩阵是一个3×3的正交矩阵，有3个自由度。处理旋转矩阵的问题时，通常采用旋转矩阵的方式来描述，也可以用旋转向量来表示，两者之间可以通过罗德里格斯(Rodrigues)变换来进行转换。

旋转矩阵和旋转向量间的转换请参考：旋转矩阵 和 旋转向量

其中，旋转向量的长度(模)表示绕轴逆时针旋转的角度(弧度)。

norm为求向量的模。

代码如下：theta = np.linalg.norm(rvec)

r = rvec / theta

R_ = np.array([[0, -r[2][0], r[1][0]],

[r[2][0], 0, -r[0][0]],

[-r[1][0], r[0][0], 0]])

R = np.cos(theta) * np.eye(3) + (1 - np.cos(theta)) * r * r.T + np.sin(theta) * R_

print('旋转矩阵')

print(R)

反变换也可以很容易的通过如下公式实现：

空间中三维坐标变换一般由三种方式实现，第一种是旋转矩阵和旋转向量；第二种是欧拉角；第三种是四元数。下面介绍旋转矩阵(旋转向量)与欧拉角实现三维空间坐标变换的方法以及两者之间的关系。

旋转矩阵

对于一个三维空间的点 P(x,y,z)P(x,y,z)，要将其绕 zz 轴旋转 θθ 角度是可以很简单地用旋转矩阵来表示的