UPDATE Vision box：点云与相机坐标

Vision/tool/utils.py

@@ -14,6 +14,66 @@ import psutil
 from psutil._common import bytes2human
 
 
+def uv_to_XY(cameraType, u, v):
+    """
+    像素坐标转相机坐标
+    Args:
+        cameraType:
+        u:
+        v:
+
+    Returns:
+    如本：
+
+ExtrinsicMatrix:
+[-0.9916700124740601, -0.003792409785091877, 0.12874870002269745, 0.10222162306308746, -0.003501748666167259, 0.9999907612800598, 0.002483875723555684, -0.08221593499183655, -0.12875692546367645, 0.0020123394206166267, -0.9916741251945496, 0.6480034589767456, 0.0, 0.0, 0.0, 1.0]
+
+IntrinsicParameters:
+[2402.101806640625, 0.0, 739.7069091796875,
+0.0, 2401.787353515625, 584.73046875,
+0.0, 0.0, 1.0]
+
+distortion:
+[-0.04248141124844551, 0.24386045336723328, -0.38333430886268616, -0.0017840253422036767, 0.0007602088153362274]
+
+图漾：
+
+depth image format list:
+	0 -size[640x480]	-	 desc:DEPTH16_640x480
+	1 -size[1280x960]	-	 desc:DEPTH16_1280x960
+	2 -size[320x240]	-	 desc:DEPTH16_320x240
+delth calib info:
+	calib size       :[1280x960]
+	calib intr       :
+	(1048.3614501953125, 0.0, 652.146240234375,
+	0.0, 1048.3614501953125, 500.26397705078125,
+	0.0, 0.0, 1.0)
+	calib extr       : (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+	calib distortion : (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+
+    """
+    x = None
+    y = None
+    zc = 1  # 设深度z为1
+    if cameraType == 'RVC':
+        u0 = 739.70
+        v0 = 584.73
+        fx = 2402.10
+        fy = 2401.78
+        x = (u-u0)*zc/fx
+        y = (v-v0)*zc/fy
+    elif cameraType == 'Pe':
+        u0 = 652.14
+        v0 = 500.26
+        fx = 1048.36
+        fy = 1048.36
+
+        x = (u - u0) * zc / fx
+        y = (v - v0) * zc / fy
+    return x, y, zc
+
+
+
 def out_bounds_dete(pm_y, pm_x, piont_y, piont_x):
     if piont_y>=pm_y:
         piont_y = pm_y-1