diff --git a/map/autoware_tp_manager/scripts/tp_checker.py b/map/autoware_tp_manager/scripts/tp_checker.py index 73a8a81f..45b1ce70 100755 --- a/map/autoware_tp_manager/scripts/tp_checker.py +++ b/map/autoware_tp_manager/scripts/tp_checker.py @@ -24,36 +24,14 @@ import open3d as o3d import rclpy from rclpy.node import Node -from scipy import spatial as sp import sensor_msgs.msg as sensor_msgs import sensor_msgs_py.point_cloud2 as pc2 +from visualization_msgs.msg import Marker, MarkerArray import std_msgs.msg as std_msgs import tp_utility as tpu import tqdm import yaml - -def mark_changes(candidate_segments, rosbag_tp: float, segment_dict, segment_df): - tp_sum = 0.0 - valid_segment_num = 0 - - for key in candidate_segments: - if key in segment_dict: - tp_sum += segment_df[segment_dict[key], 1] - valid_segment_num += 1 - if valid_segment_num > 0: - expected_tp = tp_sum / float(valid_segment_num) - else: - expected_tp = 0 - - if (expected_tp > 0 and abs(expected_tp - rosbag_tp) / expected_tp >= 0.2) or ( - expected_tp == 0 and rosbag_tp > 0 - ): - for key in candidate_segments: - if key in segment_dict: - segment_df[segment_dict[key], 2] += 1 - - class TPChecker(Node): def __init__(self): @@ -92,6 +70,8 @@ def __initialize(self, score_dir: str): ) exit() + self.output_path = os.path.join(score_dir, "checking_result.csv") + # Read the input map directory and setup the segment dictionary def __get_pcd_segments_and_scores(self): # Read the metadata file and get the list of segments @@ -120,6 +100,8 @@ def __get_pcd_segments_and_scores(self): self.segment_df[index, 1] = float(row[1]) def __show(self): + # Publish map + ros_float_dtype = sensor_msgs.PointField.FLOAT32 ros_uint32_dtype = sensor_msgs.PointField.UINT32 dtype = np.float32 @@ -164,8 +146,50 @@ def __show(self): pc2_msg = pc2.create_cloud(header, fields, points) pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, "/autoware_tp_checker", 10) + + # Publish poses + self.pose_pub = self.create_publisher(MarkerArray, "/pose_result", 10) + marker_array_msg = MarkerArray() + + for i in range(self.pose_to_publish.shape[0]): + pose = self.pose_to_publish[i, 1] + + if pose is None: + continue + marker = Marker() + + marker.header.stamp = self.get_clock().now().to_msg() + marker.header.frame_id = 'map' + marker.id = i + marker.type = Marker.SPHERE + marker.action = Marker.ADD + marker.pose = pose + # Shift toward origin + marker.pose.position.x -= origin[0] + marker.pose.position.y -= origin[1] + marker.pose.position.z -= origin[2] + + marker.scale.x = 2.0 + marker.scale.y = 2.0 + marker.scale.z = 2.0 + + marker.color.a = 1.0 + + if self.mark[i] == 1: + marker.color.r = 255.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + else: + marker.color.r = 0.0 + marker.color.g = 0.0 + marker.color.b = 255.0 + + marker_array_msg.markers.append(marker) + while True: pcd_publisher.publish(pc2_msg) + self.pose_pub.publish(marker_array_msg) + time.sleep(5) def __set_color_based_on_mark(self, mark) -> int: @@ -186,22 +210,164 @@ def __set_color_based_on_mark(self, mark) -> int: return rgba + def __mark_changed_poses(self): + 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 scan and tp + sid = tpu.stamp_search(stamp, self.scan_df, self.scan_df.shape[0]) + 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!") + 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) + + tp_sum = 0.0 + valid_segment_num = 0 + + for key in segment_set: + if key in self.segment_dict: + tp_sum += self.segment_df[self.segment_dict[key], 1] + valid_segment_num += 1 + if valid_segment_num > 0: + expected_tp = tp_sum / float(valid_segment_num) + else: + expected_tp = 0 + + # If the expected tp and the actual tp is quite different, mark the pose as changed + if (expected_tp > 0 and abs(expected_tp - closest_tp) / expected_tp >= 0.2) \ + or (expected_tp == 0 and closest_tp > 0): + self.mark[i] = 1 + + progress_bar.close() + + def __mark_changed_segments(self): + progress_bar = tqdm.tqdm(total = self.pose_df.shape[0]) + + for i in range(self.scan_df.shape[0]): + progress_bar.update(1) + stamp, tmp_scan = self.scan_df[i, :] + scan = pc2.read_points(tmp_scan, skip_nans = True) + + # Find the closest pose and tp + pid = tpu.stamp_search(stamp, self.pose_df, self.pose_df.shape[0]) + tid = tpu.stamp_search(stamp, self.tp_df, self.tp_df.shape[0]) + + if pid < 0 or tid < 0: + continue + + closest_pose = self.pose_df[pid, 1] + + # Skip invalid poses + if closest_pose[3, 0] == 0 and closest_pose[3, 1] == 0 and closest_pose[3, 2] == 0: + continue + + 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 scan: + tp = tpu.transform_p(p, closest_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) + + tp_sum = 0.0 + valid_segment_num = 0 + + for key in segment_set: + if key in self.segment_dict: + tp_sum += self.segment_df[self.segment_dict[key], 1] + valid_segment_num += 1 + if valid_segment_num > 0: + expected_tp = tp_sum / float(valid_segment_num) + else: + expected_tp = 0 + + self.dual_tp[i, :] = [closest_tp, expected_tp] + + if (expected_tp > 0 and abs(expected_tp - closest_tp) / expected_tp >= 0.2) or ( + expected_tp == 0 and closest_tp > 0 + ): + # Mark changed segments + for key in segment_set: + if key in self.segment_dict: + self.segment_df[self.segment_dict[key], 2] += 1 + + progress_bar.close() + + def __save_results(self): + print("Saving checking results to a file {0}".format(self.output_path)) + + with open(self.output_path, "w") as f: + f.write("Actual_TP,expected_TP\n") + print("Number of poses = {0}".format(self.pose_df.shape[0])) + + for i in range(self.pose_df.shape[0]): + f.write("{0},{1}\n".format(self.dual_tp[i, 0], self.dual_tp[i, 1])) + + print("Done. Saved TPs at {0}".format(self.output_path)) + + def processing( - self, score_path: str, rosbag_path: str, pose_topic: str, tp_topic: str, scan_topic: str + self, score_path: str, rosbag_path: str, pose_topic: str, tp_topic: str, scan_topic: str, + mode: str ): self.__initialize(score_path) self.__get_pcd_segments_and_scores() - tpu.collect_rosbag_tp( - rosbag_path, - pose_topic, - tp_topic, - scan_topic, - self.resolution, - mark_changes, - self.segment_dict, - self.segment_df, - ) + output_dict = tpu.parse_rosbag(rosbag_path, pose_topic, tp_topic, scan_topic) + self.pose_df = output_dict["mat_pose"] + self.pose_to_publish = output_dict["pose"] + self.tp_df = output_dict["tp"] + self.scan_df = output_dict["scan"] + self.mark = np.zeros((self.pose_to_publish.shape[0], 1)) + # Actual TP and expected TP + self.dual_tp = np.zeros((self.pose_df.shape[0], 2)) + + if mode == "check_pose": + self.__mark_changed_poses() + self.__save_results() + elif mode == "check_segment": + self.__mark_changed_segments() + else: + print("Unrecognized mode! Exit!") + exit() self.__show() @@ -232,19 +398,28 @@ def processing( required=False, type=str, ) + parser.add_argument( + "--mode", + help="Check mode", + default="check_pose", + required=False, + type=str, + ) args = parser.parse_args() # Practice with string % a bit - print("Input PCD map at {0}".format(args.score_path)) - print("Input rosbag at {0}".format(args.bag_path)) - print("Topic of NDT poses {0}".format(args.pose_topic)) - print("Topic of Transformation Probability {0}".format(args.tp_topic)) - print("Topic of scan data {0}".format(args.scan_topic)) + print("Input PCD map at: {0}".format(args.score_path)) + print("Input rosbag at: {0}".format(args.bag_path)) + print("Topic of NDT poses: {0}".format(args.pose_topic)) + 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)) # Run checker = TPChecker() checker.processing( - args.score_path, args.bag_path, args.pose_topic, args.tp_topic, args.scan_topic + args.score_path, args.bag_path, args.pose_topic, args.tp_topic, args.scan_topic, + args.mode ) diff --git a/map/autoware_tp_manager/scripts/tp_collector.py b/map/autoware_tp_manager/scripts/tp_collector.py index 5ae40b45..ed89e136 100755 --- a/map/autoware_tp_manager/scripts/tp_collector.py +++ b/map/autoware_tp_manager/scripts/tp_collector.py @@ -25,16 +25,8 @@ from rclpy.node import Node import tp_utility as tpu import yaml - - -# Update the TP of a segment at index @idx, given a newly TP value -def update_avg_tp(candidate_segments, new_tp, segment_dict, segment_df): - for key in candidate_segments: - if key in segment_dict: - i = segment_dict[key] - tp, counter = segment_df[i, [1, 2]] - segment_df[i, [1, 2]] = [tp + 1.0 / (counter + 1) * (new_tp - tp), counter + 1] - +import tqdm +import sensor_msgs_py.point_cloud2 as pc2 class TPCollector(Node): def __init__(self): @@ -118,6 +110,51 @@ 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): + progress_bar = tqdm.tqdm(total = self.scan_df.shape[0]) + + for i in range(self.scan_df.shape[0]): + progress_bar.update(1) + stamp, tmp_scan = self.scan_df[i, :] + scan = pc2.read_points(tmp_scan, skip_nans = True) + + # Find the closest pose and tp + pid = tpu.stamp_search(stamp, self.pose_df, self.pose_df.shape[0]) + tid = tpu.stamp_search(stamp, self.tp_df, self.tp_df.shape[0]) + + if pid < 0 or tid < 0: + continue + + closest_pose = self.pose_df[pid, 1].T + + # Skip invalid poses + if closest_pose[3, 0] == 0 and closest_pose[3, 1] == 0 and closest_pose[3, 2] == 0: + continue + + 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 scan: + tp = tpu.transform_p(p, closest_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) + + 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() + ##### Save the segment TPs ##### def __save_tps(self): print("Saving TP to files") @@ -145,16 +182,15 @@ def processing( self.__get_pcd_segments_and_scores() # Read the rosbag and get the ndt poses and corresponding tps - tpu.collect_rosbag_tp( - rosbag_path, - pose_topic, - tp_topic, - scan_topic, - self.resolution, - update_avg_tp, - self.segment_dict, - self.segment_df, - ) + output_dict = tpu.parse_rosbag(rosbag_path, + pose_topic, + tp_topic, + scan_topic) + self.pose_df = output_dict["mat_pose"] + self.tp_df = output_dict["tp"] + self.scan_df = output_dict["scan"] + + self.__update_avg_tp() # Save the new TPs self.__save_tps() diff --git a/map/autoware_tp_manager/scripts/tp_utility.py b/map/autoware_tp_manager/scripts/tp_utility.py index 881ed899..ee51bd38 100644 --- a/map/autoware_tp_manager/scripts/tp_utility.py +++ b/map/autoware_tp_manager/scripts/tp_utility.py @@ -23,7 +23,6 @@ from rosbag2_py import SequentialReader from rosbag2_py import StorageOptions import sensor_msgs.msg as sensor_msgs -import sensor_msgs_py.point_cloud2 as pc2 from tier4_debug_msgs.msg import Float32Stamped import tqdm @@ -113,7 +112,7 @@ def transform_p(p, trans_mat_t: np.ndarray) -> np.ndarray: ##### Stamp search ##### -def __stamp_search(stamp: int, ref_df: np.ndarray, search_size: int) -> int: +def stamp_search(stamp: int, ref_df: np.ndarray, search_size: int) -> int: left = 0 right = search_size - 1 res = -1 @@ -144,53 +143,8 @@ def __get_message_count(rosbag_path: str): return output -def __scan_callback( - scan_tuple, pose_df, pose_df_len, tp_df, tp_df_len, resolution, callback_func=None, *args -): - stamp, scan = scan_tuple - tmp_scan = pc2.read_points(scan, skip_nans=True) - - # Find the closest pose - pid = __stamp_search(stamp, pose_df, pose_df_len) - - if pid <= 0: - return - - # Get the transposed transformation matrix - closest_p_t = pose_df[pid, 1].T - - # Skip some 0 poses at the beginning of the rosbag - if closest_p_t[3, 0] == 0 and closest_p_t[3, 1] == 0 and closest_p_t[3, 2] == 0: - return - - # Find the closest tp - tid = __stamp_search(stamp, tp_df, tp_df_len) - - if tid <= 0: - return - - closest_tp = tp_df[tid, 1] - - # Transform the scan and find the segments that cover the transformed points - # Record the segments that cover the scan - segment_set = set() - - for p in tmp_scan: - tp = transform_p(p, closest_p_t) - - # Hash the point to find the segment containing it - sx = int(tp[0] / resolution) * int(resolution) - sy = int(tp[1] / resolution) * int(resolution) - seg_idx = str(sx) + "_" + str(sy) - - segment_set.add(seg_idx) - - # Update/examine the segments' avg TP - callback_func(segment_set, closest_tp, *args) - - ##### Read the input rosbag and update map's TP ##### -def collect_rosbag_tp(bag_path: str, pose_topic: str, tp_topic: str, scan_topic: str, *args): +def parse_rosbag(bag_path: str, pose_topic: str, tp_topic: str, scan_topic: str): reader = SequentialReader() bag_storage_options = StorageOptions(uri=bag_path, storage_id="sqlite3") bag_converter_options = ConverterOptions( @@ -222,9 +176,11 @@ def collect_rosbag_tp(bag_path: str, pose_topic: str, tp_topic: str, 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) pose_df_idx = 0 tp_df_idx = 0 - scan_df = FixQueue() + scan_df_idx = 0 skip_count = 0 @@ -232,14 +188,16 @@ def collect_rosbag_tp(bag_path: str, pose_topic: str, tp_topic: str, scan_topic: progress_bar.update(1) (topic, data, stamp) = reader.read_next() - if skip_count <= 2000: - skip_count += 1 - continue + # Skip some initial messages, as they contains invalid data + # if skip_count <= 2000: + # skip_count += 1 + # continue if topic == pose_topic: pose_msg = deserialize_message(data, PoseWithCovarianceStamped) stamp = pose_msg.header.stamp.sec * 1e9 + pose_msg.header.stamp.nanosec pose_df[pose_df_idx, :] = [stamp, __pose_to_mat(pose_msg.pose.pose)] + pose_to_publish[pose_df_idx, :] = [stamp, pose_msg.pose.pose] pose_df_idx += 1 elif topic == tp_topic: @@ -251,13 +209,9 @@ def collect_rosbag_tp(bag_path: str, pose_topic: str, tp_topic: str, scan_topic: elif topic == scan_topic: pc_msg = deserialize_message(data, sensor_msgs.PointCloud2) stamp = pc_msg.header.stamp.sec * 1e9 + pc_msg.header.stamp.nanosec - - scan_df.enqueue([stamp, pc_msg]) - - if scan_df.is_full(): - scan_df.drop(False, __scan_callback, pose_df, pose_df_idx, tp_df, tp_df_idx, *args) - - # Process the rest of the queue - scan_df.drop(True, __scan_callback, pose_df, pose_df_idx, tp_df, tp_df_idx, *args) + scan_df[scan_df_idx, :] = [stamp, pc_msg] + scan_df_idx += 1 progress_bar.close() + + return {"mat_pose" : pose_df, "pose" : pose_to_publish, "tp" : tp_df, "scan" : scan_df}