diff --git a/pyransac3d/torus.py b/pyransac3d/torus.py new file mode 100644 index 0000000..957768b --- /dev/null +++ b/pyransac3d/torus.py @@ -0,0 +1,107 @@ +import numpy as np +import random +import copy +from .aux import * + +class Torus: + """ + Implementation for cylinder RANSAC. + + This class finds a infinite height cilinder and returns the cylinder axis, center and radius. + This method uses 6 points to find 3 best plane equations orthogonal to each other. We could use a recursive planar RANSAC, but it would use 9 points instead, making this algorithm more efficient. + + --- + """ + + def __init__(self): + self.inliers = [] + self.center = [] + self.axis = [] + self.radius = 0 + + def fit(self, pts, thresh=0.2, maxIteration=10000): + """ + Find the parameters (axis and radius) to define a cylinder. + + :param pts: 3D point cloud as a numpy array (N,3). + :param thresh: Threshold distance from the cylinder 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. + - `axis`: Vector describing cylinder's axis np.array(1,3). + - `radius`: Radius of cylinder. + - `inliers`: Inlier's index from the original point cloud. + --- + """ + + n_points = pts.shape[0] + best_eq = [] + best_inliers = [] + + for it in range(maxIteration): + + # Samples 3 random points + id_samples = random.sample(range(1, n_points-1), 3) + pt_samples = pts[id_samples] + + # We have to find the plane equation described by those 3 points + # We find first 2 vectors that are part of this plane + # A = pt2 - pt1 + # B = pt3 - pt1 + + vecA = pt_samples[1,:] - pt_samples[0,:] + vecA_norm = vecA / np.linalg.norm(vecA) + vecB = pt_samples[2,:] - pt_samples[0,:] + vecB_norm = vecB / np.linalg.norm(vecB) + + # Now we compute the cross product of vecA and vecB to get vecC which is normal to the plane + vecC = np.cross(vecA_norm, vecB_norm) + vecC = vecC / np.linalg.norm(vecC) + + # Now we calculate the rotation of the points with rodrigues equation + P_rot = rodrigues_rot(pt_samples, vecC, [0,0,1]) + + # Find center from 3 points + # http://paulbourke.net/geometry/circlesphere/ + # Find lines that intersect the points + # Slope: + ma = 0 + mb = 0 + while(ma == 0): + ma = (P_rot[1, 1]-P_rot[0, 1])/(P_rot[1, 0]-P_rot[0, 0]) + mb = (P_rot[2, 1]-P_rot[1, 1])/(P_rot[2, 0]-P_rot[1, 0]) + if(ma == 0): + P_rot = np.roll(P_rot,-1,axis=0) + else: + break + + # Calulate the center by verifying intersection of each orthogonal line + p_center_x = (ma*mb*(P_rot[0, 1]-P_rot[2, 1]) + mb*(P_rot[0, 0]+P_rot[1, 0]) - ma*(P_rot[1, 0]+P_rot[2, 0]))/(2*(mb-ma)) + p_center_y = -1/(ma)*(p_center_x - (P_rot[0, 0]+P_rot[1, 0])/2)+(P_rot[0, 1]+P_rot[1, 1])/2 + p_center = [p_center_x, p_center_y, 0] + radius = np.linalg.norm(p_center - P_rot[0, :]) + + # Remake rodrigues rotation + center = rodrigues_rot(p_center, [0,0,1], vecC)[0] + + # Distance from a point to a line + pt_id_inliers = [] # list of inliers ids + vecC_stakado = np.stack([vecC]*n_points,0) + dist_pt = np.cross(vecC_stakado, (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.axis = vecC + self.radius = radius + + return self.center, self.axis, self.radius, self.inliers + + diff --git a/test_torus.py b/test_torus.py new file mode 100644 index 0000000..f30651e --- /dev/null +++ b/test_torus.py @@ -0,0 +1,44 @@ +import open3d as o3d +import numpy as np +import random +import copy +import pyransac3d as pyrsc + +mesh_cylinder = o3d.geometry.TriangleMesh.create_torus(torus_radius=1.0, tube_radius=0.5) +mesh_cylinder.compute_vertex_normals() +mesh_cylinder.paint_uniform_color([0.1, 0.9, 0.1]) +o3d.visualization.draw_geometries([mesh_cylinder]) +pcd_load=mesh_cylinder.sample_points_uniformly(number_of_points=2000) +o3d.visualization.draw_geometries([pcd_load]) + +points = np.asarray(pcd_load.points) + +# cil = pyrsc.Cylinder() + +# center, normal, radius, inliers = cil.fit(points, thresh=0.05) +# print("center: "+str(center)) +# print("radius: "+str(radius)) +# print("vecC: "+str(normal)) + + +# R = pyrsc.get_rotationMatrix_from_vectors([0, 0, 1], normal) + +# plane = pcd_load.select_by_index(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(inliers, invert=True) +# mesh = o3d.geometry.TriangleMesh.create_coordinate_frame(origin=[0,0,0], size = 0.2) +# cen = o3d.geometry.TriangleMesh.create_coordinate_frame(origin=center, size = 0.5) +# mesh_rot = copy.deepcopy(mesh).rotate(R, center=[0, 0, 0]) + +# mesh_cylinder = o3d.geometry.TriangleMesh.create_cylinder(radius=radius, +# height=0.5) +# mesh_cylinder.compute_vertex_normals() +# mesh_cylinder.paint_uniform_color([0.1, 0.9, 0.1]) +# mesh_cylinder = mesh_cylinder.rotate(R, center=[0, 0, 0]) +# mesh_cylinder = mesh_cylinder.translate((center[0], center[1], center[2])) +# o3d.visualization.draw_geometries([mesh_cylinder]) + +# o3d.visualization.draw_geometries([plane, not_plane, mesh,mesh_rot, mesh_cylinder]) \ No newline at end of file