# Organize pointcloud according to cells
pcd_organized = organizePointCloudByCell(pcd, PATCH_SIZE)
# Step A: Plane fitting
planes = []
sin_cos_angle_4_merge = math.sqrt(1-pow(math.pi/12,2))
MAX_MERGE_DIST = 50.0
k = 0
for i in range(no_vert_cell):
for j in range(no_hori_cell):
plane = Plane(pcd_organized[k], row=i, col=j, no_points_per_cell=PATCH_SIZE*PATCH_SIZE)
if plane.planar == True:
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)
