-
Notifications
You must be signed in to change notification settings - Fork 70
/
sphere.py
104 lines (79 loc) · 3.68 KB
/
sphere.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
import random
import numpy as np
class Sphere:
"""
Implementation for Sphere RANSAC. A Sphere is defined as points spaced from the center by a constant radius.
This class finds the center and radius of a sphere. Base on article "PGP2X: Principal Geometric Primitives Parameters Extraction"
![3D Sphere](https://raw.githubusercontent.com/leomariga/pyRANSAC-3D/master/doc/sphere.gif "3D Sphere")
---
"""
def __init__(self):
self.inliers = []
self.center = []
self.radius = 0
def fit(self, pts, thresh=0.2, maxIteration=1000):
"""
Find the parameters (center and radius) to define a Sphere.
:param pts: 3D point cloud as a numpy array (N,3).
:param thresh: Threshold distance from the Sphere hull which is considered inlier.
:param maxIteration: Number of maximum iteration which RANSAC will loop over.
:returns:
- `center`: Center of the cylinder np.array(1,3) which the cylinder axis is passing through.
- `radius`: Radius of cylinder.
- `inliers`: Inlier's index from the original point cloud.
---
"""
n_points = pts.shape[0]
best_inliers = self.inliers
for it in range(maxIteration):
# Samples 4 random points
id_samples = random.sample(range(0, n_points), 4)
pt_samples = pts[id_samples]
# We calculate the 4x4 determinant by dividing the problem in determinants of 3x3 matrix
# Multiplied by (x²+y²+z²)
d_matrix = np.ones((4, 4))
for i in range(4):
d_matrix[i, 0] = pt_samples[i, 0]
d_matrix[i, 1] = pt_samples[i, 1]
d_matrix[i, 2] = pt_samples[i, 2]
M11 = np.linalg.det(d_matrix)
# Multiplied by x
for i in range(4):
d_matrix[i, 0] = np.dot(pt_samples[i], pt_samples[i])
d_matrix[i, 1] = pt_samples[i, 1]
d_matrix[i, 2] = pt_samples[i, 2]
M12 = np.linalg.det(d_matrix)
# Multiplied by y
for i in range(4):
d_matrix[i, 0] = np.dot(pt_samples[i], pt_samples[i])
d_matrix[i, 1] = pt_samples[i, 0]
d_matrix[i, 2] = pt_samples[i, 2]
M13 = np.linalg.det(d_matrix)
# Multiplied by z
for i in range(4):
d_matrix[i, 0] = np.dot(pt_samples[i], pt_samples[i])
d_matrix[i, 1] = pt_samples[i, 0]
d_matrix[i, 2] = pt_samples[i, 1]
M14 = np.linalg.det(d_matrix)
# Multiplied by 1
for i in range(4):
d_matrix[i, 0] = np.dot(pt_samples[i], pt_samples[i])
d_matrix[i, 1] = pt_samples[i, 0]
d_matrix[i, 2] = pt_samples[i, 1]
d_matrix[i, 3] = pt_samples[i, 2]
M15 = np.linalg.det(d_matrix)
# Now we calculate the center and radius
center = [0.5 * (M12 / M11), -0.5 * (M13 / M11), 0.5 * (M14 / M11)]
radius = np.sqrt(np.dot(center, center) - (M15 / M11))
# Distance from a point
pt_id_inliers = [] # list of inliers ids
dist_pt = center - pts
dist_pt = np.linalg.norm(dist_pt, axis=1)
# Select indexes where distance is biggers than the threshold
pt_id_inliers = np.where(np.abs(dist_pt - radius) <= thresh)[0]
if len(pt_id_inliers) > len(best_inliers):
best_inliers = pt_id_inliers
self.inliers = best_inliers
self.center = center
self.radius = radius
return self.center, self.radius, self.inliers