├── README.md ├── benchmark_all_handcrafted_agents.py ├── benchmark_handcrafted_agent.py ├── data ├── human_run_logs │ ├── human_mp3d.tar.gz │ ├── mp3d_m │ │ └── short_stats.csv │ ├── mp3d_maction_trace.csv │ ├── parse_action_trace_log.ipynb │ ├── pointgoal_mp3d_m_goals_dist_dir_Human │ │ ├── action_trace.csv │ │ └── short_stats.csv │ ├── short_stats.csv │ ├── short_stats_human_mp3d_new.csv │ ├── suncg_me_action_trace.csv │ └── suncg_mf_action_trace.csv ├── mp3d3_small1k.yaml └── mp3d3_small5k.yaml ├── examples └── BasicFunctionality.ipynb ├── install_minos_and_deps.sh └── navigation ├── ClassicAgents.py ├── Control.py ├── LearnedAgents.py ├── LoggerBench.py ├── Mappers.py ├── Metrics.py ├── PyTorchPathPlanners.py ├── Reprojection.py ├── Utils.py ├── __init__.py └── visualizingUtils.py /README.md: -------------------------------------------------------------------------------- 1 | ### Code for the paper "Benchmarking Classic and Learned Navigation in Complex 3D Environments" 2 | 3 | Project website: https://sites.google.com/view/classic-vs-learned-navigation 4 | 5 | Video: https://www.youtube.com/watch?v=b1S5ZbOAchc 6 | 7 | Paper: https://arxiv.org/abs/1901.10915 8 | 9 | If you use this code or the provided environments in your research, please cite the following: 10 | 11 | @ARTICLE{Navigation2019, 12 | author = {{Mishkin}, Dmytro and {Dosovitskiy}, Alexey and {Koltun}, Vladlen}, 13 | title = "{Benchmarking Classic and Learned Navigation in Complex 3D Environments}", 14 | year = 2019, 15 | month = Jan, 16 | archivePrefix = {arXiv}, 17 | eprint = {1901.10915}, 18 | } 19 | 20 | 21 | 22 | ## Dependencies: 23 | 24 | - minos 25 | - numpy 26 | - pytorch 27 | - ORBSLAM2 28 | 29 | 30 | ## Tested with: 31 | - Ubuntu 16.04 32 | - python 3.6 33 | - pytorch 0.4, 1.0 34 | 35 | 36 | ## Benchmark 37 | 38 | 39 | - Install Anaconda https://www.anaconda.com/download/#linux 40 | 41 | - Install dependencies via ./install_minos_and_deps.sh. It should install everything except the datasets. 42 | 43 | - Obtain mp3d and suncg datasets as described here in (1) https://github.com/minosworld/minos/blob/master/README.md#installing 44 | 45 | - For benchmark handcrafted agents, run following 46 | 47 | source activate NavigationBenchmarkMinos 48 | export CUDA_VISIBLE_DEVICES=0 49 | python -utt benchmark_all_handcrafted_agents.py 50 | 51 | You may want to comment/uncomment needed agents and/or environments if need to reproduce only part of them. 52 | Agents contain random parts: RANSAC in ORBSLAM and 10% random actions in all agents. Nevertheless, results should be the same if run on the same PC. From machine to machine, results may differ (slightly) 53 | 54 | You may also want to turn on recording of videos (RGB, depth, GT map, map, beliefs) by setting VIDEO=True in [benchmark_all_handcrafted_agents.py](benchmark_all_handcrafted_agents.py). 55 | 56 | Simple example of working with agents is shown in [example](examples/BasicFunctionality.ipynb). 57 | 58 | ## Training 59 | 60 | Training and pre-trained weights for the learned agents are coming soon. 61 | 62 | -------------------------------------------------------------------------------- /benchmark_all_handcrafted_agents.py: -------------------------------------------------------------------------------- 1 | import os 2 | import shutil 3 | import time 4 | import subprocess 5 | from os.path import expanduser 6 | from shutil import copyfile 7 | 8 | n_runs =10 9 | LOGDIR = 'logs/benchmark_classic/' 10 | VIDEO=False 11 | 12 | def gettimestr(): 13 | return time.strftime("%Y-%m-%d--%H_%M_%S", time.gmtime()) 14 | try: 15 | os.makedirs(LOGDIR) 16 | except: 17 | pass 18 | agents = [ 'Blind','ClassicRGBD', 'ClassicRGB', 'ClassicGTPose', 'ClassicGTMapGTPose'] 19 | 20 | envs = ["pointgoal_mp3d_m", 21 | 'pointgoal_suncg_mf', 22 | 'pointgoal_suncg_me'] 23 | 24 | for env in envs: 25 | for ag in agents: 26 | print ("Testing", ag, "in", env) 27 | t=time.time() 28 | if "mp3d" in env: 29 | n_runs = 20 30 | else: 31 | n_runs = 10 32 | run_string = 'python3 -utt benchmark_handcrafted_agent.py --env_config ' + env + ' --logdir-prefix=' + LOGDIR + " --save-video=" + str(VIDEO) + ' --agent-type=' + ag + ' --episode-schedule=test --num-episodes-per-scene=' + str(n_runs) + ' 2>&1 | tee ' + LOGDIR+ag+env+ "_" + gettimestr()+ '_verblog.log' 33 | subprocess.call(run_string,shell=True) 34 | print ('done in', time.time() - t, 'sec') -------------------------------------------------------------------------------- /benchmark_handcrafted_agent.py: -------------------------------------------------------------------------------- 1 | def PatchNumpy(): 2 | # https://github.com/Marcello-Sega/pytim/blob/f16643653d8415038dbeb52f580e353f255cda6e/pytim/patches.py 3 | # this try/except block patches numpy and provides _validate_lengths 4 | # to skimage<=1.14.1 5 | import numpy 6 | try: 7 | numpy.lib.arraypad._validate_lengths 8 | except AttributeError: 9 | def patch_validate_lengths(ar, crop_width): 10 | return numpy.lib.arraypad._as_pairs(crop_width, ar.ndim, as_index=True) 11 | numpy.lib.arraypad._validate_lengths = patch_validate_lengths 12 | PatchNumpy() 13 | import sys 14 | import os.path 15 | import time 16 | import gc 17 | from time import sleep 18 | from copy import deepcopy 19 | import numpy as np 20 | import argparse 21 | import imageio 22 | from PIL import Image 23 | import random 24 | 25 | import gym 26 | import gym_minos 27 | from minos.config.sim_args import parse_sim_args 28 | 29 | from navigation.Utils import gettimestr, gettimestr 30 | from navigation.visualizingUtils import VideoLogWriter 31 | from navigation.ClassicAgents import * 32 | from navigation.Metrics import calcSPL 33 | from navigation.LoggerBench import TestLogger 34 | 35 | 36 | def experiment_name(agent, args): 37 | return '_'.join([args['env_config'], 38 | args['episode_schedule'], 39 | args['exp_name'], 40 | args['agent_type']]) 41 | 42 | parser = argparse.ArgumentParser(description='MINOS gym benchmark') 43 | 44 | parser.add_argument('--agent-type', type=str, 45 | default="Random", 46 | help='Random, Blind, ClassicRGB, ClassicRGBD, ClassicGTPose, ClassicGTMapGTPose')#Logging 47 | parser.add_argument('--logdir-prefix', type=str, 48 | default='LOGS/', 49 | help='init driving net from pretrained') 50 | parser.add_argument('--save-video', type=str2bool, 51 | default=False, 52 | help='') 53 | parser.add_argument('--exp-name', type=str, 54 | default='', 55 | help='Experiment name') 56 | parser.add_argument('--timing', type=str2bool, 57 | default=False, 58 | help='show time for steps') 59 | # Benchmark opts 60 | parser.add_argument('--start-from-episode', type=int, 61 | default=-1, 62 | help='skip this number of episodes') 63 | parser.add_argument('--num-episodes-per-scene', type=int, 64 | default=10, 65 | help='Number of diffent random start and end locations') 66 | parser.add_argument('--episode-schedule', type=str, 67 | default='val', 68 | help='Split to use. Possible are: train, val, test. Paper reports results on test') 69 | parser.add_argument('--seed', type=int, 70 | default=42, 71 | help='Random seed') 72 | 73 | 74 | args = parse_sim_args(parser) 75 | opts = defaultAgentOptions(args) 76 | 77 | #For storing video and logs 78 | #get map and depth anyway for video storing. 79 | #Agent has its own setup 80 | args['observations']['map'] = True 81 | args['observations']['depth'] = True 82 | args['log_action_trace'] = True 83 | 84 | 85 | RANDOM_SEED = args['seed'] 86 | 87 | random.seed(RANDOM_SEED) 88 | torch.manual_seed(RANDOM_SEED) 89 | np.random.seed(RANDOM_SEED) 90 | 91 | 92 | if __name__ == "__main__": 93 | draw_map = False 94 | if args['agent_type'] == 'Random': 95 | agent = RandomAgent(**opts) 96 | elif args['agent_type'] == 'Blind': 97 | agent = BlindAgent(**opts) 98 | elif args['agent_type'] == 'ClassicRGB': 99 | draw_map = True 100 | agent = ClassicAgentRGB(**opts) 101 | elif args['agent_type'] == 'ClassicRGBD': 102 | draw_map = True 103 | agent = ClassicAgentWithDepth(**opts) 104 | elif args['agent_type'] == 'ClassicGTPose': 105 | draw_map = True 106 | agent = CheatingAgentWithGTPose(**opts) 107 | elif args['agent_type'] == 'ClassicGTMapGTPose': 108 | draw_map = True 109 | agent = CheatingAgentWithGTPoseAndMap(**opts) 110 | else: 111 | raise ValueError(args['agent_type'] + ' inknown type of agent. Try Random, Blind, ClassicRGB, ClassicRGBD, ClassicGTPose, ClassicGTMapGTPose') 112 | logging_agent = LoggingAgent(**opts) 113 | del opts['device'] 114 | out_dir_name = os.path.join(args['logdir_prefix'], experiment_name(agent, args) + gettimestr()) 115 | if not os.path.isdir(out_dir_name): 116 | os.makedirs(out_dir_name) 117 | logger1 = TestLogger(out_dir_name) 118 | logger1.add_init_args(opts) 119 | env = gym.make('indoor-v0') 120 | env.configure(opts) 121 | logger1.add_sim_params(env.simulator.params) 122 | 123 | num_good = 0 124 | num_failed_map_inits = 0 125 | num_episodes = env._sim.get_episode_scheduler(env._sim.curr_schedule).num_states() * args['num_episodes_per_scene'] 126 | episodes_done = 0 127 | i_episode=0 128 | shortest_paths_length = [] 129 | real_paths_length = [] 130 | successes = [] 131 | coefs = [] 132 | times = [] 133 | SPL = 0.0 134 | for epps in range(num_episodes): 135 | print('Starting episode %d' % i_episode, 'out of', num_episodes) 136 | env.simulator.move_to(tilt=0) 137 | observation = env.reset() 138 | counter1 = 0 139 | #Skip episodes, which has no shortest_path_to_goal, i.e are path is untraversable. 140 | while not 'distance' in observation['observation']['measurements']['shortest_path_to_goal'].keys(): 141 | print ('Warning, no shortest path exists, trying to reset.') 142 | #print (observation) 143 | observation = env.reset() 144 | counter1+=1 145 | if counter1 > 10: 146 | action = np.zeros(8) 147 | observation, reward, done, info = env.step(action) 148 | print ('Skipping episode, because there is no shortest path for it') 149 | counter1 = 0 150 | if (opts['start_from_episode'] > 0): 151 | while i_episode < opts['run_specific']: 152 | action = np.zeros(8) 153 | observation = env.reset() 154 | observation, reward, done, info = env.step(action) 155 | i_episode+=1 156 | #Reinit map size for agent. It does not affect results, but saves memory for small maps. 157 | global_map_GT = observation['observation']['map']['data'][:,:,:3].astype(np.uint8) 158 | h = observation['observation']['map']['height'] 159 | w = observation['observation']['map']['width'] 160 | map_size_in_meters = findMapSize(h,w) 161 | agent.map_size_meters = map_size_in_meters 162 | logging_agent.map_size_meters = map_size_in_meters 163 | try: 164 | agent.mapper.map_size_meters = agent.map_size_meters 165 | except: 166 | #If agent does not have mapper, e.g. RandomAgent 167 | pass 168 | map_in_cells = getMapSizeInCells(agent.map_size_meters, opts['map_cell_size']) 169 | agent.reset() 170 | logging_agent.reset() 171 | gt_shortest_path = float(observation['observation']['measurements']['shortest_path_to_goal']['distance']) 172 | init_dist = observation['observation']['measurements']['distance_to_goal'][0] 173 | print ("MINOS shortest path = ", gt_shortest_path) 174 | shortest_paths_length.append(gt_shortest_path) 175 | done = False 176 | num_steps = 0 177 | if opts['save_video']: 178 | ep_base_fname = os.path.join(out_dir_name, str(i_episode)) 179 | VIDEO_WRITER = VideoLogWriter(ep_base_fname, map_size_in_cells = map_in_cells, draw_map = draw_map) 180 | action = None 181 | while not done: 182 | tt=time.time() 183 | with torch.no_grad(): 184 | action, agent_thinks_its_done = agent.act(observation) 185 | dummy = logging_agent.act(observation) 186 | #### Drawing 187 | t=time.time() 188 | if opts['save_video']: 189 | VIDEO_WRITER.add_frame(observation, agent, logging_agent) 190 | #### End of Drawing 191 | if opts['timing']: 192 | print (time.time() - t, ' s video save') 193 | t=time.time() 194 | observation, reward, env_success, info = env.step(action) 195 | if opts['timing']: 196 | print (time.time() - t, ' s MINOS step') 197 | num_steps += 1 198 | if num_steps % 20 == 0: 199 | print (num_steps, time.time() - tt, ' s per step') 200 | done = (env_success or (num_steps >= 500) or agent_thinks_its_done) 201 | if done: 202 | real_path = logging_agent.travelledSoFarForEval 203 | real_paths_length.append(real_path) 204 | times.append(float(num_steps)) 205 | successes.append(float(observation['success'])) 206 | coef = gt_shortest_path / (max(gt_shortest_path, real_path)) 207 | coefs.append(coef) 208 | print("Episode finished after {} steps; success={}, travelled path = {}, path to shortest path = {}".format(num_steps, observation['success'], real_path, coef)) 209 | print ("SPL = ", calcSPL(coefs, successes)) 210 | if observation['success']: 211 | num_good+=1 212 | episodes_done+=1 213 | if opts['save_video']: 214 | VIDEO_WRITER.finish() 215 | agent.reset() 216 | logging_agent.reset() 217 | print (episodes_done , "done", num_good, "success") 218 | print ('success rate = ', float(num_good)/float(episodes_done), 219 | 'average num_steps =', np.array(times).mean()) 220 | i_episode+=1 221 | logger1.add_episode_stats(epps, 222 | env._sim.scene_id, 223 | real_path, 224 | gt_shortest_path, 225 | num_steps, 226 | observation['success'], h, w, init_dist ) 227 | logger1.save() 228 | logger1.save_csv() 229 | print ('Final SPL=',calcSPL(coefs, successes), 230 | 'success rate = ', float(num_good)/float(episodes_done), 231 | 'average num_steps =', np.array(times).mean()) 232 | -------------------------------------------------------------------------------- /data/human_run_logs/human_mp3d.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ducha-aiki/navigation-benchmark/d83431d6648ac1147f53056ed32ce2caae4f702d/data/human_run_logs/human_mp3d.tar.gz -------------------------------------------------------------------------------- /data/human_run_logs/mp3d_m/short_stats.csv: -------------------------------------------------------------------------------- 1 | episode_id,h,init_dist_to_goal,map_name,num_steps,path_len,shortest_path_length,success,w 2 | 1,0,0.648757707893037,2t7WUuJeko7,45,0.40000000000000024,0.765685424949238,True,0 3 | 2,0,5.7383642879049575,2t7WUuJeko7,135,6.687364087583131,7.742640687119281,True,0 4 | 3,0,6.035800975525,2t7WUuJeko7,394,15.690966779039783,14.536753236814713,True,0 5 | 4,0,4.417195501660091,2t7WUuJeko7,87,4.075000000000002,4.607106781186547,True,0 6 | 5,0,0.10374278460878077,2t7WUuJeko7,117,14.513022641305508,0.1,True,0 7 | 6,0,5.537265366815601,2t7WUuJeko7,163,5.899609375000002,6.5526911934581165,True,0 8 | 7,0,5.7660767351416995,2t7WUuJeko7,219,8.836328124860307,6.601219330881974,True,0 9 | 8,0,1.9503619703951032,2t7WUuJeko7,225,6.609771080205075,6.928427124746188,True,0 10 | 9,0,3.56420683660949,2t7WUuJeko7,193,4.39365234375,4.738477631085023,True,0 11 | 10,0,2.5800186524485333,RPmz2sHmrrY,67,3.0906249999999984,3.589949493661167,True,0 12 | 11,0,11.740445177234967,RPmz2sHmrrY,293,12.345312499999993,13.125483399593902,True,0 13 | 12,0,2.743325362822902,RPmz2sHmrrY,69,2.784375000000001,3.3970562748477153,True,0 14 | 13,0,3.8854289365361936,RPmz2sHmrrY,79,4.849987792968752,5.20416305603426,True,0 15 | 14,0,3.958034403628985,RPmz2sHmrrY,277,10.499609374999997,10.994112549695425,True,0 16 | 15,0,8.940009832949892,RPmz2sHmrrY,347,14.672872511898118,15.035533905932738,True,0 17 | 16,0,9.400900431515417,RPmz2sHmrrY,255,14.402428763481865,14.218376618407357,True,0 18 | 17,0,4.259421930306349,RPmz2sHmrrY,117,5.849606323242188,4.88700576850888,True,0 19 | 18,0,4.3501194900269455,RPmz2sHmrrY,103,4.519589884423192,5.038477631085023,True,0 20 | 19,0,5.903768807327565,RPmz2sHmrrY,231,10.786328124999994,11.476955262170044,True,0 21 | 20,0,7.931995075917357,YVUC4YcDtcY,145,8.199218749999998,8.818376618407354,True,0 22 | 21,0,6.005597614556386,YVUC4YcDtcY,107,6.415624999999999,6.3627416997969535,True,0 23 | 22,0,11.784863935553613,YVUC4YcDtcY,175,11.675000000000015,12.155634918610403,True,0 24 | 23,0,2.5172586697602846,YVUC4YcDtcY,419,15.12727289273274,2.848528137423857,True,0 25 | 24,0,6.313793001988295,YVUC4YcDtcY,764,24.956530690475308,6.742640687119284,True,0 26 | 25,0,4.2668221035514815,YVUC4YcDtcY,77,4.6624999999999925,4.831370849898477,True,0 27 | 26,0,12.074804347346145,YVUC4YcDtcY,177,13.835937500000005,14.766904755831213,True,0 28 | 27,0,7.443786924785212,YVUC4YcDtcY,99,8.000000000000036,8.449747468305826,True,0 29 | 28,0,14.044956982773973,YVUC4YcDtcY,287,16.14995117187502,16.984062043356605,True,0 30 | 29,0,2.246252878898326,YVUC4YcDtcY,99,2.3999999999989847,2.5142135623730955,True,0 31 | 30,0,13.412036974402147,q9vSo1VnCiC,305,17.19285888671874,18.002438661763957,True,0 32 | 31,0,13.20366680834825,q9vSo1VnCiC,213,14.110855908951493,14.936753236814702,True,0 33 | 32,0,5.215374924972599,q9vSo1VnCiC,137,6.349804687500002,6.745584412271566,True,0 34 | 33,0,6.969439419544033,q9vSo1VnCiC,415,21.995049761099896,11.476955262170044,True,0 35 | 34,0,15.596087580857677,q9vSo1VnCiC,321,19.11791606233461,18.923759005323628,True,0 36 | 35,0,22.18306342922084,q9vSo1VnCiC,734,35.04996438411084,35.08183258569804,True,0 37 | 36,0,11.859368137520597,q9vSo1VnCiC,403,27.50937238289616,18.449747468305837,True,0 38 | 37,0,17.80654537453088,q9vSo1VnCiC,307,21.867968749999967,22.498275605729706,True,0 39 | 38,0,6.151768802593556,q9vSo1VnCiC,346,13.096801756534804,9.171067811865466,True,0 40 | 39,0,9.08801767644302,q9vSo1VnCiC,276,11.497498208074703,9.902438661763941,True,0 41 | 40,0,15.812728677554839,ARNzJeq3xxb,631,38.33678981329538,19.076955262170056,False,0 42 | 41,0,10.36973941652456,ARNzJeq3xxb,910,26.98528875942373,18.56690475583122,True,0 43 | 42,0,7.050239435918042,ARNzJeq3xxb,137,6.696875000000008,7.61126983722081,True,0 44 | 43,0,6.123827592275931,ARNzJeq3xxb,729,25.177240485090003,10.488225099390846,False,0 45 | -------------------------------------------------------------------------------- /data/human_run_logs/parse_action_trace_log.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import csv\n", 10 | "import numpy as np\n", 11 | "import matplotlib.pyplot as plt\n", 12 | "import os\n", 13 | "import pandas as pd\n", 14 | " \n", 15 | "def calcSPL(coefs,successes ):\n", 16 | " assert len(coefs) == len(successes)\n", 17 | " return np.mean(coefs *successes)" 18 | ] 19 | }, 20 | { 21 | "cell_type": "code", 22 | "execution_count": 2, 23 | "metadata": {}, 24 | "outputs": [ 25 | { 26 | "data": { 27 | "text/html": [ 28 | "
\n", 29 | "\n", 42 | "\n", 43 | " \n", 44 | " \n", 45 | " \n", 46 | " \n", 47 | " \n", 48 | " \n", 49 | " \n", 50 | " \n", 51 | " \n", 52 | " \n", 53 | " \n", 54 | " \n", 55 | " \n", 56 | " \n", 57 | " \n", 58 | " \n", 59 | " \n", 60 | " \n", 61 | " \n", 62 | " \n", 63 | " \n", 64 | " \n", 65 | " \n", 66 | " \n", 67 | " \n", 68 | " \n", 69 | " \n", 70 | " \n", 71 | " \n", 72 | " \n", 73 | " \n", 74 | " \n", 75 | " \n", 76 | " \n", 77 | " \n", 78 | " \n", 79 | " \n", 80 | " \n", 81 | " \n", 82 | " \n", 83 | " \n", 84 | " \n", 85 | " \n", 86 | " \n", 87 | " \n", 88 | " \n", 89 | " \n", 90 | " \n", 91 | " \n", 92 | " \n", 93 | " \n", 94 | " \n", 95 | " \n", 96 | " \n", 97 | " \n", 98 | " \n", 99 | " \n", 100 | " \n", 101 | " \n", 102 | " \n", 103 | " \n", 104 | " \n", 105 | " \n", 106 | " \n", 107 | " \n", 108 | " \n", 109 | " \n", 110 | " \n", 111 | " \n", 112 | " \n", 113 | " \n", 114 | " \n", 115 | " \n", 116 | " \n", 117 | " \n", 118 | " \n", 119 | " \n", 120 | " \n", 121 | " \n", 122 | " \n", 123 | " \n", 124 | " \n", 125 | " \n", 126 | " \n", 127 | " \n", 128 | " \n", 129 | " \n", 130 | " \n", 131 | "
taskepisodesceneIdtickpxpypzrotationactionsactionArgsforces
9294---190.08220.5512.22883.3966turnRightNaNNaN
9295---200.08220.5512.22883.2710turnRightNaNNaN
9296---210.08220.5512.22883.1453turnRightNaNNaN
9297---220.08220.5512.22883.0196turnRightNaNNaN
9298---230.08220.5512.22882.8940resetNaNNaN
\n", 132 | "
" 133 | ], 134 | "text/plain": [ 135 | " task episode sceneId tick px py pz rotation actions \\\n", 136 | "9294 - - - 19 0.0822 0.551 2.2288 3.3966 turnRight \n", 137 | "9295 - - - 20 0.0822 0.551 2.2288 3.2710 turnRight \n", 138 | "9296 - - - 21 0.0822 0.551 2.2288 3.1453 turnRight \n", 139 | "9297 - - - 22 0.0822 0.551 2.2288 3.0196 turnRight \n", 140 | "9298 - - - 23 0.0822 0.551 2.2288 2.8940 reset \n", 141 | "\n", 142 | " actionArgs forces \n", 143 | "9294 NaN NaN \n", 144 | "9295 NaN NaN \n", 145 | "9296 NaN NaN \n", 146 | "9297 NaN NaN \n", 147 | "9298 NaN NaN " 148 | ] 149 | }, 150 | "execution_count": 2, 151 | "metadata": {}, 152 | "output_type": "execute_result" 153 | } 154 | ], 155 | "source": [ 156 | "df = pd.read_csv('/home/dmishkin/dev/nav-agents-new/nav-agents/HUMAN_RUN/mp3d_maction_trace.csv')\n", 157 | "df.tail(5)" 158 | ] 159 | }, 160 | { 161 | "cell_type": "code", 162 | "execution_count": 3, 163 | "metadata": {}, 164 | "outputs": [ 165 | { 166 | "name": "stderr", 167 | "output_type": "stream", 168 | "text": [ 169 | "/usr/local/lib/python3.5/dist-packages/ipykernel_launcher.py:1: FutureWarning: Method .as_matrix will be removed in a future version. Use .values instead.\n", 170 | " \"\"\"Entry point for launching an IPython kernel.\n", 171 | "/usr/local/lib/python3.5/dist-packages/ipykernel_launcher.py:2: FutureWarning: Method .as_matrix will be removed in a future version. Use .values instead.\n", 172 | " \n", 173 | "/usr/local/lib/python3.5/dist-packages/ipykernel_launcher.py:3: FutureWarning: Method .as_matrix will be removed in a future version. Use .values instead.\n", 174 | " This is separate from the ipykernel package so we can avoid doing imports until\n" 175 | ] 176 | } 177 | ], 178 | "source": [ 179 | "pxs = df['px'].as_matrix().astype(np.float32)\n", 180 | "pzs = df['pz'].as_matrix().astype(np.float32)\n", 181 | "times = df['tick'].as_matrix().astype(np.float32)\n", 182 | "actions = df['actions'].tolist()\n", 183 | "path_lengths = []\n", 184 | "successes = []\n", 185 | "\n" 186 | ] 187 | }, 188 | { 189 | "cell_type": "code", 190 | "execution_count": 4, 191 | "metadata": {}, 192 | "outputs": [ 193 | { 194 | "name": "stdout", 195 | "output_type": "stream", 196 | "text": [ 197 | "50\n" 198 | ] 199 | } 200 | ], 201 | "source": [ 202 | "split_idxs = df['actions'] == \"goal\"\n", 203 | "changeids = df.index[split_idxs].tolist()\n", 204 | "#print (len(changeids))\n", 205 | "#print (split_idxs)\n", 206 | "i0 = 1\n", 207 | "path_lengths = []\n", 208 | "successes = []\n", 209 | "stepss = []\n", 210 | "count = 0\n", 211 | "for i in changeids[1:]:\n", 212 | " steps = i - i0\n", 213 | " successes.append(steps < 499)\n", 214 | " x = pxs[i0:i]\n", 215 | " z = pzs[i0:i]\n", 216 | " #print (steps)\n", 217 | " #print (x)\n", 218 | " #print (z)\n", 219 | " #print (df['px'][i0:i])\n", 220 | " #print (df['actions'][i0:i])\n", 221 | " #\n", 222 | " #print (\"************\")\n", 223 | " path = np.sqrt((z[1:] - z[:-1])**2 + (x[1:] - x[:-1])**2).sum()\n", 224 | " #print (path)\n", 225 | " path_lengths.append(path)\n", 226 | " stepss.append(steps)\n", 227 | " i0=i+1\n", 228 | " count+=1\n", 229 | " #if count > 3:\n", 230 | " # break\n", 231 | "print (len(successes))" 232 | ] 233 | }, 234 | { 235 | "cell_type": "code", 236 | "execution_count": 5, 237 | "metadata": {}, 238 | "outputs": [], 239 | "source": [ 240 | "tt = pd.read_csv('/home/dmishkin/dev/nav-agents-new/nav-agents/CVPR_LOGS_PLAIN/pointgoal_mp3d_m_goals_dist_dir_DFP_Plain_DFP_InformedAgent_42018-11-14--15_10_55/short_stats.csv')" 241 | ] 242 | }, 243 | { 244 | "cell_type": "code", 245 | "execution_count": 6, 246 | "metadata": {}, 247 | "outputs": [ 248 | { 249 | "name": "stdout", 250 | "output_type": "stream", 251 | "text": [ 252 | "100\n", 253 | "[ 0.76568544 4.655635 4.6526914 15.849748 19.025484 1.2\n", 254 | " 3.8384776 11.44264 27.071068 0.9485281 7.352691 14.949747\n", 255 | " 0.24142136 11.259798 7.8012195 6.5627418 3.4899495 12.284062\n", 256 | " 25.412489 1.7970563 7.804163 6.7012196 6.952691 7.518377\n", 257 | " 19.104162 9.955635 8.042641 7.359798 6.404163 16.215433\n", 258 | " 7.5012193 10.601219 1.7414213 7.138478 21.62254 4.797056\n", 259 | " 13.176955 1.6 5.407107 13.7426405 4.6071067 4.472792\n", 260 | " 6.4183764 12.681118 2.4071069 4.631371 6.0426407 5.579899\n", 261 | " 11.235534 2.1071067 ]\n", 262 | "[ 0.2 6.051164 2.5860155 7.5411816 2.77812 0.79843754\n", 263 | " 4.046178 6.4892926 3.5672677 0.4 8.643729 15.299457\n", 264 | " 0. 3.3418827 4.236092 2.2921062 3.0468357 4.083762\n", 265 | " 3.3332908 1.5875 5.00783 2.045761 8.438436 2.21775\n", 266 | " 5.288842 7.115942 8.333651 2.4962947 2.2338362 4.2023463\n", 267 | " 9.335055 3.415936 3.1164062 4.370718 4.3943706 5.290169\n", 268 | " 2.735767 2.2896557 4.869847 20.006464 7.389227 10.622734\n", 269 | " 2.19601 18.125755 1.8093896 3.853198 2.4429317 5.2736793\n", 270 | " 2.8116424 1.4750001 ]\n", 271 | "0\n", 272 | "5\n", 273 | "10\n", 274 | "15\n", 275 | "20\n", 276 | "25\n", 277 | "30\n", 278 | "35\n", 279 | "40\n", 280 | "45\n", 281 | "1\n", 282 | "6\n", 283 | "11\n", 284 | "16\n", 285 | "21\n", 286 | "26\n", 287 | "31\n", 288 | "36\n", 289 | "41\n", 290 | "46\n", 291 | "2\n", 292 | "7\n", 293 | "12\n", 294 | "17\n", 295 | "22\n", 296 | "27\n", 297 | "32\n", 298 | "37\n", 299 | "42\n", 300 | "47\n", 301 | "3\n", 302 | "8\n", 303 | "13\n", 304 | "18\n", 305 | "23\n", 306 | "28\n", 307 | "33\n", 308 | "38\n", 309 | "43\n", 310 | "48\n", 311 | "4\n", 312 | "9\n", 313 | "14\n", 314 | "19\n", 315 | "24\n", 316 | "29\n", 317 | "34\n", 318 | "39\n", 319 | "44\n", 320 | "49\n", 321 | "[0.39996624, 5.9189234, 15.977155, 12.290268, 4.0726943, 2.0607853, 11.775894, 11.285957, 8.145343, 10.927003, 3.8804953, 16.427227, 2.4001255, 6.1484756, 17.34964, 26.678608, 8.3587055, 35.79522, 3.029984, 3.457972, 10.5078945, 5.278115, 52.029747, 7.1871367, 16.710938, 8.477201, 63.761055, 6.646892, 7.9868703, 11.736518, 15.610505, 6.799643, 17.523514, 13.778861, 20.685122, 10.756981, 4.874607, 12.784451, 21.081223, 33.40106, 26.622524, 19.631336, 4.8999586, 34.653557, 18.94811, 1.7750019, 2.6687202, 24.652277, 5.4065638, 30.813797]\n", 322 | "[0.76568544, 1.2, 7.352691, 6.5627418, 7.804163, 9.955635, 7.5012193, 4.797056, 4.6071067, 4.631371, 4.655635, 3.8384776, 14.949747, 3.4899495, 6.7012196, 8.042641, 10.601219, 13.176955, 4.472792, 6.0426407, 4.6526914, 11.44264, 0.24142136, 12.284062, 6.952691, 7.359798, 1.7414213, 1.6, 6.4183764, 5.579899, 15.849748, 27.071068, 11.259798, 25.412489, 7.518377, 6.404163, 7.138478, 5.407107, 12.681118, 11.235534, 19.025484, 0.9485281, 7.8012195, 1.7970563, 19.104162, 16.215433, 21.62254, 13.7426405, 2.4071069, 2.1071067]\n" 323 | ] 324 | }, 325 | { 326 | "name": "stderr", 327 | "output_type": "stream", 328 | "text": [ 329 | "/usr/local/lib/python3.5/dist-packages/ipykernel_launcher.py:1: FutureWarning: Method .as_matrix will be removed in a future version. Use .values instead.\n", 330 | " \"\"\"Entry point for launching an IPython kernel.\n", 331 | "/usr/local/lib/python3.5/dist-packages/ipykernel_launcher.py:2: FutureWarning: Method .as_matrix will be removed in a future version. Use .values instead.\n", 332 | " \n" 333 | ] 334 | } 335 | ], 336 | "source": [ 337 | "SHORTEST = tt['shortest_path_length'].as_matrix().astype(np.float32)\n", 338 | "PL = tt['path_len'].as_matrix().astype(np.float32)\n", 339 | "print (len(SHORTEST))\n", 340 | "print (SHORTEST[:50])\n", 341 | "print (PL[:50])\n", 342 | "SHORTEST2 = []\n", 343 | "for i in range(5):\n", 344 | " for j in range(10):\n", 345 | " idx = j*5 + i\n", 346 | " SHORTEST2.append(SHORTEST[idx])\n", 347 | " print (idx)\n", 348 | "print (path_lengths)\n", 349 | "print (SHORTEST2[:50])" 350 | ] 351 | }, 352 | { 353 | "cell_type": "code", 354 | "execution_count": 7, 355 | "metadata": {}, 356 | "outputs": [ 357 | { 358 | "name": "stdout", 359 | "output_type": "stream", 360 | "text": [ 361 | "0.36896\n" 362 | ] 363 | } 364 | ], 365 | "source": [] 366 | }, 367 | { 368 | "cell_type": "code", 369 | "execution_count": 11, 370 | "metadata": {}, 371 | "outputs": [ 372 | { 373 | "name": "stdout", 374 | "output_type": "stream", 375 | "text": [ 376 | "[1. 0.20273958 0.46020028 0.53397876 1. 1.\n", 377 | " 0.6369979 0.42504644 0.5656124 0.42384642 1. 0.23366559\n", 378 | " 1. 0.5676122 0.38624546 0.30146402 1. 0.36812052\n", 379 | " 1. 1. 0.44278055 1. 0.00464006 1.\n", 380 | " 0.4160563 0.8681872 0.02731168 0.24071401 0.803616 0.47543052\n", 381 | " 1. 1. 0.6425537 1. 0.36346787 0.5953495\n", 382 | " 1. 0.422944 0.60153615 0.33638257 0.71463865 0.04831704\n", 383 | " 1. 0.05185777 1. 1. 1. 0.5574593\n", 384 | " 0.44521937 0.06838192]\n", 385 | "0.56879926 0.84\n" 386 | ] 387 | } 388 | ], 389 | "source": [ 390 | "#SHORTEST2 = np.array(SHORTEST[:50])\n", 391 | "path_lengths = np.array(path_lengths)\n", 392 | "\n", 393 | "coefs = SHORTEST2 / np.maximum(SHORTEST2, path_lengths)\n", 394 | "print (coefs)\n", 395 | "SPL = calcSPL(coefs, np.array(successes))\n", 396 | "print (SPL, np.array(successes).astype(np.float32).mean())" 397 | ] 398 | } 399 | ], 400 | "metadata": { 401 | "kernelspec": { 402 | "display_name": "Python 3", 403 | "language": "python", 404 | "name": "python3" 405 | }, 406 | "language_info": { 407 | "codemirror_mode": { 408 | "name": "ipython", 409 | "version": 3 410 | }, 411 | "file_extension": ".py", 412 | "mimetype": "text/x-python", 413 | "name": "python", 414 | "nbconvert_exporter": "python", 415 | "pygments_lexer": "ipython3", 416 | "version": "3.5.2" 417 | } 418 | }, 419 | "nbformat": 4, 420 | "nbformat_minor": 2 421 | } 422 | -------------------------------------------------------------------------------- /data/human_run_logs/pointgoal_mp3d_m_goals_dist_dir_Human/short_stats.csv: -------------------------------------------------------------------------------- 1 | episode_id,h,init_dist_to_goal,map_name,num_steps,path_len,shortest_path_length,success,w 2 | 1,0,0.648757707893037,2t7WUuJeko7,45,0.40000000000000024,0.765685424949238,True,0 3 | 2,0,5.7383642879049575,2t7WUuJeko7,135,6.687364087583131,7.742640687119281,True,0 4 | 3,0,6.035800975525,2t7WUuJeko7,394,15.690966779039783,14.536753236814713,True,0 5 | 4,0,4.417195501660091,2t7WUuJeko7,87,4.075000000000002,4.607106781186547,True,0 6 | 5,0,0.10374278460878077,2t7WUuJeko7,117,14.513022641305508,0.1,True,0 7 | 6,0,5.537265366815601,2t7WUuJeko7,163,5.899609375000002,6.5526911934581165,True,0 8 | 7,0,5.7660767351416995,2t7WUuJeko7,219,8.836328124860307,6.601219330881974,True,0 9 | 8,0,1.9503619703951032,2t7WUuJeko7,225,6.609771080205075,6.928427124746188,True,0 10 | 9,0,3.56420683660949,2t7WUuJeko7,193,4.39365234375,4.738477631085023,True,0 11 | 10,0,2.5800186524485333,RPmz2sHmrrY,67,3.0906249999999984,3.589949493661167,True,0 12 | 11,0,11.740445177234967,RPmz2sHmrrY,293,12.345312499999993,13.125483399593902,True,0 13 | 12,0,2.743325362822902,RPmz2sHmrrY,69,2.784375000000001,3.3970562748477153,True,0 14 | 13,0,3.8854289365361936,RPmz2sHmrrY,79,4.849987792968752,5.20416305603426,True,0 15 | 14,0,3.958034403628985,RPmz2sHmrrY,277,10.499609374999997,10.994112549695425,True,0 16 | 15,0,8.940009832949892,RPmz2sHmrrY,347,14.672872511898118,15.035533905932738,True,0 17 | 16,0,9.400900431515417,RPmz2sHmrrY,255,14.402428763481865,14.218376618407357,True,0 18 | 17,0,4.259421930306349,RPmz2sHmrrY,117,5.849606323242188,4.88700576850888,True,0 19 | 18,0,4.3501194900269455,RPmz2sHmrrY,103,4.519589884423192,5.038477631085023,True,0 20 | 19,0,5.903768807327565,RPmz2sHmrrY,231,10.786328124999994,11.476955262170044,True,0 21 | 20,0,7.931995075917357,YVUC4YcDtcY,145,8.199218749999998,8.818376618407354,True,0 22 | 21,0,6.005597614556386,YVUC4YcDtcY,107,6.415624999999999,6.3627416997969535,True,0 23 | 22,0,11.784863935553613,YVUC4YcDtcY,175,11.675000000000015,12.155634918610403,True,0 24 | 23,0,2.5172586697602846,YVUC4YcDtcY,419,15.12727289273274,2.848528137423857,True,0 25 | 24,0,6.313793001988295,YVUC4YcDtcY,764,24.956530690475308,6.742640687119284,True,0 26 | 25,0,4.2668221035514815,YVUC4YcDtcY,77,4.6624999999999925,4.831370849898477,True,0 27 | 26,0,12.074804347346145,YVUC4YcDtcY,177,13.835937500000005,14.766904755831213,True,0 28 | 27,0,7.443786924785212,YVUC4YcDtcY,99,8.000000000000036,8.449747468305826,True,0 29 | 28,0,14.044956982773973,YVUC4YcDtcY,287,16.14995117187502,16.984062043356605,True,0 30 | 29,0,2.246252878898326,YVUC4YcDtcY,99,2.3999999999989847,2.5142135623730955,True,0 31 | 30,0,13.412036974402147,q9vSo1VnCiC,305,17.19285888671874,18.002438661763957,True,0 32 | 31,0,13.20366680834825,q9vSo1VnCiC,213,14.110855908951493,14.936753236814702,True,0 33 | 32,0,5.215374924972599,q9vSo1VnCiC,137,6.349804687500002,6.745584412271566,True,0 34 | 33,0,6.969439419544033,q9vSo1VnCiC,415,21.995049761099896,11.476955262170044,True,0 35 | 34,0,15.596087580857677,q9vSo1VnCiC,321,19.11791606233461,18.923759005323628,True,0 36 | 35,0,22.18306342922084,q9vSo1VnCiC,734,35.04996438411084,35.08183258569804,True,0 37 | 36,0,11.859368137520597,q9vSo1VnCiC,403,27.50937238289616,18.449747468305837,True,0 38 | 37,0,17.80654537453088,q9vSo1VnCiC,307,21.867968749999967,22.498275605729706,True,0 39 | 38,0,6.151768802593556,q9vSo1VnCiC,346,13.096801756534804,9.171067811865466,True,0 40 | 39,0,9.08801767644302,q9vSo1VnCiC,276,11.497498208074703,9.902438661763941,True,0 41 | 40,0,15.812728677554839,ARNzJeq3xxb,631,38.33678981329538,19.076955262170056,False,0 42 | 41,0,10.36973941652456,ARNzJeq3xxb,910,26.98528875942373,18.56690475583122,True,0 43 | 42,0,7.050239435918042,ARNzJeq3xxb,137,6.696875000000008,7.61126983722081,True,0 44 | 43,0,6.123827592275931,ARNzJeq3xxb,729,25.177240485090003,10.488225099390846,False,0 45 | -------------------------------------------------------------------------------- /data/human_run_logs/short_stats.csv: -------------------------------------------------------------------------------- 1 | episode_id,h,init_dist_to_goal,map_name,num_steps,path_len,shortest_path_length,success,w 2 | 1,0,0.648757707893037,2t7WUuJeko7,45,0.40000000000000024,0.765685424949238,True,0 3 | 2,0,5.7383642879049575,2t7WUuJeko7,135,6.687364087583131,7.742640687119281,True,0 4 | 3,0,6.035800975525,2t7WUuJeko7,394,15.690966779039783,14.536753236814713,True,0 5 | 4,0,4.417195501660091,2t7WUuJeko7,87,4.075000000000002,4.607106781186547,True,0 6 | 5,0,0.10374278460878077,2t7WUuJeko7,117,14.513022641305508,0.1,True,0 7 | 6,0,5.537265366815601,2t7WUuJeko7,163,5.899609375000002,6.5526911934581165,True,0 8 | 7,0,5.7660767351416995,2t7WUuJeko7,219,8.836328124860307,6.601219330881974,True,0 9 | 8,0,1.9503619703951032,2t7WUuJeko7,225,6.609771080205075,6.928427124746188,True,0 10 | 9,0,3.56420683660949,2t7WUuJeko7,193,4.39365234375,4.738477631085023,True,0 11 | 10,0,2.5800186524485333,RPmz2sHmrrY,67,3.0906249999999984,3.589949493661167,True,0 12 | 11,0,11.740445177234967,RPmz2sHmrrY,293,12.345312499999993,13.125483399593902,True,0 13 | 12,0,2.743325362822902,RPmz2sHmrrY,69,2.784375000000001,3.3970562748477153,True,0 14 | 13,0,3.8854289365361936,RPmz2sHmrrY,79,4.849987792968752,5.20416305603426,True,0 15 | 14,0,3.958034403628985,RPmz2sHmrrY,277,10.499609374999997,10.994112549695425,True,0 16 | 15,0,8.940009832949892,RPmz2sHmrrY,347,14.672872511898118,15.035533905932738,True,0 17 | 16,0,9.400900431515417,RPmz2sHmrrY,255,14.402428763481865,14.218376618407357,True,0 18 | 17,0,4.259421930306349,RPmz2sHmrrY,117,5.849606323242188,4.88700576850888,True,0 19 | 18,0,4.3501194900269455,RPmz2sHmrrY,103,4.519589884423192,5.038477631085023,True,0 20 | 19,0,5.903768807327565,RPmz2sHmrrY,231,10.786328124999994,11.476955262170044,True,0 21 | 20,0,7.931995075917357,YVUC4YcDtcY,145,8.199218749999998,8.818376618407354,True,0 22 | 21,0,6.005597614556386,YVUC4YcDtcY,107,6.415624999999999,6.3627416997969535,True,0 23 | 22,0,11.784863935553613,YVUC4YcDtcY,175,11.675000000000015,12.155634918610403,True,0 24 | 23,0,2.5172586697602846,YVUC4YcDtcY,419,15.12727289273274,2.848528137423857,True,0 25 | 24,0,6.313793001988295,YVUC4YcDtcY,764,24.956530690475308,6.742640687119284,True,0 26 | 25,0,4.2668221035514815,YVUC4YcDtcY,77,4.6624999999999925,4.831370849898477,True,0 27 | 26,0,12.074804347346145,YVUC4YcDtcY,177,13.835937500000005,14.766904755831213,True,0 28 | 27,0,7.443786924785212,YVUC4YcDtcY,99,8.000000000000036,8.449747468305826,True,0 29 | 28,0,14.044956982773973,YVUC4YcDtcY,287,16.14995117187502,16.984062043356605,True,0 30 | 29,0,2.246252878898326,YVUC4YcDtcY,99,2.3999999999989847,2.5142135623730955,True,0 31 | 30,0,13.412036974402147,q9vSo1VnCiC,305,17.19285888671874,18.002438661763957,True,0 32 | 31,0,13.20366680834825,q9vSo1VnCiC,213,14.110855908951493,14.936753236814702,True,0 33 | 32,0,5.215374924972599,q9vSo1VnCiC,137,6.349804687500002,6.745584412271566,True,0 34 | 33,0,6.969439419544033,q9vSo1VnCiC,415,21.995049761099896,11.476955262170044,True,0 35 | 34,0,15.596087580857677,q9vSo1VnCiC,321,19.11791606233461,18.923759005323628,True,0 36 | 35,0,22.18306342922084,q9vSo1VnCiC,734,35.04996438411084,35.08183258569804,True,0 37 | 36,0,11.859368137520597,q9vSo1VnCiC,403,27.50937238289616,18.449747468305837,True,0 38 | 37,0,17.80654537453088,q9vSo1VnCiC,307,21.867968749999967,22.498275605729706,True,0 39 | 38,0,6.151768802593556,q9vSo1VnCiC,346,13.096801756534804,9.171067811865466,True,0 40 | 39,0,9.08801767644302,q9vSo1VnCiC,276,11.497498208074703,9.902438661763941,True,0 41 | 40,0,15.812728677554839,ARNzJeq3xxb,631,38.33678981329538,19.076955262170056,False,0 42 | 41,0,10.36973941652456,ARNzJeq3xxb,910,26.98528875942373,18.56690475583122,True,0 43 | 42,0,7.050239435918042,ARNzJeq3xxb,137,6.696875000000008,7.61126983722081,True,0 44 | 43,0,6.123827592275931,ARNzJeq3xxb,729,25.177240485090003,10.488225099390846,False,0 45 | -------------------------------------------------------------------------------- /data/human_run_logs/short_stats_human_mp3d_new.csv: -------------------------------------------------------------------------------- 1 | episode_id,h,init_dist_to_goal,map_name,num_steps,path_len,shortest_path_length,success,w 2 | 1,0,0.648757707893037,2t7WUuJeko7,45,0.40000000000000024,0.765685424949238,True,0 3 | 2,0,5.7383642879049575,2t7WUuJeko7,135,6.687364087583131,7.742640687119281,True,0 4 | 3,0,6.035800975525,2t7WUuJeko7,394,15.690966779039783,14.536753236814713,True,0 5 | 4,0,4.417195501660091,2t7WUuJeko7,87,4.075000000000002,4.607106781186547,True,0 6 | 5,0,0.10374278460878077,2t7WUuJeko7,117,14.513022641305508,0.1,True,0 7 | 6,0,5.537265366815601,2t7WUuJeko7,163,5.899609375000002,6.5526911934581165,True,0 8 | 7,0,5.7660767351416995,2t7WUuJeko7,219,8.836328124860307,6.601219330881974,True,0 9 | 8,0,1.9503619703951032,2t7WUuJeko7,225,6.609771080205075,6.928427124746188,True,0 10 | 9,0,3.56420683660949,2t7WUuJeko7,193,4.39365234375,4.738477631085023,True,0 11 | 10,0,2.5800186524485333,RPmz2sHmrrY,67,3.0906249999999984,3.589949493661167,True,0 12 | 11,0,11.740445177234967,RPmz2sHmrrY,293,12.345312499999993,13.125483399593902,True,0 13 | 12,0,2.743325362822902,RPmz2sHmrrY,69,2.784375000000001,3.3970562748477153,True,0 14 | 13,0,3.8854289365361936,RPmz2sHmrrY,79,4.849987792968752,5.20416305603426,True,0 15 | 14,0,3.958034403628985,RPmz2sHmrrY,277,10.499609374999997,10.994112549695425,True,0 16 | 15,0,8.940009832949892,RPmz2sHmrrY,347,14.672872511898118,15.035533905932738,True,0 17 | 16,0,9.400900431515417,RPmz2sHmrrY,255,14.402428763481865,14.218376618407357,True,0 18 | 17,0,4.259421930306349,RPmz2sHmrrY,117,5.849606323242188,4.88700576850888,True,0 19 | 18,0,4.3501194900269455,RPmz2sHmrrY,103,4.519589884423192,5.038477631085023,True,0 20 | 19,0,5.903768807327565,RPmz2sHmrrY,231,10.786328124999994,11.476955262170044,True,0 21 | 20,0,7.931995075917357,YVUC4YcDtcY,145,8.199218749999998,8.818376618407354,True,0 22 | 21,0,6.005597614556386,YVUC4YcDtcY,107,6.415624999999999,6.3627416997969535,True,0 23 | 22,0,11.784863935553613,YVUC4YcDtcY,175,11.675000000000015,12.155634918610403,True,0 24 | 23,0,2.5172586697602846,YVUC4YcDtcY,419,15.12727289273274,2.848528137423857,True,0 25 | 24,0,6.313793001988295,YVUC4YcDtcY,764,24.956530690475308,6.742640687119284,True,0 26 | 25,0,4.2668221035514815,YVUC4YcDtcY,77,4.6624999999999925,4.831370849898477,True,0 27 | 26,0,12.074804347346145,YVUC4YcDtcY,177,13.835937500000005,14.766904755831213,True,0 28 | 27,0,7.443786924785212,YVUC4YcDtcY,99,8.000000000000036,8.449747468305826,True,0 29 | 28,0,14.044956982773973,YVUC4YcDtcY,287,16.14995117187502,16.984062043356605,True,0 30 | 29,0,2.246252878898326,YVUC4YcDtcY,99,2.3999999999989847,2.5142135623730955,True,0 31 | 30,0,13.412036974402147,q9vSo1VnCiC,305,17.19285888671874,18.002438661763957,True,0 32 | 31,0,13.20366680834825,q9vSo1VnCiC,213,14.110855908951493,14.936753236814702,True,0 33 | 32,0,5.215374924972599,q9vSo1VnCiC,137,6.349804687500002,6.745584412271566,True,0 34 | 33,0,6.969439419544033,q9vSo1VnCiC,415,21.995049761099896,11.476955262170044,True,0 35 | 34,0,15.596087580857677,q9vSo1VnCiC,321,19.11791606233461,18.923759005323628,True,0 36 | 35,0,22.18306342922084,q9vSo1VnCiC,734,35.04996438411084,35.08183258569804,True,0 37 | 36,0,11.859368137520597,q9vSo1VnCiC,403,27.50937238289616,18.449747468305837,True,0 38 | 37,0,17.80654537453088,q9vSo1VnCiC,307,21.867968749999967,22.498275605729706,True,0 39 | 38,0,6.151768802593556,q9vSo1VnCiC,346,13.096801756534804,9.171067811865466,True,0 40 | 39,0,9.08801767644302,q9vSo1VnCiC,276,11.497498208074703,9.902438661763941,True,0 41 | 40,0,15.812728677554839,ARNzJeq3xxb,631,38.33678981329538,19.076955262170056,False,0 42 | 41,0,10.36973941652456,ARNzJeq3xxb,910,26.98528875942373,18.56690475583122,True,0 43 | 42,0,7.050239435918042,ARNzJeq3xxb,137,6.696875000000008,7.61126983722081,True,0 44 | 43,0,6.123827592275931,ARNzJeq3xxb,729,25.177240485090003,10.488225099390846,False,0 45 | -------------------------------------------------------------------------------- /data/mp3d3_small1k.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | #For resolution 256x256, FOV 90 deg 9 | #Camera.fx: 128.0 10 | #Camera.fy: 128.0 11 | #Camera.cx: 127.0 12 | #Camera.cy: 127.0 13 | 14 | #For resolution 256x256, FOV 45 deg 15 | Camera.fx: 256.0 16 | Camera.fy: 256.0 17 | Camera.cx: 127.0 18 | Camera.cy: 127.0 19 | 20 | 21 | Camera.k1: 0.0 22 | Camera.k2: 0.0 23 | Camera.p1: 0.0 24 | Camera.p2: 0.0 25 | 26 | # Camera frames per second 27 | Camera.fps: 30.0 28 | 29 | # IR projector baseline times fx (aprox.) 30 | Camera.bf: 50.0 31 | 32 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 33 | Camera.RGB: 1 34 | 35 | # Close/Far threshold. Baseline times. 36 | #ThDepth: 40.0 37 | ThDepth: 70.0 38 | 39 | # Deptmap values factor 40 | DepthMapFactor: 1.0 41 | 42 | #-------------------------------------------------------------------------------------------- 43 | # ORB Parameters 44 | #-------------------------------------------------------------------------------------------- 45 | 46 | # ORB Extractor: Number of features per image 47 | ORBextractor.nFeatures: 1000 48 | 49 | # ORB Extractor: Scale factor between levels in the scale pyramid 50 | ORBextractor.scaleFactor: 1.2 51 | 52 | # ORB Extractor: Number of levels in the scale pyramid 53 | ORBextractor.nLevels: 8 54 | 55 | # ORB Extractor: Fast threshold 56 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 57 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 58 | # You can lower these values if your images have low contrast 59 | ORBextractor.iniThFAST: 5 60 | ORBextractor.minThFAST: 1 61 | 62 | #-------------------------------------------------------------------------------------------- 63 | # Viewer Parameters 64 | #-------------------------------------------------------------------------------------------- 65 | Viewer.KeyFrameSize: 0.1 66 | Viewer.KeyFrameLineWidth: 1 67 | Viewer.GraphLineWidth: 1 68 | Viewer.PointSize:2 69 | Viewer.CameraSize: 0.15 70 | Viewer.CameraLineWidth: 2 71 | Viewer.ViewpointX: 0 72 | Viewer.ViewpointY: -10 73 | Viewer.ViewpointZ: -0.1 74 | Viewer.ViewpointF: 2000 75 | 76 | -------------------------------------------------------------------------------- /data/mp3d3_small5k.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | #For resolution 512x512, FOV 90 deg 9 | #Camera.fx: 256.0 10 | #Camera.fy: 256.0 11 | #Camera.cx: 255.0 12 | #Camera.cy: 255.0 13 | 14 | #For resolution 512x512, FOV 45 deg 15 | Camera.fx: 512.0 16 | Camera.fy: 512.0 17 | Camera.cx: 255.0 18 | Camera.cy: 255.0 19 | 20 | 21 | Camera.k1: 0.0 22 | Camera.k2: 0.0 23 | Camera.p1: 0.0 24 | Camera.p2: 0.0 25 | 26 | # Camera frames per second 27 | Camera.fps: 30.0 28 | 29 | # IR projector baseline times fx (aprox.) 30 | Camera.bf: 50.0 31 | 32 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 33 | Camera.RGB: 1 34 | 35 | # Close/Far threshold. Baseline times. 36 | #ThDepth: 40.0 37 | ThDepth: 70.0 38 | 39 | # Deptmap values factor 40 | DepthMapFactor: 1.0 41 | 42 | #-------------------------------------------------------------------------------------------- 43 | # ORB Parameters 44 | #-------------------------------------------------------------------------------------------- 45 | 46 | # ORB Extractor: Number of features per image 47 | ORBextractor.nFeatures: 5000 48 | 49 | # ORB Extractor: Scale factor between levels in the scale pyramid 50 | ORBextractor.scaleFactor: 1.2 51 | 52 | # ORB Extractor: Number of levels in the scale pyramid 53 | ORBextractor.nLevels: 8 54 | 55 | # ORB Extractor: Fast threshold 56 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 57 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 58 | # You can lower these values if your images have low contrast 59 | ORBextractor.iniThFAST: 5 60 | ORBextractor.minThFAST: 1 61 | 62 | #-------------------------------------------------------------------------------------------- 63 | # Viewer Parameters 64 | #-------------------------------------------------------------------------------------------- 65 | Viewer.KeyFrameSize: 0.1 66 | Viewer.KeyFrameLineWidth: 1 67 | Viewer.GraphLineWidth: 1 68 | Viewer.PointSize:2 69 | Viewer.CameraSize: 0.15 70 | Viewer.CameraLineWidth: 2 71 | Viewer.ViewpointX: 0 72 | Viewer.ViewpointY: -10 73 | Viewer.ViewpointZ: -0.1 74 | Viewer.ViewpointF: 2000 75 | 76 | -------------------------------------------------------------------------------- /install_minos_and_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | DIR1=$(pwd) 3 | MAINDIR=$(pwd)/3rdparty 4 | mkdir ${MAINDIR} 5 | cd ${MAINDIR} 6 | conda create -y -n "NavigationBenchmarkMinos" python=3.6 7 | source activate NavigationBenchmarkMinos 8 | conda install opencv -y 9 | conda install pytorch torchvision -c pytorch -y 10 | conda install -c conda-forge imageio -y 11 | conda install ffmpeg -c conda-forge -y 12 | curl -o- https://raw.githubusercontent.com/creationix/nvm/v0.33.7/install.sh | bash 13 | NVM_DIR="${HOME}/.nvm" 14 | [ -s "$NVM_DIR/nvm.sh" ] && \. "$NVM_DIR/nvm.sh" 15 | [ -s "$NVM_DIR/bash_completion" ] && \. "$NVM_DIR/bash_completion" 16 | nvm install node 17 | cd ${MAINDIR} 18 | git clone https://github.com/minosworld/minos 19 | cd minos 20 | cd minos/server 21 | rm -rf node_modules 22 | npm install 23 | cd ${MAINDIR} 24 | wget https://www.dropbox.com/s/nlh8ofojipghc44/headless-gl-nvidia.zip 25 | unzip headless-gl-nvidia.zip 26 | cd headless-gl-nvidia 27 | pip install glad 28 | cd src/gl_context 29 | cd build 30 | NVIDIA_VERSION=$(nvidia-smi --query-gpu=driver_version --format=csv,noheader) 31 | sed -i "s, /usr/lib/nvidia-390,/usr/lib/nvidia-${NVIDIA_VERSION:0:3},g" ../CMakeLists.txt 32 | rm CMakeCache.txt 33 | cmake .. 34 | make 35 | cd ../../.. 36 | npm install 37 | npm run rebuild 38 | cp build/Release/webgl.node ${MAINDIR}/minos/minos/server/node_modules/sstk/node_modules/gl/build/Release/webgl.node 39 | cd ${MAINDIR}/minos 40 | pip install -e . 41 | pip install tensorboardx 42 | cd gym 43 | pip install numpy --upgrade -y 44 | pip install -e . 45 | pip install imageio 46 | pip uninstall six -y 47 | pip uninstall requests -y 48 | pip uninstall websocket-client -y 49 | pip uninstall socketIO-client-2 -y 50 | cd ${MAINDIR} 51 | git clone https://github.com/msavva/socketIO-client-2 52 | cd socketIO-client-2 53 | pip install -e . 54 | cd ${MAINDIR} 55 | mkdir eigen3 56 | cd eigen3 57 | wget http://bitbucket.org/eigen/eigen/get/3.3.5.tar.gz 58 | tar -xzf 3.3.5.tar.gz 59 | cd eigen-eigen-b3f3d4950030 60 | mkdir build 61 | cd build 62 | cmake .. -DCMAKE_INSTALL_PREFIX=${MAINDIR}/eigen3_installed/ 63 | make install 64 | cd ${MAINDIR} 65 | wget https://sourceforge.net/projects/glew/files/glew/2.1.0/glew-2.1.0.zip 66 | unzip glew-2.1.0.zip 67 | cd glew-2.1.0/ 68 | cd build 69 | cmake ./cmake -DCMAKE_INSTALL_PREFIX=${MAINDIR}/glew_installed 70 | make -j4 71 | make install 72 | cd ${MAINDIR} 73 | pip install numpy --upgrade 74 | rm Pangolin -rf 75 | git clone https://github.com/stevenlovegrove/Pangolin.git 76 | cd Pangolin 77 | mkdir build 78 | cd build 79 | cmake .. -DCMAKE_PREFIX_PATH=${MAINDIR}/glew_installed/ -DCMAKE_LIBRARY_PATH=${MAINDIR}/glew_installed/lib/ -DCMAKE_INSTALL_PREFIX=${MAINDIR}/pangolin_installed 80 | cmake --build . 81 | cd ${MAINDIR} 82 | rm ORB_SLAM2 -rf 83 | rm ORB_SLAM2-PythonBindings -rf 84 | git clone https://github.com/ducha-aiki/ORB_SLAM2 85 | git clone https://github.com/ducha-aiki/ORB_SLAM2-PythonBindings 86 | cd ${MAINDIR}/ORB_SLAM2 87 | sed -i "s,cmake .. -DCMAKE_BUILD_TYPE=Release,cmake .. -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=${MAINDIR}/eigen3_installed/include/eigen3/ -DCMAKE_INSTALL_PREFIX=${MAINDIR}/ORBSLAM2_installed ,g" build.sh 88 | ln -s ${MAINDIR}/eigen3_installed/include/eigen3/Eigen ${MAINDIR}/ORB_SLAM2/Thirdparty/g2o/g2o/core/Eigen 89 | ./build.sh 90 | cd build 91 | make install 92 | cd ${MAINDIR} 93 | cd ORB_SLAM2-PythonBindings/src 94 | ln -s ${MAINDIR}/eigen3_installed/include/eigen3/Eigen Eigen 95 | cd ${MAINDIR}/ORB_SLAM2-PythonBindings 96 | mkdir build 97 | cd build 98 | CONDA_DIR=$(dirname $(dirname $(which conda))) 99 | sed -i "s,lib/python3.5/dist-packages,${CONDA_DIR}/envs/NavigationBenchmarkMinos/lib/python3.6/site-packages/,g" ../CMakeLists.txt 100 | cmake .. -DPYTHON_INCLUDE_DIR=$(python -c "from distutils.sysconfig import get_python_inc; print(get_python_inc())") -DPYTHON_LIBRARY=$(python -c "import distutils.sysconfig as sysconfig; print(sysconfig.get_config_var('LIBDIR'))")/libpython3.6m.so -DPYTHON_EXECUTABLE:FILEPATH=`which python` -DCMAKE_LIBRARY_PATH=${MAINDIR}/ORBSLAM2_installed/lib -DCMAKE_INCLUDE_PATH=${MAINDIR}/ORBSLAM2_installed/include;${MAINDIR}/eigen3_installed/include/eigen3 -DCMAKE_INSTALL_PREFIX=${MAINDIR}/pyorbslam2_installed 101 | make 102 | make install 103 | cp ${MAINDIR}/ORB_SLAM2/Vocabulary/ORBvoc.txt ${DIR1}/data/ 104 | -------------------------------------------------------------------------------- /navigation/ClassicAgents.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | import random 4 | import time 5 | from math import ceil,floor, pi, sqrt 6 | from copy import deepcopy 7 | from PIL import Image, ImageOps 8 | 9 | from .Utils import * 10 | from .Reprojection import * 11 | from .Reprojection import angleToPi_2_MinusPi_2 as norm_ang 12 | from .Control import command2NumericAction, action2Command 13 | import orbslam2 14 | from .Mappers import DirectDepthMapper, SparseDepthMapper 15 | from .PyTorchPathPlanners import DifferentiableStarPlanner 16 | 17 | 18 | 19 | class Container(object): 20 | pass 21 | 22 | def defaultAgentOptions(default_args = {'agent_type': 'ClassicRGBD'}): 23 | opts = {} 24 | #Input resolution 25 | opts['width']= 256 26 | opts['height']= 256 27 | opts['resolution']= [256, 256] 28 | #SLAM 29 | opts['slam_vocab_path'] = 'data/ORBvoc.txt' 30 | opts['slam_settings_path'] = 'data/mp3d3_small1k.yaml' 31 | if default_args['agent_type'] == 'ClassicRGB': 32 | opts['slam_settings_path'] = 'data/mp3d3_small5k.yaml' 33 | opts['height']= 512 34 | opts['width']= 512 35 | opts['resolution']= [512, 512] 36 | #Camera intristics 37 | opts['fx']= opts['width'] 38 | opts['fy']= opts['height'] 39 | opts['cx']= opts['fx'] - 1 40 | opts['cy']= opts['fy'] - 1 41 | # Depth camera parameters 42 | opts['minos_depth_near'] = 0.001 43 | opts['minos_depth_far'] = 4.0 44 | #Mapping and planning 45 | opts['map_cell_size'] = 0.1# minos parameters, m 46 | opts['map_size'] = 40.# meters 47 | opts['map_resolution'] = opts['map_cell_size'] 48 | opts['world_camera_height'] = 1.35 49 | if 'agent' in default_args: 50 | if 'eyeHeight' in default_args['agent']: 51 | opts['world_camera_height'] = 0.6 + float(default_args['agent']['eyeHeight'])/2. 52 | opts['camera_height'] = opts['world_camera_height'] 53 | opts['beta'] = 100 54 | opts['preprocess'] = False 55 | #Points from h_min to h_max contribute to obstacle map 56 | opts['h_min'] = 0.3#meters 57 | opts['h_max'] = 0.9*opts['world_camera_height']#meters 58 | #Points from near_th to far_th contribute to obstacle map 59 | opts['near_th'] = 0.1#meters 60 | opts['far_th'] = 2.5#meters 61 | #Number of 3d points in cell to create obstacle: 62 | opts['obstacle_threshold'] = float(opts['width']) / 2.0 63 | opts['obstacle_th'] = opts['obstacle_threshold'] 64 | opts['preprocess'] = True 65 | #parameters for control 66 | #rotate, if angle to goal is more than, 67 | opts['angle_threshold'] = float(np.deg2rad(15)) 68 | #Go to waypoint, until reach distance 69 | opts['pos_threshold'] = 0.15 70 | #Select waypoint, which is at least this far 71 | opts['far_pos_threshold'] = 0.5 72 | # 73 | opts['device'] = torch.device('cuda:0') 74 | for k, v in default_args.items(): 75 | opts[k] = v 76 | return opts 77 | 78 | class RandomAgent(object): 79 | r"""Simplest agent, which returns random actions, 80 | until reach the goal 81 | """ 82 | def __init__(self, 83 | num_actions = 3, 84 | time_step = 0.1, 85 | dist_threshold_to_stop = 0.01, 86 | **kwargs): 87 | super(RandomAgent, self).__init__() 88 | self.num_actions = num_actions 89 | self.time_step = time_step 90 | self.dist_threshold_to_stop = dist_threshold_to_stop 91 | self.steps = 0 92 | return 93 | def reset(self): 94 | self.steps = 0 95 | return 96 | def updateInternalState(self,minos_observation): 97 | self.obs = minos_observation 98 | self.steps+=1 99 | return 100 | def isGoalReached(self): 101 | dist = self.obs['observation']['measurements']['distance_to_goal'][0] 102 | return dist <= self.dist_threshold_to_stop 103 | def act(self, minos_observation = None, random_prob = 1.0): 104 | self.updateInternalState(minos_observation) 105 | # Act 106 | action = np.zeros(self.num_actions) 107 | #Check if we are done 108 | success = self.isGoalReached() 109 | if not success: 110 | random_action = np.random.randint(0, self.num_actions, size = (1)) 111 | action[random_action] = 1 112 | return action, success 113 | 114 | class LoggingAgent(RandomAgent): 115 | r"""Auxilary agent, for purpose of logging and 116 | calculating SPL, since MINOS does not provide such functionality. 117 | Agent uses ground truth info, which is not available to the evaluated agents. 118 | """ 119 | def __init__(self, 120 | map_size = 100, 121 | map_resolution = 0.1, 122 | device = torch.device('cpu'), 123 | **kwargs): 124 | super(LoggingAgent, self).__init__() 125 | self.map_size_meters = map_size 126 | self.map_cell_size = map_resolution 127 | self.device = device 128 | self.reset() 129 | return 130 | def getCurrentGTOrientationOnGTMap(self): 131 | return torch.tensor([[self.pose6D[0,0,0], self.pose6D[0,0,2]], 132 | [self.pose6D[0,2,0], self.pose6D[0,2,2]]]) 133 | def getCurrentGTPositionOnGTMap(self, floor = True): 134 | return projectTPsIntoWorldMap(self.pose6D.view(1,4,4), 135 | self.map_cell_size, 136 | self.map_size_meters, floor) 137 | def getMinosGeoDist(self): 138 | return self.obs['observation']['measurements']['shortest_path_to_goal']['distance'] 139 | def mapSizeInCells(self): 140 | return int(self.map_size_meters / self.map_cell_size) 141 | def initMap2D(self): 142 | return torch.zeros(1,1,self.mapSizeInCells(),self.mapSizeInCells()).float().to(self.device) 143 | def updateTravelledDist(self, obs): 144 | if self.InitGTPoseForBenchmark is None: 145 | self.InitGTPoseForBenchmark = deepcopy(obs['info']['agent_state']) 146 | self.InitGTPositionForBenchmark = deepcopy(np.array(self.InitGTPoseForBenchmark['position'])[::-1]) 147 | self.InitGTOrientationForBenchmark = deepcopy(np.array(self.InitGTPoseForBenchmark['orientation'])[::-1]) 148 | self.InitGTAngleOrig = norm_ang(np.arctan2(self.InitGTOrientationForBenchmark[0], self.InitGTOrientationForBenchmark[2])) 149 | self.CurrentGTPoseForBenchmark = torch.eye(4).float().to(self.device) 150 | pos = deepcopy(np.array(obs['info']['agent_state']['position'])[::-1]) 151 | ori = deepcopy(np.array(obs['info']['agent_state']['orientation'])[::-1]) 152 | angle = norm_ang(np.arctan2(ori[0], ori[2])) 153 | P = np.zeros((3,4)).astype(np.float32) 154 | P[:,3] = pos - self.InitGTPositionForBenchmark 155 | P03 = P[0,3] * np.cos(self.InitGTAngleOrig) - P[2,3] * np.sin(self.InitGTAngleOrig) 156 | P23 = P[2,3] * np.cos(self.InitGTAngleOrig) + P[0,3] * np.sin(self.InitGTAngleOrig) 157 | P[0,3] = P03 158 | P[2,3] = P23 159 | P[1,1] = 1.0 160 | da = angle - self.InitGTAngleOrig 161 | P[0,0] = np.cos(da) 162 | P[2,2] = np.cos(da) 163 | P[0,2] = np.sin(da) 164 | P[2,0] = -np.sin(da) 165 | CurrentGTPoseForBenchmark = homogenizeP(torch.from_numpy(deepcopy(P)).view(3,4).to(self.device).float()) 166 | step = getPosDiffLength(self.CurrentGTPoseForBenchmark.view(4,4), 167 | CurrentGTPoseForBenchmark.view(4,4).to(self.device)) 168 | self.CurrentGTPoseForBenchmark = CurrentGTPoseForBenchmark 169 | self.travelledSoFarForEval = self.travelledSoFarForEval + step.cpu().detach().item() 170 | return 171 | def setOffsetToGoal(self, observation): 172 | self.offset_to_goal = torch.tensor(observation['observation']['measurements']['offset_to_goal']) 173 | self.estimatedGoalPos2D = minosOffsetToGoal2MapGoalPosition(self.offset_to_goal, 174 | self.pose6D.squeeze(), 175 | self.map_cell_size, 176 | self.map_size_meters) 177 | self.estimatedGoalPos6D = plannedPath2TPs([self.estimatedGoalPos2D], 178 | self.map_cell_size, 179 | self.map_size_meters, 180 | 1.0).to(self.device)[0] 181 | return 182 | def updateInternalState(self, minos_observation): 183 | RandomAgent.updateInternalState(self,minos_observation) 184 | t= time.time() 185 | # 186 | if self.GT_modalities is None: 187 | self.GT_modalities = deepcopy(minos_observation['info']['agent_state']) 188 | self.pos_init = deepcopy(np.array(self.GT_modalities['position'])[::-1]) 189 | self.ori_init = deepcopy(np.array(self.GT_modalities['orientation'])[::-1]) 190 | self.angle_orig = norm_ang(np.arctan2(self.ori_init[0], self.ori_init[2])) 191 | pos = deepcopy(np.array(minos_observation['info']['agent_state']['position'])[::-1]) 192 | ori = deepcopy(np.array(minos_observation['info']['agent_state']['orientation'])[::-1]) 193 | angle = norm_ang(np.arctan2(ori[0], ori[2])) 194 | P = np.zeros((3,4)).astype(np.float32) 195 | P[:,3] = pos - self.pos_init 196 | P03 = P[0,3] * np.cos(self.angle_orig) - P[2,3] * np.sin(self.angle_orig) 197 | P23 = P[2,3] * np.cos(self.angle_orig) + P[0,3] * np.sin(self.angle_orig) 198 | P[0,3] = P03 199 | P[2,3] = P23 200 | P[1,1] = 1.0 201 | da = angle - self.angle_orig 202 | P[0,0] = np.cos(da) 203 | P[2,2] = np.cos(da) 204 | P[0,2] = np.sin(da) 205 | P[2,0] = -np.sin(da) 206 | self.pose6D = homogenizeP(torch.from_numpy(deepcopy(P)).view(3,4).to(self.device).float()) 207 | t = time.time() 208 | self.setOffsetToGoal(minos_observation) 209 | self.position_history.append(self.pose6D) 210 | #Mapping 211 | if self.MapIsHere is None: 212 | global_map_GT = minos_observation['observation']['map']['data'][:,:,:3].astype(np.uint8) 213 | w = minos_observation['observation']['map']['width'] 214 | h = minos_observation['observation']['map']['height'] 215 | global_map_GT = global_map_GT.reshape(h,w,3) 216 | global_map_GT = (global_map_GT[:,:,2] ==127).astype(np.uint8) * 255 217 | goal_cell_pose = minos_observation['info']['goal'][0]['cell'] 218 | goal_pose = minos_observation['info']['goal'][0]['position'] 219 | b1 = -10 * goal_pose[0] + goal_cell_pose['i'] 220 | b2 = -10 * goal_pose[2] + goal_cell_pose['j'] 221 | ag_i = int(minos_observation['info']['agent_state']['position'][0] * 10 + b1) 222 | ag_j = int(minos_observation['info']['agent_state']['position'][2] * 10 + b2) 223 | rotmap = Image.fromarray(global_map_GT.astype(np.uint8)) 224 | delta_w = max(ag_i, w - ag_i) 225 | delta_h = max(ag_j, h - ag_j) 226 | new_size = max(delta_w, delta_h) * 2 227 | padding = (new_size//2 - ag_i, 228 | new_size//2 - ag_j, 229 | new_size//2 - (w - ag_i), 230 | new_size//2 - (h - ag_j)) 231 | padded_map = ImageOps.expand(rotmap, padding) 232 | agent_ori = np.array(minos_observation['info']['agent_state']['orientation'])[::-1] 233 | angle = np.degrees(norm_ang(-np.pi/2.0 + np.arctan2(agent_ori[0], agent_ori[2]))) 234 | rotmap = padded_map.rotate(angle, expand=True) 235 | rotmap = np.array(rotmap).astype(np.float32) 236 | rotmap = rotmap / rotmap.max() 237 | cur_obs = torch.from_numpy(deepcopy(rotmap[:,::-1])).to(self.device).squeeze() 238 | ctr = cur_obs.size(0)//2 239 | cur_obs[ctr-1:ctr+2, ctr-1:ctr+2] = 0 240 | pd2 = (self.map2DObstacles.size(2) - cur_obs.size(0) )// 2 241 | GP = self.estimatedGoalPos2D 242 | gy = GP[0,0].long() 243 | gx = GP[0,1].long() 244 | self.map2DObstacles[0,0,pd2:pd2+cur_obs.size(0),pd2:pd2+cur_obs.size(1)] = self.map2DObstacles[0,0,pd2:pd2+cur_obs.size(0),pd2:pd2+cur_obs.size(1)].clone() + 120*cur_obs 245 | self.map2DObstacles[0,0,gy, gx] = self.map2DObstacles[0,0,gy, gx]*0 246 | self.GTMAP = self.map2DObstacles 247 | self.MapIsHere = True 248 | return True 249 | def act(self, minos_observation = None, random_prob = 1.0): 250 | self.updateInternalState(minos_observation) 251 | self.updateTravelledDist(self.obs) 252 | return True 253 | def rawMap2PlanerReady(self, rawmap, start_map, goal_map): 254 | map1 = (rawmap / float(128))**2 255 | map1 = torch.clamp(map1, min=0, max=1.0) - start_map - F.max_pool2d(goal_map, 3, stride=1, padding=1) 256 | return torch.relu(map1) 257 | def reset(self): 258 | self.InitGTPoseForBenchmark = None 259 | self.travelledSoFarForEval = 0.0 260 | self.MapIsHere = None 261 | self.map2DObstacles = self.initMap2D() 262 | self.pose6D_GT_history = [] 263 | self.GT_modalities = None 264 | self.position_history = [] 265 | self.GTMAP = self.map2DObstacles 266 | return 267 | 268 | class BlindAgent(RandomAgent): 269 | def __init__(self, 270 | pos_threshold = 0.01, 271 | angle_threshold = float(np.deg2rad(15)), 272 | **kwargs): 273 | super(BlindAgent, self).__init__() 274 | self.reset() 275 | self.pos_th = pos_threshold 276 | self.angle_th = angle_threshold 277 | return 278 | def decideWhatToDo(self): 279 | distance_to_goal = self.obs['observation']['measurements']['distance_to_goal'][0] 280 | rot_vector_to_goal = np.array(self.obs['observation']['measurements']['direction_to_goal']) 281 | angle_to_goal = -norm_ang(np.arctan2(rot_vector_to_goal[0], rot_vector_to_goal[2])) 282 | #print (angle_to_goal) 283 | command = "Idle" 284 | if distance_to_goal <= self.pos_th: 285 | return command 286 | if (abs(angle_to_goal) < self.angle_th): 287 | command = "forwards" 288 | else: 289 | if (angle_to_goal > 0) and (angle_to_goal < pi): 290 | command = 'turnLeft' 291 | elif (angle_to_goal > pi): 292 | command = 'turnRight' 293 | elif (angle_to_goal < 0) and (angle_to_goal > -pi): 294 | command = 'turnRight' 295 | else: 296 | command = 'turnLeft' 297 | return command 298 | def act(self, minos_observation = None, random_prob = 0.1): 299 | self.updateInternalState(minos_observation) 300 | # Act 301 | action = np.zeros(self.num_actions) 302 | success = self.isGoalReached() 303 | if success: 304 | return action, success 305 | command = self.decideWhatToDo() 306 | random_action = np.random.randint(0, self.num_actions, size = (1)) 307 | act_randomly = np.random.uniform(0,1,1) < random_prob 308 | if act_randomly: 309 | action[random_action] = 1 310 | else: 311 | action = command2NumericAction(command) 312 | return action, success 313 | 314 | class ClassicAgentWithDepth(RandomAgent): 315 | def __init__(self, 316 | slam_vocab_path = '', 317 | slam_settings_path = '', 318 | pos_threshold = 0.15, 319 | angle_threshold = float(np.deg2rad(15)), 320 | far_pos_threshold = 0.5, 321 | obstacle_th = 40, 322 | map_size = 80, 323 | map_resolution = 0.1, 324 | device = torch.device('cpu'), 325 | **kwargs): 326 | super(ClassicAgentWithDepth, self).__init__(**kwargs) 327 | self.slam_vocab_path = slam_vocab_path 328 | self.slam_settings_path = slam_settings_path 329 | self.slam = orbslam2.System(slam_vocab_path,slam_settings_path, orbslam2.Sensor.RGBD) 330 | self.slam.set_use_viewer(False) 331 | self.slam.initialize() 332 | self.tracking_is_OK = False 333 | self.device = device 334 | self.map_size_meters = map_size 335 | self.map_cell_size = map_resolution 336 | self.waypoint_id = 0 337 | self.slam_to_world = 1.0 338 | self.pos_th = pos_threshold 339 | self.far_pos_threshold = far_pos_threshold 340 | self.angle_th = angle_threshold 341 | self.obstacle_th = obstacle_th 342 | self.plannedWaypoints = [] 343 | self.mapper = DirectDepthMapper(**kwargs) 344 | self.planner = DifferentiableStarPlanner(**kwargs) 345 | self.timing = False 346 | self.reset() 347 | return 348 | def reset(self): 349 | super(ClassicAgentWithDepth, self).reset() 350 | self.offset_to_goal = None 351 | self.waypointPose6D = None 352 | self.unseen_obstacle = False 353 | self.action_history = [] 354 | self.plannedWaypoints = [] 355 | self.map2DObstacles = self.initMap2D() 356 | n,ch, height, width = self.map2DObstacles.size() 357 | self.coordinatesGrid = generate_2dgrid(height, 358 | width,False).to(self.device) 359 | self.pose6D = self.initPose6D() 360 | self.action_history = [] 361 | self.pose6D_history = [] 362 | self.position_history = [] 363 | self.planned2Dpath = torch.zeros((0)) 364 | self.slam.reset() 365 | self.toDoList = [] 366 | if self.waypoint_id > 92233720368547758: #not used here, reserved for hybrid agent 367 | self.waypoint_id = 0 368 | return 369 | def updateInternalState(self, minos_observation): 370 | super(ClassicAgentWithDepth, self).updateInternalState(minos_observation) 371 | rgb, depth, cur_time = self.rgbAndDAndTimeFromObservation(minos_observation) 372 | t= time.time() 373 | try: 374 | self.slam.process_image_rgbd(rgb, depth, cur_time) 375 | if self.timing: 376 | print(time.time() -t , 'ORB_SLAM2') 377 | self.tracking_is_OK = str(self.slam.get_tracking_state()) == "OK" 378 | except: 379 | print ("Warning!!!! ORBSLAM processing frame error") 380 | self.tracking_is_OK = False 381 | if not self.tracking_is_OK: 382 | self.reset() 383 | t = time.time() 384 | self.setOffsetToGoal(minos_observation) 385 | if self.tracking_is_OK: 386 | trajectory_history = np.array(self.slam.get_trajectory_points()) 387 | self.pose6D = homogenizeP(torch.from_numpy(trajectory_history[-1])[1:].view(3,4).to(self.device)).view(1,4,4) 388 | self.trajectory_history = trajectory_history 389 | if (len(self.position_history) > 1): 390 | previous_step = getPosDiffLength(self.pose6D.view(4,4), 391 | torch.from_numpy(self.position_history[-1]).view(4,4).to(self.device)) 392 | if action2Command(self.action_history[-1]) == "forwards": 393 | self.unseen_obstacle = previous_step.item() <= 0.001 #hardcoded threshold for not moving 394 | current_obstacles = self.mapper(torch.from_numpy(depth).to(self.device).squeeze(),self.pose6D).to(self.device) 395 | self.current_obstacles = current_obstacles 396 | self.map2DObstacles = torch.max(self.map2DObstacles, 397 | current_obstacles.unsqueeze(0).unsqueeze(0)) 398 | if self.timing: 399 | print(time.time() -t , 'Mapping') 400 | return True 401 | def initPose6D(self): 402 | return torch.eye(4).float().to(self.device) 403 | def mapSizeInCells(self): 404 | return int(self.map_size_meters / self.map_cell_size) 405 | def initMap2D(self): 406 | return torch.zeros(1,1,self.mapSizeInCells(),self.mapSizeInCells()).float().to(self.device) 407 | def getCurrentOrientationOnMap(self): 408 | self.pose6D = self.pose6D.view(1,4,4) 409 | return torch.tensor([[self.pose6D[0,0,0], self.pose6D[0,0,2]], 410 | [self.pose6D[0,2,0], self.pose6D[0,2,2]]]) 411 | def getCurrentPositionOnMap(self, do_floor = True): 412 | return projectTPsIntoWorldMap(self.pose6D.view(1,4,4), self.map_cell_size, self.map_size_meters, do_floor) 413 | def act(self, minos_observation, random_prob = 0.1): 414 | # Update internal state 415 | t = time.time() 416 | cc= 0 417 | updateIsOK = self.updateInternalState(minos_observation) 418 | while not updateIsOK: 419 | updateIsOK = self.updateInternalState(minos_observation) 420 | cc+=1 421 | if cc>2: 422 | break 423 | if self.timing: 424 | print (time.time() - t, " s, update internal state") 425 | self.position_history.append(self.pose6D.detach().cpu().numpy().reshape(1,4,4)) 426 | success = self.isGoalReached() 427 | if success: 428 | self.action_history.append(action) 429 | return action, success 430 | # Plan action 431 | t = time.time() 432 | self.planned2Dpath, self.plannedWaypoints = self.planPath() 433 | if self.timing: 434 | print (time.time() - t, " s, Planning") 435 | t = time.time() 436 | # Act 437 | if self.waypointPose6D is None: 438 | self.waypointPose6D = self.getValidWaypointPose6D() 439 | if self.isWaypointReached(self.waypointPose6D) or not self.tracking_is_OK: 440 | self.waypointPose6D = self.getValidWaypointPose6D() 441 | self.waypoint_id+=1 442 | action = self.decideWhatToDo() 443 | #May be random? 444 | random_idx = torch.randint(3, size = (1,)).long() 445 | random_action = torch.zeros(self.num_actions).cpu().numpy() 446 | random_action[random_idx] = 1 447 | what_to_do = np.random.uniform(0,1,1) 448 | if what_to_do < random_prob: 449 | action = random_action 450 | if self.timing: 451 | print (time.time() - t, " s, plan 2 action") 452 | self.action_history.append(action) 453 | return action, success 454 | def isWaypointGood(self,pose6d): 455 | Pinit = self.pose6D.squeeze() 456 | dist_diff = getPosDiffLength(Pinit,pose6d) 457 | valid = dist_diff > self.far_pos_threshold 458 | return valid.item() 459 | def isWaypointReached(self,pose6d): 460 | Pinit = self.pose6D.squeeze() 461 | dist_diff = getPosDiffLength(Pinit,pose6d) 462 | reached = dist_diff <= self.pos_th 463 | return reached.item() 464 | def getWaypointDistDir(self): 465 | angle = getDirection(self.pose6D.squeeze(), self.waypointPose6D.squeeze(),0,0) 466 | dist = getPosDiffLength(self.pose6D.squeeze(), self.waypointPose6D.squeeze()) 467 | return torch.cat([dist.view(1,1), torch.sin(angle).view(1,1),torch.cos(angle).view(1,1)], dim = 1) 468 | def getValidWaypointPose6D(self): 469 | good_waypoint_found = False 470 | Pinit = self.pose6D.squeeze() 471 | Pnext = self.plannedWaypoints[0] 472 | while not self.isWaypointGood(Pnext): 473 | if (len(self.plannedWaypoints) > 1): 474 | self.plannedWaypoints = self.plannedWaypoints[1:] 475 | Pnext = self.plannedWaypoints[0] 476 | else: 477 | Pnext = self.estimatedGoalPos6D.squeeze() 478 | break 479 | return Pnext 480 | def setOffsetToGoal(self, observation): 481 | self.offset_to_goal = torch.tensor(observation['observation']['measurements']['offset_to_goal']) 482 | self.estimatedGoalPos2D = minosOffsetToGoal2MapGoalPosition(self.offset_to_goal, 483 | self.pose6D.squeeze(), 484 | self.map_cell_size, 485 | self.map_size_meters) 486 | self.estimatedGoalPos6D = plannedPath2TPs([self.estimatedGoalPos2D], 487 | self.map_cell_size, 488 | self.map_size_meters, 489 | 1.0).to(self.device)[0] 490 | return 491 | def rgbAndDAndTimeFromObservation(self,minos_observation): 492 | rgb = minos_observation['observation']['sensors']['color']['data'][:,:,:3] 493 | depth = None 494 | if 'depth' in minos_observation['observation']['sensors']: 495 | depth = minos_observation['observation']['sensors']['depth']['data'] 496 | cur_time = minos_observation['observation']['time'] 497 | return rgb, depth, cur_time 498 | def prevPlanIsNotValid(self): 499 | if len(self.planned2Dpath) == 0: 500 | return True 501 | pp = torch.cat(self.planned2Dpath).detach().cpu().view(-1,2) 502 | binary_map = self.map2DObstacles.squeeze().detach() >= self.obstacle_th 503 | obstacles_on_path = (binary_map[pp[:,0].long(),pp[:,1].long()]).long().sum().item() > 0 504 | return obstacles_on_path# obstacles_nearby or obstacles_on_path 505 | def rawMap2PlanerReady(self, rawmap, start_map, goal_map): 506 | map1 = (rawmap / float(self.obstacle_th))**2 507 | map1 = torch.clamp(map1, min=0, max=1.0) - start_map - F.max_pool2d(goal_map, 3, stride=1, padding=1) 508 | return torch.relu(map1) 509 | def planPath(self, overwrite = False): 510 | t=time.time() 511 | if (not self.prevPlanIsNotValid()) and (not overwrite) and (len(self.plannedWaypoints) > 0): 512 | return self.planned2Dpath, self.plannedWaypoints 513 | self.waypointPose6D = None 514 | current_pos = self.getCurrentPositionOnMap() 515 | start_map = torch.zeros_like(self.map2DObstacles).to(self.device) 516 | start_map[0,0,current_pos[0,0].long(),current_pos[0,1].long()] = 1.0 517 | goal_map = torch.zeros_like(self.map2DObstacles).to(self.device) 518 | goal_map[0,0,self.estimatedGoalPos2D[0,0].long(),self.estimatedGoalPos2D[0,1].long()] = 1.0 519 | path, cost = self.planner(self.rawMap2PlanerReady(self.map2DObstacles, start_map, goal_map).to(self.device), 520 | self.coordinatesGrid.to(self.device), goal_map.to(self.device), start_map.to(self.device)) 521 | if len(path) == 0: 522 | return path, [] 523 | if self.timing: 524 | print(time.time() - t, ' s, Planning') 525 | t=time.time() 526 | plannedWaypoints = plannedPath2TPs(path, 527 | self.map_cell_size, 528 | self.map_size_meters, 529 | 1.0, False).to(self.device) 530 | return path, plannedWaypoints 531 | def plannerPrediction2Command(self, Pnext): 532 | command = "Idle" 533 | Pinit = self.pose6D.squeeze() 534 | d_angle_rot_th = self.angle_th 535 | pos_th = self.pos_th 536 | if getPosDiffLength(Pinit,Pnext) <= pos_th: 537 | return command 538 | d_angle = angleToPi_2_MinusPi_2(getDirection(Pinit, Pnext, ang_th = d_angle_rot_th, pos_th = pos_th)) 539 | if (abs(d_angle) < d_angle_rot_th): 540 | command = "forwards" 541 | else: 542 | if (d_angle > 0) and (d_angle < pi): 543 | command = 'turnLeft' 544 | elif (d_angle > pi): 545 | command = 'turnRight' 546 | elif (d_angle < 0) and (d_angle > -pi): 547 | command = 'turnRight' 548 | else: 549 | command = 'turnLeft' 550 | return command 551 | def decideWhatToDo(self): 552 | action = None 553 | if self.isGoalReached(): 554 | action = command2NumericAction("Idle") 555 | print ("!!!!!!!!!!!! Goal reached!") 556 | return action 557 | if self.unseen_obstacle: 558 | command = 'turnRight' 559 | return command2NumericAction(command) 560 | command = "Idle" 561 | command = self.plannerPrediction2Command(self.waypointPose6D) 562 | return command2NumericAction(command) 563 | 564 | class ClassicAgentRGB(RandomAgent): 565 | def __init__(self, 566 | slam_vocab_path = '', 567 | slam_settings_path = '', 568 | pos_threshold = 0.15, 569 | angle_threshold = float(np.deg2rad(15)), 570 | far_pos_threshold = 0.5, 571 | obstacle_th = 40, 572 | map_size = 80, 573 | map_resolution = 0.1, 574 | device = torch.device('cpu'), 575 | **kwargs): 576 | super(ClassicAgentRGB, self).__init__(**kwargs) 577 | self.slam_vocab_path = slam_vocab_path 578 | self.slam_settings_path = slam_settings_path 579 | self.slam = orbslam2.System(slam_vocab_path,slam_settings_path, orbslam2.Sensor.MONOCULAR) 580 | self.slam.set_use_viewer(False) 581 | self.slam.initialize() 582 | self.tracking_is_OK = False 583 | self.device = device 584 | self.map_size_meters = map_size 585 | self.map_cell_size = map_resolution 586 | self.waypoint_id = 0 587 | self.slam_to_world = 1.0 588 | self.pos_th = pos_threshold 589 | self.far_pos_threshold = far_pos_threshold 590 | self.angle_th = angle_threshold 591 | self.obstacle_th = obstacle_th 592 | self.plannedWaypoints = [] 593 | self.mapper = SparseDepthMapper(**kwargs) 594 | self.planner = DifferentiableStarPlanner(**kwargs) 595 | self.timing = False 596 | self.reset() 597 | return 598 | def reset(self): 599 | super(ClassicAgentRGB, self).reset() 600 | self.offset_to_goal = None 601 | self.waypointPose6D = None 602 | self.unseen_obstacle = False 603 | self.action_history = [] 604 | self.plannedWaypoints = [] 605 | self.map2DObstacles = self.initMap2D() 606 | n,ch, height, width = self.map2DObstacles.size() 607 | self.coordinatesGrid = generate_2dgrid(height, 608 | width,False).to(self.device) 609 | self.pose6D = self.initPose6D() 610 | self.action_history = [] 611 | self.pose6D_history = [] 612 | self.position_history = [] 613 | self.planned2Dpath = torch.zeros((0)) 614 | self.slam.reset() 615 | self.slam.slam_to_world = None 616 | self.slam.slam_camera_height = None 617 | self.slam.floor_level = None 618 | self.toDoList = [] 619 | if self.waypoint_id > 92233720368547758: #not used here, reserved for hybrid agent 620 | self.waypoint_id = 0 621 | return 622 | def set_slam_scale(self): 623 | if len(self.toDoList) == 0: 624 | return 625 | if ((str(self.slam.get_tracking_state()) == "OK") 626 | and (self.slam.slam_to_world is None) 627 | and (self.toDoList[0] == 'lookUp')): #We are looking down 628 | new_map_pts = np.array(self.slam.get_tracked_mappoints()).reshape(-1,3) 629 | self.slam.floor_level = np.median(new_map_pts[:,1]) 630 | self.slam.slam_camera_height = np.abs(self.slam.floor_level) 631 | self.slam.slam_to_world = self.world_camera_height / self.slam.slam_camera_height 632 | return 633 | def updateInternalState(self, minos_observation): 634 | super(ClassicAgentRGB, self).updateInternalState(minos_observation) 635 | rgb, _ , cur_time = self.rgbAndDAndTimeFromObservation(minos_observation) 636 | t= time.time() 637 | self.setOffsetToGoal(self.obs) 638 | try: 639 | self.slam.process_image_mono(rgb, cur_time) 640 | if self.timing: 641 | print(time.time() -t , 'ORB_SLAM2') 642 | except: 643 | print ("Warning!!!! ORBSLAM processing frame error") 644 | self.tracking_is_OK = False 645 | self.reset() 646 | self.tracking_is_OK = str(self.slam.get_tracking_state()) == "OK" 647 | t =time.time() 648 | self.set_slam_scale() 649 | if self.tracking_is_OK: 650 | trajectory_history = np.array(self.slam.get_trajectory_points()) 651 | self.pose6D = homogenizeP(torch.from_numpy(trajectory_history[-1]).float()[1:].view(3,4).to(self.device)).view(1,4,4) 652 | self.trajectory_history = trajectory_history 653 | if (len(self.position_history) > 1): 654 | previous_step = getPosDiffLength(self.pose6D.view(4,4), 655 | torch.from_numpy(self.position_history[-1]).view(4,4).to(self.device)) 656 | if action2Command(self.action_history[-1]) == "forwards": 657 | self.unseen_obstacle = previous_step.item() <= 0.001 #hardcoded threshold for not moving 658 | if (str(self.slam.get_tracking_state()) == "OK") and (self.slam.slam_to_world is not None): 659 | current_map_pts = np.array(self.slam.get_tracked_mappoints()).reshape(-1,3) 660 | self.current_obstacles = self.mapper(torch.from_numpy(current_map_pts).float().to(self.device).squeeze(), 661 | self.pose6D).to(self.device) 662 | self.map2DObstacles = torch.max(self.map2DObstacles, 663 | current_obstacles.unsqueeze(0).unsqueeze(0))#[0] 664 | if self.timing: 665 | print(time.time() -t , 'Mapping') 666 | return True 667 | def decideWhatToDo(self): 668 | action = None 669 | if self.isGoalReached(): 670 | action = command2NumericAction("Idle") 671 | print ("!!!!!!!!!!!! Goal reached!") 672 | return action 673 | if (str(self.slam.get_tracking_state()) == 'NOT_INITIALIZED'): 674 | if random.random() > 0.3: 675 | return command2NumericAction('forwards') 676 | else: 677 | return command2NumericAction('backwards') 678 | if len(self.toDoList) > 0: 679 | command = self.toDoList.pop(0) 680 | return command2NumericAction(command) 681 | if (str(self.slam.get_tracking_state()) == "OK") and (self.slam.slam_to_world is None) : 682 | print ("ORBSLAM INITIALIZATION") 683 | for i in range(100//5): 684 | self.toDoList.append('lookDown') 685 | if i%3 ==0: 686 | self.toDoList.append('forwards') 687 | if i%3 ==1: 688 | self.toDoList.append('backwards') 689 | for i in range(90//5): 690 | self.toDoList.append('lookUp') 691 | command = self.toDoList.pop(0) 692 | return command2NumericAction(command) 693 | command = "Idle" 694 | command = self.plannerPrediction2Command(self.waypointPose6D) 695 | return command2NumericAction(command) 696 | def initPose6D(self): 697 | return torch.eye(4).float().to(self.device) 698 | def mapSizeInCells(self): 699 | return int(self.map_size_meters / self.map_cell_size) 700 | def initMap2D(self): 701 | return torch.zeros(1,1,self.mapSizeInCells(),self.mapSizeInCells()).float().to(self.device) 702 | def getCurrentOrientationOnMap(self): 703 | self.pose6D = self.pose6D.view(1,4,4) 704 | return torch.tensor([[self.pose6D[0,0,0], self.pose6D[0,0,2]], 705 | [self.pose6D[0,2,0], self.pose6D[0,2,2]]]) 706 | def getCurrentPositionOnMap(self, do_floor = True): 707 | return projectTPsIntoWorldMap(self.pose6D.view(1,4,4), self.map_cell_size, self.map_size_meters, do_floor) 708 | def act(self, minos_observation, random_prob = 0.1): 709 | # Update internal state 710 | t = time.time() 711 | cc= 0 712 | updateIsOK = self.updateInternalState(minos_observation) 713 | while not updateIsOK: 714 | updateIsOK = self.updateInternalState(minos_observation) 715 | cc+=1 716 | if cc>2: 717 | break 718 | if self.timing: 719 | print (time.time() - t, " s, update internal state") 720 | self.position_history.append(self.pose6D.detach().cpu().numpy().reshape(1,4,4)) 721 | success = self.isGoalReached() 722 | if success: 723 | self.action_history.append(action) 724 | return action, success 725 | # Plan action 726 | t = time.time() 727 | self.planned2Dpath, self.plannedWaypoints = self.planPath() 728 | if self.timing: 729 | print (time.time() - t, " s, Planning") 730 | t = time.time() 731 | # Act 732 | if self.waypointPose6D is None: 733 | self.waypointPose6D = self.getValidWaypointPose6D() 734 | if self.isWaypointReached(self.waypointPose6D) or not self.tracking_is_OK: 735 | self.waypointPose6D = self.getValidWaypointPose6D() 736 | self.waypoint_id+=1 737 | action = self.decideWhatToDo() 738 | #May be random? 739 | random_idx = torch.randint(3, size = (1,)).long() 740 | random_action = torch.zeros(self.num_actions).cpu().numpy() 741 | random_action[random_idx] = 1 742 | what_to_do = np.random.uniform(0,1,1) 743 | if what_to_do < random_prob: 744 | action = random_action 745 | if self.timing: 746 | print (time.time() - t, " s, plan 2 action") 747 | self.action_history.append(action) 748 | return action, success 749 | def isWaypointGood(self,pose6d): 750 | Pinit = self.pose6D.squeeze() 751 | dist_diff = getPosDiffLength(Pinit,pose6d) 752 | valid = dist_diff > self.far_pos_threshold 753 | return valid.item() 754 | def isWaypointReached(self,pose6d): 755 | Pinit = self.pose6D.squeeze() 756 | dist_diff = getPosDiffLength(Pinit,pose6d) 757 | reached = dist_diff <= self.pos_th 758 | return reached.item() 759 | def getWaypointDistDir(self): 760 | angle = getDirection(self.pose6D.squeeze(), self.waypointPose6D.squeeze(),0,0) 761 | dist = getPosDiffLength(self.pose6D.squeeze(), self.waypointPose6D.squeeze()) 762 | return torch.cat([dist.view(1,1), torch.sin(angle).view(1,1),torch.cos(angle).view(1,1)], dim = 1) 763 | def getValidWaypointPose6D(self): 764 | good_waypoint_found = False 765 | Pinit = self.pose6D.squeeze() 766 | Pnext = self.plannedWaypoints[0] 767 | while not self.isWaypointGood(Pnext): 768 | if (len(self.plannedWaypoints) > 1): 769 | self.plannedWaypoints = self.plannedWaypoints[1:] 770 | Pnext = self.plannedWaypoints[0] 771 | else: 772 | Pnext = self.estimatedGoalPos6D.squeeze() 773 | break 774 | return Pnext 775 | def setOffsetToGoal(self, observation): 776 | self.offset_to_goal = torch.tensor(observation['observation']['measurements']['offset_to_goal']) 777 | self.estimatedGoalPos2D = minosOffsetToGoal2MapGoalPosition(self.offset_to_goal, 778 | self.pose6D.squeeze(), 779 | self.map_cell_size, 780 | self.map_size_meters) 781 | self.estimatedGoalPos6D = plannedPath2TPs([self.estimatedGoalPos2D], 782 | self.map_cell_size, 783 | self.map_size_meters, 784 | 1.0).to(self.device)[0] 785 | return 786 | def rgbAndDAndTimeFromObservation(self,minos_observation): 787 | rgb = minos_observation['observation']['sensors']['color']['data'][:,:,:3] 788 | depth = None 789 | if 'depth' in minos_observation['observation']['sensors']: 790 | depth = minos_observation['observation']['sensors']['depth']['data'] 791 | cur_time = minos_observation['observation']['time'] 792 | return rgb, depth, cur_time 793 | def prevPlanIsNotValid(self): 794 | if len(self.planned2Dpath) == 0: 795 | return True 796 | pp = torch.cat(self.planned2Dpath).detach().cpu().view(-1,2) 797 | binary_map = self.map2DObstacles.squeeze().detach() >= self.obstacle_th 798 | obstacles_on_path = (binary_map[pp[:,0].long(),pp[:,1].long()]).long().sum().item() > 0 799 | return obstacles_on_path# obstacles_nearby or obstacles_on_path 800 | def rawMap2PlanerReady(self, rawmap, start_map, goal_map): 801 | map1 = (rawmap / float(self.obstacle_th))**2 802 | map1 = torch.clamp(map1, min=0, max=1.0) - start_map - F.max_pool2d(goal_map, 3, stride=1, padding=1) 803 | return torch.relu(map1) 804 | def planPath(self, overwrite = False): 805 | t=time.time() 806 | if (not self.prevPlanIsNotValid()) and (not overwrite) and (len(self.plannedWaypoints) > 0): 807 | return self.planned2Dpath, self.plannedWaypoints 808 | self.waypointPose6D = None 809 | current_pos = self.getCurrentPositionOnMap() 810 | start_map = torch.zeros_like(self.map2DObstacles).to(self.device) 811 | start_map[0,0,current_pos[0,0].long(),current_pos[0,1].long()] = 1.0 812 | goal_map = torch.zeros_like(self.map2DObstacles).to(self.device) 813 | goal_map[0,0,self.estimatedGoalPos2D[0,0].long(),self.estimatedGoalPos2D[0,1].long()] = 1.0 814 | path, cost = self.planner(self.rawMap2PlanerReady(self.map2DObstacles, start_map, goal_map).to(self.device), 815 | self.coordinatesGrid.to(self.device), goal_map.to(self.device), start_map.to(self.device)) 816 | if len(path) == 0: 817 | return path, [] 818 | if self.timing: 819 | print(time.time() - t, ' s, Planning') 820 | t=time.time() 821 | plannedWaypoints = plannedPath2TPs(path, 822 | self.map_cell_size, 823 | self.map_size_meters, 824 | 1.0, False).to(self.device) 825 | return path, plannedWaypoints 826 | def plannerPrediction2Command(self, Pnext): 827 | command = "Idle" 828 | Pinit = self.pose6D.squeeze() 829 | d_angle_rot_th = self.angle_th 830 | pos_th = self.pos_th 831 | if getPosDiffLength(Pinit,Pnext) <= pos_th: 832 | return command 833 | d_angle = angleToPi_2_MinusPi_2(getDirection(Pinit, Pnext, ang_th = d_angle_rot_th, pos_th = pos_th)) 834 | if (abs(d_angle) < d_angle_rot_th): 835 | command = "forwards" 836 | else: 837 | if (d_angle > 0) and (d_angle < pi): 838 | command = 'turnLeft' 839 | elif (d_angle > pi): 840 | command = 'turnRight' 841 | elif (d_angle < 0) and (d_angle > -pi): 842 | command = 'turnRight' 843 | else: 844 | command = 'turnLeft' 845 | return command 846 | 847 | class CheatingAgentWithGTPose(ClassicAgentWithDepth): 848 | def __init__(self, 849 | pos_threshold = 0.15, 850 | angle_threshold = float(np.deg2rad(15)), 851 | far_pos_threshold = 0.5, 852 | obstacle_th = 40, 853 | map_size = 80, 854 | map_resolution = 0.1, 855 | device = torch.device('cpu'), 856 | **kwargs): 857 | RandomAgent.__init__(self, **kwargs) 858 | self.tracking_is_OK = True 859 | self.device = device 860 | self.map_size_meters = map_size 861 | self.map_cell_size = map_resolution 862 | self.waypoint_id = 0 863 | self.pos_th = pos_threshold 864 | self.far_pos_threshold = far_pos_threshold 865 | self.angle_th = angle_threshold 866 | self.obstacle_th = obstacle_th 867 | self.plannedWaypoints = [] 868 | self.mapper = DirectDepthMapper(**kwargs) 869 | self.planner = DifferentiableStarPlanner(**kwargs) 870 | self.timing = False 871 | self.reset() 872 | return 873 | def reset(self): 874 | self.steps = 0 875 | self.GT_modalities = None 876 | self.offset_to_goal = None 877 | self.waypointPose6D = None 878 | self.unseen_obstacle = False 879 | self.action_history = [] 880 | self.plannedWaypoints = [] 881 | self.map2DObstacles = self.initMap2D() 882 | n,ch, height, width = self.map2DObstacles.size() 883 | self.coordinatesGrid = generate_2dgrid(height, 884 | width,False).to(self.device) 885 | self.pose6D = self.initPose6D() 886 | self.action_history = [] 887 | self.pose6D_history = [] 888 | self.position_history = [] 889 | self.planned2Dpath = torch.zeros((0)) 890 | self.MapIsHere = None 891 | self.toDoList = [] 892 | if self.waypoint_id > 92233720368547758: #not used here, reserved for hybrid agent 893 | self.waypoint_id = 0 894 | return 895 | def updateInternalState(self, minos_observation): 896 | RandomAgent.updateInternalState(self,minos_observation) 897 | rgb, depth, cur_time = self.rgbAndDAndTimeFromObservation(minos_observation) 898 | t= time.time() 899 | # 900 | if self.GT_modalities is None: 901 | self.GT_modalities = deepcopy(minos_observation['info']['agent_state']) 902 | self.pos_init = deepcopy(np.array(self.GT_modalities['position'])[::-1]) 903 | self.ori_init = deepcopy(np.array(self.GT_modalities['orientation'])[::-1]) 904 | self.angle_orig = norm_ang(np.arctan2(self.ori_init[0], self.ori_init[2])) 905 | pos = deepcopy(np.array(minos_observation['info']['agent_state']['position'])[::-1]) 906 | ori = deepcopy(np.array(minos_observation['info']['agent_state']['orientation'])[::-1]) 907 | angle = norm_ang(np.arctan2(ori[0], ori[2])) 908 | P = np.zeros((3,4)).astype(np.float32) 909 | P[:,3] = pos - self.pos_init 910 | P03 = P[0,3] * np.cos(self.angle_orig) - P[2,3] * np.sin(self.angle_orig) 911 | P23 = P[2,3] * np.cos(self.angle_orig) + P[0,3] * np.sin(self.angle_orig) 912 | P[0,3] = P03 913 | P[2,3] = P23 914 | P[1,1] = 1.0 915 | da = angle - self.angle_orig 916 | P[0,0] = np.cos(da) 917 | P[2,2] = np.cos(da) 918 | P[0,2] = np.sin(da) 919 | P[2,0] = -np.sin(da) 920 | self.pose6D = homogenizeP(torch.from_numpy(deepcopy(P)).view(3,4).to(self.device).float()) 921 | t = time.time() 922 | self.setOffsetToGoal(minos_observation) 923 | if (len(self.position_history) > 1): 924 | previous_step = getPosDiffLength(self.pose6D.view(4,4), 925 | torch.from_numpy(self.position_history[-1]).view(4,4).to(self.device)) 926 | if action2Command(self.action_history[-1]) == "forwards": 927 | self.unseen_obstacle = previous_step.item() <= 0.001 #hardcoded threshold for not moving 928 | self.position_history.append(self.pose6D) 929 | current_obstacles = self.mapper(torch.from_numpy(depth).to(self.device).squeeze(),self.pose6D).to(self.device) 930 | self.current_obstacles = current_obstacles 931 | self.map2DObstacles = torch.max(self.map2DObstacles, 932 | current_obstacles.unsqueeze(0).unsqueeze(0)) 933 | if self.timing: 934 | print(time.time() -t , 'Mapping') 935 | return True 936 | 937 | class CheatingAgentWithGTPoseAndMap(CheatingAgentWithGTPose): 938 | def updateInternalState(self, minos_observation): 939 | RandomAgent.updateInternalState(self,minos_observation) 940 | rgb, depth, cur_time = self.rgbAndDAndTimeFromObservation(minos_observation) 941 | t= time.time() 942 | # 943 | if self.GT_modalities is None: 944 | self.GT_modalities = deepcopy(minos_observation['info']['agent_state']) 945 | self.pos_init = deepcopy(np.array(self.GT_modalities['position'])[::-1]) 946 | self.ori_init = deepcopy(np.array(self.GT_modalities['orientation'])[::-1]) 947 | self.angle_orig = norm_ang(np.arctan2(self.ori_init[0], self.ori_init[2])) 948 | pos = deepcopy(np.array(minos_observation['info']['agent_state']['position'])[::-1]) 949 | ori = deepcopy(np.array(minos_observation['info']['agent_state']['orientation'])[::-1]) 950 | angle = norm_ang(np.arctan2(ori[0], ori[2])) 951 | P = np.zeros((3,4)).astype(np.float32) 952 | P[:,3] = pos - self.pos_init 953 | P03 = P[0,3] * np.cos(self.angle_orig) - P[2,3] * np.sin(self.angle_orig) 954 | P23 = P[2,3] * np.cos(self.angle_orig) + P[0,3] * np.sin(self.angle_orig) 955 | P[0,3] = P03 956 | P[2,3] = P23 957 | P[1,1] = 1.0 958 | da = angle - self.angle_orig 959 | P[0,0] = np.cos(da) 960 | P[2,2] = np.cos(da) 961 | P[0,2] = np.sin(da) 962 | P[2,0] = -np.sin(da) 963 | self.pose6D = homogenizeP(torch.from_numpy(deepcopy(P)).view(3,4).to(self.device).float()) 964 | t = time.time() 965 | self.setOffsetToGoal(minos_observation) 966 | if (len(self.position_history) > 1): 967 | previous_step = getPosDiffLength(self.pose6D.view(4,4), 968 | torch.from_numpy(self.position_history[-1]).view(4,4).to(self.device)) 969 | if action2Command(self.action_history[-1]) == "forwards": 970 | self.unseen_obstacle = previous_step.item() <= 0.001 #hardcoded threshold for not moving 971 | self.position_history.append(self.pose6D) 972 | #Mapping 973 | if self.MapIsHere is None: 974 | global_map_GT = minos_observation['observation']['map']['data'][:,:,:3].astype(np.uint8) 975 | w = minos_observation['observation']['map']['width'] 976 | h = minos_observation['observation']['map']['height'] 977 | global_map_GT = global_map_GT.reshape(h,w,3) 978 | global_map_GT = (global_map_GT[:,:,2] ==127).astype(np.uint8) * 255 979 | goal_cell_pose = minos_observation['info']['goal'][0]['cell'] 980 | goal_pose = minos_observation['info']['goal'][0]['position'] 981 | b1 = -10 * goal_pose[0] + goal_cell_pose['i'] 982 | b2 = -10 * goal_pose[2] + goal_cell_pose['j'] 983 | ag_i = int(minos_observation['info']['agent_state']['position'][0] * 10 + b1) 984 | ag_j = int(minos_observation['info']['agent_state']['position'][2] * 10 + b2) 985 | rotmap = Image.fromarray(global_map_GT.astype(np.uint8)) 986 | delta_w = max(ag_i, w - ag_i) 987 | delta_h = max(ag_j, h - ag_j) 988 | new_size = max(delta_w, delta_h) * 2 989 | padding = (new_size//2 - ag_i, 990 | new_size//2 - ag_j, 991 | new_size//2 - (w - ag_i), 992 | new_size//2 - (h - ag_j)) 993 | padded_map = ImageOps.expand(rotmap, padding) 994 | agent_ori = np.array(minos_observation['info']['agent_state']['orientation'])[::-1] 995 | angle = np.degrees(norm_ang(-np.pi/2.0 + np.arctan2(agent_ori[0], agent_ori[2]))) 996 | rotmap = padded_map.rotate(angle, expand=True) 997 | rotmap = np.array(rotmap).astype(np.float32) 998 | rotmap = rotmap / rotmap.max() 999 | cur_obs = torch.from_numpy(deepcopy(rotmap[:,::-1])).to(self.device).squeeze() 1000 | ctr = cur_obs.size(0)//2 1001 | cur_obs[ctr-1:ctr+2, ctr-1:ctr+2] = 0 1002 | pd2 = (self.map2DObstacles.size(2) - cur_obs.size(0) )// 2 1003 | GP = self.estimatedGoalPos2D 1004 | gy = GP[0,0].long() 1005 | gx = GP[0,1].long() 1006 | self.map2DObstacles[0,0,pd2:pd2+cur_obs.size(0),pd2:pd2+cur_obs.size(1)] = self.map2DObstacles[0,0,pd2:pd2+cur_obs.size(0),pd2:pd2+cur_obs.size(1)].clone() + 120*cur_obs 1007 | self.map2DObstacles[0,0,gy, gx] = self.map2DObstacles[0,0,gy, gx]*0 1008 | self.MapIsHere = True 1009 | if self.timing: 1010 | print(time.time() -t , 'Mapping') 1011 | return True -------------------------------------------------------------------------------- /navigation/Control.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | def command2NumericAction(command): 4 | #self.available_controls = params.get('available_controls', 5 | # ['turnLeft', 'turnRight', 'forwards', 'backwards', 'strafeLeft', 'strafeRight', 'lookDown', 'lookUp'] 6 | action = torch.zeros(8) 7 | if command == 'turnLeft': 8 | action[0] = 1 9 | elif command == 'turnRight': 10 | action[1] = 1 11 | elif command == 'forwards': 12 | action[2] = 1 13 | elif command == 'backwards': 14 | action[3] = 1 15 | elif command == 'strafeLeft': 16 | action[4] = 1 17 | elif command == 'strafeRight': 18 | action[5] = 1 19 | elif command == 'lookDown': 20 | action[6] = 1 21 | elif command == 'lookUp': 22 | action[7] = 1 23 | elif command == 'Idle': 24 | action = torch.zeros(8) 25 | else: 26 | print ("Warning, unrecognized action") 27 | pass 28 | return action.numpy() 29 | 30 | def action2Command(action): 31 | command = 'Idle' 32 | if action[0] > 0: 33 | command = 'turnLeft' 34 | elif action[1] > 0: 35 | command = 'turnRight' 36 | elif action[2] > 0: 37 | command = 'forwards' 38 | elif action[3] > 0: 39 | command = 'backwards' 40 | elif action[4] > 0: 41 | command = 'strafeLeft' 42 | elif action[5] > 0: 43 | command = 'strafeRight' 44 | elif action[6] > 0: 45 | command = 'lookDown' 46 | elif action[7] > 0: 47 | command = 'lookUp' 48 | else: 49 | command = 'Idle' 50 | return command 51 | 52 | def action2Command9(action): 53 | command = 'Idle' 54 | idx = torch.argmax(action).item() 55 | if idx == 0: 56 | command = 'turnLeft' 57 | elif idx == 1: 58 | command = 'turnRight' 59 | elif idx == 2: 60 | command = 'forwards' 61 | elif idx == 3: 62 | command = 'backwards' 63 | elif idx == 4: 64 | command = 'strafeLeft' 65 | elif idx == 5: 66 | command = 'strafeRight' 67 | elif idx == 6: 68 | command = 'lookDown' 69 | elif idx == 7: 70 | command = 'lookUp' 71 | elif idx == 8: 72 | command = 'Idle' 73 | else: 74 | command = 'Idle' 75 | return command 76 | -------------------------------------------------------------------------------- /navigation/LearnedAgents.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ducha-aiki/navigation-benchmark/d83431d6648ac1147f53056ed32ce2caae4f702d/navigation/LearnedAgents.py -------------------------------------------------------------------------------- /navigation/LoggerBench.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | import csv 3 | from copy import deepcopy 4 | import os 5 | 6 | class TestLogger(): 7 | def __init__(self, logdir = "logs/"): 8 | self.episodes_detalied = [] 9 | self.episodes_stats = [] 10 | self.init_args = {} 11 | self.logdir = logdir 12 | self.detailed_fname = 'detailed_log.pickle' 13 | self.csvfname = 'short_stats.csv' 14 | self.num_eps = 0 15 | return 16 | def add_init_args(self,args): 17 | self.init_args = deepcopy(args) 18 | return 19 | def add_sim_params(self,params): 20 | self.sim_params = deepcopy(params) 21 | return 22 | #def add_observation(self, ep_id, observation, agent): 23 | # obs_to_save 24 | ## return 25 | def add_episode_stats(self, ep_id, map_name, path_len, shortest_path_length, num_steps, success, h, w, init_dist): 26 | ep1 = {"episode_id": ep_id, 27 | "map_name": map_name, 28 | "path_len": path_len, 29 | "shortest_path_length": shortest_path_length, 30 | "num_steps": num_steps, 31 | "success": success, 32 | "h": h, 33 | "w": w, 34 | "init_dist_to_goal": init_dist} 35 | self.episodes_stats.append(ep1) 36 | return 37 | def save_csv(self): 38 | with open(os.path.join(self.logdir, self.csvfname), 'w') as csvfile: 39 | fieldnames = sorted(self.episodes_stats[0].keys()) 40 | writer = csv.DictWriter(csvfile, fieldnames=fieldnames) 41 | writer.writeheader() 42 | writer.writerows(self.episodes_stats) 43 | return 44 | def save(self): 45 | full_data = {} 46 | full_data["episodes_detalied"] = self.episodes_detalied 47 | full_data["short_stats"] = self.episodes_stats 48 | #full_data["sim_params"] = self.sim_params 49 | #full_data["init_args"] = self.init_args 50 | det_name = os.path.join(self.logdir, self.detailed_fname) 51 | try: 52 | with open(det_name, 'wb') as dp: 53 | pickle.dump(full_data, dp, protocol=2) 54 | except: 55 | print("Cannot write to ", det_name) 56 | return 57 | def load(self): 58 | det_name = os.path.join(self.logdir, self.detailed_fname) 59 | with open(det_name, 'rb') as dp: 60 | full_data = pickle.load(dp) 61 | #except: 62 | # print("Cannot load from to ", det_name) 63 | self.episodes_detalied = full_data["episodes_detalied"] 64 | self.episodes_stats = full_data["short_stats"] 65 | 66 | return -------------------------------------------------------------------------------- /navigation/Mappers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | import torch.nn as nn 4 | import torch.nn.functional as F 5 | from math import ceil,floor 6 | import math 7 | from .Reprojection import getMapSizeInCells, project2dPClIntoWorldMap, ReprojectLocal2Global 8 | 9 | 10 | def DepthToLocal3D(depth, fx, fy, cx, cy): 11 | r"""Projects depth map to 3d point cloud 12 | with origin in the camera focus 13 | """ 14 | device = depth.device 15 | h,w = depth.squeeze().size() 16 | npts = h*w 17 | x = torch.linspace(0, w-1, w).to(device) 18 | y = torch.linspace(0, h-1, h).to(device) 19 | xv, yv = torch.meshgrid([x, y]) 20 | dfl = depth.t().flatten() 21 | return torch.cat([(dfl *(xv.flatten() - cx) / fx).unsqueeze(-1), #x 22 | (dfl *(yv.flatten() - cy) / fy).unsqueeze(-1), #y 23 | dfl.unsqueeze(-1)], dim = 1) #z 24 | 25 | 26 | def pointCloud2ObstaclesNonDifferentiable(pts3D, 27 | map_size = 40, 28 | cell_size = 0.2): 29 | r"""Counts number of 3d points in 2d map cell 30 | height is sum-pooled. 31 | """ 32 | device = pts3D.device 33 | map_size_in_cells = getMapSizeInCells(map_size,cell_size) - 1 34 | init_map = torch.zeros((map_size_in_cells,map_size_in_cells), device = device) 35 | if len(pts3D) <= 1: 36 | return init_map 37 | num_pts,dim = pts3D.size() 38 | pts2D = torch.cat([pts3D[:,2:3],pts3D[:,0:1]], dim = 1) 39 | data_idxs = torch.round(project2dPClIntoWorldMap(pts2D, map_size, cell_size)) 40 | if len(data_idxs) > 10: 41 | u, counts = np.unique(data_idxs.detach().cpu().numpy(), axis=0, return_counts = True) 42 | init_map[u[:,0],u[:,1] ] = torch.from_numpy(counts).to(dtype=torch.float32, device=device) 43 | return init_map 44 | 45 | class DirectDepthMapper(nn.Module): 46 | r"""Estimates obstacle map given the depth image 47 | ToDo: replace numpy histogram counting with differentiable 48 | pytorch soft count like in 49 | https://papers.nips.cc/paper/7545-unsupervised-learning-of-shape-and-pose-with-differentiable-point-clouds.pdf 50 | """ 51 | def __init__(self, 52 | #fx = 0, 53 | #fy = 0, 54 | #cx = 0, 55 | #cy = 0, 56 | camera_height = 0, 57 | near_th = 0.1, far_th = 4.0, h_min = 0.0, h_max = 1.0, 58 | map_size = 40, map_cell_size = 0.1, 59 | device = torch.device('cpu'), 60 | **kwargs): 61 | super(DirectDepthMapper, self).__init__() 62 | self.device = device 63 | #self.fx = fx 64 | #self.fy = fy 65 | #self.cx = cx 66 | #self.cy = cy 67 | self.near_th = near_th 68 | self.far_th = far_th 69 | self.h_min_th = h_min 70 | self.h_max_th = h_max 71 | self.camera_height = camera_height 72 | self.map_size_meters = map_size 73 | self.map_cell_size = map_cell_size 74 | return 75 | def forward(self, depth, pose = torch.eye(4).float()): 76 | self.device = depth.device 77 | #Works for FOV = 45 degrees in minos/sensors.yml. Should be adjusted, if FOV changed 78 | self.fx = float(depth.size(1))# / 2.0 79 | self.fy = float(depth.size(0))# / 2.0 80 | self.cx = int(self.fx)//2 - 1 81 | self.cy = int(self.fy)//2 - 1 82 | pose = pose.to(self.device) 83 | local_3d_pcl = DepthToLocal3D(depth, self.fx, self.fy, self.cx, self.cy) 84 | idxs = (torch.abs(local_3d_pcl[:,2]) < self.far_th) * (torch.abs(local_3d_pcl[:,2]) >= self.near_th) 85 | survived_points = local_3d_pcl[idxs] 86 | if len(survived_points) < 20: 87 | map_size_in_cells = getMapSizeInCells(self.map_size_meters,self.map_cell_size) - 1 88 | init_map = torch.zeros((map_size_in_cells,map_size_in_cells), device = self.device) 89 | return init_map 90 | global_3d_pcl = ReprojectLocal2Global(survived_points, pose)[:,:3] 91 | #Because originally y looks down and from agent camera height 92 | global_3d_pcl[:,1] = -global_3d_pcl[:,1] + self.camera_height 93 | idxs = (global_3d_pcl[:,1] > self.h_min_th) * (global_3d_pcl[:,1] < self.h_max_th) 94 | global_3d_pcl = global_3d_pcl[idxs] 95 | obstacle_map = pointCloud2ObstaclesNonDifferentiable( 96 | global_3d_pcl, 97 | self.map_size_meters, 98 | self.map_cell_size) 99 | return obstacle_map 100 | 101 | class SparseDepthMapper(nn.Module): 102 | r"""Estimates obstacle map given the 3d points from ORBSLAM 103 | Does not work well. 104 | """ 105 | def __init__(self, 106 | fx = 0, 107 | fy = 0, 108 | cx = 0, 109 | cy = 0, 110 | camera_height = 0, 111 | near_th = 0.1, far_th = 4.0, h_min = 0.0, h_max = 1.0, 112 | map_size = 40, map_cell_size = 0.1, 113 | device = torch.device('cpu'), 114 | **kwargs): 115 | super(SparseDepthMapper, self).__init__() 116 | self.device = device 117 | self.fx = fx 118 | self.fy = fy 119 | self.cx = cx 120 | self.cy = cy 121 | self.near_th = near_th 122 | self.far_th = far_th 123 | self.h_min_th = h_min 124 | self.h_max_th = h_max 125 | self.camera_height = camera_height 126 | self.map_size_meters = map_size 127 | self.map_cell_size = map_cell_size 128 | 129 | return 130 | def forward(self, sparse_depth, pose = torch.eye(4).float()): 131 | global_3d_pcl = sparse_depth 132 | #Because originally y looks down and from agent camera height 133 | global_3d_pcl[:,1] = -global_3d_pcl[:,1]# + self.camera_height 134 | idxs = (global_3d_pcl[:,1] > self.h_min_th) * (global_3d_pcl[:,1] < self.h_max_th) 135 | global_3d_pcl = global_3d_pcl[idxs] 136 | obstacle_map = pointCloud2ObstaclesNonDifferentiable( 137 | global_3d_pcl, 138 | self.map_size_meters, 139 | self.map_cell_size) 140 | return obstacle_map -------------------------------------------------------------------------------- /navigation/Metrics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def calcSPL(coefs,successes): 4 | assert len(coefs) == len(successes) 5 | return np.mean(np.array(coefs) *np.array(successes)) -------------------------------------------------------------------------------- /navigation/PyTorchPathPlanners.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | import random 4 | from copy import deepcopy 5 | import torch 6 | import torch.nn.functional as F 7 | import torch.nn as nn 8 | import pickle as pickle 9 | from .Utils import generate_2dgrid 10 | from time import time 11 | 12 | 13 | class SoftArgMin(nn.Module): 14 | def __init__(self, beta = 5): 15 | super(SoftArgMin, self).__init__() 16 | self.beta = beta 17 | return 18 | def forward(self, x, coords2d = None): 19 | bx_sm = F.softmax(self.beta * (-x).view(1,-1), dim=1) 20 | if coords2d is None: 21 | coords2d = generate_2dgrid(x.size(2), x.size(3),False) 22 | coords2d_flat = coords2d.view(2,-1) 23 | return (bx_sm.expand_as(coords2d_flat) * coords2d_flat).sum(dim=1) / bx_sm.sum(dim=1) 24 | 25 | class HardArgMin(nn.Module): 26 | def __init__(self): 27 | super(HardArgMin, self).__init__() 28 | return 29 | def forward(self, x, coords2d = None): 30 | val,idx = x.view(-1).min(dim=0) 31 | if coords2d is None: 32 | coords2d = generate_2dgrid(x.size(2), x.size(3),False) 33 | coords2d_flat = coords2d.view(2,-1) 34 | return coords2d_flat[:,idx].view(2) 35 | 36 | def safeROI2d(array2d, ymin,ymax,xmin,xmax): 37 | (H,W) = array2d.shape 38 | return max(0,ymin),min(ymax,H),max(0,xmin),min(xmax,W) 39 | 40 | def f2ind(ten,i): 41 | #Float to index 42 | return torch.round(ten[i]).long() 43 | 44 | def initNeights2channels(ks = 3): 45 | r"""Convolutional kernel, 46 | which maps nighborhood into channels 47 | """ 48 | weights = np.zeros((ks*ks,1,ks,ks), dtype= np.float32) 49 | for y in range(ks): 50 | for x in range(ks): 51 | weights[x*ks + y, 0, y, x] = 1.0 52 | return weights 53 | 54 | class DifferentiableStarPlanner(nn.Module): 55 | def __init__(self, max_steps = 500, visualize = False, preprocess = False, 56 | beta = 100, 57 | connectivity = "eight", 58 | device = torch.device('cpu'), **kwargs): 59 | super(DifferentiableStarPlanner, self).__init__() 60 | self.eps = 1e-12 61 | self.max_steps = max_steps; 62 | self.visualize = visualize 63 | self.inf = 1e7 64 | self.ob_cost = 10000.0 65 | self.device = device 66 | self.beta = beta 67 | self.preprocess = preprocess 68 | #self.argmin = SoftArgMin(beta) 69 | self.argmin = HardArgMin() 70 | self.neights2channels = nn.Conv2d(1, 9, kernel_size=(3,3), bias = False) 71 | self.neights2channels.weight.data = torch.from_numpy(initNeights2channels(3)) 72 | self.neights2channels.to(device) 73 | self.preprocessNet = nn.Conv2d(1, 1, kernel_size=(3,3), padding = 1, bias = False) 74 | self.preprocessNet.weight.data = torch.from_numpy(np.array([[[[0.00001, 0.0001, 0.00001], 75 | [0.0001, 1, 0.0001], 76 | [0.00001, 0.0001, 0.00001]]]], dtype=np.float32)) 77 | self.preprocessNet.to(device) 78 | if connectivity == "eight": 79 | self.gx_to_right = nn.Conv2d(1, 1, kernel_size=(1,3), bias = False) 80 | self.gx_to_right.weight.data = torch.from_numpy(np.array([[[[0, 1, -1]]]], dtype=np.float32)) 81 | self.gx_to_right.to(device) 82 | 83 | self.gx_to_left = nn.Conv2d(1, 1, kernel_size=(1,3), bias = False) 84 | self.gx_to_left.weight.data = torch.from_numpy(np.array([[[[-1, 1, 0]]]], dtype=np.float32)) 85 | self.gx_to_left.to(device) 86 | 87 | self.gy_to_up = nn.Conv2d(1, 1, kernel_size=(3,1), bias = False) 88 | self.gy_to_up.weight.data = torch.from_numpy(np.array([[[[0], [1], [-1]]]], dtype=np.float32)) 89 | self.gy_to_up.to(device) 90 | 91 | self.gy_to_down = nn.Conv2d(1, 1, kernel_size=(3,1), bias = False) 92 | self.gy_to_down.weight.data = torch.from_numpy(np.array([[[[-1], [1], [0]]]], dtype=np.float32)) 93 | self.gy_to_down.to(device) 94 | else: 95 | raise ValueError('Only "eight" connectivity now supported') 96 | return 97 | def preprocess_obstacle_map(self, obstacle_map): 98 | if self.preprocess: 99 | return self.preprocessNet(obstacle_map)#torch.clamp(self.preprocessNet(obstacle_map), min = 0.0, max = 1.0) 100 | return obstacle_map#torch.clamp(obstacle_map, min = 0.0, max = 1.0) 101 | def coords2grid(self, node_coords, h,w): 102 | grid = node_coords.squeeze() - torch.FloatTensor((h/2.0,w/2.0)).to(self.device) 103 | grid = grid / torch.FloatTensor((h/2.0,w/2.0)).to(self.device) 104 | return grid.view(1,1,1,2).flip(3) 105 | def initCloseListMap(self): 106 | return torch.zeros_like(self.start_map).float()#(self.obstacles >= 0.7).float() 107 | def initOpenListMap(self): 108 | return self.start_map.clone() 109 | def initGMap(self): 110 | return torch.clamp(self.inf* (torch.ones_like(self.start_map) - \ 111 | self.start_map.clone()), min = 0,max = self.inf) 112 | def safeROI2d(self, ymin,ymax,xmin,xmax): 113 | return int(max(0,torch.round(ymin).item())),int(min(torch.round(ymax).item(),self.height)),\ 114 | int(max(0,torch.round(xmin).item())),int(min(torch.round(xmax).item(),self.width)) 115 | def forward(self, obstacles, coords, start_map, goal_map, 116 | non_obstacle_cost_map = None, 117 | additional_steps = 50, 118 | return_path = True):###################################################### 119 | self.trav_init_time = 0 120 | self.trav_mask_time = 0 121 | self.trav_soft_time = 0 122 | self.conv_time = 0 123 | self.close_time = 0 124 | 125 | t=time() 126 | self.obstacles = self.preprocess_obstacle_map(obstacles.to(self.device)) 127 | self.start_map = start_map.to(self.device) 128 | self.been_there = torch.zeros_like(self.start_map).to(torch.device('cpu')) 129 | self.coords = coords.to(self.device) 130 | self.goal_map = goal_map.to(self.device) 131 | self.been_there = torch.zeros_like(self.goal_map).to(self.device) 132 | self.height = obstacles.size(2) 133 | self.width = obstacles.size(3) 134 | m,goal_idx = torch.max(self.goal_map.view(-1),0) 135 | ####################### 136 | c_map = self.calculateLocalPathCosts(non_obstacle_cost_map) #this might be non persistent in map update 137 | ###################### 138 | self.g_map = self.initGMap() 139 | padded_gmap = F.pad(self.g_map,(1,1, 1,1), 'replicate') 140 | ###### 141 | self.close_list_map = self.initCloseListMap() 142 | self.open_list_map = self.initOpenListMap() 143 | ####### 144 | not_done = False 145 | step = 0 146 | stopped_by_max_iter = False 147 | #print (time() -t, 'init time') 148 | t=time() 149 | if self.visualize: 150 | self.fig,self.ax = plt.subplots(1,1) 151 | self.image = self.ax.imshow(self.g_map.squeeze().cpu().detach().numpy().astype(np.float32), 152 | animated=True) 153 | self.fig.canvas.draw() 154 | not_done = (self.close_list_map.view(-1)[goal_idx].item() < 1.0) or (self.g_map.view(-1)[goal_idx].item() >= 0.9*self.ob_cost) 155 | R = 1 156 | self.start_coords= (self.coords * self.start_map.expand_as(self.coords)).sum(dim=2).sum(dim=2).squeeze() 157 | node_coords = self.start_coords 158 | self.goal_coords= (self.coords * self.goal_map.expand_as(self.coords)).sum(dim=2).sum(dim=2).squeeze() 159 | self.max_steps = 4* int(torch.sqrt(((self.start_coords -self.goal_coords )**2).sum() + 1e-6).item()) 160 | while not_done: 161 | ymin,ymax,xmin,xmax = self.safeROI2d(node_coords[0]-R, 162 | node_coords[0]+R+1, 163 | node_coords[1]-R, 164 | node_coords[1]+R+1) 165 | if (ymin-1 > 0) and (xmin-1 > 0) and (ymax+1 < self.height) and (xmax+1 < self.width): 166 | #t=time() 167 | n2c = self.neights2channels(self.g_map[:,:,ymin-1:ymax+1, xmin-1:xmax+1]) 168 | self.g_map[:,:,ymin:ymax, xmin:xmax] = torch.min(self.g_map[:,:,ymin:ymax, xmin:xmax].clone(), 169 | ( n2c +\ 170 | c_map[:,:,ymin:ymax, xmin:xmax]).min(dim = 1, 171 | keepdim = True)[0]) 172 | #self.conv_time +=time() - t 173 | t=time() 174 | self.close_list_map[:,:,ymin:ymax, xmin:xmax] = torch.max(self.close_list_map[:,:,ymin:ymax, xmin:xmax], 175 | self.open_list_map[:,:,ymin:ymax, xmin:xmax]) 176 | self.open_list_map[:,:,ymin:ymax, xmin:xmax] = F.relu(F.max_pool2d(self.open_list_map[:,:,ymin-1:ymax+1, xmin-1:xmax+1], 3, 177 | stride=1, padding=0) - self.close_list_map[:,:,ymin:ymax, xmin:xmax] -\ 178 | self.obstacles[:,:,ymin:ymax, xmin:xmax]) 179 | #self.close_time +=time() - t 180 | else: 181 | t=time() 182 | self.g_map = torch.min(self.g_map, 183 | (self.neights2channels(F.pad(self.g_map,(1,1, 1,1), 'replicate')) + c_map).min(dim = 1, 184 | keepdim = True)[0]) 185 | #self.conv_time +=time() - t 186 | #t=time() 187 | self.close_list_map = torch.max(self.close_list_map, self.open_list_map) 188 | self.open_list_map = F.relu(F.max_pool2d(self.open_list_map, 3, 189 | stride=1, padding=1) - self.close_list_map - self.obstacles) 190 | #self.close_time +=time() - t 191 | step+=1 192 | if step % 100 == 0: 193 | #print (step) 194 | if self.visualize: 195 | plt.imshow(self.g_map.cpu().detach().numpy().squeeze().astype(np.float32)) 196 | self.image.set_data(gg) 197 | self.fig.canvas.draw() 198 | if step >= self.max_steps: 199 | stopped_by_max_iter = True 200 | break 201 | not_done = (self.close_list_map.view(-1)[goal_idx].item() < 1.0) or\ 202 | (self.g_map.view(-1)[goal_idx].item() >= 0.1*self.inf) 203 | R+=1 204 | if not stopped_by_max_iter: 205 | for i in range(additional_steps): #now propagating beyong start point 206 | self.g_map = torch.min(self.g_map, 207 | (self.neights2channels(F.pad(self.g_map,(1,1, 1,1), 'replicate')) + c_map).min(dim = 1, 208 | keepdim = True)[0]) 209 | self.close_list_map = torch.max(self.close_list_map, self.open_list_map) 210 | self.open_list_map = F.relu(F.max_pool2d(self.open_list_map, 3, 211 | stride=1, padding=1) - self.close_list_map - self.obstacles) 212 | #print (time() -t, 'prop time', step, ' steps') 213 | #print (self.conv_time, self.close_time, "conv, close time" ) 214 | if return_path: 215 | t=time() 216 | out_path, cost = self.reconstructPath() 217 | #print (time() -t, 'recont time') 218 | return out_path, cost 219 | return 220 | def calculateLocalPathCosts(self, non_obstacle_cost_map = None): 221 | coords = self.coords 222 | h = coords.size(2) 223 | w = coords.size(3) 224 | obstacles_pd = F.pad(self.obstacles, (1,1, 1,1), 'replicate') 225 | if non_obstacle_cost_map is None: 226 | learned_bias = torch.ones_like(self.obstacles).to(obstacles_pd.device) 227 | else: 228 | learned_bias = non_obstacle_cost_map.to(obstacles_pd.device) 229 | left_diff_sq = self.gx_to_left(F.pad(coords[:,1:2,:,:], (1,1, 0,0), 'replicate'))**2 230 | right_diff_sq = self.gx_to_right(F.pad(coords[:,1:2,:,:], (1,1, 0,0), 'replicate'))**2 231 | up_diff_sq = self.gy_to_up(F.pad(coords[:,0:1,:,:], (0,0, 1,1), 'replicate'))**2 232 | down_diff_sq = self.gy_to_down(F.pad(coords[:,0:1,:,:], (0,0, 1,1), 'replicate'))**2 233 | out = torch.cat([#Order in from up to down, from left to right, hopefully same as in PyTorch 234 | torch.sqrt(left_diff_sq + up_diff_sq + self.eps) + self.ob_cost*torch.max(obstacles_pd[:,:,0:h,0:w], obstacles_pd[:,:,1:h+1,1:w+1]), 235 | torch.sqrt(left_diff_sq + self.eps) + self.ob_cost*torch.max(obstacles_pd[:,:,0:h,1:w+1], obstacles_pd[:,:,1:h+1,1:w+1]), 236 | torch.sqrt(left_diff_sq + down_diff_sq + self.eps) + self.ob_cost*torch.max(obstacles_pd[:,:,2:h+2,0:w], obstacles_pd[:,:,1:h+1,1:w+1]), 237 | torch.sqrt(up_diff_sq + self.eps) + self.ob_cost*torch.max(obstacles_pd[:,:,0:h,1:w+1], obstacles_pd[:,:,1:h+1,1:w+1]), 238 | 0*right_diff_sq + self.ob_cost*obstacles_pd[:,:,1:h+1,1:w+1], #current center 239 | torch.sqrt(down_diff_sq + self.eps)+ self.ob_cost*torch.max(obstacles_pd[:,:,2:h+2,1:w+1], obstacles_pd[:,:,1:h+1,1:w+1]), 240 | torch.sqrt(right_diff_sq + up_diff_sq + self.eps) + self.ob_cost*torch.max(obstacles_pd[:,:,0:h,2:w+2], obstacles_pd[:,:,1:h+1,1:w+1]), 241 | torch.sqrt(right_diff_sq + self.eps) + self.ob_cost*torch.max(obstacles_pd[:,:,1:h+1,2:w+2], obstacles_pd[:,:,1:h+1,1:w+1]), 242 | torch.sqrt(right_diff_sq + down_diff_sq + self.eps)+ self.ob_cost*torch.max(obstacles_pd[:,:,2:h+2,2:w+2], obstacles_pd[:,:,1:h+1,1:w+1]) 243 | ], dim = 1) 244 | return out + torch.clamp (learned_bias.expand_as(out), min = 0, max = self.ob_cost) 245 | def propagate_traversal(self,node_coords, close, g, coords): 246 | t=time() 247 | ymin,ymax,xmin,xmax = self.safeROI2d(node_coords[0]-1, 248 | node_coords[0]+2, 249 | node_coords[1]-1, 250 | node_coords[1]+2) 251 | #mask = torch.zeros(1,1,ymax-ymin,xmax-xmin) 252 | self.trav_init_time +=time()-t 253 | t=time() 254 | mask = close[:,:,ymin:ymax, xmin:xmax] > 0 255 | mask[:,:, f2ind(node_coords, 0)-ymin, 256 | f2ind(node_coords, 1)-xmin] = 0 257 | mask = mask > 0 258 | current_g_cost = g[:,:,ymin:ymax, xmin:xmax][mask].clone() 259 | if len(current_g_cost.view(-1)) == 0: 260 | #we are kind surrounded by obstacles, but still need to output something 261 | mask = torch.relu(1.0 - self.been_there[:,:,ymin:ymax, xmin:xmax]) 262 | mask[:,:, f2ind(node_coords, 0)-ymin, 263 | f2ind(node_coords, 1)-xmin] = 0 264 | mask = mask > 0 265 | current_g_cost = g[:,:,ymin:ymax, xmin:xmax][mask].clone() 266 | if len(current_g_cost.view(-1)) > 1: 267 | current_g_cost = (current_g_cost - torch.min(current_g_cost).item()) 268 | current_g_cost = current_g_cost + 0.41*torch.randperm(len(current_g_cost), 269 | dtype=torch.float32, device = torch.device('cpu'))/(len(current_g_cost)) 270 | self.trav_mask_time+=time() -t 271 | # 272 | t=time() 273 | coords_roi = coords[:,:,ymin:ymax, xmin:xmax] 274 | out = self.argmin(current_g_cost,coords_roi[mask.expand_as(coords_roi)]) 275 | self.trav_soft_time+=time() -t 276 | return out 277 | def getCleanCostMapAndGoodMask(self): 278 | good_mask = 1 - F.max_pool2d(self.obstacles, 3, stride=1, padding=1) 279 | costmap = self.g_map #* self.close_list_map #* good_mask 280 | obstacle_cost_corrected = 10000.0 281 | sampling_map = torch.clamp(costmap, min = 0, max = obstacle_cost_corrected) 282 | return sampling_map, good_mask 283 | def reconstructPath(self): 284 | #self.other_loop_time = 0 285 | #t=time() 286 | #print ("GOAL IS REACHED!") 287 | out_path = [] 288 | goal_coords = self.goal_coords.cpu()#(self.coords * self.goal_map.expand_as(self.coords)).sum(dim=2).sum(dim=2).squeeze() 289 | start_coords = self.start_coords.cpu()#(self.coords * self.start_map.expand_as(self.coords)).sum(dim=2).sum(dim=2).squeeze() 290 | 291 | cost = self.g_map[:,:, f2ind(goal_coords, 0), 292 | f2ind(goal_coords, 1)] 293 | #### Traversing 294 | done = False 295 | node_coords = goal_coords.cpu() 296 | out_path.append(node_coords) 297 | self.been_there = 0*self.been_there.cpu() 298 | self.been_there[:,:, f2ind(node_coords, 0), 299 | f2ind(node_coords, 1)] = 1.0 300 | self.close_list_map = self.close_list_map.cpu() 301 | self.g_map = self.g_map.cpu() 302 | self.coords = self.coords.cpu() 303 | #print ('non loop time', time() -t ) 304 | count1 = 0 305 | while not done: 306 | node_coords = self.propagate_traversal(node_coords, self.close_list_map, 307 | self.g_map, self.coords) 308 | #t=time() 309 | self.been_there[:,:, f2ind(node_coords, 0), 310 | f2ind(node_coords, 1)] = 1.0 311 | #if len(node_coords) == 0: 312 | # print("Warning! Empty next coords") 313 | # return out_path, cost 314 | if (torch.norm(node_coords - out_path[-1], 2).item() < 0.3): 315 | y = node_coords.flatten()[0].long() 316 | x = node_coords.flatten()[1].long() 317 | print(self.g_map[0,0,y-2:y+3,x-2:x+3 ]) 318 | print("loop in out_path",node_coords ) 319 | #torch.save(self.g_map, 'gmap.pt') 320 | raise ValueError('loop in out_path') 321 | return out_path, cost 322 | out_path.append(node_coords) 323 | done = torch.norm(node_coords - start_coords.cpu(), 2).item() < 0.3 324 | #self.other_loop_time+=time() -t 325 | count1+=1 326 | if count1 > 250: 327 | break 328 | return out_path, cost 329 | 330 | -------------------------------------------------------------------------------- /navigation/Reprojection.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | import torch.nn.functional as F 4 | import math 5 | from math import ceil,floor 6 | 7 | 8 | def Pzx(P): 9 | return P[(0,2),3] 10 | def getMapSizeInCells(map_size_in_meters, cell_size_in_meters): 11 | return int(ceil(map_size_in_meters / cell_size_in_meters))+1 12 | 13 | def getPosDiff(Pinit,Pfin): 14 | return Pzx(Pfin) - Pzx(Pinit) 15 | def getPosDiffLength(Pinit,Pfin): 16 | return torch.norm(getPosDiff(Pinit,Pfin)) 17 | 18 | def getPosDiffs(Ps): 19 | return Ps[1:,(0,2),3] - Ps[:(Ps.size(0)-1),(0,2),3] 20 | 21 | 22 | def angleToPi_2_MinusPi_2(angle): 23 | if angle < -np.pi: 24 | angle = 2.0*np.pi + angle 25 | if angle > np.pi: 26 | angle = -2.0*np.pi + angle 27 | return angle 28 | 29 | 30 | def getDirection(Pinit,Pfin, ang_th = 0.2, pos_th = 0.1): 31 | pos_diff = getPosDiff(Pinit,Pfin) 32 | if torch.norm(pos_diff,2).item() < pos_th: 33 | return 0 34 | else: 35 | needed_angle = torch.atan2(pos_diff[1], pos_diff[0]) 36 | current_angle = torch.atan2(Pinit[2,0], Pinit[0,0]) 37 | to_rotate = angleToPi_2_MinusPi_2(-np.pi/2.0 + needed_angle - current_angle) 38 | if torch.abs(to_rotate).item() < ang_th: 39 | return 0 40 | return to_rotate 41 | 42 | def ReprojectLocal2Global(xyz_local, P): 43 | device = xyz_local.device 44 | num, dim = xyz_local.size() 45 | if dim == 3: 46 | xyz = torch.cat([xyz_local, 47 | torch.ones((num,1), dtype = torch.float32, device=device)], dim = 1) 48 | elif dim == 4: 49 | xyz = xyz_local 50 | else: 51 | raise ValueError('3d point cloud dim is neighter 3, or 4 (homogenious)') 52 | #print(xyz.shape, P.shape) 53 | xyz_global = torch.mm(P.squeeze(),xyz.t()) 54 | return xyz_global.t() 55 | 56 | 57 | def project2dPClIntoWorldMap(zx, map_size, cell_size): 58 | device = zx.device 59 | shift = int(floor(getMapSizeInCells(map_size, cell_size)/2.0)) 60 | topdown2index = torch.tensor([[1.0/cell_size, 0, shift], 61 | [0, 1.0/cell_size, shift], 62 | [0, 0, 1]], device = device) 63 | world_coords_h = torch.cat([zx.view(-1,2), 64 | torch.ones((len(zx),1), device = device)], dim = 1) 65 | world_coords = torch.mm(topdown2index, world_coords_h.t()) 66 | return world_coords.t()[:,:2] 67 | 68 | def getPose2D(poses6d): 69 | poses6d = poses6d.view(-1,4,4); 70 | poses2d = poses6d[:,(0,2)] 71 | poses2d = poses2d[:,:,(0,2,3)] 72 | return poses2d 73 | 74 | def get_rotation_matrix(angle_in_radians): 75 | angle_in_radians = angle_in_radians.view(-1, 1, 1); 76 | sin_a = torch.sin(angle_in_radians) 77 | cos_a = torch.cos(angle_in_radians) 78 | A1_x = torch.cat([cos_a, sin_a], dim = 2) 79 | A2_x = torch.cat([-sin_a, cos_a], dim = 2) 80 | transform = torch.cat([A1_x,A2_x], dim = 1) 81 | return transform 82 | 83 | def normalizeZXOri(P): 84 | p2d = getPose2D(P) 85 | norms = torch.norm(p2d[:,0,:2],dim=1).view(-1,1,1) 86 | out = torch.cat([ 87 | torch.cat([P[:,:3,:3] / norms.expand(P.size(0),3,3), P[:,3:,:3]], dim = 1), 88 | P[:,:,3:]], dim = 2) 89 | return out 90 | 91 | def addRotWPs(P): 92 | plannedTPsNorm = normalizeZXOri(P) 93 | pos_diffs = getPosDiffs(plannedTPsNorm) 94 | 95 | angles = torch.atan2(pos_diffs[:,1], pos_diffs[:,0]) 96 | rotmats = get_rotation_matrix(angles) 97 | plannedTPsNorm[:P.size(0)-1,0,0] = rotmats[:,0,0] 98 | plannedTPsNorm[:P.size(0)-1,0,2] = rotmats[:,0,1] 99 | plannedTPsNorm[:P.size(0)-1,2,0] = rotmats[:,1,0] 100 | plannedTPsNorm[:P.size(0)-1,2,2] = rotmats[:,1,1] 101 | 102 | plannedPoints2 = plannedTPsNorm.clone() 103 | 104 | plannedPoints2[1:,0,0] = plannedTPsNorm[:P.size(0)-1,0,0] 105 | plannedPoints2[1:,0,2] = plannedTPsNorm[:P.size(0)-1,0,2] 106 | plannedPoints2[1:,2,0] = plannedTPsNorm[:P.size(0)-1,2,0] 107 | plannedPoints2[1:,2,2] = plannedTPsNorm[:P.size(0)-1,2,2] 108 | 109 | out = torch.stack((plannedPoints2.unsqueeze(0),plannedTPsNorm.unsqueeze(0)), dim=0).squeeze() 110 | out = out.permute(1,0,2,3).contiguous().view(-1,4,4) 111 | return out 112 | 113 | def plannedPath2TPs(path, cell_size, map_size, agent_h, addRot = False): 114 | path = torch.cat(path).view(-1,2) 115 | #print(path.size()) 116 | num_pts = len(path); 117 | plannedTPs = torch.eye(4).unsqueeze(0).repeat((num_pts, 1,1)) 118 | plannedTPs[:,0,3] = path[:,1]#switch back x and z 119 | plannedTPs[:,1,3] = agent_h 120 | plannedTPs[:,2,3] = path[:,0]#switch back x and z 121 | shift = int(floor(getMapSizeInCells(map_size, cell_size)/2.0)) 122 | plannedTPs[:,0,3] = plannedTPs[:,0,3] - shift 123 | plannedTPs[:,2,3] = plannedTPs[:,2,3] - shift 124 | P = torch.tensor([ 125 | [1.0/cell_size, 0, 0, 0], 126 | [0, 1.0/cell_size, 0, 0], 127 | [0, 0, 1.0/cell_size, 0], 128 | [0, 0, 0, 1]]) 129 | plannedTPs = torch.bmm(P.inverse().unsqueeze(0).expand(num_pts,4,4), plannedTPs) 130 | if addRot: 131 | return addRotWPs(plannedTPs) 132 | return plannedTPs 133 | 134 | def minosOffsetToGoal2MapGoalPosition(offset, 135 | P_curr, 136 | cell_size, 137 | map_size): 138 | device = offset.device 139 | goal_tp = minosOffsetToGoal2TP(offset, 140 | P_curr) 141 | goal_tp1 = torch.eye(4).to(device) 142 | goal_tp1[:,3:] = goal_tp 143 | projected_p = projectTPsIntoWorldMap(goal_tp1.view(1,4,4), 144 | cell_size, 145 | map_size) 146 | return projected_p 147 | def minosOffsetToGoal2TP(offset, 148 | P_curr): 149 | device = offset.device 150 | if P_curr.size(1) == 3: 151 | P_curr = homogenizeP(P_curr) 152 | GoalTP = torch.mm(P_curr.to(device), 153 | torch.cat([offset * torch.tensor([1.,1.,-1.],dtype=torch.float32,device=device), 154 | torch.tensor([1.],device=device)]).reshape(4,1)) 155 | return GoalTP 156 | def homogenizeP(tps): 157 | device = tps.device 158 | tps = tps.view(-1,3,4) 159 | return torch.cat([tps.float(),torch.tensor([0,0,0,1.0]).view(1,1,4).expand(tps.size(0),1,4).to(device)],dim = 1) 160 | def projectTPsIntoWorldMap(tps, cell_size, map_size, do_floor = True): 161 | if len(tps) == 0: 162 | return [] 163 | if type(tps) is list: 164 | return [] 165 | device = tps.device 166 | topdownP = torch.tensor([[1.0,0,0,0], 167 | [0,0,1.0,0]]).to(device) 168 | world_coords = torch.bmm(topdownP.view(1,2,4).expand(tps.size(0),2,4) , tps[:,:,3:].view(-1,4,1)) 169 | shift = int(floor(getMapSizeInCells(map_size, cell_size)/2.0)) 170 | topdown2index = torch.tensor([[1.0/cell_size, 0, shift], 171 | [0, 1.0/cell_size, shift], 172 | [0, 0, 1]]).to(device) 173 | world_coords_h = torch.cat([world_coords, torch.ones((len(world_coords),1,1)).to(device)], dim = 1) 174 | #print(topdown2index.size(),world_coords_h.size()) 175 | world_coords = torch.bmm(topdown2index.unsqueeze(0).expand(world_coords_h.size(0),3,3), world_coords_h)[:,:2,0] 176 | if do_floor: 177 | return torch.floor(world_coords.flip(1)) + 1 #for having revesrve (z,x) ordering 178 | return world_coords.flip(1) 179 | def projectTPsIntoWorldMap_numpy(tps, slam_to_world, cell_size, map_size): 180 | if len(tps) == 0: 181 | return [] 182 | if type(tps) is list: 183 | return [] 184 | #tps is expected in [n,4,4] format 185 | topdownP = np.array([[slam_to_world,0,0,0], 186 | [0,0,slam_to_world,0]]) 187 | try: 188 | world_coords = np.matmul(topdownP.reshape(1,2,4) , tps[:,:,3:].reshape(-1,4,1)) 189 | except: 190 | return [] 191 | shift = int(floor(getMapSizeInCells(map_size, cell_size)/2.0)) 192 | topdown2index = np.array([[1.0/cell_size, 0, shift], 193 | [0, 1.0/cell_size, shift], 194 | [0, 0, 1]]) 195 | world_coords_h = np.concatenate([world_coords, np.ones((len(world_coords),1,1))], axis = 1) 196 | world_coords = np.matmul(topdown2index, world_coords_h)[:,:2,0] 197 | return world_coords[:,::-1].astype(np.int32) + 1 #for having revesrve (z,x) ordering -------------------------------------------------------------------------------- /navigation/Utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import json 3 | import os 4 | import torch 5 | from PIL import Image 6 | import time 7 | 8 | def readRLE(data, w, h): 9 | out_img = [] 10 | numel = int(len(data) / 2) 11 | for i in range(numel): 12 | val = int(data[2*i]) 13 | num = int(data[2*i+1]) 14 | out_img.append(val*np.ones((num, 1), dtype=np.bool)) 15 | out_img = np.concatenate(out_img, axis=0) 16 | out_img = out_img.reshape((h, w)) 17 | return out_img 18 | 19 | 20 | def readNavgrid_10File(fname): 21 | with open(fname, 'r') as f: 22 | nav_grid = json.load(f) 23 | if 'tileAttributes' not in nav_grid: 24 | nav_grid = nav_grid['grids'][0] 25 | occ_grid = readRLE(nav_grid['tileAttributes']['occupancy']['data'], nav_grid['width'], nav_grid['height']) 26 | return occ_grid 27 | 28 | 29 | def navgridFname(suncgdir, hash1): 30 | return os.path.join('', *[suncgdir,hash1, hash1+'.furnished.grid.json']) 31 | 32 | def generate_2dgrid(h,w, centered = False): 33 | if centered: 34 | x = torch.linspace(-w/2+1, w/2, w) 35 | y = torch.linspace(-h/2+1, h/2, h) 36 | else: 37 | x = torch.linspace(0, w-1, w) 38 | y = torch.linspace(0, h-1, h) 39 | grid2d = torch.stack([y.repeat(w,1).t().contiguous().view(-1), x.repeat(h)],1) 40 | return grid2d.view(1, h, w,2).permute(0,3,1, 2) 41 | 42 | 43 | def str2bool(v): 44 | if v.lower() in ('yes', 'true', 't', 'y', '1'): 45 | return True 46 | elif v.lower() in ('no', 'false', 'f', 'n', '0'): 47 | return False 48 | 49 | 50 | def ResizePIL(np_img, size = 128): 51 | im1 = Image.fromarray(np_img) 52 | im1.thumbnail((size,size)) 53 | return np.array(im1) 54 | 55 | def prepareGray(rgb, size = 128): 56 | return torch.from_numpy(((ResizePIL(rgb.mean(axis=2), size = size)-90.0)/50.).astype(np.float32)).view(1,1,size,size) 57 | def prepareRGB(rgb, size = 128): 58 | return torch.from_numpy(((ResizePIL(rgb, size = size)-90.0)/50.).astype(np.float32)).unsqueeze(0).permute(0,3,1,2) 59 | def prepareDepth(depth, size = 128): 60 | return torch.from_numpy(((ResizePIL(depth, size = size)-1.8)/ 1.2).astype(np.float32)).view(1,1,size,size) 61 | def prepareDist(dist): 62 | return torch.from_numpy(dist ).float().view(1,1) 63 | def prepareDir(direction): 64 | return torch.from_numpy(direction).float().view(1,2) 65 | 66 | def findMapSize(h, w): 67 | map_size_in_meters = int(0.1 * 3 * max(h,w)) 68 | if map_size_in_meters % 10 != 0: 69 | map_size_in_meters = map_size_in_meters + (10 - (map_size_in_meters % 10)) 70 | return map_size_in_meters 71 | 72 | def gettimestr(): 73 | return time.strftime("%Y-%m-%d--%H_%M_%S", time.gmtime()) -------------------------------------------------------------------------------- /navigation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ducha-aiki/navigation-benchmark/d83431d6648ac1147f53056ed32ce2caae4f702d/navigation/__init__.py -------------------------------------------------------------------------------- /navigation/visualizingUtils.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import matplotlib 3 | import numpy as np 4 | import torch 5 | from copy import deepcopy 6 | from math import floor 7 | from matplotlib.patches import Wedge 8 | import io 9 | from PIL import Image 10 | from time import time 11 | from PIL import ImageFont,ImageDraw 12 | import imageio 13 | from .Reprojection import projectTPsIntoWorldMap_numpy as NP_project 14 | from .Reprojection import minosOffsetToGoal2MapGoalPosition 15 | 16 | 17 | def drawMapGoalAgentAda(fig,ax, image, omap, goalpos, agentpos, agentori, plan_path, done_path, waypoint = None): 18 | t=time() 19 | while len(ax.lines) > 0: 20 | ax.lines = [] 21 | while len(ax.artists) > 0: 22 | ax.artists = [] 23 | h,w = omap.shape 24 | offset_h = 0 25 | offset_w = 0 26 | show_h, show_w = h,w 27 | if show_h == h: 28 | image.set_data(omap) 29 | else: 30 | image.set_data(omap[offset_h:offset_h+show_h,offset_w:offset_w+show_w]) 31 | AP = agentpos.squeeze() 32 | GP = goalpos.squeeze() 33 | if len(plan_path) > 1: 34 | ax.plot(np.array(plan_path[:,1]) - offset_w, 35 | show_h - (np.array(plan_path[:,0]) - offset_h), 36 | 'o', linewidth=5.0, markersize=10, color='red') 37 | if len(done_path) > 1: 38 | ax.plot(np.array(done_path[:,1]) - offset_w, 39 | show_h - (np.array(done_path[:,0]) - offset_h), 40 | 'o', linewidth=5.0, markersize=10, color='green') 41 | draw_goal = plt.Circle((GP[1] - offset_w, show_h - (GP[0] - offset_h)), 3, color='yellow', fill=True) 42 | draw_agent = plt.Circle((AP[1] - offset_w,show_h - (AP[0] - offset_h)), 3, color='blue', fill=True) 43 | agent_angle = -(np.degrees(np.arctan2(agentori[1,0], agentori[0,0])) + 90) 44 | draw_agent_fov = Wedge((AP[1] - offset_w,show_h - (AP[0] - offset_h)), 10, 45 | agent_angle - 45, 46 | agent_angle + 45, 47 | color="blue", alpha=0.5) 48 | t=time() 49 | ax.add_artist(draw_goal) 50 | ax.add_artist(draw_agent) 51 | ax.add_artist(draw_agent_fov) 52 | if waypoint is not None: 53 | CP = waypoint.squeeze() 54 | draw_waypoint = plt.Circle((CP[1] - offset_w,show_h - (CP[0] - offset_h)), 3, color='orange', fill=False) 55 | ax.add_artist(draw_waypoint) 56 | fig.canvas.draw() 57 | #print(time() - t, 's, draw') 58 | return 59 | 60 | def drawMapGoalAgent(fig,ax, image, omap, goalpos, agentpos, agentori, plan_path, done_path, waypoint = None): 61 | t=time() 62 | while len(ax.lines) > 0: 63 | ax.lines = [] 64 | while len(ax.artists) > 0: 65 | ax.artists = [] 66 | h,w = omap.shape 67 | offset_h = 0 68 | offset_w = 0 69 | show_h, show_w = h,w 70 | if show_h == h: 71 | image.set_data(omap) 72 | else: 73 | image.set_data(omap[offset_h:offset_h+show_h,offset_w:offset_w+show_w]) 74 | AP = agentpos.squeeze() 75 | GP = goalpos.squeeze() 76 | if len(plan_path) > 1: 77 | ax.plot(np.array(plan_path[:,1]) - offset_w, 78 | show_h - (np.array(plan_path[:,0]) - offset_h), 79 | '.', linewidth=0.2, color='red') 80 | if len(done_path) > 1: 81 | ax.plot(np.array(done_path[:,1]) - offset_w, 82 | show_h - (np.array(done_path[:,0]) - offset_h), 83 | '.', linewidth=0.2, color='green') 84 | draw_goal = plt.Circle((GP[1] - offset_w, show_h - (GP[0] - offset_h)), 3, color='yellow', fill=True) 85 | draw_agent = plt.Circle((AP[1] - offset_w,show_h - (AP[0] - offset_h)), 3, color='blue', fill=True) 86 | agent_angle = -(np.degrees(np.arctan2(agentori[1,0], agentori[0,0])) + 90) 87 | draw_agent_fov = Wedge((AP[1] - offset_w,show_h - (AP[0] - offset_h)), 10, 88 | agent_angle - 45, 89 | agent_angle + 45, 90 | color="blue", alpha=0.5) 91 | t=time() 92 | ax.add_artist(draw_goal) 93 | ax.add_artist(draw_agent) 94 | ax.add_artist(draw_agent_fov) 95 | if waypoint is not None: 96 | CP = waypoint.squeeze() 97 | draw_waypoint = plt.Circle((CP[1] - offset_w,show_h - (CP[0] - offset_h)), 3, color='orange', fill=False) 98 | ax.add_artist(draw_waypoint) 99 | fig.canvas.draw() 100 | #print(time() - t, 's, draw') 101 | return 102 | 103 | def fig2img ( fig ): 104 | w,h = fig.canvas.get_width_height() 105 | return np.frombuffer(fig.canvas.tostring_rgb(), dtype=np.uint8).reshape(w, h, 3) 106 | 107 | def prepareImgForVideo(rgb, depth, global_map, status): 108 | dr = np.expand_dims(depth,2) 109 | depth3 = np.concatenate([dr, dr, dr], axis = 2) 110 | upper = np.concatenate([rgb, depth3], axis = 1) 111 | pil_rgb = Image.fromarray(upper) 112 | if len(status) > 0: 113 | draw = ImageDraw.Draw(pil_rgb) 114 | try: 115 | font = ImageFont.truetype("/usr/share/fonts/truetype/freefont/FreeMono.ttf", 24, encoding="unic") 116 | except: 117 | font = ImageFont.load_default() 118 | draw.text((30, 30),status,(255,255,255),font=font) 119 | out_img = np.concatenate([np.array(pil_rgb), global_map], axis = 0) 120 | return out_img 121 | 122 | def initMapImgFig(init_map): 123 | fig,ax = plt.subplots(1,1,figsize=(50, 50)) 124 | fig.tight_layout(pad=0) 125 | h,w = init_map.shape 126 | offset_h = 0 127 | offset_w = 0 128 | show_h, show_w = h,w 129 | image = ax.imshow(init_map,extent=[0, show_h, 0, show_w])#, animated=True) 130 | ax.invert_yaxis() 131 | fig.canvas.draw() 132 | return fig, ax, image 133 | def getVideoLoggingImg(pos_history, 134 | slam_to_world, 135 | map_cell_size, 136 | map_size, 137 | obstacle_map, 138 | goalpos, 139 | agent_pos, 140 | agent_ori, 141 | planned_path, 142 | observation, 143 | fig,ax, image, 144 | status, 145 | waypoint = None): 146 | depth_preprocess = observation['observation']['sensors']['depth']['data'] 147 | depth_preprocess = (depth_preprocess/(1e-7 + depth_preprocess.max()) * 255).astype(np.uint8) 148 | rgbimg = observation['observation']['sensors']['color']['data'][:,:,:3] 149 | h,w = depth_preprocess.shape 150 | if True: 151 | if (type(pos_history) is list ) and (len(pos_history) > 2): 152 | try: 153 | ph = np.stack(pos_history) 154 | except: 155 | for i in pos_history: 156 | print (i) 157 | sys.exit(0) 158 | already_path = ph.reshape(-1,4,4) 159 | else: 160 | already_path = [] 161 | already_path_map = NP_project(already_path, 162 | slam_to_world, 163 | map_cell_size, 164 | map_size) 165 | drawMapGoalAgent(fig,ax, image, 166 | obstacle_map, 167 | goalpos, 168 | agent_pos, 169 | agent_ori, 170 | planned_path, 171 | already_path_map, 172 | waypoint) 173 | map2save = np.array(Image.fromarray(fig2img(fig)).resize((2*w, 2*h), Image.BILINEAR)) 174 | else:#except Exception as e: 175 | print(e) 176 | map2save = np.array(Image.fromarray(fig2img(fig)).resize((2*w, 2*h), Image.BILINEAR)) 177 | video_frame = prepareImgForVideo(rgbimg, 178 | depth_preprocess, 179 | map2save ,status) 180 | return video_frame 181 | 182 | class VideoLogWriter(): 183 | def __init__(self, dirname, 184 | map_size_in_cells =200, 185 | draw_map = True): 186 | self.dirname = dirname 187 | self.draw_map = draw_map 188 | self.map_in_cells = map_size_in_cells 189 | self.init_map_for_video = np.eye(map_size_in_cells,dtype = np.float32) * 50 190 | self.RGB_VIDEO_WRITER = imageio.get_writer(dirname + "_RGB.mp4", fps=10) 191 | self.DEPTH_VIDEO_WRITER = imageio.get_writer(dirname + "_depth.mp4", fps=10) 192 | if self.draw_map: 193 | self.fig,self.ax,self.image = initMapImgFig(self.init_map_for_video) 194 | self.MAP_VIDEO_WRITER = imageio.get_writer(dirname + "_map.mp4", fps=10) 195 | self.figGT, self.axGT,self.imageGT = initMapImgFig(self.init_map_for_video) 196 | self.GT_MAP_VIDEO_WRITER = imageio.get_writer(dirname + "_GTmap.mp4", fps=10) 197 | return 198 | def add_frame(self, observation, agent, logging_agent): ###RGB and depth 199 | self.RGB_VIDEO_WRITER.append_data(observation['observation']['sensors']['color']['data'][:,:,:3]) 200 | depth1 = observation['observation']['sensors']['depth']['data'] 201 | depth1 = np.clip(depth1, 0,4) 202 | depth1 = (255 * (depth1/4.0)).astype(np.uint8) 203 | self.DEPTH_VIDEO_WRITER.append_data(depth1) 204 | if self.draw_map: 205 | #Map 206 | try: 207 | pp = agent.planned2Dpath 208 | pp = torch.cat(pp).detach().cpu().numpy().reshape(-1,2) 209 | except: 210 | pp = [] 211 | pos1 = agent.getCurrentPositionOnMap().detach().cpu().numpy() 212 | 213 | ori1 = agent.getCurrentOrientationOnMap().detach().cpu().numpy() 214 | map1 = torch.zeros_like(logging_agent.map2DObstacles) 215 | try: 216 | waypoint = agent.getWaypointPositionOnMap().detach().cpu().numpy() 217 | already_path = np.stack(agent.position_history).reshape(-1,4,4) 218 | stw=agent.slam_to_world 219 | except: 220 | waypoint = None 221 | stw=1.0 222 | already_path = [] 223 | already_path_map = NP_project(already_path, 224 | 1.0, 225 | logging_agent.map_cell_size, 226 | logging_agent.map_size_meters) 227 | drawMapGoalAgentAda(self.fig,self.ax, self.image, 228 | 30*agent.rawMap2PlanerReady(agent.map2DObstacles,map1,map1).detach().cpu().squeeze().numpy(), 229 | agent.estimatedGoalPos2D.detach().cpu().numpy(), 230 | pos1, 231 | ori1, 232 | pp, 233 | already_path_map, 234 | waypoint) 235 | map2 = fig2img(self.fig) 236 | mh,mw,mch = map2.shape 237 | self.MAP_VIDEO_WRITER.append_data(map2) 238 | #######GT map 239 | pos1 = logging_agent.getCurrentGTPositionOnGTMap().detach().cpu().numpy() 240 | ori1 = logging_agent.getCurrentGTOrientationOnGTMap().detach().cpu().numpy() 241 | map1_gt = torch.zeros_like(logging_agent.map2DObstacles) 242 | waypoint = None 243 | try: 244 | already_path = np.stack(logging_agent.pose6D_GT_history).reshape(-1,4,4) 245 | except: 246 | already_path = [] 247 | already_path_map = NP_project(already_path, 248 | 1.0, 249 | logging_agent.map_cell_size, 250 | logging_agent.map_size_meters) 251 | drawMapGoalAgentAda(self.figGT,self.axGT, self.imageGT, 252 | 100*logging_agent.rawMap2PlanerReady(logging_agent.GTMAP, map1_gt,map1_gt).detach().cpu().squeeze().numpy(), 253 | minosOffsetToGoal2MapGoalPosition(logging_agent.offset_to_goal, 254 | logging_agent.pose6D.squeeze(), 255 | logging_agent.map_cell_size, 256 | logging_agent.map_size_meters).detach().cpu().numpy(), 257 | pos1, 258 | ori1, 259 | [], 260 | already_path_map, 261 | None) 262 | map2 = fig2img(self.figGT) 263 | self.GT_MAP_VIDEO_WRITER.append_data(map2) 264 | #### End of Drawing 265 | return 266 | def finish(self): 267 | self.RGB_VIDEO_WRITER.close() 268 | self.DEPTH_VIDEO_WRITER.close() 269 | if self.draw_map: 270 | self.GT_MAP_VIDEO_WRITER.close() 271 | self.MAP_VIDEO_WRITER.close() 272 | return 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | --------------------------------------------------------------------------------