-
Notifications
You must be signed in to change notification settings - Fork 0
/
ransac3d.py
35 lines (33 loc) · 991 Bytes
/
ransac3d.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
import random
import open3d as o3d
import numpy as np
import utils
from ptypes import Plane
def Ransac3d(
points,
numberOfIterations = 100,
tolerance = 1):
'''
**** RANSAC 3D IMPLEMENTATION ****
'''
maxInlierscount = 0
for iteration in range(0, numberOfIterations):
(idx1, idx2, idx3) = utils.GenerateRandomIndicesTuple(points)
# create a plane from the three points
currentPlane = Plane.FromPoints(
points[idx1],
points[idx2],
points[idx3]
)
# find the count of points lying within tolerance of this plane
inlierIndices = []
for i in range(0, len(points)):
#TODO: do we need to skip the points used to make the plane?
distance = currentPlane.GetDistance(points[i])
if distance < tolerance:
inlierIndices.append(i)
if len(inlierIndices) > maxInlierscount:
fitPlane = currentPlane
fitIndices = inlierIndices
maxInlierscount = len(inlierIndices)
return (fitPlane, fitIndices)