深度相机，通过2d检测得到目标坐标系的3d检测框

news2024/9/22 12:31:47

算法流程图如下
在这里插入图片描述

通过yolo模型获得每个目标的2D边界框。这些边界框由其中心点坐标 (center_x, center_y) 和尺寸 (size_x, size_y) 表示。
center_x 和 center_y 表示2D边界框在图像平面中的中心点坐标，而 size_x 和 size_y 则分别表示边界框的宽度和高度。

相机内参矩阵 K 包含了相机的焦距（fx 和 fy）以及主点坐标（光学中心）px 和 py。相机内参矩阵的形式如下：
```
K = [ fx   0  px
       0  fy  py
       0   0   1 ]
```
根据针孔相机模型，图像中的2D点 (u, v) 和3D空间中的点 (X, Y, Z) 的关系可以表示为：
```
X = Z * (u - px) / fx
Y = Z * (v - py) / fy
```
其中，Z 是从深度图像中获取的深度值，(u, v) 是图像中的像素坐标，即2D边界框的中心点坐标 (center_x, center_y)。
通过这个公式，计算出在3D空间中对应的 X 和 Y 坐标，这些坐标连同深度 Z 一起定义了2D边界框在世界坐标系中的3D位置 (X, Y, Z)。

在得到 Z 值之后，可以进一步计算3D边界框的宽度 W 和高度 H。这些值通过以下公式计算：
```
W = Z * (size_x / fx)
H = Z * (size_y / fy)
```
这里的 size_x 和 size_y 是2D边界框的宽度和高度，fx 和 fy 是相机的焦距，Z 代表的是2D边界框中心点的深度值。
通过这些公式，可以将2D边界框的尺寸投影到3D空间，得到3D边界框的宽度和高度。

最终，代码将计算出的3D位置 (X, Y, Z) 和尺寸 (W, H, D) 组装成一个 BoundingBox3D 消息，该消息包含了3D边界框的中心位置和尺寸。

这整个过程将图像中的2D信息和深度图像中的距离信息结合起来，从而生成了目标在世界坐标系中的3D表示。

最后，生成的3D检测结果（包括边界框和关键点），生成的3D边界框转换到目标坐标系（例如base_link）。这个方法通过应用平移和旋转操作，将3D边界框对齐到指定的target_frame参数对应的坐标系中。

获取转换信息
- 获取从当前坐标系到目标坐标系（例如base_link）的转换信息。这个信息包括：
  - 平移向量：描述如何将物体从一个坐标系移动到另一个坐标系的平移操作。
  - 旋转四元数：描述如何旋转物体以对齐到目标坐标系。
应用平移和旋转
- 接受生成的3D边界框以及从get_transform`方法获得的平移和旋转信息。转换过程如下：
a. 平移
- 首先，对3D边界框的中心位置（x, y, z）应用平移操作。平移向量translation被加到3D边界框的中心位置坐标上，使得3D边界框的位置移动到目标坐标系。
b. 旋转
- 然后，使用旋转四元数rotation对3D边界框的中心位置和尺寸进行旋转。这个旋转会改变3D边界框的方向，使其对齐到目标坐标系的方向。
具体过程：
- 位置转换：通过qv_mult方法，旋转四元数与位置向量进行乘法运算，得到旋转后的位置，再加上平移向量，最终得到转换后的3D边界框中心位置。
- 尺寸转换：同样的旋转操作也会应用到边界框的尺寸向量上，以确保3D边界框在新坐标系中的形状和方向正确。
更新坐标系信息
- 最后，3D边界框的信息（如位置、尺寸）被更新为新坐标系中的数据，并且3D边界框的frame_id属性被设置为目标坐标系（target_frame，例如base_link）。