Revert chunk to chunk parser

labforge · Mar 7, 2024 · 1a01eea · 1a01eea
1 parent e2e2ccb
commit 1a01eea
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 45 deletions.
diff --git a/common/chunk_parser.py b/common/chunk_parser.py
@@ -326,31 +326,19 @@ def get_chunkdata_by_id(rawdata: np.ndarray, chunk_id: int = 0):
     if rawdata is None or len(rawdata) == 0 or chunk_id < 0:
         return chunk_data
 
-    pos = len(rawdata) - 8
+    pos = len(rawdata) - 4
     while pos >= 0:
-        chkid = int.from_bytes(rawdata[pos:(pos + 4)], 'big')  # transmitted as big-endian
-        chunk_len = int.from_bytes(rawdata[pos+4:(pos + 8)], 'big')  # transmitted as big-endian
-        if chunk_len == 0:
-            chunk_len = 4
-            pos -= chunk_len
-            print("void")
-        else:
+        chunk_len = int.from_bytes(rawdata[pos:(pos + 4)], 'big')  # transmitted as big-endian
+        if chunk_len > 0 and (pos - 4 - chunk_len) > 0:
+            pos -= 4
+            chkid = int.from_bytes(rawdata[pos:(pos + 4)], 'big')  # transmitted as big-endian
+
             pos -= chunk_len
-            print("valid chunk?")
             if chkid == chunk_id:
                 chunk_data = rawdata[pos:(pos + chunk_len)]  # transmitted as little-endian
+                break
 
-
-        # if chunk_len > 0 and (pos - 4 - chunk_len) > 0:
-        #     pos -= 4
-        #     chkid = int.from_bytes(rawdata[pos:(pos + 4)], 'big')  # transmitted as big-endian
-        #
-        #     pos -= chunk_len
-        #     if chkid == chunk_id:
-        #         chunk_data = rawdata[pos:(pos + chunk_len)]  # transmitted as little-endian
-        #         break
-        #
-        # pos -= 4
+        pos -= 4
 
     return chunk_data
 

diff --git a/sparse3d/demo.py b/sparse3d/demo.py
@@ -265,23 +265,21 @@ def process_points_and_matches(keypoints, matches, pc, image_left, image_right,
                                   flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
         cv2.imwrite(image_filename_correspondence, image_correspondence)
 
-    # with open(ply_filename, 'w') as ply_file:
-    #     # PLY header
-    #     ply_file.write("ply\n")
-    #     ply_file.write("format ascii 1.0\n")
-    #     ply_file.write(f"element vertex {len(intermediate_list)}\n")
-    #     ply_file.write("property float x\n")
-    #     ply_file.write("property float y\n")
-    #     ply_file.write("property float z\n")
-    #     ply_file.write("property uchar red\n")
-    #     ply_file.write("property uchar green\n")
-    #     ply_file.write("property uchar blue\n")
-    #     ply_file.write("end_header\n")
-    print("===== RESULTS ======")
-    for point in intermediate_list:
-        print(f"{point[0]} {point[1]} {point[2]} {point[3]} {point[4]} {point[5]}\n")
-        # ply_file.write(f"{point[0]} {point[1]} {point[2]} {point[3]} {point[4]} {point[5]}\n")
-        # ply_file.write("\n")
+    with open(ply_filename, 'w') as ply_file:
+        # PLY header
+        ply_file.write("ply\n")
+        ply_file.write("format ascii 1.0\n")
+        ply_file.write(f"element vertex {len(intermediate_list)}\n")
+        ply_file.write("property float x\n")
+        ply_file.write("property float y\n")
+        ply_file.write("property float z\n")
+        ply_file.write("property uchar red\n")
+        ply_file.write("property uchar green\n")
+        ply_file.write("property uchar blue\n")
+        ply_file.write("end_header\n")
+        for point in intermediate_list:
+            ply_file.write(f"{point[0]} {point[1]} {point[2]} {point[3]} {point[4]} {point[5]}\n")
+            ply_file.write("\n")
 
     # Save point cloud to a PLY file
     print(f'Point cloud saved to {ply_filename}')
@@ -303,24 +301,16 @@ def handle_buffer(pvbuffer: eb.PvBuffer, device: eb.PvDeviceGEV):
         image1 = pvbuffer.GetMultiPartContainer().GetPart(1).GetImage()
         image_data_right = image1.GetDataPointer()
         image_data_right = cv2.cvtColor(image_data_right, cv2.COLOR_YUV2BGR_YUY2)
-        print('.')
 
         # Parses the feature points from the buffer
         keypoints = decode_chunk(device=device, buffer=pvbuffer, chunk='FeaturePoints')
-        if keypoints is not None:
-            print('k')
 
         # Use matches with coordinates instead
         matches = decode_chunk(device=device, buffer=pvbuffer, chunk="FeatureMatches")
-        if matches is not None:
-            print('m')
 
         # parses sparse point cloud from the buffer
         # returns a list of Point3D(x,y,z). NaN values are set for unmatched points.
         pc = decode_chunk(device=device, buffer=pvbuffer, chunk='SparsePointCloud')
-        if pc is not None:
-            print('p')
-
         timestamp = pvbuffer.GetTimestamp()
 
         if pc is not None and len(pc) > 0 and keypoints is not None and len(keypoints) > 0 and len(matches) > 0: