分享

三维坐标系旋转

 bit54song 2020-03-21


在做单目三维位姿估计(即估计目标物相对相机的姿态或相机相对目标物的姿态)时会用到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标定获取相机内参矩阵和畸变系数)。函数输出的是旋转矩阵rvectvec

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


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

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

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

norm为求向量的模。

代码如下:

  1. theta = np.linalg.norm(rvec)
  2. r = rvec / theta
  3. R_ = np.array([[0, -r[2][0], r[1][0]],
  4. [r[2][0], 0, -r[0][0]],
  5. [-r[1][0], r[0][0], 0]])
  6. R = np.cos(theta) * np.eye(3) + (1 - np.cos(theta)) * r * r.T + np.sin(theta) * R_
  7. print('旋转矩阵')
  8. print(R)

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


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

旋转矩阵

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

 欧拉角

 

 此处得到结论:自旋转的“先转的放前面”


定角(Fixed angles)

围绕固定的坐标系转动。固定坐标系的原点,坐标系再围绕已经固定的轴转动,全程原坐标系不动

注意!移动位置的顺序可以调换,但是旋转的顺序不能调换,结果不一样。

以X-Y-Z型为例子:即先围绕X轴进行转动γ°,然后围绕Y轴进行转动β°,最后围绕Z轴进行转动α°。注意逆时针为正方向。

X-Y-Z型公式:

重点先转的轴的\large R放后面运算,如下

代码:

  1. def isRotationMatrix(R):
  2. Rt = np.transpose(R) #旋转矩阵R的转置
  3. shouldBeIdentity = np.dot(Rt, R) #R的转置矩阵乘以R
  4. I = np.identity(3, dtype=R.dtype) # 3阶单位矩阵
  5. n = np.linalg.norm(I - shouldBeIdentity) #np.linalg.norm默认求二范数
  6. return n < 1e-6 # 目的是判断矩阵R是否正交矩阵(旋转矩阵按道理须为正交矩阵,如此其返回值理论为0)
  7. def rotationMatrixToAngles(R):
  8. assert (isRotationMatrix(R)) #判断是否是旋转矩阵(用到正交矩阵特性)
  9. sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0]) #矩阵元素下标都从0开始(对应公式中是sqrt(r11*r11+r21*r21)),sy=sqrt(cosβ*cosβ)
  10. singular = sy < 1e-6 # 判断β是否为正负90°
  11. if not singular: #β不是正负90°
  12. x = math.atan2(R[2, 1], R[2, 2])
  13. y = math.atan2(-R[2, 0], sy)
  14. z = math.atan2(R[1, 0], R[0, 0])
  15. else: #β是正负90°
  16. x = math.atan2(-R[1, 2], R[1, 1])
  17. y = math.atan2(-R[2, 0], sy) #当z=0时,此公式也OK,上面图片中的公式也是OK的
  18. z = 0
  19. return np.array([x, y, z])

 备注:np.linalg.norm(求范数)

 举例:

由角度推旋转矩阵

由旋转矩阵推角度


欧拉角(Euler angles)

“自旋转”,围绕当下(自己)的坐标系某轴转动,就是每次旋转,都固定被围绕的某一轴,另两轴动。

每次旋转,整个坐标系都会改变位置。

以Z-Y-Z型为例的公式:

重点先转的轴的\large R放前面运算,如下

举例:

矩阵转角度:

注意:自旋转的“先转的放前面”


欧拉角转旋转矩阵 

欧拉角通过将刚体绕过原点的轴(i,j,k)旋转θ,分解成三步,如下图(蓝色是起始坐标系,而红色的是旋转之后的坐标系) 
这里写图片描述 
如果将每一个角度用旋转矩阵表示如下: 
这里写图片描述 
所以,容易得到,欧拉角转旋转矩阵如下: 
这里写图片描述

代码:

  1. /**
  2. 欧拉角计算对应的旋转矩阵
  3. **/
  4. Mat eulerAnglesToRotationMatrix(Vec3f &theta)
  5. {
  6.     // 计算旋转矩阵的X分量
  7.     Mat R_x = (Mat_<double>(3,3) <<
  8.                1,       0,              0,
  9.                0,       cos(theta[0]),   -sin(theta[0]),
  10.                0,       sin(theta[0]),   cos(theta[0])
  11.                );
  12.     // 计算旋转矩阵的Y分量
  13.     Mat R_y = (Mat_<double>(3,3) <<
  14.                cos(theta[1]),    0,      sin(theta[1]),
  15.                0,               1,      0,
  16.                -sin(theta[1]),   0,      cos(theta[1])
  17.                );
  18.     // 计算旋转矩阵的Z分量
  19.     Mat R_z = (Mat_<double>(3,3) <<
  20.                cos(theta[2]),    -sin(theta[2]),      0,
  21.                sin(theta[2]),    cos(theta[2]),       0,
  22.                0,               0,                  1);
  23.     // 合并 
  24.     Mat R = R_z * R_y * R_x;
  25.     return R;
  26. }

 旋转矩阵转欧拉角 

将旋转矩阵表示如下: 
这里写图片描述 
则可以如下表示欧拉角: 
这里写图片描述

代码:

  1. /**
  2. * 功能: 1. 检查是否是旋转矩阵
  3. **/
  4. bool isRotationMatrix(Mat &R)
  5. {
  6. Mat Rt;
  7. transpose(R, Rt);
  8. Mat shouldBeIdentity = Rt * R;
  9. Mat I = Mat::eye(3,3, shouldBeIdentity.type());
  10. return norm(I, shouldBeIdentity) < 1e-6;
  11. }
  12. /**
  13. * 功能: 1. 通过给定的旋转矩阵计算对应的欧拉角
  14. **/
  15. Vec3f rotationMatrixToEulerAngles(Mat &R)
  16. {
  17. assert(isRotationMatrix(R));
  18. float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) + R.at<double>(1,0) * R.at<double>(1,0) );
  19. bool singular = sy < 1e-6; // If
  20. float x, y, z;
  21. if (!singular) {
  22. x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
  23. y = atan2(-R.at<double>(2,0), sy);
  24. z = atan2(R.at<double>(1,0), R.at<double>(0,0));
  25. } else {
  26. x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
  27. y = atan2(-R.at<double>(2,0), sy);
  28. z = 0;
  29. }
  30. return Vec3f(x, y, z);
  31. }

旋转向量转欧拉角(经过四元数)代码如下:

  1. # 从旋转向量转换为欧拉角
  2. def get_euler_angle(rotation_vector):
  3. # calculate rotation angles
  4. theta = cv2.norm(rotation_vector, cv2.NORM_L2)
  5. # transformed to quaterniond
  6. w = math.cos(theta / 2)
  7. x = math.sin(theta / 2)*rotation_vector[0][0] / theta
  8. y = math.sin(theta / 2)*rotation_vector[1][0] / theta
  9. z = math.sin(theta / 2)*rotation_vector[2][0] / theta
  10. ysqr = y * y
  11. # pitch (x-axis rotation)
  12. t0 = 2.0 * (w * x + y * z)
  13. t1 = 1.0 - 2.0 * (x * x + ysqr)
  14. print('t0:{}, t1:{}'.format(t0, t1))
  15. pitch = math.atan2(t0, t1)
  16. # yaw (y-axis rotation)
  17. t2 = 2.0 * (w * y - z * x)
  18. if t2 > 1.0:
  19. t2 = 1.0
  20. if t2 < -1.0:
  21. t2 = -1.0
  22. yaw = math.asin(t2)
  23. # roll (z-axis rotation)
  24. t3 = 2.0 * (w * z + x * y)
  25. t4 = 1.0 - 2.0 * (ysqr + z * z)
  26. roll = math.atan2(t3, t4)
  27. print('pitch:{}, yaw:{}, roll:{}'.format(pitch, yaw, roll))
  28. # 单位转换:将弧度转换为度
  29. Y = int((pitch/math.pi)*180)
  30. X = int((yaw/math.pi)*180)
  31. Z = int((roll/math.pi)*180)
  32. return 0, Y, X, Z

在3D 空间中,表示物体的旋转可以由三个欧拉角来表示: 
pitch围绕X轴旋转,叫俯仰角。 
yaw围绕Y轴旋转,叫偏航角。 
roll围绕Z轴旋转,叫翻滚角。 
这三个角的顺序对旋转结果有影响。 
欧拉角

(欧拉角与四元数的转换关系: 
http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html)

四元数到欧拉角的转换公式如下: 
 四元数到欧拉角的转换公式 
arctan和arcsin的结果为[-pi/2,pi/2],不能覆盖所有的欧拉角,因此采用atan2代替arctan: 

用atan2代替arctan

参考:http://blog./2016/12/rotation-in-3d-space

https://blog.csdn.net/aic1999/article/details/82415357#commentBox

https://www.cnblogs.com/aoru45/p/9781540.html

https://blog.csdn.net/u012423865/article/details/78219787#commentsedit

https://blog.csdn.net/u013512448/article/details/77804161

    本站是提供个人知识管理的网络存储空间,所有内容均由用户发布,不代表本站观点。请注意甄别内容中的联系方式、诱导购买等信息,谨防诈骗。如发现有害或侵权内容,请点击一键举报。
    转藏 分享 献花(0

    0条评论

    发表

    请遵守用户 评论公约

    类似文章 更多