def unpack_plane(mesh_, faces_idx):
faces = mesh_.faces[faces_idx]
area = sum(mesh_.area_faces[faces_idx])
normals = mesh_.face_normals[faces_idx]
vertices = mesh_.vertices[faces]
# if area > 0.4:
# mesh_.visual.face_colors[faces_idx] = trimesh.visual.random_color()
#TODO: use ransac find plane
centroids = np.array([(fv[0] + fv[1] + fv[2]) / 3 for fv in vertices])
try:
plane = pyrsc.Plane()
eq, inliers = plane.fit(np.array(centroids),
thresh=0.01,
minPoints=100,
maxIteration=100)
normal = eq[:3]
except:
normal = np.average(normals, axis=0)
points_idx = np.unique(faces)
points = mesh_.vertices[points_idx]
new_plane = Plane()
new_plane.set_attribute('faces_idx', faces_idx)
new_plane.set_attribute('faces', faces)
new_plane.set_attribute('area', area)
new_plane.set_attribute('normals', normals)
new_plane.set_attribute('normal', normal)
new_plane.set_attribute('vertices', vertices)
new_plane.set_attribute('points_idx', points_idx)
new_plane.set_attribute('points', points)
return new_plane
def ransac_find_planes(pcd):
planes = []
max_x = np.amax(pcd[:, 0])
min_x = np.amin(pcd[:, 0])
max_y = np.amax(pcd[:, 1])
min_y = np.amin(pcd[:, 1])
max_z = np.amax(pcd[:, 2])
min_z = np.amin(pcd[:, 2])
# xyz = pcd.copy()
xyz_split = [pcd.copy()]
for xyz in xyz_split:
iters = 0
while (iters < 25):
plane = pyrsc.Plane()
eq, inliers = plane.fit(xyz,
thresh=0.01,
minPoints=10000,
maxIteration=100)
if (inliers.shape[0] > 10000):
planes.append(np.array([eq, xyz[inliers]], dtype=object))
xyz = np.delete(xyz, inliers, axis=0)
print(eq)
iters = 0
else:
iters += 1
return np.array(planes, dtype=object)
def inrange_segment_plane (pcd):
# Remove points out of range of 80 meters
arr = np.asarray(pcd.points)
r = np.sqrt(np.square(arr[:,0])+np.square(arr[:,1]))
inrange = np.where(r < 80)
pcd_inrange = pcd.select_by_index(inrange[0], invert=False)
arr_inrange = np.asarray(pcd_inrange.points)
# Segment plane with RANSAC algorithm
#segmplane = dfpc0.segment_plane(distance_threshold=0.5, ransac_n=3, num_iterations=120)
plane1 = pyransac3d.Plane()
best_eq, best_inliers = plane1.fit(arr_inrange, thresh=0.25, minPoints=1, maxIteration=100)
#inliers_result = pd.DataFrame(segmplane[1])
# Convert list to numpy array
#inliers_result_arr = np.asarray(inliers_result)
#inliers_result_arr.astype(np.uint32)
# Remove plane
pcd_out = pcd_inrange.select_by_index(best_inliers, invert=True)
#pcd_plane = dfpc0.select_by_index(inliers_result_arr, invert=False)
# Visualize planes
#o3d.visualization.draw_geometries([pcd_out])
#o3d.visualization.draw_geometries([pcd_plane])
# Clustering
with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:
labels = np.array(pcd_out.cluster_dbscan(eps=0.6, min_points=25, print_progress=False))
#eps: distance to neighbor in cluster
#min_points: min number of points required to form a cluster
max_label = labels.max()
print(f"point cloud has {max_label + 1} clusters")
colors = plt.get_cmap("tab20")(labels / (max_label if max_label > 0 else 1))
colors[labels < 0] = 0
pcd_out.colors = o3d.utility.Vector3dVector(colors[:, :3])
#visualization
# o3d.visualization.draw_geometries([pcd_out])
print("pcd_out")
print(pcd_out)
labelst = labels.transpose()
print("labelst")
print(labelst)
return pcd_out
def ransac_fit_plane(pcd):
planes = []
for xyz in pcd:
plane = pyrsc.Plane()
try:
eq, inliers = plane.fit(xyz,
thresh=0.01,
minPoints=10000,
maxIteration=100)
print(eq, inliers)
planes.append(np.array([eq, xyz[inliers]], dtype=object))
except ValueError:
print(f'Unlikely a plane due to a size of {len(xyz)} points.')
# np.save('surfaces', np.array(planes, dtype=object))
# print(f'Saved')
# plot_layout2(pcd, planes)
return planes
import sys
import numpy as np
import open3d as o3d
sys.path.append(".")
import pyransac3d as pyrsc
# Load saved point cloud and visualize it
pcd_load = o3d.io.read_point_cloud("tests/dataset/caixa.ply")
# o3d.visualization.draw_geometries([pcd_load])
points = np.asarray(pcd_load.points)
plano1 = pyrsc.Plane()
best_eq, best_inliers = plano1.fit(points, 0.01)
plane = pcd_load.select_by_index(best_inliers).paint_uniform_color([1, 0, 0])
obb = plane.get_oriented_bounding_box()
obb2 = plane.get_axis_aligned_bounding_box()
obb.color = [0, 0, 1]
obb2.color = [0, 1, 0]
not_plane = pcd_load.select_by_index(best_inliers, invert=True)
o3d.visualization.draw_geometries([not_plane, plane, obb, obb2])
# VectUb = VectB/np.linalg.norm(VectB)
# W = np.cross(VectUa,VectUb)
#
# D=np.sum(np.multiply(W, losowy_punkt_klaster1_3)) #obliczenie D
# np.subtract(losowy_punkt_klaster1_1,losowy_punkt_klaster1_3)
# W=list(W)
# for temp in range(len(cluster1_x)-1): #stworzenie listy W do obliczania dystaunsu
# W.append(W)
#
# distance = (np.add((np.multiply(coord,W)),D)) #
# print(distance)
######################
cloud1 = np.array(list(zip(cluster1_x, cluster1_y, cluster1_z)))
cloud2 = np.array(list(zip(cluster2_x, cluster1_y, cluster2_z)))
cloud3 = np.array(list(zip(cluster3_x, cluster3_y, cluster3_z)))
plane1 = pyransac3d.Plane()
plane2 = pyransac3d.Plane()
plane3 = pyransac3d.Plane()
best_eq1, best_inliners1 = plane1.fit(cloud1,
thresh=0.01,
minPoints=100,
maxIteration=1000)
best_eq2, best_inliners2 = plane1.fit(cloud2,
thresh=0.01,
minPoints=100,
maxIteration=1000)
best_eq3, best_inliners3 = plane1.fit(cloud3,
thresh=0.01,
minPoints=100,
maxIteration=1000)
print(f'Best equasion for plane 1 Ax+Bx+Cx+D=0:{best_eq1}')
from csv import reader
import numpy as np
import pyransac3d as pyrsc
def read_csv(name, nl="\n", dl=","):
cloud = []
with open(name, newline=nl) as csvfile:
CsvReader = reader(csvfile, delimiter=dl)
for xx, yy, zz in CsvReader:
cloud.append([float(xx), float(yy), float(zz)])
return cloud
cloud = np.array(read_csv("cloud_c.xyz"))
plane = pyrsc.Plane()
best_eq, best_inliers = plane.fit(cloud,
thresh=0.02,
minPoints=100,
maxIteration=1000)
print(f'best equation Ax+By+Cz+D = {best_eq}')
print(f'best inliers: {best_inliers}')
# ########################################################
# ########################################################
X = np.array(list_of_read_points_from_csv)
# ########################################################
# ########################################################
# ransac_algorithm(X, 10, 0.1)
# ########################################################
# ########################################################
k_mean_lib(X)
# ########################################################
# ########################################################
# points = load_points(.) # Load your point cloud as a numpy array (N, 3)
points = X
# Example 1 - Planar RANSAC
plane1 = pyrsc.Plane()
best_eq, best_inliers = plane1.fit(points, 0.01)
# Results in the plane equation Ax+By+Cz+D: [1, 0.5, 2, 0]
# Example 2 - Spherical RANSAC
# Loading a noisy sphere's point cloud with r = 5 centered in 0 we can use the following code:
# points = load_points(.) # Load your point cloud as a numpy array (N, 3)
points = X
sph = pyrsc.Sphere()
center, radius, inliers = sph.fit(points, thresh=0.4)
# center: [0.010462385575072288, -0.2855090643954039, 0.02867848979091283]
# radius: 5.085218633039647
# ########################################################
# ########################################################
from csv import reader
import numpy
import pyransac3d
def read_csv(name,nl="\n",dl=","):
cloud=[]
with open(name,newline=nl) as csvfile:
csvreader=reader(csvfile,delimiter=dl)
for xx, yy, zz in csvreader:
cloud.append([float(xx), float(yy), float(zz)])
return cloud
if __name__=="__main__":
cloud=numpy.array(read_csv("LidarData.xyz"))
plane=pyransac3d.Plane()
best_eq, best_inliers=plane.fit(cloud, thresh=0.01, minPoints=100, maxIteration=1000)
print(f'best euation Ax+By+Cz+D:{best_eq}')
print(f'best inliers:{best_inliers}')
