diff --git a/dataset_converter/pohang.py b/dataset_converter/pohang.py index c680e0c19c1e98a7f5a0a130a0fd07432f4ccdac..e403c89009a424e62c3a6b0cf7f68a021adbc844 100644 --- a/dataset_converter/pohang.py +++ b/dataset_converter/pohang.py @@ -271,6 +271,42 @@ def process_imu( def process_navigation(data_folder: Path, rosbag_path: Path): data_path = data_folder.joinpath("navigation") + # GPS Part + data = txt_preprocess(data_path.joinpath("gps.txt")) + with rosbag.Bag(rosbag_path, check_bag_mode(rosbag_path)) as bag: + for idx, l in enumerate(data): + l.remove("N") + l.remove("E") + l = list(map(float, l)) + t = rospy.Time.from_sec(l[0]) + + # NavSatFix Part + gps = NavSatFix() + gps.header.seq = idx + gps.header.stamp = t + gps.header.frame_id = NAVIGATION_PROCESS["gps.txt"][0] + gps.latitude = l[2] + gps.longitude = l[3] + gps.altitude = l[-1] + + bag.write(NAVIGATION_PROCESS["gps.txt"][1], gps, t) + + # PohangGPS Part + gps = PohangGPS() + gps.header.seq = idx + gps.header.stamp = t + gps.header.frame_id = NAVIGATION_PROCESS["gps.txt"][0] + gps.gps_time = l[1] + gps.latitude = l[2] + gps.longitude = l[3] + gps.heading = l[4] + gps.gps_quality_indicator = int(l[5]) + gps.num_satellites = int(l[6]) + gps.horizontal_dilution_of_precision = l[7] + gps.geoid = l[8] + + bag.write(NAVIGATION_PROCESS["gps.txt"][1] + "_raw", gps, t) + # AHRS Part data = txt_preprocess(data_path.joinpath("ahrs.txt")) @@ -302,7 +338,7 @@ def process_navigation(data_folder: Path, rosbag_path: Path): with rosbag.Bag(rosbag_path, check_bag_mode(rosbag_path)) as bag: for idx, l in enumerate(data): l = list(map(float, l)) - t = rospy.Time.from_sec(t0 + t_diff * idx) + t = rospy.Time.from_sec(t0 + t_diff * idx) # time calibration imu = Imu() imu.header.seq = idx @@ -324,10 +360,56 @@ def process_navigation(data_folder: Path, rosbag_path: Path): # Baseline Part # frame_id unknown data = txt_preprocess(data_path.joinpath("baseline.txt")) + + # perform time correction + # find the first aligned data point + # all gps time + gps_time_reference = txt_preprocess(data_path.joinpath("gps.txt")) + gps_time_reference = [float(gps[0]) for gps in gps_time_reference] + first_baseline = 0 + first_gps_reference = 0 + + while first_baseline < len(data) and first_gps_reference < len(gps_time_reference): + gps_ref_t = gps_time_reference[first_gps_reference] + + baseline_t = list(map(float, data[first_baseline])) + baseline_t = baseline_t[0] + + if np.abs(gps_ref_t - baseline_t) <= 0.01: + print( + f"Found baseline idx: {first_baseline}, gps idx: {first_gps_reference}" + ) + print(f"Found baseline time: {baseline_t}, gps idx: {gps_ref_t}") + break + + if gps_ref_t > baseline_t: + first_baseline += 1 + else: + first_gps_reference += 1 + + gps_reference_idx = first_gps_reference with rosbag.Bag(rosbag_path, check_bag_mode(rosbag_path)) as bag: - for idx, l in enumerate(data): + for idx, l in enumerate(data[first_baseline:]): l = list(map(float, l)) - t = rospy.Time.from_sec(l[0]) + + baseline_t = l[0] + + # find the first gpt_time >= baseline_t + gps_reference_idx = bisect.bisect_left( + gps_time_reference, baseline_t, lo=gps_reference_idx + ) + if gps_reference_idx >= len(gps_time_reference): + break + gps_ref_t = gps_time_reference[gps_reference_idx] + + # if error is small, we use original time + if np.abs(gps_ref_t - baseline_t) <= 0.01: + t = rospy.Time.from_sec(l[0]) + else: + print( + f"Corrected Time {gps_ref_t}(idx: {gps_reference_idx}) baseline idx {idx}" + ) + t = rospy.Time.from_sec(gps_ref_t) pose = PoseStamped() pose.header.seq = idx @@ -343,42 +425,6 @@ def process_navigation(data_folder: Path, rosbag_path: Path): bag.write(NAVIGATION_PROCESS["baseline.txt"][1], pose, t) - # GPS Part - data = txt_preprocess(data_path.joinpath("gps.txt")) - with rosbag.Bag(rosbag_path, check_bag_mode(rosbag_path)) as bag: - for idx, l in enumerate(data): - l.remove("N") - l.remove("E") - l = list(map(float, l)) - t = rospy.Time.from_sec(l[0]) - - # NavSatFix Part - gps = NavSatFix() - gps.header.seq = idx - gps.header.stamp = t - gps.header.frame_id = NAVIGATION_PROCESS["gps.txt"][0] - gps.latitude = l[2] - gps.longitude = l[3] - gps.altitude = l[-1] - - bag.write(NAVIGATION_PROCESS["gps.txt"][1], gps, t) - - # PohangGPS Part - gps = PohangGPS() - gps.header.seq = idx - gps.header.stamp = t - gps.header.frame_id = NAVIGATION_PROCESS["gps.txt"][0] - gps.gps_time = l[1] - gps.latitude = l[2] - gps.longitude = l[3] - gps.heading = l[4] - gps.gps_quality_indicator = int(l[5]) - gps.num_satellites = int(l[6]) - gps.horizontal_dilution_of_precision = l[7] - gps.geoid = l[8] - - bag.write(NAVIGATION_PROCESS["gps.txt"][1] + "_raw", gps, t) - def process_stereo(data_folder: Path, rosbag_path: Path): data_path = data_folder.joinpath("stereo")