Ошибка соединения узлов для алгоритма поиска пути

---

Я пытаюсь соединить узлы на карте, чтобы построить граф узлов для алгоритма A*. Мой способ сделать это таков:

Когда узел создается, я ставлю галочку слева от него. Если эта коробка является препятствием, я двигаюсь дальше (потому что я не могу соединить 2 узла, если между ними есть препятствие). Если коробка является другим узлом, они соединены. Если поле ясно (не препятствие или узел), я ставлю галочку слева от него. Это продолжается до тех пор, пока не будет найдено препятствие или узел, или пока не будут проверены 7 ящиков слева от узла.

Затем я бы повторил описанный выше шаг для ящиков выше и северо-западнее узла.

Однако мой код для этого не дает желаемых результатов. Вместо линий, соединяющих узлы, рисуется куча почти случайных линий.

Я перепробовал все, что мог, чтобы понять, почему, но совершенно застрял.

Изображение всех узлов на карте (обратите внимание, что начальная и конечная точки не отображаются, так как они будут выбираться каждый раз пользователем):

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

Изображение всех препятствий распознанных на карте:

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

Что я уже пробовал:

2 функции, к которым относится эта проблема:

(Матрица-это двумерный массив всех препятствий, узлов и четких путей, нанесенных на карту)

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)

Остальная часть кода для справки:

#!/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()

---