added figs

arcgis_terrain.py

@@ -9,6 +9,9 @@ from math import gcd
 import math
 from pyproj import Transformer, Proj, transform
 from scipy.interpolate import griddata
+from scipy import interpolate
+from scipy import ndimage
+import time
 
 '''
 THIS FILE IS DUPLICATED IN ANOTHER PROJECT AT: https://git.caslab.ece.vt.edu/hlarkin3/ags_grabber
@@ -156,8 +159,8 @@ def get_terrain_map(lat_lon = [0,0], sample_dist = 10, extent = 100, heading = 0
         y_row = np.empty([sc,0])
         e_row = np.empty([sc,0])
         for i in range(cc): # inner loop for x values O_O
-            x_values = [np.linspace(lhc_pt[0] + i * sample_extents, lhc_pt[0] + (i + 1) * sample_extents, sc)]
-            y_values = [np.linspace(lhc_pt[1] + j * sample_extents, lhc_pt[1] + (j + 1) * sample_extents, sc)]
+            x_values = [np.linspace(lhc_pt[0] + i * sample_extents, lhc_pt[0] + (i + 1 - 1/sc) * sample_extents, sc)]
+            y_values = [np.linspace(lhc_pt[1] + j * sample_extents, lhc_pt[1] + (j + 1 - 1/sc) * sample_extents, sc)]
             [X,Y] = np.meshgrid(x_values, y_values)
             # put in rotation here
             geo_points = [point_rotation(origin = [cen_pt[0],cen_pt[1]],pt = [xp,yp],ang = heading) for xv,yv in zip(X,Y) for xp,yp in zip(xv,yv)]
@@ -175,22 +178,24 @@ def get_terrain_map(lat_lon = [0,0], sample_dist = 10, extent = 100, heading = 0
             xs = np.array([e['location']['x'] for e in elv_samples], dtype=float).reshape([sc,sc])
             ys = np.array([e['location']['y'] for e in elv_samples], dtype=float).reshape([sc,sc])
             es = np.array([e['values'][0] for e in elv_samples], dtype=float).reshape([sc,sc])
-
-            # TODO: helps in some edge cases where terrain doesnt line up nicely
-            # if not np.all(xs == X) or not np.all(ys == Y):
-            #     # xs = np.array([e['location']['x'] for e in elv_samples], dtype=float)
-            #     # ys = np.array([e['location']['y'] for e in elv_samples], dtype=float)
-            #     qs = np.stack([xs.reshape(len(elv_samples)), ys.reshape(len(elv_samples))]).T
-            #     es = es.reshape(len(elv_samples))
-            #     # data came back in weird ordering, need to re-order
-            #     print("re-ordering data ...")
-            #     es_square = np.zeros([sc,sc])
-            #     for scx in range(sc):
-            #         for scy in range(sc): # maybe the least efficient way to do this
-            #             idx = np.where(np.all(qs == np.asarray([X[scx,scy],Y[scx,scy]]),axis = 1))
-            #             es_square[scx,scy] = es[idx]
-            #     es = es_square
-            #     print("done re-ordering")
+            if not np.all(xs == X) or not np.all(ys == Y):
+                # xs = np.array([e['location']['x'] for e in elv_samples], dtype=float)
+                # ys = np.array([e['location']['y'] for e in elv_samples], dtype=float)
+                qs = np.stack([xs.reshape(len(elv_samples)), ys.reshape(len(elv_samples))]).T
+                es = es.reshape(len(elv_samples))
+                # data came back in weird ordering, need to re-order
+                print("re-ordering data ...")
+                es_square = np.zeros([sc,sc])
+                for scx in range(sc):
+                    for scy in range(sc): # maybe the least efficient way to do this
+                        idx = np.where(np.all(np.abs(qs - np.asarray([X[scx,scy],Y[scx,scy]])) <= 1e-9,axis = 1))
+                        # print(idx)
+                        try:
+                            es_square[scx,scy] = es[idx]
+                        except ValueError as e:
+                            print("hellfire")
+                es = es_square
+                print("done re-ordering")
 
             # then just the tuple of all
             data_temp = []
@@ -208,6 +213,18 @@ def get_terrain_map(lat_lon = [0,0], sample_dist = 10, extent = 100, heading = 0
         y = np.append(y, y_row, axis=0)
         e = np.append(e, e_row, axis=0)
 
+    # flip elevation data up to down to match other layers
+    # e = np.flipud(e)
+    # TESTING SOMETHING
+    # e = np.fliplr(np.rot90(np.rot90(e)))
+    # interpolate terrain to match size/resolution of other layers
+    c = np.int(extent/sample_dist)
+    scale_factor = 3/20 # factor to get 6.66667m mapping from 1m mapping (1/6.6667)
+    scaled_extent = np.ceil(scale_factor*extent).astype(np.int)
+
+    factor = scaled_extent/e.shape[0]
+    e_interp = ndimage.zoom(e, factor, order = 3)
+
     if show_plot:
         # Attaching 3D axis to the figure
         grid_x, grid_y = np.mgrid[min(x):max(x):100j, min(y):max(y):100j]
@@ -228,7 +245,7 @@ def get_terrain_map(lat_lon = [0,0], sample_dist = 10, extent = 100, heading = 0
         print("y max: {}".format(np.max(y)))
         print("y max - min: {}".format(np.max(y)-np.min(y)))
 
-    return [e,x,y,data,lat_lon]
+    return [e,e_interp,x,y,data,lat_lon]
     # savedimg = elv_layer.export_image(bbox=elv_layer.extent, size=[3840,2160], f='image', save_folder='.', save_file='testerino.jpg')
 
 if __name__ == "__main__":

mrmh_model/params.py

@@ -91,7 +91,7 @@ class Default:
 
 
         # data parameters
-        self.params.setdefault('plot_data', True) # plots data in plotly viewer upon finishing
+        self.params.setdefault('plot_data', False) # plots data in plotly viewer upon finishing
         self.params.setdefault('save_data', True) # saves risk-cost and waypoint related data in json upon finishing
         self.params.setdefault('save_folder', 'kentland_n3_s2_rc') # which folder to save data in
 

mrmh_model/terrain.py

@@ -64,21 +64,21 @@ class Terrain(space.Space):
 
             print("collecting terrain data ...")
             terrain_location = self.params.get('anchor_point', False)
-            [e,x,y,data,cen_pt] = get_terrain_map(terrain_location, sample_dist = 2*self.res,
+            [e,_,x,y,data,cen_pt] = get_terrain_map(terrain_location, sample_dist = self.res,
                                       extent = self._xrange, heading=self.params.get('heading'), show_plot=False, verbosity=False)
             e = e - np.min(e)
 
             # interpolate terrain to match size/resolution of other layers
-            x_temp = np.linspace(0,self._xrange,np.int(self._xrange/(2*self.res))) # get correct size of terrain map
-            y_temp = np.linspace(0,self._xrange,np.int(self._xrange/(2*self.res)))
-            f = interpolate.interp2d(x_temp, y_temp, e, kind='quintic')
+            # x_temp = np.linspace(0,self._xrange,np.int(self._xrange/(2*self.res))) # get correct size of terrain map
+            # y_temp = np.linspace(0,self._xrange,np.int(self._xrange/(2*self.res)))
+            # f = interpolate.interp2d(x_temp, y_temp, e, kind='quintic')
             x_temp = np.linspace(0,self._xrange,np.int(self._xrange/(1*self.res)))
             y_temp = np.linspace(0,self._xrange,np.int(self._xrange/(1*self.res)))
-            e_interp = f(x_temp, y_temp)
+            # e_interp = f(x_temp, y_temp)
 
-            self.h = e_interp
+            self.h = e
 
-            self.terrain_data = [e_interp, x_temp, x_temp, data, cen_pt]
+            self.terrain_data = [e, x_temp, y_temp, data, cen_pt]
             print("terrain data collected!")
 
         if self.params.get('save_terrain', False):

scouter/feature_grabber.py

@@ -164,7 +164,7 @@ def grab_features(anchor_point, extent, sample_dist = 10, heading = 0, save_file
                 # f.write(str(save_data))
 
     # save terrain as csv file (this method is pretty slow, but can compensate with interp)
-    [e,x,y,data,ll_pt] = get_terrain_map(lat_lon=anchor_point,
+    [_,e,x,y,data,ll_pt] = get_terrain_map(lat_lon=anchor_point,
                                          sample_dist = sample_dist,
                                          extent = extent,
                                          heading = heading)

scouter/terrain_viewer.py

@@ -22,6 +22,9 @@ def linear_features(parameters):
     bwlf = np.fliplr(np.rot90(np.rot90(bwlf)))
     bwinac = np.fliplr(np.rot90(np.rot90(bwinac)))
     szelev = np.fliplr(np.rot90(np.rot90(szelev)))
+    # bwlf = np.rot90(np.rot90(np.rot90(np.fliplr(np.rot90(bwlf)))))
+    # bwinac = np.rot90(np.rot90(np.rot90(np.fliplr(np.rot90(bwinac)))))
+    # szelev = np.rot90(np.rot90(np.rot90(np.fliplr(np.rot90(szelev)))))
 
     # rescale z data to match map size
     map_res = parameters['res']
@@ -50,28 +53,19 @@ def linear_features(parameters):
     lf_elev_pts = lf_elev_pts[np.nonzero(lf_elev_pts)]
     inac_elev_pts = inac_elev_pts[np.nonzero(inac_elev_pts)]
     # normalize elevation to have zero at lowest point
-    # WAS WORKING HERE
-    # lf_elev_pts = lf_elev_pts - np.min(lf_elev_pts)
-    # inac_elev_pts = inac_elev_pts - np.min(inac_elev_pts)
+    if lf_elev_pts.size != 0:
+        lf_elev_pts = lf_elev_pts - np.min(lf_elev_pts)
+    if inac_elev_pts.size != 0:
+        inac_elev_pts = inac_elev_pts - np.min(inac_elev_pts)
 
     lf_pts = np.array(lf_idx)*feat_res + parameters['xlims'][0] # in units of meters now
     inac_pts = np.array(inac_idx)*feat_res + parameters['xlims'][0]
-    # lf_elev_pts = np.array(lf_elev_idx)*feat_res + parameters['xlims'][0] # in units of meters now
-    # inac_elev_pts = np.array(inac_elev_idx)*feat_res + parameters['xlims'][0]
 
     # pair linear feature points and inac points with their elevation
 
     # lf_pts = np.vstack([lf_pts, lf_elev_pts])
     # inac_pts = np.vstack([inac_pts, inac_elev_pts])
 
-    # inac_pts = np.vstack([inac_idx, elev_idx[inac_idx[0],inac_idx[1]].flatten()])
-
-    # flip y axis
-    # lf_pts[1] = parameters['xlims'][1] - lf_pts[1] + parameters['xlims'][0]
-    # inac_pts[1] = parameters['xlims'][1] - inac_pts[1] + parameters['xlims'][0]
-
-    # might still need to do some rotations/flips
-
     return lf_pts, inac_pts
 
 def plot_all(parameters, mc_object, robot_paths, searcher_paths, title = "Sim", smooth_paths = True,
@@ -112,6 +106,7 @@ def plot_all(parameters, mc_object, robot_paths, searcher_paths, title = "Sim",
             y_idx = np.argmin(np.abs(y_terr - pt[1]))
             if z_terr_func.ev(pt[0], pt[1]) >= (z_traj[j]-4):
                 z_traj[j] = z_terr_func.ev(pt[0], pt[1]) + 4
+                pass
             else:
                 pass
 
@@ -122,7 +117,7 @@ def plot_all(parameters, mc_object, robot_paths, searcher_paths, title = "Sim",
 
         # add some smoothing for long trajectories
         if smooth_paths:
-            tck, unused = interpolate.splprep([x_traj, y_traj, z_traj], k=5, s=750)
+            tck, unused = interpolate.splprep([x_traj, y_traj, z_traj], k=5, s=500)
             unew = np.linspace(0,1,30)
             smoothed_traj = interpolate.splev(unew, tck)
             x_traj_smoothed = smoothed_traj[0]
@@ -177,17 +172,23 @@ def plot_all(parameters, mc_object, robot_paths, searcher_paths, title = "Sim",
     # get linear feature info
     [lf_points, inac_points] = linear_features(parameters)
 
-    # lf_points = np.vstack([lf_points, elev_points])
-    # inac_points = np.vstack([inac_points, elev_points])
     lf_points = np.vstack([lf_points, z_terr_func.ev(lf_points[0], lf_points[1])+1])
     inac_points = np.vstack([inac_points, z_terr_func.ev(inac_points[0], inac_points[1])+1])
 
 
     plot_data.append(go.Scatter3d(z = lf_points[2], x = lf_points[0], y = lf_points[1],
-                                  mode = "markers", name="L. Feat.", marker=dict(size = 5, color = 'orange')))
+                                  mode = "markers", name="L. Feat.", marker=dict(size = 3, color = 'skyblue')))
 
     plot_data.append(go.Scatter3d(z = inac_points[2], x = inac_points[0], y = inac_points[1],
-                                  mode = "markers", name="Inac. Areas", marker=dict(size = 5, color = 'skyblue')))
+                                  mode = "markers", name="Inac. Areas", marker=dict(size = 3, color = 'salmon')))
+
+    # add ipp to plot
+    x_ipp = x_terr.shape[0]
+    y_ipp = y_terr.shape[0]
+    z_ipp = z_terr_func.ev(x_ipp, y_ipp) + 4
+
+    plot_data.append(go.Scatter3d(z = z_ipp, x = x_ipp, y = y_ipp,
+                                  name="IPP", marker=dict(color='limeGreen', size = 12, line=dict(color='black',width=8))))
 
     fig = go.Figure(data = plot_data)
     fig.update_layout(title=title, autosize=False, width=2000, height=1200,

terrain_animation.py

@@ -64,7 +64,7 @@ for i in range(ani_length):
     print(samp_range)
     print(samp_dist)
     print("-------------")
-    [e_tmp, x_tmp, y_tmp, data_tmp, cen_pt_ll] = get_terrain_map(terrainLocation, sample_dist = samp_dist, extent = samp_range, heading = i*2, show_plot = False, verbosity = False)
+    [_, e_tmp, x_tmp, y_tmp, data_tmp, cen_pt_ll] = get_terrain_map(terrainLocation, sample_dist = samp_dist, extent = samp_range, heading = i*2, show_plot = False, verbosity = False)
     # e[:, :, i] = e_tmp
     # x[:, :, i] = x_tmp
     # y[:, :, i] = y_tmp

test_robotgp.py

@@ -158,7 +158,7 @@ def main(iteration = 0, parameters = -1):
                  mc_object = mc,
                  robot_paths = robot_paths_local,
                  searcher_paths = mc.searcher_class.searchers_list,
-                 smooth_paths = False,
+                 smooth_paths = True,
                  show_heatmap = True,
                  show_contours = True,
                  cs_name = 'thermal'
@@ -194,6 +194,9 @@ if __name__ == "__main__":
 
     kentland_linfeat = 'C:\\Users\\Larkin\\ags_grabber\\matlab_data\\BW_LFandInac_Zelev_kentland.mat' # filename used for linear features/inac
     hmpark_linfeat = 'C:\\Users\\Larkin\\ags_grabber\\matlab_data\\BW_LFandInac_Zelev_hmpark.mat' # filename used for linear features/inac
+
+    # KENTLAND case
+
     # n_max = 6
     # s_max = 2
     # global_fail_max = 1000
@@ -202,7 +205,7 @@ if __name__ == "__main__":
     # start_time = time.time()
     #
     # for n in range(1, n_max + 1):
-    #     for s in range(2,s_max + 1):
+    #     for s in range(1,s_max + 1):
     #             params = ({
     #                 'save_folder': 'kentland_n{}_s{}_basic'.format(n,s),
     #                 'lp_model': 'custom',
@@ -212,7 +215,7 @@ if __name__ == "__main__":
     #                 'anchor_point': [37.197730, -80.585233], # kentland
     #                 'num_searchers': s,
     #                 'num_robots': n,
-    #                 'lp_filename': kentland_heatmap
+    #                 'lp_filename': kentland_heatmap,
     #                 'lin_feat_filename': kentland_linfeat
     #             })
     #             params = Default(params).params
@@ -225,8 +228,6 @@ if __name__ == "__main__":
     #                         try:
     #                             main(iteration = counter, parameters=params)
     #                             print("done run")
-    #                             print("stahp")
-    #                             print("stahp pls")
     #                         except RuntimeError as e:
     #                             print("\n\n ------ bad memory, re trying ------\n")
     #                             global_fails += 1
@@ -238,6 +239,7 @@ if __name__ == "__main__":
 
     #-----------------------------------------------------------------------------------------------
 
+    # HMPARK case
     n_max = 6
     s_max = 2
     global_fail_max = 1000
@@ -245,7 +247,7 @@ if __name__ == "__main__":
     avg_runs = 5
     start_time = time.time()
 
-    for n in range(1, n_max + 1):
+    for n in range(4, n_max + 1):
         for s in range(2,s_max + 1):
                 params = ({
                     'save_folder': 'hmpark_n{}_s{}_basic'.format(n,s),