python 匹配linux时间戳

白嫖璜老板系列，璜老板牛逼
时间戳匹配的详细注释我有在python 用pose来拼接点云里写过

import argparse
import os
from tqdm import tqdm

parser = argparse.ArgumentParser()
parser.add_argument("--left_pcd_dir", type=str, default="pandar40L")
parser.add_argument("--right_pcd_dir", type=str, default="pandar40R")
parser.add_argument("--back_left_dir", type=str, default="robosenseM")
parser.add_argument("--max_time_diff", type=int, default=100000000)  # 100ms, 这里是按照100ms的阈值去匹配的 
args = parser.parse_args()

def read_timestamps(base_dir, separtor ,suffix):
    names = [f for f in os.listdir(base_dir) if f.endswith(suffix)]
    names = [x[:-len(suffix)].split(separtor) for x in names]
    return [(int(secs), int(nsecs)) for secs, nsecs in names]

def calc_diff(ts1, ts2):
    return (ts1[0] - ts2[0]) * 1000000000 + (ts1[1] - ts2[1])

def generate_pcd_file_name(timestamp):
    return str(timestamp[0]) + '_' + str(timestamp[1]) + '.pcd'

def matche_n_pcds(left_pcd_dir, right_pcd_dir, back_left_dir, max_time_diff):
    left_pcd_timestamps = sorted(read_timestamps(left_pcd_dir, '_' ,'.pcd'))
    right_pcd_timestamps = sorted(read_timestamps(right_pcd_dir,'_' ,'.pcd'))
    back_left_timestamps = sorted(read_timestamps(back_left_dir, '_' , '.pcd'))
    matched = []
    p = 0
    q = 0
    r = 0

    for i, ts in enumerate(left_pcd_timestamps):
        ################### left_pcd-right_pcd match start ###################
        while p < len(right_pcd_timestamps) and right_pcd_timestamps[p] < ts:
            p += 1
        closest_before = p - 1 if p > 0 else None
        closest_after = p if p < len(right_pcd_timestamps) else None

        if closest_before is None and closest_after is None:
            continue
        
        closest_diff = None
        closest = None
        
        if closest_before is not None:
            time_diff = calc_diff(ts, right_pcd_timestamps[closest_before])
            if time_diff < max_time_diff:
                closest_diff = time_diff
                closest = closest_before
        
        if closest_after is not None:
            time_diff = calc_diff(right_pcd_timestamps[closest_after], ts)
            if time_diff < max_time_diff and (closest_diff is None or time_diff < closest_diff):
                closest_diff = time_diff
                closest = closest_after

        if closest is None:
            continue
        
        right_pcd_matched = [right_pcd_timestamps[closest][0], right_pcd_timestamps[closest][1]]

        ################### left_pcd-back_left_pcd match start ###################
        while q < len(back_left_timestamps) and back_left_timestamps[q] < ts:
            q += 1
        closest_before = q - 1 if q > 0 else None
        closest_after = q if q < len(back_left_timestamps) else None

        if closest_before is None and closest_after is None:
            continue
        
        closest_diff = None
        closest = None
        
        if closest_before is not None:
            time_diff = calc_diff(ts, back_left_timestamps[closest_before])
            if time_diff < max_time_diff:
                closest_diff = time_diff
                closest = closest_before
        
        if closest_after is not None:
            time_diff = calc_diff(back_left_timestamps[closest_after], ts)
            if time_diff < max_time_diff and (closest_diff is None or time_diff < closest_diff):
                closest_diff = time_diff
                closest = closest_after

        if closest is None:
            continue

        back_left_matched = [back_left_timestamps[closest][0], back_left_timestamps[closest][1]]

        ################### End of match ###################
        matched.append([generate_pcd_file_name(ts), generate_pcd_file_name(right_pcd_matched), 
                    generate_pcd_file_name(back_left_matched)])

    return matched
    
def main():
    matched_pcd = matche_n_pcds(args.left_pcd_dir, args.right_pcd_dir, 
                                args.back_left_dir, args.max_time_diff)

    for i in tqdm(range(len(matched_pcd))):
        #输出匹配好的路径
        print(os.path.join(args.left_pcd_dir,matched_pcd[i][0]), \
                        os.path.join(args.right_pcd_dir, matched_pcd[i][1]), \
                        os.path.join(args.back_left_dir, matched_pcd[i][2]))

if __name__ == '__main__':
    main()

正常linux时间戳：

import os 
import argparse
     
parser = argparse.ArgumentParser()
parser.add_argument("--pcd_dir", type=str, default="")
parser.add_argument("--image_dir", type=str, default="")
parser.add_argument("--max_time_diff", type=int, default=100000000)  # 10ms
args = parser.parse_args()

def read_timestamps(base_dir, separtor ,suffix):
    names = [f for f in os.listdir(base_dir) if f.endswith(suffix)]
    ts = [x[:-len(suffix)].split(separtor) for x in names]
    return ts

def calc_diff(ts1, ts2):
    ts_a = int(ts1[0]) * 1000000000 + int(ts1[1])
    ts_b = int(ts2[0]) * 1000000000 + int(ts2[1])
    return ts_a - ts_b

def generate_file_name(timestamp, suffix):
    return str(timestamp[0]) + '.' + str(timestamp[1]) + suffix

def match_n_ts(master, slave1, max_time_diff):
    master_ts_list = sorted(read_timestamps(master, '.' ,'.pcd'))
    slave1_ts_list = sorted(read_timestamps(slave1, '.' ,'.png'))
    matched = []
    slave1_idx = 0
    for ts in master_ts_list:
        ################### slave1 match start ###################
        while slave1_idx < len(slave1_ts_list) and calc_diff(slave1_ts_list[slave1_idx], ts) < 0:
            slave1_idx += 1
        closest_before = slave1_idx - 1 if slave1_idx > 0 else None
        closest_after = slave1_idx if slave1_idx < len(slave1_ts_list) else None

        if closest_before is None and closest_after is None:
            continue
        
        closest_diff = None
        closest = None
        
        if closest_before is not None:
            time_diff = calc_diff(ts, slave1_ts_list[closest_before])
            if time_diff < max_time_diff:
                closest_diff = time_diff
                closest = closest_before
        
        if closest_after is not None:
            time_diff = calc_diff(slave1_ts_list[closest_after], ts)
            if time_diff < max_time_diff and (closest_diff is None or time_diff < closest_diff):
                closest_diff = time_diff
                closest = closest_after

        if closest is None:
            continue
        
        slave1_ts_matched = [slave1_ts_list[closest][0], slave1_ts_list[closest][1]]

        ################### End of match ###################
        matched.append([generate_file_name(ts, '.pcd'), generate_file_name(slave1_ts_matched, '.png')])
    return matched
    
def main():
    match_ts_result = match_n_ts(args.pcd_dir, args.image_dir, args.max_time_diff)
    match_path_result = []
    for i in range(len(match_ts_result)):
        match_path_result.append([os.path.join(args.pcd_dir,match_ts_result[i][0]), \
            os.path.join(args.image_dir, match_ts_result[i][1])])

comments