def filtre_ROR(point, k=10, r=5): ## Statistical Outlier Removal pointss = PyntCloud(points=point) kdtree_ids = pointss.add_structure("kdtree", leafsize=16) new_filtre = pointss.get_filter("ROR", kdtree_id=kdtree_ids, and_apply=False, k=k, r=r) return new_filtre
def processData(dataTemp): try: os.remove("cur_mesh.ply") os.remove("cur_mesh.obj") except: print("Fresh") f = open("data.txt", "w") print(dataTemp) data = dataTemp.decode("utf-8").replace(" ", "").split("\n") data = data[:max(0, len(data) - 2)] for d in range(len(data)): data[d] = data[d].split(",") data[d] = [float(e) for e in data[d]] data = np.array(data) average = np.array([np.average(data[:, i]) for i in range(3)]) average[2] = min(data[:, 2]) print(average) data = np.array([np.array(point) - average for point in data]) frame = pd.DataFrame(data=data, columns=['x', 'y', 'z']) pc = PyntCloud(frame) kd_id = pc.add_structure("kdtree") pc.get_filter("SOR", and_apply=True, kdtree_id=kd_id, k=20, z_max=0.4) convex_hull_id = pc.add_structure("convex_hull") convex_hull = pc.structures[convex_hull_id] pc.mesh = convex_hull.get_mesh() pc.to_file("cur_mesh.ply", also_save=["mesh"]) pc.to_file("cur_mesh.obj", also_save=["mesh", "points"]) for i in range(3): data[:, i] /= max(np.abs(data[:, i])) polarData = [] for r in range(len(data)): rho = np.sqrt(data[r][0]**2 + data[r][1]**2) if data[r][0] < 0: # if x < 0 phi = np.arctan2(data[r][1], data[r][0]) + math.pi else: phi = np.arctan2(data[r][1], data[r][0]) if phi < 0: phi += math.pi * 2 # phi from [0, 360) # round to nearest 20deg for now prop = 1.0 / 20.0 if phi < 0: phi += math.pi * 2 phi /= math.pi * 2.0 phi = min(phi // prop, 19) z = data[r][2] # round to 1/4 radius prop2 = 1.0 / 4.0 rho = min(rho // prop2, 3.0) # round to 1/6 height prop3 = 1.0 / 6.0 z = min(z // prop3, 5.0) polarData.append([int(rho), int(phi), int(z)]) #print(polarData) m_data = np.zeros((20, 4, 6)) for p in polarData: m_data[p[1], p[0], p[2]] += 1 string = "0b" for rows in range(m_data.shape[0]): for cols in range(m_data.shape[1]): for theta in range(m_data.shape[2]): if (m_data[rows, cols, theta] < 1): string += "0" m_data[rows, cols, theta] = 0 else: string += "1" m_data[rows, cols, theta] = 1 print(m_data) f.write(string) f.close()