(mesh, info_on_pix, cc, end_pt, global_mesh, input_edge, mask, valid_map, inpaint_id, npath_map=None, fpath_map=None, npath=None)
| 436 | return my_fpath_map, my_npath_map, npath, fpath |
| 437 | |
| 438 | def plan_path(mesh, info_on_pix, cc, end_pt, global_mesh, input_edge, mask, valid_map, inpaint_id, npath_map=None, fpath_map=None, npath=None): |
| 439 | my_npath_map = np.zeros_like(input_edge) - 1 |
| 440 | my_fpath_map = np.zeros_like(input_edge) - 1 |
| 441 | sub_mesh = mesh.subgraph(list(cc)).copy() |
| 442 | pnodes = netx.periphery(sub_mesh) |
| 443 | ends = [*end_pt] |
| 444 | edge_id = global_mesh.nodes[ends[0]]['edge_id'] |
| 445 | pnodes = sorted(pnodes, |
| 446 | key=lambda x: np.hypot((x[0] - ends[0][0]), (x[1] - ends[0][1])), |
| 447 | reverse=True)[0] |
| 448 | if npath is None: |
| 449 | npath = [*netx.shortest_path(sub_mesh, (ends[0][0], ends[0][1]), pnodes, weight='length')] |
| 450 | else: |
| 451 | if (ends[0][0], ends[0][1]) == npath[0]: |
| 452 | npath = npath |
| 453 | elif (ends[0][0], ends[0][1]) == npath[-1]: |
| 454 | npath = npath[::-1] |
| 455 | else: |
| 456 | import pdb; pdb.set_trace() |
| 457 | for np_node in npath: |
| 458 | my_npath_map[np_node[0], np_node[1]] = edge_id |
| 459 | fpath = [] |
| 460 | if global_mesh.nodes[ends[0]].get('far') is None: |
| 461 | print("None far") |
| 462 | else: |
| 463 | fnodes = global_mesh.nodes[ends[0]].get('far') |
| 464 | dmask = mask + 0 |
| 465 | did = 0 |
| 466 | while True: |
| 467 | did += 1 |
| 468 | if did > 3: |
| 469 | return my_fpath_map, my_npath_map, -1 |
| 470 | dmask = cv2.dilate(dmask, np.ones((3, 3)), iterations=1) |
| 471 | ffnode = [fnode for fnode in fnodes if (dmask[fnode[0], fnode[1]] > 0 and mask[fnode[0], fnode[1]] == 0 and\ |
| 472 | global_mesh.nodes[fnode].get('inpaint_id') != inpaint_id + 1)] |
| 473 | if len(ffnode) > 0: |
| 474 | fnode = ffnode[0] |
| 475 | break |
| 476 | |
| 477 | fpath.append((fnode[0], fnode[1])) |
| 478 | disp_diff = 0. |
| 479 | for n_loc in npath: |
| 480 | if mask[n_loc[0], n_loc[1]] != 0: |
| 481 | disp_diff = abs(abs(1. / info_on_pix[(n_loc[0], n_loc[1])][0]['depth']) - abs(1. / ends[0][2])) |
| 482 | break |
| 483 | barrel_dir = np.array([[1, 0], [1, 1], [0, 1], [-1, 1], [-1, 0], [-1, -1], [0, -1], [1, -1]]) |
| 484 | n2f_dir = (int(fnode[0] - npath[0][0]), int(fnode[1] - npath[0][1])) |
| 485 | while True: |
| 486 | if barrel_dir[0, 0] == n2f_dir[0] and barrel_dir[0, 1] == n2f_dir[1]: |
| 487 | n2f_barrel = barrel_dir.copy() |
| 488 | break |
| 489 | barrel_dir = np.roll(barrel_dir, 1, axis=0) |
| 490 | for step in range(0, len(npath)): |
| 491 | if step == 0: |
| 492 | continue |
| 493 | elif step == 1: |
| 494 | next_dir = (npath[step][0] - npath[step - 1][0], npath[step][1] - npath[step - 1][1]) |
| 495 | while True: |
no test coverage detected