提交更新

script/trans_and_export_data.py

@@ -575,54 +575,54 @@ if __name__ == '__main__':
                 fileuri = os.path.join(origin_root_path,child_dir)+ "/pcd/" + an['fileuri'].split("/")[-1]
                 anno[fileuri] = an['labels_box3D']
     
-    for pcd_ind,pcd_file in enumerate(pcd_list):
-        if pcd_file not in anno:
-            print('the anno is null : ', pcd_file)
-            continue
-        print(pcd_file)
-        boxx = anno[pcd_file]
-        bbox_flag,bboxes = read_all_3d_bboxes(boxx, rotMat[child_dir])
-        if bbox_flag == False or len(bboxes) < 1:
-            continue      
-        xyzi,pcd,converted_pcd = parse_pandarmind_pcd(pcd_file, rotMat[child_dir])
-        xyz = np.array(converted_pcd.points)
-        for bbox in bboxes:
-            # 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)
-            angle2 = car_yaw_cal(generate_car_yaw_cal_angle,bbox[2])
-            mapInfoExport = get_map_data(exportCenterBL[0],exportCenterBL[1],angle2)
-            print("call get ex data isInMap = ",mapInfoExport)
-            if mapInfoExport[0] != 1:
-                continue;
-            laneAngle = mapInfoExport[10];
-            detaAngel = laneAngle - angle2;
-            while detaAngel > 180:
-                detaAngel -= 360
-            while detaAngel < -180:
-                detaAngel += 360
-            print("angle = ",angle2," laneAngle = ",laneAngle," detaAngel = ",detaAngel)  
-            if abs(detaAngel) > 30:
-                continue;
-            #获取到汽车点云符合条件，可以添加到新点云里
-            # f1.write('%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n'%(exportCenterBL[0],exportCenterBL[1],bbox[4][0], bbox[4][1], bbox[4][2],bbox[6][0], bbox[6][1], bbox[6][2],angle2,laneAngle))
-            
-            idx = Check_Add_Cloud_box(save_cloud_list,bbox,g_converted_pcd,child_dir,pcd_file.split("/")[-1],generate_pcd_path)
-            if idx == -1:
-                continue;
-            flag = in_hull(xyz,bbox[1])
-            pickcloud = xyz[flag]
-
-            bottom_center = (bbox[1][5,:] + bbox[1][7,:]) / 2.0
-            ground_z = (-m[3] - m[0] * bottom_center[0] - m[1] * bottom_center[1]) / m[2]
-            delta_z = ground_z - bottom_center[2]
-
-            dz = np.array([0,0,delta_z])
-            pickcloud = np.add(pickcloud,dz)
-            print(pickcloud)
-            bbox[1] += dz
-
-            Add_Cloud_box(save_cloud_list,idx,bbox,pickcloud)
+        for pcd_ind,pcd_file in enumerate(pcd_list):
+            if pcd_file not in anno:
+                print('the anno is null : ', pcd_file)
+                continue
+            print(pcd_file)
+            boxx = anno[pcd_file]
+            bbox_flag,bboxes = read_all_3d_bboxes(boxx, rotMat[child_dir])
+            if bbox_flag == False or len(bboxes) < 1:
+                continue      
+            xyzi,pcd,converted_pcd = parse_pandarmind_pcd(pcd_file, rotMat[child_dir])
+            xyz = np.array(converted_pcd.points)
+            for bbox in bboxes:
+                # 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)
+                angle2 = car_yaw_cal(generate_car_yaw_cal_angle,bbox[2])
+                mapInfoExport = get_map_data(exportCenterBL[0],exportCenterBL[1],angle2)
+                print("call get ex data isInMap = ",mapInfoExport)
+                if mapInfoExport[0] != 1:
+                    continue;
+                laneAngle = mapInfoExport[10];
+                detaAngel = laneAngle - angle2;
+                while detaAngel > 180:
+                    detaAngel -= 360
+                while detaAngel < -180:
+                    detaAngel += 360
+                print("angle = ",angle2," laneAngle = ",laneAngle," detaAngel = ",detaAngel)  
+                if abs(detaAngel) > 30:
+                    continue;
+                #获取到汽车点云符合条件，可以添加到新点云里
+                # f1.write('%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n'%(exportCenterBL[0],exportCenterBL[1],bbox[4][0], bbox[4][1], bbox[4][2],bbox[6][0], bbox[6][1], bbox[6][2],angle2,laneAngle))
+                
+                idx = Check_Add_Cloud_box(save_cloud_list,bbox,g_converted_pcd,child_dir,pcd_file.split("/")[-1],generate_pcd_path)
+                if idx == -1:
+                    continue;
+                flag = in_hull(xyz,bbox[1])
+                pickcloud = xyz[flag]
+
+                bottom_center = (bbox[1][5,:] + bbox[1][7,:]) / 2.0
+                ground_z = (-m[3] - m[0] * bottom_center[0] - m[1] * bottom_center[1]) / m[2]
+                delta_z = ground_z - bottom_center[2]
+
+                dz = np.array([0,0,delta_z])
+                pickcloud = np.add(pickcloud,dz)
+                print(pickcloud)
+                bbox[1] += dz
+
+                Add_Cloud_box(save_cloud_list,idx,bbox,pickcloud)
 
 
     for cloud in save_cloud_list: