提交配置更新提交更新

script/trans_data.py

@@ -17,6 +17,7 @@ import yaml
 from jfxmap_lib.jfxmap import init_jfxmap, get_map_data 
 from utils.compute_yaw import compute_yaw,car_yaw_cal
 
+from seg_ww_ground import ground_segmentation
 
 def compute_pitch(A,B,C):
   origin_direction = [0,0,1]
@@ -488,7 +489,8 @@ if __name__ == '__main__':
       f1.write('lat,lon,x,y,z,l,w,h,rot_y\n')
       # print(file_path)
       # print(boxx)
-      bbox_flag,bboxes = read_all_3d_bboxes(boxx, np.identity(4))
+      # bbox_flag,bboxes = read_all_3d_bboxes(boxx, np.identity(4))
+      bbox_flag,bboxes = read_all_3d_bboxes(boxx, rotMat[origin_child_dir])
       if bbox_flag == False or len(bboxes) < 1:
         continue     
       # print(bbox_flag)
@@ -498,21 +500,28 @@ if __name__ == '__main__':
       print(xyzi)
       print(pcd)
       print(converted_pcd)
+      b = np.where( (converted_pcd[:,2] <= -3.5) & (converted_pcd[:,2] > -7) )
+      cropped_cropped = converted_pcd[b]
+      print(cropped_cropped.shape)
 
-      xyz = np.array(pcd.points)
+      seg,m = ground_segmentation(data=cropped_cropped[:,:3])
+      
+      xyz = np.array(converted_pcd.points)
       savecloud = o3d.geometry.PointCloud()
-      savecloud += g_pcd
+      savecloud += g_converted_pcd
       drawpointcloud = []
-      merge_geos = [pcd]
-      exportDrawCloud = [g_pcd]
+      merge_geos = [converted_pcd]
+      exportDrawCloud = [g_converted_pcd]
       for bbox in bboxes:
-        out_center_BL = get_loc_from_origin([bbox[4][0], bbox[4][1], bbox[4][2]], 0, origin_Trans)
+        # out_center_BL = get_loc_from_origin([bbox[4][0], bbox[4][1], bbox[4][2]], 0, origin_Trans)
+        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)
         print(out_center_BL)
         angle = car_yaw_cal(131.419987,bbox[2])
         mapInfo = get_map_data(out_center_BL[0],out_center_BL[1],angle)
         print("call get data isInMap = ",mapInfo)
         f1.write('%f,%f,%f,%f,%f,%f,%f,%f,%f\n'%(out_center_BL[0],out_center_BL[1],bbox[4][0], bbox[4][1], bbox[4][2],bbox[6][0], bbox[6][1], bbox[6][2],angle))
-        exportCenterBL = get_loc_from_origin([bbox[4][0], bbox[4][1], bbox[4][2]], 0, generate_Trans)
+        # exportCenterBL = get_loc_from_origin([bbox[4][0], bbox[4][1], bbox[4][2]], 0, generate_Trans)
+        lidar_loc_ex,out_BL_ex,exportCenterBL = get_loc([bbox[4][0], bbox[4][1], bbox[4][2],bbox[6][0],bbox[6][1],bbox[6][2],bbox[2]], 0, generate_Trans,generate_kitti2origin)
         print(exportCenterBL)
         angle = car_yaw_cal(79.89299572540227,bbox[2])
         mapInfoExport = get_map_data(exportCenterBL[0],exportCenterBL[1],angle)
@@ -526,7 +535,7 @@ if __name__ == '__main__':
         while detaAngel < -180:
           detaAngel += 360
         print("angle = ",angle," laneAngle = ",laneAngle," detaAngel = ",detaAngel)  
-        if abs(detaAngel) > 100:
+        if abs(detaAngel) > 30:
           continue;
         flag = in_hull(xyz,bbox[1])
         pickcloud = xyz[flag]