Use range query to search for segments when scan topic is not available

Signed-off-by: Anh Nguyen <anh.nguyen.2@tier4.jp>

map/autoware_tp_manager/scripts/tp_checker.py

@@ -44,6 +44,7 @@ def __init__(self):
         self.changed_dir = None  # A directory contains the segments that need to be examined
         self.result_csv = None  # Path to the result CSV file
         self.tp_path = None  # Path to the file that contains TPs of map segments
+        self.range = 50.0
 
     def __initialize(self, score_dir: str):
         if not os.path.exists(score_dir):
@@ -210,6 +211,43 @@ def __set_color_based_on_mark(self, mark) -> int:
         rgba = struct.unpack("I", tmp_rgb)[0]
 
         return rgba
+
+    def __find_candidate_segments(self, stamp, t_pose):
+        if not(self.scan_df is None) and self.scan_df.shape[0] > 0:
+            sid = tpu.stamp_search(stamp, self.scan_df, self.scan_df.shape[0])
+
+            if sid < 0:
+                return None
+
+            closest_scan = pc2.read_points(self.scan_df[sid, 1], skip_nans=True)
+
+            segment_set = set()
+
+            for p in closest_scan:
+                tp = tpu.transform_p(p, t_pose)
+
+                # Hash the point to find the segment containing it
+                sx = int(tp[0] / self.resolution) * int(self.resolution)
+                sy = int(tp[1] / self.resolution) * int(self.resolution)
+                seg_idx = str(sx) + "_" + str(sy)
+
+                segment_set.add(seg_idx)
+
+            return segment_set
+        else:
+            segment_set = set()
+            # If scans are not available, use range query 
+            lx = int((t_pose[3, 0] - self.range) / self.resolution) * int(self.resolution)
+            ux = int((t_pose[3, 0] + self.range) / self.resolution) * int(self.resolution)
+            ly = int((t_pose[3, 1] - self.range) / self.resolution) * int(self.resolution)
+            uy = int((t_pose[3, 1] + self.range) / self.resolution) * int(self.resolution)
+
+            for sx in range(lx, ux + 1, 1):
+                for sy in range(ly, uy + 1, 1):
+                    segment_set.add(str(sx) + "_" + str(sy))
+
+            return segment_set
+
 
     def __mark_changed_poses(self):
         progress_bar = tqdm.tqdm(total=self.pose_df.shape[0])
@@ -229,30 +267,20 @@ def __mark_changed_poses(self):
                 # print("Invalid pose. Skip!")
                 continue
 
-            # Find the closest scan and tp
-            sid = tpu.stamp_search(stamp, self.scan_df, self.scan_df.shape[0])
+            # Find the closest tp
             tid = tpu.stamp_search(stamp, self.tp_df, self.tp_df.shape[0])
 
-            if sid < 0 or tid < 0:
-                print("No closest scan and tp were found. Skip!")
+            if tid < 0:
+                print("No closest tp were found. Skip!")
                 continue
-
-            closest_scan = pc2.read_points(self.scan_df[sid, 1], skip_nans=True)
             closest_tp = self.tp_df[tid, 1]
 
-            # Transform the scan and find the segments that cover the transformed points
-            segment_set = set()
-
-            for p in closest_scan:
-                tp = tpu.transform_p(p, pose)
-
-                # Hash the point to find the segment containing it
-                sx = int(tp[0] / self.resolution) * int(self.resolution)
-                sy = int(tp[1] / self.resolution) * int(self.resolution)
-                seg_idx = str(sx) + "_" + str(sy)
-
-                segment_set.add(seg_idx)
+            # Find the candidate segments that cover the pose
+            segment_set = self.__find_candidate_segments(stamp, pose)
 
+            if segment_set is None:
+                continue
+
             tp_sum = 0.0
             valid_segment_num = 0
 
@@ -353,7 +381,10 @@ def processing(
         tp_topic: str,
         scan_topic: str,
         mode: str,
+        range: float
     ):
+        if range > 0:
+            self.range = range
         self.__initialize(score_path)
         self.__get_pcd_segments_and_scores()
 
@@ -411,6 +442,13 @@ def processing(
         required=False,
         type=str,
     )
+    parser.add_argument(
+        "--range",
+        help="The range to query segments that cover a pose",
+        default=50.0,
+        required=False,
+        type=float,
+    )
 
     args = parser.parse_args()
 
@@ -421,10 +459,11 @@ def processing(
     print("Topic of Transformation Probability: {0}".format(args.tp_topic))
     print("Topic of scan data: {0}".format(args.scan_topic))
     print("Mode of checking: {0}".format(args.mode))
+    print("Range to query map segments that cover a pose: {0}".format(args.range))
 
     # Run
     checker = TPChecker()
 
     checker.processing(
-        args.score_path, args.bag_path, args.pose_topic, args.tp_topic, args.scan_topic, args.mode
+        args.score_path, args.bag_path, args.pose_topic, args.tp_topic, args.scan_topic, args.mode, args.range
     )

map/autoware_tp_manager/scripts/tp_collector.py

@@ -51,8 +51,6 @@ def __initialize(self, pcd_map_dir: str, output_path: str, resolution: float):
         self.output_path = output_path
         self.resolution = resolution
 
-        print("Type resolution = {0}".format(type(self.resolution)))
-
         if not os.path.exists(pcd_map_dir):
             print("Error: {0} does not exist!".format(pcd_map_dir))
             exit()
@@ -111,7 +109,42 @@ def __get_pcd_segments_and_scores(self):
                 for index, row in enumerate(reader):
                     self.segment_df[index, [1, 2]] = [float(row[1]), 1]
 
-    def __update_avg_tp(self):
+    def __find_candidate_segments(self, stamp, t_pose):
+        if self.scan_df.shape[0] > 0:
+            sid = tpu.stamp_search(stamp, self.scan_df, self.scan_df.shape[0])
+
+            if sid < 0:
+                return None
+
+            closest_scan = pc2.read_points(self.scan_df[sid, 1], skip_nans=True)
+
+            segment_set = set()
+
+            for p in closest_scan:
+                tp = tpu.transform_p(p, t_pose)
+
+                # Hash the point to find the segment containing it
+                sx = int(tp[0] / self.resolution) * int(self.resolution)
+                sy = int(tp[1] / self.resolution) * int(self.resolution)
+                seg_idx = str(sx) + "_" + str(sy)
+
+                segment_set.add(seg_idx)
+
+            return segment_set
+        else:
+            # If scans are not available, use range query 
+            lx = int((t_pose[3, 0] - 50.0) / self.resolution) * int(self.resolution)
+            ux = int((t_pose[3, 0] + 50.0) / self.resolution) * int(self.resolution)
+            ly = int((t_pose[3, 1] - 50.0) / self.resolution) * int(self.resolution)
+            uy = int((t_pose[3, 1] + 50.0) / self.resolution) * int(self.resolution)
+
+            for sx in range(start = lx, stop = ux, step = 1):
+                for sy in range(start = ly, stop = uy, step = 1):
+                    segment_set.add(str(sx) + "_" + str(sy))
+
+            return segment_set
+
+    def __update_tp_by_scan(self):
         progress_bar = tqdm.tqdm(total=self.scan_df.shape[0])
 
         for i in range(self.scan_df.shape[0]):
@@ -158,6 +191,66 @@ def __update_avg_tp(self):
 
         progress_bar.close()
 
+    def __update_tp_by_pose(self):
+        # If scans are not available, examine pose and update TPs of segments in vicinity
+        progress_bar = tqdm.tqdm(total=self.pose_df.shape[0])
+
+        for i in range(self.pose_df.shape[0]):
+            progress_bar.update(1)
+            stamp, pose = self.pose_df[i, :]
+            # Transposed pose for multiplication, as the points are in a row-major table
+            pose = pose.T
+            # Skip invalid poses
+            # print("Pose at {0} = {1}".format(i, pose))
+            if pose is None:
+                # print("None pose. Skip!")
+                continue
+
+            if pose[3, 0] == 0 and pose[3, 1] == 0 and pose[3, 2] == 0:
+                # print("Invalid pose. Skip!")
+                continue
+
+            # Find the closest tp
+            tid = tpu.stamp_search(stamp, self.tp_df, self.tp_df.shape[0])
+
+            if tid < 0:
+                print("No closest tp were found. Skip!")
+                continue
+            closest_tp = self.tp_df[tid, 1]
+
+            # Use range query to find segments that cover the current pose
+            segment_set = set()
+
+            lx = int((pose[3, 0] - 50.0) / self.resolution) * int(self.resolution)
+            ux = int((pose[3, 0] + 50.0) / self.resolution) * int(self.resolution)
+            ly = int((pose[3, 1] - 50.0) / self.resolution) * int(self.resolution)
+            uy = int((pose[3, 1] + 50.0) / self.resolution) * int(self.resolution)
+
+            for sx in range(lx, ux + 1):
+                for sy in range(ly, uy + 1, 1):
+                    segment_set.add(str(sx) + "_" + str(sy))
+
+            if segment_set is None:
+                continue
+
+            for key in segment_set:
+                if key in self.segment_dict:
+                    i = self.segment_dict[key]
+                    tp, counter = self.segment_df[i, [1, 2]]
+                    self.segment_df[i, [1, 2]] = [
+                        tp + 1.0 / (counter + 1) * (closest_tp - tp),
+                        counter + 1,
+                    ]
+
+        progress_bar.close()
+
+    def __update_avg_tp(self):
+
+        if not(self.scan_df is None) and self.scan_df.shape[0] > 0:
+            self.__update_tp_by_scan()
+        else:
+            self.__update_tp_by_pose()
+
     # Save the segment TPs
     def __save_tps(self):
         print("Saving TP to files")

map/autoware_tp_manager/scripts/tp_utility.py

@@ -26,64 +26,6 @@
 from tier4_debug_msgs.msg import Float32Stamped
 import tqdm
 
-
-class FixQueue:
-    def __init__(self):
-        self.queue_limit = 100  # Max number of items in the queue
-        self.data_queue = None  # Queue of data
-        self.head = 0  # Index to append a new item (enqueue index)
-        self.tail = 0  # Index to start remove/process items (dequeue index)
-        self.current_size = 0  # Number of items in the queue
-
-    def enqueue(self, item):
-        if self.is_full():
-            print("Cannot add new item. Queue is full!")
-            return False
-
-        if self.is_empty() or self.data_queue.shape[1] != len(item):
-            self.data_queue = np.ndarray((self.queue_limit, len(item)), dtype=object)
-        self.data_queue[self.tail, :] = item
-        self.tail += 1
-
-        if self.tail == self.queue_limit:
-            self.tail = 0
-
-        self.current_size += 1
-
-        return True
-
-    def drop(self, limitless=False, callback_func=None, *args):
-        if limitless:
-            end_id = self.tail
-        else:
-            end_id = self.head + int(self.queue_limit / 2)
-
-        if self.head < end_id:
-            drop_list = np.arange(self.head, end_id)
-        else:
-            drop_list = np.arange(self.head, end_id + self.queue_limit) % self.queue_limit
-
-        for i in drop_list:
-            # Process the item at index i
-            callback_func(self.data_queue[i], *args)
-
-        self.current_size -= len(drop_list)
-        self.head = end_id
-
-        if self.head == self.queue_limit:
-            self.head = 0
-
-    def is_full(self) -> bool:
-        if self.current_size == self.queue_limit:
-            return True
-        return False
-
-    def is_empty(self) -> bool:
-        if self.current_size == 0:
-            return True
-        return False
-
-
 # Some utility functions that is shared by both the tp collector and tp checker
 # Convert a pose to a 4x4 transformation matrix
 def __pose_to_mat(pose: Pose) -> np.array:
@@ -157,27 +99,36 @@ def parse_rosbag(bag_path: str, pose_topic: str, tp_topic: str, scan_topic: str)
     msg_count_by_topic = __get_message_count(bag_path)
     progress_bar = tqdm.tqdm(total=msg_count_by_topic["total"])
 
-    if msg_count_by_topic[pose_topic] == 0:
+    if not(pose_topic in msg_count_by_topic) or (msg_count_by_topic[pose_topic] == 0):
         print(
             "Error: the input rosbag contains no message from the topic {0}. Exit!".format(
                 pose_topic
             )
         )
-    if msg_count_by_topic[tp_topic] == 0:
+        pose_df = None
+        pose_to_publish = None
+    else:
+        pose_df = np.ndarray((msg_count_by_topic[pose_topic], 2), dtype=object)
+        pose_to_publish = np.ndarray((msg_count_by_topic[pose_topic], 2), dtype=object)
+
+    if not(tp_topic in msg_count_by_topic) or (msg_count_by_topic[tp_topic] == 0):
         print(
             "Error: the input rosbag contains no message from the topic {0}. Exit!".format(tp_topic)
         )
-    if msg_count_by_topic[scan_topic] == 0:
+        tp_df = None
+    else:
+        tp_df = np.ndarray((msg_count_by_topic[tp_topic], 2), dtype=object)
+
+    if not(scan_topic in msg_count_by_topic) or (msg_count_by_topic[scan_topic] == 0):
         print(
-            "Error: the input rosbag contains no message from the topic {0}. Exit!".format(
+            "The input rosbag contains no message from the topic {0}.!".format(
                 scan_topic
             )
         )
-
-    pose_df = np.ndarray((msg_count_by_topic[pose_topic], 2), dtype=object)
-    tp_df = np.ndarray((msg_count_by_topic[tp_topic], 2), dtype=object)
-    pose_to_publish = np.ndarray((msg_count_by_topic[pose_topic], 2), dtype=object)
-    scan_df = np.ndarray((msg_count_by_topic[scan_topic], 2), dtype=object)
+        scan_df = None
+    else:
+        scan_df = np.ndarray((msg_count_by_topic[scan_topic], 2), dtype=object)
+
     pose_df_idx = 0
     tp_df_idx = 0
     scan_df_idx = 0