提交更新

inv_mat.py

+import numpy as np
+
+#6-2
+ORIGIN2KITTI = [[0.9988722862129534, -0.04214125427326351, -0.021869396973478106,
+      10.0], [0.0409985649537926, 0.99789214163878, -0.05030299521200135, 0.0], [
+      0.023943130694148426, 0.04934965393845363, 0.9984945358632253, 0.9000000000000004],
+    [0.0, 0.0, 0.0, 1.0]]
+
+ORIGIN2KITTI = np.array(ORIGIN2KITTI)
+
+print(np.linalg.inv(ORIGIN2KITTI))
\ No newline at end of file

script/test.py

@@ -15,28 +15,41 @@ import time
 
 if __name__ == '__main__':
 
-    ticks = time.time()
-    str_time = time.strftime("%Y-%m-%d_%H:%M:%S", time.localtime()) 
-    ticks2 = time.time()
-    delay = ticks2 - ticks
-    str = "10_1/pcd/1629734723.170913000.pcd"
-    print(str.split('/'))
-    str1 = str.split('/')[-1]
-    print(str1[0:str1.find('.pcd')])
+    # ticks = time.time()
+    # str_time = time.strftime("%Y-%m-%d_%H:%M:%S", time.localtime()) 
+    # ticks2 = time.time()
+    # delay = ticks2 - ticks
+    # str = "10_1/pcd/1629734723.170913000.pcd"
+    # print(str.split('/'))
+    # str1 = str.split('/')[-1]
+    # print(str1[0:str1.find('.pcd')])
 
-    file_path = "/home/oscar/ros/git/jfxmap_python/pointcloud1.pcd"
-    pcd = o3d.io.read_point_cloud(file_path)
-    print(pcd)
-    print(pcd.dimension())
-    center = np.array([0,0,0])
-    scalepcd = pcd.scale(0.001,center)
-    print(scalepcd)
-    # downpcd = pcd.voxel_down_sample(0.01)
-    # print(downpcd)
-    def save_pcd(vis):
-    #io.write_point_cloud(file_path, pcd, write_ascii=True)
-    #print(file_path)
-        return False
-    key_to_callback = {}
-    key_to_callback[ord("S")] = save_pcd
-    o3d.visualization.draw_geometries_with_key_callbacks([scalepcd], key_to_callback)
\ No newline at end of file
+    load_path = "/home/oscar/ros/git/jfxmap_python/"
+    load_dir = ""
+    json_path = os.path.join(load_path,load_path + ".json")
+    anno = {}
+    with open(json_path, 'r',encoding='utf-8',errors='ignore') as fp:
+      jsn = json.load(fp)
+      annos = jsn["annotations"]
+      for an in annos:
+        fileuri = an['fileuri'].split("/")[-1]
+        anno[fileuri] = an['labels_box3D']
+
+    bin_path = os.path.join(load_path,"pcd")
+    bin_list = glob.glob(os.path.join(bin_path,"*.bin"))
+    for bin_file in bin_list:
+        bin = np.fromfile(bin_file,dtype=np.float32)
+        bin = bin.reshape(-1,3)
+        fileuri = bin_file.split("/")[-1]
+        point_cloud = o3d.geometry.PointCloud()
+        point_cloud.points = o3d.utility.Vector3dVector(bin)
+        
+        # downpcd = pcd.voxel_down_sample(0.01)
+        # print(downpcd)
+        def save_pcd(vis):
+        #io.write_point_cloud(file_path, pcd, write_ascii=True)
+        #print(file_path)
+            return False
+        key_to_callback = {}
+        key_to_callback[ord("S")] = save_pcd
+        o3d.visualization.draw_geometries_with_key_callbacks([scalepcd], key_to_callback)
\ No newline at end of file

script/utils/compute_yaw.py

@@ -110,17 +110,17 @@ def compute_yaw(car_yaw_in_lidar):
     return yaw
 
 if __name__ == "__main__":
-    lat1 = 31.2840452
-    lon1 = 121.1597165
+    lat1 = 31.27651749
+    lon1 = 121.1843769
     p1 = np.array([lat1, lon1])
     p11 = gaussian.Convert_v2(p1)
 
     lidar_x = p11[0]
     lidar_y = p11[1]
 
-    R = [[-0.330806583166,  -0.941249847412,  0.067936882377], 
-       [0.943696200848,  -0.329787790775,  0.026026830077],
-	[-0.002092991956,  0.072721630335,  0.997349977493]]
+    R = [[-0.080108232796, -0.995879292488, 0.042511798441], 
+       [0.996389091015, -0.081207185984, -0.024783149362],
+	[0.028133289889, 0.040372956544, 0.998788475990]]
 
     #取x轴两个点坐标用来标定lidar的x轴的绝对航向
     tmp_loc1 = [30,0,0]

script/utils/gaussian.py

@@ -84,3 +84,7 @@ def Inverse_v2(inXY): # XY --> BL
     outLonLat = np.array([B, L])
     return outLonLat
 
+print(Convert_v2([31.27811229,121.1914875]))
+
+a = '%f,%f'%(1.0210,32.121)
+print(a)
\ No newline at end of file