├── .gitmodules ├── losses ├── sla_nll.py ├── da_nll.py ├── sla_balanced_ce.py └── da_model_free_kl_div.py ├── run_train_exp_04_dslp_alpha_ib.sh ├── run_eval_exp_04_dslp_alpha_ib.sh ├── graph_inference ├── dense_nms.py ├── grid_map.py ├── dsla_weight_matrix.py ├── max_likelihood_graph.py └── graph_func.py ├── eval ├── eval_iou.py ├── eval_f1_score.py └── eval_dir_accuracy.py ├── viz ├── viz_dataset.py └── viz_dense.py ├── models ├── unet_dsla.py └── unet.py ├── .gitignore ├── README.md ├── datamodule_preproc.py ├── train.py └── LICENSE /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "predictive-world-models"] 2 | path = predictive-world-models 3 | url = https://github.com/robin-karlsson0/predictive-world-models.git 4 | -------------------------------------------------------------------------------- /losses/sla_nll.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | 4 | def ce(output, label, eps=1e-14): 5 | '''Cross-entropy term. 6 | ''' 7 | return -label * torch.log(output + eps) 8 | 9 | 10 | def eval_sla_nll(output, label, drivable_N): 11 | ''' 12 | ''' 13 | nll = ce(output, label) + ce(1 - output, 1 - label) 14 | nll = torch.sum(nll, dim=(1, 2, 3)) 15 | # Mean over 'drivable' elems 16 | nll = torch.div(nll, drivable_N) 17 | # Mean over batch dim 18 | nll = torch.mean(nll) 19 | 20 | return nll 21 | -------------------------------------------------------------------------------- /losses/da_nll.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | 4 | def eval_da_nll(out_da, label_da): 5 | ''' 6 | Args: 7 | out_da: (batch_n, ang_range_disc, n, n) 8 | label_da: dimension (batch_n, ang_range_disc, n, n) 9 | ''' 10 | ########################## 11 | # GENERATE TARGET LABEL 12 | ########################## 13 | a = torch.sum(label_da, dim=1) # (B,H,W) 14 | dir_path_label = ~torch.isclose(a, torch.ones_like(a)) # (B,H,W) 15 | dir_path_N = torch.sum(dir_path_label, dim=(-2, -1)) 16 | 17 | nll = -1 * label_da * torch.log(out_da) 18 | nll = torch.sum(nll, dim=(1)) 19 | nll = dir_path_label * nll 20 | # Mean over 'drivable' elems 21 | nll = torch.sum(nll, dim=(-2, -1)) 22 | nll = torch.div(nll, dir_path_N) 23 | # Mean over batch dim 24 | nll = torch.mean(nll) 25 | 26 | return nll 27 | -------------------------------------------------------------------------------- /run_train_exp_04_dslp_alpha_ib.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | python train.py \ 4 | --accelerator gpu \ 5 | --devices 1 \ 6 | --precision 32 \ 7 | --num_workers 4 \ 8 | --profiler simple \ 9 | --max_epochs 1000 \ 10 | --lr 1e-3 \ 11 | --weight_decay 1e-4 \ 12 | --enc_str 1x16,1x16,1x32,1x32,1x64,1x64,1x128,1x256 \ 13 | --sla_dec_str 1x64,1x64,1x32,1x32,1x16,1x16,1x8,1x8 \ 14 | --da_dec_str 1x64,1x64,1x32,1x32,1x16,1x16,1x8,1x8 \ 15 | --input_ch 5 \ 16 | --out_feat_ch 32 \ 17 | --num_angs 36 \ 18 | --sla_head_layers 1 \ 19 | --da_head_layers 1 \ 20 | --base_channels 32 \ 21 | --dropout_prob 0 \ 22 | --batch_size 16 \ 23 | --train_data_dir ./data/bev_nuscenes_256px_v01_boston_seaport_gt_preproc_train \ 24 | --val_data_dir ./data/bev_nuscenes_256px_v01_boston_seaport_unaug_gt_eval_preproc \ 25 | --test_data_dir ./data/bev_nuscenes_256px_v01_boston_seaport_unaug_gt_eval_preproc \ 26 | --gradient_clip_val 35 \ 27 | --check_val_every_n_epoch 1 \ 28 | --num_sanity_val_steps=0 \ 29 | --viz_dir ./data/bev_nuscenes_256px_viz \ 30 | --do_augmentation \ 31 | -------------------------------------------------------------------------------- /run_eval_exp_04_dslp_alpha_ib.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | python train.py \ 4 | --accelerator gpu \ 5 | --devices 1 \ 6 | --precision 32 \ 7 | --num_workers 4 \ 8 | --profiler simple \ 9 | --max_epochs 1000 \ 10 | --lr 1e-3 \ 11 | --weight_decay 1e-4 \ 12 | --enc_str 1x16,1x16,1x32,1x32,1x64,1x64,1x128,1x256 \ 13 | --sla_dec_str 1x64,1x64,1x32,1x32,1x16,1x16,1x8,1x8 \ 14 | --da_dec_str 1x64,1x64,1x32,1x32,1x16,1x16,1x8,1x8 \ 15 | --input_ch 5 \ 16 | --out_feat_ch 32 \ 17 | --num_angs 36 \ 18 | --sla_head_layers 1 \ 19 | --da_head_layers 1 \ 20 | --base_channels 32 \ 21 | --dropout_prob 0 \ 22 | --batch_size 1 \ 23 | --train_data_dir ./data/bev_nuscenes_256px_v01_boston_seaport_gt_preproc_train \ 24 | --val_data_dir ./data/bev_nuscenes_256px_v01_boston_seaport_unaug_gt_eval_preproc \ 25 | --test_data_dir ./data/bev_nuscenes_256px_v01_boston_seaport_unaug_gt_eval_preproc \ 26 | --gradient_clip_val 35 \ 27 | --check_val_every_n_epoch 1 \ 28 | --num_sanity_val_steps=0 \ 29 | --viz_dir ./data/bev_nuscenes_256px_viz \ 30 | --do_augmentation \ 31 | --checkpoint_path checkpoints/exp_04_dslp_alpha_ib.ckpt \ 32 | --do_test \ 33 | --output_test_dir results/exp_04_alpha_ib \ 34 | -------------------------------------------------------------------------------- /graph_inference/dense_nms.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def dense_nonmax_sup(array, m, threshold=0.0): 5 | '''Reduces a dense array to its maximum values (or "peaks"). 6 | The algorithm iterates through each array element (i, j) one by one, and 7 | finds the highest value in its neighborhood (m, m). If (i, j) IS NOT the 8 | highest value, (i, j) is suppressed to zero. 9 | Args: 10 | array: Dense 2D float array of shape (n, n) 11 | m (int): Neighborhood size 12 | threshold (float): Elements bellow value zero regardles of neighborhood 13 | Returns: 14 | array_sup: Dense 2D float array with only maximum valued elements 15 | remain non-zero. 16 | ''' 17 | # Add padding to enable regular iteration 18 | array_pad = np.pad(array, m, "constant") 19 | array_sup = np.copy(array_pad) 20 | 21 | for i in range(m, array_pad.shape[0] - m): 22 | for j in range(m, array_pad.shape[1] - m): 23 | # Slice (m, m) neighbourhood around (i, j) 24 | neigh_array = array_pad[i - m:i + m, j - m:j + m] 25 | # Get maximum value in neighbourhood 26 | neigh_max = np.max(neigh_array) 27 | # Set (i, j) to zero if not largets value 28 | if (array_pad[i, j] < neigh_max or array_pad[i, j] < threshold): 29 | array_sup[i, j] = 0.0 30 | 31 | # Remove padding 32 | array_sup = array_sup[m:-m, m:-m] 33 | 34 | return array_sup 35 | -------------------------------------------------------------------------------- /losses/sla_balanced_ce.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | 4 | def ce(output, label, eps=1e-14): 5 | '''Cross-entropy term. 6 | ''' 7 | return -label * torch.log(output + eps) 8 | 9 | 10 | def loss_sla_balanced_ce(output, label, alpha, drivable_N): 11 | '''Computes the 'Soft-Lane Affordance' loss for an output-label tensor pair. 12 | 13 | By removing obvious 'obstacle elements' from the output, the model is able 14 | to learn about the actual road scene more effectively. 15 | 16 | ''' 17 | # Compute the ratio between 'True' and 'False' label path elements 18 | label_elements = torch.sum(label.detach(), (1, 2, 3), keepdim=True) 19 | beta = torch.div(label_elements + 1, drivable_N) # (batch_n,1,1,1) 20 | 21 | loss = beta * ce(1 - output, 1 - label) + alpha * (1 - beta) * ce( 22 | output, label) 23 | loss = torch.sum(loss, dim=(1, 2, 3), keepdim=True) 24 | 25 | loss = torch.div(loss, drivable_N + 1) 26 | 27 | loss = torch.mean(loss) 28 | 29 | # Loss contribution 30 | loss_neg = beta * ce(1 - output, 1 - label) 31 | loss_neg = torch.sum(loss_neg, dim=(1, 2, 3), keepdim=True) 32 | loss_neg = torch.div(loss_neg, drivable_N - label_elements + 1) 33 | loss_neg = torch.mean(loss_neg) 34 | 35 | loss_pos = alpha * (1 - beta) * ce(output, label) 36 | loss_pos = torch.sum(loss_pos, dim=(1, 2, 3), keepdim=True) 37 | loss_pos = torch.div(loss_pos, label_elements + 1) 38 | loss_pos = torch.mean(loss_pos) 39 | 40 | return loss, loss_neg.item(), loss_pos.item() 41 | -------------------------------------------------------------------------------- /eval/eval_iou.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | 4 | 5 | def eval_iou(pred_paths: list, 6 | gt_map: np.array, 7 | lane_width=7, 8 | gt_map_dilations=3, 9 | height=256, 10 | width=256): 11 | ''' 12 | Args: 13 | pred_paths: List of np.array (N,2) representing a trajectory of N 14 | (i, j) pnts. 15 | gt_lane: Lanes plotted onto a dense boolean map (H, W). 16 | lane_width: 7 corresponds to 9 pixels (?). 17 | 18 | Returns: 19 | IoU value. 20 | ''' 21 | #################### 22 | # Prediction map 23 | #################### 24 | pred_map = np.zeros((height, width, 3), dtype=np.uint8) 25 | for path in pred_paths: 26 | pnts = path.astype(np.int32) 27 | pnts = pnts.reshape((-1, 1, 2)) 28 | pred_map = cv2.polylines(pred_map, [pnts], 29 | isClosed=False, 30 | color=(255, 255, 255), 31 | thickness=lane_width) 32 | pred_map = pred_map / 255 33 | pred_map = pred_map[:, :, 0] 34 | pred_map = pred_map.astype(bool) 35 | 36 | ############ 37 | # GT map 38 | ############ 39 | gt_map = (255. * gt_map).astype(np.uint8) 40 | gt_map = np.expand_dims(gt_map, -1) 41 | gt_map = np.tile(gt_map, (1, 1, 3)) 42 | kernel = np.ones((3, 3), np.uint8) 43 | gt_map = cv2.dilate(gt_map, kernel, iterations=gt_map_dilations) 44 | gt_map = gt_map / 255 45 | gt_map = gt_map[:, :, 0] 46 | gt_map = gt_map.astype(bool) 47 | 48 | and_elems = np.logical_and(pred_map, gt_map) 49 | union_elems = np.logical_or(pred_map, gt_map) 50 | 51 | iou = np.sum(and_elems) / np.sum(union_elems) 52 | 53 | return iou 54 | -------------------------------------------------------------------------------- /eval/eval_f1_score.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | 4 | 5 | def eval_f1_score(pred_paths: list, 6 | gt_map: np.array, 7 | drivable: np.array, 8 | lane_width=7, 9 | gt_map_dilations=3, 10 | height=256, 11 | width=256): 12 | ''' 13 | Evaluate the F1 score over all road elements. 14 | 15 | Args: 16 | pred_paths: List of np.array (N,2) representing a trajectory of N 17 | (i, j) pnts. 18 | gt_lane: Lanes plotted onto a dense boolean map (H, W). 19 | drivable: Road region plotten onto a dense boolean map (H, W). 20 | lane_width: 7 corresponds to 9 pixels (?). 21 | 22 | Returns: 23 | F1 score. 24 | ''' 25 | #################### 26 | # Prediction map 27 | #################### 28 | pred_map = np.zeros((height, width, 3), dtype=np.uint8) 29 | for path in pred_paths: 30 | pnts = path.astype(np.int32) 31 | pnts = pnts.reshape((-1, 1, 2)) 32 | pred_map = cv2.polylines(pred_map, [pnts], 33 | isClosed=False, 34 | color=(255, 255, 255), 35 | thickness=lane_width) 36 | pred_map = pred_map / 255 37 | pred_map = pred_map[:, :, 0] 38 | pred_map = pred_map.astype(bool) 39 | 40 | ############ 41 | # GT map 42 | ############ 43 | gt_map = (255. * gt_map).astype(np.uint8) 44 | gt_map = np.expand_dims(gt_map, -1) 45 | gt_map = np.tile(gt_map, (1, 1, 3)) 46 | kernel = np.ones((3, 3), np.uint8) 47 | gt_map = cv2.dilate(gt_map, kernel, iterations=gt_map_dilations) 48 | gt_map = gt_map / 255 49 | gt_map = gt_map[:, :, 0] 50 | gt_map = gt_map.astype(bool) 51 | 52 | tp = np.logical_and(pred_map, gt_map) + np.logical_and(~pred_map, ~gt_map) 53 | fp = pred_map * np.logical_xor(pred_map, tp) 54 | fn = gt_map * np.logical_xor(gt_map, tp) 55 | 56 | tp[drivable == 0] = 0 57 | fp[drivable == 0] = 0 58 | fn[drivable == 0] = 0 59 | 60 | tp_sum = np.sum(tp) 61 | fp_sum = np.sum(fp) 62 | fn_sum = np.sum(fn) 63 | f1_score = 2 * tp_sum / (2 * tp_sum + fp_sum + fn_sum) 64 | 65 | return f1_score 66 | -------------------------------------------------------------------------------- /losses/da_model_free_kl_div.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.special 3 | import torch 4 | 5 | 6 | #################### 7 | # DEBUG FUNCTION 8 | #################### 9 | def integrate_distribution(dist, dist_range): 10 | '''Integrate a distribution using the trapezoidal approximation rule. 11 | 12 | Args: 13 | dist: Distribution values in 1D array. 14 | dist_range: Distrbution range in 1D array. 15 | 16 | Returns: 17 | Integration sum as float. 18 | ''' 19 | N = dist.shape[0] 20 | integ_sum = 0.0 21 | for i in range(N - 1): 22 | partion_range = dist_range[i + 1] - dist_range[i] 23 | dist_val = dist[i] + dist[i + 1] 24 | integ_sum += partion_range * dist_val / 2.0 25 | 26 | return integ_sum 27 | 28 | 29 | def biternion_to_angle(x, y): 30 | '''Converts biternion tensor representation to positive angle tensor. 31 | Args: 32 | x: Biternion 'x' component of shape (batch_n, n, n) 33 | y: Biternion 'y' component of shape (batch_n, n, n) 34 | ''' 35 | ang = torch.atan2(y, x) 36 | # Add 360 deg to negative angle elements 37 | mask = (ang < 0).float() 38 | ang = ang + 2.0 * np.pi * mask 39 | return ang 40 | 41 | 42 | def loss_da_kl_div(output_da, mm_ang_label): 43 | ########################## 44 | # GENERATE TARGET LABEL 45 | ########################## 46 | a = torch.sum(mm_ang_label, dim=1) # (B,H,W) 47 | dir_path_label = ~torch.isclose(a, torch.ones_like(a)) # (B,H,W) 48 | 49 | ################# 50 | # COMPUTE LOSS 51 | ################# 52 | 53 | # Try just maximizing log liklihood? 54 | KL_div = mm_ang_label * (torch.log(mm_ang_label + 1e-14) - 55 | torch.log(output_da + 1e-14)) 56 | 57 | # Sum distribution over every element-> dim (batch_n, y, x, 1) 58 | # num_angs = output_da.shape[1] 59 | KL_div = torch.sum(KL_div, dim=1) # * (2.0 * np.pi / num_angs) 60 | 61 | # Zero non-path elements 62 | KL_div = KL_div * dir_path_label # [:, 0].unsqueeze(-1) 63 | 64 | # Sum all element losses -> dim (batch_n) 65 | KL_div = torch.sum(KL_div, dim=(1, 2)) 66 | 67 | # Make loss invariant to path length by average element loss 68 | # - Summing all '1' elements -> dim (batch_n) 69 | dir_path_label_N = torch.sum(dir_path_label, dim=(1, 2)) 70 | KL_div = torch.div(KL_div, dir_path_label_N + 1) 71 | 72 | # Average of all batch losses to scalar 73 | KL_div = torch.mean(KL_div) 74 | 75 | return KL_div 76 | -------------------------------------------------------------------------------- /eval/eval_dir_accuracy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.signal import argrelextrema 3 | 4 | 5 | def eval_dir_acc(da: np.array, 6 | mm_gt_angs_tensor: np.array, 7 | threshold_ang: float = 90) -> float: 8 | ''' 9 | Args: 10 | da: Predicted directional probabilities (D, H, W). 11 | gt_lane: Lanes plotted onto a dense boolean map (H, W). 12 | mm_gt_angs_tensor: GT lane graph encoded directional probabilities 13 | (D, H, W). 14 | threshold_ang: Predicted direction correct if sufficient probability 15 | by integrating this interval. 16 | 17 | Returns: 18 | 19 | ''' 20 | num_dirs, I, J = da.shape 21 | 22 | delta_ang = 360 / num_dirs 23 | 24 | # Index interval i - delta_idx : i + delta_idx corresponding to angle 25 | delta_idx = int(np.floor(0.5 * (threshold_ang / delta_ang))) 26 | 27 | # Probability of direction interval given uniform probabilities 28 | p_dir_uniform = 1. / num_dirs 29 | p_dir_uniform_int = np.sum(2 * delta_idx * p_dir_uniform) 30 | 31 | # Find indices of elements having a GT direction encoded 32 | 33 | have_dirs = np.max(mm_gt_angs_tensor, axis=0) > p_dir_uniform 34 | i_idxs, j_idxs = np.where(have_dirs) 35 | 36 | # List with boolean values for correct direction 37 | dir_in_thresh = [] 38 | 39 | for idx in range(len(i_idxs)): 40 | 41 | i = i_idxs[idx] 42 | j = j_idxs[idx] 43 | 44 | p_dir_pred = da[:, i, j] 45 | p_dir_gt = mm_gt_angs_tensor[:, i, j] 46 | 47 | # Find GT directions as local maximums 48 | # NOTE: Perturbs values to avoid plateaus 49 | p_dir_gt += 1e-5 * np.arange(0, len(p_dir_gt)) 50 | p_dir_gt[p_dir_gt < p_dir_uniform] = 0 51 | gt_dir_idxs = argrelextrema(p_dir_gt, np.greater, mode='wrap') 52 | 53 | for gt_dir_idx in gt_dir_idxs: 54 | gt_dir_idx = gt_dir_idx[0] 55 | 56 | # Sum predicted directional probabilities within threshold angle 57 | idx_0 = gt_dir_idx - delta_idx 58 | idx_1 = gt_dir_idx + delta_idx 59 | p_dir_pred_idxs = np.arange(idx_0, idx_1, 1) 60 | 61 | # Reflect undershoot/overshoot idxs to other side of cyclical range 62 | mask = p_dir_pred_idxs < 0 63 | p_dir_pred_idxs[mask] = p_dir_pred_idxs[mask] + num_dirs 64 | mask = p_dir_pred_idxs >= num_dirs 65 | p_dir_pred_idxs[mask] = p_dir_pred_idxs[mask] - num_dirs 66 | 67 | p_dir_pred_int = p_dir_pred[p_dir_pred_idxs] 68 | p_dir_pred_int = np.sum(p_dir_pred_int) 69 | 70 | if p_dir_pred_int > p_dir_uniform_int: 71 | dir_in_thresh.append(True) 72 | else: 73 | dir_in_thresh.append(False) 74 | 75 | acc = np.sum(dir_in_thresh) / len(dir_in_thresh) 76 | 77 | return acc 78 | -------------------------------------------------------------------------------- /graph_inference/grid_map.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def grid_adj_mat(I, J, connectivity='4'): 5 | '''Generates an adjacency matrix for a 2D grid world. 6 | 7 | NOTE: Assumes that the grid origo is in the top-left corner. 8 | 9 | Node representation: 10 | 11 | Node idx 'i' denotes a location (i.e. element) in the grid world. 12 | Total nodes 'n' = I*J 13 | A node 'i' is connected to other nodes 'j' that have nonzero entries in A. 14 | 15 | How to use: 16 | 17 | Neighbor nodes 'j':s of node 'i' 18 | Ex: get_neighbor_nodes(0, A) --> [1, 30, 31] 19 | 20 | The only reachable nodes from node 0 are nodes 1, 30, and 31. 21 | 22 | Args: 23 | I (int): Row count 24 | J (int): Col count 25 | connectivity (str): Four- or eight-directional grid connectivity. 26 | ''' 27 | # Number of elements 28 | n = I * J 29 | A = np.zeros((n, n), dtype=np.int8) 30 | 31 | diag_block = np.zeros((J, J), dtype=np.int8) 32 | for idx in range(J - 1): 33 | diag_block[idx, idx + 1] = 1 34 | diag_block[idx + 1, idx] = 1 35 | 36 | if connectivity == "4": 37 | side_block = np.eye(J, dtype=np.int8) 38 | elif connectivity == "8": 39 | side_block = np.eye(J, dtype=np.int8) + diag_block 40 | else: 41 | raise Exception("Undefined connectivity") 42 | 43 | # First block row 44 | if I == 1: 45 | A[0:J, 0:J] = diag_block 46 | else: 47 | A[0:J, 0:J] = diag_block 48 | A[0:J, J:2 * J] = side_block 49 | 50 | # Last block row 51 | A[-1 * J:, -2 * J:-1 * J] = side_block 52 | A[-1 * J:, -1 * J:] = diag_block 53 | 54 | # Middle block rows 55 | for idx in range(1, I - 1): 56 | i_start = idx * J 57 | i_end = (idx + 1) * J 58 | A[i_start:i_end, (idx - 1) * J:(idx + 0) * J] = side_block 59 | A[i_start:i_end, (idx + 0) * J:(idx + 1) * J] = diag_block 60 | A[i_start:i_end, (idx + 1) * J:(idx + 2) * J] = side_block 61 | 62 | return A 63 | 64 | 65 | def get_neighbor_nodes(node_idx, A): 66 | '''Returns a list of node indices corresponing to neighbors of given node. 67 | 68 | Each node has one row. 69 | Connected nodes have nonzero column entries. 70 | ''' 71 | return np.nonzero(A[node_idx, :])[0].tolist() 72 | 73 | 74 | def node_coord2idx(i, j, J): 75 | '''Returns the node idx for a grid map coordinate (i,j) having width 'J'. 76 | 77 | Assumes grid map origo is in the top-left corner, and nodes are arranged as 78 | i --> row and j --> col 79 | 80 | j 81 | ________________ 82 | i | (0,0)_1 (0,1)_2 ... 83 | | (1,0)_? ... 84 | 85 | ''' 86 | return J * i + j 87 | 88 | 89 | def node_idx2coord(idx, J): 90 | '''Returns a coordinate tuple (i,j) for a node in a grid map of width 'J'. 91 | 92 | j 93 | ______________ 94 | i | 0 1 2 95 | | 3 4 5 96 | 97 | J = 3 98 | for node 4: 99 | i = floor( 4 / 3) = 1 100 | j = 4 % 3 = 1 101 | ''' 102 | i = int(np.floor(idx / J)) 103 | j = idx % J 104 | return (i, j) 105 | -------------------------------------------------------------------------------- /viz/viz_dataset.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import torch 4 | 5 | 6 | def viz_angs(angs: torch.Tensor, height=256, width=256) -> np.array: 7 | ''' 8 | Args: 9 | angs: Row matrix for element-wise angles (N, 3) 10 | [i, j, ang (rad)] 11 | 12 | Returns: 13 | (dir_x, dir_y): Dense x-y direction matrices 14 | ''' 15 | dir_x = np.zeros((height, width)) 16 | dir_y = np.zeros((height, width)) 17 | 18 | max_num_angs = angs.shape[0] 19 | for idx in range(max_num_angs): 20 | 21 | i, j, ang = angs[idx] 22 | i = int(i.item()) 23 | j = int(j.item()) 24 | ang = ang.item() 25 | 26 | # Negative entries [-1, -1, -1] means end of list 27 | if i < 0: 28 | break 29 | 30 | dx = np.cos(ang) 31 | dy = np.sin(ang) 32 | 33 | dir_x[i, j] = dx 34 | dir_y[i, j] = dy 35 | 36 | return dir_x, dir_y 37 | 38 | 39 | def viz_dataset_sample(x: torch.tensor, 40 | x_hat: torch.tensor, 41 | label: dict, 42 | file_path: str = None, 43 | viz_gt_lanes: bool = False): 44 | ''' 45 | Args: 46 | inputs: (2, H, W) 47 | labels: (3, H, W) 48 | ''' 49 | 50 | dir_x_full, dir_y_full = viz_angs(label['angs_full']) 51 | 52 | fig = plt.gcf() 53 | fig.set_size_inches(20, 15) 54 | 55 | cols = 4 56 | rows = 3 57 | 58 | if viz_gt_lanes: 59 | dir_x_gt, dir_y_gt = viz_angs(label['gt_angs']) 60 | rows += 1 61 | 62 | plt.subplot(rows, cols, 1) 63 | plt.imshow(x[0].numpy()) 64 | plt.subplot(rows, cols, 2) 65 | plt.imshow(x[1].numpy()) 66 | plt.subplot(rows, cols, 3) 67 | plt.imshow(x[2:5].numpy().transpose(1, 2, 0)) 68 | plt.subplot(rows, cols, 4) 69 | plt.imshow(x[0].numpy() + 2 * label['traj_present'].numpy(), 70 | vmin=0, 71 | vmax=2) 72 | 73 | plt.subplot(rows, cols, 5) 74 | plt.imshow(x_hat[0].numpy()) 75 | plt.subplot(rows, cols, 6) 76 | plt.imshow(x_hat[1].numpy()) 77 | plt.subplot(rows, cols, 7) 78 | plt.imshow(x_hat[2:5].numpy().transpose(1, 2, 0)) 79 | plt.subplot(rows, cols, 8) 80 | plt.imshow(x_hat[0].numpy() + 2 * label['traj_full'].numpy(), 81 | vmin=0, 82 | vmax=2) 83 | 84 | plt.subplot(rows, cols, 9) 85 | plt.imshow(label['dynamic'].numpy()) 86 | plt.subplot(rows, cols, 11) 87 | plt.imshow(dir_x_full, vmin=-1, vmax=1) 88 | plt.subplot(rows, cols, 12) 89 | plt.imshow(dir_y_full, vmin=-1, vmax=1) 90 | 91 | if viz_gt_lanes: 92 | plt.subplot(rows, cols, 14) 93 | plt.imshow(x_hat[0].numpy() + 2 * label['gt_lanes'].numpy(), 94 | vmin=0, 95 | vmax=2) 96 | plt.subplot(rows, cols, 15) 97 | plt.imshow(dir_x_gt, vmin=-1, vmax=1) 98 | plt.subplot(rows, cols, 16) 99 | plt.imshow(dir_y_gt, vmin=-1, vmax=1) 100 | 101 | plt.tight_layout() 102 | 103 | if file_path is not None: 104 | plt.savefig(file_path) 105 | plt.clf() 106 | plt.close() 107 | else: 108 | plt.show() 109 | -------------------------------------------------------------------------------- /models/unet_dsla.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | import torch.nn as nn 4 | 5 | from models.unet import UnetDecoder, UnetEncoder 6 | 7 | 8 | def get_dsla_output_layers(output_tensor, batch=True): 9 | '''Returns a list of correctly sliced DSLA tensors. 10 | Args: 11 | output_tensor: DSLA model output tensor (batch_n, 11, dim, dim) 12 | batch: Retains the batch dimension if 'True' 13 | Returns: 14 | list[0]: SLA (1 layer) 15 | list[1]: DA_mean (3 layers) 16 | list[2]: DA_var (3 layers) 17 | list[3]: DA_w (3 layers) 18 | list[4]: entry_pnt (1 layer) 19 | list[5]: exit_pnt (1 layer) 20 | ''' 21 | if batch: 22 | outputs_sla = output_tensor[:, 0:1] 23 | outputs_dir_mean = output_tensor[:, 1:4] 24 | outputs_dir_var = output_tensor[:, 4:7] 25 | outputs_dir_weight = output_tensor[:, 7:10] 26 | else: 27 | outputs_sla = output_tensor[0:1] 28 | outputs_dir_mean = output_tensor[1:4] 29 | outputs_dir_var = output_tensor[4:7] 30 | outputs_dir_weight = output_tensor[7:10] 31 | 32 | return (outputs_sla, outputs_dir_mean, outputs_dir_var, outputs_dir_weight) 33 | 34 | 35 | class UnetDSLA(nn.Module): 36 | 37 | def __init__(self, 38 | enc_str, 39 | sla_dec_str, 40 | da_dec_str, 41 | input_ch=2, 42 | out_feat_ch=512, 43 | num_angs=32): 44 | super(UnetDSLA, self).__init__() 45 | 46 | self.unet_encoder = UnetEncoder(enc_str, input_ch) 47 | 48 | bottleneck_ch = int(enc_str.split(',')[-1].split('x')[-1]) 49 | self.unet_decoder_sla = UnetDecoder(enc_str, sla_dec_str, 50 | bottleneck_ch, out_feat_ch) 51 | self.unet_decoder_da = UnetDecoder(enc_str, da_dec_str, bottleneck_ch, 52 | out_feat_ch) 53 | 54 | # Output head 1 : Soft lane affordance 55 | self.sla_head = [] 56 | self.sla_head.append(nn.Conv2d(out_feat_ch, 1, 1, stride=1, padding=0)) 57 | self.sla_head.append(nn.Sigmoid()) 58 | self.sla_head = nn.Sequential(*self.sla_head) 59 | 60 | # Output head 2 : Directional affordance 61 | self.da_head = [] 62 | self.da_head.append( 63 | nn.Conv2d(out_feat_ch, num_angs, 1, stride=1, padding=0)) 64 | self.da_head.append(nn.Softmax(dim=1)) 65 | self.da_head = nn.Sequential(*self.da_head) 66 | 67 | def forward(self, x): 68 | 69 | x_bottleneck, enc_outs = self.unet_encoder(x) 70 | h_sla = self.unet_decoder_sla(x_bottleneck, enc_outs) 71 | h_da = self.unet_decoder_da(x_bottleneck, enc_outs) 72 | 73 | out_sla = self.sla_head(h_sla) 74 | out_da = self.da_head(h_da) 75 | 76 | out = torch.cat((out_sla, out_da), dim=1) 77 | 78 | return out 79 | 80 | 81 | if __name__ == '__main__': 82 | 83 | enc_str = '2x32,2x32,2x64,2x64,2x128,2x128,2x256,2x256' 84 | dec_str = '1x128,1x128,1x64,1x64,1x32,1x32,1x16,1x16' 85 | input_ch = 5 86 | out_feat_ch = 32 87 | num_angs = 32 88 | sla_head_layers = 3 89 | da_head_layers = 3 90 | 91 | model = UnetDSLA(enc_str, dec_str, input_ch, out_feat_ch, num_angs, 92 | sla_head_layers, da_head_layers) 93 | 94 | input_size = 256 95 | x = torch.rand((32, input_ch, input_size, input_size)) 96 | # (B, C, H, W) 97 | print('x:', x.shape) 98 | 99 | y = model(x) 100 | # (B, C, H, W) 101 | print('y:', y.shape) 102 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | 155 | # PyCharm 156 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 157 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 158 | # and can be added to the global gitignore or merged into this file. For a more nuclear 159 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 160 | #.idea/ 161 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Learning to Predict Navigational Patterns from Partial Observations 2 | 3 | Code accompanying the paper "Learning to Predict Navigational Patterns from Partial Observations" (RA-L 2023). 4 | 5 | The paper presents a self-supervised method to learn navigational patterns in structured environments from partial observations of other agents. The navigational patterns are represented as a directional soft lane probability (DSLP) field. We also present a method for inferring the most likely discrete path or lane graph based on the predicted DSLP field. 6 | 7 | Paper link: [Predictive World Models from Real-World Partial Observations](https://arxiv.org/abs/2304.13242) 8 | 9 | Video presentation link: TODO 10 | 11 | Data directory link: [Google Drive directory](https://drive.google.com/drive/folders/1ylLDDdaxGEOZOJ9b6YXumRtXbOGTncVi?usp=sharing) 12 | 13 | ![Overview image](https://github.com/robin-karlsson0/dslp/assets/34254153/e9daf5be-05fa-4736-8d3e-a4cd9d6a5d1b) 14 | 15 | # Installation 16 | 17 | The code is tested with Python 3.9 on Ubuntu 22.04. 18 | 19 | Download all submodules 20 | ``` 21 | git submodule update --init --recursive 22 | ``` 23 | 24 | The submodules are used for the following tasks 25 | 26 | 1. predictive-world-models: Predictive world model repository 27 | 2. vdvae: Code for implementing the predictive world model. Fork of the original VDVAE repository modified to a dual encoder posterior matching HVAE model. 28 | 29 | ## Install dependencies 30 | 31 | Follow README instructions in `predictive-world-models/` 32 | 33 | Downgrade Pytorch Lightning --> 1.9.0 (for CLI implementation to work) 34 | ``` 35 | pip uninstall pytorch-lightning 36 | pip install pytorch-lightning==1.9.0 37 | ``` 38 | 39 | 40 | ## Evaluation data 41 | 42 | Download and extract the following compressed directories into the local `data/` directory. 43 | 44 | [Data directory](https://drive.google.com/drive/folders/1ylLDDdaxGEOZOJ9b6YXumRtXbOGTncVi?usp=sharing) 45 | 46 | ``` 47 | dslp/ 48 | └───data/ 49 | | bev_nuscenes_256px_v01_boston_seaport_unaug_gt_full_eval_preproc.tar.gz 50 | | bev_nuscenes_256px_v01_boston_seaport_unaug_gt_eval_preproc.tar.gz 51 | ``` 52 | 53 | Evaluate on partial observations: [bev_nuscenes_256px_v01_boston_seaport_unaug_gt_eval_preproc.tar.gz](https://drive.google.com/file/d/16M4y5Hu9-c5jXMi9anViCOfzudSHpIgE/view?usp=drive_link) 54 | 55 | Evaluate on full observations: [bev_nuscenes_256px_v01_boston_seaport_unaug_gt_full_eval_preproc.tar.gz](https://drive.google.com/file/d/1g_wysAgmMryLTq4svXg8hzmg-BlcXs0r/view?usp=sharing) 56 | 57 | 58 | ## Training data 59 | 60 | Download and extract the following compressed directories into the local `data/` directory. 61 | 62 | _Note: The training datasets are 33 and 35 GB in size._ 63 | 64 | [Data directory](https://drive.google.com/drive/folders/1ylLDDdaxGEOZOJ9b6YXumRtXbOGTncVi?usp=sharing) 65 | 66 | ``` 67 | dslp/ 68 | └───data/ 69 | | bev_nuscenes_256px_v01_boston_seaport_gt_full_preproc_train 70 | | bev_nuscenes_256px_v01_boston_seaport_gt_preproc_train 71 | | bev_nuscenes_256px_viz.tar.gz 72 | ``` 73 | 74 | Train on partial observations: 75 | [bev_nuscenes_256px_v01_boston_seaport_gt_preproc_train.tar.gz](https://drive.google.com/file/d/1p4zpkLiSJxDACB9EQKboGQr89dBIBA-g/view?usp=drive_link) 76 | 77 | Train on full observations: 78 | [bev_nuscenes_256px_v01_boston_seaport_gt_full_preproc_train.tar.gz]() 79 | 80 | Static set of visualization samples used to monitor progress (required for running the code!): 81 | [bev_nuscenes_256px_viz.tar.gz](https://drive.google.com/file/d/1JMIQ48yr5tSGxgYRCMGXsEhl8N05-ieJ/view?usp=drive_link) 82 | 83 | 84 | ## Checkpoint files 85 | 86 | Download checkpoint files into the local `checkpoints/` directory. 87 | 88 | [Data directory](https://drive.google.com/drive/folders/1ylLDDdaxGEOZOJ9b6YXumRtXbOGTncVi?usp=sharing) 89 | 90 | ``` 91 | dslp/ 92 | └───checkpoints/ 93 | | ... 94 | ``` 95 | 96 | | Experiment | NLL | IoU | 97 | |---------------------------|------------|--| 98 | | [exp_04_dslp_alpha_ib.ckpt](https://drive.google.com/file/d/1oy1RlmDJojKdJg8-LDUWbpUeaOJWoQqk/view?usp=drive_link) | **12.325** | 0.442 | 99 | | [exp_08_dslp_region_1.ckpt](https://drive.google.com/file/d/1Z00VNKtLvj1-GBQa8peNAD8WmzKEl1Th/view?usp=drive_link) | 13.174 | 0.423 | 100 | | [exp_09_dslp_region_1_2.ckpt](https://drive.google.com/file/d/1pry0prA-QKcOBbHtoA1p1O6HYOreWQWM/view?usp=drive_link) | 12.557 | **0.444** | 101 | 102 | # Evaluation 103 | 104 | Run the evaluation script to recompute the main experiment results. The script assumes the datasets and checkpoints are set up as instructed. 105 | 106 | ``` 107 | sh run_eval_exp_04_dslp_alpha_ib.sh 108 | ``` 109 | 110 | # Training 111 | 112 | Run the training script to recreate the main experiment DSLP model. The script assumes the datasets and checkpoints are set up as instructed. 113 | 114 | ``` 115 | sh run_train_exp_04_dslp_alpha_ib.sh 116 | ``` 117 | 118 | # Experimental results 119 | 120 | Summary of results and baselines 121 | 122 | | Model | NLL | IoU | 123 | |---------------------------|------------|--| 124 | | [STSU](https://arxiv.org/abs/2110.01997) (supervised) | - | 0.389 | 125 | | [LaneGraphNet](https://arxiv.org/abs/2105.00195) (supervised) | - | 0.420 | 126 | | [DSLA]() | 15.095 | 0.427 | 127 | | DSLP const alpha | 12.663 | 0.418 | 128 | | DSLP mean alpha_ib | 12.482 | 0.410 | 129 | | *DSLP alpha_ib | 12.325 | 0.442 | 130 | | DSLP full obs. | **12.205** | **0.454** | 131 | 132 | _'*' Our main result for partially observable worlds_ 133 | 134 | All experiment quantitative result logs and output visualizations are uploaded. 135 | 136 | [Data directory](https://drive.google.com/drive/folders/1ylLDDdaxGEOZOJ9b6YXumRtXbOGTncVi?usp=sharing) 137 | 138 | ``` 139 | dslp/ 140 | └───results/ 141 | └───exp_01_dsla/ 142 | | | eval.txt <--Evaluation log 143 | | | results.txt <-- Evaluation summary 144 | | | viz_000.png <-- Output visualizations 145 | | | ... 146 | | 147 | └───exp_02_dslp_const_alpha/ 148 | └───exp_03_dslp_mean_alpha_ib/ 149 | └───exp_04_dslp_alpha_ib/ <-- Main result 150 | └───exp_05_dslp_full_obs/ 151 | └───exp_06_dslp_no_world_model/ 152 | └───exp_07_dslp_no_aug/ 153 | └───exp_08_dslp_region_1/ 154 | └───exp_09_dslp_region_1_2/ 155 | ``` -------------------------------------------------------------------------------- /models/unet.py: -------------------------------------------------------------------------------- 1 | import pytorch_lightning as pl 2 | import torch 3 | from torch import nn 4 | 5 | 6 | class UnetEncoder(pl.LightningModule): 7 | 8 | def __init__( 9 | self, 10 | enc_str: str, 11 | input_ch=1, 12 | ): 13 | ''' 14 | Args: 15 | enc_str: Sequence of (#layers, #filters). Last pair is bottleneck. 16 | Ex: '2x64,2x128,2x256' 17 | ''' 18 | super().__init__() 19 | 20 | enc_blocks = self.parse_blocks_string(enc_str) 21 | 22 | ############## 23 | # Encoders 24 | ############## 25 | self.enc_blocks = nn.ModuleList() 26 | self.downsamplers = nn.ModuleList() 27 | ch_prev = input_ch 28 | 29 | for num_layers, num_filters in enc_blocks[:-1]: 30 | 31 | enc_block = [] 32 | for layer_idx in range(num_layers): 33 | enc_block.append( 34 | nn.Conv2d(ch_prev, 35 | num_filters, 36 | kernel_size=3, 37 | stride=1, 38 | padding=1, 39 | bias=False)) 40 | enc_block.append(nn.BatchNorm2d(num_filters)), 41 | enc_block.append(nn.LeakyReLU()) 42 | ch_prev = num_filters 43 | self.enc_blocks.append(nn.Sequential(*enc_block)) 44 | 45 | downsampler = nn.Sequential( 46 | nn.Conv2d(ch_prev, 47 | ch_prev, 48 | kernel_size=3, 49 | stride=2, 50 | padding=1, 51 | bias=False), 52 | nn.BatchNorm2d(ch_prev), 53 | nn.LeakyReLU(), 54 | ) 55 | self.downsamplers.append(downsampler) 56 | 57 | ################ 58 | # Bottleneck 59 | ################ 60 | num_layers, num_filters = enc_blocks[-1] 61 | 62 | bottleneck = [] 63 | for layer_idx in range(num_layers): 64 | bottleneck.append( 65 | nn.Conv2d(ch_prev, 66 | num_filters, 67 | kernel_size=3, 68 | stride=1, 69 | padding=1, 70 | bias=False)) 71 | bottleneck.append(nn.BatchNorm2d(num_filters)), 72 | bottleneck.append(nn.LeakyReLU()) 73 | ch_prev = num_filters 74 | self.bottleneck = nn.Sequential(*bottleneck) 75 | 76 | def forward(self, x): 77 | # Encoder 78 | encoder_outs = {} 79 | num_blocks = len(self.enc_blocks) 80 | for idx in range(num_blocks): 81 | encoder_out = self.enc_blocks[idx](x) 82 | x = self.downsamplers[idx](encoder_out) 83 | 84 | res = encoder_out.shape[-1] 85 | encoder_outs[res] = encoder_out 86 | 87 | # Bottleneck 88 | x = self.bottleneck(x) 89 | 90 | return x, encoder_outs 91 | 92 | @staticmethod 93 | def parse_blocks_string(enc_str): 94 | ''' 95 | Args: 96 | enc_str: Sequence of (#layers, #filters) 97 | Ex: '2x64,2x128,2x256' 98 | ''' 99 | enc_blocks = [] 100 | s = enc_str.split(',') 101 | for ss in s: 102 | num_layers, num_filters = ss.split('x') 103 | enc_bloc = (int(num_layers), int(num_filters)) 104 | enc_blocks.append(enc_bloc) 105 | 106 | return enc_blocks 107 | 108 | 109 | class UnetDecoder(pl.LightningModule): 110 | 111 | def __init__( 112 | self, 113 | enc_str: str, 114 | dec_str: str, 115 | bottleneck_ch, 116 | output_ch=1, 117 | output_activation='sigmoid', 118 | ): 119 | ''' 120 | NOTE: Number of filters must be in exponentially increasing order 121 | Ex: 2x16,2x32,2x64,2x128,2x256,2x512,2x1024,2x2048 122 | 123 | Args: 124 | dec_str: Sequence of (#layers, #filters). 125 | Ex: '2x256,2x128,2x64' 126 | ''' 127 | super().__init__() 128 | 129 | enc_blocks = self.parse_blocks_string(enc_str) 130 | dec_blocks = self.parse_blocks_string(dec_str) 131 | 132 | self.upsample = nn.Upsample(scale_factor=2, 133 | mode="bilinear", 134 | align_corners=False) 135 | 136 | self.bottleneck_ch = bottleneck_ch 137 | 138 | ############## 139 | # Decoders 140 | ############## 141 | self.dec_blocks = nn.ModuleList() 142 | ch_prev = self.bottleneck_ch 143 | 144 | num_blocks = len(dec_blocks) 145 | 146 | for block_idx in range(num_blocks - 1): 147 | 148 | num_layers, num_filters = dec_blocks[block_idx] 149 | _, enc_filters = enc_blocks[num_blocks - block_idx - 1] 150 | 151 | dec_block = [] 152 | for layer_idx in range(num_layers): 153 | if layer_idx == 0 and block_idx != 0: 154 | ch_prev += enc_filters 155 | dec_block.append( 156 | nn.Conv2d(ch_prev, 157 | num_filters, 158 | kernel_size=3, 159 | stride=1, 160 | padding=1, 161 | bias=False)) 162 | dec_block.append(nn.BatchNorm2d(num_filters)), 163 | dec_block.append(nn.LeakyReLU()) 164 | ch_prev = num_filters 165 | self.dec_blocks.append(nn.Sequential(*dec_block)) 166 | 167 | # Special last block 168 | block_idx = num_blocks - 1 169 | num_layers, num_filters = dec_blocks[block_idx] 170 | _, enc_filters = enc_blocks[num_blocks - block_idx - 1] 171 | dec_block = [] 172 | for layer_idx in range(num_layers): 173 | if layer_idx == 0 and block_idx != 0: 174 | ch_prev += enc_filters 175 | dec_block.append( 176 | nn.Conv2d(ch_prev, 177 | num_filters, 178 | kernel_size=3, 179 | stride=1, 180 | padding=1, 181 | bias=False)) 182 | dec_block.append(nn.BatchNorm2d(num_filters)), 183 | dec_block.append(nn.LeakyReLU()) 184 | ch_prev = num_filters 185 | dec_block.append( 186 | nn.Conv2d(ch_prev, output_ch, kernel_size=3, stride=1, padding=1)) 187 | if output_activation == 'sigmoid': 188 | dec_block.append(nn.Sigmoid()) 189 | elif output_activation == 'leaky_relu': 190 | dec_block.append(nn.LeakyReLU()) 191 | elif output_activation == 'relu': 192 | dec_block.append(nn.ReLU()) 193 | else: 194 | # Output logits 195 | pass 196 | self.dec_blocks.append(nn.Sequential(*dec_block)) 197 | 198 | def forward(self, x, enc_outs): 199 | ''' 200 | TODO: Add in optional 'enc_outs: dict' 201 | ''' 202 | x = self.dec_blocks[0](x) 203 | 204 | num_blocks = len(self.dec_blocks) 205 | for idx in range(1, num_blocks): 206 | x = self.upsample(x) 207 | 208 | res = x.shape[-1] 209 | enc_out = enc_outs[res] 210 | x = torch.cat((x, enc_out), dim=1) 211 | x = self.dec_blocks[idx](x) 212 | 213 | return x 214 | 215 | @staticmethod 216 | def parse_blocks_string(dec_str): 217 | ''' 218 | Args: 219 | enc_str: Sequence of (#layers, #filters) 220 | Ex: '2x64,2x128,2x256' 221 | ''' 222 | dec_blocks = [] 223 | s = dec_str.split(',') 224 | for ss in s: 225 | num_layers, num_filters = ss.split('x') 226 | enc_bloc = (int(num_layers), int(num_filters)) 227 | dec_blocks.append(enc_bloc) 228 | 229 | return dec_blocks 230 | 231 | 232 | if __name__ == '__main__': 233 | 234 | input_size = 256 235 | base_ch = 2 236 | input_ch = 5 237 | output_ch = 256 238 | bottleneck_ch = 256 239 | 240 | ############# 241 | # Encoder 242 | ############# 243 | # enc_str = '2x64,2x128,2x256,2x512,2x1024' 244 | # enc_str = '2x16,2x32,2x64,2x128,2x256,2x512,2x1024,2x2048' 245 | enc_str = '2x32,2x32,2x64,2x64,2x128,2x128,2x256,2x256' 246 | # 256 128 64 32 16 8 4 2 247 | unet_encoder = UnetEncoder(enc_str, input_ch) 248 | 249 | x = torch.rand((32, input_ch, input_size, input_size)) 250 | x_bottleneck, enc_outs = unet_encoder(x) 251 | 252 | print(f'{x.shape} --> {x_bottleneck.shape}') 253 | for res in enc_outs.keys(): 254 | print(f'enc_out[{res}] : {enc_outs[res].shape}') 255 | 256 | ############# 257 | # Decoder 258 | ############# 259 | # dec_str = '1x1024,2x512,2x256,2x128,2x64' 260 | # dec_str = '2x2048,2x1024,2x512,2x256,2x128,2x64,2x32,2x16' 261 | dec_str = '2x256,2x256,2x128,2x128,2x64,2x64,2x32,2x32' 262 | # 2 4 8 16 32 64 128 256 263 | unet_decoder = UnetDecoder(enc_str, dec_str, bottleneck_ch, output_ch) 264 | 265 | y = unet_decoder(x_bottleneck, enc_outs) 266 | 267 | print(f'{x_bottleneck.shape} --> {y.shape}') -------------------------------------------------------------------------------- /graph_inference/dsla_weight_matrix.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | 5 | from graph_inference.grid_map import (get_neighbor_nodes, node_coord2idx, 6 | node_idx2coord) 7 | 8 | 9 | def smoothen_sla_map(sla_map, sla_threshold=0.1, kernel_size=8, power=8): 10 | '''Smooth SLA grid map to penalize paths close to border. 11 | ''' 12 | # sla_map[sla_map >= sla_threshold] = 1. 13 | # sla_map[sla_map < 1.] = 0. 14 | 15 | # kernel = (kernel_size, kernel_size) 16 | # sla_map_ = cv2.blur(sla_map, kernel) 17 | # sla_map = sla_map_ * sla_map 18 | 19 | # sla_map = sla_map**power 20 | 21 | return sla_map 22 | 23 | 24 | def sigmoid(z): 25 | return 1 / (1 + np.exp(-z)) 26 | 27 | 28 | def csch(x): 29 | return 2. / (np.e**x - np.e**(-x)) 30 | 31 | 32 | def sigmoid_sla_map(sla_map, weight, kernel_size=3, num_blurs=3): 33 | sla_map = sigmoid(weight * sla_map - 0.5 * weight) 34 | 35 | kernel = (kernel_size, kernel_size) 36 | for _ in range(num_blurs): 37 | sla_map_ = cv2.blur(sla_map, kernel) 38 | sla_map = sla_map_ * sla_map 39 | # sla_map = sigmoid(weight * sla_map - 0.5 * weight) 40 | 41 | return sla_map 42 | 43 | 44 | def unit_vector(vector): 45 | '''Returns the unit vector of the vector. 46 | ''' 47 | return vector / np.linalg.norm(vector) 48 | 49 | 50 | def angle_between(v1, v2): 51 | '''Returns the angle in radians between vectors 'v1' and 'v2' 52 | ''' 53 | v1_u = unit_vector(v1) 54 | v2_u = unit_vector(v2) 55 | return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0)) 56 | 57 | 58 | def neigh_direction(pnt, neigh_pnt): 59 | '''Returns angle [rad] between two points clockwise from x-axis. 60 | 61 | Args: 62 | pnt: Coordinates of node (i,j) 63 | neigh_pnt: Coordinates of neighbor node (i,j) 64 | ''' 65 | vec = [neigh_pnt[0] - pnt[0], neigh_pnt[1] - pnt[1]] 66 | 67 | # Image --> Cartesian coordinates 68 | vec[1] = -vec[1] 69 | 70 | neigh_angle = angle_between(vec, (1, 0)) 71 | 72 | # When vector pointing downwards 73 | if vec[1] < 0.: 74 | neigh_angle = 2. * np.pi - neigh_angle 75 | 76 | return neigh_angle 77 | 78 | 79 | def angle_diff(ang1, ang2): 80 | '''Difference in radians for two angles [rad]. 81 | Ref: https://stackoverflow.com/questions/1878907/the-smallest-difference-between-2-angles 82 | ''' 83 | a = ang1 - ang2 84 | a = (a + np.pi) % (2. * np.pi) - np.pi 85 | return a 86 | 87 | 88 | def deg2rad(deg): 89 | return deg * np.pi / 180. 90 | 91 | 92 | def ang2idx(ang): 93 | ''' 94 | Returns the idx corresponding to one of the angles {0, 45, 90, 125, 180, 95 | 225, 270, 315} representing directions {L, TL, T, TR, R, BR, B, BL}. 96 | Args: 97 | ang: Radians 98 | ''' 99 | for idx in range(8): 100 | if np.isclose(ang, deg2rad(idx * 45)): 101 | return idx 102 | raise Exception(f'Given angle {ang} ({ang*180/np.pi}) not in the set') 103 | 104 | 105 | def compute_da_contribution_map(da_num): 106 | ''' 107 | phi: Directional affordance directions 108 | theta: Predefined directional intervals 109 | 110 | Args: 111 | da_num: Number of discretized directions (i.e. 32) 112 | 113 | Returns: 114 | c_mat: Contribution mapping da_idx --> dir_idx 115 | 116 | c_mat[da_i, dir_j] --> Contrib. of p_DA(phi=i) --> p_Dir(theta=j) 117 | 118 | dir_1 dir_2 ... 119 | ---------------- 120 | da_1 | 1 0 <-- Sums to 1 (how da_1 is distributed among 121 | da_2 | 0 1 dir_1, ..., dir_N) 122 | ... | 123 | ''' 124 | delta_phi = 2 * np.pi / da_num 125 | 126 | dir_num = 8 127 | 128 | # NOTE: Count right-most region 'R' as two regions (+45, -45) 129 | # Do reduction later 130 | c_mat = np.zeros((da_num, dir_num + 1)) 131 | 132 | thetas = [0, 22.5, 67.5, 112.5, 157.5, 202.5, 247.5, 292.5, 337.5, 360] 133 | thetas = [theta * np.pi / 180. for theta in thetas] 134 | for phi_idx in range(da_num): 135 | phi_0 = phi_idx * delta_phi 136 | phi_1 = (phi_idx + 1) * delta_phi 137 | for theta_idx in range(dir_num + 1): 138 | theta_0 = thetas[theta_idx] 139 | theta_1 = thetas[theta_idx + 1] 140 | 141 | # DA region after 142 | if theta_1 < phi_0: 143 | continue 144 | # DA region before 145 | if phi_1 < theta_0: 146 | break 147 | # Intersection 3: DA entering 148 | if phi_0 <= theta_0 and phi_1 <= theta_1: 149 | intersection = phi_1 - theta_0 150 | # Intersection 4: DA leaving 151 | elif theta_0 < phi_0 and theta_1 <= phi_1: 152 | intersection = theta_1 - phi_0 153 | # Intersection 5: DA within 154 | elif theta_0 <= phi_0 and phi_1 <= theta_1: 155 | intersection = phi_1 - phi_0 156 | # Intersection 6: DA enclose 157 | elif phi_0 <= theta_0 and theta_1 <= phi_1: 158 | intersection = theta_1 - phi_0 159 | else: 160 | raise Exception('Unspecified condition') 161 | 162 | # Contribution ratio from p(phi) --> p(theta) 163 | c_ratio = intersection / delta_phi 164 | 165 | c_mat[phi_idx, theta_idx] = c_ratio 166 | 167 | # Sum and reduce the first and last interval 168 | # (corresponding to the same -22.5 --> 22.5 interval) 169 | c_mat[:, 0] += c_mat[:, -1] 170 | c_mat = c_mat[:, :-1] 171 | 172 | return c_mat 173 | 174 | 175 | def dsla_weighted_adj_mat( 176 | A, 177 | sla_map, 178 | da_map, 179 | sla_threshold=0.1, 180 | da_threshold=1., 181 | eps=0, #1e-12, 182 | smoothing_kernel_size=8, 183 | smoothing_power=8): 184 | ''' 185 | ''' 186 | # For penalizing paths close to border 187 | # sla_map = smoothen_sla_map(sla_map, 188 | # kernel_size=smoothing_kernel_size, 189 | # power=smoothing_power) 190 | mask = sla_map == 0 191 | sla_map = sigmoid_sla_map(sla_map, 6, num_blurs=3) 192 | sla_map[mask] = 0 193 | 194 | # Col count 195 | I, J = sla_map.shape 196 | 197 | # All nodes unreachable by default 198 | weighted_A = np.ones(A.shape) * np.inf 199 | 200 | # DA --> Direction contribution mapping 201 | # c_map[da_i, dir_j] --> Contribution of prob da_i to prob dir_j 202 | da_num = da_map.shape[0] 203 | c_map = compute_da_contribution_map(da_num) 204 | 205 | # Range associated with 'directionless' space (DA spread out) 206 | dir_prob_thresh = 0.1 * 1 / 8 # Uniform probability 207 | 208 | # Compute directional adjacency weight node-by-node 209 | # NOTE: Coordinates (i,j) == (row, col) in image coordinates 210 | # (0,0) is top-left corner 211 | # (127,0) is bottom-left corner 212 | # TODO: Get nonzero indices from SLA map 213 | for i in range(I): 214 | for j in range(J): 215 | 216 | # Skip nodes without SLA 217 | # if sla_map[i, j] < eps: 218 | if sla_map[j, i] <= eps: 219 | continue 220 | 221 | # Transform p(DA) --> p(Dir): p_dir[p(dir=0), ... p(dir=N)] 222 | # p_dir = np.zeros((8)) 223 | # for da_idx in range(da_num): 224 | # p_dir += c_map[da_idx] * da_map[ 225 | # da_idx, j, i] # TODO Confirm direction (i, j) 226 | 227 | # p_dir_1 = [c_dir_1, c_dir_2, ... , c_dir_32] x [p_da_1, p_da_2, ... , p_da_32].T 228 | p_dir = np.matmul(c_map.T, da_map[:, j, i]) 229 | 230 | # Apply convolution to allow diagonal transitions in straight 231 | # directional fields 232 | kernel = np.array([0.125, 0.75, 0.125]) 233 | p_dir_padding = np.pad(p_dir, 1, 'wrap') 234 | p_dir_padding = np.convolve(p_dir_padding, kernel, mode='same') 235 | p_dir = p_dir_padding[1:-1] 236 | 237 | # Node index for current node and surrounding neighbors 238 | node_idx = node_coord2idx(i, j, J) 239 | neigh_idxs = get_neighbor_nodes(node_idx, A) 240 | 241 | # Compute directional adjacency neighbor-by-neighbor 242 | for neigh_idx in neigh_idxs: 243 | 244 | neigh_i, neigh_j = node_idx2coord(neigh_idx, J) 245 | 246 | # sla = sla_map[neigh_i, neigh_j] 247 | sla = sla_map[neigh_j, neigh_i] 248 | 249 | # Non-SLA nodes unreachable 250 | # if sla_map[neigh_i, neigh_j] <= eps: 251 | if sla_map[neigh_j, neigh_i] <= eps: 252 | continue 253 | 254 | # Directional angle (convert to Cartesian coordinates) 255 | # ang = neigh_direction((j, i), (neigh_j, neigh_i)) 256 | ang = neigh_direction((i, j), (neigh_i, neigh_j)) 257 | dir_idx = ang2idx(ang) 258 | 259 | p_dir_neigh = p_dir[dir_idx] 260 | 261 | # If directional angle is within limits ==> Reachable node 262 | if p_dir_neigh > dir_prob_thresh: 263 | 264 | # NEW 265 | dx = neigh_i - i 266 | dy = neigh_j - j 267 | dist = np.sqrt((dx)**2 + (dy)**2) 268 | 269 | # SLA penalty = - log( SLA ) 270 | # i.e. penalty incresing as SLA decreases 271 | # cost = -1e6 * np.log(sla**256 + 1e-320) + 1e-3 * dist # 1. 272 | cost = csch(sla) + 1e-6 * dist 273 | 274 | weighted_A[node_idx, neigh_idx] = cost 275 | 276 | return weighted_A 277 | 278 | 279 | if __name__ == '__main__': 280 | 281 | c_mat = compute_da_contribution_map(32) 282 | 283 | np.set_printoptions(precision=2, suppress=True) 284 | print(c_mat) 285 | print() 286 | -------------------------------------------------------------------------------- /datamodule_preproc.py: -------------------------------------------------------------------------------- 1 | import glob 2 | import gzip 3 | import os 4 | import pickle 5 | 6 | import cv2 7 | import numpy as np 8 | import pytorch_lightning as pl 9 | import scipy.special 10 | import torch 11 | from torch.utils.data import DataLoader 12 | from torchvision import transforms 13 | 14 | 15 | class PreprocBEVDataset(): 16 | ''' 17 | Intensity: Value interval (0,1) 18 | ''' 19 | 20 | def __init__( 21 | self, 22 | abs_root_path, 23 | do_rotation=False, 24 | do_aug=False, 25 | get_gt_labels=False, 26 | ): 27 | self.abs_root_path = abs_root_path 28 | self.sample_paths = glob.glob( 29 | os.path.join(self.abs_root_path, '*', '*.pkl.gz')) 30 | 31 | self.sample_paths = [ 32 | os.path.relpath(path, self.abs_root_path) 33 | for path in self.sample_paths 34 | ] 35 | self.sample_paths.sort() 36 | 37 | self.do_rotation = do_rotation 38 | self.do_aug = do_aug 39 | self.get_gt_labels = get_gt_labels 40 | 41 | self.transf_rgb = torch.nn.Sequential( 42 | transforms.ColorJitter(brightness=.5, contrast=.5, saturation=.5), 43 | transforms.GaussianBlur(3, sigma=(0.001, 2.0)), 44 | ) 45 | 46 | def __len__(self): 47 | return len(self.sample_paths) 48 | 49 | def __getitem__(self, idx): 50 | 51 | sample_path = self.sample_paths[idx] 52 | sample_path = os.path.join(self.abs_root_path, sample_path) 53 | input, label = self.read_compressed_pickle(sample_path) 54 | 55 | # Add auxhilary labels 56 | drivable = input[0:1].clone() 57 | label['drivable'] = drivable 58 | 59 | # Add label channel dims 60 | # label['traj_present'] = label['traj_present'].unsqueeze(0) 61 | # label['traj_present'] = label['traj_present'].float() 62 | 63 | traj = label['traj_full'].numpy().astype(float) 64 | kernel = np.ones((3, 3), np.uint8) 65 | traj = cv2.dilate(traj, kernel) 66 | label['traj_full'] = torch.tensor(traj, dtype=torch.float32) 67 | label['traj_full'] = label['traj_full'].unsqueeze(0) 68 | 69 | # Transform list of angles to multimodal distribution tensor 70 | # NOTE: Unobserved elements have uniform distribution 71 | num_discr = 36 72 | m_max = 88 73 | mm_ang_full_tensor = self.gen_multimodal_vonmises_distrs( 74 | label['angs_full'], num_discr, m_max) 75 | mm_ang_full_tensor = np.transpose(mm_ang_full_tensor, (2, 0, 1)) 76 | label['mm_ang_full_tensor'] = torch.tensor(mm_ang_full_tensor) 77 | 78 | if self.get_gt_labels: 79 | 80 | gt_lanes = label['gt_lanes'].numpy().astype(float) 81 | gt_lanes = cv2.dilate(gt_lanes, kernel) 82 | label['gt_lanes'] = torch.tensor(gt_lanes, dtype=torch.float32) 83 | label['gt_lanes'] = label['gt_lanes'].unsqueeze(0) 84 | 85 | mm_gt_angs_tensor = self.gen_multimodal_vonmises_distrs( 86 | label['gt_angs'], num_discr, m_max) 87 | mm_gt_angs_tensor = np.transpose(mm_gt_angs_tensor, (2, 0, 1)) 88 | label['mm_gt_angs_tensor'] = torch.tensor(mm_gt_angs_tensor) 89 | 90 | # Random rotation 91 | # # TODO Need fix for new multimodal angle repr. 92 | # if self.do_rotation: 93 | # k = random.randrange(0, 4) 94 | # tensor_rot = torch.rot90(tensor, k, (-2, -1)) 95 | # tensor_rot_ = tensor_rot.clone() 96 | # if k == 1: 97 | # tensor_rot[-2] = tensor_rot_[-1] * (-1) 98 | # tensor_rot[-1] = tensor_rot_[-2] 99 | # elif k == 2: 100 | # tensor_rot[-2] = tensor_rot_[-2] * (-1) 101 | # tensor_rot[-1] = tensor_rot_[-1] * (-1) 102 | # elif k == 3: 103 | # tensor_rot[-2] = tensor_rot_[-1] 104 | # tensor_rot[-1] = tensor_rot_[-2] * (-1) 105 | # tensor = tensor_rot 106 | 107 | # Augmentation for intensity and RGB map (to limit overfitting) 108 | if self.do_aug: 109 | # Intensity 110 | # Randomly samples a set of augmentations 111 | input_int = input[1].clone().numpy() 112 | input_int = self.rand_aug_int(input_int) 113 | input[1] = torch.tensor(input_int) 114 | # RGB 115 | input_rgb = (255 * input[2:5]).type(torch.uint8) 116 | input_rgb = self.transf_rgb(input_rgb) 117 | input[2:5] = input_rgb.float() / 255 118 | 119 | # Transform input value range (0, 1) --> (-1, 1) 120 | input = (2 * input) - 1. 121 | 122 | # Remove unrelated entries 123 | rm_keys = ['map', 'scene_idx', 'ego_global_x', 'ego_global_y'] 124 | for rm_key in rm_keys: 125 | if rm_key in label.keys(): 126 | del label[rm_key] 127 | 128 | # Ensure that all tensors are of the same type 129 | for key in label.keys(): 130 | label[key] = label[key].type(torch.float) 131 | 132 | return input, label 133 | 134 | def rand_aug_int(self, 135 | x, 136 | num_augs_min=1, 137 | num_augs_max=4, 138 | p_cat_distr=[0.3, 0.15, 0.15, 0.4]): 139 | num_augs = np.random.randint(num_augs_min, num_augs_max) 140 | augs = np.random.choice(np.arange(4), size=num_augs, p=p_cat_distr) 141 | for aug_idx in augs: 142 | if aug_idx == 0: 143 | x = self.sharpen(x) 144 | elif aug_idx == 1: 145 | x = self.gaussian_blur(x) 146 | x = self.sharpen(x) 147 | elif aug_idx == 2: 148 | x = self.box_blur(x) 149 | x = self.sharpen(x) 150 | elif aug_idx == 3: 151 | x = self.scale(x) 152 | else: 153 | raise Exception('Undefined augmentation') 154 | x = self.normalize(x) 155 | 156 | return x 157 | 158 | def gen_multimodal_vonmises_distrs(self, 159 | angs, 160 | num_discr, 161 | vonmises_m, 162 | height=256, 163 | width=256): 164 | ''' 165 | Args: 166 | angs: (N,3) 167 | num_discr: Number of elements discretizing (0, 2*pi) 168 | vonmises_m: Von Mises distribution concentration parameter. 169 | 170 | Returns: 171 | Tensor with multimodal von Mises distributions for labeled elements 172 | w. dim(num_discr, H, W) 173 | ''' 174 | ang_range = np.linspace(0, 2 * np.pi, num_discr) 175 | vonmises_b = scipy.special.i0(vonmises_m) 176 | 177 | # Add angles into element-wise lists 178 | ang_dict = {} 179 | for idx in range(angs.shape[0]): 180 | i, j, ang = angs[idx] 181 | i = int(i.item()) 182 | j = int(j.item()) 183 | ang = ang.item() 184 | 185 | # Negative entries [-1, -1, -1] means end of list 186 | if i < 0: 187 | break 188 | 189 | # Initialize empty array for first encountered element 190 | if (i, j) not in ang_dict.keys(): 191 | ang_dict[(i, j)] = [] 192 | 193 | # Add angle to multimodal distribution for element 194 | ang_dict[(i, j)].append(ang) 195 | 196 | # Initialize uniform distribution tensor 197 | distr_tensor = np.ones((height, width, num_discr)) / num_discr 198 | 199 | # Create multimodal von Mises distribution for elements 200 | for elem in ang_dict.keys(): 201 | i, j = elem 202 | 203 | num_angs = len(ang_dict[(i, j)]) 204 | 205 | mm_distr = np.zeros_like(ang_range) 206 | 207 | for mode_idx in range(num_angs): 208 | 209 | mode_ang = ang_dict[(i, j)][mode_idx] 210 | 211 | distr = np.exp(vonmises_m * np.cos(ang_range - mode_ang)) 212 | distr /= (2.0 * np.pi * vonmises_b) 213 | 214 | # Preserve significance of each mode independent of frequency 215 | mm_distr = np.maximum(distr, mm_distr) 216 | 217 | # Normalize distribution 218 | mm_distr /= self.integrate_distribution(mm_distr, ang_range) 219 | 220 | distr_tensor[i, j] = mm_distr 221 | 222 | return distr_tensor 223 | 224 | @staticmethod 225 | def integrate_distribution(dist, dist_range): 226 | '''Integrate a distribution using the trapezoidal approximation rule. 227 | 228 | Args: 229 | dist: Distribution values in 1D array. 230 | dist_range: Distrbution range in 1D array. 231 | 232 | Returns: 233 | Integration sum as float. 234 | ''' 235 | N = dist.shape[0] 236 | integ_sum = 0.0 237 | for i in range(N - 1): 238 | partion_range = dist_range[i + 1] - dist_range[i] 239 | dist_val = dist[i] + dist[i + 1] 240 | integ_sum += partion_range * dist_val / 2.0 241 | 242 | return integ_sum 243 | 244 | @staticmethod 245 | def read_compressed_pickle(path): 246 | try: 247 | with gzip.open(path, "rb") as f: 248 | pkl_obj = f.read() 249 | obj = pickle.loads(pkl_obj) 250 | return obj 251 | except IOError as error: 252 | print(error) 253 | 254 | @staticmethod 255 | def sharpen(array): 256 | kernel = np.array([[0, -1, 0], [-1, 5, -1], [0, -1, 0]]) 257 | return cv2.filter2D(array, -1, kernel) 258 | 259 | @staticmethod 260 | def gaussian_blur(array): 261 | kernel = (1 / 16) * np.array([[1, 2, 1], [2, 4, 2], [1, 2, 1]]) 262 | return cv2.filter2D(array, -1, kernel) 263 | 264 | @staticmethod 265 | def box_blur(array): 266 | kernel = (1 / 9) * np.array([[1, 1, 1], [1, 1, 1], [1, 1, 1]]) 267 | return cv2.filter2D(array, -1, kernel) 268 | 269 | @staticmethod 270 | def scale(array, thresh_min=0.25, thresh_max=0.75): 271 | scale = np.random.normal(loc=1., scale=0.2) 272 | scale = max(scale, thresh_min) 273 | scale = min(scale, thresh_max) 274 | return scale * array 275 | 276 | @staticmethod 277 | def normalize(array): 278 | mask = array > 1. 279 | array[mask] = 1. 280 | mask = array < 0. 281 | array[mask] = 0. 282 | return array 283 | 284 | 285 | class BEVDataPreprocModule(pl.LightningDataModule): 286 | 287 | def __init__( 288 | self, 289 | train_data_dir: str = "./", 290 | val_data_dir: str = "./", 291 | test_data_dir: str = "./", 292 | batch_size: int = 128, 293 | num_workers: int = 0, 294 | persistent_workers=True, 295 | do_rotation: bool = False, 296 | do_aug: bool = False, 297 | ): 298 | super().__init__() 299 | self.train_data_dir = train_data_dir 300 | self.val_data_dir = val_data_dir 301 | self.test_data_dir = test_data_dir 302 | self.batch_size = batch_size 303 | self.num_workers = num_workers 304 | self.persistent_workers = persistent_workers 305 | 306 | self.bev_dataset_train = PreprocBEVDataset( 307 | self.train_data_dir, 308 | do_rotation=do_rotation, 309 | do_aug=do_aug, 310 | ) 311 | # NOTE Loads GT lane map for evaluation 312 | self.bev_dataset_val = PreprocBEVDataset(self.val_data_dir, 313 | get_gt_labels=True) 314 | self.bev_dataset_test = PreprocBEVDataset(self.test_data_dir, 315 | get_gt_labels=True) 316 | 317 | def train_dataloader(self, shuffle=True): 318 | return DataLoader( 319 | self.bev_dataset_train, 320 | batch_size=self.batch_size, 321 | num_workers=self.num_workers, 322 | persistent_workers=self.persistent_workers, 323 | shuffle=shuffle, 324 | ) 325 | 326 | def val_dataloader(self, shuffle=False): 327 | return DataLoader( 328 | self.bev_dataset_val, 329 | batch_size=self.batch_size, 330 | num_workers=self.num_workers, 331 | persistent_workers=self.persistent_workers, 332 | shuffle=shuffle, 333 | ) 334 | 335 | def test_dataloader(self, shuffle=False): 336 | return DataLoader( 337 | self.bev_dataset_test, 338 | batch_size=self.batch_size, 339 | num_workers=self.num_workers, 340 | persistent_workers=self.persistent_workers, 341 | shuffle=shuffle, 342 | ) 343 | 344 | 345 | if __name__ == '__main__': 346 | ''' 347 | For visualizing dataset tensors. 348 | ''' 349 | 350 | from viz.viz_dataset import viz_dataset_sample 351 | 352 | batch_size = 1 353 | 354 | ############################### 355 | # Load preprocessed dataset 356 | ############################### 357 | 358 | bev = BEVDataPreprocModule('bev_nuscenes_256px_v01_job01_rl_preproc', 359 | 'bev_nuscenes_256px_v01_job01_rl_preproc', 360 | 'bev_nuscenes_256px_v01_job01_rl_preproc', 361 | batch_size, 362 | do_rotation=False, 363 | do_aug=False) 364 | 365 | dataloader = bev.train_dataloader(shuffle=False) 366 | 367 | for idx, batch in enumerate(dataloader): 368 | 369 | inputs, labels = batch 370 | 371 | # Transform input value range (-1, 1) --> (0, 1) 372 | inputs = 0.5 * (inputs + 1) 373 | 374 | # Remove batch index in each tensor 375 | inputs = inputs[0] 376 | for key in labels.keys(): 377 | labels[key] = labels[key][0] 378 | 379 | viz_dataset_sample(inputs, labels) 380 | -------------------------------------------------------------------------------- /graph_inference/max_likelihood_graph.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | 3 | import cv2 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import scipy.interpolate as si 7 | from numpy.random import default_rng 8 | 9 | 10 | def pnt_dist(pose_0: np.array, pose_1: np.array): 11 | ''' 12 | Returns the Euclidean distance between two poses. 13 | dist = sqrt( dx**2 + dy**2 ) 14 | 15 | Args: 16 | pose_0: 1D vector [x, y] 17 | pose_1: 18 | ''' 19 | dist = np.sqrt(np.sum((pose_1 - pose_0)**2)) 20 | return dist 21 | 22 | 23 | def bspline(cv, n=100, degree=3): 24 | """ 25 | Calculate n samples on a bspline 26 | 27 | Ref: https://stackoverflow.com/questions/28279060/splines-with-python-using-control-knots-and-endpoints 28 | 29 | Args: 30 | cv: Array ov control vertices 31 | n: Number of samples to return 32 | degree: Curve degree 33 | """ 34 | cv = np.asarray(cv) 35 | count = cv.shape[0] 36 | 37 | # Prevent degree from exceeding count-1, otherwise splev will crash 38 | degree = np.clip(degree, 1, count - 1) 39 | 40 | # Calculate knot vector 41 | kv = np.array([0] * degree + list(range(count - degree + 1)) + 42 | [count - degree] * degree, 43 | dtype='int') 44 | 45 | # Calculate query range 46 | u = np.linspace(0, (count - degree), n) 47 | 48 | # Calculate result 49 | return np.array(si.splev(u, (kv, cv.T, degree))).T 50 | 51 | 52 | def bspline_equidistance(cv, dist=10, n=100, degree=3): 53 | ''' 54 | ''' 55 | spline = bspline(cv, n, degree) 56 | 57 | # Resample equidistant spline 58 | # Compute path length 59 | ds = [] 60 | for idx in range(spline.shape[0] - 1): 61 | pnt_0 = spline[idx] 62 | pnt_1 = spline[idx + 1] 63 | d = pnt_dist(pnt_0, pnt_1) 64 | ds.append(d) 65 | path_length = np.sum(ds) 66 | 67 | # Number of pnts 68 | num_pnts = int(path_length // dist) 69 | 70 | # Assuming that 'data' is rows x dims (where dims is the dimensionality) 71 | # Ref: https://stackoverflow.com/questions/19117660/how-to-generate-equispaced-interpolating-values 72 | data = spline 73 | diffs = data[1:, :] - data[:-1, :] 74 | dist = np.linalg.norm(diffs, axis=1) 75 | u = np.cumsum(dist) 76 | u = np.hstack([[0], u]) 77 | t = np.linspace(0, u[-1], num_pnts) 78 | resampled = si.interpn((u, ), data, t) 79 | 80 | return resampled 81 | 82 | 83 | def find_connected_entry_exit_pairs(entry_paths, connecting_paths, exit_paths): 84 | 85 | connected_pnts = [] 86 | for entry_idx, entry_path in enumerate(entry_paths): 87 | 88 | entry_pnt_0, entry_pnt_1 = entry_path 89 | print('entry') 90 | print(entry_idx, entry_pnt_0) 91 | print(entry_idx, entry_pnt_1) 92 | 93 | for con_idx, connecting_path in enumerate(connecting_paths): 94 | 95 | connecting_pnt_0, connecting_pnt_1 = connecting_path 96 | print('\t connecting') 97 | print('\t', con_idx, connecting_pnt_0) 98 | print('\t', con_idx, connecting_pnt_1) 99 | 100 | if entry_pnt_1 == connecting_pnt_0: 101 | 102 | for exit_idx, exit_path in enumerate(exit_paths): 103 | 104 | # NOTE Mixed indices 105 | exit_pnt_0, exit_pnt_1 = exit_path 106 | 107 | print('\t\t exit') 108 | print('\t\t', exit_idx, exit_pnt_0) 109 | print('\t\t', exit_idx, exit_pnt_1) 110 | 111 | if connecting_pnt_1 == exit_pnt_0: 112 | 113 | connected_pnts.append((entry_pnt_0, exit_pnt_1)) 114 | print('\t\t\t Connected', entry_pnt_0, exit_pnt_1) 115 | 116 | return connected_pnts 117 | 118 | 119 | def preproc_entry_exit_pairs(pnt_pairs, 120 | pnt_scalar=2, 121 | img_frame_dim=256, 122 | lane_width_px=12): 123 | 124 | lane_width_px = pnt_scalar * lane_width_px 125 | 126 | pnt_pairs_preproc = [] 127 | 128 | for pnt_pair in pnt_pairs: 129 | 130 | entry_i = pnt_pair[0][0] 131 | entry_j = pnt_pair[0][1] 132 | exit_i = pnt_pair[1][0] 133 | exit_j = pnt_pair[1][1] 134 | 135 | entry_i *= pnt_scalar 136 | entry_j *= pnt_scalar 137 | exit_i *= pnt_scalar 138 | exit_j *= pnt_scalar 139 | 140 | entry_i = int(entry_i) 141 | entry_j = int(entry_j) 142 | exit_i = int(exit_i) 143 | exit_j = int(exit_j) 144 | 145 | dist = np.sqrt((exit_i - entry_i)**2 + (exit_j - entry_j)**2) 146 | if dist < 2 * lane_width_px: 147 | continue 148 | 149 | # Check if both 'left' 150 | # if entry_i < 10 and exit_i < 10: 151 | # # print('left') 152 | # continue 153 | # Check if both 'bottom' 154 | # if entry_j > (img_frame_dim - 10) and exit_j > (img_frame_dim - 10): 155 | # # print('bottom') 156 | # continue 157 | # Check if both 'bottom' 158 | # print(entry_i, img_frame_dim - 10) 159 | # print(exit_i, img_frame_dim - 10) 160 | # if entry_i > (img_frame_dim - 10) and exit_i > (img_frame_dim - 10): 161 | # # print('right') 162 | # continue 163 | # Check if both 'left' 164 | # if entry_j < 10 and exit_j < 10: 165 | # # print('top') 166 | # continue 167 | 168 | pnt_pairs_preproc.append(([entry_i, entry_j], [exit_i, exit_j])) 169 | 170 | return pnt_pairs_preproc 171 | 172 | 173 | def path_to_mask(path: np.array, height: int, width: int): 174 | ''' 175 | Converts a set of path points to a boolean mask. 176 | 177 | Args: 178 | path: Matrix (N,2) of (i, j) image coordinates (int). 179 | height: Height of image frame. 180 | width: Width of image frame. 181 | 182 | Returns: 183 | Boolean mask of size (height, width) with 'True' values for path points. 184 | 185 | ''' 186 | path_mask = np.zeros((256, 256), dtype=bool) 187 | path_mask[path[:, 1], path[:, 0]] = True 188 | 189 | return path_mask 190 | 191 | 192 | def path_nll_da(path, da, eps=1e-24): 193 | ''' 194 | Compute the negative log likelihood of path given predicted directional affordance. 195 | 196 | Args: 197 | path: Matrix (N,2) of (i, j) image coordinates (int). 198 | da: Tensor (D,H,W) with directional affordance probabilty values. 199 | 200 | Returns: 201 | Mean NLL of path. 202 | ''' 203 | num_dirs, height, width = da.shape 204 | path_mask = path_to_mask(path, height, width) 205 | 206 | delta_phi = 2. * np.pi / num_dirs 207 | 208 | nll = 0 209 | for idx in range(path.shape[0] - 1): 210 | # Compute angle to next point 211 | i0 = path[idx, 0] 212 | j0 = path[idx, 1] 213 | 214 | i1 = path[idx + 1, 0] 215 | j1 = path[idx + 1, 1] 216 | 217 | di = i1 - i0 218 | dj = j1 - j0 219 | 220 | ang = np.arctan2(di, dj) - 0.5 * np.pi 221 | if ang < 0: 222 | ang += 2. * np.pi 223 | 224 | # Get idx of corresponding probability of angle interval 225 | # Ex: 23.456 // 10 := 2 226 | ang_idx = int(ang // delta_phi) 227 | 228 | # Prob of path direction 229 | # Apply convolution to allow diagonal transitions in straight 230 | # directional fields 231 | #p_dir = da[:, j0, i0] 232 | #kernel = np.array([0.25, 0.50, 0.25]) 233 | #p_dir_padding = np.pad(p_dir, 1, 'wrap') 234 | #p_dir_padding = np.convolve(p_dir_padding, kernel, mode='same') 235 | #p_dir = p_dir_padding[1:-1] 236 | #prob = p_dir[ang_idx] 237 | 238 | prob = da[ang_idx, j0, i0] 239 | 240 | nll += -1 * np.log(prob + eps) 241 | 242 | return nll # / np.sum(path_mask) 243 | 244 | 245 | def path_nll_sla(path: np.array, sla: np.array, eps=1e-24): 246 | ''' 247 | Compute the negative log likelihood of path given predicted soft lane affordance. 248 | 249 | Args: 250 | path: Matrix (N,2) of (i, j) image coordinates (int). 251 | sla: Matrix (H,W) with soft lane affordance probabilty values. 252 | 253 | Returns: 254 | Mean NLL of path. 255 | ''' 256 | height, width = sla.shape 257 | path_mask = path_to_mask(path, height, width) 258 | 259 | if (sla[path_mask] == 0).any(): 260 | return np.inf 261 | 262 | nll = -1 * np.log(path_mask * sla + eps) 263 | nll = path_mask * nll 264 | 265 | return np.sum(nll) # / np.sum(path_mask) 266 | 267 | 268 | def find_max_likelihood_path(entry_i, 269 | entry_j, 270 | exit_i, 271 | exit_j, 272 | sla, 273 | da, 274 | num_samples=1000, 275 | num_pnts=50, 276 | nll_da_weight=1, 277 | img_frame_dim=256, 278 | sampling_distr_ratio=0.6): 279 | ''' 280 | ''' 281 | rng = default_rng() 282 | 283 | half_dist = 0.5 * img_frame_dim 284 | var = (half_dist * sampling_distr_ratio)**2 285 | vals = rng.multivariate_normal((half_dist, half_dist), 286 | cov=var * np.eye(2), 287 | size=num_samples) 288 | 289 | best_path = None 290 | best_nll_sla = None 291 | best_nll_da = None 292 | best_cv = None 293 | best_nll = np.inf 294 | paths = [] 295 | 296 | for idx in range(num_samples): 297 | 298 | val = vals[idx] 299 | 300 | # Constrain path to be within image frame 301 | if (val < 0).any() or (val >= 256).any(): 302 | continue 303 | 304 | cv_i = val[0] 305 | cv_j = val[1] 306 | 307 | cv = np.array([[entry_j, entry_i], [cv_j, cv_i], [exit_j, exit_i]]) 308 | 309 | # path = bspline(cv, n=num_pnts, degree=2) 310 | path = bspline_equidistance(cv, dist=10, n=num_pnts, degree=2) 311 | path = path.astype(int) 312 | paths.append(path) 313 | 314 | nll_sla = path_nll_sla(path, sla) 315 | nll_da = path_nll_da(path, da) 316 | nll = nll_sla + nll_da_weight * nll_da 317 | 318 | if nll < best_nll: 319 | best_cv = cv 320 | best_path = path 321 | best_nll_sla = nll_sla 322 | best_nll_da = nll_da 323 | best_nll = nll 324 | 325 | return best_path, best_nll_sla, best_nll_da, best_cv, paths 326 | 327 | 328 | def find_max_likelihood_paths(pnt_pairs, 329 | sla, 330 | da, 331 | num_samples=1000, 332 | num_pnts=50): 333 | ''' 334 | ''' 335 | paths = [] 336 | for pnt_pair in pnt_pairs: 337 | 338 | entry_j = pnt_pair[0][0] 339 | entry_i = pnt_pair[0][1] 340 | exit_j = pnt_pair[1][0] 341 | exit_i = pnt_pair[1][1] 342 | 343 | best_path, best_nll_sla, best_nll_da, _, _ = find_max_likelihood_path( 344 | entry_i, 345 | entry_j, 346 | exit_i, 347 | exit_j, 348 | sla, 349 | da, 350 | num_samples=num_samples, 351 | num_pnts=num_pnts) 352 | 353 | # No connecting polynomial found 354 | if best_path is None: 355 | continue 356 | 357 | paths.append(best_path) 358 | 359 | return paths 360 | 361 | 362 | def find_max_likelihood_graph(sla, 363 | da, 364 | entry_paths, 365 | con_paths, 366 | exit_paths, 367 | num_samples=1000, 368 | num_pnts=50): 369 | ''' 370 | ''' 371 | # NOTE Temporary function for all entry to all exit connectivity 372 | pnt_pairs = [] 373 | for entry_path in entry_paths: 374 | for exit_path in exit_paths: 375 | pnt_pairs.append([entry_path[0], exit_path[1]]) 376 | 377 | pnt_pairs = preproc_entry_exit_pairs(pnt_pairs) 378 | 379 | paths = find_max_likelihood_paths(pnt_pairs, 380 | sla, 381 | da, 382 | num_samples=num_samples, 383 | num_pnts=num_pnts) 384 | 385 | return paths 386 | 387 | 388 | if __name__ == '__main__': 389 | 390 | with open('sample_12.pkl', 'rb') as file: 391 | sample = pickle.load(file) 392 | 393 | sla = sample['sla'] 394 | da = sample['da'] 395 | entry_paths = sample['entry_paths'] 396 | connecting_paths = sample['connecting_pnts'] 397 | exit_paths = sample['exit_paths'] 398 | 399 | # TODO Fix inexact DAG points 400 | # pnt_pairs = find_connected_entry_exit_pairs(entry_paths, connecting_paths, exit_paths) 401 | 402 | # NOTE Temporary function for all entry to all exit connectivity 403 | pnt_pairs = [] 404 | for entry_path in entry_paths: 405 | for exit_path in exit_paths: 406 | pnt_pairs.append([entry_path[0], exit_path[1]]) 407 | 408 | for pnts in pnt_pairs: 409 | print(pnts) 410 | 411 | plt.imshow(cv2.resize(sla, (128, 128), interpolation=cv2.INTER_LINEAR)) 412 | plt.show() 413 | 414 | pnt_pairs = preproc_entry_exit_pairs(pnt_pairs) 415 | 416 | for pnts in pnt_pairs: 417 | print(pnts) 418 | 419 | paths = find_max_likelihood_paths(pnt_pairs, sla, da) 420 | 421 | plt.imshow(sla) 422 | for path in paths: 423 | i, j = path.T 424 | plt.plot(i, j, 'k-') 425 | di = i[-1] - i[-2] 426 | dj = j[-1] - j[-2] 427 | plt.arrow(i[-2], 428 | j[-2], 429 | di, 430 | dj, 431 | head_width=5, 432 | facecolor='k', 433 | length_includes_head=True) 434 | plt.show() 435 | -------------------------------------------------------------------------------- /viz/viz_dense.py: -------------------------------------------------------------------------------- 1 | import copy 2 | import os 3 | 4 | import cv2 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | import torch 8 | 9 | 10 | def hsv_to_rgb(H, S, V): 11 | '''Returns the RGB values of a given HSV color. 12 | 13 | Ref: https://en.wikipedia.org/wiki/HSL_and_HSV 14 | 15 | Args: 16 | H: Hue value in range (0, 2*pi). 17 | S: Saturation value in range (0, 1). 18 | V: Value in range (0, 1). 19 | ''' 20 | C = V * S 21 | 22 | H_prime = H / (np.pi / 3.0) 23 | 24 | X = C * (1.0 - abs(H_prime % 2.0 - 1.0)) 25 | 26 | if 0.0 <= H_prime and H_prime <= 1.0: 27 | R_1, G_1, B_1 = (C, X, 0) 28 | elif H_prime <= 2.0: 29 | R_1, G_1, B_1 = (X, C, 0) 30 | elif H_prime <= 3.0: 31 | R_1, G_1, B_1 = (0, C, X) 32 | elif H_prime <= 4.0: 33 | R_1, G_1, B_1 = (0, X, C) 34 | elif H_prime <= 5.0: 35 | R_1, G_1, B_1 = (X, 0, C) 36 | elif H_prime <= 6.0: 37 | R_1, G_1, B_1 = (C, 0, X) 38 | else: 39 | R_1, G_1, B_1 = (0, 0, 0) 40 | 41 | m = V - C 42 | 43 | R = R_1 + m 44 | G = G_1 + m 45 | B = B_1 + m 46 | 47 | return R, G, B 48 | 49 | 50 | def visualize_dense(context, 51 | output_sla, 52 | output_mean, 53 | output_var, 54 | output_weight, 55 | output_entry, 56 | output_exit, 57 | output_size=128 * 10, 58 | man_threshold=0.2): 59 | ''' 60 | Args: 61 | context: (n, n) in range (0, 255) 62 | output_sla: (n, n) in range (0, 1) 63 | output_mean: (3, n, n) in range (-1, 1) #6 64 | output_var: (3, n, n) in range (0, 1) 65 | output_weight: (3, n, n) in range (0, 1) 66 | ''' 67 | 68 | ################### 69 | # RESIZE INPUTS 70 | ################### 71 | dim = (output_size, output_size) 72 | 73 | context_seg = copy.deepcopy(context) 74 | context_seg = cv2.resize(context_seg, dim, interpolation=cv2.INTER_NEAREST) 75 | context = cv2.resize(context, dim, interpolation=cv2.INTER_NEAREST) 76 | 77 | output_sla = cv2.resize(output_sla, dim, interpolation=cv2.INTER_LINEAR) 78 | output_entry = cv2.resize(output_entry, 79 | dim, 80 | interpolation=cv2.INTER_LINEAR) 81 | output_exit = cv2.resize(output_exit, dim, interpolation=cv2.INTER_LINEAR) 82 | 83 | mean_0 = cv2.resize(output_mean[0], dim, interpolation=cv2.INTER_NEAREST) 84 | mean_1 = cv2.resize(output_mean[1], dim, interpolation=cv2.INTER_NEAREST) 85 | mean_2 = cv2.resize(output_mean[2], dim, interpolation=cv2.INTER_NEAREST) 86 | 87 | var_0 = cv2.resize(output_var[0], dim, interpolation=cv2.INTER_NEAREST) 88 | var_1 = cv2.resize(output_var[1], dim, interpolation=cv2.INTER_NEAREST) 89 | var_2 = cv2.resize(output_var[2], dim, interpolation=cv2.INTER_NEAREST) 90 | 91 | weight_0 = cv2.resize(output_weight[0], 92 | dim, 93 | interpolation=cv2.INTER_NEAREST) 94 | weight_1 = cv2.resize(output_weight[1], 95 | dim, 96 | interpolation=cv2.INTER_NEAREST) 97 | weight_2 = cv2.resize(output_weight[2], 98 | dim, 99 | interpolation=cv2.INTER_NEAREST) 100 | 101 | # Create BGR image for 'context' 102 | context = context.astype(np.uint8) 103 | context = cv2.cvtColor(context, cv2.COLOR_GRAY2BGR) 104 | 105 | ######### 106 | # SLA 107 | ######### 108 | # Create a mask with SLA elements over a threshold intensity 109 | mask = 255 * (output_sla > 0.1).astype(np.uint8) 110 | # Extract masked elements from SLA array ('non-elements' are 0) 111 | sla_masked = cv2.bitwise_and(mask, (255.0 * output_sla).astype(np.uint8)) 112 | # Create BGR heatmap from SLA elements 113 | sla_masked = cv2.cvtColor(sla_masked, cv2.COLOR_GRAY2BGR) 114 | sla_masked = cv2.applyColorMap(sla_masked, cv2.COLORMAP_HOT) 115 | 116 | # Combine 'context' and 'masked SLA heatmap' 117 | sla = cv2.addWeighted(sla_masked, 0.8, context, 1, 0) 118 | 119 | ########### 120 | # Entry 121 | ########### 122 | # Create a mask with Maneuver elements over a threshold intensity 123 | # mask_entry = 255*(output_entry >= man_threshold).astype(np.uint8) 124 | # Extract masked elmements from Man array ('non-elements' are 0) 125 | # output_entry_ = cv2.bitwise_or(output_entry, output_entry, mask=mask_entry) 126 | # Rescale so that interval (threshold, 1) -> (0, 1) 127 | # output_entry_ = (output_entry_ - man_threshold) / (1.0 - man_threshold) 128 | # Rescale values from 0-->1 to 0.5-->1 129 | # output_entry_ = (output_entry_ + 0.5) / 1.5 130 | # Create BGR heatmap from Man elements 131 | # entry_masked = cv2.cvtColor((255*output_entry_).astype(np.uint8), cv2.COLOR_GRAY2BGR) 132 | # entry_masked = cv2.applyColorMap(entry_masked, cv2.COLORMAP_JET) 133 | 134 | ########## 135 | # Exit 136 | ########## 137 | # Create a mask with Maneuver elements over a threshold intensity 138 | # mask_exit = 255*(output_exit >= man_threshold).astype(np.uint8) 139 | # Extract masked elmements from Man array ('non-elements' are 0) 140 | # output_exit_ = cv2.bitwise_or(output_exit, output_exit, mask=mask_exit) 141 | # Rescale so that interval (threshold, 1) -> (0, 1) 142 | # output_exit_ = (output_exit_ - man_threshold) / (1.0 - man_threshold) 143 | # Rescale values from 0-->1 to 0.5-->1 144 | # output_exit_ = (output_exit_ + 0.5) / 1.5 145 | # Create BGR heatmap from Man elements 146 | # exit_masked = cv2.cvtColor((255-255*output_exit_).astype(np.uint8), cv2.COLOR_GRAY2BGR) 147 | # exit_masked = cv2.applyColorMap(exit_masked, cv2.COLORMAP_JET) 148 | 149 | ############################# 150 | # COMBINE 'SLA' AND 'MAN' 151 | ############################# 152 | # entry_masked = cv2.bitwise_or(entry_masked, entry_masked, mask=mask_entry) 153 | # mask_inv = cv2.bitwise_not(mask_entry) 154 | # masked_sla = cv2.bitwise_or(sla, sla, mask=mask_inv) 155 | # sla = cv2.bitwise_or(masked_sla, entry_masked) 156 | 157 | # exit_masked = cv2.bitwise_or(exit_masked, exit_masked, mask=mask_exit) 158 | # mask_inv = cv2.bitwise_not(mask_exit) 159 | # masked_sla = cv2.bitwise_or(sla, sla, mask=mask_inv) 160 | # sla = cv2.bitwise_or(masked_sla, exit_masked) 161 | 162 | ############### 163 | # DIRECTION 164 | ############### 165 | vec_interval = 30 166 | vec_dist_len = 50 167 | vec_thickness = 2 168 | alpha = 0.7 169 | 170 | for j in range(0, output_size, vec_interval): 171 | for i in range(0, output_size, vec_interval): 172 | 173 | if context_seg[j, i] == 0: 174 | continue 175 | 176 | arrows = copy.deepcopy(sla) 177 | 178 | pnt_0_i = i 179 | pnt_0_j = j 180 | pnt_0 = (pnt_0_i, pnt_0_j) 181 | 182 | # Dist 0 183 | ang = mean_0[j, i] 184 | intensity = 1 - var_0[j, i] 185 | w_0 = weight_0[j, i] 186 | 187 | di = np.cos(ang) 188 | dj = np.sin(ang) 189 | 190 | pnt_1_i = int(round(pnt_0_i + w_0 * vec_dist_len * di)) 191 | pnt_1_j = int(round(pnt_0_j - w_0 * vec_dist_len * dj)) 192 | pnt_1 = (pnt_1_i, pnt_1_j) 193 | 194 | R, G, B = hsv_to_rgb(ang, intensity, intensity) 195 | color = (int(R * 255), int(G * 255), int(B * 255)) 196 | 197 | if intensity > 0.4: 198 | # Black arrow for contour 199 | cv2.arrowedLine(arrows, 200 | pnt_0, 201 | pnt_1, (0, 0, 0), 202 | vec_thickness + 1, 203 | tipLength=0.5, 204 | line_type=cv2.LINE_AA) 205 | cv2.arrowedLine(arrows, 206 | pnt_0, 207 | pnt_1, 208 | color, 209 | vec_thickness, 210 | tipLength=0.5, 211 | line_type=cv2.LINE_AA) 212 | 213 | # Dist 1 214 | ang = mean_1[j, i] 215 | intensity = 1 - var_1[j, i] 216 | w_1 = weight_1[j, i] 217 | 218 | di = np.cos(ang) 219 | dj = np.sin(ang) 220 | 221 | pnt_1_i = int(round(pnt_0_i + w_1 * vec_dist_len * di)) 222 | pnt_1_j = int(round(pnt_0_j - w_1 * vec_dist_len * dj)) 223 | pnt_1 = (pnt_1_i, pnt_1_j) 224 | 225 | R, G, B = hsv_to_rgb(ang, intensity, intensity) 226 | color = (int(R * 255), int(G * 255), int(B * 255)) 227 | 228 | if intensity > 0.4: 229 | # Black arrow for contour 230 | cv2.arrowedLine(arrows, 231 | pnt_0, 232 | pnt_1, (0, 0, 0), 233 | vec_thickness + 1, 234 | tipLength=0.5, 235 | line_type=cv2.LINE_AA) 236 | cv2.arrowedLine(arrows, 237 | pnt_0, 238 | pnt_1, 239 | color, 240 | vec_thickness, 241 | tipLength=0.5, 242 | line_type=cv2.LINE_AA) 243 | 244 | # Dist 2 245 | ang = mean_2[j, i] 246 | intensity = 1 - var_2[j, i] 247 | w_2 = weight_2[j, i] 248 | 249 | di = np.cos(ang) 250 | dj = np.sin(ang) 251 | 252 | pnt_1_i = int(round(pnt_0_i + w_2 * vec_dist_len * di)) 253 | pnt_1_j = int(round(pnt_0_j - w_2 * vec_dist_len * dj)) 254 | pnt_1 = (pnt_1_i, pnt_1_j) 255 | 256 | R, G, B = hsv_to_rgb(ang, intensity, intensity) 257 | color = (int(R * 255), int(G * 255), int(B * 255)) 258 | 259 | if intensity > 0.4: 260 | # Black arrow for contour 261 | cv2.arrowedLine(arrows, 262 | pnt_0, 263 | pnt_1, (0, 0, 0), 264 | vec_thickness + 1, 265 | tipLength=0.5, 266 | line_type=cv2.LINE_AA) 267 | cv2.arrowedLine(arrows, 268 | pnt_0, 269 | pnt_1, 270 | color, 271 | vec_thickness, 272 | tipLength=0.5, 273 | line_type=cv2.LINE_AA) 274 | 275 | cv2.addWeighted(arrows, alpha, sla, 1 - alpha, 0, sla) 276 | 277 | sla = cv2.cvtColor(sla, cv2.COLOR_BGR2RGB) 278 | return sla 279 | 280 | 281 | def visualize_dense_softmax(context, 282 | output_sla, 283 | output_dir, 284 | output_entry, 285 | output_size=128 * 10, 286 | man_threshold=0.9): 287 | ''' 288 | Args: 289 | context: (n, n) in range (0, 255) 290 | output_sla: (n, n) in range (0, 1) 291 | output_dir: (64, n, n) in range (-1, 1) 292 | ''' 293 | 294 | ################### 295 | # RESIZE INPUTS 296 | ################### 297 | dim = (output_size, output_size) 298 | 299 | context_seg = copy.deepcopy(context) 300 | context_seg = cv2.resize(context_seg, dim, interpolation=cv2.INTER_NEAREST) 301 | context = cv2.resize(context, dim, interpolation=cv2.INTER_NEAREST) 302 | 303 | output_sla = cv2.resize(output_sla, dim, interpolation=cv2.INTER_LINEAR) 304 | # output_entry = cv2.resize(output_entry, 305 | # dim, 306 | # interpolation=cv2.INTER_LINEAR) 307 | 308 | dir_N = output_dir.shape[0] 309 | output_dirs = np.zeros((dir_N, output_size, output_size)) 310 | for dir_n in range(dir_N): 311 | output_dirs[dir_n] = cv2.resize(output_dir[dir_n], 312 | dim, 313 | interpolation=cv2.INTER_NEAREST) 314 | 315 | # Create BGR image for 'context' 316 | context = context.astype(np.uint8) 317 | context = cv2.cvtColor(context, cv2.COLOR_GRAY2BGR) 318 | 319 | ######### 320 | # SLA 321 | ######### 322 | # Create a mask with SLA elements over a threshold intensity 323 | mask = 255 * (output_sla > 0.0).astype(np.uint8) 324 | # Extract masked elements from SLA array ('non-elements' are 0) 325 | sla_masked = cv2.bitwise_and(mask, (255.0 * output_sla).astype(np.uint8)) 326 | # Create BGR heatmap from SLA elements 327 | sla_masked = cv2.cvtColor(sla_masked, cv2.COLOR_GRAY2BGR) 328 | sla_masked = cv2.applyColorMap(sla_masked, cv2.COLORMAP_HOT) 329 | 330 | # Combine 'context' and 'masked SLA heatmap' 331 | sla = cv2.addWeighted(sla_masked, 0.8, context, 1, 0) 332 | 333 | ######### 334 | # MAN 335 | ######### 336 | # # Create a mask with Maneuver elements over a threshold intensity 337 | # mask = 255 * (output_entry >= man_threshold).astype(np.uint8) 338 | # # Extract masked elmements from Man array ('non-elements' are 0) 339 | # output_entry_ = cv2.bitwise_or(output_entry, output_entry, mask=mask) 340 | # # Rescale so that interval (threshold, 1) -> (0, 1) 341 | # output_entry_ = (output_entry_ - man_threshold) / (1.0 - man_threshold) 342 | # # Create BGR heatmap from Man elements 343 | # man_masked = cv2.cvtColor((255 * output_entry_).astype(np.uint8), 344 | # cv2.COLOR_GRAY2BGR) 345 | # man_masked = cv2.applyColorMap(man_masked, cv2.COLORMAP_JET) 346 | # 347 | # ############################# 348 | # # COMBINE 'SLA' AND 'MAN' 349 | # ############################# 350 | # man_masked = cv2.bitwise_or(man_masked, man_masked, mask=mask) 351 | # 352 | # mask_inv = cv2.bitwise_not(mask) 353 | # masked_sla = cv2.bitwise_or(sla, sla, mask=mask_inv) 354 | # 355 | # sla = cv2.bitwise_or(masked_sla, man_masked) 356 | 357 | ############### 358 | # DIRECTION 359 | ############### 360 | vec_interval = 30 361 | vec_dist_len = 50 362 | vec_thickness = 2 363 | alpha = 0.7 364 | 365 | for j in range(0, output_size, vec_interval): 366 | for i in range(0, output_size, vec_interval): 367 | 368 | if output_sla[j, i] <= 0: 369 | continue 370 | 371 | arrows = copy.deepcopy(sla) 372 | 373 | # Starting point of line 374 | pnt_0_i = i 375 | pnt_0_j = j 376 | pnt_0 = (pnt_0_i, pnt_0_j) 377 | 378 | for mode_idx in range(dir_N): 379 | 380 | dir_prob = output_dirs[mode_idx, j, i] 381 | 382 | ang = (mode_idx + 0.5) * 2. * np.pi / dir_N 383 | 384 | di = np.cos(ang) 385 | dj = np.sin(ang) 386 | 387 | pnt_1_i = int(round(pnt_0_i + dir_prob * vec_dist_len * di)) 388 | pnt_1_j = int(round(pnt_0_j - dir_prob * vec_dist_len * dj)) 389 | pnt_1 = (pnt_1_i, pnt_1_j) 390 | 391 | R, G, B = hsv_to_rgb(ang, 1, 1) 392 | color = (int(R * 255), int(G * 255), int(B * 255)) 393 | 394 | cv2.arrowedLine(arrows, 395 | pnt_0, 396 | pnt_1, 397 | color, 398 | vec_thickness, 399 | tipLength=0.5, 400 | line_type=cv2.LINE_AA) 401 | 402 | cv2.addWeighted(arrows, alpha, sla, 1 - alpha, 0, sla) 403 | 404 | 405 | # # ML directionality 406 | # dir_n_max = np.argmax(output_dirs[:, j, i]) 407 | # 408 | # ang = dir_n_max * 2. * np.pi / dir_N 409 | # weight = 1. 410 | # 411 | # di = np.cos(ang) 412 | # dj = np.sin(ang) 413 | # 414 | # pnt_1_i = int(round(pnt_0_i + weight * vec_dist_len * di)) 415 | # pnt_1_j = int(round(pnt_0_j - weight * vec_dist_len * dj)) 416 | # pnt_1 = (pnt_1_i, pnt_1_j) 417 | # 418 | # R, G, B = hsv_to_rgb(ang, 1, 1) 419 | # color = (int(R * 255), int(G * 255), int(B * 255)) 420 | # 421 | # if weight > 0: 422 | # cv2.arrowedLine(arrows, 423 | # pnt_0, 424 | # pnt_1, 425 | # color, 426 | # vec_thickness, 427 | # tipLength=0.5, 428 | # line_type=cv2.LINE_AA) 429 | # 430 | # cv2.addWeighted(arrows, alpha, sla, 1 - alpha, 0, sla) 431 | 432 | sla = cv2.cvtColor(sla, cv2.COLOR_BGR2RGB) 433 | return sla 434 | 435 | 436 | def visualize_dir_label(output_viz, 437 | mm_ang_tensor, 438 | reduction=3, 439 | color=(0, 0, 0)): 440 | ''' 441 | Overlays directional label on top of output visualization 442 | 443 | Args: 444 | output_viz: RGB image (H,W,3) representing output. 445 | mm_ang_tensor: Probability of angle intervals (num_angs, H, W). 446 | reduction: Interval for sparsifying label (i.e. every 6th element). 447 | color: RGB code for directional label arrows. 448 | 449 | Returns: 450 | output_viz with direction label as black arrows. 451 | ''' 452 | # Extract elements with observed directionality 453 | num_angs = mm_ang_tensor.shape[0] 454 | 455 | # Reduce entries 456 | i_max = mm_ang_tensor.shape[1] 457 | j_max = mm_ang_tensor.shape[2] 458 | mask = torch.ones_like(mm_ang_tensor, dtype=torch.bool) 459 | for j in range(0, j_max, reduction): 460 | for i in range(0, i_max, reduction): 461 | mask[:, i, j] = 0 462 | mm_ang_tensor[mask] = 0 463 | 464 | # Remove non-observed directions 465 | avg_prob = 1 / num_angs 466 | mask = mm_ang_tensor > avg_prob 467 | mm_ang_tensor = mask * mm_ang_tensor 468 | 469 | # Extract directions as (N,3) matrix 470 | # [0]: directional 471 | angs = torch.nonzero(mm_ang_tensor) 472 | 473 | for idx in range(angs.shape[0]): 474 | 475 | # NOTE Indices switched! 476 | ang_idx, j, i = angs[idx] 477 | ang_idx = int(ang_idx.item()) 478 | pnt_0_i = int(i.item()) 479 | pnt_0_j = int(j.item()) 480 | 481 | resize_coeff = 5 482 | pnt_0_i *= resize_coeff 483 | pnt_0_j *= resize_coeff 484 | 485 | pnt_0 = (pnt_0_i, pnt_0_j) 486 | 487 | ang = ang_idx * (1 / num_angs) + (1 / num_angs) 488 | ang *= 2. * np.pi 489 | 490 | # NOTE Indices switched! 491 | ang_prob = mm_ang_tensor[ang_idx, j, i] / torch.sum(mm_ang_tensor[:, j, 492 | i]) 493 | ang_prob = ang_prob.item() 494 | 495 | di = np.cos(ang) 496 | dj = np.sin(ang) 497 | 498 | vec_dist_len = 50 499 | pnt_1_i = int(round(pnt_0_i + ang_prob * vec_dist_len * di)) 500 | pnt_1_j = int(round(pnt_0_j - ang_prob * vec_dist_len * dj)) 501 | pnt_1 = (pnt_1_i, pnt_1_j) 502 | 503 | arrows = copy.deepcopy(output_viz) 504 | 505 | vec_thickness = 1 506 | alpha = 0.7 507 | cv2.arrowedLine(arrows, 508 | pnt_0, 509 | pnt_1, 510 | color, 511 | vec_thickness, 512 | tipLength=0.5, 513 | line_type=cv2.LINE_AA) 514 | 515 | cv2.addWeighted(arrows, alpha, output_viz, 1 - alpha, 0, output_viz) 516 | 517 | return output_viz 518 | -------------------------------------------------------------------------------- /graph_inference/graph_func.py: -------------------------------------------------------------------------------- 1 | import copy 2 | 3 | import cv2 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | 7 | #from graph_inference.a_star import a_star 8 | from graph_inference.dense_nms import dense_nonmax_sup 9 | from graph_inference.dsla_weight_matrix import (angle_between, 10 | dsla_weighted_adj_mat) 11 | from graph_inference.grid_map import (get_neighbor_nodes, grid_adj_mat, 12 | node_coord2idx, node_idx2coord) 13 | from losses.da_model_free_kl_div import integrate_distribution 14 | 15 | # from preproc.conditional_dsla import comp_descrete_entry_points 16 | 17 | 18 | def discretize_border_regions(dense_map, value, nms_pnts=[]): 19 | ''' 20 | Ref: https://learnopencv.com/find-center-of-blob-centroid-using-opencv-cpp-python/ 21 | ''' 22 | # Convert to binary map 23 | dense_map = dense_map == value 24 | dense_map = (255. * dense_map).astype(np.uint8) 25 | # NOTE: Dilate in order to reduce posibility of zero-thickness clusters 26 | # NOTE: May remove overlap with existing NMS points 27 | kernel = np.ones((3, 3), np.uint8) 28 | dense_map = cv2.dilate(dense_map, kernel, iterations=1) 29 | dense_map = cv2.erode(dense_map, kernel, iterations=2) 30 | dense_map = cv2.dilate(dense_map, kernel, iterations=1) # NOTE 2 !!! 31 | 32 | # Find separated clusters 33 | # _, contours, _ = cv2.findContours(dense_map, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) 34 | contours, _ = cv2.findContours(dense_map, cv2.RETR_TREE, 35 | cv2.CHAIN_APPROX_SIMPLE) 36 | 37 | entry_pnt_list = [] 38 | for c in contours: 39 | 40 | # Skip if a NMS point already exist within contour 41 | is_inside = -1 42 | for nms_pnt in nms_pnts: 43 | nms_pnt_rev = (nms_pnt[1], nms_pnt[0]) 44 | is_inside = cv2.pointPolygonTest(c, nms_pnt_rev, False) 45 | if is_inside == 1 or is_inside == 0: 46 | break 47 | 48 | if is_inside == 1 or is_inside == 0: 49 | continue 50 | 51 | # Calculate moments for each contour 52 | M = cv2.moments(c) 53 | # Skip bad shapes 54 | if M["m10"] == 0 or M["m00"] == 0 or M["m01"] == 0: 55 | continue 56 | # Calculate x,y coordinate of center 57 | c_x = int(M["m10"] / M["m00"]) 58 | c_y = int(M["m01"] / M["m00"]) 59 | 60 | entry_pnt_list.append((c_y, c_x)) 61 | 62 | return entry_pnt_list 63 | 64 | 65 | # def search_path(A, 66 | # weighted_A, 67 | # start_node_idx, 68 | # goal_node_idx, 69 | # I, 70 | # J, 71 | # heuristic_type="max_axis"): 72 | # ''' 73 | # ''' 74 | # 75 | # d, par = a_star(A, weighted_A, start_node_idx, goal_node_idx, J, 76 | # "manhattan") 77 | # 78 | # d_arr = np.array(d).reshape((I, J)) 79 | # d_arr[d_arr == np.inf] = 0. 80 | # 81 | # path = [] 82 | # 83 | # par_idx = goal_node_idx 84 | # while True: 85 | # 86 | # path.insert(0, par_idx) 87 | # par_idx = par[par_idx] 88 | # 89 | # # Goal is unreachable 90 | # if par_idx == -1: 91 | # break 92 | # 93 | # return d_arr, path 94 | 95 | 96 | def path_idx2coord(path, J): 97 | '''Converts a list of vertex indices [idx_1, ] to coordinates [[i,j]_1, ] 98 | ''' 99 | coords = [] 100 | for path_vert in path: 101 | i = (int(path_vert / J)) 102 | j = (path_vert % J) 103 | coords.append([i, j]) 104 | 105 | return coords 106 | 107 | 108 | # def compute_path_divergence(start_pnt, pnts): 109 | # '''Returns the divergence [rad] between a path and a set of paths. Each path 110 | # is represented by points. 111 | # 112 | # Divergence means the angle spanned by the path direction relative to single 113 | # point. The path direction is represented by a path coordinate ahead of the 114 | # starting point. 115 | # 116 | # Example: 117 | # start_pnt = path_1[i] 118 | # pnts = [ path_1[i+5], path_2[i+5], path_3[i+5] ] 119 | # 120 | # Args: 121 | # start_pnt (tuple): Single point representing path center (i,j). 122 | # pnts (list): List of tuples representing future direction of paths. 123 | # 124 | # Returns: 125 | # (float): Divergence angle [rad]. 126 | # ''' 127 | # # Convert points to vectors 128 | # vecs = [] 129 | # for pnt in pnts: 130 | # dx = pnt[0] - start_pnt[0] 131 | # dy = pnt[1] - start_pnt[1] 132 | # vec = np.array([dx, dy]) 133 | # vecs.append(vec) 134 | # 135 | # # Order points in counter-clockwise order 136 | # angs = [] 137 | # for vec in vecs: 138 | # ang = angle_between(vec, [1, 0]) 139 | # angs.append(ang) 140 | # 141 | # ordered_angs_idxs = np.argsort(angs) 142 | # 143 | # delta_angs = [] 144 | # # Angle between vectors 145 | # for idx in range(len(ordered_angs_idxs) - 1): 146 | # vec1_idx = ordered_angs_idxs[idx] 147 | # vec2_idx = ordered_angs_idxs[idx + 1] 148 | # 149 | # vec1 = vecs[vec1_idx] 150 | # vec2 = vecs[vec2_idx] 151 | # 152 | # delta_ang = angle_between(vec1, vec2) 153 | # delta_angs.append(delta_ang) 154 | # 155 | # # Angle between last and first vector 156 | # delta_ang = 2. * np.pi - sum(delta_angs) 157 | # delta_angs.append(delta_ang) 158 | # 159 | # div_ang = np.sum(delta_angs) - np.max(delta_angs) 160 | # 161 | # return div_ang 162 | 163 | 164 | # def find_fork_point(path_list, div_ang_threshold, lookahead_idx): 165 | # '''Finds the point where a set of paths diverges. 166 | # 167 | # NOTE: The earliest fork point is the second point. 168 | # 169 | # Args: 170 | # path_list (list): List of lists of point coordinate tuples. 171 | # div_ang_threshold (float): Exceeding this angle [rad] denotes 172 | # diverging paths. 173 | # 174 | # Returns: 175 | # (int) List index, or 'None' for single and non-diverging paths. 176 | # ''' 177 | # N = np.min([len(path) for path in path_list]) 178 | # forking_pnt = None 179 | # for i in range(1, N - lookahead_idx): 180 | # start_pnt = path_list[0][i] 181 | # 182 | # pnts = [pnt[i + lookahead_idx] for pnt in path_list] 183 | # 184 | # div_ang = compute_path_divergence(start_pnt, pnts) 185 | # 186 | # if div_ang > np.pi / 4: 187 | # #forking_pnt = i PREV 188 | # break 189 | # 190 | # forking_pnt = i 191 | # 192 | # return forking_pnt 193 | 194 | 195 | # def unify_entry_paths(path_list, div_ang_threshold, lookahead_idx): 196 | # '''Unifies all path coordinates up to the fork point. 197 | # 198 | # Args: 199 | # path_list (list): List of lists of point coordinate tuples. 200 | # div_ang_threshold (float): Exceeding this angle [rad] denotes 201 | # diverging paths. 202 | # ''' 203 | # if len(path_list) == 1: 204 | # start_pnt = path_list[0][0] 205 | # end_pnt = path_list[0][1] 206 | # entry_path = [start_pnt, end_pnt] 207 | # 208 | # connecting_paths = [path[1:] for path in path_list] 209 | # 210 | # return entry_path, connecting_paths 211 | # 212 | # # Find path until all paths start to diverge 213 | # fork_pnt = find_fork_point(path_list, div_ang_threshold, lookahead_idx) 214 | # 215 | # start_pnt = path_list[0][0] 216 | # if fork_pnt: 217 | # end_pnt = path_list[0][fork_pnt] 218 | # else: 219 | # end_pnt = path_list[0][1] 220 | # 221 | # entry_path = [start_pnt, end_pnt] 222 | # # Replace the entry path with the common path 223 | # connecting_paths = [path[fork_pnt:] for path in path_list] 224 | # 225 | # return entry_path, connecting_paths 226 | 227 | 228 | def comp_entry_exit_pnts(out_sla, 229 | out_da, 230 | lane_width_px, 231 | region_width=2, 232 | p_tresh=0.4): 233 | ''' 234 | Coordinate system: 235 | i 236 | ------------------- 237 | j | 0 1 ... 128 | 238 | | 1 | 239 | | ... | 240 | | 128 | 241 | ------------------- 242 | 243 | Args: 244 | out_sla: 245 | out_da: (C,H,W) Categorical probability distribution of directionality 246 | 247 | Returns: 248 | List of entry points [(i,j), ... ] and exit points [(i,j), ... ] 249 | ''' 250 | # Points from NMS 251 | mask = np.zeros_like(out_sla, dtype=bool) 252 | mask[region_width:-region_width, region_width:-region_width] = True 253 | out_sla_nms = copy.deepcopy(out_sla) 254 | out_sla_nms[mask] = 0 255 | out_sla_nms = dense_nonmax_sup(out_sla_nms, lane_width_px) 256 | 257 | # Compute element in none (0) | in (1) | out (2) state 258 | num_ang_discr = out_da.shape[0] 259 | idx_0_deg = 0 260 | idx_90_deg = num_ang_discr // 4 261 | idx_180_deg = num_ang_discr // 2 262 | idx_270_deg = (3 * num_ang_discr) // 4 263 | 264 | flow_entry = np.zeros((128, 128), dtype=int) 265 | flow_exit = np.zeros_like(flow_entry) 266 | 267 | H, W = out_sla.shape 268 | # Top side (i: 0, j: 0 --> J) 269 | for i in range(0, region_width): 270 | for j in range(0, W): 271 | if out_sla[i, j] == 0: 272 | continue 273 | p_exit = np.sum(out_da[idx_0_deg:idx_180_deg, i, j]) 274 | p_entry = 1 - p_exit 275 | if p_exit > p_tresh: 276 | flow_exit[i, j] = 1 277 | if p_entry > p_tresh: 278 | flow_entry[i, j] = 1 279 | 280 | # Right side (i: 0 --> I, j: J ) 281 | for i in range(0, W): 282 | for j in range(H - region_width, H): 283 | if out_sla[i, j] == 0: 284 | continue 285 | p_entry = np.sum(out_da[idx_90_deg:idx_270_deg, i, j]) 286 | p_exit = 1 - p_entry 287 | if p_exit > p_tresh: 288 | flow_exit[i, j] = 1 289 | if p_entry > p_tresh: 290 | flow_entry[i, j] = 1 291 | 292 | # Bottom side (i: I, j: 0 --> J) 293 | for i in range(W - region_width, W): 294 | for j in range(0, H): 295 | if out_sla[i, j] == 0: 296 | continue 297 | p_entry = np.sum(out_da[idx_0_deg:idx_180_deg, i, j]) 298 | p_exit = 1 - p_entry 299 | if p_exit > p_tresh: 300 | flow_exit[i, j] = 1 301 | if p_entry > p_tresh: 302 | flow_entry[i, j] = 1 303 | 304 | # Left side (i: 0 --> 128, j: 0) 305 | for i in range(0, W): 306 | for j in range(0, region_width): 307 | if out_sla[i, j] == 0: 308 | continue 309 | p_exit = np.sum(out_da[idx_90_deg:idx_270_deg, i, j]) 310 | p_entry = 1 - p_exit 311 | if p_exit > p_tresh: 312 | flow_exit[i, j] = 1 313 | if p_entry > p_tresh: 314 | flow_entry[i, j] = 1 315 | 316 | # Points from NMS 317 | i_idxs, j_idxs = np.where(out_sla_nms > 0.05) 318 | nms_pnts = [pnt for pnt in zip(i_idxs.tolist(), j_idxs.tolist())] 319 | 320 | # Point direction 321 | entry_pnts = [] 322 | exit_pnts = [] 323 | for nms_pnt in nms_pnts: 324 | i, j = nms_pnt 325 | if flow_entry[i, j] == 1: 326 | entry_pnts.append(nms_pnt) 327 | if flow_exit[i, j] == 1: 328 | exit_pnts.append(nms_pnt) 329 | # else: 330 | # entry_pnts.append(nms_pnt) 331 | # exit_pnts.append(nms_pnt) 332 | 333 | # Points from grouping 334 | entry_pnts += discretize_border_regions(flow_entry, 1, entry_pnts) 335 | exit_pnts += discretize_border_regions(flow_exit, 1, exit_pnts) 336 | 337 | # Remove points too far from any SLA prediction 338 | in_sla_region = out_sla > 0.05 339 | # out_sla_grown = (255 * out_sla_grown).astype(np.uint8) 340 | # kernel = np.ones((3, 3), np.uint8) 341 | # out_sla_grown = cv2.dilate(out_sla_grown, kernel, iterations=5) 342 | # out_sla_grown /= 255 343 | entry_pnts_ = [] 344 | for pnt in entry_pnts: 345 | i = pnt[0] 346 | j = pnt[1] 347 | if in_sla_region[i, j]: 348 | entry_pnts_.append(pnt) 349 | entry_pnts = entry_pnts_ 350 | 351 | exit_pnts_ = [] 352 | for pnt in exit_pnts: 353 | i = pnt[0] 354 | j = pnt[1] 355 | if in_sla_region[i, j]: 356 | exit_pnts_.append(pnt) 357 | exit_pnts = exit_pnts_ 358 | 359 | return entry_pnts, exit_pnts 360 | 361 | 362 | def viz_entry_exit_pnts(sla_map, entry_pnts, exit_pnts): 363 | for pnt in entry_pnts: 364 | i = pnt[0] 365 | j = pnt[1] 366 | sla_map[i, j] = -1 367 | for pnt in exit_pnts: 368 | i = pnt[0] 369 | j = pnt[1] 370 | sla_map[i, j] = 2 371 | plt.imshow(sla_map) 372 | plt.show() 373 | 374 | 375 | def viz_weighted_A(sla_map, A, weighted_A, eps=0, scale_factor=10, t=1, l=0.1): 376 | 377 | dim = (128 * scale_factor, 128 * scale_factor) 378 | sla_map_viz = cv2.resize(sla_map, dim, interpolation=cv2.INTER_LINEAR) 379 | sla_map_viz = (255 * sla_map_viz).astype(np.uint8) 380 | sla_map_viz = cv2.applyColorMap(sla_map_viz, cv2.COLORMAP_HOT) 381 | sla_map_viz = cv2.cvtColor(sla_map_viz, cv2.COLOR_BGR2RGB) 382 | 383 | I, J = sla_map.shape 384 | for i in range(0, I, 3): 385 | for j in range(0, J, 3): 386 | 387 | # Skip nodes without SLA 388 | # if sla_map[i, j] < eps: 389 | if sla_map[j, i] <= eps: 390 | continue 391 | 392 | # Node index for current node and surrounding neighbors 393 | node_idx = node_coord2idx(i, j, J) 394 | neigh_idxs = get_neighbor_nodes(node_idx, A) 395 | 396 | # Compute directional adjacency neighbor-by-neighbor 397 | for neigh_idx in neigh_idxs: 398 | 399 | if weighted_A[node_idx, neigh_idx] < np.inf: 400 | 401 | neigh_i, neigh_j = node_idx2coord(neigh_idx, J) 402 | 403 | pnt0 = (i * scale_factor, j * scale_factor) 404 | pnt1 = (neigh_i * scale_factor, neigh_j * scale_factor) 405 | sla_map_viz = cv2.arrowedLine(sla_map_viz, 406 | pnt0, 407 | pnt1, (0, 0, 255), 408 | thickness=t, 409 | tipLength=l) 410 | 411 | plt.imshow(sla_map_viz) 412 | plt.show() 413 | 414 | 415 | def comp_entry_exits(out_sla, out_da, lane_width_px=6): 416 | ''' 417 | Returns: 418 | entry_paths (list): [ [(i,j)_0, None], ... ] 419 | connecting_pnts (list): [ None ] 420 | exit_paths (list): [ [None, (i.j)_1], ... ] 421 | ''' 422 | entry_pnts, exit_pnts = comp_entry_exit_pnts(out_sla, out_da, 423 | lane_width_px) 424 | 425 | # Build DAG 426 | entry_paths = [[(pnt[1], pnt[0]), None] for pnt in entry_pnts] 427 | exit_paths = [[None, (pnt[1], pnt[0])] for pnt in exit_pnts] 428 | 429 | connecting_paths = [None] 430 | 431 | return entry_paths, connecting_paths, exit_paths 432 | 433 | 434 | def comp_graph(out_sla, 435 | out_da, 436 | lane_width_px=6, 437 | div_ang_threshold=np.pi / 8, 438 | lookahead_idx=6, 439 | scale=1.): 440 | ''' 441 | Args: 442 | out_sla: (128,128) Numpy matrices 443 | out_entry: 444 | out_exit: 445 | out_dir_0: 446 | out_dir_1: 447 | out_dir_2: 448 | div_ang_threshold: 449 | lookahead_idx: 450 | 451 | Returns: 452 | entry_paths (list): [ [(i,j)_0, (i.j)_1], ... ] 453 | connecting_pnts (list): [ [(i,j)_0, (i.j)_1], ... ] 454 | exit_paths (list): [ [(i,j)_0, (i.j)_1], ... ] 455 | ''' 456 | ################### 457 | # Preprocessing 458 | ################### 459 | # Smoothen SLA field 460 | # Determine entry and exit points 461 | entry_pnts, exit_pnts = comp_entry_exit_pnts(out_sla, out_da, 462 | lane_width_px) 463 | 464 | # List with (i,j) coordinates as tuples 465 | # NOTE: Origo is bottom left when viewed as plot 466 | # ==> Need to switch coordinates for 'entry' and 'exit' points 467 | # entry_pnts = comp_descrete_entry_points(out_entry, scale) 468 | # exit_pnts = comp_descrete_entry_points(out_exit, scale) 469 | 470 | # Eight-directional connected grid world adjacency matrix 471 | I, J = (128, 128) 472 | A = grid_adj_mat(I, J, "8") 473 | 474 | # out_da = np.roll(out_da, 8, axis=0) 475 | 476 | weighted_A = dsla_weighted_adj_mat(A, out_sla, out_da) 477 | 478 | # out_da_perm = np.zeros_like(out_da) 479 | # out_da_perm[] 480 | 481 | # out_da2 = np.zeros_like(out_da) 482 | # out_da2[26] = 1 483 | # weighted_A = dsla_weighted_adj_mat(A, out_sla, out_da2) 484 | 485 | # viz_weighted_A(out_sla, A, weighted_A) 486 | 487 | ### 488 | entry_paths = [] 489 | connecting_paths = [] 490 | exit_paths = [] 491 | ### 492 | 493 | tree_list = [] 494 | ### 495 | 496 | for entry_pnt in entry_pnts: 497 | # for entry_pnt in [(126, 8)]: 498 | # for entry_pnt in [(0, 20), (22, 125, (127, 73))]: 499 | print(f"Entry point: {entry_pnt}") 500 | 501 | # NOTE: Need to switch coordinates 502 | start_i = entry_pnt[1] 503 | start_j = entry_pnt[0] 504 | start_node_idx = node_coord2idx(start_i, start_j, J) 505 | 506 | path_list = [] 507 | 508 | for exit_pnt in exit_pnts: 509 | # for exit_pnt in [(107, 1)]: 510 | print(f" Search for exit point: {exit_pnt}") 511 | 512 | goal_i = exit_pnt[1] 513 | goal_j = exit_pnt[0] 514 | goal_node_idx = node_coord2idx(goal_i, goal_j, J) 515 | 516 | d_arr, path = search_path(A, weighted_A, start_node_idx, 517 | goal_node_idx, I, J) 518 | 519 | # Skip unreachable goal 520 | if len(path) == 1: 521 | continue 522 | 523 | path = path_idx2coord(path, J) 524 | 525 | path_list.append(path) 526 | 527 | # Skip entry point not connected to any exit point 528 | if len(path_list) == 0: 529 | continue 530 | 531 | # NOTE: SHOULD BE DONE WHILE CHECKING END POINTS TOO 532 | # (OTHERWISE REDUCE TO EARLY AND NOT CONNECT) 533 | entry_path, connecting_paths = unify_entry_paths( 534 | path_list, div_ang_threshold, lookahead_idx) 535 | entry_paths.append(entry_path) 536 | if connecting_paths: 537 | # connecting_paths += connecting_paths_ 538 | tree_list.append(connecting_paths) 539 | 540 | connecting_paths = [] 541 | 542 | # Unify exit paths in all trees 543 | for exit_pnt in exit_pnts: 544 | 545 | # Reverse (i,j) coordinates 546 | exit_pnt = exit_pnt[::-1] 547 | 548 | # For each exit point, find all paths in all trees 549 | # Each tree can only have one such path 550 | exit_path_dicts = [] 551 | for tree_idx in range(len(tree_list)): 552 | 553 | path_list = tree_list[tree_idx] 554 | 555 | for path_idx in range(len(path_list)): 556 | 557 | path = path_list[path_idx] 558 | 559 | #print(exit_pnt, tuple(path[-1]), tuple(path[-1]) == exit_pnt) 560 | if tuple(path[-1]) == exit_pnt: 561 | match_dict = {'tree_idx': tree_idx, 'path_idx': path_idx} 562 | exit_path_dicts.append(match_dict) 563 | 564 | # Collect paths into a path_list 565 | # Reverse paths 566 | # Unify paths 567 | # Reverse paths 568 | # Replace paths 569 | 570 | path_list = [] 571 | for dict_idx in range(len(exit_path_dicts)): 572 | tree_idx = exit_path_dicts[dict_idx]['tree_idx'] 573 | path_idx = exit_path_dicts[dict_idx]['path_idx'] 574 | path = tree_list[tree_idx][path_idx] 575 | path_list.append(path) 576 | 577 | if len(path_list) == 0: 578 | continue 579 | 580 | # Reverse all paths 581 | path_list = [path[::-1] for path in path_list] 582 | 583 | exit_path, connecting_paths_ = unify_entry_paths( 584 | path_list, div_ang_threshold, lookahead_idx) 585 | 586 | # Reverse all paths 587 | exit_path = exit_path[::-1] 588 | connecting_paths_ = [path[::-1] for path in connecting_paths_] 589 | 590 | exit_paths.append(exit_path) 591 | connecting_paths += connecting_paths_ 592 | 593 | # Build DAG 594 | entry_pnts = [path[0] for path in entry_paths] 595 | fork_pnts = [path[1] for path in entry_paths] 596 | join_pnts = [path[0] for path in exit_paths] 597 | exit_pnts = [path[1] for path in exit_paths] 598 | 599 | connecting_pnts = [[path[0], path[-1]] for path in connecting_paths] 600 | 601 | return entry_paths, connecting_pnts, exit_paths 602 | -------------------------------------------------------------------------------- /train.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import cv2 4 | import matplotlib as mpl 5 | 6 | mpl.use('agg') # Must be before pyplot import to avoid memory leak 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | import pytorch_lightning as pl 10 | import torch 11 | from pytorch_lightning.callbacks.model_checkpoint import ModelCheckpoint 12 | 13 | from datamodule_preproc import PreprocBEVDataset 14 | from eval.eval_dir_accuracy import eval_dir_acc 15 | from eval.eval_f1_score import eval_f1_score 16 | from eval.eval_iou import eval_iou 17 | from graph_inference.graph_func import comp_entry_exits 18 | from graph_inference.max_likelihood_graph import find_max_likelihood_graph 19 | from losses.da_model_free_kl_div import loss_da_kl_div 20 | from losses.da_nll import eval_da_nll 21 | from losses.sla_balanced_ce import loss_sla_balanced_ce 22 | from losses.sla_nll import eval_sla_nll 23 | from models.unet_dsla import UnetDSLA 24 | from viz.viz_dense import visualize_dense_softmax, visualize_dir_label 25 | 26 | 27 | class DSLAModel(pl.LightningModule): 28 | ''' 29 | ''' 30 | 31 | def __init__( 32 | self, 33 | sla_alpha, 34 | lr, 35 | weight_decay, 36 | base_channels, 37 | enc_str, 38 | sla_dec_str, 39 | da_dec_str, 40 | dropout_prob, 41 | print_interval, 42 | checkpoint_interval, 43 | batch_size, 44 | input_ch, 45 | out_feat_ch, 46 | num_angs, 47 | sla_head_layers, 48 | da_head_layers, 49 | num_workers, 50 | train_data_dir, 51 | val_data_dir, 52 | test_data_dir, 53 | viz_size_per_fig, 54 | viz_dir, 55 | optimizer, 56 | test_batch_idxs, 57 | test_start_batch_idx, 58 | viz_size, 59 | output_test_dir, 60 | **kwargs, 61 | ): 62 | super().__init__() 63 | 64 | self.save_hyperparameters() 65 | 66 | self.sla_alpha = sla_alpha 67 | self.lr = lr 68 | self.weight_decay = weight_decay 69 | self.base_channels = base_channels 70 | self.dropout_prob = dropout_prob 71 | self.print_interval = print_interval 72 | self.checkpoint_interval = checkpoint_interval 73 | self.batch_size = batch_size 74 | self.num_workers = num_workers 75 | self.train_data_dir = train_data_dir 76 | self.val_data_dir = val_data_dir 77 | self.test_data_dir = test_data_dir 78 | self.viz_size_per_fig = viz_size_per_fig 79 | self.optimizer = optimizer 80 | if test_batch_idxs is not None: 81 | self.test_batch_idxs = test_batch_idxs 82 | else: 83 | self.test_batch_idxs = [] 84 | self.test_start_batch_idx = test_start_batch_idx 85 | self.viz_size = viz_size 86 | self.output_test_dir = output_test_dir 87 | 88 | # Set of choosen samples for visualization 89 | self.viz_dataset = PreprocBEVDataset(viz_dir, get_gt_labels=True) 90 | 91 | ########### 92 | # Model 93 | ########### 94 | self.model = UnetDSLA(enc_str, sla_dec_str, da_dec_str, input_ch, 95 | out_feat_ch, num_angs) 96 | 97 | def configure_optimizers(self): 98 | if self.optimizer == 'adam': 99 | optimizer = torch.optim.Adam(self.parameters(), 100 | lr=self.lr, 101 | weight_decay=self.weight_decay) 102 | elif self.optimizer == 'sgd': 103 | optimizer = torch.optim.SGD(self.parameters(), 104 | lr=self.lr, 105 | weight_decay=self.weight_decay) 106 | else: 107 | raise IOError(f'Invalid optimizer ({self.optimizer})') 108 | 109 | power = 0.9 110 | scheduler = torch.optim.lr_scheduler.PolynomialLR( 111 | optimizer, self.trainer.max_epochs, power) 112 | return [optimizer], [scheduler] 113 | 114 | def dsla_objective(self, output, traj_label, ang_label, drivable): 115 | ''' Computes a multi-objective loss to traing a model 116 | 117 | Output tensor shape: (minibatch_idx, output layers, n, n) 118 | 119 | Output layers: 120 | -------------------------------------------------- 121 | [0] Soft lane affordance (1 layer) 122 | -------------------------------------------------- 123 | [1] Directional mean 1 (3 layers) # 6 layers 124 | [2] Directional mean 2 125 | [3] Directional mean 3 126 | -------------------------------------------------- 127 | [4] Directional var 1 (3 layers) 128 | [5] Directional var 2 129 | [6] Directional var 2 130 | -------------------------------------------------- 131 | [7] Directional weight 1 (3 layers) 132 | [8] Directional weight 2 133 | [9] Directional weight 3 134 | -------------------------------------------------- 135 | ''' 136 | output_sla = output[:, 0:1] 137 | output_da = output[:, 1:37] 138 | 139 | # Remove non-road elements 140 | mask = (drivable == 0) 141 | # Compute drivable elements for each batch 142 | drivable_N = torch.sum(~mask, dim=(1, 2, 3), keepdim=True) 143 | 144 | # Soft Lane Affordance loss [batch_n, 1, n, n] 145 | loss_sla, loss_l1, loss_ce = loss_sla_balanced_ce( 146 | output_sla, traj_label, self.sla_alpha, drivable_N) 147 | 148 | # Directional Affordance loss 149 | loss_da = loss_da_kl_div(output_da, ang_label) 150 | 151 | loss = 300 * loss_sla + loss_da 152 | 153 | return loss, loss_sla.item(), loss_da.item(), loss_l1, loss_ce 154 | 155 | def eval_objective(self, output, traj_label, ang_label, drivable): 156 | ''' 157 | ''' 158 | output_sla = output[:, 0:1] 159 | output_da = output[:, 1:37] 160 | 161 | mask = (drivable == 0) 162 | drivable_N = torch.sum(~mask, dim=(1, 2, 3)) 163 | 164 | sla_nll = eval_sla_nll(output_sla, traj_label, drivable_N) 165 | da_nll = eval_da_nll(output_da, ang_label) 166 | 167 | return sla_nll.item(), da_nll.item() 168 | 169 | def forward(self, x): 170 | y = self.model.forward(x) 171 | return y 172 | 173 | def training_step(self, batch, batch_idx): 174 | 175 | input, labels = batch 176 | 177 | traj_label = labels['traj_full'] 178 | mm_ang_tensor = labels['mm_ang_full_tensor'] 179 | drivable = labels['drivable'] 180 | 181 | output_tensor = self.forward(input) 182 | 183 | lst = self.dsla_objective( 184 | output_tensor, 185 | traj_label, 186 | mm_ang_tensor, 187 | drivable, 188 | ) 189 | loss, loss_sla, loss_da, loss_sla_l1, loss_sla_ce = lst 190 | 191 | self.log_dict({ 192 | 'lr': self.optimizers().param_groups[0]["lr"], 193 | 'train_loss': loss, 194 | 'train_loss_sla': loss_sla, 195 | 'train_loss_da': loss_da, 196 | 'train_loss_sla_l1': loss_sla_l1, 197 | 'train_loss_sla_ce': loss_sla_ce, 198 | }) 199 | 200 | return loss 201 | 202 | def validation_step(self, batch, batch_idx): 203 | 204 | input, labels = batch 205 | 206 | traj_label = labels['gt_lanes'] 207 | mm_ang_tensor = labels['mm_gt_angs_tensor'] 208 | drivable = labels['drivable'] 209 | 210 | output = self.forward(input) 211 | 212 | sla_nll, da_nll = self.eval_objective( 213 | output, 214 | traj_label, 215 | mm_ang_tensor, 216 | drivable, 217 | ) 218 | 219 | self.log_dict({ 220 | 'val_sla_nll': sla_nll, 221 | 'val_da_nll': da_nll, 222 | }, 223 | sync_dist=True) 224 | 225 | def validation_epoch_end(self, val_step_outputs): 226 | 227 | # Load a static set of visualization examples 228 | vizs = [] 229 | num_samples = len(self.viz_dataset) 230 | for sample_idx in range(num_samples): 231 | input, label = self.viz_dataset[sample_idx] 232 | viz, _, _, _ = self.viz_output(input, label) 233 | vizs.append(viz) 234 | 235 | # Arrange viz side-by-side 236 | vizs = np.concatenate(vizs, axis=1) 237 | 238 | plt.figure(figsize=((num_samples * self.viz_size_per_fig, 239 | self.viz_size_per_fig))) 240 | plt.imshow(vizs) 241 | plt.tight_layout() 242 | 243 | self.logger.experiment.add_figure('viz', 244 | plt.gcf(), 245 | global_step=self.current_epoch) 246 | 247 | def test_step(self, batch, batch_idx): 248 | ''' 249 | ''' 250 | input, label = batch 251 | 252 | if input.shape[0] != 1: 253 | raise IOError('Test function requires batch size = 1') 254 | 255 | # Skip batches before starting idx 256 | if batch_idx < self.test_start_batch_idx: 257 | return 258 | 259 | # Skip unlisted batches if any are specified 260 | if len(self.test_batch_idxs) != 0: 261 | if batch_idx not in self.test_batch_idxs: 262 | return 263 | 264 | if not os.path.isdir(self.output_test_dir): 265 | os.makedirs(self.output_test_dir) 266 | 267 | filename = f'viz_{str(batch_idx).zfill(3)}.png' 268 | filepath = os.path.join(self.output_test_dir, filename) 269 | 270 | ################################################ 271 | # Negative log likelihood evaluation metrics 272 | ################################################ 273 | traj_label = label['gt_lanes'] 274 | mm_ang_tensor = label['mm_gt_angs_tensor'] 275 | drivable = label['drivable'] 276 | 277 | output = self.forward(input) 278 | 279 | sla_nll, da_nll = self.eval_objective( 280 | output, 281 | traj_label, 282 | mm_ang_tensor, 283 | drivable, 284 | ) 285 | 286 | ################################################# 287 | # Maximum likelihood graph evaluation metrics 288 | ################################################# 289 | # Remove batch indices 290 | input_no_b = input[0] 291 | label_no_b = {key: value[0] for key, value in zip(label.keys(), 292 | label.values())} 293 | rgb_viz, iou, f1_score, dir_acc = self.viz_output(input_no_b, 294 | label_no_b, 295 | do_graph=True) 296 | plt.figure(figsize=((self.viz_size, self.viz_size))) 297 | plt.imshow(rgb_viz) 298 | plt.tight_layout() 299 | plt.savefig(filepath) 300 | 301 | # iou = 0. 302 | # f1_score = 0. 303 | # dir_acc = 0. 304 | 305 | eval_file = os.path.join(self.output_test_dir, 'eval.txt') 306 | if os.path.isfile(eval_file): 307 | mode = 'a' 308 | else: 309 | mode = 'w' 310 | with open(eval_file, mode) as f: 311 | f.write( 312 | f'{batch_idx},{sla_nll},{da_nll},{iou},{f1_score},{dir_acc}\n') 313 | 314 | return sla_nll, da_nll, iou, f1_score, dir_acc 315 | 316 | def test_epoch_end(self, test_step_outputs): 317 | 318 | sla_nlls = [] 319 | da_nlls = [] 320 | ious = [] 321 | f1_scores = [] 322 | dir_accs = [] 323 | for out in test_step_outputs: 324 | sla_nll, da_nll, iou, f1_score, dir_acc = out 325 | sla_nlls.append(sla_nll) 326 | da_nlls.append(da_nll) 327 | ious.append(iou) 328 | f1_scores.append(f1_score) 329 | dir_accs.append(dir_acc) 330 | 331 | sla_nll_mean = np.mean(sla_nlls) 332 | da_nll_mean = np.mean(da_nlls) 333 | iou_mean = np.mean(ious) 334 | f1_scores_mean = np.mean(f1_scores) 335 | dir_accs_mean = np.mean(dir_accs) 336 | 337 | print('') 338 | print('\nEvaluation result') 339 | print(f'\tsla_nll_mean: {sla_nll_mean:.3f}') 340 | print(f'\tda_nll_mean: {da_nll_mean:.3f}') 341 | print(f'\tiou_mean: {iou_mean:.3f}') 342 | print(f'\tf1_scores_mean: {f1_scores_mean:.3f}') 343 | print(f'\tdir_accs_mean: {dir_accs_mean:.3f}') 344 | print('') 345 | 346 | def viz_output(self, 347 | input: torch.tensor, 348 | label: dict, 349 | do_graph: bool = False, 350 | use_cuda: bool = True) -> np.array: 351 | ''' 352 | Args: 353 | input: (5, 256, 256) 354 | label: Dict with tensor 355 | do_graph: Overlay inferred graph if True 356 | 357 | Returns: 358 | RGB image (1280,1280,3) 359 | ''' 360 | gt_lanes = label['gt_lanes'] 361 | mm_gt_angs_tensor = label['mm_gt_angs_tensor'] 362 | drivable = label['drivable'] 363 | 364 | input = input.unsqueeze(0) 365 | if use_cuda: 366 | input = input.cuda() 367 | with torch.no_grad(): 368 | output = self.forward(input) 369 | output = output[0].cpu().numpy() 370 | input = input[0].cpu().numpy() 371 | mm_gt_angs_tensor = mm_gt_angs_tensor.cpu() # [0] 372 | drivable = drivable[0].cpu() 373 | 374 | output_sla = output[0:1] 375 | output_da = output[1:37] 376 | 377 | # Remove non-drivable region 378 | mask = (drivable == 1).numpy() 379 | output_sla[0][mask == 0] = 0.0 380 | 381 | # Dense visualization 382 | drivable_in = input[0] 383 | markings = input[1] 384 | context = 0.1 * drivable_in + 0.9 * markings 385 | context = (255 * context).astype(np.int8) 386 | rgb_viz = visualize_dense_softmax(context, output_sla[0], output_da, 387 | None) 388 | 389 | # Overlay direction label (NOT predicted direction) 390 | rgb_viz = visualize_dir_label(rgb_viz, mm_gt_angs_tensor) 391 | 392 | # For skipping RGB visualization (to save time) 393 | # rgb_viz = np.zeros((1280, 1280, 3), dtype=np.uint8) 394 | 395 | # Overlay inferred road lane network graph 396 | if do_graph: 397 | # Downscale images for search function 398 | ds_size = 128 399 | out_sla_ds = cv2.resize(output_sla[0], (ds_size, ds_size), 400 | interpolation=cv2.INTER_LINEAR) 401 | num_dirs = output_da.shape[0] 402 | out_da_ds = np.zeros((num_dirs, ds_size, ds_size)) 403 | for dir_n in range(num_dirs): 404 | out_da_ds[dir_n] = cv2.resize(output_da[dir_n], 405 | (ds_size, ds_size), 406 | interpolation=cv2.INTER_NEAREST) 407 | # Normalize prob values 408 | out_da_ds /= np.sum(out_da_ds, axis=(0)) 409 | entry_paths, connecting_pnts, exit_paths = comp_entry_exits( 410 | out_sla_ds, out_da_ds) 411 | 412 | paths = find_max_likelihood_graph(output_sla[0], 413 | output_da, 414 | entry_paths, 415 | connecting_pnts, 416 | exit_paths, 417 | num_samples=1000, 418 | num_pnts=100) 419 | 420 | ########################## 421 | # Numerical evaluation 422 | ########################## 423 | iou = eval_iou(paths, gt_lanes[0, 0].cpu().numpy()) 424 | f1_score = eval_f1_score(paths, gt_lanes[0, 0].cpu().numpy(), 425 | drivable[0].numpy()) 426 | dir_acc = eval_dir_acc(output_da, mm_gt_angs_tensor.numpy()) 427 | 428 | # Transform path coordinates to image frame 429 | # 10 x (256 --> 128) 430 | scale_factor = 10 431 | paths = [scale_factor * (path // 2) for path in paths] 432 | 433 | t = 3 434 | l = 0.5 435 | color = (0, 102, 204) 436 | 437 | for path in paths: 438 | # pnts = np.expand_dims(path, 1) # (N, 1, 2) 439 | pnts = path.astype(np.int32) 440 | pnts = pnts.reshape((-1, 1, 2)) 441 | rgb_viz = cv2.polylines(rgb_viz, [pnts], 442 | isClosed=False, 443 | color=color, 444 | thickness=t) 445 | rgb_viz = cv2.arrowedLine(rgb_viz, 446 | pnts[-2, 0], 447 | pnts[-1, 0], 448 | color=color, 449 | thickness=t, 450 | tipLength=l) 451 | else: 452 | iou = None 453 | f1_score = None 454 | dir_acc = None 455 | 456 | return rgb_viz, iou, f1_score, dir_acc 457 | 458 | @staticmethod 459 | def add_model_specific_args(parent_parser): 460 | parser = parent_parser.add_argument_group('DSLAModel') 461 | parser.add_argument('--sla_alpha', type=float, default=1.) 462 | parser.add_argument('--lr', type=float, default=1e-3) 463 | parser.add_argument('--weight_decay', type=float, default=1e-4) 464 | parser.add_argument( 465 | '--enc_str', 466 | type=str, 467 | default='2x32,2x32,2x64,2x64,2x128,2x128,2x256,2x256') 468 | parser.add_argument( 469 | '--sla_dec_str', 470 | type=str, 471 | default='2x256,2x256,2x128,2x128,2x64,2x64,2x32,2x32') 472 | parser.add_argument( 473 | '--da_dec_str', 474 | type=str, 475 | default='2x256,2x256,2x128,2x128,2x64,2x64,2x32,2x32') 476 | parser.add_argument('--base_channels', type=int, default=64) 477 | parser.add_argument('--dropout_prob', type=float, default=0) 478 | parser.add_argument('--print_interval', type=int, default=100) 479 | parser.add_argument('--checkpoint_interval', type=int, default=1000) 480 | parser.add_argument('--batch_size', type=int, default=2) 481 | parser.add_argument('--input_ch', type=int, default=5) 482 | parser.add_argument('--out_feat_ch', type=int, default=512) 483 | parser.add_argument('--num_angs', type=int, default=32) 484 | parser.add_argument('--sla_head_layers', type=int, default=3) 485 | parser.add_argument('--da_head_layers', type=int, default=3) 486 | parser.add_argument('--viz_size_per_fig', type=int, default=12) 487 | parser.add_argument('--viz_dir', type=str) 488 | parser.add_argument('--optimizer', 489 | type=str, 490 | default='adam', 491 | help='adam|sgd') 492 | parser.add_argument('--test_batch_idxs', 493 | type=int, 494 | nargs='+', 495 | help='11 12 etc') 496 | parser.add_argument('--test_start_batch_idx', type=int, default=0) 497 | parser.add_argument('--viz_size', 498 | type=int, 499 | default=12, 500 | help='Size of output viz image') 501 | parser.add_argument('--output_test_dir', type=str, default='.') 502 | return parent_parser 503 | 504 | if __name__ == '__main__': 505 | from argparse import ArgumentParser, BooleanOptionalAction 506 | 507 | from datamodule_preproc import BEVDataPreprocModule 508 | 509 | torch.set_float32_matmul_precision('high') 510 | 511 | parser = ArgumentParser() 512 | parser.add_argument('--train_data_dir', type=str) 513 | parser.add_argument('--val_data_dir', type=str) 514 | parser.add_argument('--test_data_dir', type=str) 515 | parser.add_argument('--num_workers', type=int, default=0) 516 | parser.add_argument('--do_augmentation', action=BooleanOptionalAction) 517 | parser.add_argument('--checkpoint_path', type=str, default=None) 518 | parser.add_argument('--do_test', action=BooleanOptionalAction) 519 | # Add program level args 520 | # Add model speficic args 521 | parser = DSLAModel.add_model_specific_args(parser) 522 | # Add all the vailable trainer option to argparse 523 | parser = pl.Trainer.add_argparse_args(parser) 524 | args = parser.parse_args() 525 | 526 | dict_args = vars(args) 527 | model = DSLAModel(**dict_args) 528 | 529 | if args.checkpoint_path is not None: 530 | checkpoint = torch.load(args.checkpoint_path) 531 | model.load_state_dict(checkpoint['state_dict'], strict=False) 532 | 533 | # To save every checkpoint 534 | checkpoint_callback = ModelCheckpoint( 535 | save_top_k=-1, 536 | monitor="val_sla_nll", 537 | filename="checkpoint_{epoch:02d}", 538 | ) 539 | # Ref: https://github.com/Lightning-AI/lightning/issues/3648 540 | trainer = pl.Trainer.from_argparse_args(args, 541 | callbacks=[checkpoint_callback]) 542 | 543 | datamodule = BEVDataPreprocModule( 544 | train_data_dir=args.train_data_dir, 545 | val_data_dir=args.val_data_dir, 546 | test_data_dir=args.test_data_dir, 547 | batch_size=args.batch_size, 548 | do_rotation=False, 549 | do_aug=args.do_augmentation, 550 | num_workers=args.num_workers, 551 | ) 552 | 553 | if args.do_test: 554 | trainer.test(model, datamodule=datamodule) 555 | else: 556 | trainer.fit(model, datamodule=datamodule) 557 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | --------------------------------------------------------------------------------