cell_diameter = np.linalg.norm(plane.pcd[0][0]-plane.pcd[-1][0]) plane.cell_distance_trunctuated=pow(min(max(cell_diameter*sin_cos_angle_4_merge,20.0),MAX_MERGE_DIST),2) planes.append(plane) k += 1 # Step B: Histogram spherical_coordinates = np.zeros((no_hori_cell*no_vert_cell,2)) planar_arr = np.zeros((no_hori_cell*no_vert_cell)) ## Represent normals as spherical coordinates i = 0 for plane in planes: if plane.planar == True: n_proj_norm = math.sqrt(plane.normal_x*plane.normal_x+plane.normal_y*plane.normal_y) plane.polar = math.acos(-plane.normal_z); plane.azimuth = math.atan2(plane.normal_x/n_proj_norm,plane.normal_y/n_proj_norm); #print("Pol: \t", plane.polar, "\t Azh: \t", plane.azimuth) spherical_coordinates[i] = np.array([plane.polar, plane.azimuth]) planar_arr[i] = 1 # remember that this plane is planar i += 1 ## Initialize Histogram no_of_bins_pr_coordinate = 20 # why? no_of_bins = no_of_bins_pr_coordinate * no_of_bins_pr_coordinate # why? no_of_planar_planes = 0 histogram = np.zeros((no_of_bins)) bins = np.zeros((planar_arr.shape[0])) for i in range(len(planes)):