plots 1 and 3 are now correct

Author: zhanwenchen

README.md

@@ -16,4 +16,4 @@ consideration is that Phil sucks at C++ but no one sucks at Python.
 
 # TODOs
 
-- [] BUG: Histogram grid active region is upside down.
+- [] BUG: Histogram grid active region is upside down. Potential cause: have been using \[x\]\[y\] but should have been \[y\]\[x\]

lib/histogram_grid.py

@@ -27,8 +27,10 @@ def from_map(cls, map_fname, active_region_dimension, resolution, robot_location
             reader = csv.reader(f, delimiter=" ")
             lines = list(reader)
 
+        # Convert string "1"s and "0"s to integer 1s and 0s
         lines = list(map(lambda l: list(map(int, l)), lines))
-#         print(*lines, sep="\n")
+        print("histogram_grid: histogram =")
+        print(*lines, sep="\n")
         dimension = (len(lines[0]), len(lines))
         hg = cls(dimension, resolution, robot_location, active_region_dimension)
         hg.histogram_grid = lines
@@ -62,15 +64,17 @@ def update_certainty_at_continuous_point(self, continuous_point, certainty):
             certainty: certainty value to set.
         """
         discrete_x, discrete_y = self.continuous_point_to_discrete_point(continuous_point)
-        self.histogram_grid[discrete_x][discrete_y] = certainty
+        # self.histogram_grid[discrete_x][discrete_y] = certainty
+        self.histogram_grid[discrete_y][discrete_x] = certainty
 
 
     def get_certainty_at_discrete_point(self, discrete_point):
         """
         Returns the certainty of an object being present at the given node
         """
         discrete_x, discrete_y = discrete_point
-        return self.histogram_grid[discrete_x][discrete_y]
+        # return self.histogram_grid[discrete_x][discrete_y]
+        return self.histogram_grid[discrete_y][discrete_x]
 
 
     def get_continuous_distance_between_discrete_points(self, discrete_start, discrete_end):
@@ -158,8 +162,8 @@ def get_obstacles(self):
             for col_idx, cell in enumerate(row):
                 if cell != 0:
                     # print("histogram_grid: obstacle =", (row_idx, col_idx))
-                    obstacles_points_x.append(row_idx)
-                    obstacles_points_y.append(col_idx)
+                    obstacles_points_x.append(col_idx)
+                    obstacles_points_y.append(row_idx)
         # print("histogram_grid: obstacles_points =", list(zip(obstacles_points_x, obstacles_points_y)))
         return obstacles_points_x, obstacles_points_y
 

lib/path_planner.py

@@ -70,6 +70,7 @@ def generate_histogram(self, robot_location):
                 polar_histogram.add_certainty_to_bin_at_angle(robot_to_node_angle, delta_certainty)
                 histogram_grid.get_certainty_at_discrete_point(node_considered)
 
+
         polar_histogram.smooth_histogram(self.l)
 
 

lib/robot.py

@@ -128,6 +128,7 @@ def loop(self, num_steps, draw=True):
                     fill=False
                 )
             )
+            simulation_plot.invert_yaxis()
 
             # 2. Plot the polar histogram
             num_bins = self.path_planner.polar_histogram.num_bins
@@ -155,10 +156,10 @@ def my_autopct(pct):
 
             # 3. Plot the valley
             histogram_grid_active_region = self.path_planner.histogram_grid.get_histogram_grid_active_region(active_region_min_x, active_region_min_y, active_region_max_x, active_region_max_y)
-            print('histogram =')
+            print('active region histogram =')
             print(*histogram_grid_active_region, sep='\n')
             histogram_grid_plot.clear()
-            histogram_grid_plot.matshow(histogram_grid_active_region, origin="lower")
+            histogram_grid_plot.matshow(histogram_grid_active_region, origin="upper")
 
 
             # self.draw(ax)
@@ -185,6 +186,7 @@ def my_autopct(pct):
                 active_region_min_x, active_region_min_y, active_region_max_x, active_region_max_y = self.path_planner.histogram_grid.get_active_region(self.location)
                 rectangle.set_bounds(active_region_min_x, active_region_min_y, active_region_max_x - active_region_min_x, active_region_max_y - active_region_min_y)
 
+                # simulation_plot.invert_yaxis()
 
                 # 2. Replot the polar histogram
                 # sectors = self.path_planner.get_sectors() # NOTE: sectors are only valid
@@ -214,10 +216,10 @@ def my_autopct(pct):
 
                 # 3. Replot the histogram_grid
                 histogram_grid_active_region = self.path_planner.histogram_grid.get_histogram_grid_active_region(active_region_min_x, active_region_min_y, active_region_max_x, active_region_max_y)
-                print('histogram =')
+                print('active region histogram =')
                 print(*histogram_grid_active_region, sep='\n')
                 histogram_grid_plot.clear()
-                histogram_grid_plot.matshow(histogram_grid_active_region, origin="lower")
+                histogram_grid_plot.matshow(histogram_grid_active_region, origin="upper")
 
 
                 # 4. Actually display the plots