def test_best_fit_plane(points, plane_expected):
points = Points(points).set_dimension(3)
plane_fit = Plane.best_fit(points)
assert plane_fit.is_close(plane_expected)
assert plane_fit.point.is_close(plane_expected.point)
def calc_point_vectors(self):
for i,point in enumerate(self.point_data):
#find center of each cluster, as well as its endpoints using a convex hull
dists, indeces = self.kd_tree.query(point,10)
points = []
for i in range(1,len(indeces)-1):
points.append(self.point_data[indeces[i]])
plane = Plane.best_fit(points)
self.point_vectors.append(plane.project_vector(point))
printProgressBar(i,len(self.point_cloud)-1)
def Plane_fitting(Limage):
unique_planes = np.unique(Limage)
numpoints = []
planes = []
for i in range(1,len(unique_planes)):
loc = np.where(Limage==unique_planes[i])
numpoints.append(len(loc[0]))
extractedpoints = np.vstack(loc).transpose()
plane = Plane.best_fit(extractedpoints)
planes.append(plane)
return planes,numpoints
def fit_plane_scipy(P=None):
from skspatial.objects import Points, Plane
from skspatial.plotting import plot_3d
points = Points([[0, 0, 0], [1, 3, 5], [-5, 6, 3], [3, 6, 7], [-2, 6, 7]
]) if P is None else Points(P)
plane = Plane.best_fit(points)
plot_3d(
points.plotter(c='k', s=0.1, depthshade=False),
plane.plotter(alpha=0.8, lims_x=(-5, 5), lims_y=(-5, 5)),
)
plt.show()
def find_normals(points, k=3):
normals = []
for point in points:
from skspatial.objects import Points, Plane
distance = np.linalg.norm(points - point, axis=1)
#closest_inds = np.argpartition(distance, 3)
#x0, x1, x2 = points[closest_inds[:3]]
#normal = np.cross((x1 - x0), (x2 - x0))
closest_inds = np.argpartition(distance, k)
close_points = points[closest_inds[:k]]
normal = np.asarray(Plane.best_fit(close_points).normal)
normals.append(normal)
return normals
def test_from_points(arrays):
points = Points(arrays)
assume(not points.are_collinear(tol=1))
# The plane must contain each point.
plane = Plane.from_points(*points)
points = points.set_dimension(plane.dimension)
for point in points:
assert plane.contains_point(point, abs_tol=ATOL)
# The plane of best fit should be the same
# as the plane from three points.
plane_fit = Plane.best_fit(points)
assert plane_fit.is_close(plane, abs_tol=ATOL)
def compute(self):
if len(self.points) < 3 or self.origin is None or self.vector is None:
return
points = Points(self.points)
self.plane = Plane.best_fit(points)
self.vector = np.array(self.plane.project_point(self.vector))
self.origin = np.array(self.plane.project_point(self.origin))
self.xAxis = self.normalizeVector(self.vector - self.origin)
self.normal = np.array(self.plane.normal)
a = np.argmax(np.abs(self.normal))
if self.normal[a] < 0: self.normal *= -1
self.zAxis = self.normalizeVector(self.normal)
self.yAxis = -np.cross(self.xAxis, self.zAxis)
self.vectorBasis = [self.xAxis, self.yAxis, self.zAxis]
self.quaternion = self.get_quaternion(
[[1, 0, 0], [0, 1, 0], [0, 0, 1]], self.vectorBasis)
def test_best_fit_plane(data):
n_points = data.draw(st.integers(min_value=3, max_value=5))
points = Points([data.draw(arrays_fixed(3)) for _ in range(n_points)])
assume(not points.are_collinear(tol=ATOL))
plane_fit = Plane.best_fit(points)
# The best fit plane could have a higher dimension than the points
# (e.g., 2D points have a 3D plane of best fit).
# So, we convert the points dimension to that of the best fit plane.
dim_fit = plane_fit.dimension
points = points.set_dimension(dim_fit)
plane = data.draw(planes(dim_fit))
error_plane = plane.sum_squares(points)
error_fit = plane_fit.sum_squares(points)
assert error_fit <= error_plane + ATOL
"""
3D Plane of Best Fit
====================
Fit a plane to multiple 3D points.
"""
from skspatial.objects import Plane
from skspatial.objects import Points
from skspatial.plotting import plot_3d
points = Points([[0, 0, 0], [1, 3, 5], [-5, 6, 3], [3, 6, 7], [-2, 6, 7]])
plane = Plane.best_fit(points)
plot_3d(
points.plotter(c='k', s=50, depthshade=False),
plane.plotter(alpha=0.2, lims_x=(-5, 5), lims_y=(-5, 5)),
)
def test_best_fit_plane_failure(points, message_expected):
with pytest.raises(ValueError, match=message_expected):
Plane.best_fit(points)
def custom_remove_plane(
original_cloud,
nneg_margin=0.2,
ppos_margin=0.01,
#visualization parameters
mytitle="custom_remove_planes",
mytuples=None,
params=None, #camera parameters,json file (P)
fov_step=None,
configuration_file=None, #object properties ,json file (O)
rotate=False,
#statements
print_statements=True,
visualization_on=False,
):
"""
cloud = point cloud
"""
cloud = copy.deepcopy(original_cloud)
##
mytuples = list(
zip(("pos_margin", "neg_margin"), (ppos_margin, nneg_margin)))
# initialize empty list
selected_points = []
# print instructions
if print_statements == True:
print("")
print("1) Please pick at least 3 points using [shift + left click]")
print(" Press [shift + right click] to undo point picking")
print("2) After picking points, press 'Q' to close the window")
# we need 3 points
while len(selected_points) != 3:
# selected points
# in theory one can select any number of points, but we want three to find the plane
selected_points = pick_points(cloud)
# access the coordinates of the points
# we want only 3 points to find the plane
coordinates_points = [
list(np.asarray(cloud.points)[selected_points[i]]) for i in range(3)
]
# translate in scikit language the newly found coordinates
points = Points(coordinates_points)
#identify the plane through the points
plane = Plane.best_fit(points)
# for each point in the point cloud,
# if it lies on the plane identified by the manually selected 3 points, it gets excluded
all_points = np.asarray(cloud.points)
all_indexes = list(range(len(all_points)))
all_list_points = [list(e) for i, e in enumerate(all_points[:])]
inliers = [
list(all_points[i]) for i in range(len(all_points))
if is_point_in_plane(all_points[i],
plane.normal,
neg_margin=nneg_margin,
pos_margin=ppos_margin)
]
if print_statements == True:
print("")
print("plane point: ", plane.point)
print("normal to the plane: ", plane.normal)
print("")
print("total number of points: ", len(all_points))
print("number excluded points : ", len(inliers))
print("with pos_margin= %s ; neg_margin= %s" %
(ppos_margin, nneg_margin))
print("")
print("calculating indexes (might take a while...)")
## find the indexes of the outliers
#inliers_idx = [i for i, e in enumerate(all_list_points) if e in inliers] #slower
inliers_idx = [all_list_points.index(e) for e in inliers] #faster
# include the invert of the outliers indexes
temp_cloud = copy.deepcopy(cloud)
cloud_new = temp_cloud.select_by_index(inliers_idx, invert=True)
#visualize eventually
if visualization_on == True:
continue_statement = "y"
while continue_statement == "y":
mytuples, inliers_idx, update_p = update_parameters(
mytuples, all_points, plane.normal, inliers_idx)
if update_p == "y":
# include the invert of the outliers indexes
temp_cloud = copy.deepcopy(cloud)
cloud_new = temp_cloud.select_by_index(inliers_idx,
invert=True)
#o3d.visualization.draw_geometries([cloud_new])
#display_inlier_outlier(cloud, inliers_idx)
custom_draw_geometry_outliers(
cloud_new,
inliers_idx,
mytitle=mytitle,
mytuples=mytuples,
params=params, #camera parameters,json file (P)
fov_step=None,
configuration_file=
configuration_file, #object properties ,json file (O)
rotate=rotate)
print('Delete the space in red (y/n): ')
delete_statement = input()
print('continue finding planes (y/n): ')
continue_statement = input()
if str(delete_statement) == "y":
return cloud_new
else:
print("no points deleted")
return cloud
def test_best_fit_plane_failure(points):
with pytest.raises(Exception):
Plane.best_fit(points)
def check_intercept_cluster(self,c):
#find center of each cluster, as well as its endpoints using a convex hull
clust = self.cluster_points[c]
label = self.labels[self.cluster_labels[c][0]]
clust_hull = CH(clust, qhull_options="QJ")
endpointsIndeces = clust_hull.vertices
avgPoint = get_average_point(clust)
self.clust_centers.append(avgPoint)
max_dist = 0
#use endpoints to calculate radius of cluster
for e in endpointsIndeces:
point = clust[e]
if dist(point,avgPoint) > max_dist:
max_dist = dist(point,avgPoint)
#Test 1: Distance ratios
#create graph of points in the cluster
clust_graph = nx.Graph()
for i in range(len(clust)):
clust_graph.add_node(i)
for i in range(len(clust)):
for n in range(i,len(clust)):
clust_graph.add_edge(i,n, dist= dist(clust[i],clust[n]))
#create sub-graph of close points to use for geodesic distances
sub_graph = [(u,v,d) for (u,v,d) in clust_graph.edges(data=True) if d['dist'] <=max_dist / 3]
clust_graph = nx.Graph()
clust_graph.add_edges_from(sub_graph)
max_ratio = 0
#calculate both euclidean and geodesic distances for each pair of endpoints
for e1 in endpointsIndeces:
p1 = clust[e1]
for e2 in endpointsIndeces:
if e1 != e2 and nx.has_path(clust_graph,source=e1,target=e2):
p2 = clust[e2]
path= nx.shortest_path(clust_graph,source=e1,target=e2,weight='dist')
geo_dist = 0
for i in range((len(path)-1)):
d = dist(clust[path[i]],clust[path[i+1]])
geo_dist = geo_dist + d
euclid_dist = dist(p1,p2)
ratio = geo_dist/euclid_dist
if ratio > max_ratio:
max_ratio = ratio
#if the ratio is above the threshold mark it as an intercept cluster
if(max_ratio > self.distance_ratio_threshold and label not in self.dist_intercept_clusters):
self.dist_intercept_clusters.append(label)
#Test 2: Sphericity
#add the buffered radius to list of radii for future use
self.clust_radii.append(max_dist*self.radius_buffer)
#calculate the sphericity using volume of the cluster and the volume of the sphere that is created using the center point and that farthest endpoint
box_vol = clust_hull.volume
sphere_vol = 4/3*math.pi * (max_dist**3)
sphericity = box_vol / sphere_vol
# if this ratio is above the threshold, mark it as an intercept cluster
if sphericity > self.sphericity_threshold:
self.sphere_intercept_clusters.append(label)
#print(str(max_ratio) +" "+str(sphericity))
#calculate spherictity
#printProgressBar(c,len(self.cluster_points)-1)
#Test 3: planar distance
clust_plane = Plane.best_fit(clust)
avg_planar_dist = 0
for p in clust:
avg_planar_dist = avg_planar_dist + clust_plane.distance_point(p)
avg_planar_dist = (avg_planar_dist / len(clust) )/max_dist
if avg_planar_dist > self.planar_threshold:
self.planar_intercept_clusters.append(label)
data = {'label':int(label),'sphericity':float(sphericity),'planar_dist':avg_planar_dist,'dist_ratio':float(max_ratio),'center':[float(avgPoint[0]),float(avgPoint[1]),float(avgPoint[2])],'adjacent_clusters':[]}
self.clust_data.append(data)
