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)):
