avoid NaN and add void label

Author: toniodo

README.md

@@ -23,7 +23,8 @@ The script [convert_pointcloud](convert_pointcloud.py) reads a given rosbag and
   * `--id [String]`: Name of the sequence (default `'00'`)
   * `-n, --name [String]`: Name of the folder of the point cloud files (default `'velodyne'`)
 
-These indications are also available by using `-h`, it will display all the above informations
+These indications are also available by using `-h`, it will display all the above informations. Note that if the name of the sequence already exists,
+it will continue to add new files to it without overwriting.
 
 ## Automatic labeling using normal orientation threshold
 
@@ -33,7 +34,8 @@ We have developed this algorithm for labeling point clouds:
 
 1. Estimate the normal on each point considering a number of neighboors to estimate the local surface
 2. For each normal, compares its angle with a reference vector (here, the relative vertical)
-3. If the angle is above the define threshold, the point is labeled as _obstacle_ (`1`) otherwise, the point is labeled as _free space_ (`0`)
+3. If the angle is above the define threshold, the point is labeled as _obstacle_ (`2`) otherwise, the point is labeled as _free space_ (`1`)
+4. If the intensity or the coordinates of the point are 0, the point is labeled as _void_ (`0`)
 
 * We have the following options:
   * `-d, --dataset [String]`: Path to the dataset

convert_pointcloud.py

@@ -96,7 +96,7 @@
 bag = rosbag.Bag(path_bagfile, "r")
 for index, (topic, msg, t) in tqdm(enumerate(bag.read_messages(topic_lidar))):
     # Generate the point list
-    pc_list = list(pc2.read_points(msg))
+    pc_list = list(pc2.read_points(msg, skip_nans=True))
     # Convert the list into an array
     pc_array = np.array(pc_list)
     x = pc_array[:, 0]

label_normals.py

@@ -121,14 +121,14 @@ def label_points(normals, reference=np.array([0, 0, 1])):
     else:
         normalised_ref = reference
 
-    # Determine angle threshold 
+    # Determine angle threshold
     cos_angle = np.cos(np.deg2rad(angle_degree))
 
     # Calculate dot product for all normals at once
     dot_products = np.dot(normals, normalised_ref)
 
     # Check threshold and create an array of labels based on the dot product values
-    label_array = np.where(np.abs(dot_products) <= cos_angle, 1, 0)
+    label_array = np.where(np.abs(dot_products) <= cos_angle, 2, 1)
 
     return label_array
 
@@ -142,6 +142,13 @@ def write_label(label_array, folder_path, filename):
             # Id is set by default to 0
             file.write(bin_label)
 
+# If the intensity or the coordinates of a point is 0, label it as void (0)
+def label_void(data, labels):
+    for i in range(data.shape[0]):
+        if (data[i, 0] == 0 and data[i, 1] == 0 and data[i, 2] == 0) or data[i, 3] == 0:
+            labels[i] = 0
+    return labels
+
 
 # List of all files in the bin folder
 bin_folder = os.path.join(id_sequence_folder, folder_data)
@@ -159,5 +166,7 @@ def write_label(label_array, folder_path, filename):
         normals = estimate_normals(data_bin, nb_neighboors)
         # Generate an array of labels
         labels = label_points(normals)
+        # Check void points
+        labels = label_void(data_bin, labels)
         # Write the resulting .label file
         write_label(labels, label_folder, file_name)