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

算法流程图如下

1. 输入同步：

• 订阅三个主题：
  • 深度图像 (depth_image)。
  • 相机信息 (depth_info)。
  • 2D目标检测 (detections)。
• 使用 message_filters.ApproximateTimeSynchronizer 来同步这些输入，以确保处理的消息是对应的。

2. 计算2D边界框的中心位置和尺寸

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

3. 使用深度图像找到对应的深度（Z）值

• 2D边界框的中心点对应图像中的一个像素点，通过深度图像可以获取该像素点的深度值（即Z值）。深度图像的每个像素值表示从相机到该点的距离，通常以毫米或米为单位。

4. 通过相机的内参将其转换为世界坐标系中的3D位置（X, Y, Z）

• 相机内参矩阵 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)。

5. 计算3D边界框的尺寸

• 在得到 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边界框的宽度和高度。

6. 生成3D边界框消息

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

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

7. 发布结果：

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

8. 坐标转换过程：

1. 获取转换信息

   • 获取从当前坐标系到目标坐标系（例如base_link）的转换信息。这个信息包括：
     • 平移向量：描述如何将物体从一个坐标系移动到另一个坐标系的平移操作。
     • 旋转四元数：描述如何旋转物体以对齐到目标坐标系。

2. 应用平移和旋转

   • 接受生成的3D边界框以及从get_transform`方法获得的平移和旋转信息。转换过程如下：

   a. 平移

   • 首先，对3D边界框的中心位置（x, y, z）应用平移操作。平移向量translation被加到3D边界框的中心位置坐标上，使得3D边界框的位置移动到目标坐标系。

   b. 旋转

   • 然后，使用旋转四元数rotation对3D边界框的中心位置和尺寸进行旋转。这个旋转会改变3D边界框的方向，使其对齐到目标坐标系的方向。

   具体过程：

   • 位置转换 ：通过qv_mult方法，旋转四元数与位置向量进行乘法运算，得到旋转后的位置，再加上平移向量，最终得到转换后的3D边界框中心位置。
   • 尺寸转换：同样的旋转操作也会应用到边界框的尺寸向量上，以确保3D边界框在新坐标系中的形状和方向正确。

3. 更新坐标系信息

   • 最后，3D边界框的信息（如位置、尺寸）被更新为新坐标系中的数据，并且3D边界框的frame_id属性被设置为目标坐标系（target_frame，例如base_link）。