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