original_docstring: 
          Runs the CPU version of the RANSAC algorithm on the provided data. If no
          arguments are passed for x_eval or y_eval, the algorithm will utilize
          the data already loaded by the class with the getData() routine.
  
          Setting the debug flag to [T] will cause statements to be printed to
          the command line. It is recommended to set this flag to [F] when timing
  
          THE MATH
  
          A plane is defined as:
  
          $$ ax + by + cz + d = 0 $$
  
          Given three points, the constants can be determined:
  
          $$ a = [(y_2 - y_1)(z_3 - z_1) - (z_2 - z_1)(y_3 - y_2)] $$
          $$ b = [(z_2 - z_1)(x_3 - x_1) - (x_2 - x_1)(z_3 - z_2)] $$
          $$ c = [(x_2 - x_1)(y_3 - y_1) - (y_2 - y_1)(x_3 - x_2)] $$
          $$ d = -(a * x_n + b * y_n + c * z_n) $$
  
          Consider all the other points in the point cloud and calculate the
          distance to the fit plane
  
          $$distance = \frac{a*x_4 + b*y_4 + c*z_5 + d}{\sqrt{a^2 + b^2 + c^2}}$$
  
          REFERENCES
          https://en.wikipedia.org/wiki/Random_sample_consensus
          https://medium.com/@ajithraj_gangadharan/3d-ransac-algorithm-for-lidar-pcd-segmentation-315d2a51351
          
docstring: Runs the CPU version of the RANSAC algorithm on the provided data. If no
  arguments are passed for x_eval or y_eval, the algorithm will utilize
  the data already loaded by the class with the getData() routine.
  
  Setting the debug flag to [T] will cause statements to be printed to
  the command line. It is recommended to set this flag to [F] when timing
  
  THE MATH
  
  A plane is defined as.
  
  
  
  Given three points, the constants can be determined.
  
  
  
  Consider all the other points in the point cloud and calculate the
  distance to the fit plane
  
  
  
  
docstring_tokens: Runs the CPU version of the RANSAC algorithm on the provided data . If no arguments are passed for x_eval or y_eval the algorithm will utilize the data already loaded by the class with the getData () routine . Setting the debug flag to [ T ] will cause statements to be printed to the command line . It is recommended to set this flag to [ F ] when timing THE MATH A plane is defined as . Given three points the constants can be determined . Consider all the other points in the point cloud and calculate the distance to the fit plane
code: def cpu(self,x_eval=None,y_eval=None,debug=True):
          """
          Runs the CPU version of the RANSAC algorithm on the provided data. If no
          arguments are passed for x_eval or y_eval, the algorithm will utilize
          the data already loaded by the class with the getData() routine.
  
          Setting the debug flag to [T] will cause statements to be printed to
          the command line. It is recommended to set this flag to [F] when timing
  
          THE MATH
  
          A plane is defined as:
  
          $$ ax + by + cz + d = 0 $$
  
          Given three points, the constants can be determined:
  
          $$ a = [(y_2 - y_1)(z_3 - z_1) - (z_2 - z_1)(y_3 - y_2)] $$
          $$ b = [(z_2 - z_1)(x_3 - x_1) - (x_2 - x_1)(z_3 - z_2)] $$
          $$ c = [(x_2 - x_1)(y_3 - y_1) - (y_2 - y_1)(x_3 - x_2)] $$
          $$ d = -(a * x_n + b * y_n + c * z_n) $$
  
          Consider all the other points in the point cloud and calculate the
          distance to the fit plane
  
          $$distance = \frac{a*x_4 + b*y_4 + c*z_5 + d}{\sqrt{a^2 + b^2 + c^2}}$$
  
          REFERENCES
          https://en.wikipedia.org/wiki/Random_sample_consensus
          https://medium.com/@ajithraj_gangadharan/3d-ransac-algorithm-for-lidar-pcd-segmentation-315d2a51351
          """
          if debug: print("Running CPU Version")
  
          # Assign the Local Data (If applicable)
          if x_eval is not None: self.x_eval = x_eval
          if y_eval is not None: self.y_eval = y_eval
  
          # Get up Variables
          # num_pts_train = self.x_train.shape[0]
          # y_eval = np.zeros(num_pts_eval)
  
          # Perform the Iterations
          pts_idx_inliers = [] # For points already on the plane
          pts_best = []
          constants_best = []
          for i in range(self.num_iters):
              # Get a Random Sampling of Candidate Indexs
              pts_idx = np.random.randint(0,self.x_eval.shape[0],(3))
              # pts_idx_inliers(pts_idx)
  
              if debug: print("Candidate Points Index:\n",pts_idx)
  
              # Get the Candidate Points
              pts = self.x_eval[pts_idx,:]
              if debug: print("Candidate Points:\n",pts)
  
              # Calculate the Constants for the given points
  
              # $$ a = [(y_2 - y_1)(z_3 - z_1) - (z_2 - z_1)(y_3 - y_2)] $$
              a = (pts[1,1]-pts[0,1])*(pts[2,2]-pts[0,2]) - (pts[1,2]-pts[0,2])*(pts[2,1]-pts[1,1])
  
              # $$ b = [(z_2 - z_1)(x_3 - x_1) - (x_2 - x_1)(z_3 - z_2)] $$
              b = (pts[1,2]-pts[0,2])*(pts[2,0]-pts[0,0]) - (pts[1,0]-pts[0,0])*(pts[2,2]-pts[1,2])
  
              # $$ c = [(x_2 - x_1)(y_3 - y_1) - (y_2 - y_1)(x_3 - x_2)] $$
              c = (pts[1,0]-pts[0,0])*(pts[2,1]-pts[0,1]) - (pts[1,1]-pts[0,1])*(pts[2,0]-pts[1,0])
  
              # $$ d = -(a * x_n + b * y_n + c * z_n) $$
              d = -(a*pts[0,0] + b*pts[0,1] + c*pts[0,2])
  
              # $$ psq = sqrt{(a^2) + (b^2 + (c^2)} $$
              psq = max(0.1,np.sqrt(a*a + b*b + c*c))
  
              if debug: print(a,b,c,d)
  
              # Evaluate the Performance of the Fit
              pts_inliers = None
              for j in range(self.x_eval.shape[0]):
                  if debug: print(j,self.x_eval.shape[0])
                  # We do not want to compare with points used to calc a,b,c,d
                  # if j in pts_idx: continue
  
                  # Calc distance between point and plane
                  if debug: print(j)
                  dist = math.fabs(a*self.x_eval[j,0] + b*self.x_eval[j,1] + c*self.x_eval[j,2] + d)/psq
  
                  # Check whether or no the point is a good fit
                  if (dist <= self.dist_thresh):
                      # Add to the list if inlier points
                      if pts_inliers is None:
                          pts_inliers = self.x_eval[j,:]
                      else:
                          pts_inliers = np.vstack((pts_inliers,self.x_eval[j,:]))
  
              # Check if the current inliers is better than the best so far
              if len(pts) > len(pts_best):
                  pts_best = pts
                  constants_best = np.array([a,b,c,d])
  
          return pts_best, constants_best
code_tokens: def cpu ( self , x_eval = None , y_eval = None , debug = True ) : if debug : print ( "Running CPU Version" ) if x_eval is not None : self . x_eval = x_eval if y_eval is not None : self . y_eval = y_eval pts_idx_inliers = [ ] pts_best = [ ] constants_best = [ ] for i in range ( self . num_iters ) : pts_idx = np . random . randint ( 0 , self . x_eval . shape [ 0 ] , ( 3 ) ) if debug : print ( "Candidate Points Index:\n" , pts_idx ) pts = self . x_eval [ pts_idx , : ] if debug : print ( "Candidate Points:\n" , pts ) a = ( pts [ 1 , 1 ] - pts [ 0 , 1 ] ) * ( pts [ 2 , 2 ] - pts [ 0 , 2 ] ) - ( pts [ 1 , 2 ] - pts [ 0 , 2 ] ) * ( pts [ 2 , 1 ] - pts [ 1 , 1 ] ) b = ( pts [ 1 , 2 ] - pts [ 0 , 2 ] ) * ( pts [ 2 , 0 ] - pts [ 0 , 0 ] ) - ( pts [ 1 , 0 ] - pts [ 0 , 0 ] ) * ( pts [ 2 , 2 ] - pts [ 1 , 2 ] ) c = ( pts [ 1 , 0 ] - pts [ 0 , 0 ] ) * ( pts [ 2 , 1 ] - pts [ 0 , 1 ] ) - ( pts [ 1 , 1 ] - pts [ 0 , 1 ] ) * ( pts [ 2 , 0 ] - pts [ 1 , 0 ] ) d = - ( a * pts [ 0 , 0 ] + b * pts [ 0 , 1 ] + c * pts [ 0 , 2 ] ) psq = max ( 0.1 , np . sqrt ( a * a + b * b + c * c ) ) if debug : print ( a , b , c , d ) pts_inliers = None for j in range ( self . x_eval . shape [ 0 ] ) : if debug : print ( j , self . x_eval . shape [ 0 ] ) if debug : print ( j ) dist = math . fabs ( a * self . x_eval [ j , 0 ] + b * self . x_eval [ j , 1 ] + c * self . x_eval [ j , 2 ] + d ) / psq if ( dist <= self . dist_thresh ) : if pts_inliers is None : pts_inliers = self . x_eval [ j , : ] else : pts_inliers = np . vstack ( ( pts_inliers , self . x_eval [ j , : ] ) ) if len ( pts ) > len ( pts_best ) : pts_best = pts constants_best = np . array ( [ a , b , c , d ] ) return pts_best , constants_best
short_docstring: Runs the CPU version of the RANSAC algorithm on the provided data.
short_docstring_tokens: Runs the CPU version of the RANSAC algorithm on the provided data .
comment: """
          Runs the CPU version of the RANSAC algorithm on the provided data. If no
          arguments are passed for x_eval or y_eval, the algorithm will utilize
          the data already loaded by the class with the getData() routine.
  
          Setting the debug flag to [T] will cause statements to be printed to
          the command line. It is recommended to set this flag to [F] when timing
  
          THE MATH
  
          A plane is defined as:
  
          $$ ax + by + cz + d = 0 $$
  
          Given three points, the constants can be determined:
  
          $$ a = [(y_2 - y_1)(z_3 - z_1) - (z_2 - z_1)(y_3 - y_2)] $$
          $$ b = [(z_2 - z_1)(x_3 - x_1) - (x_2 - x_1)(z_3 - z_2)] $$
          $$ c = [(x_2 - x_1)(y_3 - y_1) - (y_2 - y_1)(x_3 - x_2)] $$
          $$ d = -(a * x_n + b * y_n + c * z_n) $$
  
          Consider all the other points in the point cloud and calculate the
          distance to the fit plane
  
          $$distance = \frac{a*x_4 + b*y_4 + c*z_5 + d}{\sqrt{a^2 + b^2 + c^2}}$$
  
          REFERENCES
          https://en.wikipedia.org/wiki/Random_sample_consensus
          https://medium.com/@ajithraj_gangadharan/3d-ransac-algorithm-for-lidar-pcd-segmentation-315d2a51351
          """$SEP$# Assign the Local Data (If applicable)$SEP$# Get up Variables$SEP$# num_pts_train = self.x_train.shape[0]$SEP$# y_eval = np.zeros(num_pts_eval)$SEP$# Perform the Iterations$SEP$# For points already on the plane$SEP$# Get a Random Sampling of Candidate Indexs$SEP$# pts_idx_inliers(pts_idx)$SEP$# Get the Candidate Points$SEP$# Calculate the Constants for the given points$SEP$# $$ a = [(y_2 - y_1)(z_3 - z_1) - (z_2 - z_1)(y_3 - y_2)] $$$SEP$# $$ b = [(z_2 - z_1)(x_3 - x_1) - (x_2 - x_1)(z_3 - z_2)] $$$SEP$# $$ c = [(x_2 - x_1)(y_3 - y_1) - (y_2 - y_1)(x_3 - x_2)] $$$SEP$# $$ d = -(a * x_n + b * y_n + c * z_n) $$$SEP$# $$ psq = sqrt{(a^2) + (b^2 + (c^2)} $$$SEP$# Evaluate the Performance of the Fit$SEP$# We do not want to compare with points used to calc a,b,c,d$SEP$# if j in pts_idx: continue$SEP$# Calc distance between point and plane$SEP$# Check whether or no the point is a good fit$SEP$# Add to the list if inlier points$SEP$# Check if the current inliers is better than the best so far