提交更新

script/trans_data.py

@@ -11,7 +11,7 @@ from scipy.spatial.transform import Rotation as R
 import cv2
 from scipy.spatial import Delaunay
 import scipy
-from utils.getloc_coll import get_loc, get_world_loc
+from utils.getloc_coll import get_loc, get_world_loc,get_loc_from_origin
 from easydict import EasyDict
 import yaml
 from jfxmap_lib.jfxmap import init_jfxmap, get_map_data 
@@ -503,7 +503,7 @@ if __name__ == '__main__':
       drawpointcloud = []
       merge_geos = [pcd]
       for bbox in bboxes:
-        lidar_loc, out_BL, out_center_BL = get_loc([bbox[4][0], bbox[4][1], bbox[4][2], bbox[6][0], bbox[6][1], bbox[6][2], bbox[2]], 0, origin_Trans, origin_kitti2origin)
+        out_center_BL = get_loc_from_origin([bbox[4][0], bbox[4][1], bbox[4][2]], 0, origin_Trans)
         print(out_center_BL)
         angle = bbox[2]/np.pi*180
         mapInfo = get_map_data(out_center_BL[0],out_center_BL[1],angle)

script/utils/getloc_coll.py

@@ -202,6 +202,15 @@ def get_loc(boxes_3d, car_type, Trans, kitti2origin):
 
   return lidar_loc, out_BL, out_center_BL
 
+def get_loc_from_origin(lidar_loc, car_type, Trans):
+
+  tmp_loc = np.ones([4])
+  tmp_loc[:3] = lidar_loc[:3]
+  world_loc = np.dot(Trans,tmp_loc)
+
+  out_BL = Inverse_v2(world_loc[:2])
+
+  return out_BL
 
 def get_world_loc(boxes_3d, Trans, kitti2origin):
   '''