背景：　最近在研究BundleFusion，跑通官方數據集后，就想著制作自己的數據集來運行bundlefusion．KinectV2相機可直接獲取的圖像的分辨率分為三個HD 1920x1080, QHD: 960X540，SD: 512x424.我選擇是中間的分辨率qhd.

錄制了一段時間我去查看，彩色圖和深度圖，發現兩者的幀數不相同，我就很納悶了，還想著用什么方法對兩個進行同步處理，但是我去查看了一下TUM數據集，發現數據集里面的深度圖和彩色圖的幀數也不相同，我恍然大悟，在orb-slam2運行tum數據集時，需要使用TUM數據集中的associate.py腳本，按照時間戳的遠近將深度圖和彩色圖匹配起來，得到associate.txt文件，這個里面是彩色圖時間戳，以時間戳命名的圖像的名字，以及對應這個彩色圖的深度圖的時間戳，和以這個時間戳命名的圖像的名字．這樣在運行代碼的時候就按照這個匹配結果去文件夾下讀取相應的圖像．

可以在我的git上下載完整代碼，然后放到你的catkin_ws/src下然后catkin_make進行編譯

https://github.com/Serena2018/save_color_depth_from_kinect2_with_ros

當然你需要提前配置好libfreenect2和iai-kinect2,然后運行roslaunch kinect2_bridge kinect2_bridge.launch

然后使用 rostopic list　查看當前系統上發布出來的話題．

這樣運行上面下載的代碼就可以訂閱發布出來的彩色圖像和深度圖像的數據．

?

然后我使用了TUM數據集提供的associate.py工具，在保存下來的圖像中找到對應，得到associate.txt文件，然后直接讀取這個文件，去文件目錄下找相應的圖像，并按照bundlefusion需要的圖像的命名格式對圖像進行重命名，將重命名后的圖像保存到新路徑下

## in another script we have find the association of color and depth and now
## in this script we will pick the correspondents of color and depth images
## according their timestamps then rename them according the standards of
##  bundlefusion dataset and then store them in separate folders.## step one: read the assoc.txt into this project.import sys
import os
import shutildef read_file_list(filename):"""Reads a trajectory from a text file.File format:The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp.Input:filename -- File nameOutput:dict -- dictionary of (stamp,data) tuples"""file = open(filename)data = file.read()lines = data.replace(",", " ").replace("\t", " ").split("\n")#strip() function can be used to remove space located in the head or the tail places of a string# v.strip('0')remove 0# list = [[v.strip() for v in line.split(" ") if v.strip() != ""] for line in lines if#         len(line) > 0 and line[0] != "#"]# list = [(float(l[0]), l[1:]) for l in list if len(l) > 1]list= []listResult = []for line in lines:tmpList = []if len(line) > 0 and line[0] != "#":for v in line.split(" "):if v.strip() != "":tmpList.append(v.strip())list.append(tmpList)for l in list:if len(l) > 1:listResult.append((float(l[0]), l[1:]))return dict(listResult)def associate(first_list, second_list, offset, max_difference):"""Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aimto find the closest match for every input tuple.Input:first_list -- first dictionary of (stamp,data) tuplessecond_list -- second dictionary of (stamp,data) tuplesoffset -- time offset between both dictionaries (e.g., to model the delay between the sensors)max_difference -- search radius for candidate generationOutput:matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))"""## obatin all keysfirst_keys = list(first_list)second_keys = list(second_list)potential_matches = [(abs(a - (b + offset)), a, b)for a in first_keysfor b in second_keysif abs(a - (b + offset)) < max_difference]potential_matches.sort()matches = []for diff, a, b in potential_matches:if a in first_keys and b in second_keys:first_keys.remove(a)second_keys.remove(b)matches.append((a, b))matches.sort()return matchesdef read_data_from_file(file_path):associ_file = open(file_path)data = associ_file.read()lines = data.replace(',', ' ').replace('\t', ' ').split('\n')print(f"read {len(lines)} from {file_path}.")# come here we have obtain every line of this associ.txt and in each line we can index arbitrary part of it# we mainly work on the first and the third part and then to find the correspondent color and depth image,# and the rename them and store them.list = [[v.strip() for v in line.split(" ") if v.strip() != " "] for line in lines if len(line) > 0 and line[0] != "#"]return listdef load_images_rename_store(file_data, image_folder, output_folder):print(f"The length of file is {len(file_data)}.")print(f"the path of image_folder is {image_folder}.")print(f"the path of output_folder is {output_folder}.")rgb_path = image_folder + "/rgb/"depth_path = image_folder + "/depth/"print("**********************")rgbs = os.listdir(rgb_path)# print(len(rgbs))# for rgb in sorted(rgbs):#     print(rgb)print("**********************")depths = os.listdir(depth_path)# print(len(depths))# for depth in sorted(depths):#     print(depth)couples = []print("............................")for data in file_data:couple_list = []##I must search the frame name in the folder by the exactly name from the filergb_name = data[1].split("/")[1]depth_name = data[3].split("/")[1]couple_list.append(rgb_name)couple_list.append(depth_name)couples.append(couple_list)print(f"length of couples is {len(couples)}.")# for couple in sorted(couples):#     print(couple)frame_id = 0for couple in sorted(couples):if couple[0] in sorted(rgbs) and couple[1] in sorted(depths):print(f"\nframe_id is {frame_id}.\n")print(rgb_path + "/" + couple[0])print(output_folder + "/rgb/" + "frame-" + str(frame_id).zfill(6) + ".color.png")print("\n")print(depth_path + "/" + couple[1])print(output_folder + "/depth/" + "frame-" + str(frame_id).zfill(6) + ".depth.png")shutil.copyfile(rgb_path + "/" + couple[0], output_folder + "/rgb/" + "frame-" + str(frame_id).zfill(6) + ".color.png")shutil.copyfile(depth_path + "/" + couple[1], output_folder + "/depth/" + "frame-" + str(frame_id).zfill(6) + ".depth.png")frame_id = frame_id + 1## argvs: file_path, images_path, output_path
if __name__ == '__main__':if len(sys.argv) == 8:path_rgb_file = sys.argv[1]path_depth_file = sys.argv[2]associate_file_name = sys.argv[3]offset = sys.argv[4]max_diff = sys.argv[5]image_path = sys.argv[6]output_path = sys.argv[7]first_list = read_file_list(path_rgb_file)second_list = read_file_list(path_depth_file)matches = associate(first_list, second_list, float(offset), float(max_diff))filename = image_path + associate_file_name# when you use with open you don't need to use close.with open(filename, 'w') as fp:for a, b in matches:fp.write("%f %s %f %s" % (a, " ".join(first_list[a]), b - float(offset), " ".join(second_list[b])) + '\n')# fp = open(filename, 'w')# for a, b in matches:#     fp.write(#         "%f %s %f %s" % (a, " ".join(first_list[a]), b - float(offset), " ".join(second_list[b])) + '\n')#     print("%f %s %f %s" % (a, " ".join(first_list[a]), b - float(offset), " ".join(second_list[b])))# fp.close()file_path = image_path + associate_file_namelist = read_data_from_file(file_path)load_images_rename_store(list, image_path, output_path)else:print("usage: path_rgb.txt path_depth.txt offset(0 default) max_diff(0.02 default), images_path, output_path")

?