Adding minimum distance suppression

semantic_mapping/src/ObjConsistencyMapper.py

@@ -219,11 +219,27 @@ def push_item_to_pushing_to(self, adding:dict, metadata:dict) -> str:
         It filters objects based on raw distance (rather than anything more fancy) at present.
             This is done to work out whether to update or to create a new entry.
         """
+        adding_pos = utils.getPoint(adding[self.consistency_args.position_attr]);
 
         query = {};
         for element in self.consistency_args.cross_ref_attr:
             query[element] = adding[element];
 
+        # For objects really close together, it might be a double detection...
+        if self.consistency_args.batch_nums_setup():
+            query[self.consistency_args.last_observation_batch] = adding[self.consistency_args.observation_batch_num];
+            possible_results:list = self.pushing_to.queryIntoCollection(query);
+            for element in possible_results:
+                element_pos = utils.getPoint(element[self.consistency_args.position_attr]);
+                dist = numpy.linalg.norm(adding_pos - element_pos);
+                if dist < 0.5:
+                    metadata['obj_uid'] = "";
+                    return adding, metadata;
+
+            query = {};
+            for element in self.consistency_args.cross_ref_attr:
+                query[element] = adding[element];
+
         # This means we have a greedy implementation that just takes the closest 
         # option each time. This is almost certainly fine (bar for really uncertain cases).
         if self.consistency_args.batch_nums_setup():
@@ -251,7 +267,6 @@ def push_item_to_pushing_to(self, adding:dict, metadata:dict) -> str:
 
         # Doing the filtering to work out which object you want to look at (if any).
         print("Max distance", max_distance);
-        adding_pos = utils.getPoint(adding[self.consistency_args.position_attr]);
         updating = None;
         num_prev_observations = math.inf;
         for element in possible_results:

semantic_mapping/src/main.py

@@ -198,6 +198,9 @@ def num_observation_threshold_query_callback(query_dict:dict):
         arena_boundary_region:orion_actions.msg.SOMBoxRegion = arena_boundary_regions.returns[0] if len(arena_boundary_regions.returns) else None;
 
     def push_person_callback(adding:dict, metadata:dict):
+        if len(metadata['obj_uid']) == 0:
+            return adding, metadata;
+
         object_position = utils.dict_to_obj(adding["obj_position"], geometry_msgs.msg.Pose());
 
         if object_region_manager.point_in_region(arena_boundary_region, object_position.position) == False: