3D prob map added

.gitignore

@@ -2,3 +2,4 @@ __pycache__/
 .ipynb_checkpoints/
 .vscode/
 *.ipynb
+.DS_Store

ProbMap.py

@@ -44,6 +44,10 @@ class ProbMap:
         self.center_y = center_y
         self.init_val = init_val
         self.false_alarm_prob = false_alarm_prob
+        # pre-calculated v for detected or not detected targets
+        self.v_for_1 = np.log(self.false_alarm_prob/(1-self.false_alarm_prob))
+        self.v_for_0 = np.log((1-self.false_alarm_prob)/self.false_alarm_prob)
+        
 
         self._left_lower_x = self.center_x - self.width / 2.0 * self.resolution
         self._left_lower_y = self.center_y - self.height / 2.0 * self.resolution
@@ -143,16 +147,18 @@ class ProbMap:
             point_ind = tuple(
                 self.get_xy_index_from_xy_pos(x_pos, y_pos))
             # meas_index[point_ind] = meas_confidence
+            meas_confidence = 1 - self.false_alarm_prob
             meas_index[point_ind] = np.log(
                 self.false_alarm_prob/meas_confidence)
+        # logging.debug(f"THE DETECTED: {meas_index}")
         return meas_index
 
-    def generate_zero_meas(self):
-        def cut(x): return 1e-6 if x <= 1e-6 else 1 - \
-            1e-6 if x >= 1-1e-6 else x
-        meas_confidence = cut(np.random.normal(0.85, 0.1))
-        x = np.log((1-self.false_alarm_prob)/(1-meas_confidence))
-        return x
+    # def generate_zero_meas(self):
+    #     def cut(x): return 1e-6 if x <= 1e-6 else 1 - \
+    #         1e-6 if x >= 1-1e-6 else x
+    #     meas_confidence = cut(np.random.normal(0.85, 0.1))
+    #     x = np.log((1-self.false_alarm_prob)/(1-meas_confidence))
+    #     return x
 
     def map_update(self, local_measurement, neighbor_measurement, N, d):
         """Update the probability map using measurements from local and neighbors
@@ -174,9 +180,10 @@ class ProbMap:
 
         # Time decaying factor
         # NOTE Fine tune this param to get a good performance
-        alpha = 8
-        T = 0.1
-        decay_factor = np.exp(-alpha*T)
+        # alpha = 8
+        # T = 0.1
+        # decay_factor = np.exp(-alpha*T)
+        decay_factor = 0.9
         # The diagram below shows the composition of the information for each update
         # ┌─────────────────────────────────────────────────────┐
         # │ Whole area                  .─────────.             │
@@ -203,20 +210,21 @@ class ProbMap:
 
         # update all existing grids (Area 1,2,3,4)
         for cell_ind in list(self.non_empty_cell):
-            # Check if it's in area 2 or 4 (means we have local measuments about it)
+            # Check if it's in area 2 or 4 (means we have local measurements about it)
             if cell_ind in local_measurement:
                 v_local = local_measurement[cell_ind]
                 del local_measurement[cell_ind]
             else:
                 # If not, we believe there is no targets in that grid
-                v_local = self.generate_zero_meas()
+                # v_local = self.generate_zero_meas()
+                v_local = self.v_for_0
 
             if cell_ind in neighbor_measurement:
                 v_neighbors = neighbor_measurement[cell_ind]
                 del neighbor_measurement[cell_ind]
             else:
                 v_neighbors = sum(
-                    [self.generate_zero_meas() for i in range(d)])
+                    [self.v_for_0 for i in range(d)])
 
             Q = weight_local*(self.non_empty_cell[cell_ind] + v_local) + weight_neighbor * (
                 d*self.non_empty_cell[cell_ind]+v_neighbors)
@@ -231,12 +239,15 @@ class ProbMap:
                 try:
                     v_local = local_measurement[cell_ind]
                 except KeyError:
-                    v_local = self.generate_zero_meas()
+                    # v_local = self.generate_zero_meas()
+                    v_local = self.v_for_0
                 try:
                     v_neighbors = neighbor_measurement[cell_ind]
                 except KeyError:
+                    # v_neighbors = sum(
+                    #     [self.generate_zero_meas() for i in range(d)])
                     v_neighbors = sum(
-                        [self.generate_zero_meas() for i in range(d)])
+                        [self.v_for_0 for i in range(d)])
                 Q = weight_local*(self.init_val + v_local) + weight_neighbor * (
                     d*self.init_val+v_neighbors)
                 self.set_value_from_xy_index(
@@ -267,8 +278,8 @@ class ProbMap:
         Args:
             threshold (float): Values higher than this will be returned
         """
-        logging.debug(f"{self.non_empty_cell}")
-        lower_threshold = 0.1
+        # logging.debug(f"{self.non_empty_cell}")
+        lower_threshold = 0.05
         if threshold < 0.5:
             # shrink the lower threshold value
             lower_threshold *= threshold
@@ -293,7 +304,9 @@ class ProbMap:
                 if normed_prob >= threshold:
                     self.prob_map[cell_ind] = normed_prob
                 else:
+                    self.delete_value_from_xy_index(cell_ind)
                     del self.prob_map[cell_ind]
+                    pass
                 # if normed_prob <= lower_threshold*8:
                 #     # keep some uncertainty between the lower and upper thresholds
                 #     self.delete_value_from_xy_index(cell_ind)
@@ -311,6 +324,7 @@ class ProbMap:
                     # logging.debug(f"Deleting {cell_ind},{prob}")
                     # keep some uncertainty between the lower and upper thresholds
                     self.delete_value_from_xy_index(cell_ind)
+                    # pass
                     
 
     def get_target_est(self, threshold, normalization=False):

Robot.py

@@ -26,36 +26,36 @@ class Robot:
         self.position = position
         # facing direction
         self.ang = np.random.random()*360
-        self.desierd_position = self.position
+        self.desired_position = self.position
         self.in_position = True
         self.speed = 0.0
         self.rate = self.simulator.rate
         self.max_speed = 100.0  # m/s
 
-    def waypoint_ctrl(self, speed=None, desierd_pos: np.array = None):
+    def waypoint_ctrl(self, speed=None, desired_pos: np.array = None):
         # if got new waypoint, update
-        if not np.any(desierd_pos == None):
-            logging.debug(f"{self.name}{self.id} set new desierd position")
-            self.desierd_position = desierd_pos
+        if not np.any(desired_pos == None):
+            # logging.debug(f"{self.name}{self.id} set new desired position")
+            self.desired_position = desired_pos
 
         if speed is None:
-            logging.debug(f"{self.name}_{self.id} set to max speed")
+            # logging.debug(f"{self.name}_{self.id} set to max speed")
             self.speed = self.max_speed
         else:
             if speed > self.max_speed:
                 logging.warning("Given speed is over maximum")
             self.speed = speed
 
-        if np.any(self.position != self.desierd_position):
-            logging.debug(
-                f"{self.name}_{self.id} moving to {self.desierd_position}")
+        if np.any(self.position != self.desired_position):
+            # logging.debug(
+            #     f"{self.name}_{self.id} moving to {self.desired_position}")
             self.in_position = False
             one_time_step_length = self.speed/self.rate
-            difference = self.desierd_position - self.position
+            difference = self.desired_position - self.position
             distance = np.linalg.norm(difference)
             # if it's close enough to the goal position, make it in position
             if distance <= one_time_step_length:
-                self.position = self.desierd_position
+                self.position = self.desired_position
                 self.in_position = True
             else:
                 # move to the goal point at the given speed

Simsim.py

@@ -1,9 +1,17 @@
+import logging
+from collections import deque
+
 import matplotlib.pyplot as plt
 import numpy as np
+from matplotlib import cm
+from matplotlib.ticker import LinearLocator
+
 from target import Target
 from tracker import Tracker
-import logging
-from collections import deque
+
+import sys
+import numpy
+numpy.set_printoptions(threshold=sys.maxsize)
 
 
 class Simsim:
@@ -36,9 +44,14 @@ class Simsim:
 
         plt.ion()
 
-        self.fig, (self.plt_sim, self.plt_pm) = plt.subplots(
-            1, 2, figsize=(10, 5), dpi=160)
-        plt.axis('equal')
+        # self.fig, (self.plt_sim, self.plt_pm) = plt.subplots(
+        #     1, 2, figsize=(10, 5), dpi=160)
+        fig = plt.figure(figsize=(20, 8))
+        self.plt_sim = fig.add_subplot(121)
+        # plt.axis('equal')
+        self.plt_pm = fig.add_subplot(122, projection='3d')
+        # plt.axis('equal')
+        self.plt_sim.axis('equal')
 
     def add_tracker(self, name, position, sensor_rad):
         tracker = Tracker(self, name, len(self.trackers),
@@ -72,8 +85,8 @@ class Simsim:
             self.plt_pm.cla()
             self.plt_sim.set_xlim(0, self.map_size[0])
             self.plt_sim.set_ylim(0, self.map_size[1])
-            self.plt_pm.set_xlim(10000, 20000)
-            self.plt_pm.set_ylim(10000, 20000)
+            self.plt_pm.set_xlim(1000, 2000)
+            self.plt_pm.set_ylim(1000, 2000)
 
             if ground_truth:
                 for target in self.targets:
@@ -85,7 +98,7 @@ class Simsim:
                 self.plt_sim.plot([t0.position[0], t1.position[0]],
                                   [t0.position[1], t1.position[1]],
                                   linewidth=1, color='g', alpha=0.5)
-
+            Z = np.zeros([2000, 2000])
             for t in self.trackers:
                 # draw the trackers
                 self.plt_sim.scatter(t.position[0], t.position[1],
@@ -95,16 +108,29 @@ class Simsim:
                 circle = plt.Circle(
                     (t.position), t.sensor.coverage_radius, fill=False, color='grey', alpha=0.3)
                 self.plt_sim.add_patch(circle)
-
                 for est in t.target_estimates:
                     det_pos = est[0:2]
                     # det_abs_pos = det_pos+t.position
                     self.plt_sim.scatter(det_pos[0], det_pos[1],
                                          marker='^', s=2)
-                    for ind in t.prob_map.prob_map:
-                        new_ind = np.array(ind)  # - self.map_size
-                        self.plt_pm.scatter(
-                            new_ind[0], new_ind[1], marker='s', s=1, c='r', alpha=t.prob_map.prob_map[ind])
-                        self.plt_pm.annotate(f"{t.prob_map.prob_map[ind]:.3e}", (new_ind[0], new_ind[1]+5),
-                                             fontsize=2)
+                # t = self.trackers[2]
+                for ind in t.prob_map.prob_map:
+                    Z[ind] = t.prob_map.prob_map[ind]
+                    for dx in [-2, -1, 0, 1, 2]:
+                        for dy in [-2, -1, 0, 1, 2]:
+                            if Z[ind[0]+dx, ind[1]+dy] <= t.prob_map.prob_map[ind]:
+                                Z[ind[0]+dx, ind[1] +
+                                    dy] = t.prob_map.prob_map[ind]
+            # print(Z)
+            X = np.arange(1000, 2000, 1)
+            Y = np.arange(1000, 2000, 1)
+            X, Y = np.meshgrid(X, Y)
+            self.plt_pm.set_zlim(0, 1.01)
+            # print(Z)
+            self.plt_pm.plot_surface(
+                X, Y, Z[1000:, 1000:], cmap='RdBu_r', rcount=150, ccount=150, antialiased=True)
+            self.plt_pm.view_init(elev=35., azim=0)
+            plt.gca().invert_yaxis()
+            # self.plt_pm.plot_surface(X, Y, Z, cmap=cm.bwr, linewidth=5, antialiased=True)
+            self.plt_pm.zaxis.set_major_locator(LinearLocator(10))
             plt.pause(1/self.rate)

main.py

@@ -9,7 +9,7 @@ if __name__ == "__main__":
     s.add_tracker('tracker', np.array([300, 500]), sensor_rad=250)
     s.add_tracker('tracker', np.array([700, 500]), sensor_rad=250)
     s.add_tracker('tracker', np.array([620, 220]), sensor_rad=150)
-    s.add_target('tgt', np.array([150, 50]))
+    # s.add_target('tgt', np.array([150, 50]))
     s.add_target('tgt', np.array([500, 490]))
     s.add_target('tgt', np.array([600, 290]))
     s.add_target('tgt', np.array([180, 500]))
@@ -17,4 +17,4 @@ if __name__ == "__main__":
     s.add_target('tgt', np.array([300, 500]))
     s.add_target('tgt', np.array([630, 500]))
     s.add_edges([[0, 1], [1, 2], [0, 2], [3, 0]])
-    s.run(logging.DEBUG, ground_truth=True)
+    s.run(logging.DEBUG, ground_truth=1)

target.py

@@ -12,5 +12,5 @@ class Target(Robot):
             self.ang = (self.ang + np.deg2rad(15)) % 360
             rot_mat = np.asarray([[np.cos(self.ang), -np.sin(self.ang)],
                                   [np.sin(self.ang), np.cos(self.ang)]])
-            self.waypoint_ctrl(speed=20, desierd_pos=(self.position
+            self.waypoint_ctrl(speed=20, desired_pos=(self.position
                                                       - np.dot([0, 10], rot_mat)))

tracker.py

@@ -51,9 +51,9 @@ class Tracker(Robot):
 
         self.area_width = 2000  # meter
         self.area_height = 2000  # meter
-        self.resolution = 0.1  # meter
+        self.resolution = 1  # meter
         self.prob_map = ProbMap(self.area_width, self.area_height, self.resolution,
-                                center_x=0.0, center_y=0.0, init_val=0.05,
+                                center_x=0.0, center_y=0.0, init_val=0.6,
                                 false_alarm_prob=0.05)
 
         self.observations = dict()  # type: dict[tuple]
@@ -124,13 +124,13 @@ class Tracker(Robot):
             self.ang = (self.ang + np.deg2rad(np.random.randint(-360, 360))) % 360
             rot_mat = np.asarray([[np.cos(self.ang), -np.sin(self.ang)],
                                   [np.sin(self.ang), np.cos(self.ang)]])
-            self.waypoint_ctrl(speed=20, desierd_pos=(self.position
+            self.waypoint_ctrl(speed=20, desired_pos=(self.position
                                                       - np.dot([0, 10], rot_mat)))
 
     def job(self):
         self.random_moving()
         self.sensing()
-        logging.debug(f"{self.log_head} OBSERVATION: {self.observations}")
+        # logging.debug(f"{self.log_head} OBSERVATION: {self.observations}")
         shareable_v = self.prob_map.generate_shareable_v(
             self.observations)
 
@@ -160,7 +160,9 @@ class Tracker(Robot):
         self.prob_map.consensus(neighbors_map)
 
         self.target_estimates = self.prob_map.get_target_est(
-            0.6, normalization=False)
+            0.5, normalization=True)
         logging.debug(
             f"{self.name}_{self.id} ProbMap: {self.prob_map.prob_map}")
+        logging.debug(
+            f"{self.name}_{self.id} NonEmp: {self.prob_map.non_empty_cell}")
         # print(target_estimates)