├── 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 | " task | \n",
47 | " episode | \n",
48 | " sceneId | \n",
49 | " tick | \n",
50 | " px | \n",
51 | " py | \n",
52 | " pz | \n",
53 | " rotation | \n",
54 | " actions | \n",
55 | " actionArgs | \n",
56 | " forces | \n",
57 | "
\n",
58 | " \n",
59 | " \n",
60 | " \n",
61 | " 9294 | \n",
62 | " - | \n",
63 | " - | \n",
64 | " - | \n",
65 | " 19 | \n",
66 | " 0.0822 | \n",
67 | " 0.551 | \n",
68 | " 2.2288 | \n",
69 | " 3.3966 | \n",
70 | " turnRight | \n",
71 | " NaN | \n",
72 | " NaN | \n",
73 | "
\n",
74 | " \n",
75 | " 9295 | \n",
76 | " - | \n",
77 | " - | \n",
78 | " - | \n",
79 | " 20 | \n",
80 | " 0.0822 | \n",
81 | " 0.551 | \n",
82 | " 2.2288 | \n",
83 | " 3.2710 | \n",
84 | " turnRight | \n",
85 | " NaN | \n",
86 | " NaN | \n",
87 | "
\n",
88 | " \n",
89 | " 9296 | \n",
90 | " - | \n",
91 | " - | \n",
92 | " - | \n",
93 | " 21 | \n",
94 | " 0.0822 | \n",
95 | " 0.551 | \n",
96 | " 2.2288 | \n",
97 | " 3.1453 | \n",
98 | " turnRight | \n",
99 | " NaN | \n",
100 | " NaN | \n",
101 | "
\n",
102 | " \n",
103 | " 9297 | \n",
104 | " - | \n",
105 | " - | \n",
106 | " - | \n",
107 | " 22 | \n",
108 | " 0.0822 | \n",
109 | " 0.551 | \n",
110 | " 2.2288 | \n",
111 | " 3.0196 | \n",
112 | " turnRight | \n",
113 | " NaN | \n",
114 | " NaN | \n",
115 | "
\n",
116 | " \n",
117 | " 9298 | \n",
118 | " - | \n",
119 | " - | \n",
120 | " - | \n",
121 | " 23 | \n",
122 | " 0.0822 | \n",
123 | " 0.551 | \n",
124 | " 2.2288 | \n",
125 | " 2.8940 | \n",
126 | " reset | \n",
127 | " NaN | \n",
128 | " NaN | \n",
129 | "
\n",
130 | " \n",
131 | "
\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 |
--------------------------------------------------------------------------------