相机坐标系转换世界坐标系，zed&imu&depth

1.问题背景

相机的安装的是带一定的倾角，而且车辆是行驶在非铺装路面，车辆是会倾斜的。

1.1 根据内参消除畸变，修正焦点

转换关系

焦距（fx, fy）：焦距参数表示成像平面与相机光心之间的距离，它们决定了成像的大小。在数学上，fx 和 fy
是归一化焦距，它们与相机的实际焦距 f 以及像素尺寸 dx 和 dy（单位通常为毫米/像素）有关，具体关系为 fx = f * dx，fy
= f * dy。焦距参数影响成像的视角和物体在图像中的大小。如果相机的分辨率提高，即像素数增加，而物理尺寸保持不变，则 fx 和 fy 会相应增加 。

主点（cx, cy）：主点是成像平面上与相机光心投影点相对应的点，通常位于图像的中心位置。在像素坐标系中，cx 和 cy
表示这个点的位置，它们是图像的几何中心。如果图像被裁剪或缩放，这些值会相应地改变，因为它们与图像的原点有关

    def grab_depth(self , msg_depth , x  , y ):
        # 获取相机内参
        x = int(x)
        y = int(y)
        fx = self.get_parameter('fx').value
        fy = self.get_parameter('fy').value
        cx = self.get_parameter('cx').value
        cy = self.get_parameter('cy').value
        
        # 从深度图像消息中获取深度数据
        depths = np.frombuffer(msg_depth.data, dtype=np.float32)

        # 计算x,y像素在相机坐标系中的位置
        CenterIdx = x  + msg_depth.width * y
        z_l = float(depths[CenterIdx])
        if math.isnan(z_l) or math.isinf(z_l):
            return []
        x_l = (x - cx) / fx * z_l
        y_l = (y - cy) / fy * z_l
        return [x_l, y_l, z_l]

2.2四元数转换欧拉角

			float yaw = std::atan2(2 * (w * z + x * y), (1 - 2 * (x * x + y * y)));//偏航角
            float pitch = std::asin(-2 * (z * x - y * w));//俯仰角
            float roll = std::atan2(2 * (w * y + z * x), (1 - 2 * (y * y + z * z)));//翻滚角

3.另一个小问题

zed官方提供了一版conf文件，也就是内参文件，供你消除畸变，二维平面的深度值转换为三维世界的物理坐标。但是官方提供的文件有一定的误差，导致有误。