Error connecting nodes for path finding algorithm

See more:

Python

OpenCV

I am trying to connect nodes on a map in order to build a node graph for an A* algorithm. My way of doing this is as so:

When a node is created, I check the box to the left of it. If this box is an obstacle, I move on (because I cannot connect 2 nodes if there is an obstacle in between them). If the box is another Node, they are connected. If the box is clear (not an obstacle or node), I check the box to the left of that. This goes on until either an obstacle or node is found, or until 7 boxes to the left of the Node have been checked.

I would then repeat the above step for boxes above and North West of the Node.

My code for doing so, however, does not produce the desired results. Instead of lines drawn connecting the nodes, a bunch of almost random lines are drawn instead.

I have tried everything I can as to figure out why, but am completely stuck.

Image of all of the nodes on the map (note that no starting and ending point is displayed because they will be chosen each time by the user):

https://i.imgur.com/RMhSGoq.jpg[^]

Image of all the obstacles recognized on the map:

https://i.imgur.com/RpiY2tf.jpg[^]

What I have tried:

The 2 functions to which this problem concerns:

(Matrix is a 2d array of all the obstacles, nodes, and clear paths mapped out)

def connect_to_other_Nodes(i,j):
    for a in range (0,7):
        if Matrix [i-a][j] == "o":
            break
        if Matrix [i-a][j] == "N":
            draw_line(i,j,i-a,j)


def createNewNode(i,j):
    newNode = Node()
    newNode.xcoordinate = i
    newNode.ycoordinate = j

    Nodes.append(newNode)
    
    converted_j = j * int(boxsize)
    converted_i = i * int(boxsize)
    cv2.rectangle(roomimg, (converted_j,converted_i), (converted_j+int(boxsize), converted_i+int(boxsize)),(0,255,255), 5)
    
    connect_to_other_Nodes(i,j)

The rest of the code for reference:

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Thu Mar 15 21:23:40 2018

@author: 2020shatgiskesselldd
"""
import cv2
import numpy as np
import scipy.signal
import math


class Node:
    xcoordinate = 0;
    ycoordinate = 0;



roomimg = cv2.imread("/Users/2020shatgiskessell/Desktop/Maps/medium2.jpg")

# edge detection
# ret, thresh = cv2.threshold(roomimg, 127, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C)
thresh = cv2.cvtColor(roomimg, cv2.COLOR_BGR2GRAY)
edge = cv2.Canny(thresh, 100, 200)
height,width,channels = roomimg.shape

#define the dimensions of the grid
def estimate_noise(I):

  H, W = I.shape

  M = [[1, -2, 1],
       [-2, 4, -2],
       [1, -2, 1]]

  sigma = np.sum(np.sum(np.absolute(scipy.signal.convolve2d(np.array(I), M))))

  sigma = sigma * np.sqrt(0.5 * np.pi) / (6 * (W-2) * (H-2))

  return sigma

boxsize = (math.pow(estimate_noise(edge),-0.708)* 112.32)
matrix_icrement_width = int(width/int(boxsize))
matrix_icrement_height = int(height/int(boxsize))
Matrix =  [[0 for x in range(matrix_icrement_width)] for y in range(matrix_icrement_height)] 
Nodes = []

#defines what are obstacles and what are not
cut_off_point = 15
print (boxsize)
#U HAVE TO CHANGE CUT OFF POINT BASED ON EVERY IMAGE

box_num = 0
boxesdrawn = 0
for i in range (0,height, int(boxsize)):
    for j in range (0,width, int(boxsize)):
        #1. DRAW THE BLOCKS
        roi_gray = edge[i:i+int(boxsize),j:j+int(boxsize)]
        #2. FIND INTENSITY OF ROI
        roi_avg_intensity = np.mean(roi_gray)
        #3. BASED ON THAT, SEE IF ROI IS AN OBSTACLE OR NOT
        #print(box_num,"(",i,",",j,")")
        
        if roi_avg_intensity > cut_off_point:
            # if box_num < 200:
                # print("roi_avg_intensity:", roi_avg_intensity)
                
            #DRAW RECTANGLE AROUND OBSATCLE
            cv2.rectangle(roomimg, (j,i), (j+int(boxsize), i+int(boxsize)),(0,0,255))
            #store top left corneer of ROI in Matrx
            Matrix[int(i/int(boxsize))][int(j/int(boxsize))] = "o"
            boxesdrawn += 1
        else:
            #store top left corneer of ROI in Matrx
            Matrix[int(i/int(boxsize))][int(j/int(boxsize))] = "c"
        box_num += 1
        
nodescreated = 0


def connect_to_other_Nodes(i,j):
    for a in range (0,7):
        if Matrix [i-a][j] == "o":
            break
        if Matrix [i-a][j] == "N":
            draw_line(i,j,i-a,j)


def createNewNode(i,j):
    newNode = Node()
    newNode.xcoordinate = i
    newNode.ycoordinate = j

    Nodes.append(newNode)
    
    converted_j = j * int(boxsize)
    converted_i = i * int(boxsize)
    cv2.rectangle(roomimg, (converted_j,converted_i), (converted_j+int(boxsize), converted_i+int(boxsize)),(0,255,255), 5)
    
    connect_to_other_Nodes(i,j)

#SO I KNOW THAT THE createNewNode(i,j) function is working as should
#AND I KNOW THAT THE Matrix DATA STRUCUTRE IS ALSO ALL G
#PERHAPS, INSTEAD OF USING A IMAGE, JUST USE A .TXT FILE AND ONLY DEAL WITH Matrix
def traverse_matrix():
    for i in range (0,matrix_icrement_width-1):
        for j in range (0,matrix_icrement_height-1):
            if Matrix[i][j]== "o" :
                #if u r on an obstacle, dont do anything
                continue
            
            if Matrix[i][j-2] == "N":
                #if there is a node 2 spaces behind u, dont do antything
                continue
            
            if Matrix[i][j-1] == "N":
                #if there is a node 2 spaces behind u, dont do antything
                continue
            
            if Matrix[i-2][j] == "N":
                #if there is a node 2 spaces behind u, dont do antything
                continue
            
            if Matrix[i-1][j] == "N":
                #if there is a node 2 spaces behind u, dont do antything
                continue
        
            if Matrix[i][j-1] == "o":
                #if u were on an obstacle, but not anymore
                createNewNode(i,j)
                Matrix[i][j] = "N"
            
            if Matrix[i+1][j] == "c":
                if Matrix[i-1][j] == "o":
                    #if the space below u is a path, but the space above u is an obstacle
                    createNewNode(i,j)
                    Matrix[i][j] = "N"
                
            if Matrix[i][j+1] == "o":
                #if the space infront of u is an obstacle
                createNewNode(i,j)
                Matrix[i][j] = "N"

                

def print_matrix():
    f = open('obstaclemap.txt', 'w')
    f.write('\n'.join([''.join(['{:4}'.format(item) for item in row]) 
      for row in Matrix]))
    f.close()

def draw_line(x1,y1, x2,y2):
    #convert Matrix point to points on the image
    x1 = x1 * int(boxsize)
    #deal with the center instead of the top left corner
    x1 = x1 + int(int(boxsize)/2)
    
    y1 = y1 * int(boxsize)
    y1 = y1 + int(int(boxsize)/2)
    
    x2 = x2 * int(boxsize)
    x2 = x2 + int(int(boxsize)/2)

    y2 = y2 * int(boxsize)
    y2 = y2 + int(int(boxsize)/2)


    cv2.line(roomimg,(x1,y1), (x2,y2), (255,255,255), 2)


def astar ():
    
#A STAR TIME
#1. Start with node N
    for node in Nodes:
        print (node.xcoordinate)
#2. Calculate distance between node N and its nearest neighbores in the NESW directions (possible add northeast, northwest, southeast, soutthwest)
#3. Calculate the distance of each of those nodes to the end point (the hueristic)
#4. Add them up
#5. Add the lowest cost node to the list
#6. N = lowest cost node
#FIGURE OUT WHAT TO DO AT END


traverse_matrix()
#print_matrix()
#astar()

#cv2.imwrite('roomimg.jpg', roomimg)
cv2.imshow("image", roomimg)


if cv2.waitKey(0) & 0xff == 27:
    cv2.destroyAllWindows()