文章目录

• 1.1 单目相机的三维姿态解算
• • 1. 1 相机模型
  • 1.2 四个坐标系
  • • 1.2.1 像素坐标系u,v) to 图像坐标系x,y)
    • 1.2.2 图像坐标系x,y) to 相机坐标系Xc,Yc,Zc)
    • 1.2.3 世界坐标系Xw,Yw，Zw) to 相机坐标系Xc,Yc,Zc)
  • 1.3 小结
  • • 1.3.1 相机的内参数 K:
    • 1.3.2 相机外参数矩阵 M
    • 1.3.3 公式总结
• 2 .实际成像模型：
• • 2.1 镜头的畸变模型
  • 2.2 镜头的偏心畸变模型 ：
• 3. ROS的相机标定
• 4. 相机姿态估计：
• 4.1 PnP问题
• • 4.2 旋转矩阵 ：
  • • 4.2.1 绕着X轴旋转 ψ
    • 4.2.2 绕着Y轴旋转 θ
    • 4.2.3 绕着Z轴旋转
  • 4.3 旋转向量
• 4.4 旋转矩阵和旋转向量相互转换：
• 5. 相机三维姿态欧式角：
• • 5.1 旋转矩阵 to 三维姿态角
• 6. 实验环节：
• 7. 参考资料 ：

1.1 单目相机的三维姿态解算

首先我们得知道：什么是单目相机的三维姿态解算

单目相机的三维姿态解算是指在计算机视觉领域中，使用单个摄像机来确定一个物体在三维空间中的位置和姿态。这是通过分析摄像机捕捉到的图像中物体的形状和几何关系来实现的。单目相机的三维姿态解算是计算机视觉领域中的一个挑战，因为没有深度信息，因此必须使用视差、光流和其他图像处理技术来推断三维空间中物体的位置。

1. 1 相机模型

数码相机图像拍摄的过程实际上是一个光学成像的过程。相机的成像过程涉及到四个坐标系世界坐标系、相机坐标系、图像坐标系、像素坐标系以及这四个坐标系的转换。
相机模型是光学成像模型的简化，目前有线性模型和非线性模型两种。实际的成像系统是透镜成像的非线性模型。最基本的透镜成像原理如图所示:

1.2 四个坐标系

世界坐标系: 是客观三维世界的绝对坐标系，也称客观坐标系。因为数码相机安放在三维空间中，我们需要世界坐标系这个基准坐标系来描述数码相机的位置，并且用它来描述安放在此三维环境中的其它任何物体的位置，用Xw,Yww表示其坐标值
相机坐标系光心坐标系):以相机的光心为坐标原点，X轴和Y轴分别平行于图像坐标系的X轴和Y轴，相机的光轴为Z轴，用Xe,Yc,Z。示其坐标值。
图像坐标系:以CCD图像平面的中心为坐标原点，又轴和Y轴分别平行于图像平面的两条垂直边，用ac,y)表示其坐标值。图像坐标系是用物理单位例如毫米)表示像素在图像中的位置。
像素坐标系:以CCD图像平面的左上角顶点为原点，X轴和Y轴分别平行于图像坐标系的X轴和Y轴，用uu)表示其坐标值。数码相机采集的图像首先是形成标准电信号的形式，然后再通过模数转换变换为数字图像。每幅图像的存储形式是M X N的数组，M行N列的图像中的每一个元素的数值代表的是图像点的灰度。这样的每个元素叫像素，像素坐标系就是以像素为单位的图像坐标系

1.2.1 像素坐标系u,v) to 图像坐标系x,y)

最近发现自己对于公式不太敏感了这是个坏消息

像素 -> 图像坐标系
只是坐标原点位置不一致，大小不一致，设计伸缩变换以及平移变化

{u=xdx+u0v=ydy+v0\left\{\begin{array}{l} u=\frac{x}{d x}+u_0 \\ v=\frac{y}{d y}+v_0 \end{array}\right. {u=dxx​+u0​v=dyy​+v0​​

这个方程组可以理解为将 $x,y)$ 坐标系中的点 $x,y)$ 映射到另一个坐标系中的点 $u,v)$ ，其中 $u$ 和 $v$ 分别是 $x,y)$ 在 $x$ 和 $y$ 方向上的导数，再加上常数 u0u_0u0​ 和 v0v_0v0​ 。

[uv1]=[1dx0u001dyv0001][xy1]\left[\begin{array}{c} u \\ v \\ 1 \end{array}\right]=\left[\begin{array}{ccc} \frac{1}{d x} & 0 & u_0 \\ 0 & \frac{1}{d y} & v_0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{l} x \\ y \\ 1 \end{array}\right] ​uv1​​=​dx1​00​0dy1​0​u0​v0​1​​​xy1​​

是的，上面两个公式是等价的，可以相互推导出来。事实上，第二个公式是第一个公式的齐 次坐标形式，它将二维平面上的点 $x,y)$ 表示为三维齐次坐标 $x,y,1)$ ，将二维平面上的点 的坐标变换为三维坐标系中的点的坐标，同时在变换过程中保持点的齐次性。

齐次坐标就是在原有的坐标基础上加一个维度，使得坐标更好的表现旋转等操作

1.2.2 图像坐标系x,y) to 相机坐标系Xc,Yc,Zc)

可以看到两个三角形相似，用初中数学可知

ΔABOc∼ΔoCOcΔPBOc∼ΔpCOc\begin{aligned} \Delta A B O_c \sim \Delta o C O_c \\ \Delta P B O_c \sim \Delta p C O_c \end{aligned} ΔABOc​∼ΔoCOc​ΔPBOc​∼ΔpCOc​​

这里看出 我们从二维推导出三维需要的信息可以从 x : f = Xc : Zc ,同理就可以得到图像的深度信息
ABoC=AOcoOc=PBpC=Xcx=Zcf=Ycy\frac{A B}{\mathrm{oC}}=\frac{A O_c}{o O_c}=\frac{P B}{p C}=\frac{X_c}{x}=\frac{Z_c}{f}=\frac{Y_c}{y} oCAB​=oOc​AOc​​=pCPB​=xXc​​=fZc​​=yYc​​

下面这个公式应该是从三维扩展到二维

x=fXcZc,y=fYcZcx=f \frac{X_c}{Z_c}, y=f \frac{Y_c}{Z_c} x=fZc​Xc​​,y=fZc​Yc​​

这个公式是从三维到二维，本质是从相似三角形来的

Zc[xy1]=[f0000f000010][XcYcZc1]Z_c\left[\begin{array}{l} x \\ y \\ 1 \end{array}\right]=\left[\begin{array}{llll} f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}\right]\left[\begin{array}{c} X_c \\ Y_c \\ Z_c \\ 1 \end{array}\right] Zc​​xy1​​=​f00​0f0​001​000​​​Xc​Yc​Zc​1​​

这个是上述公式的齐次坐标表现形式，本质上是等价的

1.2.3 世界坐标系Xw,Yw，Zw) to 相机坐标系Xc,Yc,Zc)

物体之间的坐标系表示坐标系的旋转变换加上平移变换
世界坐标系到相机坐标系转换关系也如此

平移加旋转

• [Xw,Yw,Zw]\left[X_w, Y_w, Z_w\right][Xw​,Yw​,Zw​] 是该点在世界坐标系中的坐标。
• [Xc,Yc,Zc]\left[X_c, Y_c, Z_c\right][Xc​,Yc​,Zc​] 是该点在相机坐标系中的坐标。
• $R$ 是一个 $3\times 3$ 的旋转矩阵，表示将世界坐标系中的向量旋转到相机坐标系中的向量所 需的旋转变换。
• $T$ 是一个 $3\times 1$ 的平移向量，表示将世界坐标系中的原点平移至相机坐标系中的原点所 需的平移变换。
  [XcYcZc]=R[XwYwZw]+T\left[\begin{array}{l} X_c \\ Y_c \\ Z_c \end{array}\right]=R\left[\begin{array}{l} X_w \\ Y_w \\ Z_w \end{array}\right]+T ​Xc​Yc​Zc​​​=R​Xw​Yw​Zw​​​+T
  思考 ： 向量相乘就能表示他的旋转吗？
答 ： 向量相乘本身不能表示旋转，但是向量相乘可以用来实现旋转变换。具体来说，我们可以使用矩阵乘法来表示旋转变换。

[XcYcZc1]=[RT0→1][XwYwZw1],R:3×3,T:3×1\left[\begin{array}{c} X_c \\ Y_c \\ Z_c \\ 1 \end{array}\right]=\left[\begin{array}{cc} R & T \\ \overrightarrow{0} & 1 \end{array}\right]\left[\begin{array}{c} X_w \\ Y_w \\ Z_w \\ 1 \end{array}\right], \mathrm{R}: 3 \times 3, \mathrm{~T}: 3 \times 1 ​Xc​Yc​Zc​1​​=[R0​T1​]​Xw​Yw​Zw​1​​,R:3×3, T:3×1
问： 其实我有点看不懂 ， 2x2的矩阵为什么和 4x1 的矩阵能够相乘？ 还有那个→0是什么来的？

答： 这个矩阵由一个 $3\times 3$ 的旋转矩阵 $\mathrm{R}$ 和一个 $3\times 1$ 的平移向量 $\mathrm{T}$ 组成，以及一个 $4\times 4$ 的单位矩阵中的最后一个元素为 1 。其中， 0→\overrightarrow{0}0 表示一个 $3\times 1$ 的零向量，是为了填充齐次 变换矩阵的最后一列，使得它变成一个 $4\times 4$ 的矩阵。

当你吧这个矩阵展开的时候 ，实际上是这个样子：

[RT0→1]=[R11R12R13T1R21R22R23T2R31R32R33T30001]\left[\begin{array}{cc} R & T \\ \overrightarrow{0} & 1 \end{array}\right]=\left[\begin{array}{cccc} R_{11} & R_{12} & R_{13} & T_1 \\ R_{21} & R_{22} & R_{23} & T_2 \\ R_{31} & R_{32} & R_{33} & T_3 \\ 0 & 0 & 0 & 1 \end{array}\right] [R0​T1​]=​R11​R21​R31​0​R12​R22​R32​0​R13​R23​R33​0​T1​T2​T3​1​​
.> 是不是一目了然了

1.3 小结

阿木视频省略了很多信息，要静下心来看 ,不过这也说明她的可自学性很差

好了 接下来我们硬啃， 首先我们得知道这些参数的定义 ，有些我们可以轻易看出 f 是焦距 的就不说了

1.3.1 相机的内参数 K:

K=[ax0u00ayv0001]K=\left[\begin{array}{ccc} a_x & 0 & u_0 \\ 0 & a_y & v_0 \\ 0 & 0 & 1 \end{array}\right] K=​ax​00​0ay​0​u0​v0​1​​

这个矩阵描述了相机的内部参数，包括水平和垂直方向上的焦距 axa_xax​ 和 aya_yay​ ，相机主点的像素 坐标 u0,v0)\leftu_0, v_0\right)u0​,v0​) ，以及一个缩放因子 1 。这个矩阵将归一化的相机坐标映射到成像平面坐标。

1.3.2 相机外参数矩阵 M

M=[m11m12m13m14m21m22m23m24m31m32m33m34]M=\left[\begin{array}{llll} m_{11} & m_{12} & m_{13} & m_{14} \\ m_{21} & m_{22} & m_{23} & m_{24} \\ m_{31} & m_{32} & m_{33} & m_{34} \end{array}\right] M=​m11​m21​m31​​m12​m22​m32​​m13​m23​m33​​m14​m24​m34​​​

这个矩阵辐述了相机的外部参数，包括旋转矩阵 $R$ 和平移向量 $t$ 。 这个矩阵将一个点从世界 坐标系变换到相机坐标系。

问 ·那么相机的内参数的值 和外参数的值怎么得到呢？
答 ： 相机的内参数和外参数可以通过相机标定得到。相机标定是指通过拍摄一组已知的物体或者棋盘格图案，在计算机上对图像进行分析，得到相机的内参数和外参数的过程。在相机标定时，需要拍摄一组已知的场景，比如一个平面的棋盘格图案，或者一组已知的三维点。然后，通过在图像中检测出这些点的位置，可以得到相机的内参数和外参数。

首先我们来看第一个等号：
Zcuv1)=1dx0u001dyv0001)f0000f000010)Rt0T1)XwYwZw1)Z_c\left\begin{array}{l} u \\ v \\ 1 \end{array}\right)=\left\begin{array}{ccc} \frac{1}{d x} & 0 & u_0 \\ 0 & \frac{1}{d y} & v_0 \\ 0 & 0 & 1 \end{array}\right)\left\begin{array}{llll} f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}\right)\left\begin{array}{ll} R & t \\ 0^T & 1 \end{array}\right)\left\begin{array}{l} X_w \\ Y_w \\ Z_w \\ 1 \end{array}\right) Zc​​uv1​​=​dx1​00​0dy1​0​u0​v0​1​​​f00​0f0​001​000​​R0T​t1​)​Xw​Yw​Zw​1​​

1.3.3 公式总结

这个其实是上面对应那张图的 ，变换方法 ，从右边往左边看 ,世界坐标系乘以一个旋转变换的矩阵
进行刚体变换变成了一个摄像机坐标，再乘以一个透视投影坐标变成了图像坐标，对于前面熟悉的话，这个其实很好看懂的
然后进行下一步 ：
他把 公式化简了

也就是说 投影透视矩阵乘以变换等于相机内置参数 K

相机内置参数 乘以 旋转平移量就等于 M ，相机外置参数 ，可以看出相机外置参数包括内置参数

这就是世界坐标系变换成相机坐标系的全过程了

2 .实际成像模型：

由于相机光学系统存在加工和装配的误差，透镜就不能满足物和成像相似三角形关系

2.1 镜头的畸变模型

相机标定是通过观察已知的物体来获取相机内外参数和畸变参数的过程。一般来说，需要使用多组已知三维坐标的物体点在图像中的对应二维坐标来进行标定。通过标定得到的相机参数和畸变参数，可以用于矫正相机采集的图像。

参数	含义	获取方式
xr,yrx_r, y_rxr​,yr​	畸变前的像素坐标	由相机获取
xi,yix_i, y_ixi​,yi​	畸变后的像素坐标	由相机获取
σx\sigma_xσx​	切向畸变对 xrx_rxr​ 的影响值	由 xr,yr,k1,k2x_r, y_r, k_1, k_2xr​,yr​,k1​,k2​ 计算得到
σy\sigma_yσy​	切向畸变对 yry_ryr​ 的影响值	由 xr,yr,k1,k2x_r, y_r, k_1, k_2xr​,yr​,k1​,k2​ 计算得到
k1k_1k1​	径向畸变参数	相机标定得到
k2k_2k2​	径向畸变参数	相机标定得到

其中 mrxr,yr)m_r\leftx_r, y_r\right)mr​xr​,yr​) 表示实际投影点的像平面坐标系下的物理坐标， mixi,yi)m_i\leftx_i, y_i\right)mi​xi​,yi​) 表示理想投影点的像平 面坐标系下的物理坐标。镜头的畸变模型可表示为:
{xi=xr+σxyi=yr+σy\left\{\begin{array}{l} x_i=x_r+\sigma_x \\ y_i=y_r+\sigma_y \end{array}\right. {xi​=xr​+σx​yi​=yr​+σy​​
σx\sigma_xσx​ 和 σy\sigma_yσy​ 是非线性畸变值，它包括径向畸变、偏心畸变和薄棱镜畸变等。
理论上来说镜头都存在径向和切向畸变，但是通常径向畸变较大，切向畸变较小。径向畸变的模型 可由下面的模型来表示:
{σx=xrk1r2+k2r4+⋯)σy=yrk1r2+k2r4+⋯)\left\{\begin{array}{l} \sigma_x=x_r\leftk_1 r^2+k_2 r^4+\cdots\right) \\ \sigma_y=y_r\leftk_1 r^2+k_2 r^4+\cdots\right) \end{array}\right. {σx​=xr​k1​r2+k2​r4+⋯)σy​=yr​k1​r2+k2​r4+⋯)​
其中 k1,k2,k3,…k_1, k_2, k_3, \ldotsk1​,k2​,k3​,… 表示径向畸变系数， r=xr2+yr2r=\sqrt{x_r^2+y_r^2}r=xr2​+yr2​​ ，通常情况下径向畸变系数只考虑到一阶 或二阶就可以满足精度需求了。

2.2 镜头的偏心畸变模型 ：

偏心畸变模型是由于多个光学镜头的光轴不能完全共线产生的，这种畸变是由径向和切向畸变共同 构成的
具体参数如下

参数	含义	获取方式
xr,yrx_r, y_rxr​,yr​	畸变前的像素坐标	由相机获取
xi,yix_i, y_ixi​,yi​	畸变后的像素坐标	由相机获取
r2r^2r2	畸变前的像素距离，即径向畸变参数	由 xr,yrx_r, y_rxr​,yr​ 计算得到
k1k_1k1​	径向畸变参数	相机标定得到
k2k_2k2​	径向畸变参数	相机标定得到
p1p_1p1​	切向畸变参数	相机标定得到
p2p_2p2​	切向畸变参数	相机标定得到
σx\sigma_xσx​	切向畸变对 xrx_rxr​ 的影响值	由 xr,yr,p1,p2x_r, y_r, p_1, p_2xr​,yr​,p1​,p2​ 计算得到
σy\sigma_yσy​	切向畸变对 yry_ryr​ 的影响值	由 xr,yr,p1,p2x_r, y_r, p_1, p_2xr​,yr​,p1​,p2​ 计算得到

数学模型可表示如下:
{σx=p1xr3xr2+yr2)+2p2xryr+o[xr,yr)4]σy=p2xr3xr2+yr2)+2p1xryr+o[xr,yr)4]\left\{\begin{array}{l} \sigma_x=p_1 x_r\left3 x_r^2+y_r^2\right)+2 p_2 x_r y_r+o\left[\leftx_r, y_r\right)^4\right] \\ \sigma_y=p_2 x_r\left3 x_r^2+y_r^2\right)+2 p_1 x_r y_r+o\left[\leftx_r, y_r\right)^4\right] \end{array}\right. ⎩⎨⎧​σx​=p1​xr​3xr2​+yr2​)+2p2​xr​yr​+o[xr​,yr​)4]σy​=p2​xr​3xr2​+yr2​)+2p1​xr​yr​+o[xr​,yr​)4]​
其中 p1,p2p_1, p_2p1​,p2​ 为切向畸变系数。薄棱镜畸变是由于镜头设计制造缺陷和加工安装所造成的，如镜头与 相机成像平面有一个很小的倾角等。因为薄棱镜畸变非常小，通常不考虑，这里只考虑径向畸变和 偏心畸变，畸变总的可以表示为:
{xi=xr+xrk1r2+k2r4)+p1xr3xr2+yr2)+2p2xryryi=xr+yrk1r2+k2r4)+p2xr3xr2+yr2)+2p1xryr\left\{\begin{array}{l} x_i=x_r+x_r\leftk_1 r^2+k_2 r^4\right)+p_1 x_r\left3 x_r^2+y_r^2\right)+2 p_2 x_r y_r \\ y_i=x_r+y_r\leftk_1 r^2+k_2 r^4\right)+p_2 x_r\left3 x_r^2+y_r^2\right)+2 p_1 x_r y_r \end{array}\right. {xi​=xr​+xr​k1​r2+k2​r4)+p1​xr​3xr2​+yr2​)+2p2​xr​yr​yi​=xr​+yr​k1​r2+k2​r4)+p2​xr​3xr2​+yr2​)+2p1​xr​yr​​

补充参数含义

3. ROS的相机标定

• 不是很重要 过 ！

4. 相机姿态估计：

相机姿态估计的基本思想是利用相机的内参以及已知物体在空间坐标与一一对应的图像坐标的投影 关系来求出此时物体相对于已知物体在三维空间坐标的外参，也即是旋转矩阵以及平移向量。

4.1 PnP问题

此关键算法是解决N点透视投影问题，也称作PnP Perspective-n-Point) 问题。

N点透视投影的问题：
。N点透视投影问题是计算机图形学领域中的一个经典问题，它用于将三维场景投影到二维平面上。
在N点透视投影问题中，我们希望将一个三维场景投影到一个二维平面上，以便于显示或者处理。为了实现这个目标，我们需要确定一个投影矩阵，它可以将三维场景中的每个点映射到二维平面上的对应点。
在传统的三维计算机图形学中，通常使用四个点的透视投影矩阵来解决N点透视投影问题。这四个点通常称为“裁剪空间”，其中两个点位于近裁剪面上，两个点位于远裁剪面上。通过确定这四个点的位置，并使用透视投影矩阵，我们可以将三维场景投影到二维平面上。
对于不同的应用场景和需求，可能需要使用不同数量的点来解决N点透视投影问题。在实际应用中，通常需要通过试错和调整来确定最适合的投影矩阵和点数，以获得最佳的投影效果。

3D点 $X,Y,Z)$ 与 $2Dx,y)$ 点的联系

OpenCV提供 solvePnP 以及 solvePnPRansac 函数来求解出相机相对于已知物体的三维空间坐标 系的旋转和平移向量。
根据上式 $x=M\cdot [R\mid t]\cdot X$ ，要求出外参数 $[R\mid t]$ ，我们必须知道相机基本矩阵 $M$ ，以及已知物体 在三维空间坐标点 $X$ ，与之对应的图像像素坐标点 $x$ 。

具体代码示例

4.2 旋转矩阵 ：

矩阵运算显然是计算机三维坐标变换最简单方便的计算方法，因此在 opencv、opengl、工业机器 人等开发中，提到位姿旋转变换，多半用的是旋转矩阵。然而，用矩阵来表示一个旋转关系有两个 缺点:
首先，通过旋转矩阵不能直观地看出旋转的方向和角度，假设给定一个旋转矩阵，要求旋转方向不 变，旋转角度变成一半，那么新的旋转矩阵计算起来就比较麻烦了。
另一方面，旋转变换本身只有 3 个自由度，但旋转矩阵有 9 个元素，因此旋转矩阵中的元素不是相互 独立的，这在非线性优化中会带来问题。

4.2.1 绕着X轴旋转 ψ

Rxψ)=[1000cos⁡ψ−sin⁡ψ0sin⁡ψcos⁡ψ]R_x\psi)=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \psi & -\sin \psi \\ 0 & \sin \psi & \cos \psi \end{array}\right] Rx​ψ)=​100​0cosψsinψ​0−sinψcosψ​​

ψ ：psi /普西/

4.2.2 绕着Y轴旋转 θ

Ryθ)=[cos⁡θ0sin⁡θ010−sin⁡θ0cos⁡θ]R_y\theta)=\left[\begin{array}{ccc} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end{array}\right] Ry​θ)=​cosθ0−sinθ​010​sinθ0cosθ​​

θ ：theta /西塔/

4.2.3 绕着Z轴旋转

Rzϕ)=[cos⁡ϕ−sin⁡ϕ0sin⁡ϕcos⁡ϕ0001]R_z\phi)=\left[\begin{array}{ccc} \cos \phi & -\sin \phi & 0 \\ \sin \phi & \cos \phi & 0 \\ 0 & 0 & 1 \end{array}\right] Rz​ϕ)=​cosϕsinϕ0​−sinϕcosϕ0​001​​

φ=Φ：phi /five/

4.3 旋转向量

旋转向量
向量旋转公式最早由 Rodrigues 提出，用一个三维向量来表示三维旋转变换，该向量的方向是旋转 轴，其模则是旋转角度。百度百科中有其详细的介绍与推导，我在这边只列一下最重要的公式。 设旋转向量的单位向量为 $r$ ，模为 $\theta$ 。三维点 或者说三维向量) $p$ 在旋转向量 $r$ 的作用下变换至 p′p^{\prime}p′ ， 则:

p′=cos⁡θ⋅p+1−cos⁡θ)p⋅r)r+sin⁡θ⋅r×pp^{\prime}=\cos \theta \cdot p+1-\cos \theta)p \cdot r) r+\sin \theta \cdot r \times p p′=cosθ⋅p+1−cosθ)p⋅r)r+sinθ⋅r×p

参数	定义	获取方式
$p$	需要旋转的向量	通常由问题或任务确定
$r$	旋转轴的方向向量	通常由问题或任务确定
$\theta$	旋转角度	通常由问题或任务确定
p′p^\primep′	旋转后的向量	由公式计算得到
$\cos \theta$	旋转角度的余弦值	cos⁡θ=x∣r∣\cos\theta = \frac{x}{\left|{\bf r}\right|}cosθ=∣r∣x​，其中$x$是$p$在$\mathbf{r}$方向上的投影，$\left|\mathbf{r}\right|$是$\mathbf{r}$的模长
$\sin \theta$	旋转角度的正弦值	sin⁡θ=∣r×p∣∣r∣∣p∣\sin\theta = \frac{\left|{\bf r}\times {\bf p}\right|}{\left|{\bf r}\right|\left|{\bf p}\right|}sinθ=∣r∣∣p∣∣r×p∣​，其中$\mathbf{r}\times \mathbf{p}$是$\mathbf{r}$和$\mathbf{p}$的叉积
$1-\cos \theta )p\cdot r)r$	旋转轴上的投影分量	$1-\cos \theta )p\cdot r)r$是$p$在$\mathbf{r}$方向上的投影分量
$\sin \theta \cdot r\times p$	旋转轴垂直方向上的分量	$\sin \theta \cdot r\times p$是$\mathbf{r}$和$\mathbf{p}$的叉积方向上的分量

4.4 旋转矩阵和旋转向量相互转换：

opencv中有函数Rodrigues)用于旋转矩阵和旋转向量的转换。参照opencv文档，设旋转向量的单 位向量 r=[rxryrz]Tr=\left[r_x r_y r_z\right]^Tr=[rx​ry​rz​]T ，旋转角度为 $\theta$ ，对应的旋转矩阵为 $R$ ，则 $r$ 到 $R$ 的转换是：

R=cos⁡θI+1−cos⁡θ)rrT+sin⁡θ[0−rzryrz0−rx−ryrx0]R=\cos \theta I+1-\cos \theta) r r^T+\sin \theta\left[\begin{array}{ccc} 0 & -r_z & r_y \\ r_z & 0 & -r_x \\ -r_y & r_x & 0 \end{array}\right] R=cosθI+1−cosθ)rrT+sinθ​0rz​−ry​​−rz​0rx​​ry​−rx​0​​

$$\text{ 其中 }I\text{ 是三阶单位矩阵。反过来 }R\text{ 到 }r\text{ 的转换则可以利用等式: }$$
R−RT2=sin⁡θ[0−rzryrz0−rx−ryrx0]\frac{R-R^T}{2}=\sin \theta\left[\begin{array}{ccc} 0 & -r_z & r_y \\ r_z & 0 & -r_x \\ -r_y & r_x & 0 \end{array}\right] 2R−RT​=sinθ​0rz​−ry​​−rz​0rx​​ry​−rx​0​​

5. 相机三维姿态欧式角：

相机三维姿态欧式角
偏航角就是绕Yaw AxisZ轴)旋转的角度
滚转角就是绕Rol $\mathrm{Axis}Y$ 轴)旋转的角度
俯仰角就是绕Pitch AxisX轴)旋转的角度

这个又是经典的阿木可学性太差的问题，不能说它没内容，就说她直接放在 谁能快速学懂她

这里我都不想琢磨她写的写的这段伪代码讲的是什么了
我们来看看另一个更加容易懂的解释

5.1 旋转矩阵 to 三维姿态角

旋转矩阵表示为：
R=[R11R12R13R21R22R23R31R32R33]R=\left[\begin{array}{lll} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{array}\right] R=​R11​R21​R31​​R12​R22​R32​​R13​R23​R33​​​
其中， R11、R12、R13、R21、R22、R23、R31、R32、R33R_{11} 、 R_{12} 、 R_{13} 、 R_{21} 、 R_{22} 、 R_{23} 、 R_{31} 、 R_{32} 、 R_{33}R11​、R12​、R13​、R21​、R22​、R23​、R31​、R32​、R33​ 均为实数。
计算物体的俯仰角 pitch, 可以通过旋转矩阵的第三行向量在 $xy$ 平面上的投影长度 计算:
cos⁡pitch)=R132+R232\cos p i t c h)=\sqrt{R_{13}^2+R_{23}^2} cospitch)=R132​+R232​​
计算物体在绕 $y$ 轴旋转的角度 $yaw$ ，可以使用反正切函数 $\mathrm{atan}2y,x)$ 计算：
yaw =atan⁡2R21,R11)\text { yaw }=\operatorname{atan} 2\leftR_{21}, R_{11}\right)  yaw =atan2R21​,R11​)
其中， $\mathrm{atan}2y,x)$ 函数是一个带有两个参数的反正切函数，可以处理所有四个象 限的角度值。
计算物体在绕 $x$ 轴旋转的角度roll，可以使用反正切函数atan $2y,x)$ 计算:
roll =atan⁡2−R32,R33)\text { roll }=\operatorname{atan} 2\left-R_{32}, R_{33}\right)  roll =atan2−R32​,R33​)

其中，负号是为了得到正确的角度值。

以上三个角度值，即为物体的姿态角。

这里的atan 是 arctan的简写 即反正切函数

同时我们注意到 ，求一个角度Θ的反正切值就是求这个角度本身，公式推导如下 ：
tan⁡−1tan⁡θ))=tan⁡−1yx)=arctan⁡yx)=θ\begin{aligned} \tan ^{-1}\tan \theta)) & =\tan ^{-1}\left\frac{y}{x}\right) \\ & =\arctan \left\frac{y}{x}\right) \\ & =\theta \end{aligned} tan−1tanθ))​=tan−1xy​)=arctanxy​)=θ​

6. 实验环节：

 rosrun camera calibration cameracalibrator.py .-size 8x6 .-square 0.024 image:=/zed/zed node/rgb/image-rectolor

可以看到标定过程

点到上面开始标定

得到一系列输出参数

根据输出更新我们这个标定的量：

更新下检测二维码的长度

代码讲解我们跳过

直接执行这段代码

python catkin_ws/src/zed-reader/zed_reader_aruco.py

可以看到检测到二维码

同时我们可以通过订阅这个话题得到消息

rostopic echo /vision_results/pose

这样的话就可以订阅到她的位置信息：

7. 参考资料 ：

阿木实验室 Promethus 二次开发课程

查看全文

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.dgrt.cn/a/2122321.html

如若内容造成侵权/违法违规/事实不符，请联系一条长河网进行投诉反馈，一经查实，立即删除！

相关文章：

【无人机Prometheus】视觉部分第一课学习笔记

文章目录1.1 单目相机的三维姿态解算1. 1 相机模型1.2 四个坐标系1.2.1 像素坐标系u,v) to 图像坐标系x,y)1.2.2 图像坐标系x,y) to 相机坐标系Xc,Yc,Zc)1.2.3 世界坐标系Xw,Yw，Zw) to 相机坐标系Xc,Yc,Zc)1.3 小结1.3.1 相机的内参数 K:1.3.2 相机外参数矩阵……

编程日记2023/3/31 7:18:20

多线程：生产者和消费者等待唤醒机制)代码实现

生产者和消费者模式概述【应用】 概述 1.生产者消费者模式是一个十分经典的多线程协作的模式，弄懂生产者消费者问题能够让我们 对多线程编程的理解更加深刻。 2.所谓生产者消费者问题，实际上主要是包含了两类线程： 一类是生产者线程用于生……

编程日记2023/3/31 7:17:53

Java常量、运算符、条件语句（超详细讲解/保姆级教学）

🙈作者简介：练习时长两年半的Java up主 🙉个人主页：老茶icon 🙊 ps:点赞👍是免费的，却可以让写博客的作者开兴好久好久😎 📚系列专栏：Java全栈，计……

编程日记2023/3/31 7:17:13

操作系统 计算机操作系统教程笔记

进程是操作系统对一个正在运行的程序的一种抽象。在一个系统上可以同时运行多个进程，而每个进程都好像在独占地使用硬件。而并发运行，则是说一个进程的指令和另一个进程的指令是交错执行的。 在大多数系统中，需要运行的进程数是多于可以运行它……

编程日记2023/3/31 7:16:45

openGauss数据库安装及使用

openGauss数据库安装及使用openGauss数据库什么是openGauss系统架构运行环境技术指标安装与登录安装环境获取安装包单节点安装openGauss数据库 openGauss是一款开源关系型数据库管理系统，采用木兰宽松许可证v2发行。openGauss内核深度融合华为在数据库领域多年的经验……

编程日记2023/3/31 7:16:16

CSS的选择器，元素显示模式，背景

1. CSS 的复合选择器
1.1 什么是复合选择器
在 CSS 中，可以根据选择器的类型把选择器分为基础选择器和复合选择器，复合选择器是建立在基础选择器之上，对基本选择器进行组合形成的。  复合选择器可以更准确、更高效的选择目标元素&#xf……

编程日记2023/3/31 7:15:41

CSS 的三大特性

1. CSS 的三大特性
CSS 有三个非常重要的三个特性：层叠性、继承性、优先级。
1.1 层叠性
相同选择器给设置相同的样式，此时一个样式就会覆盖（层叠）另一个冲突的样式。层叠性主要解决样式冲突 的问题 层叠性原则： ……

编程日记2023/3/31 7:15:40

CSS 定位与元素的显示与隐藏

CSS 定位
1.1 为什么需要定位
提问： 以下情况使用标准流或者浮动能实现吗？
某个元素可以自由的在一个盒子内移动位置，并且压住其他盒子. 提问： 以下情况使用标准流或者浮动能实现吗？当我们滚动窗口的时候&#xff……

编程日记2023/3/31 7:15:39

谷粒商城二十订单服务

rabbitmq相关知识
// 静态页面的引入，静态资源上传nginx等192.168.56.10 gulimall.com
192.168.56.10 search.gulimall.com
192.168.56.10 item.gulimall.com
192.168.56.10 auth.gulimall.com
192.168.56.10 cart.gulimall.com
192.168.56.10 order.gulimall.com-……

编程日记2023/3/31 7:15:38

CSS 高级技巧

CSS 高级技巧
1. 精灵图
1.1 为什么需要精灵图 一个网页中往往会应用很多小的背景图像作为修饰，当网页中的图像过多时，服务器就会频繁地接收和发送
请求图片，造成服务器请求压力过大，这将大大降低页面的加载速度。
因此&#……

编程日记2023/3/31 7:15:38

cocos3.10环境配置

https://blog.csdn.net/hfutdog/article/details/71368171…

编程日记2023/4/16 15:00:54

cocos-lua中添加lfs的库

方案一：
1、转载：https://blog.csdn.net/sc1987910/article/details/79622014
2、上述转载的博客会编译不通过，需要在lfs.h中添加： #ifdef __cplusplus extern "C" { #endif #include <lua.h> #include <lau……

编程日记2023/4/16 15:00:53

转载各种编码格式的讲解

https://blog.csdn.net/Deft_MKJing/article/details/79460485…

编程日记2023/4/16 15:00:53

cocos-lua游戏中横屏，竖屏动态切换

1、转载自 https://blog.csdn.net/oJianYue12/article/details/80927700
2、注意点: AndroidManifest.xml文件中的配置 android:configChanges"orientation|screenSize"；android:screenOrientation"sensorLandscape"
这两个配置需要根据自……

编程日记2023/4/16 15:00:52

lua require机制

https://www.cnblogs.com/softidea/p/5242941.html…

编程日记2023/4/16 15:00:52

lua程序设计（一）

一、lua的概述 1.简单易学 2.比较灵活，数据结构只有表一种
二、 1、变量命名规范：建议遵循驼峰规则；区分大小写（注意变量命名）；避免下划线开头并紧接着一个大写字母（可能与lua中对 应的全局变量……

编程日记2023/4/16 15:00:51

cocos2d-3.10 整合版本连接

官方给出的是在：http://www.cocos2d-x.org/filedown/CocosForWin-v3.10.exe 如果下载不了，可以在这里下http://cdn.cocos2d-x.org/CocosForWin-v3.10.exe…

编程日记2023/4/16 15:00:51

code=45, title=禁止登录, message=登录失败，建议升级最新版本后重试，或通过问题反馈与我们联系。

如果你是采用 java 开发的，你可以参考本文章，java 和 kotlin 都是可以相互转换的。 在解决之前，先说明环境: JDK版本：java version "17.0.3.1" 【Oracle JDK】 Kotlin版本：1.8.20 采取simbot核心包开发&am……

编程日记2023/4/16 15:00:51

cocos2d屏幕适配方案以及winsize,framesize,VisibleSize,contentSize的区别和联系

一、首先要吐槽小cocos官方把这个问题描述的很模糊，讲解的不清不楚，很多人工作两三年的人也不明白。
二、言归正传：阐述下winsize，framesize,VisibleSize,contentSize的概念。frameSize表示的是屏幕的分辨率， 这里多说……

编程日记2023/4/16 15:00:50

lua协程详解

https://www.cnblogs.com/zrtqsk/p/4374360.html…

编程日记2023/4/16 15:00:50