<jupyter_start><jupyter_text>**Void detection using optical flow and vanishing points voting**In this work, we rely on algorithms that ***extract vanishing points from a pair of images*** captured via a monocular camera. These points are ***voted using optical flow*** to ***estimate the furthest empty plane*** from the camera to which the linear trajectory from the quadcopter is free of obstacles. --- The void detection algorithm can be split into1.   Feature extraction2.   Optical flow extraciton and clustering3.   Vanishing point extraction4.   Vanish point voting  **Configuration**<jupyter_code># for SIFT features
!pip install opencv-contrib-python==4.4.0.44
import numpy as np
import cv2 as cv
from matplotlib import pyplot as plt
import sys
import math
np.set_printoptions(threshold=sys.maxsize)
import warnings
warnings.simplefilter(action='ignore', category=FutureWarning)
from sklearn.cluster import KMeans
import seaborn as sns
from scipy import stats
from sklearn import metrics
from scipy.spatial.distance import cdist
from sklearn.neighbors import NearestNeighbors
cd /content/drive/My Drive/NavigationNet<jupyter_output>/content/drive/My Drive/NavigationNet<jupyter_text>**Initial parameters and functions**<jupyter_code># Parameters for lucas kanade optical flow
lk_params = dict( winSize  = (15,15),
                  maxLevel = 5,
                  criteria = (cv.TERM_CRITERIA_EPS | cv.TERM_CRITERIA_COUNT, 10, 0.03))
#image grid for intesection computation
side = 10
w = 1280
h = 720
def reject_outliers_mask(data, m=2):
    return abs(data - np.mean(data)) < m * np.std(data)
def reject_outliers(data,list1,list2,list3,m=1):
  mask = reject_outliers_mask(data,m)
  a = np.ma.compress_nd(np.ma.MaskedArray(list1, mask=~mask))
  b = np.ma.compress_nd(np.ma.MaskedArray(list2, mask=~mask))
  c = np.ma.compress_nd(np.ma.MaskedArray(list3, mask=~mask))
  return a,b,c
def calculate_G_ab(side,w,h,m_list,c_list,angle_list):  
  G_ab = np.zeros((int(h/side),int(w/side)))
  for idx,(a,b,theta) in enumerate(zip(m_list,c_list,angle_list)):
    for (c,d,gamma) in zip(m_list[(idx+1):len(m_list)],
                          c_list[(idx+1):len(m_list)],
                          angle_list[(idx+1):len(m_list)]):
      if (theta>155):
        theta = theta-180     
      if abs(theta-gamma) < 15 and abs(theta-gamma) > 5: 
        x = (d-b)/(a-c)
        y = a*x+b
        if x >= 0 and x < w and y >= 0 and y < h:
            G_ab[int(y/side),int(x/side)] = G_ab[int(y/side),int(x/side)] + 1
  return G_ab
def auto_canny(image, sigma=0.33):
	# compute the median of the single channel pixel intensities
	v = np.median(image)
	# apply automatic Canny edge detection using the computed median
	lower = int(max(0, (1.0 - sigma) * v))
	upper = int(min(255, (1.0 + sigma) * v))
	edged = cv.Canny(image, lower, upper)
	# return the edged image
	return edged
def img_reading(img1_o,img2_o):
  img1 = cv.cvtColor(img1_o,cv.COLOR_BGR2GRAY)
  img2 = cv.cvtColor(img2_o,cv.COLOR_BGR2GRAY)
  return img1,img2
def feature_extraction(img1,img2):
  # initiate detector
  extractor = cv.xfeatures2d.SIFT_create(nOctaveLayers=3)
  # find the keypoints and descriptors
  kp1, des1 = extractor.detectAndCompute(img1,None)
  kp2, des2 = extractor.detectAndCompute(img2,None)  
  # create array of feature points in img1
  pt0 = [m.pt for m in kp1]
  pt0 = np.float32(pt0).reshape(-1,1,2)
  return pt0
def compute_OF(img1,img2,pt0):
  # calculate optical flow
  p1, st, err = cv.calcOpticalFlowPyrLK(img1, img2, pt0, None, **lk_params)
  # calculate inverse optical flow
  p0r, _st, _err = cv.calcOpticalFlowPyrLK(img2, img1, p1, None, **lk_params)
  # validate optical flow to inverse flow
  d = abs(pt0-p0r).reshape(-1, 2).max(-1)
  good = d < 0.05
  # select good points
  good_new = p1[good==1]
  good_old = pt0[good==1]

  # create array of c and m coefficients of the lines (y = m*x + c)
  angle = []
  magnitude = []
  for i,(new,old) in enumerate(zip(good_new,good_old)):
    a,b = new.ravel()
    c,d = old.ravel()
    m = math.degrees(math.sqrt(pow(d-b,2)+pow(a-c,2)))
    a = math.degrees(math.atan2((b-d),(c-a)))
    if a < 0:
      a = a + 360
    angle.append(a)
    magnitude.append(m)
  return good_old,good_new,angle,magnitude
def magnitude_filter(angle,magnitude,good_old,good_new):
  # define number of itens to be eliminated
  k = int(len(magnitude)*0.1)
  # create array of ordered magnitude vectors
  idx = np.argpartition(magnitude,k)
  # mask to remove the bigger 10% magnitude OF vectors
  mask = np.ones_like(magnitude, dtype=bool)
  mask[idx[:k]] = False
  magnitude_MagValid = np.ma.compress_nd(np.ma.MaskedArray(magnitude, mask=~mask))
  x_new_MagValid = np.ma.compress_nd(np.ma.MaskedArray(good_new.reshape(-1,2)[:,0], 
                                                      mask=~mask))
  y_new_MagValid = np.ma.compress_nd(np.ma.MaskedArray(good_new.reshape(-1,2)[:,1], 
                                                      mask=~mask))
  angle_MagValid = np.ma.compress_nd(np.ma.MaskedArray(angle, mask=~mask))
  return x_new_MagValid, y_new_MagValid, magnitude_MagValid, angle_MagValid
def choose_sigma(angle_MagValid):
  # create vector to hold the number of filtered angles
  sum_mask = []
  # stop criteria: 0.05% of previous number of filtered vectors
  for i in range(1,10):
    mask = reject_outliers_mask(angle_MagValid,i)
    s = np.sum(mask)  
    sum_mask.append(s)
    if i > 1:
      if (s-sum_mask[-2]) < 0.05*sum_mask[-2]:
        sigma = i  
        break
  return sigma
def angular_filter(x_new_MagValid, y_new_MagValid, magnitude_MagValid, angle_MagValid):  
  # decision of sigma value of poisson sigma parameter 
  sigma = 2 #choose_sigma(angle_MagValid)
  # mask and angle poisson filtering
  mask = reject_outliers_mask(angle_MagValid,sigma)
  magnitudeValid = np.ma.compress_nd(np.ma.MaskedArray(magnitude_MagValid, 
                                                       mask=~mask))
  angleValid = np.ma.compress_nd(np.ma.MaskedArray(angle_MagValid, mask=~mask))
  xValid = np.ma.compress_nd(np.ma.MaskedArray(x_new_MagValid, mask=~mask))
  yValid = np.ma.compress_nd(np.ma.MaskedArray(y_new_MagValid, mask=~mask))
  return xValid,yValid,angleValid,magnitudeValid
def choose_n(X):
  calinski = []
  davies = []
  # range of calinski + davies-bouldin test
  K = range(2, 16)
  for k in K:
      # Building and fitting the model
      kmeanModel = KMeans(n_clusters=k,random_state=1).fit(X)
      kmeanModel.fit(X)
      labels = kmeanModel.labels_ 
      # calculating index
      calinski.append(metrics.calinski_harabasz_score(X, labels))
      davies.append(metrics.davies_bouldin_score(X, labels))
  # get number of cluster for bigger calinski and lower davies-bouldin
  n_clusters = 2 + np.argmax(np.array([i / j for i, j in zip(calinski, davies)]),
                            axis=None)
  return n_clusters
def clustering(X):
  n_clusters = 12 #choose_n(X)
  kmeans = KMeans(n_clusters = n_clusters, 
                  max_iter = 200).fit(X) 
  pred_y = kmeans.fit_predict(X) 
  return kmeans,pred_y,n_clusters
def choose_k(dataset,kmeans,n_clusters):
  mean_value = []
  # decision of k value of k neareast neighbors
  # stop criteria: 0.1% of previous estimated magnitudes
  for i in range(5, 100, 5):   
      knn = NearestNeighbors(n_neighbors=i) 
      knn.fit(dataset[:,0:2])
      idx = knn.kneighbors(kmeans.cluster_centers_[:,0:2].reshape(-1,2),
                          return_distance=False)
      m = np.array(np.mean(dataset[idx,3],axis=1))
      mean_value = np.append(mean_value,m,axis=0) 
      if i > 5:
        div = [i / j for i, j in zip(mean_value.reshape(-1, n_clusters)[-2,:] - m,
                                  mean_value.reshape(-1, n_clusters)[-2,:])]
        if div[np.argmax(np.array(div), axis=None)] < 0.1:
          n_neighbors = i
          break
  return n_neighbors
def NN(dataset,kmeans,n_clusters):
  n_neighbors = 10 #choose_k(dataset,kmeans,n_clusters)
  # fit nearest neighbors for each cluster center for optimal k
  knn = NearestNeighbors(n_neighbors=n_neighbors)
  knn.fit(dataset[:,0:2])
  kmeans_cluster_centers_mag = np.zeros((n_clusters,1))
  for i in range(0,n_clusters):
    idx = knn.kneighbors(kmeans.cluster_centers_[i,0:2].reshape(-1,2), return_distance=False)
    kmeans_cluster_centers_mag[i] = np.mean(dataset[idx.reshape(-1),3])
  
  # getting center of lowest magnitude (furtherst plane)
  of_idx = np.argmin(kmeans_cluster_centers_mag, axis=None)
  of_min_x = kmeans.cluster_centers_[of_idx,0]
  of_min_y = kmeans.cluster_centers_[of_idx,1]

  return n_neighbors,kmeans_cluster_centers_mag,of_idx,of_min_x,of_min_y,knn
def pre_VP_extracting(img2):
  cropped = img2[int(h/2):h,0:w]
  # gaussian filter for canny transformation processing
  blurred = cv.GaussianBlur(img2, (5, 5), 0)
  # canny edge filter
  edges = auto_canny(blurred,sigma=0.33)
  # line recognition via hough transform
  lines = cv.HoughLinesP(edges,rho = 1,theta = np.pi/180,
                        threshold = 100,minLineLength = 50,
                        maxLineGap = 10)
  return lines
def VP_extracting(lines,n_clusters,kmeans):
  boundary = 40
  angle=[]
  # calculate, store and plot lines
  for idx,(x1,y1,x2,y2) in enumerate(lines.reshape(-1,4)):
      if x1 == x2: continue # pure horizontal lines
      elif y1 == y2: continue # pure vertical lines
      elif y1 < int(h/2) or y2 < int(h/2): continue # pure vertical lines
      else: 
        angle_l = math.degrees(math.atan2((y2-y1),(x2-x1)))
        if angle_l < 0:
          angle_l = angle_l + 360
          angle_wrap = angle_l - 180
        else:
          angle_wrap = angle_l
        if angle_wrap>boundary and angle_wrap<(180-boundary) and abs(angle_wrap-90)>10:
          A = np.vstack([(x1,x2),np.ones(len((x1,x2)))]).T
          m, c = np.linalg.lstsq(A, (y1,y2))[0]      
          x_y0 = int(-c/m)
          x_yh = int((h-c)/m)
          x_mean = (x1+x2)/2
          y_mean = (y1+y2)/2
          if lines.shape[0] > 100:
            x_diff = kmeans.cluster_centers_[:,0] - np.ones((1,n_clusters))*x_mean
            y_diff = kmeans.cluster_centers_[:,1] - np.ones((1,n_clusters))*y_mean
            r = np.sqrt(x_diff*x_diff + y_diff*y_diff)  
            idx = np.argmin(r, axis=None)
          else:
            idx = 0;
          name = "list_"+str(idx)
          n = [m,c,angle_wrap]
          globals()[name] = np.append(globals()[name],n,axis=0) 
def compute_Gab(n_clusters): 
  # initialize variables
  G_ab = np.zeros((int(h/side),int(w/side),n_clusters))

  # compute greatest intersection point
  for i in range(0,n_clusters):
    name = "list_"+str(i)
    G_ab[:,:,i] = calculate_G_ab(side,w,h,
                                globals()[name][:,0],globals()[name][:,1],
                                globals()[name][:,2])
  return G_ab
def result(G_ab,kmeans,kmeans_cluster_centers_mag,of_idx,of_min_x,of_min_y,n_clusters,knn,dataset):
  dist = np.ones(n_clusters)*100
  valid = np.ones(n_clusters)
  for i in range(0,n_clusters): 
    # get pixel coordinates of VP point plane cantidate
    y_idx,x_idx = np.unravel_index(np.argmax(G_ab[:,:,i], axis=None),
                                    G_ab[:,:,i].shape)
    if (y_idx or x_idx): 
      # if vanishing point is not out of image
      x = int((x_idx+1.5)*side)
      y = int((y_idx+1.5)*side)
      dist[i] = math.hypot((int(of_min_y)-y), (int(of_min_x)-x))
    else: 
      valid[i] = 100

  # select nearest VP to OF cluster center
  nearest_VP = np.argmin(np.array(np.multiply(dist,valid)))
  # get pixel coordinates nearest VP to OF cluster center
  y_idx,x_idx = np.unravel_index(np.argmax(G_ab[:,:,nearest_VP], axis=None),
                                G_ab[:,:,nearest_VP].shape)
  x = int((x_idx+1.5)*side)
  y = int((y_idx+1.5)*side)
  for i in range(0,n_clusters): 
    # get distance of nearest VP to all cluster centers
    dist[i] = math.hypot((int(kmeans.cluster_centers_[i,1])-y), 
                        (int(kmeans.cluster_centers_[i,0])-x))
  # get magnitude for VP
  nearest_VP_mag_idx = knn.kneighbors(np.array(((x+of_min_x)/2,
                                                (y+of_min_y)/2)).reshape(-1,2), 
                                      return_distance=False)
  nearest_VP_mag = np.mean(dataset[nearest_VP_mag_idx.reshape(-1),3])
  if abs(kmeans_cluster_centers_mag[of_idx]-nearest_VP_mag) < 0.05*kmeans_cluster_centers_mag[of_idx]:
    print("VP valid")
  else:
    print("VP not valid")
def main(img1_o,img2_o):

  ######################
  ### pre processing ###
  ######################
  # convert images to greyscale
  img1,img2 = img_reading(img1_o,img2_o)   
  # create array of feature points in img1
  pt0 = feature_extraction(img1,img2)
  
  ####################
  ### optical flow ###
  ####################
  # calculate optical flow
  good_old,good_new,angle,magnitude = compute_OF(img1,img2,pt0)  
  # remove the bigger 10% magnitude OF vectors
  x_new_MagValid,y_new_MagValid,magnitude_MagValid,angle_MagValid = magnitude_filter(angle,magnitude,good_old,good_new)
  # angle poisson filtering
  xValid,yValid,angleValid,magnitudeValid = angular_filter(x_new_MagValid, y_new_MagValid, magnitude_MagValid, angle_MagValid)  
  # identify if OF number filtered features is valid
  if len(xValid) > 100:
    print("OF identified")
  else:
    print("OF points NOT enough")
    breakpoint()  
  # creating datasets for unsupervisioned training
  dataset = np.array(list(zip(xValid, yValid,angleValid,
                              magnitudeValid))).reshape(len(xValid), 4)  
  # clustering
  kmeans,pred_y,n_clusters = clustering(dataset[:,0:3])
  # knn for clusters centers magnitude
  n_neighbors,kmeans_cluster_centers_mag,of_idx,of_min_x,of_min_y,knn = NN(dataset,kmeans,n_clusters)
  # identify if the drone is not in front of wall
  if abs(np.std(kmeans_cluster_centers_mag)) >= 500:
        print("Multiple planes available")
  else:
    print("Multiple planes NOT available")
    breakpoint()
  #####################
  ### VP extracting ###
  #####################
  # VP extracting Hough Lines
  lines = pre_VP_extracting(img2)
  # create n_clustered lists for m, c and angle of line
  for i in range(0,n_clusters):
    name = "list_"+str(i)
    globals()[name] = [0., 0., 0.]
  VP_extracting(lines,n_clusters,kmeans)
  # reorganize lists in 3 columns shape
  for i in range(0,n_clusters):
    name = "list_"+str(i)
    globals()[name] = np.array(globals()[name]).reshape(-1,3)
    globals()[name] = globals()[name][1:-1,:]
  G_ab = compute_Gab(n_clusters)
  result(G_ab,kmeans,kmeans_cluster_centers_mag,of_idx,of_min_x,of_min_y,n_clusters,knn,dataset)<jupyter_output><empty_output><jupyter_text>**Run**<jupyter_code>img1_o = cv.imread('./9_9_FRONT_LEFT.png') # queryImage
img2_o = cv.imread('./9_10_FRONT_LEFT.png') # trainImage
main(img1_o,img2_o)<jupyter_output>OF identified
Multiple planes available
VP valid
{'fraction_non_alphanumeric': 0.13162484905407218, 'fraction_numerical': 0.023480477660002684, 'mean_word_length': 3.398642667453526, 'pattern_counts': {'":': 0, '<': 24, '<?xml version=': 0, '>': 22, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 1, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 11, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '3', 'dir': 'jupyter-structured-clean-dedup', 'id': '397382', 'license': 'MIT', 'n_tokens_mistral': 5678, 'n_tokens_neox': 5395, 'n_words': 1125, 'path': 'notebook/public_opticalFlow_NavigationNet.ipynb', 'repo_name': 'debOliveira/voidDetection', 'size': '14906'}