From b6f8eff47222ea60a5f9612bf72d08f0d7be4c32 Mon Sep 17 00:00:00 2001
From: Petros626 <62354721+Petros626@users.noreply.github.com>
Date: Wed, 30 Apr 2025 10:26:08 +0200
Subject: [PATCH] Correct LiDAR (3DBox) -> Camera (3DBox) calculation

Doesn't adjust for box center height.
Uses different matrix multiplication order.
---
 src/data_process/transformation.py | 16 ++++++----------
 1 file changed, 6 insertions(+), 10 deletions(-)

diff --git a/src/data_process/transformation.py b/src/data_process/transformation.py
index 58021b4..6102ca0 100644
--- a/src/data_process/transformation.py
+++ b/src/data_process/transformation.py
@@ -42,16 +42,12 @@ def camera_to_lidar(x, y, z, V2C=None, R0=None, P2=None):
     return tuple(p)
 
 
-def lidar_to_camera(x, y, z, V2C=None, R0=None, P2=None):
+def lidar_to_camera(x, y, z, h, V2C=None, R0=None, P2=None):
+    z = -h/2
     p = np.array([x, y, z, 1])
-    if V2C is None or R0 is None:
-        p = np.matmul(cnf.Tr_velo_to_cam, p)
-        p = np.matmul(cnf.R0, p)
-    else:
-        p = np.matmul(V2C, p)
-        p = np.matmul(R0, p)
-    p = p[0:3]
-    return tuple(p)
+    
+    p = np.dot(p, np.dot(V2C.T, R0.T))
+    return tuple(p[:3])
 
 
 def camera_to_lidar_point(points):
@@ -98,7 +94,7 @@ def lidar_to_camera_box(boxes, V2C=None, R0=None, P2=None):
     for box in boxes:
         x, y, z, h, w, l, rz = box
         (x, y, z), h, w, l, ry = lidar_to_camera(
-            x, y, z, V2C=V2C, R0=R0, P2=P2), h, w, l, -rz - np.pi / 2
+            x, y, z, h, V2C=V2C, R0=R0, P2=P2), h, w, l, -rz - np.pi / 2
         # ry = angle_in_limit(ry)
         ret.append([x, y, z, h, w, l, ry])
     return np.array(ret).reshape(-1, 7)