From 66541c6d329dd263ed1ae40fc6b07cc36f8e102d Mon Sep 17 00:00:00 2001 From: slz Date: Wed, 5 Aug 2026 15:12:16 +0800 Subject: [PATCH] fix: normalize line endings to LF (#118) --- .cmake-format.py | 206 ++-- .gitattributes | 15 + .../multicamera_sync/Python/Config.ini | 16 +- .../multicamera_sync/Python/README.md | 12 +- .../multicamera_sync/Python/SyncFramesMain.py | 180 +-- .../script/config_frameMatch-Astra2_new.py | 784 ++++++------- .../config_frameMatch-systemTimestamp.py | 806 ++++++------- .../Python/script/config_frameMatch.py | 850 +++++++------- .../script/config_frameMatch_Gemini2VL.py | 820 ++++++------- .../Python/script/config_frameMatch_g330.py | 852 +++++++------- orbbec_camera_msgs/msg/DeviceInfo.msg | 14 +- orbbec_camera_msgs/srv/GetCameraInfo.srv | 4 +- orbbec_camera_msgs/srv/GetDeviceInfo.srv | 6 +- orbbec_description/urdf/astra_2.urdf.xacro | 832 ++++++------- orbbec_description/urdf/gemini_2.urdf.xacro | 938 +++++++-------- orbbec_description/urdf/gemini_2_L.urdf.xacro | 1044 ++++++++--------- .../urdf/gemini_335_L_336_L.urdf.xacro | 1044 ++++++++--------- 17 files changed, 4219 insertions(+), 4204 deletions(-) diff --git a/.cmake-format.py b/.cmake-format.py index 14e300d5..8b8f33c5 100644 --- a/.cmake-format.py +++ b/.cmake-format.py @@ -1,104 +1,104 @@ -# ----------------------------- -# Options affecting formatting. -# ----------------------------- -with section("format"): - - # Disable formatting entirely, making cmake-format a no-op - disable = False - - # How wide to allow formatted cmake files - line_width = 120 - - # How many spaces to tab for indent - tab_size = 4 - - # If true, lines are indented using tab characters (utf-8 0x09) instead of - # space characters (utf-8 0x20). In cases where the layout would - # require a fractional tab character, the behavior of the fractional - # indentation is governed by - use_tabchars = False - - # If is True, then the value of this variable indicates how - # fractional indentions are handled during whitespace replacement. If set to - # 'use-space', fractional indentation is left as spaces (utf-8 0x20). If set - # to `round-up` fractional indentation is replaced with a single tab character - # (utf-8 0x09) effectively shifting the column to the next tabstop - fractional_tab_policy = 'use-space' - - # If an argument group contains more than this many sub-groups (parg or kwarg - # groups) then force it to a vertical layout. - max_subgroups_hwrap = 4 - - # If a positional argument group contains more than this many arguments, then - # force it to a vertical layout. - max_pargs_hwrap = 6 - - # If a cmdline positional group consumes more than this many lines without - # nesting, then invalidate the layout (and nest) - max_rows_cmdline = 2 - - # If true, separate flow control names from their parentheses with a space - separate_ctrl_name_with_space = False - - # If true, separate function names from parentheses with a space - separate_fn_name_with_space = False - - # If a statement is wrapped to more than one line, than dangle the closing - # parenthesis on its own line. - dangle_parens = True - - # If the trailing parenthesis must be 'dangled' on its on line, then align it - # to this reference: `prefix`: the start of the statement, `prefix-indent`: - # the start of the statement, plus one indentation level, `child`: align to - # the column of the arguments - dangle_align = 'prefix' - - # If the statement spelling length (including space and parenthesis) is - # smaller than this amount, then force reject nested layouts. - min_prefix_chars = 4 - - # If the statement spelling length (including space and parenthesis) is larger - # than the tab width by more than this amount, then force reject un-nested - # layouts. - max_prefix_chars = 10 - - # If a candidate layout is wrapped horizontally but it exceeds this many - # lines, then reject the layout. - max_lines_hwrap = 2 - - # What style line endings to use in the output. - line_ending = 'unix' - - # Format command names consistently as 'lower' or 'upper' case - command_case = 'canonical' - - # Format keywords consistently as 'lower' or 'upper' case - keyword_case = 'unchanged' - - # A list of command names which should always be wrapped - always_wrap = [] - - # If true, the argument lists which are known to be sortable will be sorted - # lexicographicall - enable_sort = True - - # If true, the parsers may infer whether or not an argument list is sortable - # (without annotation). - autosort = False - - # By default, if cmake-format cannot successfully fit everything into the - # desired linewidth it will apply the last, most agressive attempt that it - # made. If this flag is True, however, cmake-format will print error, exit - # with non-zero status code, and write-out nothing - require_valid_layout = False - - # A dictionary mapping layout nodes to a list of wrap decisions. See the - # documentation for more information. - layout_passes = {} - -# ----------------------------- -# Options affecting comment formatting (indirectly). -# ----------------------------- -with section("markup"): - # Disabling markup avoids merging consecutive comment lines +# ----------------------------- +# Options affecting formatting. +# ----------------------------- +with section("format"): + + # Disable formatting entirely, making cmake-format a no-op + disable = False + + # How wide to allow formatted cmake files + line_width = 120 + + # How many spaces to tab for indent + tab_size = 4 + + # If true, lines are indented using tab characters (utf-8 0x09) instead of + # space characters (utf-8 0x20). In cases where the layout would + # require a fractional tab character, the behavior of the fractional + # indentation is governed by + use_tabchars = False + + # If is True, then the value of this variable indicates how + # fractional indentions are handled during whitespace replacement. If set to + # 'use-space', fractional indentation is left as spaces (utf-8 0x20). If set + # to `round-up` fractional indentation is replaced with a single tab character + # (utf-8 0x09) effectively shifting the column to the next tabstop + fractional_tab_policy = 'use-space' + + # If an argument group contains more than this many sub-groups (parg or kwarg + # groups) then force it to a vertical layout. + max_subgroups_hwrap = 4 + + # If a positional argument group contains more than this many arguments, then + # force it to a vertical layout. + max_pargs_hwrap = 6 + + # If a cmdline positional group consumes more than this many lines without + # nesting, then invalidate the layout (and nest) + max_rows_cmdline = 2 + + # If true, separate flow control names from their parentheses with a space + separate_ctrl_name_with_space = False + + # If true, separate function names from parentheses with a space + separate_fn_name_with_space = False + + # If a statement is wrapped to more than one line, than dangle the closing + # parenthesis on its own line. + dangle_parens = True + + # If the trailing parenthesis must be 'dangled' on its on line, then align it + # to this reference: `prefix`: the start of the statement, `prefix-indent`: + # the start of the statement, plus one indentation level, `child`: align to + # the column of the arguments + dangle_align = 'prefix' + + # If the statement spelling length (including space and parenthesis) is + # smaller than this amount, then force reject nested layouts. + min_prefix_chars = 4 + + # If the statement spelling length (including space and parenthesis) is larger + # than the tab width by more than this amount, then force reject un-nested + # layouts. + max_prefix_chars = 10 + + # If a candidate layout is wrapped horizontally but it exceeds this many + # lines, then reject the layout. + max_lines_hwrap = 2 + + # What style line endings to use in the output. + line_ending = 'unix' + + # Format command names consistently as 'lower' or 'upper' case + command_case = 'canonical' + + # Format keywords consistently as 'lower' or 'upper' case + keyword_case = 'unchanged' + + # A list of command names which should always be wrapped + always_wrap = [] + + # If true, the argument lists which are known to be sortable will be sorted + # lexicographicall + enable_sort = True + + # If true, the parsers may infer whether or not an argument list is sortable + # (without annotation). + autosort = False + + # By default, if cmake-format cannot successfully fit everything into the + # desired linewidth it will apply the last, most agressive attempt that it + # made. If this flag is True, however, cmake-format will print error, exit + # with non-zero status code, and write-out nothing + require_valid_layout = False + + # A dictionary mapping layout nodes to a list of wrap decisions. See the + # documentation for more information. + layout_passes = {} + +# ----------------------------- +# Options affecting comment formatting (indirectly). +# ----------------------------- +with section("markup"): + # Disabling markup avoids merging consecutive comment lines enable_markup = False \ No newline at end of file diff --git a/.gitattributes b/.gitattributes index e69de29b..923a704f 100644 --- a/.gitattributes +++ b/.gitattributes @@ -0,0 +1,15 @@ +# Normalize text files to LF in both the repository and working tree. +* text=auto eol=lf + +# Treat generated assets and bundled binaries as binary files. +*.stl binary +*.STL binary +*.so binary +*.so.* binary +*.png binary +*.jpg binary +*.jpeg binary +*.gif binary +*.pdf binary +*.zip binary +*.gz binary diff --git a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/Config.ini b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/Config.ini index fcfc7475..ff67167e 100644 --- a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/Config.ini +++ b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/Config.ini @@ -1,8 +1,8 @@ -[Parameter] -# Unit: FPS. Please fill in according to the actual dataset frame rate, -# otherwise the matching will be inaccurate. -frameRate=30 -# Unit: ms. When the timestamp range difference of a matched frame group -# is greater than or equal to tspRangeThreshold, the file name will be marked. -tspRangeThreshold=4 - +[Parameter] +# Unit: FPS. Please fill in according to the actual dataset frame rate, +# otherwise the matching will be inaccurate. +frameRate=30 +# Unit: ms. When the timestamp range difference of a matched frame group +# is greater than or equal to tspRangeThreshold, the file name will be marked. +tspRangeThreshold=4 + diff --git a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/README.md b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/README.md index e0805178..b405b24b 100644 --- a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/README.md +++ b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/README.md @@ -1,7 +1,7 @@ -## config_frameMatch.py - -**Definition of the three folders:** - -- **matchFrames**: Data constrained within a half-frame. -- **notMatchFrames**: Data not constrained within a half-frame, unmatched. +## config_frameMatch.py + +**Definition of the three folders:** + +- **matchFrames**: Data constrained within a half-frame. +- **notMatchFrames**: Data not constrained within a half-frame, unmatched. - **abnormal**: Data within the group that exceeds the `tspRangeThreshold` setting. \ No newline at end of file diff --git a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/SyncFramesMain.py b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/SyncFramesMain.py index e85cb04a..e7e8caa9 100644 --- a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/SyncFramesMain.py +++ b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/SyncFramesMain.py @@ -1,90 +1,90 @@ -import os -import subprocess -import json -import platform - -def list_subdirectories(path): - for f in os.listdir(path): - if os.path.isdir(os.path.join(path, f)): - yield f - -def read_pid_from_device_json(frames_dir): - file_path = frames_dir + "/DevicesInfo.txt" - if not os.path.exists(file_path): - print(file_path + " not exists. Please check it") - return "" - - with open(file_path, 'r') as f: - data = json.load(f) - - if not 'devicePid' in data: - print("Not found 'devicePid' in " + file_path) - return "" - - return data['devicePid'] - -def execute_sync_width_pid(frames_dir, pid): - sync_script_file="" - if pid.lower() == "0x0675": - sync_script_file="script/config_frameMatch_Gemini2VL.py" - elif pid.lower() == "0x0670" or pid.lower() == "0x0701": - sync_script_file="script/config_frameMatch-systemTimestamp.py" - elif pid.lower() == "0x0660": - sync_script_file="script/config_frameMatch-Astra2_new.py" - elif pid.lower() == "0x0800" or pid.lower() == "0x0804" or pid.lower() == "0x0803" or pid.lower() == "0x0807" or pid.lower() == "0x0801" or pid.lower() == "0x0805" or pid.lower() == "0x080b" or pid.lower() == "0x080e" or pid.lower() == "0x080f" or pid.lower() == "0x0A13": - sync_script_file="script/config_frameMatch_g330.py" - else: - sync_script_file="script/config_frameMatch.py" - - if len(sync_script_file) > 0: - print(f"execute synchronize frames. script_file={sync_script_file}, frames_dir={frames_dir}") - if platform.system() == "Windows": - output = subprocess.check_output(["python", sync_script_file, os.path.abspath(f"{frames_dir}"), os.getcwd()]) - print(output.decode("gbk")) - else: - output = subprocess.check_output(["python3", sync_script_file, os.path.abspath(f"{frames_dir}"), os.getcwd()]) - print(output.decode("utf-8")) - else: - print("Invalid pid=" + pid + ", not match sync frame script") - -def sync_frames(frames_dir): - pid = read_pid_from_device_json(frames_dir) - if len(pid) <= 0: - print("Get pid failed.") - return - - execute_sync_width_pid(frames_dir, pid) - -def main(): - frames_output_path = os.path.abspath("../output") - - subdirectories = list(list_subdirectories(frames_output_path)) - - if not subdirectories: - print("No subdirectories found in the specified directory.") - - for i, subdir in enumerate(subdirectories): - print(f"{i + 1}. {subdir}") - - choice = input("Please select a subdirectory by number (or 'q' to quit): ") - - if choice.lower() == 'q': - return - - is_index_valid = False - try: - index = int(choice) - 1 - if 0 <= index < len(subdirectories): - is_index_valid = True - else: - print(f"Invalid choice. index={index}. Please enter a number between 1 and the total number of subdirectories.") - except ValueError: - print("Invalid choice(ValueException). Please enter a number between 1 and the total number of subdirectories") - if is_index_valid: - selected_subdir = subdirectories[index] - print(f"You selected the subdirectory: {selected_subdir}") - sync_frames(f"{frames_output_path }/{selected_subdir}") - - -if __name__ == "__main__": - main() +import os +import subprocess +import json +import platform + +def list_subdirectories(path): + for f in os.listdir(path): + if os.path.isdir(os.path.join(path, f)): + yield f + +def read_pid_from_device_json(frames_dir): + file_path = frames_dir + "/DevicesInfo.txt" + if not os.path.exists(file_path): + print(file_path + " not exists. Please check it") + return "" + + with open(file_path, 'r') as f: + data = json.load(f) + + if not 'devicePid' in data: + print("Not found 'devicePid' in " + file_path) + return "" + + return data['devicePid'] + +def execute_sync_width_pid(frames_dir, pid): + sync_script_file="" + if pid.lower() == "0x0675": + sync_script_file="script/config_frameMatch_Gemini2VL.py" + elif pid.lower() == "0x0670" or pid.lower() == "0x0701": + sync_script_file="script/config_frameMatch-systemTimestamp.py" + elif pid.lower() == "0x0660": + sync_script_file="script/config_frameMatch-Astra2_new.py" + elif pid.lower() == "0x0800" or pid.lower() == "0x0804" or pid.lower() == "0x0803" or pid.lower() == "0x0807" or pid.lower() == "0x0801" or pid.lower() == "0x0805" or pid.lower() == "0x080b" or pid.lower() == "0x080e" or pid.lower() == "0x080f" or pid.lower() == "0x0A13": + sync_script_file="script/config_frameMatch_g330.py" + else: + sync_script_file="script/config_frameMatch.py" + + if len(sync_script_file) > 0: + print(f"execute synchronize frames. script_file={sync_script_file}, frames_dir={frames_dir}") + if platform.system() == "Windows": + output = subprocess.check_output(["python", sync_script_file, os.path.abspath(f"{frames_dir}"), os.getcwd()]) + print(output.decode("gbk")) + else: + output = subprocess.check_output(["python3", sync_script_file, os.path.abspath(f"{frames_dir}"), os.getcwd()]) + print(output.decode("utf-8")) + else: + print("Invalid pid=" + pid + ", not match sync frame script") + +def sync_frames(frames_dir): + pid = read_pid_from_device_json(frames_dir) + if len(pid) <= 0: + print("Get pid failed.") + return + + execute_sync_width_pid(frames_dir, pid) + +def main(): + frames_output_path = os.path.abspath("../output") + + subdirectories = list(list_subdirectories(frames_output_path)) + + if not subdirectories: + print("No subdirectories found in the specified directory.") + + for i, subdir in enumerate(subdirectories): + print(f"{i + 1}. {subdir}") + + choice = input("Please select a subdirectory by number (or 'q' to quit): ") + + if choice.lower() == 'q': + return + + is_index_valid = False + try: + index = int(choice) - 1 + if 0 <= index < len(subdirectories): + is_index_valid = True + else: + print(f"Invalid choice. index={index}. Please enter a number between 1 and the total number of subdirectories.") + except ValueError: + print("Invalid choice(ValueException). Please enter a number between 1 and the total number of subdirectories") + if is_index_valid: + selected_subdir = subdirectories[index] + print(f"You selected the subdirectory: {selected_subdir}") + sync_frames(f"{frames_output_path }/{selected_subdir}") + + +if __name__ == "__main__": + main() diff --git a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch-Astra2_new.py b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch-Astra2_new.py index b6551274..8d83851d 100644 --- a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch-Astra2_new.py +++ b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch-Astra2_new.py @@ -1,392 +1,392 @@ -import os.path -import argparse -import sys -import json -import shutil -import configparser - -from pathlib import Path -from datetime import datetime - -## Config.ini configuration -# Unit: FPS. Please fill in according to the actual dataset frame rate, -# otherwise it will cause inaccurate matching. -frameRate = -1 -# Unit: ms. When the timestamp range difference of a matched frame group -# is greater than or equal to tspRangeThreshold, the file name will be marked. -tspRangeThreshold = -1 - -## Parsed automatically by Python script, DO NOT modify -primaryId = '-1' # Host ID -mainPythonWorkPath = "" # Working directory of SyncFramesMain.py -sourceFramesPath = "" # DO NOT modify, fixed value, path to TotalMode directory -sourceRootPath = "" # Root directory containing TotalMode -resultPath = "" # Path to save sync analysis results -streamProfileDict = dict() # Store Python results - - -class PictureInfo(object): - class Struct(object): - def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): - self.deviceId = deviceId - self.sensorType = sensorType - self.syncTimeStamp = syncTimeStamp - self.picturePath = picturePath - self.deviceIdFull = deviceIdFull - self.fileExt = fileExt - - def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): - return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt) - - -class StreamProfileInfo: - def __init__(self, sensorType, width, height, format, fps): - self.sensorType = sensorType - self.width = width - self.height = height - self.format = format - self.fps = fps - - -def initPrimaryId(): - global primaryId - - deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" - if not os.path.exists(deviceInfoPath): - print(f"Initialize primaryId failed. {deviceInfoPath} not exists") - return False - - with open(deviceInfoPath, 'r') as f: - data = json.load(f) - - if not 'devices' in data: - print(f"Initialize primaryId failed. {deviceInfoPath} not contain item 'devices'") - return False - - for item in data['devices']: - if 'isPrimaryDevice' in item and 'index' in item and item['isPrimaryDevice']: - primaryId = str(item['index']) - return True - - return False - - -def initSyncConfigParams(): - global frameRate, tspRangeThreshold - - configPath = f"{mainPythonWorkPath}/Config.ini" - if not os.path.exists(configPath): - print(f"Initliaze synchronized config parameter failed. {configPath} not exists.") - return False - - # Create ConfigParser object - config = configparser.ConfigParser() - - # Read ini file - config.read(configPath, 'utf-8') - - if not config.has_option('Parameter', 'frameRate'): - print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}") - return False - - if not config.has_option('Parameter', 'tspRangeThreshold'): - print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}") - return False - - frameRate = config.getfloat('Parameter', 'frameRate') - if frameRate <= 0 or frameRate >= 1000: - print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}") - return False - - tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold') - if tspRangeThreshold <= 0: - print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}") - return False - - return True - - -def getDeviceCount(): - deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" - if not os.path.exists(deviceInfoPath): - print(f"Get device count failed. {deviceInfoPath} not exists") - return 0 - - with open(deviceInfoPath, 'r') as f: - data = json.load(f) - - if not 'devices' in data: - print(f"Get device count failed. {deviceInfoPath} not contain item 'devices'") - return 0 - - return len(data['devices']) - - -def initProfileInfoDict(): - global streamProfileDict - - streamProfileDict = dict() - profilePath = f"{sourceRootPath}/StreamProfileInfo.txt" - if not os.path.exists(profilePath): - print(f"Initialize profile information dictionary failed. {profilePath} not exists") - return False - - with open(profilePath, 'r') as f: - data = json.load(f) - - if not 'streamProfiles' in data: - print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'") - return False - - for item in data['streamProfiles']: - if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item: - profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps']) - streamProfileDict.setdefault(item['sensorType'], profileInfo) - else: - print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}") - - return len(streamProfileDict.items()) > 0 - - -def initResultDir(): - global resultPath - timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S") - resultPath = f"{sourceRootPath}/results-{timeText}" - if not os.path.exists(resultPath): - os.makedirs(resultPath) - - -def isFrameFile(fileName): - fileExt = os.path.splitext(fileName) - if len(fileExt) < 2: - return False - - frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"} - return fileExt[1] in frameExts - - -def initPictureInfoDictionary(rootFilePath, pictureInfoDict): - frameFileCount = 0 - for dirpath, dirnames, filenames in os.walk(rootFilePath): - # print(f"dirpath={dirpath}, dirpath.basename=" + os.path.basename(dirpath)) - for fileName in filenames: - filePath = os.path.join(dirpath, fileName) - if not isFrameFile(fileName): - continue - - frameFileCount += 1 - fileNameNoExt = os.path.splitext(fileName)[0] - pictureNameList = fileNameNoExt.split('_') - deviceId = pictureNameList[2][5:] - sensorType = pictureNameList[0].replace('#', '_') - syncTimeStamp = int(pictureNameList[3][1:]) - - # Create struct - pictureInfo = PictureInfo() - info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1]) - - # Format dictionary: one key corresponds to one array - pictureInfoDict.setdefault(deviceId, []).append(info) - - if 0 == frameFileCount: - print("initialize pictureInfoDict failed. Not found frame file") - - -def matchFrame(pictureInfoDict): - global primaryId, frameRate - global streamProfileDict - - # Get comparison image array, take the Color array of the first device for comparison - compareList = [] - for key in pictureInfoDict: - listTmp = list(pictureInfoDict[key]) - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == 'color': - compareList.append(info) - - sorted_dict = dict(sorted(pictureInfoDict.items(), key=lambda x: x[1], reverse=False)) - pictureInfoDict = sorted_dict - - # Dynamically analyze based on stream opening status to reduce loop iterations - hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict - hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict - hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict - - # Match adjacent frames - resultDict = {} - consumedList = [] - dictIndex = 0 - frameTspGap = int(1000.0/frameRate+0.5) - for comparePic in compareList: - resultDict.setdefault(dictIndex, []).append(comparePic) - for key in pictureInfoDict: - listTmp = list(pictureInfoDict[key]) - # Compare Color - if hasColorProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'color' and info.syncTimeStamp >= comparePic.syncTimeStamp and info.syncTimeStamp - comparePic.syncTimeStamp <= frameTspGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare Depth - if hasDepthProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'depth' and info.syncTimeStamp >= comparePic.syncTimeStamp and info.syncTimeStamp - comparePic.syncTimeStamp <= frameTspGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare IR - if hasIRProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir' and info.syncTimeStamp >= comparePic.syncTimeStamp and info.syncTimeStamp - comparePic.syncTimeStamp <= frameTspGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Calculate deviation - for info in resultDict[dictIndex]: - info.tspDiff = info.syncTimeStamp - comparePic.syncTimeStamp - - dictIndex += 1 - - return resultDict - - -def safe_make_dir(path): - if not os.path.exists(path): - os.makedirs(path) - - -def handleSyncFrames(): - global resultPath - global sourceFramesPath - global streamProfileDict - - fp = open(f"{resultPath}/frameMatchLog.txt", "w") - - if not initProfileInfoDict(): - print("Initialize stream profile dictionary failed. exit") - return - print(f"Stream profiles: {streamProfileDict}") - streamProfileCount = len(streamProfileDict.items()) - - pictureInfoDict = {} - - initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict) - if len(pictureInfoDict) <= 0: - print("init pictureInfoDict failed. pictureInfoDict.len = 0") - return - - # # Dump pictureInfoDict - # for key in pictureInfoDict: - # print("===================key:%s" % key) - # listTmp = list(pictureInfoDict[key]) - # for info in listTmp: - # print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath)) - - deviceCount = getDeviceCount() - print("=================device count:%d" % deviceCount) - - resultDict = {} - resultDict = matchFrame(pictureInfoDict) - - for key in resultDict: - fp.write("===================result key:%s\n" % key) - print("===================result key:%s" % key) - listTmp = list(resultDict[key]) - for info in listTmp: - fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % ( - info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) - print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % ( - info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) - - matchPath = f"{resultPath}/matchFrames/" - safe_make_dir(matchPath) - notMatchPath = f"{resultPath}/notMatchFrames/" - abnormalPath = f"{resultPath}/abnormal/" - - for key in resultDict: - listTmp = list(resultDict[key]) - if (len(listTmp) == (deviceCount * streamProfileCount)): - haveAbnormalData = False - for info in listTmp: - if info.tspDiff >= tspRangeThreshold: - path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt - shutil.copy(info.picturePath, path) - haveAbnormalData = True - else: - path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt - shutil.copy(info.picturePath, path) - - if haveAbnormalData: - safe_make_dir(abnormalPath) - for info in listTmp: - if info.tspDiff >= tspRangeThreshold: - path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt - shutil.copy(info.picturePath, path) - else: - path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt - shutil.copy(info.picturePath, path) - else: - safe_make_dir(notMatchPath) - for info in listTmp: - newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt - shutil.copy(info.picturePath, newFilePath) - - -def main(): - if not initPrimaryId(): - print("Initialize primaryId failed. exit.") - return - - if not initSyncConfigParams(): - print("Initialized Sync config parameter failed. exit.") - return - - initResultDir() - - print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}") - handleSyncFrames() - - return - - -if __name__ == '__main__': - parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Astra2'") - parser.add_argument('frames_dir', type=str, help="Frames directory") - parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory") - args = parser.parse_args() - - sourceRootPath = args.frames_dir - sourceFramesPath = f"{args.frames_dir}/TotalModeFrames" - mainPythonWorkPath = args.main_script_root_dir - print(f"Start of Astra2 synchronize frame. sourceRootPath={sourceRootPath}") - - main() - - print("Finish of Astra2 synchronize frame.") +import os.path +import argparse +import sys +import json +import shutil +import configparser + +from pathlib import Path +from datetime import datetime + +## Config.ini configuration +# Unit: FPS. Please fill in according to the actual dataset frame rate, +# otherwise it will cause inaccurate matching. +frameRate = -1 +# Unit: ms. When the timestamp range difference of a matched frame group +# is greater than or equal to tspRangeThreshold, the file name will be marked. +tspRangeThreshold = -1 + +## Parsed automatically by Python script, DO NOT modify +primaryId = '-1' # Host ID +mainPythonWorkPath = "" # Working directory of SyncFramesMain.py +sourceFramesPath = "" # DO NOT modify, fixed value, path to TotalMode directory +sourceRootPath = "" # Root directory containing TotalMode +resultPath = "" # Path to save sync analysis results +streamProfileDict = dict() # Store Python results + + +class PictureInfo(object): + class Struct(object): + def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): + self.deviceId = deviceId + self.sensorType = sensorType + self.syncTimeStamp = syncTimeStamp + self.picturePath = picturePath + self.deviceIdFull = deviceIdFull + self.fileExt = fileExt + + def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): + return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt) + + +class StreamProfileInfo: + def __init__(self, sensorType, width, height, format, fps): + self.sensorType = sensorType + self.width = width + self.height = height + self.format = format + self.fps = fps + + +def initPrimaryId(): + global primaryId + + deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" + if not os.path.exists(deviceInfoPath): + print(f"Initialize primaryId failed. {deviceInfoPath} not exists") + return False + + with open(deviceInfoPath, 'r') as f: + data = json.load(f) + + if not 'devices' in data: + print(f"Initialize primaryId failed. {deviceInfoPath} not contain item 'devices'") + return False + + for item in data['devices']: + if 'isPrimaryDevice' in item and 'index' in item and item['isPrimaryDevice']: + primaryId = str(item['index']) + return True + + return False + + +def initSyncConfigParams(): + global frameRate, tspRangeThreshold + + configPath = f"{mainPythonWorkPath}/Config.ini" + if not os.path.exists(configPath): + print(f"Initliaze synchronized config parameter failed. {configPath} not exists.") + return False + + # Create ConfigParser object + config = configparser.ConfigParser() + + # Read ini file + config.read(configPath, 'utf-8') + + if not config.has_option('Parameter', 'frameRate'): + print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}") + return False + + if not config.has_option('Parameter', 'tspRangeThreshold'): + print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}") + return False + + frameRate = config.getfloat('Parameter', 'frameRate') + if frameRate <= 0 or frameRate >= 1000: + print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}") + return False + + tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold') + if tspRangeThreshold <= 0: + print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}") + return False + + return True + + +def getDeviceCount(): + deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" + if not os.path.exists(deviceInfoPath): + print(f"Get device count failed. {deviceInfoPath} not exists") + return 0 + + with open(deviceInfoPath, 'r') as f: + data = json.load(f) + + if not 'devices' in data: + print(f"Get device count failed. {deviceInfoPath} not contain item 'devices'") + return 0 + + return len(data['devices']) + + +def initProfileInfoDict(): + global streamProfileDict + + streamProfileDict = dict() + profilePath = f"{sourceRootPath}/StreamProfileInfo.txt" + if not os.path.exists(profilePath): + print(f"Initialize profile information dictionary failed. {profilePath} not exists") + return False + + with open(profilePath, 'r') as f: + data = json.load(f) + + if not 'streamProfiles' in data: + print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'") + return False + + for item in data['streamProfiles']: + if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item: + profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps']) + streamProfileDict.setdefault(item['sensorType'], profileInfo) + else: + print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}") + + return len(streamProfileDict.items()) > 0 + + +def initResultDir(): + global resultPath + timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S") + resultPath = f"{sourceRootPath}/results-{timeText}" + if not os.path.exists(resultPath): + os.makedirs(resultPath) + + +def isFrameFile(fileName): + fileExt = os.path.splitext(fileName) + if len(fileExt) < 2: + return False + + frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"} + return fileExt[1] in frameExts + + +def initPictureInfoDictionary(rootFilePath, pictureInfoDict): + frameFileCount = 0 + for dirpath, dirnames, filenames in os.walk(rootFilePath): + # print(f"dirpath={dirpath}, dirpath.basename=" + os.path.basename(dirpath)) + for fileName in filenames: + filePath = os.path.join(dirpath, fileName) + if not isFrameFile(fileName): + continue + + frameFileCount += 1 + fileNameNoExt = os.path.splitext(fileName)[0] + pictureNameList = fileNameNoExt.split('_') + deviceId = pictureNameList[2][5:] + sensorType = pictureNameList[0].replace('#', '_') + syncTimeStamp = int(pictureNameList[3][1:]) + + # Create struct + pictureInfo = PictureInfo() + info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1]) + + # Format dictionary: one key corresponds to one array + pictureInfoDict.setdefault(deviceId, []).append(info) + + if 0 == frameFileCount: + print("initialize pictureInfoDict failed. Not found frame file") + + +def matchFrame(pictureInfoDict): + global primaryId, frameRate + global streamProfileDict + + # Get comparison image array, take the Color array of the first device for comparison + compareList = [] + for key in pictureInfoDict: + listTmp = list(pictureInfoDict[key]) + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == 'color': + compareList.append(info) + + sorted_dict = dict(sorted(pictureInfoDict.items(), key=lambda x: x[1], reverse=False)) + pictureInfoDict = sorted_dict + + # Dynamically analyze based on stream opening status to reduce loop iterations + hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict + hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict + hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict + + # Match adjacent frames + resultDict = {} + consumedList = [] + dictIndex = 0 + frameTspGap = int(1000.0/frameRate+0.5) + for comparePic in compareList: + resultDict.setdefault(dictIndex, []).append(comparePic) + for key in pictureInfoDict: + listTmp = list(pictureInfoDict[key]) + # Compare Color + if hasColorProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'color' and info.syncTimeStamp >= comparePic.syncTimeStamp and info.syncTimeStamp - comparePic.syncTimeStamp <= frameTspGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare Depth + if hasDepthProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'depth' and info.syncTimeStamp >= comparePic.syncTimeStamp and info.syncTimeStamp - comparePic.syncTimeStamp <= frameTspGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare IR + if hasIRProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir' and info.syncTimeStamp >= comparePic.syncTimeStamp and info.syncTimeStamp - comparePic.syncTimeStamp <= frameTspGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Calculate deviation + for info in resultDict[dictIndex]: + info.tspDiff = info.syncTimeStamp - comparePic.syncTimeStamp + + dictIndex += 1 + + return resultDict + + +def safe_make_dir(path): + if not os.path.exists(path): + os.makedirs(path) + + +def handleSyncFrames(): + global resultPath + global sourceFramesPath + global streamProfileDict + + fp = open(f"{resultPath}/frameMatchLog.txt", "w") + + if not initProfileInfoDict(): + print("Initialize stream profile dictionary failed. exit") + return + print(f"Stream profiles: {streamProfileDict}") + streamProfileCount = len(streamProfileDict.items()) + + pictureInfoDict = {} + + initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict) + if len(pictureInfoDict) <= 0: + print("init pictureInfoDict failed. pictureInfoDict.len = 0") + return + + # # Dump pictureInfoDict + # for key in pictureInfoDict: + # print("===================key:%s" % key) + # listTmp = list(pictureInfoDict[key]) + # for info in listTmp: + # print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath)) + + deviceCount = getDeviceCount() + print("=================device count:%d" % deviceCount) + + resultDict = {} + resultDict = matchFrame(pictureInfoDict) + + for key in resultDict: + fp.write("===================result key:%s\n" % key) + print("===================result key:%s" % key) + listTmp = list(resultDict[key]) + for info in listTmp: + fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % ( + info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) + print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % ( + info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) + + matchPath = f"{resultPath}/matchFrames/" + safe_make_dir(matchPath) + notMatchPath = f"{resultPath}/notMatchFrames/" + abnormalPath = f"{resultPath}/abnormal/" + + for key in resultDict: + listTmp = list(resultDict[key]) + if (len(listTmp) == (deviceCount * streamProfileCount)): + haveAbnormalData = False + for info in listTmp: + if info.tspDiff >= tspRangeThreshold: + path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt + shutil.copy(info.picturePath, path) + haveAbnormalData = True + else: + path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt + shutil.copy(info.picturePath, path) + + if haveAbnormalData: + safe_make_dir(abnormalPath) + for info in listTmp: + if info.tspDiff >= tspRangeThreshold: + path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt + shutil.copy(info.picturePath, path) + else: + path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt + shutil.copy(info.picturePath, path) + else: + safe_make_dir(notMatchPath) + for info in listTmp: + newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt + shutil.copy(info.picturePath, newFilePath) + + +def main(): + if not initPrimaryId(): + print("Initialize primaryId failed. exit.") + return + + if not initSyncConfigParams(): + print("Initialized Sync config parameter failed. exit.") + return + + initResultDir() + + print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}") + handleSyncFrames() + + return + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Astra2'") + parser.add_argument('frames_dir', type=str, help="Frames directory") + parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory") + args = parser.parse_args() + + sourceRootPath = args.frames_dir + sourceFramesPath = f"{args.frames_dir}/TotalModeFrames" + mainPythonWorkPath = args.main_script_root_dir + print(f"Start of Astra2 synchronize frame. sourceRootPath={sourceRootPath}") + + main() + + print("Finish of Astra2 synchronize frame.") diff --git a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch-systemTimestamp.py b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch-systemTimestamp.py index 302917cb..7bec6097 100644 --- a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch-systemTimestamp.py +++ b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch-systemTimestamp.py @@ -1,403 +1,403 @@ -import os.path -import argparse -import sys -import json -import shutil -import configparser - -from pathlib import Path -from datetime import datetime - -## Config.ini configuration -# Unit: FPS. Please fill in according to the actual dataset frame rate, -# otherwise it will cause inaccurate matching. -frameRate = -1 -# Unit: ms. When the timestamp range difference of a matched frame group -# is greater than or equal to tspRangeThreshold, the file name will be marked. -tspRangeThreshold = -1 - -## Parsed automatically by Python script, DO NOT modify -primaryId = '-1' # Host ID -mainPythonWorkPath = "" # Working directory of SyncFramesMain.py -sourceFramesPath = "" # DO NOT modify, fixed value, path to TotalMode directory -sourceRootPath = "" # Root directory containing TotalMode -resultPath = "" # Path to save sync analysis results -streamProfileDict = dict() # Store Python results - -class PictureInfo(object): - class Struct(object): - def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): - self.deviceId = deviceId - self.sensorType = sensorType - self.syncTimeStamp = syncTimeStamp - self.picturePath = picturePath - self.deviceIdFull = deviceIdFull - self.fileExt = fileExt - - def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): - return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt) - -class StreamProfileInfo: - def __init__(self, sensorType, width, height, format, fps): - self.sensorType = sensorType - self.width = width - self.height = height - self.format = format - self.fps = fps - -def initPrimaryId(): - global primaryId - - deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" - if not os.path.exists(deviceInfoPath): - print(f"Initialize primaryId failed. {deviceInfoPath} not exists") - return False - - with open(deviceInfoPath, 'r') as f: - data = json.load(f) - - if not 'devices' in data: - print(f"Initialize primaryId failed. {deviceInfoPath} not contain item 'devices'") - return False - - for item in data['devices']: - if 'isPrimaryDevice' in item and 'index' in item and item['isPrimaryDevice']: - primaryId = str(item['index']) - return True - - return False - -def initSyncConfigParams(): - global frameRate, tspRangeThreshold - - configPath = f"{mainPythonWorkPath}/Config.ini" - if not os.path.exists(configPath): - print(f"Initliaze synchronized config parameter failed. {configPath} not exists.") - return False - - # Create ConfigParser object - config = configparser.ConfigParser() - - # Read ini file - config.read(configPath, 'utf-8') - - if not config.has_option('Parameter', 'frameRate'): - print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}") - return False - - if not config.has_option('Parameter', 'tspRangeThreshold'): - print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}") - return False - - frameRate = config.getfloat('Parameter', 'frameRate') - if frameRate <= 0 or frameRate >= 1000: - print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}") - return False - - tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold') - if tspRangeThreshold <= 0: - print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}") - return False - - return True - -def getDeviceCount(): - deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" - if not os.path.exists(deviceInfoPath): - print(f"Get device count failed. {deviceInfoPath} not exists") - return 0 - - with open(deviceInfoPath, 'r') as f: - data = json.load(f) - - if not 'devices' in data: - print(f"Get device count failed. {deviceInfoPath} not contain item 'devices'") - return 0 - - return len(data['devices']) - -def initProfileInfoDict(): - global streamProfileDict - - streamProfileDict = dict() - profilePath = f"{sourceRootPath}/StreamProfileInfo.txt" - if not os.path.exists(profilePath): - print(f"Initialize profile information dictionary failed. {profilePath} not exists") - return False - - with open(profilePath, 'r') as f: - data = json.load(f) - - if not 'streamProfiles' in data: - print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'") - return False - - for item in data['streamProfiles']: - if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item: - profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps']) - streamProfileDict.setdefault(item['sensorType'], profileInfo) - else: - print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}") - - return len(streamProfileDict.items()) > 0 - -def initResultDir(): - global resultPath - timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S") - resultPath = f"{sourceRootPath}/results-{timeText}" - if not os.path.exists(resultPath): - os.makedirs(resultPath) - -def isFrameFile(fileName): - fileExt = os.path.splitext(fileName) - if len(fileExt) < 2: - return False - - frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"} - return fileExt[1] in frameExts - -def initPictureInfoDictionary(rootFilePath, pictureInfoDict): - frameFileCount = 0 - for dirpath, dirnames, filenames in os.walk(rootFilePath): - # print(f"dirpath={dirpath}, dirpath.basename=" + os.path.basename(dirpath)) - for fileName in filenames: - filePath = os.path.join(dirpath, fileName) - if not isFrameFile(fileName): - continue - - frameFileCount += 1 - fileNameNoExt = os.path.splitext(fileName)[0] - pictureNameList = fileNameNoExt.split('_') - deviceId = pictureNameList[2][5:] - sensorType = pictureNameList[0].replace('#', '_') - syncTimeStamp = int(pictureNameList[5][1:]) - - # Create struct - pictureInfo = PictureInfo() - info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1]) - - # Format dictionary: one key corresponds to one array - pictureInfoDict.setdefault(deviceId, []).append(info) - - if 0 == frameFileCount: - print("initialize pictureInfoDict failed. Not found frame file") - - -def matchFrame(pictureInfoDict): - global primaryId, frameRate - global streamProfileDict - - # Get comparison image array, take the Color array of the first device for comparison - compareList = [] - for key in pictureInfoDict: - listTmp = list(pictureInfoDict[key]) - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == 'color': - compareList.append(info) - - # Dynamically analyze based on stream opening status to reduce loop iterations - hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict - hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict - hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict - hasIRLeftProfile = 'OB_SENSOR_IR_LEFT' in streamProfileDict - hasIRRightProfile = 'OB_SENSOR_IR_RIGHT' in streamProfileDict - - # Match adjacent frames - resultDict = {} - consumedList = [] - dictIndex = 0 - frameTspHalfGap = int(1000.0/frameRate/2.0+0.5) - for comparePic in compareList: - resultDict.setdefault(dictIndex, []).append(comparePic) - for key in pictureInfoDict: - listTmp = list(pictureInfoDict[key]) - # Compare Color - if hasColorProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'color' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare Depth - if hasDepthProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'depth' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # IR - if hasIRProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Left IR - if hasIRLeftProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir_left' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Right IR - if hasIRRightProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir_right' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Calculate deviation - minTsp = min(resultDict[dictIndex], key=lambda x: x.syncTimeStamp) - for info in resultDict[dictIndex]: - info.tspDiff = info.syncTimeStamp - minTsp.syncTimeStamp - - dictIndex += 1 - - return resultDict - - -def safe_make_dir(path): - if not os.path.exists(path): - os.makedirs(path) - - -def handleSyncFrames(): - global resultPath - global sourceFramesPath - global streamProfileDict - - fp = open(f"{resultPath}/frameMatchLog.txt", "w") - - if not initProfileInfoDict(): - print("Initialize stream profile dictionary failed. exit") - return - print(f"Stream profiles: {streamProfileDict}") - streamProfileCount = len(streamProfileDict.items()) - - pictureInfoDict = {} - - initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict) - if len(pictureInfoDict) <= 0: - print("init pictureInfoDict failed. pictureInfoDict.len = 0") - return - - deviceCount = getDeviceCount() - print("=================device count:%d" % deviceCount) - - resultDict = {} - resultDict = matchFrame(pictureInfoDict) - - for key in resultDict: - fp.write("===================result key:%s\n" % key) - print("===================result key:%s" % key) - listTmp = list(resultDict[key]) - for info in listTmp: - fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % ( - info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) - print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % ( - info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) - - matchPath = f"{resultPath}/matchFrames/" - safe_make_dir(matchPath) - notMatchPath = f"{resultPath}/notMatchFrames/" - abnormalPath = f"{resultPath}/abnormal/" - - for key in resultDict: - listTmp = list(resultDict[key]) - if (len(listTmp) == (deviceCount * streamProfileCount)): - haveAbnormalData = False - for info in listTmp: - if info.tspDiff >= tspRangeThreshold: - path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt - shutil.copy(info.picturePath, path) - haveAbnormalData = True - else: - path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt - shutil.copy(info.picturePath, path) - - if haveAbnormalData: - safe_make_dir(abnormalPath) - for info in listTmp: - if info.tspDiff >= tspRangeThreshold: - path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt - shutil.copy(info.picturePath, path) - else: - path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt - shutil.copy(info.picturePath, path) - else: - safe_make_dir(notMatchPath) - for info in listTmp: - newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt - shutil.copy(info.picturePath, newFilePath) - -def main(): - if not initPrimaryId(): - print("Initialize primaryId failed. exit.") - return - - if not initSyncConfigParams(): - print("Initialized Sync config parameter failed. exit.") - return - - initResultDir() - - print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}") - handleSyncFrames() - - return - - -if __name__ == '__main__': - parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Gemini 2'") - parser.add_argument('frames_dir', type=str, help="Frames directory") - parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory") - args = parser.parse_args() - - sourceRootPath = args.frames_dir - sourceFramesPath = f"{args.frames_dir}/TotalModeFrames" - mainPythonWorkPath = args.main_script_root_dir - print(f"Start of Gemini2 synchronize frame. sourceRootPath={sourceRootPath}") - - main() - - print("Finish of Gemini2 synchronize frame.") +import os.path +import argparse +import sys +import json +import shutil +import configparser + +from pathlib import Path +from datetime import datetime + +## Config.ini configuration +# Unit: FPS. Please fill in according to the actual dataset frame rate, +# otherwise it will cause inaccurate matching. +frameRate = -1 +# Unit: ms. When the timestamp range difference of a matched frame group +# is greater than or equal to tspRangeThreshold, the file name will be marked. +tspRangeThreshold = -1 + +## Parsed automatically by Python script, DO NOT modify +primaryId = '-1' # Host ID +mainPythonWorkPath = "" # Working directory of SyncFramesMain.py +sourceFramesPath = "" # DO NOT modify, fixed value, path to TotalMode directory +sourceRootPath = "" # Root directory containing TotalMode +resultPath = "" # Path to save sync analysis results +streamProfileDict = dict() # Store Python results + +class PictureInfo(object): + class Struct(object): + def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): + self.deviceId = deviceId + self.sensorType = sensorType + self.syncTimeStamp = syncTimeStamp + self.picturePath = picturePath + self.deviceIdFull = deviceIdFull + self.fileExt = fileExt + + def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): + return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt) + +class StreamProfileInfo: + def __init__(self, sensorType, width, height, format, fps): + self.sensorType = sensorType + self.width = width + self.height = height + self.format = format + self.fps = fps + +def initPrimaryId(): + global primaryId + + deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" + if not os.path.exists(deviceInfoPath): + print(f"Initialize primaryId failed. {deviceInfoPath} not exists") + return False + + with open(deviceInfoPath, 'r') as f: + data = json.load(f) + + if not 'devices' in data: + print(f"Initialize primaryId failed. {deviceInfoPath} not contain item 'devices'") + return False + + for item in data['devices']: + if 'isPrimaryDevice' in item and 'index' in item and item['isPrimaryDevice']: + primaryId = str(item['index']) + return True + + return False + +def initSyncConfigParams(): + global frameRate, tspRangeThreshold + + configPath = f"{mainPythonWorkPath}/Config.ini" + if not os.path.exists(configPath): + print(f"Initliaze synchronized config parameter failed. {configPath} not exists.") + return False + + # Create ConfigParser object + config = configparser.ConfigParser() + + # Read ini file + config.read(configPath, 'utf-8') + + if not config.has_option('Parameter', 'frameRate'): + print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}") + return False + + if not config.has_option('Parameter', 'tspRangeThreshold'): + print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}") + return False + + frameRate = config.getfloat('Parameter', 'frameRate') + if frameRate <= 0 or frameRate >= 1000: + print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}") + return False + + tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold') + if tspRangeThreshold <= 0: + print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}") + return False + + return True + +def getDeviceCount(): + deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" + if not os.path.exists(deviceInfoPath): + print(f"Get device count failed. {deviceInfoPath} not exists") + return 0 + + with open(deviceInfoPath, 'r') as f: + data = json.load(f) + + if not 'devices' in data: + print(f"Get device count failed. {deviceInfoPath} not contain item 'devices'") + return 0 + + return len(data['devices']) + +def initProfileInfoDict(): + global streamProfileDict + + streamProfileDict = dict() + profilePath = f"{sourceRootPath}/StreamProfileInfo.txt" + if not os.path.exists(profilePath): + print(f"Initialize profile information dictionary failed. {profilePath} not exists") + return False + + with open(profilePath, 'r') as f: + data = json.load(f) + + if not 'streamProfiles' in data: + print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'") + return False + + for item in data['streamProfiles']: + if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item: + profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps']) + streamProfileDict.setdefault(item['sensorType'], profileInfo) + else: + print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}") + + return len(streamProfileDict.items()) > 0 + +def initResultDir(): + global resultPath + timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S") + resultPath = f"{sourceRootPath}/results-{timeText}" + if not os.path.exists(resultPath): + os.makedirs(resultPath) + +def isFrameFile(fileName): + fileExt = os.path.splitext(fileName) + if len(fileExt) < 2: + return False + + frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"} + return fileExt[1] in frameExts + +def initPictureInfoDictionary(rootFilePath, pictureInfoDict): + frameFileCount = 0 + for dirpath, dirnames, filenames in os.walk(rootFilePath): + # print(f"dirpath={dirpath}, dirpath.basename=" + os.path.basename(dirpath)) + for fileName in filenames: + filePath = os.path.join(dirpath, fileName) + if not isFrameFile(fileName): + continue + + frameFileCount += 1 + fileNameNoExt = os.path.splitext(fileName)[0] + pictureNameList = fileNameNoExt.split('_') + deviceId = pictureNameList[2][5:] + sensorType = pictureNameList[0].replace('#', '_') + syncTimeStamp = int(pictureNameList[5][1:]) + + # Create struct + pictureInfo = PictureInfo() + info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1]) + + # Format dictionary: one key corresponds to one array + pictureInfoDict.setdefault(deviceId, []).append(info) + + if 0 == frameFileCount: + print("initialize pictureInfoDict failed. Not found frame file") + + +def matchFrame(pictureInfoDict): + global primaryId, frameRate + global streamProfileDict + + # Get comparison image array, take the Color array of the first device for comparison + compareList = [] + for key in pictureInfoDict: + listTmp = list(pictureInfoDict[key]) + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == 'color': + compareList.append(info) + + # Dynamically analyze based on stream opening status to reduce loop iterations + hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict + hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict + hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict + hasIRLeftProfile = 'OB_SENSOR_IR_LEFT' in streamProfileDict + hasIRRightProfile = 'OB_SENSOR_IR_RIGHT' in streamProfileDict + + # Match adjacent frames + resultDict = {} + consumedList = [] + dictIndex = 0 + frameTspHalfGap = int(1000.0/frameRate/2.0+0.5) + for comparePic in compareList: + resultDict.setdefault(dictIndex, []).append(comparePic) + for key in pictureInfoDict: + listTmp = list(pictureInfoDict[key]) + # Compare Color + if hasColorProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'color' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare Depth + if hasDepthProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'depth' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # IR + if hasIRProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Left IR + if hasIRLeftProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir_left' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Right IR + if hasIRRightProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir_right' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Calculate deviation + minTsp = min(resultDict[dictIndex], key=lambda x: x.syncTimeStamp) + for info in resultDict[dictIndex]: + info.tspDiff = info.syncTimeStamp - minTsp.syncTimeStamp + + dictIndex += 1 + + return resultDict + + +def safe_make_dir(path): + if not os.path.exists(path): + os.makedirs(path) + + +def handleSyncFrames(): + global resultPath + global sourceFramesPath + global streamProfileDict + + fp = open(f"{resultPath}/frameMatchLog.txt", "w") + + if not initProfileInfoDict(): + print("Initialize stream profile dictionary failed. exit") + return + print(f"Stream profiles: {streamProfileDict}") + streamProfileCount = len(streamProfileDict.items()) + + pictureInfoDict = {} + + initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict) + if len(pictureInfoDict) <= 0: + print("init pictureInfoDict failed. pictureInfoDict.len = 0") + return + + deviceCount = getDeviceCount() + print("=================device count:%d" % deviceCount) + + resultDict = {} + resultDict = matchFrame(pictureInfoDict) + + for key in resultDict: + fp.write("===================result key:%s\n" % key) + print("===================result key:%s" % key) + listTmp = list(resultDict[key]) + for info in listTmp: + fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % ( + info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) + print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % ( + info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) + + matchPath = f"{resultPath}/matchFrames/" + safe_make_dir(matchPath) + notMatchPath = f"{resultPath}/notMatchFrames/" + abnormalPath = f"{resultPath}/abnormal/" + + for key in resultDict: + listTmp = list(resultDict[key]) + if (len(listTmp) == (deviceCount * streamProfileCount)): + haveAbnormalData = False + for info in listTmp: + if info.tspDiff >= tspRangeThreshold: + path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt + shutil.copy(info.picturePath, path) + haveAbnormalData = True + else: + path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt + shutil.copy(info.picturePath, path) + + if haveAbnormalData: + safe_make_dir(abnormalPath) + for info in listTmp: + if info.tspDiff >= tspRangeThreshold: + path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt + shutil.copy(info.picturePath, path) + else: + path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt + shutil.copy(info.picturePath, path) + else: + safe_make_dir(notMatchPath) + for info in listTmp: + newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt + shutil.copy(info.picturePath, newFilePath) + +def main(): + if not initPrimaryId(): + print("Initialize primaryId failed. exit.") + return + + if not initSyncConfigParams(): + print("Initialized Sync config parameter failed. exit.") + return + + initResultDir() + + print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}") + handleSyncFrames() + + return + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Gemini 2'") + parser.add_argument('frames_dir', type=str, help="Frames directory") + parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory") + args = parser.parse_args() + + sourceRootPath = args.frames_dir + sourceFramesPath = f"{args.frames_dir}/TotalModeFrames" + mainPythonWorkPath = args.main_script_root_dir + print(f"Start of Gemini2 synchronize frame. sourceRootPath={sourceRootPath}") + + main() + + print("Finish of Gemini2 synchronize frame.") diff --git a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch.py b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch.py index 9b89a655..8d32f384 100644 --- a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch.py +++ b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch.py @@ -1,425 +1,425 @@ -import os.path -import argparse -import sys -import json -import shutil -import configparser - -from pathlib import Path -from datetime import datetime - -import os -import re - -## Config.ini configuration -# Unit: FPS. Please fill in according to the actual dataset frame rate, -# otherwise it will cause inaccurate matching. -frameRate = -1 -# Unit: ms. When the timestamp range difference of a matched frame group -# is greater than or equal to tspRangeThreshold, the file name will be marked. -tspRangeThreshold = -1 - -## Parsed automatically by Python script, DO NOT modify -primaryId = '-1' # Host ID -mainPythonWorkPath = "" # Working directory of SyncFramesMain.py -sourceFramesPath = "" # DO NOT modify, fixed value, path to TotalMode directory -sourceRootPath = "" # Root directory containing TotalMode -resultPath = "" # Path to save sync analysis results -streamProfileDict = dict() # Store Python results - -class PictureInfo(object): - class Struct(object): - def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): - self.deviceId = deviceId - self.sensorType = sensorType - self.syncTimeStamp = syncTimeStamp - self.picturePath = picturePath - self.deviceIdFull = deviceIdFull - self.fileExt = fileExt - - def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): - return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt) - -class StreamProfileInfo: - def __init__(self, sensorType, width, height, format, fps): - self.sensorType = sensorType - self.width = width - self.height = height - self.format = format - self.fps = fps - -def initPrimaryId(): - global primaryId - - deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" - if not os.path.exists(deviceInfoPath): - print(f"Initialize primaryId failed. {deviceInfoPath} not exists") - return False - - with open(deviceInfoPath, 'r') as f: - data = json.load(f) - - if not 'devices' in data: - print(f"Initialize primaryId failed. {deviceInfoPath} not contain item 'devices'") - return False - - for item in data['devices']: - if 'isPrimaryDevice' in item and 'index' in item and item['isPrimaryDevice']: - primaryId = str(item['index']) - return True - - return False - -def initSyncConfigParams(): - global frameRate, tspRangeThreshold - - configPath = f"{mainPythonWorkPath}/Config.ini" - if not os.path.exists(configPath): - print(f"Initliaze synchronized config parameter failed. {configPath} not exists.") - return False - - # Create ConfigParser object - config = configparser.ConfigParser() - - # Read ini file - config.read(configPath, 'utf-8') - - if not config.has_option('Parameter', 'frameRate'): - print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}") - return False - - if not config.has_option('Parameter', 'tspRangeThreshold'): - print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}") - return False - - frameRate = config.getfloat('Parameter', 'frameRate') - if frameRate <= 0 or frameRate >= 1000: - print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}") - return False - - tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold') - if tspRangeThreshold <= 0: - print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}") - return False - - return True - -def getDeviceCount(): - deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" - if not os.path.exists(deviceInfoPath): - print(f"Get device count failed. {deviceInfoPath} not exists") - return 0 - - with open(deviceInfoPath, 'r') as f: - data = json.load(f) - - if not 'devices' in data: - print(f"Get device count failed. {deviceInfoPath} not contain item 'devices'") - return 0 - - return len(data['devices']) - -def initProfileInfoDict(): - global streamProfileDict - - streamProfileDict = dict() - profilePath = f"{sourceRootPath}/StreamProfileInfo.txt" - if not os.path.exists(profilePath): - print(f"Initialize profile information dictionary failed. {profilePath} not exists") - return False - - with open(profilePath, 'r') as f: - data = json.load(f) - - if not 'streamProfiles' in data: - print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'") - return False - - for item in data['streamProfiles']: - if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item: - profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps']) - streamProfileDict.setdefault(item['sensorType'], profileInfo) - else: - print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}") - - return len(streamProfileDict.items()) > 0 - -def initResultDir(): - global resultPath - timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S") - resultPath = f"{sourceRootPath}/results-{timeText}" - if not os.path.exists(resultPath): - os.makedirs(resultPath) - -def isFrameFile(fileName): - fileExt = os.path.splitext(fileName) - if len(fileExt) < 2: - return False - - frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"} - return fileExt[1] in frameExts - -def extract_number(filename): - """Extract the numeric part after 'g' in the filename""" - match_g = re.search(r'g(\d+)', filename) - if match_g: - return int(match_g.group(1)) - - """Extract the numeric part after 'd' in the filename""" - match_d = re.search(r'd(\d+)', filename) - if match_d: - return int(match_d.group(1)) - return float('inf') - -def initPictureInfoDictionary(rootFilePath, pictureInfoDict): - frameFileCount = 0 - for dirpath, dirnames, filenames in os.walk(rootFilePath): - filenames.sort(key=extract_number) # Sort filenames by the numeric part after 'd' and 'g' - for fileName in filenames: - filePath = os.path.join(dirpath, fileName) - if not isFrameFile(fileName): - continue - - frameFileCount += 1 - fileNameNoExt = os.path.splitext(fileName)[0] - pictureNameList = fileNameNoExt.split('_') - deviceId = pictureNameList[2][5:] - sensorType = pictureNameList[0].replace('#', '_') - syncTimeStamp = int(pictureNameList[3][1:]) - - # Create struct - pictureInfo = PictureInfo() - info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1]) - - # Format dictionary: one key corresponds to one array - pictureInfoDict.setdefault(deviceId, []).append(info) - - if 0 == frameFileCount: - print("initialize pictureInfoDict failed. Not found frame file") - - -def matchFrame(pictureInfoDict): - global primaryId, frameRate - global streamProfileDict - - # Get comparison image array, use the Color array of the first device for comparison - compareList = [] - for key in pictureInfoDict: - listTmp = list(pictureInfoDict[key]) - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == 'color': - compareList.append(info) - - # Dynamically analyze based on stream opening status to reduce loop iterations - hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict - hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict - hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict - hasIRLeftProfile = 'OB_SENSOR_IR_LEFT' in streamProfileDict - hasIRRightProfile = 'OB_SENSOR_IR_RIGHT' in streamProfileDict - - # Match adjacent frames - resultDict = {} - consumedList = [] - dictIndex = 0 - frameTspHalfGap = int(1000.0/frameRate/2.0+0.5) - for comparePic in compareList: - resultDict.setdefault(dictIndex, []).append(comparePic) - for key in pictureInfoDict: - listTmp = list(pictureInfoDict[key]) - # Compare color - if hasColorProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'color' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare depth - if hasDepthProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'depth' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare IR - if hasIRProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare left IR - if hasIRLeftProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir_left' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare right IR - if hasIRRightProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir_right' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Calculate deviation - minTsp = min(resultDict[dictIndex], key=lambda x: x.syncTimeStamp) - for info in resultDict[dictIndex]: - info.tspDiff = info.syncTimeStamp - minTsp.syncTimeStamp - - dictIndex += 1 - - return resultDict - - -def safe_make_dir(path): - if not os.path.exists(path): - os.makedirs(path) - - -def handleSyncFrames(): - global resultPath - global sourceFramesPath - global streamProfileDict - - fp = open(f"{resultPath}/frameMatchLog.txt", "w") - - if not initProfileInfoDict(): - print("Initialize stream profile dictionary failed. exit") - return - print(f"Stream profiles: {streamProfileDict}") - streamProfileCount = len(streamProfileDict.items()) - - pictureInfoDict = {} - - initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict) - if len(pictureInfoDict) <= 0: - print("init pictureInfoDict failed. pictureInfoDict.len = 0") - return - - # # Dump pictureInfoDict - # for key in pictureInfoDict: - # print("===================key:%s" % key) - # listTmp = list(pictureInfoDict[key]) - # for info in listTmp: - # print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath)) - - deviceCount = getDeviceCount() - print("=================device count:%d" % deviceCount) - - resultDict = {} - resultDict = matchFrame(pictureInfoDict) - - for key in resultDict: - fp.write("===================result key:%s\n" % key) - print("===================result key:%s" % key) - listTmp = list(resultDict[key]) - for info in listTmp: - fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % ( - info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) - print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % ( - info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) - - matchPath = f"{resultPath}/matchFrames/" - safe_make_dir(matchPath) - notMatchPath = f"{resultPath}/notMatchFrames/" - abnormalPath = f"{resultPath}/abnormal/" - - for key in resultDict: - listTmp = list(resultDict[key]) - if (len(listTmp) == (deviceCount * streamProfileCount)): - haveAbnormalData = False - for info in listTmp: - if info.tspDiff >= tspRangeThreshold: - path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt - shutil.copy(info.picturePath, path) - haveAbnormalData = True - else: - path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt - shutil.copy(info.picturePath, path) - - if haveAbnormalData: - safe_make_dir(abnormalPath) - for info in listTmp: - if info.tspDiff >= tspRangeThreshold: - path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt - shutil.copy(info.picturePath, path) - else: - path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt - shutil.copy(info.picturePath, path) - else: - safe_make_dir(notMatchPath) - for info in listTmp: - newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt - shutil.copy(info.picturePath, newFilePath) - -def main(): - if not initPrimaryId(): - print("Initialize primaryId failed. exit.") - return - - if not initSyncConfigParams(): - print("Initialized Sync config parameter failed. exit.") - return - - initResultDir() - - print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}") - handleSyncFrames() - - return - - -if __name__ == '__main__': - parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Devices'") - parser.add_argument('frames_dir', type=str, help="Frames directory") - parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory") - args = parser.parse_args() - - sourceRootPath = args.frames_dir - sourceFramesPath = f"{args.frames_dir}/TotalModeFrames" - mainPythonWorkPath = args.main_script_root_dir - print(f"Start of CommonFrameMatch synchronize frame. sourceRootPath={sourceRootPath}") - - main() - - print("Finish of CommonFrameMatch synchronize frame.") +import os.path +import argparse +import sys +import json +import shutil +import configparser + +from pathlib import Path +from datetime import datetime + +import os +import re + +## Config.ini configuration +# Unit: FPS. Please fill in according to the actual dataset frame rate, +# otherwise it will cause inaccurate matching. +frameRate = -1 +# Unit: ms. When the timestamp range difference of a matched frame group +# is greater than or equal to tspRangeThreshold, the file name will be marked. +tspRangeThreshold = -1 + +## Parsed automatically by Python script, DO NOT modify +primaryId = '-1' # Host ID +mainPythonWorkPath = "" # Working directory of SyncFramesMain.py +sourceFramesPath = "" # DO NOT modify, fixed value, path to TotalMode directory +sourceRootPath = "" # Root directory containing TotalMode +resultPath = "" # Path to save sync analysis results +streamProfileDict = dict() # Store Python results + +class PictureInfo(object): + class Struct(object): + def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): + self.deviceId = deviceId + self.sensorType = sensorType + self.syncTimeStamp = syncTimeStamp + self.picturePath = picturePath + self.deviceIdFull = deviceIdFull + self.fileExt = fileExt + + def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): + return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt) + +class StreamProfileInfo: + def __init__(self, sensorType, width, height, format, fps): + self.sensorType = sensorType + self.width = width + self.height = height + self.format = format + self.fps = fps + +def initPrimaryId(): + global primaryId + + deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" + if not os.path.exists(deviceInfoPath): + print(f"Initialize primaryId failed. {deviceInfoPath} not exists") + return False + + with open(deviceInfoPath, 'r') as f: + data = json.load(f) + + if not 'devices' in data: + print(f"Initialize primaryId failed. {deviceInfoPath} not contain item 'devices'") + return False + + for item in data['devices']: + if 'isPrimaryDevice' in item and 'index' in item and item['isPrimaryDevice']: + primaryId = str(item['index']) + return True + + return False + +def initSyncConfigParams(): + global frameRate, tspRangeThreshold + + configPath = f"{mainPythonWorkPath}/Config.ini" + if not os.path.exists(configPath): + print(f"Initliaze synchronized config parameter failed. {configPath} not exists.") + return False + + # Create ConfigParser object + config = configparser.ConfigParser() + + # Read ini file + config.read(configPath, 'utf-8') + + if not config.has_option('Parameter', 'frameRate'): + print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}") + return False + + if not config.has_option('Parameter', 'tspRangeThreshold'): + print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}") + return False + + frameRate = config.getfloat('Parameter', 'frameRate') + if frameRate <= 0 or frameRate >= 1000: + print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}") + return False + + tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold') + if tspRangeThreshold <= 0: + print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}") + return False + + return True + +def getDeviceCount(): + deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" + if not os.path.exists(deviceInfoPath): + print(f"Get device count failed. {deviceInfoPath} not exists") + return 0 + + with open(deviceInfoPath, 'r') as f: + data = json.load(f) + + if not 'devices' in data: + print(f"Get device count failed. {deviceInfoPath} not contain item 'devices'") + return 0 + + return len(data['devices']) + +def initProfileInfoDict(): + global streamProfileDict + + streamProfileDict = dict() + profilePath = f"{sourceRootPath}/StreamProfileInfo.txt" + if not os.path.exists(profilePath): + print(f"Initialize profile information dictionary failed. {profilePath} not exists") + return False + + with open(profilePath, 'r') as f: + data = json.load(f) + + if not 'streamProfiles' in data: + print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'") + return False + + for item in data['streamProfiles']: + if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item: + profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps']) + streamProfileDict.setdefault(item['sensorType'], profileInfo) + else: + print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}") + + return len(streamProfileDict.items()) > 0 + +def initResultDir(): + global resultPath + timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S") + resultPath = f"{sourceRootPath}/results-{timeText}" + if not os.path.exists(resultPath): + os.makedirs(resultPath) + +def isFrameFile(fileName): + fileExt = os.path.splitext(fileName) + if len(fileExt) < 2: + return False + + frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"} + return fileExt[1] in frameExts + +def extract_number(filename): + """Extract the numeric part after 'g' in the filename""" + match_g = re.search(r'g(\d+)', filename) + if match_g: + return int(match_g.group(1)) + + """Extract the numeric part after 'd' in the filename""" + match_d = re.search(r'd(\d+)', filename) + if match_d: + return int(match_d.group(1)) + return float('inf') + +def initPictureInfoDictionary(rootFilePath, pictureInfoDict): + frameFileCount = 0 + for dirpath, dirnames, filenames in os.walk(rootFilePath): + filenames.sort(key=extract_number) # Sort filenames by the numeric part after 'd' and 'g' + for fileName in filenames: + filePath = os.path.join(dirpath, fileName) + if not isFrameFile(fileName): + continue + + frameFileCount += 1 + fileNameNoExt = os.path.splitext(fileName)[0] + pictureNameList = fileNameNoExt.split('_') + deviceId = pictureNameList[2][5:] + sensorType = pictureNameList[0].replace('#', '_') + syncTimeStamp = int(pictureNameList[3][1:]) + + # Create struct + pictureInfo = PictureInfo() + info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1]) + + # Format dictionary: one key corresponds to one array + pictureInfoDict.setdefault(deviceId, []).append(info) + + if 0 == frameFileCount: + print("initialize pictureInfoDict failed. Not found frame file") + + +def matchFrame(pictureInfoDict): + global primaryId, frameRate + global streamProfileDict + + # Get comparison image array, use the Color array of the first device for comparison + compareList = [] + for key in pictureInfoDict: + listTmp = list(pictureInfoDict[key]) + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == 'color': + compareList.append(info) + + # Dynamically analyze based on stream opening status to reduce loop iterations + hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict + hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict + hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict + hasIRLeftProfile = 'OB_SENSOR_IR_LEFT' in streamProfileDict + hasIRRightProfile = 'OB_SENSOR_IR_RIGHT' in streamProfileDict + + # Match adjacent frames + resultDict = {} + consumedList = [] + dictIndex = 0 + frameTspHalfGap = int(1000.0/frameRate/2.0+0.5) + for comparePic in compareList: + resultDict.setdefault(dictIndex, []).append(comparePic) + for key in pictureInfoDict: + listTmp = list(pictureInfoDict[key]) + # Compare color + if hasColorProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'color' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare depth + if hasDepthProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'depth' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare IR + if hasIRProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare left IR + if hasIRLeftProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir_left' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare right IR + if hasIRRightProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir_right' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Calculate deviation + minTsp = min(resultDict[dictIndex], key=lambda x: x.syncTimeStamp) + for info in resultDict[dictIndex]: + info.tspDiff = info.syncTimeStamp - minTsp.syncTimeStamp + + dictIndex += 1 + + return resultDict + + +def safe_make_dir(path): + if not os.path.exists(path): + os.makedirs(path) + + +def handleSyncFrames(): + global resultPath + global sourceFramesPath + global streamProfileDict + + fp = open(f"{resultPath}/frameMatchLog.txt", "w") + + if not initProfileInfoDict(): + print("Initialize stream profile dictionary failed. exit") + return + print(f"Stream profiles: {streamProfileDict}") + streamProfileCount = len(streamProfileDict.items()) + + pictureInfoDict = {} + + initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict) + if len(pictureInfoDict) <= 0: + print("init pictureInfoDict failed. pictureInfoDict.len = 0") + return + + # # Dump pictureInfoDict + # for key in pictureInfoDict: + # print("===================key:%s" % key) + # listTmp = list(pictureInfoDict[key]) + # for info in listTmp: + # print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath)) + + deviceCount = getDeviceCount() + print("=================device count:%d" % deviceCount) + + resultDict = {} + resultDict = matchFrame(pictureInfoDict) + + for key in resultDict: + fp.write("===================result key:%s\n" % key) + print("===================result key:%s" % key) + listTmp = list(resultDict[key]) + for info in listTmp: + fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % ( + info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) + print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % ( + info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) + + matchPath = f"{resultPath}/matchFrames/" + safe_make_dir(matchPath) + notMatchPath = f"{resultPath}/notMatchFrames/" + abnormalPath = f"{resultPath}/abnormal/" + + for key in resultDict: + listTmp = list(resultDict[key]) + if (len(listTmp) == (deviceCount * streamProfileCount)): + haveAbnormalData = False + for info in listTmp: + if info.tspDiff >= tspRangeThreshold: + path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt + shutil.copy(info.picturePath, path) + haveAbnormalData = True + else: + path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt + shutil.copy(info.picturePath, path) + + if haveAbnormalData: + safe_make_dir(abnormalPath) + for info in listTmp: + if info.tspDiff >= tspRangeThreshold: + path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt + shutil.copy(info.picturePath, path) + else: + path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt + shutil.copy(info.picturePath, path) + else: + safe_make_dir(notMatchPath) + for info in listTmp: + newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt + shutil.copy(info.picturePath, newFilePath) + +def main(): + if not initPrimaryId(): + print("Initialize primaryId failed. exit.") + return + + if not initSyncConfigParams(): + print("Initialized Sync config parameter failed. exit.") + return + + initResultDir() + + print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}") + handleSyncFrames() + + return + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Devices'") + parser.add_argument('frames_dir', type=str, help="Frames directory") + parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory") + args = parser.parse_args() + + sourceRootPath = args.frames_dir + sourceFramesPath = f"{args.frames_dir}/TotalModeFrames" + mainPythonWorkPath = args.main_script_root_dir + print(f"Start of CommonFrameMatch synchronize frame. sourceRootPath={sourceRootPath}") + + main() + + print("Finish of CommonFrameMatch synchronize frame.") diff --git a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch_Gemini2VL.py b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch_Gemini2VL.py index 2d769150..8f8e0846 100644 --- a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch_Gemini2VL.py +++ b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch_Gemini2VL.py @@ -1,410 +1,410 @@ -import os.path -import argparse -import sys -import json -import shutil -import configparser - -from pathlib import Path -from datetime import datetime - -## Config.ini configuration -# Unit: FPS. Please fill in according to the actual dataset frame rate, -# otherwise it will cause inaccurate matching. -frameRate = -1 -# Unit: ms. When the timestamp range difference of a matched frame group -# is greater than or equal to tspRangeThreshold, the file name will be marked. -tspRangeThreshold = -1 - -## Parsed automatically by Python script, DO NOT modify -primaryId = '-1' # Host ID -mainPythonWorkPath = "" # Working directory of SyncFramesMain.py -sourceFramesPath = "" # DO NOT modify, fixed value, path to TotalMode directory -sourceRootPath = "" # Root directory containing TotalMode -resultPath = "" # Path to save sync analysis results -streamProfileDict = dict() # Store Python results - -class PictureInfo(object): - class Struct(object): - def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): - self.deviceId = deviceId - self.sensorType = sensorType - self.syncTimeStamp = syncTimeStamp - self.picturePath = picturePath - self.deviceIdFull = deviceIdFull - self.fileExt = fileExt - - def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): - return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt) - -class StreamProfileInfo: - def __init__(self, sensorType, width, height, format, fps): - self.sensorType = sensorType - self.width = width - self.height = height - self.format = format - self.fps = fps - -def initPrimaryId(): - global primaryId - - deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" - if not os.path.exists(deviceInfoPath): - print(f"Initialize primaryId failed. {deviceInfoPath} not exists") - return False - - with open(deviceInfoPath, 'r') as f: - data = json.load(f) - - if not 'devices' in data: - print(f"Initialize primaryId failed. {deviceInfoPath} not contain item 'devices'") - return False - - for item in data['devices']: - if 'isPrimaryDevice' in item and 'index' in item and item['isPrimaryDevice']: - primaryId = str(item['index']) - return True - - return False - -def initSyncConfigParams(): - global frameRate, tspRangeThreshold - - configPath = f"{mainPythonWorkPath}/Config.ini" - if not os.path.exists(configPath): - print(f"Initliaze synchronized config parameter failed. {configPath} not exists.") - return False - - # Create ConfigParser object - config = configparser.ConfigParser() - - # Read ini file - config.read(configPath, 'utf-8') - - if not config.has_option('Parameter', 'frameRate'): - print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}") - return False - - if not config.has_option('Parameter', 'tspRangeThreshold'): - print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}") - return False - - frameRate = config.getfloat('Parameter', 'frameRate') - if frameRate <= 0 or frameRate >= 1000: - print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}") - return False - - tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold') - if tspRangeThreshold <= 0: - print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}") - return False - - return True - -def getDeviceCount(): - deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" - if not os.path.exists(deviceInfoPath): - print(f"Get device count failed. {deviceInfoPath} not exists") - return 0 - - with open(deviceInfoPath, 'r') as f: - data = json.load(f) - - if not 'devices' in data: - print(f"Get device count failed. {deviceInfoPath} not contain item 'devices'") - return 0 - - return len(data['devices']) - -def initProfileInfoDict(): - global streamProfileDict - - streamProfileDict = dict() - profilePath = f"{sourceRootPath}/StreamProfileInfo.txt" - if not os.path.exists(profilePath): - print(f"Initialize profile information dictionary failed. {profilePath} not exists") - return False - - with open(profilePath, 'r') as f: - data = json.load(f) - - if not 'streamProfiles' in data: - print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'") - return False - - for item in data['streamProfiles']: - if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item: - profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps']) - streamProfileDict.setdefault(item['sensorType'], profileInfo) - else: - print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}") - - return len(streamProfileDict.items()) > 0 - -def initResultDir(): - global resultPath - timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S") - resultPath = f"{sourceRootPath}/results-{timeText}" - if not os.path.exists(resultPath): - os.makedirs(resultPath) - -def isFrameFile(fileName): - fileExt = os.path.splitext(fileName) - if len(fileExt) < 2: - return False - - frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"} - return fileExt[1] in frameExts - -def initPictureInfoDictionary(rootFilePath, pictureInfoDict): - frameFileCount = 0 - for dirpath, dirnames, filenames in os.walk(rootFilePath): - # print(f"dirpath={dirpath}, dirpath.basename=" + os.path.basename(dirpath)) - for fileName in filenames: - filePath = os.path.join(dirpath, fileName) - if not isFrameFile(fileName): - continue - - frameFileCount += 1 - fileNameNoExt = os.path.splitext(fileName)[0] - pictureNameList = fileNameNoExt.split('_') - deviceId = pictureNameList[2][5:] - sensorType = pictureNameList[0].replace('#', '_') - syncTimeStamp = int(pictureNameList[3][1:]) - - # Create structure - pictureInfo = PictureInfo() - info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1]) - - # Format dictionary: one key corresponds to one array - pictureInfoDict.setdefault(deviceId, []).append(info) - - if 0 == frameFileCount: - print("initialize pictureInfoDict failed. Not found frame file") - - -def matchFrame(pictureInfoDict): - global primaryId, frameRate - global streamProfileDict - - # Get comparison image array, take the Color array of the first device for comparison - compareList = [] - for key in pictureInfoDict: - listTmp = list(pictureInfoDict[key]) - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == 'color': - compareList.append(info) - - # Dynamically analyze based on stream status to reduce loop iterations - hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict - hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict - hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict - hasIRLeftProfile = 'OB_SENSOR_IR_LEFT' in streamProfileDict - hasIRRightProfile = 'OB_SENSOR_IR_RIGHT' in streamProfileDict - - # Match adjacent frames - resultDict = {} - consumedList = [] - dictIndex = 0 - frameTspHalfGap = int(1000.0/frameRate/2.0+0.5) - for comparePic in compareList: - resultDict.setdefault(dictIndex, []).append(comparePic) - for key in pictureInfoDict: - listTmp = list(pictureInfoDict[key]) - # Compare color - if hasColorProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'color' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare depth - if hasDepthProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'depth' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare IR - if hasIRProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare left IR - if hasIRLeftProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir_left' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare right IR - if hasIRRightProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir_right' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Calculate deviation - minTsp = min(resultDict[dictIndex], key=lambda x: x.syncTimeStamp) - for info in resultDict[dictIndex]: - info.tspDiff = info.syncTimeStamp - minTsp.syncTimeStamp - - dictIndex += 1 - - return resultDict - - -def safe_make_dir(path): - if not os.path.exists(path): - os.makedirs(path) - - -def handleSyncFrames(): - global resultPath - global sourceFramesPath - global streamProfileDict - - fp = open(f"{resultPath}/frameMatchLog.txt", "w") - - if not initProfileInfoDict(): - print("Initialize stream profile dictionary failed. exit") - return - print(f"Stream profiles: {streamProfileDict}") - streamProfileCount = len(streamProfileDict.items()) - - pictureInfoDict = {} - - initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict) - if len(pictureInfoDict) <= 0: - print("init pictureInfoDict failed. pictureInfoDict.len = 0") - return - - # # Dump pictureInfoDict - # for key in pictureInfoDict: - # print("===================key:%s" % key) - # listTmp = list(pictureInfoDict[key]) - # for info in listTmp: - # print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath)) - - deviceCount = getDeviceCount() - print("=================device count:%d" % deviceCount) - - resultDict = {} - resultDict = matchFrame(pictureInfoDict) - - for key in resultDict: - fp.write("===================result key:%s\n" % key) - print("===================result key:%s" % key) - listTmp = list(resultDict[key]) - for info in listTmp: - fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % ( - info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) - print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % ( - info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) - - matchPath = f"{resultPath}/matchFrames/" - safe_make_dir(matchPath) - notMatchPath = f"{resultPath}/notMatchFrames/" - abnormalPath = f"{resultPath}/abnormal/" - - for key in resultDict: - listTmp = list(resultDict[key]) - if (len(listTmp) == (deviceCount * streamProfileCount)): - haveAbnormalData = False - for info in listTmp: - if info.tspDiff >= tspRangeThreshold: - path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt - shutil.copy(info.picturePath, path) - haveAbnormalData = True - else: - path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt - shutil.copy(info.picturePath, path) - - if haveAbnormalData: - safe_make_dir(abnormalPath) - for info in listTmp: - if info.tspDiff >= tspRangeThreshold: - path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt - shutil.copy(info.picturePath, path) - else: - path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt - shutil.copy(info.picturePath, path) - else: - safe_make_dir(notMatchPath) - for info in listTmp: - newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt - shutil.copy(info.picturePath, newFilePath) - -def main(): - if not initPrimaryId(): - print("Initialize primaryId failed. exit.") - return - - if not initSyncConfigParams(): - print("Initialized Sync config parameter failed. exit.") - return - - initResultDir() - - print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}") - handleSyncFrames() - - return - - -if __name__ == '__main__': - parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Gemini 2 VL'") - parser.add_argument('frames_dir', type=str, help="Frames directory") - parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory") - args = parser.parse_args() - - sourceRootPath = args.frames_dir - sourceFramesPath = f"{args.frames_dir}/TotalModeFrames" - mainPythonWorkPath = args.main_script_root_dir - print(f"Start of Gemini2VL synchronize frame. sourceRootPath={sourceRootPath}") - - main() - - print("Finish of Gemini2VL synchronize frame.") +import os.path +import argparse +import sys +import json +import shutil +import configparser + +from pathlib import Path +from datetime import datetime + +## Config.ini configuration +# Unit: FPS. Please fill in according to the actual dataset frame rate, +# otherwise it will cause inaccurate matching. +frameRate = -1 +# Unit: ms. When the timestamp range difference of a matched frame group +# is greater than or equal to tspRangeThreshold, the file name will be marked. +tspRangeThreshold = -1 + +## Parsed automatically by Python script, DO NOT modify +primaryId = '-1' # Host ID +mainPythonWorkPath = "" # Working directory of SyncFramesMain.py +sourceFramesPath = "" # DO NOT modify, fixed value, path to TotalMode directory +sourceRootPath = "" # Root directory containing TotalMode +resultPath = "" # Path to save sync analysis results +streamProfileDict = dict() # Store Python results + +class PictureInfo(object): + class Struct(object): + def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): + self.deviceId = deviceId + self.sensorType = sensorType + self.syncTimeStamp = syncTimeStamp + self.picturePath = picturePath + self.deviceIdFull = deviceIdFull + self.fileExt = fileExt + + def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt): + return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt) + +class StreamProfileInfo: + def __init__(self, sensorType, width, height, format, fps): + self.sensorType = sensorType + self.width = width + self.height = height + self.format = format + self.fps = fps + +def initPrimaryId(): + global primaryId + + deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" + if not os.path.exists(deviceInfoPath): + print(f"Initialize primaryId failed. {deviceInfoPath} not exists") + return False + + with open(deviceInfoPath, 'r') as f: + data = json.load(f) + + if not 'devices' in data: + print(f"Initialize primaryId failed. {deviceInfoPath} not contain item 'devices'") + return False + + for item in data['devices']: + if 'isPrimaryDevice' in item and 'index' in item and item['isPrimaryDevice']: + primaryId = str(item['index']) + return True + + return False + +def initSyncConfigParams(): + global frameRate, tspRangeThreshold + + configPath = f"{mainPythonWorkPath}/Config.ini" + if not os.path.exists(configPath): + print(f"Initliaze synchronized config parameter failed. {configPath} not exists.") + return False + + # Create ConfigParser object + config = configparser.ConfigParser() + + # Read ini file + config.read(configPath, 'utf-8') + + if not config.has_option('Parameter', 'frameRate'): + print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}") + return False + + if not config.has_option('Parameter', 'tspRangeThreshold'): + print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}") + return False + + frameRate = config.getfloat('Parameter', 'frameRate') + if frameRate <= 0 or frameRate >= 1000: + print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}") + return False + + tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold') + if tspRangeThreshold <= 0: + print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}") + return False + + return True + +def getDeviceCount(): + deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" + if not os.path.exists(deviceInfoPath): + print(f"Get device count failed. {deviceInfoPath} not exists") + return 0 + + with open(deviceInfoPath, 'r') as f: + data = json.load(f) + + if not 'devices' in data: + print(f"Get device count failed. {deviceInfoPath} not contain item 'devices'") + return 0 + + return len(data['devices']) + +def initProfileInfoDict(): + global streamProfileDict + + streamProfileDict = dict() + profilePath = f"{sourceRootPath}/StreamProfileInfo.txt" + if not os.path.exists(profilePath): + print(f"Initialize profile information dictionary failed. {profilePath} not exists") + return False + + with open(profilePath, 'r') as f: + data = json.load(f) + + if not 'streamProfiles' in data: + print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'") + return False + + for item in data['streamProfiles']: + if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item: + profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps']) + streamProfileDict.setdefault(item['sensorType'], profileInfo) + else: + print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}") + + return len(streamProfileDict.items()) > 0 + +def initResultDir(): + global resultPath + timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S") + resultPath = f"{sourceRootPath}/results-{timeText}" + if not os.path.exists(resultPath): + os.makedirs(resultPath) + +def isFrameFile(fileName): + fileExt = os.path.splitext(fileName) + if len(fileExt) < 2: + return False + + frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"} + return fileExt[1] in frameExts + +def initPictureInfoDictionary(rootFilePath, pictureInfoDict): + frameFileCount = 0 + for dirpath, dirnames, filenames in os.walk(rootFilePath): + # print(f"dirpath={dirpath}, dirpath.basename=" + os.path.basename(dirpath)) + for fileName in filenames: + filePath = os.path.join(dirpath, fileName) + if not isFrameFile(fileName): + continue + + frameFileCount += 1 + fileNameNoExt = os.path.splitext(fileName)[0] + pictureNameList = fileNameNoExt.split('_') + deviceId = pictureNameList[2][5:] + sensorType = pictureNameList[0].replace('#', '_') + syncTimeStamp = int(pictureNameList[3][1:]) + + # Create structure + pictureInfo = PictureInfo() + info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1]) + + # Format dictionary: one key corresponds to one array + pictureInfoDict.setdefault(deviceId, []).append(info) + + if 0 == frameFileCount: + print("initialize pictureInfoDict failed. Not found frame file") + + +def matchFrame(pictureInfoDict): + global primaryId, frameRate + global streamProfileDict + + # Get comparison image array, take the Color array of the first device for comparison + compareList = [] + for key in pictureInfoDict: + listTmp = list(pictureInfoDict[key]) + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == 'color': + compareList.append(info) + + # Dynamically analyze based on stream status to reduce loop iterations + hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict + hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict + hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict + hasIRLeftProfile = 'OB_SENSOR_IR_LEFT' in streamProfileDict + hasIRRightProfile = 'OB_SENSOR_IR_RIGHT' in streamProfileDict + + # Match adjacent frames + resultDict = {} + consumedList = [] + dictIndex = 0 + frameTspHalfGap = int(1000.0/frameRate/2.0+0.5) + for comparePic in compareList: + resultDict.setdefault(dictIndex, []).append(comparePic) + for key in pictureInfoDict: + listTmp = list(pictureInfoDict[key]) + # Compare color + if hasColorProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'color' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare depth + if hasDepthProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'depth' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare IR + if hasIRProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare left IR + if hasIRLeftProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir_left' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare right IR + if hasIRRightProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir_right' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Calculate deviation + minTsp = min(resultDict[dictIndex], key=lambda x: x.syncTimeStamp) + for info in resultDict[dictIndex]: + info.tspDiff = info.syncTimeStamp - minTsp.syncTimeStamp + + dictIndex += 1 + + return resultDict + + +def safe_make_dir(path): + if not os.path.exists(path): + os.makedirs(path) + + +def handleSyncFrames(): + global resultPath + global sourceFramesPath + global streamProfileDict + + fp = open(f"{resultPath}/frameMatchLog.txt", "w") + + if not initProfileInfoDict(): + print("Initialize stream profile dictionary failed. exit") + return + print(f"Stream profiles: {streamProfileDict}") + streamProfileCount = len(streamProfileDict.items()) + + pictureInfoDict = {} + + initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict) + if len(pictureInfoDict) <= 0: + print("init pictureInfoDict failed. pictureInfoDict.len = 0") + return + + # # Dump pictureInfoDict + # for key in pictureInfoDict: + # print("===================key:%s" % key) + # listTmp = list(pictureInfoDict[key]) + # for info in listTmp: + # print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath)) + + deviceCount = getDeviceCount() + print("=================device count:%d" % deviceCount) + + resultDict = {} + resultDict = matchFrame(pictureInfoDict) + + for key in resultDict: + fp.write("===================result key:%s\n" % key) + print("===================result key:%s" % key) + listTmp = list(resultDict[key]) + for info in listTmp: + fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % ( + info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) + print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % ( + info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) + + matchPath = f"{resultPath}/matchFrames/" + safe_make_dir(matchPath) + notMatchPath = f"{resultPath}/notMatchFrames/" + abnormalPath = f"{resultPath}/abnormal/" + + for key in resultDict: + listTmp = list(resultDict[key]) + if (len(listTmp) == (deviceCount * streamProfileCount)): + haveAbnormalData = False + for info in listTmp: + if info.tspDiff >= tspRangeThreshold: + path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt + shutil.copy(info.picturePath, path) + haveAbnormalData = True + else: + path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt + shutil.copy(info.picturePath, path) + + if haveAbnormalData: + safe_make_dir(abnormalPath) + for info in listTmp: + if info.tspDiff >= tspRangeThreshold: + path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + info.fileExt + shutil.copy(info.picturePath, path) + else: + path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt + shutil.copy(info.picturePath, path) + else: + safe_make_dir(notMatchPath) + for info in listTmp: + newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + info.fileExt + shutil.copy(info.picturePath, newFilePath) + +def main(): + if not initPrimaryId(): + print("Initialize primaryId failed. exit.") + return + + if not initSyncConfigParams(): + print("Initialized Sync config parameter failed. exit.") + return + + initResultDir() + + print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}") + handleSyncFrames() + + return + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Gemini 2 VL'") + parser.add_argument('frames_dir', type=str, help="Frames directory") + parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory") + args = parser.parse_args() + + sourceRootPath = args.frames_dir + sourceFramesPath = f"{args.frames_dir}/TotalModeFrames" + mainPythonWorkPath = args.main_script_root_dir + print(f"Start of Gemini2VL synchronize frame. sourceRootPath={sourceRootPath}") + + main() + + print("Finish of Gemini2VL synchronize frame.") diff --git a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch_g330.py b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch_g330.py index 345cb9c5..400fac46 100644 --- a/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch_g330.py +++ b/orbbec_camera/examples/multi_camera_synced_verification_tool/multicamera_sync/Python/script/config_frameMatch_g330.py @@ -1,426 +1,426 @@ -import os.path -import argparse -import sys -import json -import shutil -import configparser - -from pathlib import Path -from datetime import datetime - -import os -import re - -## Config.ini configuration -# Unit: FPS. Please fill in according to the actual frame rate of the dataset, otherwise the matching will be inaccurate. -frameRate = -1 -# Unit: ms. When the timestamp range of a matched group of data frames is greater than or equal to tspRangeThreshold, the filename will be marked. -tspRangeThreshold = -1 - -## Automatically parsed by Python script, do not modify -primaryId = '-1' # Primary device ID -mainPythonWorkPath = "" # Working directory of SyncFramesMain.py -sourceFramesPath = "" # Do not modify, fixed value, path to TotalMode directory -sourceRootPath = "" # Root directory where TotalMode is stored -resultPath = "" # Save sync analysis results -streamProfileDict = dict() # Save Python results - -class PictureInfo(object): - class Struct(object): - def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt, exp, gain): - self.deviceId = deviceId - self.sensorType = sensorType - self.syncTimeStamp = syncTimeStamp - self.picturePath = picturePath - self.deviceIdFull = deviceIdFull - self.fileExt = fileExt - self.exp = exp - self.gain = gain - - def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt, exp, gain): - return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt, exp, gain) - -class StreamProfileInfo: - def __init__(self, sensorType, width, height, format, fps): - self.sensorType = sensorType - self.width = width - self.height = height - self.format = format - self.fps = fps - -def initPrimaryId(): - global primaryId - - deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" - if not os.path.exists(deviceInfoPath): - print(f"Initialize primaryId failed. {deviceInfoPath} not exists") - return False - - with open(deviceInfoPath, 'r') as f: - data = json.load(f) - - if not 'devices' in data: - print(f"Initialize primaryId failed. {deviceInfoPath} not contain item 'devices'") - return False - - for item in data['devices']: - if 'isPrimaryDevice' in item and 'index' in item and item['isPrimaryDevice']: - primaryId = str(item['index']) - return True - - return False - -def initSyncConfigParams(): - global frameRate, tspRangeThreshold - - configPath = f"{mainPythonWorkPath}/Config.ini" - if not os.path.exists(configPath): - print(f"Initliaze synchronized config parameter failed. {configPath} not exists.") - return False - - # Create ConfigParser object - config = configparser.ConfigParser() - - # Read ini file - config.read(configPath, 'utf-8') - - if not config.has_option('Parameter', 'frameRate'): - print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}") - return False - - if not config.has_option('Parameter', 'tspRangeThreshold'): - print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}") - return False - - frameRate = config.getfloat('Parameter', 'frameRate') - if frameRate <= 0 or frameRate >= 1000: - print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}") - return False - - tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold') - if tspRangeThreshold <= 0: - print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}") - return False - - return True - -def getDeviceCount(): - deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" - if not os.path.exists(deviceInfoPath): - print(f"Get device count failed. {deviceInfoPath} not exists") - return 0 - - with open(deviceInfoPath, 'r') as f: - data = json.load(f) - - if not 'devices' in data: - print(f"Get device count failed. {deviceInfoPath} not contain item 'devices'") - return 0 - - return len(data['devices']) - -def initProfileInfoDict(): - global streamProfileDict - - streamProfileDict = dict() - profilePath = f"{sourceRootPath}/StreamProfileInfo.txt" - if not os.path.exists(profilePath): - print(f"Initialize profile information dictionary failed. {profilePath} not exists") - return False - - with open(profilePath, 'r') as f: - data = json.load(f) - - if not 'streamProfiles' in data: - print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'") - return False - - for item in data['streamProfiles']: - if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item: - profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps']) - streamProfileDict.setdefault(item['sensorType'], profileInfo) - else: - print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}") - - return len(streamProfileDict.items()) > 0 - -def initResultDir(): - global resultPath - timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S") - resultPath = f"{sourceRootPath}/results-{timeText}" - if not os.path.exists(resultPath): - os.makedirs(resultPath) - -def isFrameFile(fileName): - fileExt = os.path.splitext(fileName) - if len(fileExt) < 2: - return False - - frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"} - return fileExt[1] in frameExts - -def extract_number(filename): - match_d = re.search(r'_d(\d+)', filename) - if match_d: - return int(match_d.group(1)) - - match_g = re.search(r'_g(\d+)', filename) - if match_g: - return int(match_g.group(1)) - - return float('inf') - -def initPictureInfoDictionary(rootFilePath, pictureInfoDict): - frameFileCount = 0 - for dirpath, dirnames, filenames in os.walk(rootFilePath): - filenames.sort(key=extract_number) # Sort files by numbers following '_d' and '_g' in filename, ascending - for fileName in filenames: - filePath = os.path.join(dirpath, fileName) - if not isFrameFile(fileName): - continue - - frameFileCount += 1 - fileNameNoExt = os.path.splitext(fileName)[0] - pictureNameList = fileNameNoExt.split('_') - deviceId = pictureNameList[2][5:] - sensorType = pictureNameList[0].replace('#', '_') - syncTimeStamp = int(pictureNameList[3][1:]) - exp = str(pictureNameList[6][0:]) - gain = str(pictureNameList[7][0:]) - - # Create structure - pictureInfo = PictureInfo() - info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1],exp,gain) - - # Format dictionary: one key corresponds to one array - pictureInfoDict.setdefault(deviceId, []).append(info) - - if 0 == frameFileCount: - print("initialize pictureInfoDict failed. Not found frame file") - - -def matchFrame(pictureInfoDict): - global primaryId, frameRate - global streamProfileDict - - # Get comparison image array, use Color array of the first device for comparison - compareList = [] - for key in pictureInfoDict: - listTmp = list(pictureInfoDict[key]) - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == 'color': - compareList.append(info) - - # Dynamically analyze according to stream situation to reduce loop count - hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict - hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict - hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict - hasIRLeftProfile = 'OB_SENSOR_IR_LEFT' in streamProfileDict - hasIRRightProfile = 'OB_SENSOR_IR_RIGHT' in streamProfileDict - - # Match adjacent frames - resultDict = {} - consumedList = [] - dictIndex = 0 - frameTspHalfGap = int(1000.0/frameRate/2.0+0.5) - for comparePic in compareList: - resultDict.setdefault(dictIndex, []).append(comparePic) - for key in pictureInfoDict: - listTmp = list(pictureInfoDict[key]) - # Compare color - if hasColorProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'color' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare depth - if hasDepthProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'depth' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare IR - if hasIRProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare IR Left - if hasIRLeftProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir_left' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Compare IR Right - if hasIRRightProfile: - for info in listTmp: - if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: - continue - - if info in consumedList: - continue - - if info.sensorType == 'ir_right' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: - resultDict.setdefault(dictIndex, []).append(info) - consumedList.append(info) - break - - # Calculate deviation - minTsp = min(resultDict[dictIndex], key=lambda x: x.syncTimeStamp) - for info in resultDict[dictIndex]: - info.tspDiff = info.syncTimeStamp - minTsp.syncTimeStamp - - dictIndex += 1 - - return resultDict - - -def safe_make_dir(path): - if not os.path.exists(path): - os.makedirs(path) - - -def handleSyncFrames(): - global resultPath - global sourceFramesPath - global streamProfileDict - - fp = open(f"{resultPath}/frameMatchLog.txt", "w") - - if not initProfileInfoDict(): - print("Initialize stream profile dictionary failed. exit") - return - print(f"Stream profiles: {streamProfileDict}") - streamProfileCount = len(streamProfileDict.items()) - - pictureInfoDict = {} - - initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict) - if len(pictureInfoDict) <= 0: - print("init pictureInfoDict failed. pictureInfoDict.len = 0") - return - - # # Dump pictureInfoDict - # for key in pictureInfoDict: - # print("===================key:%s" % key) - # listTmp = list(pictureInfoDict[key]) - # for info in listTmp: - # print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath)) - - deviceCount = getDeviceCount() - print("=================device count:%d" % deviceCount) - - resultDict = {} - resultDict = matchFrame(pictureInfoDict) - - for key in resultDict: - fp.write("===================result key:%s\n" % key) - print("===================result key:%s" % key) - listTmp = list(resultDict[key]) - for info in listTmp: - fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % ( - info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) - print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % ( - info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) - - matchPath = f"{resultPath}/matchFrames/" - safe_make_dir(matchPath) - notMatchPath = f"{resultPath}/notMatchFrames/" - abnormalPath = f"{resultPath}/abnormal/" - - for key in resultDict: - listTmp = list(resultDict[key]) - if (len(listTmp) == (deviceCount * streamProfileCount)): - haveAbnormalData = False - for info in listTmp: - if info.tspDiff >= tspRangeThreshold: - path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt - shutil.copy(info.picturePath, path) - haveAbnormalData = True - else: - path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt - shutil.copy(info.picturePath, path) - - if haveAbnormalData: - safe_make_dir(abnormalPath) - for info in listTmp: - if info.tspDiff >= tspRangeThreshold: - path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt - shutil.copy(info.picturePath, path) - else: - path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt - shutil.copy(info.picturePath, path) - else: - safe_make_dir(notMatchPath) - for info in listTmp: - newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( - info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt - shutil.copy(info.picturePath, newFilePath) - -def main(): - if not initPrimaryId(): - print("Initialize primaryId failed. exit.") - return - - if not initSyncConfigParams(): - print("Initialized Sync config parameter failed. exit.") - return - - initResultDir() - - print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}") - handleSyncFrames() - - return - - -if __name__ == '__main__': - parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Devices'") - parser.add_argument('frames_dir', type=str, help="Frames directory") - parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory") - args = parser.parse_args() - - sourceRootPath = args.frames_dir - sourceFramesPath = f"{args.frames_dir}/TotalModeFrames" - mainPythonWorkPath = args.main_script_root_dir - print(f"Start of CommonFrameMatch synchronize frame. sourceRootPath={sourceRootPath}") - - main() - - print("Finish of CommonFrameMatch synchronize frame.") +import os.path +import argparse +import sys +import json +import shutil +import configparser + +from pathlib import Path +from datetime import datetime + +import os +import re + +## Config.ini configuration +# Unit: FPS. Please fill in according to the actual frame rate of the dataset, otherwise the matching will be inaccurate. +frameRate = -1 +# Unit: ms. When the timestamp range of a matched group of data frames is greater than or equal to tspRangeThreshold, the filename will be marked. +tspRangeThreshold = -1 + +## Automatically parsed by Python script, do not modify +primaryId = '-1' # Primary device ID +mainPythonWorkPath = "" # Working directory of SyncFramesMain.py +sourceFramesPath = "" # Do not modify, fixed value, path to TotalMode directory +sourceRootPath = "" # Root directory where TotalMode is stored +resultPath = "" # Save sync analysis results +streamProfileDict = dict() # Save Python results + +class PictureInfo(object): + class Struct(object): + def __init__(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt, exp, gain): + self.deviceId = deviceId + self.sensorType = sensorType + self.syncTimeStamp = syncTimeStamp + self.picturePath = picturePath + self.deviceIdFull = deviceIdFull + self.fileExt = fileExt + self.exp = exp + self.gain = gain + + def make_struct(self, deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt, exp, gain): + return self.Struct(deviceId, sensorType, syncTimeStamp, picturePath, deviceIdFull, fileExt, exp, gain) + +class StreamProfileInfo: + def __init__(self, sensorType, width, height, format, fps): + self.sensorType = sensorType + self.width = width + self.height = height + self.format = format + self.fps = fps + +def initPrimaryId(): + global primaryId + + deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" + if not os.path.exists(deviceInfoPath): + print(f"Initialize primaryId failed. {deviceInfoPath} not exists") + return False + + with open(deviceInfoPath, 'r') as f: + data = json.load(f) + + if not 'devices' in data: + print(f"Initialize primaryId failed. {deviceInfoPath} not contain item 'devices'") + return False + + for item in data['devices']: + if 'isPrimaryDevice' in item and 'index' in item and item['isPrimaryDevice']: + primaryId = str(item['index']) + return True + + return False + +def initSyncConfigParams(): + global frameRate, tspRangeThreshold + + configPath = f"{mainPythonWorkPath}/Config.ini" + if not os.path.exists(configPath): + print(f"Initliaze synchronized config parameter failed. {configPath} not exists.") + return False + + # Create ConfigParser object + config = configparser.ConfigParser() + + # Read ini file + config.read(configPath, 'utf-8') + + if not config.has_option('Parameter', 'frameRate'): + print(f"Initliaze synchronized config parameter failed. Not define 'frameRate' in ${configPath}") + return False + + if not config.has_option('Parameter', 'tspRangeThreshold'): + print(f"Initliaze synchronized config parameter failed. Not define 'tspRangeThreshold' in ${configPath}") + return False + + frameRate = config.getfloat('Parameter', 'frameRate') + if frameRate <= 0 or frameRate >= 1000: + print(f"Initliaze synchronized config parameter failed. Invalid frameRate={frameRate}") + return False + + tspRangeThreshold = config.getfloat('Parameter', 'tspRangeThreshold') + if tspRangeThreshold <= 0: + print(f"Initliaze synchronized config parameter failed. Invalid tspRangeThreshold={tspRangeThreshold}") + return False + + return True + +def getDeviceCount(): + deviceInfoPath = f"{sourceRootPath}/DevicesInfo.txt" + if not os.path.exists(deviceInfoPath): + print(f"Get device count failed. {deviceInfoPath} not exists") + return 0 + + with open(deviceInfoPath, 'r') as f: + data = json.load(f) + + if not 'devices' in data: + print(f"Get device count failed. {deviceInfoPath} not contain item 'devices'") + return 0 + + return len(data['devices']) + +def initProfileInfoDict(): + global streamProfileDict + + streamProfileDict = dict() + profilePath = f"{sourceRootPath}/StreamProfileInfo.txt" + if not os.path.exists(profilePath): + print(f"Initialize profile information dictionary failed. {profilePath} not exists") + return False + + with open(profilePath, 'r') as f: + data = json.load(f) + + if not 'streamProfiles' in data: + print(f"Initialize profile information dictionary failed. {profilePath} not contain item 'streamProfiles'") + return False + + for item in data['streamProfiles']: + if 'sensorType' in item and 'width' in item and 'height' in item and 'format' in item and 'fps' in item: + profileInfo = StreamProfileInfo(item['sensorType'], item['width'], item['height'], item['format'], item['fps']) + streamProfileDict.setdefault(item['sensorType'], profileInfo) + else: + print(f"Initialize profile information dictionary. Error invalid StreamProfile, {item}") + + return len(streamProfileDict.items()) > 0 + +def initResultDir(): + global resultPath + timeText = datetime.now().strftime("%Y-%m-%d_%H%M%S") + resultPath = f"{sourceRootPath}/results-{timeText}" + if not os.path.exists(resultPath): + os.makedirs(resultPath) + +def isFrameFile(fileName): + fileExt = os.path.splitext(fileName) + if len(fileExt) < 2: + return False + + frameExts = {".jpeg", ".jpg", ".png", ".raw", "bmp"} + return fileExt[1] in frameExts + +def extract_number(filename): + match_d = re.search(r'_d(\d+)', filename) + if match_d: + return int(match_d.group(1)) + + match_g = re.search(r'_g(\d+)', filename) + if match_g: + return int(match_g.group(1)) + + return float('inf') + +def initPictureInfoDictionary(rootFilePath, pictureInfoDict): + frameFileCount = 0 + for dirpath, dirnames, filenames in os.walk(rootFilePath): + filenames.sort(key=extract_number) # Sort files by numbers following '_d' and '_g' in filename, ascending + for fileName in filenames: + filePath = os.path.join(dirpath, fileName) + if not isFrameFile(fileName): + continue + + frameFileCount += 1 + fileNameNoExt = os.path.splitext(fileName)[0] + pictureNameList = fileNameNoExt.split('_') + deviceId = pictureNameList[2][5:] + sensorType = pictureNameList[0].replace('#', '_') + syncTimeStamp = int(pictureNameList[3][1:]) + exp = str(pictureNameList[6][0:]) + gain = str(pictureNameList[7][0:]) + + # Create structure + pictureInfo = PictureInfo() + info = pictureInfo.make_struct(deviceId, sensorType, syncTimeStamp, filePath, os.path.basename(dirpath), os.path.splitext(fileName)[1],exp,gain) + + # Format dictionary: one key corresponds to one array + pictureInfoDict.setdefault(deviceId, []).append(info) + + if 0 == frameFileCount: + print("initialize pictureInfoDict failed. Not found frame file") + + +def matchFrame(pictureInfoDict): + global primaryId, frameRate + global streamProfileDict + + # Get comparison image array, use Color array of the first device for comparison + compareList = [] + for key in pictureInfoDict: + listTmp = list(pictureInfoDict[key]) + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == 'color': + compareList.append(info) + + # Dynamically analyze according to stream situation to reduce loop count + hasColorProfile = 'OB_SENSOR_COLOR' in streamProfileDict + hasDepthProfile = 'OB_SENSOR_DEPTH' in streamProfileDict + hasIRProfile = 'OB_SENSOR_IR' in streamProfileDict + hasIRLeftProfile = 'OB_SENSOR_IR_LEFT' in streamProfileDict + hasIRRightProfile = 'OB_SENSOR_IR_RIGHT' in streamProfileDict + + # Match adjacent frames + resultDict = {} + consumedList = [] + dictIndex = 0 + frameTspHalfGap = int(1000.0/frameRate/2.0+0.5) + for comparePic in compareList: + resultDict.setdefault(dictIndex, []).append(comparePic) + for key in pictureInfoDict: + listTmp = list(pictureInfoDict[key]) + # Compare color + if hasColorProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'color' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare depth + if hasDepthProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'depth' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare IR + if hasIRProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare IR Left + if hasIRLeftProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir_left' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Compare IR Right + if hasIRRightProfile: + for info in listTmp: + if info.deviceId == primaryId and info.sensorType == comparePic.sensorType: + continue + + if info in consumedList: + continue + + if info.sensorType == 'ir_right' and abs(info.syncTimeStamp - comparePic.syncTimeStamp) < frameTspHalfGap: + resultDict.setdefault(dictIndex, []).append(info) + consumedList.append(info) + break + + # Calculate deviation + minTsp = min(resultDict[dictIndex], key=lambda x: x.syncTimeStamp) + for info in resultDict[dictIndex]: + info.tspDiff = info.syncTimeStamp - minTsp.syncTimeStamp + + dictIndex += 1 + + return resultDict + + +def safe_make_dir(path): + if not os.path.exists(path): + os.makedirs(path) + + +def handleSyncFrames(): + global resultPath + global sourceFramesPath + global streamProfileDict + + fp = open(f"{resultPath}/frameMatchLog.txt", "w") + + if not initProfileInfoDict(): + print("Initialize stream profile dictionary failed. exit") + return + print(f"Stream profiles: {streamProfileDict}") + streamProfileCount = len(streamProfileDict.items()) + + pictureInfoDict = {} + + initPictureInfoDictionary(Path(sourceFramesPath), pictureInfoDict) + if len(pictureInfoDict) <= 0: + print("init pictureInfoDict failed. pictureInfoDict.len = 0") + return + + # # Dump pictureInfoDict + # for key in pictureInfoDict: + # print("===================key:%s" % key) + # listTmp = list(pictureInfoDict[key]) + # for info in listTmp: + # print("=====================value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % (info.deviceId,info.sensorType,info.syncTimeStamp,info.picturePath)) + + deviceCount = getDeviceCount() + print("=================device count:%d" % deviceCount) + + resultDict = {} + resultDict = matchFrame(pictureInfoDict) + + for key in resultDict: + fp.write("===================result key:%s\n" % key) + print("===================result key:%s" % key) + listTmp = list(resultDict[key]) + for info in listTmp: + fp.write("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s\n" % ( + info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) + print("=====================result value : deviceId:%s\tsensorType:%s\tsyncTimeStamp:%s\tpicturePath:%s" % ( + info.deviceId, info.sensorType, info.syncTimeStamp, info.picturePath)) + + matchPath = f"{resultPath}/matchFrames/" + safe_make_dir(matchPath) + notMatchPath = f"{resultPath}/notMatchFrames/" + abnormalPath = f"{resultPath}/abnormal/" + + for key in resultDict: + listTmp = list(resultDict[key]) + if (len(listTmp) == (deviceCount * streamProfileCount)): + haveAbnormalData = False + for info in listTmp: + if info.tspDiff >= tspRangeThreshold: + path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt + shutil.copy(info.picturePath, path) + haveAbnormalData = True + else: + path = matchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt + shutil.copy(info.picturePath, path) + + if haveAbnormalData: + safe_make_dir(abnormalPath) + for info in listTmp: + if info.tspDiff >= tspRangeThreshold: + path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_xxxxxx" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt + shutil.copy(info.picturePath, path) + else: + path = abnormalPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt + shutil.copy(info.picturePath, path) + else: + safe_make_dir(notMatchPath) + for info in listTmp: + newFilePath = notMatchPath + str(key) + "_" + info.sensorType + "_" + info.deviceIdFull + "_" + str( + info.syncTimeStamp) + "_[" + str(info.tspDiff) + "]" + "_"+ str(info.exp) + "_"+ str(info.gain) + info.fileExt + shutil.copy(info.picturePath, newFilePath) + +def main(): + if not initPrimaryId(): + print("Initialize primaryId failed. exit.") + return + + if not initSyncConfigParams(): + print("Initialized Sync config parameter failed. exit.") + return + + initResultDir() + + print(f"PrimaryId={primaryId}, frameRate={frameRate}, tspRangeThreshold={tspRangeThreshold}") + handleSyncFrames() + + return + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description="Synchronize frames support for 'Orbbec Devices'") + parser.add_argument('frames_dir', type=str, help="Frames directory") + parser.add_argument('main_script_root_dir', type=str, help="SyncFramesMain.py working directory") + args = parser.parse_args() + + sourceRootPath = args.frames_dir + sourceFramesPath = f"{args.frames_dir}/TotalModeFrames" + mainPythonWorkPath = args.main_script_root_dir + print(f"Start of CommonFrameMatch synchronize frame. sourceRootPath={sourceRootPath}") + + main() + + print("Finish of CommonFrameMatch synchronize frame.") diff --git a/orbbec_camera_msgs/msg/DeviceInfo.msg b/orbbec_camera_msgs/msg/DeviceInfo.msg index 4763a37a..4a035711 100644 --- a/orbbec_camera_msgs/msg/DeviceInfo.msg +++ b/orbbec_camera_msgs/msg/DeviceInfo.msg @@ -1,7 +1,7 @@ -std_msgs/Header header -string name -string serial_number -string firmware_version -string supported_min_sdk_version -string current_sdk_version -string hardware_version +std_msgs/Header header +string name +string serial_number +string firmware_version +string supported_min_sdk_version +string current_sdk_version +string hardware_version diff --git a/orbbec_camera_msgs/srv/GetCameraInfo.srv b/orbbec_camera_msgs/srv/GetCameraInfo.srv index b68fb771..64b5d0f0 100644 --- a/orbbec_camera_msgs/srv/GetCameraInfo.srv +++ b/orbbec_camera_msgs/srv/GetCameraInfo.srv @@ -1,2 +1,2 @@ ---- -sensor_msgs/CameraInfo info +--- +sensor_msgs/CameraInfo info diff --git a/orbbec_camera_msgs/srv/GetDeviceInfo.srv b/orbbec_camera_msgs/srv/GetDeviceInfo.srv index be08a553..32f711d2 100644 --- a/orbbec_camera_msgs/srv/GetDeviceInfo.srv +++ b/orbbec_camera_msgs/srv/GetDeviceInfo.srv @@ -1,4 +1,4 @@ ---- -DeviceInfo info -bool success +--- +DeviceInfo info +bool success string message \ No newline at end of file diff --git a/orbbec_description/urdf/astra_2.urdf.xacro b/orbbec_description/urdf/astra_2.urdf.xacro index d3a4a3f1..295b0bac 100755 --- a/orbbec_description/urdf/astra_2.urdf.xacro +++ b/orbbec_description/urdf/astra_2.urdf.xacro @@ -1,418 +1,418 @@ - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/orbbec_description/urdf/gemini_2.urdf.xacro b/orbbec_description/urdf/gemini_2.urdf.xacro index dab6f664..20125eca 100755 --- a/orbbec_description/urdf/gemini_2.urdf.xacro +++ b/orbbec_description/urdf/gemini_2.urdf.xacro @@ -1,471 +1,471 @@ - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/orbbec_description/urdf/gemini_2_L.urdf.xacro b/orbbec_description/urdf/gemini_2_L.urdf.xacro index 27064d92..4f9805bd 100644 --- a/orbbec_description/urdf/gemini_2_L.urdf.xacro +++ b/orbbec_description/urdf/gemini_2_L.urdf.xacro @@ -1,524 +1,524 @@ - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/orbbec_description/urdf/gemini_335_L_336_L.urdf.xacro b/orbbec_description/urdf/gemini_335_L_336_L.urdf.xacro index 676e76f2..43733811 100644 --- a/orbbec_description/urdf/gemini_335_L_336_L.urdf.xacro +++ b/orbbec_description/urdf/gemini_335_L_336_L.urdf.xacro @@ -1,524 +1,524 @@ - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +