Coverage for src/carbs/utilities/graphs.py: 94%

1""" Implements graph search algorithms.

3This module contains the functions for graph search algorithms.

4"""

5import numpy as np

6from warnings import warn

8import carbs.utilities.graphs_subroutines as subs

11def k_shortest(log_cost_in, k):

12 """This implents k-shortest path algorithm.

14 This ports the implementation from

15 `here <https://ba-tuong.vo-au.com/codes.html>`_.

16 to python. The following are comments from the original implementation.

18 Meral Shirazipour

19 This function is based on Yen's k-Shortest Path algorithm (1971)

20 This function calls a slightly modified function dijkstra()

21 by Xiaodong Wang 2004.

22 * log_cost_in must have positive weights/costs

23 Modified by BT VO

24 Replaces dijkstra's algorithm with Derek O'Connor's

25 Bellman-Ford-Moore implementation which allows negative entries in cost

26 matrices provided there are no negative cycles and used in GLMB filter

27 codes for prediction * netCostMatrix can have negative weights/costs

29 Args:

30 log_cost_in (Numpy array): Input cost matrix, inf represents absence

31 of a link

32 k (int): Maximum number of paths to find

34 Returns:

35 tuple containing:

37 - paths (list): List with each element being a list of

38 indices into the cost matrix for the shortest path

39 - costs (list): List of costs associated with each path

40 """

42 if k == 0:

43 paths = []

44 costs = []

45 return (paths, costs)

47 if log_cost_in.size > 1:

48 log_cost = np.squeeze(log_cost_in.copy())

49 else:

50 log_cost = log_cost_in.copy()

52 num_states = log_cost.size

53 cost_mat = np.zeros((num_states, num_states))

55 # sort in descending order and save index

56 sort_inds = np.argsort(log_cost)

57 sort_inds = sort_inds[::-1]

58 log_cost = [log_cost[ii] for ii in sort_inds]

60 for ii in range(0, num_states):

61 if ii >= 1:

62 cost_mat[0:ii, ii] = log_cost[ii]

64 cost_pad = np.zeros((num_states + 2, num_states + 2))

65 cost_pad[0, 1:-1] = log_cost

66 cost_pad[0, -1] = np.finfo(float).eps

67 cost_pad[1:-1, -1] = np.finfo(float).eps

68 cost_pad[1:-1, 1:-1] = cost_mat

70 (paths, costs) = __k_short_helper(cost_pad, 0, num_states + 1, k)

72 for p in range(0, len(paths)):

73 if np.array_equal(np.array(paths[p]), np.array([0, num_states + 1])):

74 paths[p] = []

75 else:

76 sub = paths[p][1:-1]

77 for ii in range(0, len(sub)):

78 sub[ii] = sub[ii] - 1

79 paths[p] = [sort_inds[ii] for ii in sub]

81 return (paths, costs)

84def __k_short_helper(net_cost_mat, src, dst, k_paths):

85 if src > net_cost_mat.shape[0] or dst > net_cost_mat.shape[0]:

86 msg = "Source or destination nodes not part of cost matrix"

87 warn(msg, RuntimeWarning)

88 return ([], [])

90 (cost, path, _) = bfm_shortest_path(net_cost_mat, src, dst)

91 if len(path) == 0:

92 return ([], [])

94 P = []

95 P.append((path, cost))

96 cur_p = 0

98 X = []

99 X.append((len(P) - 1, path, cost))

100

101 S = []

102 S.append(path[0])

103

104 shortest_paths = []

105 shortest_paths.append(path)

106 tot_costs = []

107 tot_costs.append(cost)

108

109 while (len(shortest_paths) < k_paths) and (len(X) != 0):

110 for ii in range(0, len(X)):

111 if X[ii][0] == cur_p:

112 del X[ii]

113 break

114 P_ = P[cur_p][0].copy()

115

116 w = S[cur_p]

117 for ii in range(0, len(P_)):

118 if w == P_[ii]:

119 w_ind_in_path = ii

120

121 for ind_dev_vert in range(w_ind_in_path, len(P_) - 1):

122 temp_cost_mat = net_cost_mat.copy()

123 for ii in range(0, ind_dev_vert - 1):

124 v = P_[ii]

125 temp_cost_mat[v, :] = np.inf

126 temp_cost_mat[:, v] = np.inf

127

128 sp_same_sub_p = []

129 sp_same_sub_p.append(P_)

130 for sp in shortest_paths:

131 if len(sp) > ind_dev_vert:

132 if np.array_equal(

133 np.array(P_[0 : ind_dev_vert + 1]),

134 np.array(sp[0 : ind_dev_vert + 1]),

135 ):

136 sp_same_sub_p.append(sp)

137 v_ = P_[ind_dev_vert]

138 for sp in sp_same_sub_p:

139 nxt = sp[ind_dev_vert + 1]

140 temp_cost_mat[v_, nxt] = np.inf

141

142 sub_P = P_[0 : ind_dev_vert + 1]

143 cost_sub_P = 0

144 for ii in range(0, len(sub_P) - 1):

145 cost_sub_P += net_cost_mat[sub_P[ii], sub_P[ii + 1]]

146

147 (c, dev_p, _) = bfm_shortest_path(temp_cost_mat, P_[ind_dev_vert], dst)

148 if len(dev_p) > 0:

149 tmp_path = sub_P[0:-2] + dev_p

150 P.append((tmp_path, cost_sub_P + c))

151 S.append(P_[ind_dev_vert])

152 X.append((len(P) - 1, P[-1][0], P[-1][1]))

153

154 if len(X) > 0:

155 shortest_x_cost = X[0][2]

156 shortest_x = X[0][0]

157 for ii in range(1, len(X)):

158 if X[ii][2] < shortest_x_cost:

159 shortest_x = X[ii][0]

160 shortest_x_cost = X[ii][2]

161

162 cur_p = shortest_x

163 shortest_paths.append(P[cur_p][0])

164 tot_costs.append(P[cur_p][1])

165

166 return (shortest_paths, tot_costs)

167

168

169def bfm_shortest_path(ncm, src, dst):

170 """This implements the Bellman-Ford-Moore shortest path algorithm.

171

172 This ports the implementation from

173 `here <https://ba-tuong.vo-au.com/codes.html>`_.

174 to python. The following are comments from the original implementation.

175

178

179 Redistribution and use in source and binary forms, with or without

180 modification, are permitted provided that the following conditions are

181 met:

182

183 * Redistributions of source code must retain the above copyright

184 notice, this list of conditions and the following disclaimer.

185 * Redistributions in binary form must reproduce the above copyright

186 notice, this list of conditions and the following disclaimer in

187 the documentation and/or other materials provided with the distribution

188

189 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

190 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

191 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

192 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

193 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

194 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

195 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

196 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

197 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

198 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

199 POSSIBILITY OF SUCH DAMAGE.

200

201 Basic form of the Bellman-Ford-Moore Shortest Path algorithm

202 Assumes G(N,A) is in sparse adjacency matrix form, with abs(N) = n,

203 abs(A) = m = nnz(G). It constructs a shortest path tree with root r whichs

204 is represented by an vector of parent 'pointers' p, along with a vector

205 of shortest path lengths D.

206 Complexity: O(mn)

207 Derek O'Connor, 19 Jan, 11 Sep 2012. derekroconnor@eircom.net

208

209 Unlike the original BFM algorithm, this does an optimality test on the

210 SP Tree p which may greatly reduce the number of iters to convergence.

211 USE:

212 n=10^6; G=sprand(n,n,5/n); r=1; format long g;

213 tic; [p,D,iter] = BFMSpathOT(G,r);toc, disp([(1:10)' p(1:10) D(1:10)]);

214 WARNING:

215 This algorithm performs well on random graphs but may perform

216 badly on real problems.

217

218 Args:

219 ncm (numpy array): cost matrix

220 src (int): source index

221 dst (int): destination index

222

223 Returns:

224 tuple containing

225

226 - dist (float): Path distance

227 - path (list): Indices of the path

228 - pred (int): number of iterations

229 """

230 (pred, D, _) = __bfm_helper(ncm, src)

231 dist = D[dst]

232

233 if dist == np.inf:

234 path = []

235 else:

236 path = [dst]

237 while not (path[0] == src):

238 path = [pred[path[0]]] + path

239

240 return (dist, path, pred)

241

242

243def __bfm_helper(G, r):

244 """

247

248 Redistribution and use in source and binary forms, with or without

249 modification, are permitted provided that the following conditions are

250 met:

251

252 * Redistributions of source code must retain the above copyright

253 notice, this list of conditions and the following disclaimer.

254 * Redistributions in binary form must reproduce the above copyright

255 notice, this list of conditions and the following disclaimer in

256 the documentation and/or other materials provided with the distribution

257

258 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

259 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

260 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

261 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

262 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

263 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

264 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

265 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

266 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

267 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

268 POSSIBILITY OF SUCH DAMAGE.

269 """

270

271 def init(G):

272 # Transforms the sparse matrix G into the list-of-arcs form

273 # and intializes the shortest path parent-pointer and distance

274 # arrays, p and D.

275 # Derek O'Connor, 21 Jan 2012

276 inds = np.argwhere(np.abs(G.T) >= np.finfo(float).eps)

277 tail = inds[:, 1]

278 head = inds[:, 0]

279 W = G[tail, head]

280 sort_inds = np.lexsort((tail, head))

281 tail = [tail[ii] for ii in sort_inds]

282 head = [head[ii] for ii in sort_inds]

283 W = [W[ii] for ii in sort_inds]

284 n = G.shape[1]

285 m = np.count_nonzero(G)

286 p = [0] * n

287 D = np.inf * np.ones((n, 1))

288 return (m, n, p, D, tail, head, W)

289

290 (m, n, p, D, tail, head, W) = init(G)

291 p[r] = int(0)

292 D[r] = 0

293 for ii in range(0, n - 1):

294 optimal = True

295 for arc in range(0, m):

296 u = tail[arc]

297 v = head[arc]

298 duv = W[arc]

299 if D[v] > D[u] + duv:

300 D[v] = D[u] + duv

301 p[v] = u

302 optimal = False

303

304 n_iters = ii + 1

305 if optimal:

306 break

307

308 return (p, D, n_iters)

309

310

311def murty_m_best(cost_mat, m):

312 """This implements Murty's m-best ranked optimal assignment.

313

314 This ports the implementation from

315 `here <https://ba-tuong.vo-au.com/codes.html>`_.

316 to python. The following are comments from the original implementation.

317

318 MURTY Murty's Algorithm for m-best ranked optimal assignment problem

319 Port of Ba Tuong Vo's 2015 Matlab code

320 NOTE: the assignment is zero based indexing

321

322 Args:

323 cost_mat (numpy array): Cost matrix

324 m (int): Number of best ranked assignments to find

325

326 Returns:

327 tuple containing

328

329 - assigns (numpy array): Array of best paths

330 - costs (numpy array): Cost of each path

331 """

332 if len(cost_mat.shape) == 1 or len(cost_mat.shape) > 2:

333 raise RuntimeError("Cost matrix must be 2D array")

334

335 if m == 0:

336 return ([], [])

337 blk = -np.log(np.ones((cost_mat.shape[0], cost_mat.shape[0])))

338

339 cm = np.hstack((cost_mat, blk))

340 x = cm.min()

341 cm = cm - x

342

343 (assigns, costs) = __murty_helper(cm, m)

344

345 for ii, a in enumerate(assigns):

346 costs[ii] += x * np.count_nonzero(a >= 0)

347 assigns += 1

348

349 # remove extra entries

350 assigns = assigns[:, 0 : cost_mat.shape[0]]

351 # dummy assignmets are clutter

352 assigns[np.where(assigns > cost_mat.shape[1])] = 0

353 assigns = assigns.astype(int)

354 return (assigns, costs)

355

356

357def murty_m_best_all_meas_assigned(cost_mat, m):

358 """This implements Murty's m-best ranked optimal assignment.

359

360 This ports the implementation from

361 `here <https://ba-tuong.vo-au.com/codes.html>`_.

362 to python. The following are comments from the original implementation.

363

364 MURTY Murty's Algorithm for m-best ranked optimal assignment problem

365 Port of Ba Tuong Vo's 2015 Matlab code

366 NOTE: the assignment is zero based indexing

367

368 Args:

369 cost_mat (numpy array): Cost matrix

370 m (int): Number of best ranked assignments to find

371

372 Returns:

373 tuple containing

374

375 - assigns (numpy array): Array of best paths

376 - costs (numpy array): Cost of each path

377 """

378 if len(cost_mat.shape) == 1 or len(cost_mat.shape) > 2:

379 raise RuntimeError("Cost matrix must be 2D array")

380

381 if m == 0:

382 return ([], [])

383

384 cm = cost_mat

385 x = cm.min()

386 cm = cm - x

387

388 (assigns, costs) = __murty_helper(cm, m)

389

390 for ii, a in enumerate(assigns):

391 costs[ii] += x * np.count_nonzero(a >= 0)

392 assigns += 1

393

394 # remove extra entries

395 assigns = assigns[:, 0 : cost_mat.shape[0]]

396 # dummy assignmets are clutter

397 assigns[np.where(assigns > cost_mat.shape[1])] = 0

398 assigns = assigns.astype(int)

399 return (assigns, costs)

400

401

402def __murty_helper(p0, m):

403 (s0, c0) = subs.assign_opt(p0)

404 s0 = s0.T

405

406 if m == 1:

407 return (s0.reshape((1, s0.size)), np.array([c0]))

408

409 (n_rows, n_cols) = p0.shape

410 # preallocate arrays

411 blk_sz = 1000

412 ans_lst_P = np.zeros((n_rows, n_cols, blk_sz))

413 ans_lst_S = np.zeros((n_rows, blk_sz), dtype=int)

414 ans_lst_C = np.nan * np.ones(blk_sz)

415

416 ans_lst_P[:, :, 0] = p0

417 ans_lst_S[:, 0] = s0.T

418 ans_lst_C[0] = c0

419 ans_nxt_ind = 1

420

421 assigns = np.nan * np.ones((n_rows, m))

422 costs = np.zeros(m)

423

424 for ii in range(0, m):

425 # if cleared break

426 if np.isnan(ans_lst_C).all():

427 assigns = assigns[:, 0:ans_nxt_ind]

428 costs = costs[0:ans_nxt_ind]

429 break

430

431 # find lowest cost solution

432 idx_top = np.nanargmin(ans_lst_C[0:ans_nxt_ind])

433 assigns[:, ii] = ans_lst_S[:, idx_top]

434 costs[ii] = ans_lst_C[idx_top]

435

436 P_now = ans_lst_P[:, :, idx_top]

437 S_now = ans_lst_S[:, idx_top]

438

439 ans_lst_C[idx_top] = np.nan

440

441 for aw, aj in enumerate(S_now):

442 if aj >= 0:

443 P_tmp = P_now.copy()

444 if aj <= n_cols - n_rows - 1:

445 P_tmp[aw, aj] = np.inf

446 else:

447 P_tmp[aw, (n_cols - n_rows) :] = np.inf

448

449 (S_tmp, C_tmp) = subs.assign_opt(P_tmp)

450

451 S_tmp = S_tmp.T

452 if (S_tmp >= 0).all():

453 # allocate more space as needed

454 if ans_nxt_ind >= len(ans_lst_C):

455 ans_lst_P = np.concatenate(

456 (ans_lst_P, np.zeros((n_rows, n_cols, blk_sz))), axis=2

457 )

458 ans_lst_S = np.concatenate(

459 (ans_lst_S, np.zeros((n_rows, blk_sz), dtype=int)), axis=1

460 )

461 ans_lst_C = np.hstack((ans_lst_C, np.nan * np.ones(blk_sz)))

462

463 ans_lst_P[:, :, ans_nxt_ind] = P_tmp

464 ans_lst_S[:, ans_nxt_ind] = S_tmp

465 ans_lst_C[ans_nxt_ind] = C_tmp

466 ans_nxt_ind += 1

467

468 v_tmp = P_now[aw, aj]

469 P_now[aw, :] = np.inf

470 P_now[:, aj] = np.inf

471 P_now[aw, aj] = v_tmp

472

473 return (assigns.T, costs)

474

475

476def a_star_search(maze, start, end, cost=1):

477 """

478 Returns a list of tuples as a path from the given start to the given end in the given maze

479 :param maze:

480 :param cost

481 :param start:

482 :param end:

483 :return:

484 """

485

486 # Create start and end node with initized values for g, h and f

487 start_node = subs.AStarNode(None, tuple(start))

488 start_node.g = start_node.h = start_node.f = 0

489 end_node = subs.AStarNode(None, tuple(end))

490 end_node.g = end_node.h = end_node.f = 0

491

492 # Initialize both yet_to_visit and visited list

493 # in this list we will put all node that are yet_to_visit for exploration.

494 # From here we will find the lowest cost node to expand next

495 yet_to_visit_list = []

496

497 # in this list we will put all node those already explored so that we don't explore it again

498 visited_list = []

499

500 # Add the start node

501 yet_to_visit_list.append(start_node)

502

503 # Adding a stop condition. This is to avoid any infinite loop and stop

504 # execution after some reasonable number of steps

505 outer_iterations = 0

506 max_iterations = 100000

507

508 # what squares do we search . search movement is left-right-top-bottom

509 # (4 movements) from every positon

510 move = [

511 [-1, 0], # go up

512 [0, -1], # go left

513 [1, 0], # go down

514 [0, 1], # go right

515 [1, 1], # diagonals

516 [1, -1],

517 [-1, 1],

518 [-1, -1],

519 ]

520

521 """

522 1) We first get the current node by comparing all f cost and selecting the lowest cost node for further expansion

523 2) Check max iteration reached or not . Set a message and stop execution

524 3) Remove the selected node from yet_to_visit list and add this node to visited list

525 4) Perofmr Goal test and return the path else perform below steps

526 5) For selected node find out all children (use move to find children)

527 a) get the current postion for the selected node (this becomes parent node for the children)

528 b) check if a valid position exist (boundary will make few nodes invalid)

529 c) if any node is a wall then ignore that

530 d) add to valid children node list for the selected parent

531

532 For all the children node

533 a) if child in visited list then ignore it and try next node

534 b) calculate child node g, h and f values

535 c) if child in yet_to_visit list then ignore it

536 d) else move the child to yet_to_visit list

537 """

538 # find maze has got how many rows and columns

539 no_rows, no_columns = np.shape(maze)

540

541 # Loop until you find the end

542

543 while len(yet_to_visit_list) > 0:

544 # Every time any node is referred from yet_to_visit list, counter of limit operation incremented

545 outer_iterations += 1

546

547 # Get the current node

548 current_node = yet_to_visit_list[0]

549 current_index = 0

550 for index, item in enumerate(yet_to_visit_list):

551 if item.f < current_node.f:

552 current_node = item

553 current_index = index

554

555 # if we hit this point return the path such as it may be no solution or

556 # computation cost is too high

557 if outer_iterations > max_iterations:

558 print("giving up on pathfinding too many iterations")

559 return subs.astar_return_path(current_node, maze)

560

561 # Pop current node out off yet_to_visit list, add to visited list

562 yet_to_visit_list.pop(current_index)

563 visited_list.append(current_node)

564

565 # test if goal is reached or not, if yes then return the path

566 if current_node == end_node:

567 return subs.astar_return_path(current_node, maze)

568

569 # Generate children from all adjacent squares

570 children = []

571

572 for new_position in move:

573 # Get node position

574 node_position = (

575 current_node.position[0] + new_position[0],

576 current_node.position[1] + new_position[1],

577 )

578

579 # Make sure within range (check if within maze boundary)

580 if (

581 node_position[0] > (no_rows - 1)

582 or node_position[0] < 0

583 or node_position[1] > (no_columns - 1)

584 or node_position[1] < 0

585 or node_position[0] < 0

586 and node_position[1] < 0

587 or node_position[0] > (no_rows - 1)

588 and node_position[1] < 0

589 or node_position[0] < 0

590 and node_position[1] < (no_columns - 1)

591 or node_position[0] > (no_rows - 1)

592 and node_position[1] > (no_columns - 1)

593 ):

594 continue

595

596 # Make sure walkable terrain

597 if maze[node_position[0]][node_position[1]] != 0:

598 continue

599

600 # Create new node

601 new_node = subs.AStarNode(current_node, node_position)

602

603 # Append

604 children.append(new_node)

605

606 # Loop through children

607 for child in children:

608 # Child is on the visited list (search entire visited list)

609 if (

610 len(

611 [

612 visited_child

613 for visited_child in visited_list

614 if visited_child == child

615 ]

616 )

617 > 0

618 ):

619 continue

620

621 # Create the f, g, and h values

622 child.g = current_node.g + cost

623

624 # Heuristic costs calculated here, this is using euclidian distance

625 child.h = np.sqrt(

626 (

627 ((child.position[0] - end_node.position[0]) ** 2)

628 + ((child.position[1] - end_node.position[1]) ** 2)

629 )

630 )

631

632 # dx = abs(child.position[0] - end_node.position[0]) # diagonal heuristic

633 # dy = abs(child.position[1] - end_node.position[1])

634 # child.h = dx + dy - 1 + min(dx, dy)

635

636 child.f = child.g + child.h

637

638 # Child is already in the yet_to_visit list and g cost is already lower

639 if len([i for i in yet_to_visit_list if child == i and child.g > i.g]) > 0:

640 continue

641

642 # Add the child to the yet_to_visit list

643 yet_to_visit_list.append(child)