出版社类网站模板软件免费下载网站有哪些

当前位置： 首页 > news >正文

出版社类网站模板,软件免费下载网站有哪些,一般做网站是在什么网站找素材,wordpress自适应 the7前言 因为我司「七月在线」关于dexcap的复现/优化接近尾声了#xff0c;故准备把dexcap的源码也分析下。​下周则分析下iDP3的源码——为队伍「iDP3人形的复现/优化」助力 最开始#xff0c;dexcap的源码分析属于此文《DexCap——斯坦福李飞飞团队泡茶机器人#xff1a;带…前言 因为我司「七月在线」关于dexcap的复现/优化接近尾声了故准备把dexcap的源码也分析下。​下周则分析下iDP3的源码——为队伍「iDP3人形的复现/优化」助力 最开始dexcap的源码分析属于此文《DexCap——斯坦福李飞飞团队泡茶机器人带灵巧手和动作捕捉的数据收集系统(含硬件清单)》的第三部分 然原理讲解、硬件配置、源码解析都放在一篇文章里的话篇幅会显得特别长故把源码的部分抽取出来独立成此文 第一部分 STEP1_collect_data 1.1 calculate_offset_vis_calib.py 这段代码是一个用于校准和保存偏移量、方向偏移量的脚本用于从指定目录中读取数据计算偏移量和方向偏移量并将结果保存到指定目录中以下是代码的部分解释 导入库 import argparse # 用于解析命令行参数 import numpy as np # 用于数值计算 import os # 用于操作系统相关功能 import sys # 用于系统相关功能 from scipy.spatial.transform import Rotation as R # 用于旋转矩阵和欧拉角转换 from transforms3d.euler import euler2mat # 用于将欧拉角转换为旋转矩阵 主函数 if name main:parser argparse.ArgumentParser(descriptioncalibrate and save in dataset folder) # 创建命令行参数解析器parser.add_argument(–directory, typestr, default, helpDirectory with saved data) # 添加目录参数parser.add_argument(–default, typestr, defaultdefault_offset, helpDirectory with saved data) # 添加默认目录参数args parser.parse_args() # 解析命令行参数if os.path.exists({}/calib_offset.txt.format(args.directory)): # 检查目标目录中是否已经存在calib_offset.txt文件response (input(fcalib_offset.txt already exists. Do you want to override? (y/n): # 提示用户是否覆盖文件).strip().lower())if response ! y: # 如果用户选择不覆盖退出程序print(Exiting program without overriding the existing directory.)sys.exit() 读取默认偏移量和方向偏移量 default_offset np.loadtxt(os.path.join(args.default, calib_offset.txt)) # 读取默认偏移量default_ori np.loadtxt(os.path.join(args.default, calib_ori_offset.txt)) # 读取默认方向偏移量default_offset_left np.loadtxt(os.path.join(args.default, calib_offset_left.txt)) # 读取左手默认偏移量default_ori_left np.loadtxt(os.path.join(args.default, calib_ori_offset_left.txt)) # 读取左手默认方向偏移量default_ori_matrix euler2mat(*default_ori) # 将默认方向偏移量转换为旋转矩阵default_ori_matrix_left euler2mat(default_ori_left) # 将左手默认方向偏移量转换为旋转矩阵 提取偏移量和方向偏移量 frame_dirs os.listdir(./test_data) # 列出test_data目录中的所有文件list [] # 存储偏移量的列表list_ori [] # 存储方向偏移量的列表list_left [] # 存储左手偏移量的列表list_ori_left [] # 存储左手方向偏移量的列表for frame_dir in frame_dirs: #
1.2 data_recording.py——配置多个Realsense T265相机和一个L515相机 这段代码是一个用于记录和处理Realsense相机数据的Python脚本 顺带再回顾一下 DexCap团队设计了一个装载摄像机的背包「如图(a)、(b)所示为方便大家对照特把上图再贴一下如下」 在正前面它通过胸部摄像机支架的4个插槽集成了4个相机最顶部是一台Intel Realsense L515 RGB-D LiDAR摄像机顶部下面是3个Realsense T265鱼眼SLAM跟踪相机(分别为红、绿、蓝)用于在人类数据收集过程中捕捉观察结果 1.2.1 一系列定义比如保存帧数据的函数 它可以捕捉颜色图像、深度图像、姿态数据以及手部关节数据并将这些数据保存到指定的目录中 首先导入库 示例用法python data_recording.py -s –store_hand -o ./save_data_scenario_1 import argparse # 用于解析命令行参数 import copy # 用于复制对象 import numpy as np # 用于数值计算 import open3d as o3d # 用于3D数据处理 import os # 用于操作系统相关功能 import shutil # 用于文件操作 import sys # 用于系统相关功能 import pyrealsense2 as rs # 用于Realsense相机操作 import cv2 # 用于图像处理from enum import IntEnum # 用于定义枚举类型 from realsense_helper import get_profiles # 导入自定义的Realsense帮助函数 from transforms3d.quaternions import axangle2quat, qmult, quat2mat, mat2quat # 用于四元数操作 import redis # 用于Redis数据库操作 import concurrent.futures # 用于并发执行 from hyperparameters import # 导入超参数 其次定义一个枚举类型 Preset用于表示不同的预设配置 class Preset(IntEnum):Custom 0Default 1Hand 2HighAccuracy 3HighDensity 4MediumDensity 5 然后保存帧数据的函数 def save_frame(frame_id,out_directory,color_buffer,depth_buffer,pose_buffer,pose2_buffer,pose3_buffer,rightHandJoint_buffer,leftHandJoint_buffer,rightHandJointOri_buffer,leftHandJointOri_buffer,save_hand, ):frame_directory os.path.join(outdirectory, fframe{frame_id}) # 创建帧目录os.makedirs(frame_directory, exist_okTrue) # 如果目录不存在则创建cv2.imwrite(os.path.join(frame_directory, color_image.jpg),color_buffer[frame_id][:, :, ::-1], # 保存颜色图像)cv2.imwrite(os.path.join(frame_directory, depth_image.png), depth_buffer[frame_id] # 保存深度图像)np.savetxt(os.path.join(frame_directory, pose.txt), pose_buffer[frame_id]) # 保存姿态数据np.savetxt(os.path.join(frame_directory, pose_2.txt), pose2_buffer[frame_id]) # 保存第二个姿态数据np.savetxt(os.path.join(frame_directory, pose_3.txt), pose3_buffer[frame_id]) # 保存第三个姿态数据if save_hand: # 如果需要保存手部数据np.savetxt(os.path.join(frame_directory, right_hand_joint.txt),rightHandJoint_buffer[frame_id], # 保存右手关节数据)np.savetxt(os.path.join(frame_directory, left_hand_joint.txt),leftHandJoint_buffer[frame_id], # 保存左手关节数据)np.savetxt(os.path.join(frame_directory, right_hand_joint_ori.txt),rightHandJointOri_buffer[frame_id], # 保存右手关节方向数据)np.savetxt(os.path.join(frame_directory, left_hand_joint_ori.txt),leftHandJointOri_buffer[frame_id], # 保存左手关节方向数据)return fframe {frame_id 1} saved # 返回保存帧的消息
1.2.2 RealsenseProcessor 的类获取多个相机的RGB-D帧和位姿数据 接下来定义了一个名为 RealsenseProcessor 的类用于处理Realsense相机的数据。它可以配置多个Realsense T265相机和一个L515相机获取RGB-D帧和位姿数据并可视化和存储这些数据 以下是对RealsenseProcessor类的详细解读 类定义和初始化 初始化方法接受多个参数用于配置T265相机的序列号、总帧数、是否存储帧、输出目录、是否保存手部数据和是否启用可视化 class RealsesneProcessor: # 定义Realsense处理器类def init( # 初始化方法self,first_t265_serial, # 第一个T265相机的序列号second_t265_serial, # 第二个T265相机的序列号thrid_t265_serial, # 第三个T265相机的序列号total_frame, # 总帧数store_frameFalse, # 是否存储帧默认为Falseout_directoryNone, # 输出目录默认为Nonesave_handFalse, # 是否保存手部数据默认为Falseenable_visualizationTrue, # 是否启用可视化默认为True):self.first_t265_serial first_t265_serial # 初始化第一个T265相机的序列号self.second_t265_serial second_t265_serial # 初始化第二个T265相机的序列号self.thrid_t265_serial thrid_t265_serial # 初始化第三个T265相机的序列号self.store_frame store_frame # 初始化是否存储帧self.out_directory out_directory # 初始化输出目录self.total_frame total_frame # 初始化总帧数self.save_hand save_hand # 初始化是否保存手部数据self.enable_visualization enable_visualization # 初始化是否启用可视化self.rds None # 初始化Redis连接 初始化各种缓冲区用于存储彩色图像、 深度图像、位姿数据和手部关节数据(包含左右两手的关节位置、关节方向) self.color_buffer [] # 初始化彩色图像缓冲区self.depth_buffer [] # 初始化深度图像缓冲区self.pose_buffer [] # 初始化第一个T265相机的位姿缓冲区self.pose2_buffer [] # 初始化第二个T265相机的位姿缓冲区self.pose3_buffer [] # 初始化第三个T265相机的位姿缓冲区self.pose2_image_buffer [] # 初始化第二个T265相机的图像缓冲区self.pose3_image_buffer [] # 初始化第三个T265相机的图像缓冲区self.rightHandJoint_buffer [] # 初始化右手关节位置缓冲区self.leftHandJoint_buffer [] # 初始化左手关节位置缓冲区self.rightHandJointOri_buffer [] # 初始化右手关节方向缓冲区self.leftHandJointOri_buffer [] # 初始化左手关节方向缓冲区 获取T265相机配置 具体方法是根据T265相机的序列号和管道配置返回一个配置对象 def get_rs_t265_config(self, t265_serial, t265_pipeline):t265_config rs.config()t265_config.enable_device(t265_serial)t265_config.enable_stream(rs.stream.pose)return t265_config 配置流 该方法配置并启动多个Realsense相机的流包括一个L515相机和三个T265相机 如果启用了手部数据保存功能则连接到Redis服务器 def configure_stream(self): # 配置流的方法# 连接到Redis服务器if self.save_hand: # 如果启用了手部数据保存功能self.rds redis.Redis(hostlocalhost, port6669, db0) # 连接到本地Redis服务器 配置并启动L515相机的深度和彩色流 # 创建一个管道self.pipeline rs.pipeline() # 创建一个Realsense管道config rs.config() # 创建一个配置对象color_profiles, depth_profiles get_profiles() # 获取彩色和深度流的配置文件w, h, fps, fmt depth_profiles[1] # 获取深度流的宽度、高度、帧率和格式config.enable_stream(rs.stream.depth, w, h, fmt, fps) # 启用深度流w, h, fps, fmt color_profiles[18] # 获取彩色流的宽度、高度、帧率和格式config.enable_stream(rs.stream.color, w, h, fmt, fps) # 启用彩色流 配置并启动三个T265相机的位姿流具体而言  先配置 # 配置第一个T265相机的流ctx rs.context() # 创建一个Realsense上下文self.t265_pipeline rs.pipeline(ctx) # 创建一个T265管道t265_config rs.config() # 创建一个T265配置对象t265_config.enable_device(self.first_t265_serial) # 启用第一个T265相机# 配置第二个T265相机的流ctx_2 rs.context() # 创建另一个Realsense上下文self.t265_pipeline_2 rs.pipeline(ctx_2) # 创建第二个T265管道t265_config_2 self.get_rs_t265_config(self.second_t265_serial, self.t265_pipeline_2) # 获取第二个T265相机的配置# 配置第三个T265相机的流ctx_3 rs.context() # 创建第三个Realsense上下文self.t265_pipeline_3 rs.pipeline(ctx_3) # 创建第三个T265管道t265_config_3 self.get_rs_t265_config(self.thrid_t265_serial, self.t265_pipeline_3) # 获取第三个T265相机的配置   再启动 self.t265_pipeline.start(t265_config) # 启动第一个T265管道self.t265_pipeline_2.start(t265_config_2) # 启动第二个T265管道self.t265_pipeline_3.start(t265_config_3) # 启动第三个T265管道pipeline_profile self.pipeline.start(config) # 启动L515管道并获取配置文件depth_sensor pipeline_profile.get_device().first_depth_sensor() # 获取深度传感器depth_sensor.set_option(rs.option.visual_preset, Preset.HighAccuracy) # 设置深度传感器的选项为高精度self.depth_scale depth_sensor.get_depth_scale() # 获取深度比例align_to rs.stream.color # 对齐到彩色流self.align rs.align(align_to) # 创建对齐对象 如果启用了可视化功能则初始化Open3D可视化器 self.vis None # 初始化可视化器if self.enable_visualization: # 如果启用了可视化功能self.vis o3d.visualization.Visualizer() # 创建Open3D可视化器self.vis.create_window() # 创建可视化窗口self.vis.get_view_control().change_field_of_view(step1.0) # 改变视野 获取RGB-D帧——get_rgbd_frame_from_realsense 该方法从Realsense相机获取RGB-D顾并将深度帧与彩色帧对齐 def get_rgbd_frame_from_realsense(self, enable_visualizationFalse): # 从Realsense获取RGB-D帧的方法frames self.pipeline.wait_for_frames() # 等待获取帧数据# 将深度帧对齐到彩色帧aligned_frames self.align.process(frames) # 对齐帧数据# 获取对齐后的帧aligned_depth_frame aligned_frames.get_depth_frame() # 获取对齐后的深度帧color_frame aligned_frames.get_color_frame() # 获取对齐后的彩色帧depth_image (np.asanyarray(aligned_depth_frame.get_data()) // 4) # 将深度帧数据转换为numpy数组并除以4以获得以米为单位的深度值适用于L515相机color_image np.asanyarray(color_frame.get_data()) # 将彩色帧数据转换为numpy数组 如果启用了可视化功能则将深度图像和彩色图像转换为Open3D的RGBD图像对象 rgbd None # 初始化RGBD图像对象if enable_visualization: # 如果启用了可视化功能depth_image_o3d o3d.geometry.Image(depth_image) # 将深度图像转换为Open3D图像对象color_image_o3d o3d.geometry.Image(color_image) # 将彩色图像转换为Open3D图像对象rgbd o3d.geometry.RGBDImage.create_from_color_and_depth(color_image_o3d, # 彩色图像depth_image_o3d, # 深度图像depth_trunc4.0, # 深度截断值超过此值的深度将被忽略convert_rgb_to_intensityFalse, # 是否将RGB图像转换为强度图像) # 创建RGBD图像对象return rgbd, depth_image, color_image # 返回RGBD图像、深度图像和彩色图像 通过frame_to_pose_conversion方法实现帧到位姿的转换 staticmethod # 静态方法装饰器 def frame_to_pose_conversion(input_t265_frames): # 定义帧到位姿转换的方法pose_frame input_t265_frames.get_pose_frame() # 获取位姿帧pose_data pose_frame.get_pose_data() # 获取位姿数据pose_3x3 quat2mat( # 将四元数转换为3x3旋转矩阵np.array([pose_data.rotation.w, # 四元数的w分量pose_data.rotation.x, # 四元数的x分量pose_data.rotation.y, # 四元数的y分量pose_data.rotation.z, # 四元数的z分量]))pose_4x4 np.eye(4) # 创建4x4单位矩阵pose_4x4[:3, :3] pose_3x3 # 将3x3旋转矩阵赋值给4x4矩阵的左上角pose_4x4[:3, 3] [ # 将平移向量赋值给4x4矩阵的右上角pose_data.translation.x, # 位姿的x平移分量pose_data.translation.y, # 位姿的y平移分量pose_data.translation.z, # 位姿的z平移分量]return pose_4x4 # 返回4x4位姿矩阵 处理帧 该方法处理每一帧数据首先通过get_rgbd_frame_from_realsense获取三个T265相机的RGB-D帧数据然后通过frame_to_pose_conversion把帧数据换成位姿数据 def process_frame(self): # 定义处理帧的方法frame_count 0 # 初始化帧计数器first_frame True # 标记是否为第一帧try:while frame_count self.total_frame: # 循环处理每一帧直到达到总帧数t265_frames self.t265_pipeline.wait_for_frames() # 获取第一个T265相机的帧数据t265_frames_2 self.t265_pipeline_2.wait_for_frames() # 获取第二个T265相机的帧数据t265_frames_3 self.t265_pipeline_3.wait_for_frames() # 获取第三个T265相机的帧数据rgbd, depth_frame, color_frame self.get_rgbd_frame_from_realsense() # 获取RGB-D帧数据# 获取第一个T265相机的位姿数据pose_4x4 RealsesneProcessor.frame_to_pose_conversion(input_t265_framest265_frames)# 获取第二个T265相机的位姿数据pose_4x4_2 RealsesneProcessor.frame_to_pose_conversion(input_t265_framest265_frames_2)# 获取第三个T265相机的位姿数据pose_4x4_3 RealsesneProcessor.frame_to_pose_conversion(input_t265_framest265_frames_3) 如果启用了手部数据保存功能则从Redis服务器获取手部关节数据 if self.save_hand: # 如果启用了手部数据保存功能# 获取左手关节位置数据leftHandJointXyz np.frombuffer(self.rds.get(rawLeftHandJointXyz), dtypenp.float64).reshape(21, 3)# 获取右手关节位置数据rightHandJointXyz np.frombuffer(self.rds.get(rawRightHandJointXyz), dtypenp.float64).reshape(21, 3)# 获取左手关节方向数据leftHandJointOrientation np.frombuffer(self.rds.get(rawLeftHandJointOrientation), dtypenp.float64).reshape(21, 4)# 获取右手关节方向数据rightHandJointOrientation np.frombuffer(self.rds.get(rawRightHandJointOrientation), dtypenp.float64).reshape(21, 4) 将位姿数据转换为4x4矩阵并应用校正矩阵 corrected_pose pose_4x4 between_cam # 应用校正矩阵 且转换为Open3D格式的L515相机内参 # 转换为Open3D格式的L515相机内参o3d_depth_intrinsic o3d.camera.PinholeCameraIntrinsic(1280,720,898.2010498046875,897.86669921875,657.4981079101562,364.30950927734375,) 如果是第一帧则初始化Open3D的点云和坐标系并添加到可视化器中 if first_frame: # 如果是第一帧if self.enable_visualization: # 如果启用了可视化功能pcd o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, o3d_depth_intrinsic) # 创建点云pcd.transform(corrected_pose) # 应用校正矩阵rgbd_mesh o3d.geometry.TriangleMesh.create_coordinate_frame(size0.3) # 创建RGBD坐标系rgbd_mesh.transform(corrected_pose) # 应用校正矩阵rgbd_previous_pose copy.deepcopy(corrected_pose) # 复制校正矩阵chest_mesh o3d.geometry.TriangleMesh.create_coordinate_frame(size0.3) # 创建胸部坐标系chest_mesh.transform(pose_4x4) # 应用位姿矩阵chest_previous_pose copy.deepcopy(pose_4x4) # 复制位姿矩阵left_hand_mesh (o3d.geometry.TriangleMesh.create_coordinate_frame(size0.3)) # 创建左手坐标系left_hand_mesh.transform(pose_4x4_2) # 应用位姿矩阵left_hand_previous_pose copy.deepcopy(pose_4x4_2) # 复制位姿矩阵right_hand_mesh (o3d.geometry.TriangleMesh.create_coordinate_frame(size0.3)) # 创建右手坐标系right_hand_mesh.transform(pose_4x4_3) # 应用位姿矩阵right_hand_previous_pose copy.deepcopy(pose_4x4_3) # 复制位姿矩阵self.vis.add_geometry(pcd) # 添加点云到可视化器self.vis.add_geometry(rgbd_mesh) # 添加RGBD坐标系到可视化器self.vis.add_geometry(chest_mesh) # 添加胸部坐标系到可视化器self.vis.add_geometry(left_hand_mesh) # 添加左手坐标系到可视化器self.vis.add_geometry(right_hand_mesh) # 添加右手坐标系到可视化器view_params (self.vis.get_view_control().convert_to_pinhole_camera_parameters()) # 获取视图参数first_frame False # 标记为非第一帧 如果不是第一帧则更新点云和坐标系的位姿 else:if self.enable_visualization: # 如果启用了可视化功能new_pcd o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, o3d_depth_intrinsic) # 创建新的点云new_pcd.transform(corrected_pose) # 应用校正矩阵rgbd_mesh.transform(np.linalg.inv(rgbd_previous_pose)) # 逆变换上一个RGBD坐标系rgbd_mesh.transform(corrected_pose) # 应用校正矩阵rgbd_previous_pose copy.deepcopy(corrected_pose) # 复制校正矩阵chest_mesh.transform(np.linalg.inv(chest_previous_pose)) # 逆变换上一个胸部坐标系chest_mesh.transform(pose_4x4) # 应用位姿矩阵chest_previous_pose copy.deepcopy(pose_4x4) # 复制位姿矩阵left_hand_mesh.transform(np.linalg.inv(left_hand_previous_pose)) # 逆变换上一个左手坐标系left_hand_mesh.transform(pose_4x4_2) # 应用位姿矩阵left_hand_previous_pose copy.deepcopy(pose_4x4_2) # 复制位姿矩阵right_hand_mesh.transform(np.linalg.inv(right_hand_previous_pose)) # 逆变换上一个右手坐标系right_hand_mesh.transform(pose_4x4_3) # 应用位姿矩阵right_hand_previous_pose copy.deepcopy(pose_4x4_3) # 复制位姿矩阵pcd.points new_pcd.points # 更新点云的点pcd.colors new_pcd.colors # 更新点云的颜色self.vis.update_geometry(pcd) # 更新点云几何self.vis.update_geometry(rgbd_mesh) # 更新RGBD坐标系几何self.vis.update_geometry(chest_mesh) # 更新胸部坐标系几何self.vis.update_geometry(left_hand_mesh) # 更新左手坐标系几何self.vis.update_geometry(right_hand_mesh) # 更新右手坐标系几何self.vis.get_view_control().convert_from_pinhole_camera_parameters(view_params) # 恢复视图参数 再更新可视化器 if self.enable_visualization: # 如果启用了可视化功能self.vis.poll_events() # 处理可视化事件self.vis.update_renderer() # 更新渲染器 如果启用了帧存储功能则将深度图像、彩色图像和位姿数据存储到缓冲区中 处理完所有帧后停止所有相机的流并保存所有帧数据主函数 主函数解析命令行参数创建 RealsenseProcessor 对象并配置和处理帧数据 import concurrent.futures # 导入并发库用于多线程处理def main(args): # 定义主函数接受命令行参数realsense_processor RealsesneProcessor( # 创建Realsense处理器对象first_t265_serial11622110012, # 第一个T265相机的序列号second_t265_serial909212110944, # 第二个T265相机的序列号thrid_t265_serial929122111181, # 第三个T265相机的序列号total_frame10000, # 总帧数store_frameargs.store_frame, # 是否存储帧out_directoryargs.out_directory, # 输出目录save_handargs.store_hand, # 是否保存手部数据enable_visualizationargs.enable_vis, # 是否启用可视化)realsense_processor.configure_stream() # 配置Realsense流realsense_processor.process_frame() # 处理帧数据if name main: # 主程序入口# 设置命令行参数解析器parser argparse.ArgumentParser(descriptionProcess frames and save data.)parser.add_argument(-s,–store_frame,actionstore_true,helpFlag to indicate whether to store frames, # 是否存储帧的标志)parser.add_argument(–store_hand,actionstore_true,helpFlag to indicate whether to store hand joint position and orientation, # 是否保存手部关节位置和方向的标志)parser.add_argument(-v,–enable_vis,actionstore_true,helpFlag to indicate whether to enable open3d visualization, # 是否启用Open3D可视化的标志)parser.add_argument(-o,–out_directory,typestr,helpOutput directory for saved data, # 保存数据的输出目录default./saved_data, # 默认输出目录为./saved_data)args parser.parse_args() # 解析命令行参数 如果输出目录已存在提示用户是否覆盖目录 # 检查输出目录是否存在if os.path.exists(args.out_directory):response (input(f{args.out_directory} already exists. Do you want to override? (y/n): ).strip().lower())if response ! y: # 如果用户选择不覆盖退出程序print(Exiting program without overriding the existing directory.)sys.exit()else:shutil.rmtree(args.out_directory) # 如果用户选择覆盖删除现有目录 如果启用了帧存储功能则创建输出目录 if args.store_frame:os.makedirs(args.out_directory, exist_okTrue) # 创建输出目录 调用 main 函数开始处理帧数据 # 如果用户选择覆盖删除现有目录main(args) # 调用主函数
1.3 demo_clipping_3d.py——可视化点云数据且选择每个演示的起始帧/结束帧 这段代码是一个用于可视化点云数据(PCD文件)并选择每个演示的起始帧、结束帧的脚本以下是详细解读 导入库 导入各种库和模块用于处理命令行参数、文件操作、点云数据处理、键盘事件监听等 import argparse # 用于解析命令行参数 import os # 用于操作系统相关功能 import copy # 用于复制对象 import zmq # 用于消息传递 import cv2 # 用于图像处理 import sys # 用于系统相关功能 import json # 用于处理JSON数据 import shutil # 用于文件操作 import open3d as o3d # 用于3D数据处理 import numpy as np # 用于数值计算 import platform # 用于获取操作系统信息 from pynput import keyboard # 用于监听键盘事件 from transforms3d.quaternions import qmult, quat2mat # 用于四元数操作 from transforms3d.axangles import axangle2mat # 用于轴角转换 from scipy.spatial.transform import Rotation # 用于旋转矩阵和欧拉角转换 from transforms3d.euler import quat2euler, mat2euler, quat2mat, euler2mat # 用于欧拉角和矩阵转换 from visualizer import * # 导入自定义的可视化模块 定义全局变量用于存储剪辑标记、当前剪辑、是否切换到下一帧或上一帧的标志 clip_marks [] # 存储剪辑标记 current_clip {} # 存储当前剪辑 next_frame False # 标记是否切换到下一帧 previous_frame False # 标记是否切换到上一帧 定义键盘事件处理函数用于处理不同的键盘输入 Key.up保存剪辑标记到 clip_marks. json 文件 Key.down标记切换到上一帧 Key.page_down 标记切换到下一帧 Key.end标记当前剪辑的结束帧 Key.insert标记当前剪辑的起始帧 def on_press(key): # 定义键盘按下事件处理函数global next_frame, previous_frame, delta_movement_accu, delta_ori_accu, delta_movement_accu_left, delta_ori_accu_left, adjust_movement, adjust_right, frame, step, dataset_folder, clip_marks, current_clip# 确定操作系统类型os_type platform.system()assert os_type Windows # 仅支持Windows系统framefolder frame{}.format(frame) # 当前帧文件夹名称# Windows特定的键绑定在AttributeError部分处理if key keyboard.Key.up: # y正方向with open(os.path.join(dataset_folder, clip_marks.json), w) as f:json.dump(clip_marks, f, indent4) # 保存剪辑标记到JSON文件elif key keyboard.Key.down: # y负方向previous_frame True # 切换到上一帧elif key keyboard.Key.page_down:next_frame True # 切换到下一帧elif key keyboard.Key.end:if start in current_clip.keys():print(end, frame_folder)current_clip[end] frame_folder # 标记当前剪辑的结束帧clip_marks.append(current_clip) # 添加当前剪辑到剪辑标记列表current_clip {} # 重置当前剪辑elif key keyboard.Key.insert:print(start, frame_folder)current_clip[start] frame_folder # 标记当前剪辑的起始帧else:print(Key error, key) # 处理其他键的错误 数据可视化类 定义数据可祝化类 ReplayDatavisualizer继承自DataVisualizer. replay_frames 方法用于可视化单帧数据并处理键盘事件以切换帧 class ReplayDataVisualizer(DataVisualizer): # 定义数据重放可视化类继承自DataVisualizerdef init(self, directory):super().init(directory) # 调用父类的初始化方法def replay_frames(self): # 定义重放帧的方法可视化单帧数据global delta_movement_accu, delta_ori_accu, next_frame, previous_frame, frameif self.R_delta_init is None:self.initialize_canonical_frame() # 初始化标准帧self._load_frame_data(frame) # 加载当前帧数据self.vis.add_geometry(self.pcd) # 添加点云数据到可视化器self.vis.add_geometry(self.coord_frame_1) # 添加坐标系1到可视化器self.vis.add_geometry(self.coord_frame_2) # 添加坐标系2到可视化器self.vis.add_geometry(self.coord_frame_3) # 添加坐标系3到可视化器for joint in self.left_joints self.right_joints:self.vis.add_geometry(joint) # 添加关节数据到可视化器for cylinder in self.left_line_set self.right_line_set:self.vis.add_geometry(cylinder) # 添加连线数据到可视化器next_frame True # 初始化为下一帧try:with keyboard.Listener(on_presson_press) as listener: # 监听键盘事件while True:if next_frame True:next_frame Falseframe 10 # 切换到下一帧if previous_frame True:previous_frame Falseframe - 10 # 切换到上一帧self._load_frame_data(frame) # 加载当前帧数据self.step 1 # 增加步数self.vis.update_geometry(self.pcd) # 更新点云数据self.vis.update_geometry(self.coord_frame_1) # 更新坐标系1self.vis.update_geometry(self.coord_frame_2) # 更新坐标系2self.vis.update_geometry(self.coord_frame_3) # 更新坐标系3for joint in self.left_joints self.right_joints:self.vis.update_geometry(joint) # 更新关节数据for cylinder in self.left_line_set self.right_line_set:self.vis.update_geometry(cylinder) # 更新连线数据self.vis.poll_events() # 处理可视化事件self.vis.update_renderer() # 更新渲染器listener.join() # 等待监听器结束finally:print(cumulative_correction , self.cumulative_correction) # 打印累计修正值 主函数 主函数解析命令行参数获取数据目录 if name main: # 主程序入口parser argparse.ArgumentParser(descriptionVisualize saved frame data.) # 创建命令行参数解析器parser.add_argument(–directory, typestr, default./saved_data, helpDirectory with saved data) # 添加目录参数args parser.parse_args() # 解析命令行参数 检查数据目录是否存在如果存在 clip_marks. json 文件询问用户是否覆盖 assert os.path.exists(args.directory), fgiven directory: {args.directory} not exists # 确认目录存在if os.path.exists(os.path.join(args.directory, clip_marks.json)): # 检查剪辑标记文件是否存在response (input(fclip_marks.json already exists. Do you want to override? (y/n): ).strip().lower())if response ! y:print(Exiting program without overriding the existing directory.) # 如果用户选择不覆盖退出程序sys.exit() 初始化 ReplayDatavisualizer 对象并加载校准偏移量和方向偏移量 dataset_folder args.directory # 设置数据集文件夹visualizer ReplayDataVisualizer(args.directory) # 创建数据重放可视化器对象visualizer.right_hand_offset np.loadtxt({}/calib_offset.txt.format(args.directory)) # 加载右手偏移量visualizer.right_hand_ori_offset np.loadtxt({}/calib_ori_offset.txt.format(args.directory)) # 加载右手方向偏移量visualizer.left_hand_offset np.loadtxt({}/calib_offset_left.txt.format(args.directory)) # 加载左手偏移量visualizer.left_hand_ori_offset np.loadtxt({}/calib_ori_offset_left.txt.format(args.directory)) # 加载左手方向偏移量 调用replay_frames 方法开始可视化和处理键盛事件 visualizer.replay_frames() # 开始重放帧
1.4 redis_glove_server.py——接收UDP数据包并将手部关节数据存储到Redis数据库 这段代码是一个用于接收UDP数据包并将手部关节数据存储到Redis数据库的Python脚本 所谓UDP (User Datagram Protocol用户数据报协议其是一种简单的、面向无连接的传输层协议 与TCP (Transmission Control Protocol 传输控制协议不同UDP不提供可靠性、数据包顺序和流量控制等功能。UDP主要用于 需要快速传输且对数据丢失不敏感的应用场景例如视频流、在线游戏和实时通信等 以下是代码的详细解读 导入库 import socket # 用于网络通信 import json # 用于处理JSON数据 import redis # 用于连接Redis数据库 import numpy as np # 用于数值计算 初始化Redis连接 # 初始化Redis连接 redis_host localhost # Redis服务器主机名 redis_port 6669 # Redis服务器端口 redis_password # Redis服务器密码如果没有密码则保持为空字符串 r redis.StrictRedis(hostredis_host, portredis_port, passwordredis_password, decode_responsesTrue ) # 创建Redis连接对象 定义手部关节名称 # 定义左手和右手的关节名称 left_hand_joint_names [leftHand,leftThumbProximal, leftThumbMedial, leftThumbDistal, leftThumbTip,leftIndexProximal, leftIndexMedial, leftIndexDistal, leftIndexTip,leftMiddleProximal, leftMiddleMedial, leftMiddleDistal, leftMiddleTip,leftRingProximal, leftRingMedial, leftRingDistal, leftRingTip,leftLittleProximal, leftLittleMedial, leftLittleDistal, leftLittleTip]right_hand_joint_names [rightHand,rightThumbProximal, rightThumbMedial, rightThumbDistal, rightThumbTip,rightIndexProximal, rightIndexMedial, rightIndexDistal, rightIndexTip,rightMiddleProximal, rightMiddleMedial, rightMiddleDistal, rightMiddleTip,rightRingProximal, rightRingMedial, rightRingDistal, rightRingTip,rightLittleProximal, rightLittleMedial, rightLittleDistal, rightLittleTip] 归一化函数 def normalize_wrt_middle_proximal(hand_positions, is_leftTrue): # 定义相对于中指近端关节的归一化函数middle_proximal_idx left_hand_joint_names.index(leftMiddleProximal) # 获取左手中指近端关节的索引if not is_left:middle_proximal_idx right_hand_joint_names.index(rightMiddleProximal) # 获取右手中指近端关节的索引wrist_position hand_positions[0] # 获取手腕位置middle_proximal_position hand_positions[middle_proximal_idx] # 获取中指近端关节位置bone_length np.linalg.norm(wrist_position - middle_proximal_position) # 计算手腕到中指近端关节的骨骼长度normalized_hand_positions (middle_proximal_position - hand_positions) / bone_length # 归一化手部关节位置return normalized_hand_positions # 返回归一化后的手部关节位置 启动服务器函数 创建并绑定UDP套接字 def start_server(port): # 定义启动服务器的函数接受端口号作为参数s socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # 使用SOCK_DGRAM创建UDP套接字s.bind((192.168.0.200, port)) # 绑定套接字到指定的IP地址和端口print(fServer started, listening on port {port} for UDP packets…) # 打印服务器启动信息 无限循环接收UDP数据包 while True: # 无限循环持续接收数据data, address s.recvfrom(64800) # 接收UDP数据包最大数据包大小为64800字节decoded_data data.decode() # 解码数据# 尝试解析JSON数据try:received_json json.loads(decoded_data) # 解析JSON数据# 初始化数组以存储手部关节位置和方向left_hand_positions np.zeros((21, 3)) # 初始化左手关节位置数组21个关节每个关节3个坐标right_hand_positions np.zeros((21, 3)) # 初始化右手关节位置数组21个关节每个关节3个坐标left_hand_orientations np.zeros((21, 4)) # 初始化左手关节方向数组21个关节每个关节4个方向分量right_hand_orientations np.zeros((21, 4)) # 初始化右手关节方向数组21个关节每个关节4个方向分量 解析接收到的数据包提取手部关节位置和方向数据 # 遍历JSON数据以提取手部关节位置和方向for joint_name in left_hand_joint_names: # 遍历左手关节名称列表joint_data received_json[scene][actors][0][body][joint_name] # 获取关节数据joint_position np.array(list(joint_data[position].values())) # 获取关节位置joint_rotation np.array(list(joint_data[rotation].values())) # 获取关节方向left_hand_positions[left_hand_joint_names.index(joint_name)] joint_position # 存储关节位置left_hand_orientations[left_hand_joint_names.index(joint_name)] joint_rotation # 存储关节方向for joint_name in right_hand_joint_names: # 遍历右手关节名称列表joint_data received_json[scene][actors][0][body][joint_name] # 获取关节数据joint_position np.array(list(joint_data[position].values())) # 获取关节位置joint_rotation np.array(list(joint_data[rotation].values())) # 获取关节方向right_hand_positions[right_hand_joint_names.index(joint_name)] joint_position # 存储关节位置right_hand_orientations[right_hand_joint_names.index(joint_name)] joint_rotation # 存储关节方向 计算相对于中指近端关节的相对距离并进行归一化 # 计算相对于中指近端关节的相对距离并归一化left_middle_proximal_idx left_hand_joint_names.index(leftMiddleProximal) # 获取左手中指近端关节的索引right_middle_proximal_idx right_hand_joint_names.index(rightMiddleProximal) # 获取右手中指近端关节的索引left_wrist_position left_hand_positions[0] # 获取左手手腕位置right_wrist_position right_hand_positions[0] # 获取右手手腕位置left_middle_proximal_position left_hand_positions[left_middle_proximal_idx] # 获取左手中指近端关节位置right_middle_proximal_position right_hand_positions[right_middle_proximal_idx] # 获取右手中指近端关节位置left_bone_length np.linalg.norm(left_wrist_position - left_middle_proximal_position) # 计算左手骨骼长度right_bone_length np.linalg.norm(right_wrist_position - right_middle_proximal_position) # 计算右手骨骼长度normalized_left_hand_positions (left_middle_proximal_position - left_hand_positions) / left_bone_length # 归一化左手关节位置normalized_right_hand_positions (right_middle_proximal_position - right_hand_positions) / right_bone_length # 归一化右手关节位置 将原始和归一化后的手部关节数据存储到Redis数据库中 r.set(leftHandJointXyz, np.array(normalized_left_hand_positions).astype(np.float64).tobytes()) # 将归一化后的左手关节位置存储到Redisr.set(rightHandJointXyz, np.array(normalized_right_hand_positions).astype(np.float64).tobytes()) # 将归一化后的右手关节位置存储到Redisr.set(rawLeftHandJointXyz, np.array(left_hand_positions).astype(np.float64).tobytes()) # 将原始左手关节位置存储到Redisr.set(rawRightHandJointXyz, np.array(right_hand_positions).astype(np.float64).tobytes()) # 将原始右手关节位置存储到Redisr.set(rawLeftHandJointOrientation, np.array(left_hand_orientations).astype(np.float64).tobytes()) # 将原始左手关节方向存储到Redisr.set(rawRightHandJointOrientation, np.array(right_hand_orientations).astype(np.float64).tobytes()) # 将原始右手关节方向存储到Redis 打印接收到的手部关节位置数据 print(\n\n)print( * 50)print(np.round(left_hand_positions, 3)) # 打印左手关节位置print(-*50)print(np.round(right_hand_positions, 3)) # 打印右手关节位置except json.JSONDecodeError: # 捕获JSON解析错误print(Invalid JSON received:) # 打印错误信息 主程序入口 if name main: # 主程序入口start_server(14551) # 启动服务器监听端口14551
1.5 replay_human_traj_vis.py——可视化保存点云数据和位姿(可用于查看所有帧和校准) 1.6 transform_to_robot_table.py——将可视化点云数据放置在机器人桌面上 1.7 visualizer.py——可视化手部关节数据 1.7.1 一系列定义比如五个手指等 导入各种库和模块用于处理命令行参数、文件操作、点云数据处理、键盘事件监听等定义手部关节之间的连接线用于可视化手部骨架 lines np.array([# 拇指[1, 2], [2, 3], [3, 4],# 食指[5, 6], [6, 7], [7, 8],# 中指[9, 10], [10, 11], [11, 12],# 无名指[13, 14], [14, 15], [15, 16],# 小指[17, 18], [18, 19], [19, 20],# 连接近端关节[1, 5], [5, 9], [9, 13], [13, 17],# 连接手掌[0, 1], [17, 0] ]) 定义一系列全局变量用于存储累积的校正、当前帧、步长等信息 delta_movement_accu np.array([0.0, 0.0, 0.0]) # 累积的位移校正 delta_ori_accu np.array([0.0, 0.0, 0.0]) # 累积的方向校正 delta_movement_accu_left np.array([0.0, 0.0, 0.0]) # 左手累积的位移校正 delta_ori_accu_left np.array([0.0, 0.0, 0.0]) # 左手累积的方向校正 adjust_movement True # 是否调整位移 adjust_right True # 是否调整右手 next_frame False # 是否切换到下一帧 frame 0 # 当前帧 step 0.01 # 步长 fixed_transform np.array([0.0, 0.0, 0.0]) # 固定变换 一些辅助函数 将手腕位置平移到原点 def translate_wrist_to_origin(joint_positions): # 将手腕位置平移到原点wrist_position joint_positions[0] # 获取手腕位置updated_positions joint_positions - wrist_position # 平移所有关节位置return updated_positions # 返回平移后的关节位置 应用位姿矩阵 def apply_pose_matrix(joint_positions, pose_matrix): # 应用位姿矩阵homogeneous_joint_positions np.hstack([joint_positions, np.ones((joint_positions.shape[0], 1))]) # 将关节位置转换为齐次坐标transformed_positions np.dot(homogeneous_joint_positions, pose_matrix.T) # 应用位姿矩阵transformed_positions_3d transformed_positions[:, :3] # 提取3D坐标return transformed_positions_3d # 返回变换后的关节位置 创建或更新圆柱体 def create_or_update_cylinder(start, end, radius0.003, cylinder_listNone, cylinder_idx-1): # 创建或更新圆柱体cyl_length np.linalg.norm(end - start) # 计算圆柱体的长度new_cylinder o3d.geometry.TriangleMesh.create_cylinder(radiusradius, heightcyl_length, resolution20, split4) # 创建新的圆柱体new_cylinder.paint_uniform_color([1, 0, 0]) # 将圆柱体涂成红色new_cylinder.translate(np.array([0, 0, cyl_length / 2])) # 平移圆柱体direction end - start # 计算方向向量direction / np.linalg.norm(direction) # 归一化方向向量up np.array([0, 0, 1]) # 圆柱体的默认向上向量rotation_axis np.cross(up, direction) # 计算旋转轴rotation_angle np.arccos(np.dot(up, direction)) # 计算旋转角度if np.linalg.norm(rotation_axis) ! 0: # 如果旋转轴不为零rotation_axis / np.linalg.norm(rotation_axis) # 归一化旋转轴rotation_matrix o3d.geometry.get_rotation_matrix_from_axis_angle(rotation_axis * rotation_angle) # 计算旋转矩阵new_cylinder.rotate(rotation_matrix, centernp.array([0, 0, 0])) # 旋转圆柱体new_cylinder.translate(start) # 平移圆柱体到起始位置if cylinder_list[cylinder_idx] is not None: # 如果圆柱体列表中已有圆柱体cylinder_list[cylinder_idx].vertices new_cylinder.vertices # 更新顶点cylinder_list[cylinder_idx].triangles new_cylinder.triangles # 更新三角形cylinder_list[cylinder_idx].vertex_normals new_cylinder.vertex_normals # 更新顶点法线cylinder_list[cylinder_idx].vertex_colors new_cylinder.vertex_colors # 更新顶点颜色else:cylinder_list[cylinder_idx] new_cylinder # 添加新的圆柱体到列表中
1.7.2 DataVisualizer类用于可视化点云数据和手部关节数据 接下来定义了一个名为DataVisualizer的类用于可视化点云数据和手部关节数据 类定义和初始化 class DataVisualizer: # 定义DataVisualizer类def init(self, directory): # 初始化方法接受数据目录作为参数self.directory directory # 初始化数据目录self.base_pcd None # 初始化基础点云对象self.pcd None # 初始化点云对象self.img_backproj None # 初始化图像反投影对象self.coord_frame_1 None # 初始化坐标系1self.coord_frame_2 None # 初始化坐标系2self.coord_frame_3 None # 初始化坐标系3self.right_hand_offset None # 初始化右手偏移量self.right_hand_ori_offset None # 初始化右手方向偏移量self.left_hand_offset None # 初始化左手偏移量self.left_hand_ori_offset None # 初始化左手方向偏移量self.pose1_prev np.eye(4) # 初始化第一个位姿矩阵self.pose2_prev np.eye(4) # 初始化第二个位姿矩阵self.pose3_prev np.eye(4) # 初始化第三个位姿矩阵self.vis o3d.visualization.Visualizer() # 创建Open3D可视化器self.vis.create_window() # 创建可视化窗口self.vis.get_view_control().change_field_of_view(step1.0) # 改变视野self.between_cam np.eye(4) # 初始化相机之间的变换矩阵self.between_cam[:3, :3] np.array([[1.0, 0.0, 0.0],[0.0, -1.0, 0.0],[0.0, 0.0, -1.0]])self.between_cam[:3, 3] np.array([0.0, 0.076, 0.0]) # 设置平移部分self.between_cam_2 np.eye(4) # 初始化第二个相机之间的变换矩阵self.between_cam_2[:3, :3] np.array([[1.0, 0.0, 0.0],[0.0, 1.0, 0.0],[0.0, 0.0, 1.0]])self.between_cam_2[:3, 3] np.array([0.0, -0.032, 0.0]) # 设置平移部分self.between_cam_3 np.eye(4) # 初始化第三个相机之间的变换矩阵self.between_cam_3[:3, :3] np.array([[1.0, 0.0, 0.0],[0.0, 1.0, 0.0],[0.0, 0.0, 1.0]])self.between_cam_3[:3, 3] np.array([0.0, -0.064, 0.0]) # 设置平移部分self.canonical_t265_ori None # 初始化标准T265方向# 可视化左手21个关节self.left_joints [] # 初始化左手关节列表self.right_joints [] # 初始化右手关节列表self.left_line_set [None for _ in lines] # 初始化左手连线列表self.right_line_set [None for _ in lines] # 初始化右手连线列表for i in range(21): # 遍历21个关节for joint in [self.left_joints, self.right_joints]: # 遍历左手和右手关节列表# 为指尖和手腕创建较大的球体为其他关节创建较小的球体radius 0.011 if i in [0, 4, 8, 12, 16, 20] else 0.007sphere o3d.geometry.TriangleMesh.create_sphere(radiusradius) # 创建球体# 将手腕和近端关节涂成绿色if i in [0, 1, 5, 9, 13, 17]:sphere.paint_uniform_color([0, 1, 0])# 将指尖涂成红色elif i in [4, 8, 12, 16, 20]:sphere.paint_uniform_color([1, 0, 0])# 将其他关节涂成蓝色else:sphere.paint_uniform_color([0, 0, 1])# 将拇指涂成粉色if i in [1, 2, 3, 4]:sphere.paint_uniform_color([1, 0, 1])joint.append(sphere) # 将球体添加到关节列表中self.vis.add_geometry(sphere) # 将球体添加到可视化器中self.step 0 # 初始化步数self.distance_buffer [] # 初始化距离缓冲区self.R_delta_init None # 初始化旋转矩阵self.cumulative_correction np.array([0.0, 0.0, 0.0]) # 初始化累计修正值 初始化标准帧的方法 def initialize_canonical_frame(self): # 初始化标准帧的方法if self.R_delta_init is None: # 如果旋转矩阵未初始化self._load_frame_data(0) # 加载第0帧数据pose_ori_matirx self.pose3_prev[:3, :3] # 获取第三个位姿的旋转矩阵pose_ori_correction_matrix np.dot(np.array([[0, -1, 0],[0, 0, 1],[1, 0, 0]]), euler2mat(0, 0, 0)) # 计算修正矩阵pose_ori_matirx np.dot(pose_ori_matirx, pose_ori_correction_matrix) # 应用修正矩阵self.canonical_t265_ori np.array([[1, 0, 0],[0, -1, 0],[0, 0, -1]]) # 初始化标准T265方向x_angle, y_angle, z_angle mat2euler(self.pose3_prev[:3, :3]) # 将旋转矩阵转换为欧拉角self.canonical_t265_ori np.dot(self.canonical_t265_ori, euler2mat(-z_angle, x_angle 0.3, y_angle)) # 应用欧拉角修正self.R_delta_init np.dot(self.canonical_t265_ori, pose_ori_matirx.T) # 计算初始旋转矩阵 重放关键帧校准的方法 def replay_keyframes_calibration(self):可视化单帧global delta_movement_accu, delta_ori_accu, next_frame, frameif self.R_delta_init is None:self.initialize_canonical_frame() # 初始化标准帧self._load_frame_data(frame) # 加载当前帧数据self.vis.add_geometry(self.pcd) # 添加点云数据到可视化器self.vis.add_geometry(self.coord_frame_1) # 添加坐标系1到可视化器self.vis.add_geometry(self.coord_frame_2) # 添加坐标系2到可视化器self.vis.add_geometry(self.coord_frame_3) # 添加坐标系3到可视化器for joint in self.left_joints self.right_joints:self.vis.add_geometry(joint) # 添加关节数据到可视化器for cylinder in self.left_line_set self.right_line_set:self.vis.add_geometry(cylinder) # 添加连线数据到可视化器next_frame True # 初始化为下一帧try:with keyboard.Listener(on_presson_press) as listener: # 监听键盘事件while True:if next_frame True:next_frame Falseframe 10 # 切换到下一帧self._load_frame_data(frame) # 加载当前帧数据self.step 1 # 增加步数self.vis.update_geometry(self.pcd) # 更新点云数据self.vis.update_geometry(self.coord_frame_1) # 更新坐标系1self.vis.update_geometry(self.coord_frame_2) # 更新坐标系2self.vis.update_geometry(self.coord_frame_3) # 更新坐标系3for joint in self.left_joints self.right_joints:self.vis.update_geometry(joint) # 更新关节数据for cylinder in self.left_line_set self.right_line_set:self.vis.update_geometry(cylinder) # 更新连线数据self.vis.poll_events() # 处理可视化事件self.vis.update_renderer() # 更新渲染器listener.join() # 等待监听器结束finally:print(cumulative_correction , self.cumulative_correction) # 打印累计修正值 重放所有帧的方法 def replay_all_frames(self):连续可视化所有帧try:if self.R_delta_init is None:self.initialize_canonical_frame() # 初始化标准帧frame 0 # 初始化帧计数器first_frame True # 标记是否为第一帧while True:if not self._load_frame_data(frame): # 加载当前帧数据break # 如果无法加载数据退出循环if first_frame:self.vis.add_geometry(self.pcd) # 添加点云数据到可视化器self.vis.add_geometry(self.coord_frame_1) # 添加坐标系1到可视化器self.vis.add_geometry(self.coord_frame_2) # 添加坐标系2到可视化器self.vis.add_geometry(self.coord_frame_3) # 添加坐标系3到可视化器for joint in self.left_joints self.right_joints:self.vis.add_geometry(joint) # 添加关节数据到可视化器for cylinder in self.left_line_set self.right_line_set:self.vis.add_geometry(cylinder) # 添加连线数据到可视化器else:self.vis.update_geometry(self.pcd) # 更新点云数据self.vis.update_geometry(self.coord_frame_1) # 更新坐标系1self.vis.update_geometry(self.coord_frame_2) # 更新坐标系2self.vis.update_geometry(self.coord_frame_3) # 更新坐标系3for joint in self.left_joints self.right_joints:self.vis.update_geometry(joint) # 更新关节数据for cylinder in self.left_line_set self.right_line_set:self.vis.update_geometry(cylinder) # 更新连线数据self.vis.poll_events() # 处理可视化事件self.vis.update_renderer() # 更新渲染器if first_frame:view_params self.vis.get_view_control().convert_to_pinhole_camera_parameters() # 获取视图参数else:self.vis.get_view_control().convert_from_pinhole_camera_parameters(view_params) # 恢复视图参数self.step 1 # 增加步数frame 5 # 增加帧计数器if first_frame:first_frame False # 标记为非第一帧finally:self.vis.destroy_window() # 销毁可视化窗口 反投影点的方法 def _back_project_point(self, point, intrinsics): 将单个点从3D反投影到2D图像空间 x, y, z pointfx, fy intrinsics[0, 0], intrinsics[1, 1]cx, cy intrinsics[0, 2], intrinsics[1, 2]u (x * fx / z) cxv (y * fy / z) cyreturn int(u), int(v)
1.7.3 _load_frame_data加载给定帧的点云和位姿数据并进行可视化处理 最后是加载帧数据的方法即_load_frame_data这个方法用于加载给定帧的点云和位姿数据并进行可视化处理 方法定义和参数说明 def _load_frame_data(self, frame, vis_2dFalse, load_table_pointsFalse):Load point cloud and poses for a given frameparam frame: frame count in integerreturn whether we can successfully load all data from frame subdirectoryglobal delta_movement_accu, delta_ori_accu, delta_movement_accu_left, delta_ori_accu_left # 全局变量用于存储累积的平移和旋转校正print(fframe {frame}) # 打印当前帧编号if adjust_movement: # 如果启用了平移校正print(adjusting translation) # 打印平移校正信息else:print(adjusting rotation) # 打印旋转校正信息 初始化最顶部的L515相机内参 # L515:o3d_depth_intrinsic o3d.camera.PinholeCameraIntrinsic(1280, 720,898.2010498046875,897.86669921875,657.4981079101562,364.30950927734375) # 初始化L515相机的内参 处理桌面点云数据 if load_table_points: # 如果需要加载桌面点云数据table_color_image_o3d o3d.io.read_image(os.path.join(self.table_frame, frame_0, color_image.jpg)) # 读取桌面彩色图像table_depth_image_o3d o3d.io.read_image(os.path.join(self.table_frame, frame_0, depth_image.png)) # 读取桌面深度图像table_rgbd o3d.geometry.RGBDImage.create_from_color_and_depth(table_color_image_o3d, table_depth_image_o3d, depth_trunc4.0,convert_rgb_to_intensityFalse) # 创建RGBD图像table_pose_4x4 np.loadtxt(os.path.join(self.table_frame, frame_0, pose.txt)) # 读取桌面位姿table_corrected_pose table_pose_4x4 self.between_cam # 应用校正矩阵self.table_pcd o3d.geometry.PointCloud.create_from_rgbd_image(table_rgbd, o3d_depth_intrinsic) # 创建点云self.table_pcd.transform(table_corrected_pose) # 应用校正矩阵 加载当前帧数据 framedir os.path.join(self.directory, fframe{frame}) # 当前帧目录color_image_o3d o3d.io.read_image(os.path.join(frame_dir, color_image.jpg)) # 读取彩色图像depth_image_o3d o3d.io.read_image(os.path.join(frame_dir, depth_image.png)) # 读取深度图像rgbd o3d.geometry.RGBDImage.create_from_color_and_depth(color_image_o3d, depth_image_o3d, depth_trunc4.0,convert_rgb_to_intensityFalse) # 创建RGBD图像pose_4x4 np.loadtxt(os.path.join(frame_dir, pose.txt)) # 读取位姿if load_table_points:pose_4x4[:3, 3] fixed_transform.T # 应用固定变换corrected_pose pose_4x4 self.between_cam # 应用校正矩阵 检查位姿文件是否存在 pose_path os.path.join(frame_dir, pose.txt) # 位姿文件路径pose_2_path os.path.join(frame_dir, pose_2.txt) # 第二个位姿文件路径pose_3_path os.path.join(frame_dir, pose_3.txt) # 第三个位姿文件路径if not all(os.path.exists(path) for path in [pose_path, pose_2_path, pose_3_path]): # 检查所有位姿文件是否存在return False # 如果有文件不存在返回False 创建或更新点云 if self.pcd is None: # 如果点云对象为空self.pcd o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, o3d_depth_intrinsic) # 创建点云self.pcd.transform(corrected_pose) # 应用校正矩阵else:new_pcd o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, o3d_depth_intrinsic) # 创建新的点云new_pcd.transform(corrected_pose) # 应用校正矩阵self.pcd.points new_pcd.points # 更新点云的点self.pcd.colors new_pcd.colors # 更新点云的颜色 加载并校正位姿 pose_1 np.loadtxt(pose_path) # 读取第一个位姿if load_table_points:pose_1[:3, 3] fixed_transform.T # 应用固定变换pose_1 pose_1 self.between_cam # 应用校正矩阵pose_2 np.loadtxt(pose_2_path) # 读取第二个位姿if load_table_points:pose_2[:3, 3] fixed_transform.T # 应用固定变换pose_2 pose_2 self.between_cam_2 # 应用校正矩阵pose_3 np.loadtxt(pose_3_path) # 读取第三个位姿if load_table_points:pose_3[:3, 3] fixed_transform.T # 应用固定变换pose_3 pose_3 self.between_cam_3 # 应用校正矩阵 创建或更新坐标系 if self.coord_frame_1 is None: # 如果坐标系1为空self.coord_frame_1 o3d.geometry.TriangleMesh.create_coordinate_frame(size0.1) # 创建坐标系1self.coord_frame_2 o3d.geometry.TriangleMesh.create_coordinate_frame(size0.1) # 创建坐标系2self.coord_frame_3 o3d.geometry.TriangleMesh.create_coordinate_frame(size0.1) # 创建坐标系3self.coord_frame_1 self.coord_frame_1.transform(np.linalg.inv(self.pose1_prev)) # 逆变换上一个位姿self.coord_frame_1 self.coord_frame_1.transform(pose_1) # 应用当前位姿self.pose1_prev copy.deepcopy(pose_1) # 复制当前位姿self.coord_frame_2 self.coord_frame_2.transform(np.linalg.inv(self.pose2_prev)) # 逆变换上一个位姿self.coord_frame_2 self.coord_frame_2.transform(pose_2) # 应用当前位姿self.pose2_prev copy.deepcopy(pose_2) # 复制当前位姿self.coord_frame_3 self.coord_frame_3.transform(np.linalg.inv(self.pose3_prev)) # 逆变换上一个位姿self.coord_frame_3 self.coord_frame_3.transform(pose_3) # 应用当前位姿self.pose3_prev copy.deepcopy(pose_3) # 复制当前位姿 加载并处理左手关节数据 # left hand, read from jointleft_hand_joint_xyz np.loadtxt(os.path.join(frame_dir, left_hand_joint.txt)) # 读取左手关节位置self.left_hand_joint_xyz left_hand_joint_xyz # 存储左手关节位置left_hand_joint_xyz translate_wrist_to_origin(left_hand_joint_xyz) # 平移手腕到原点left_hand_joint_ori np.loadtxt(os.path.join(frame_dir, left_hand_joint_ori.txt))[0] # 读取左手关节方向self.left_hand_wrist_ori left_hand_joint_ori # 存储左手手腕方向left_rotation_matrix Rotation.from_quat(left_hand_joint_ori).as_matrix().T # 计算旋转矩阵left_hand_joint_xyz_reshaped left_hand_joint_xyz[:, :, np.newaxis] # 重塑关节位置left_transformed_joint_xyz np.matmul(left_rotation_matrix, left_hand_joint_xyz_reshaped) # 应用旋转矩阵left_hand_joint_xyz left_transformed_joint_xyz[:, :, 0] # 更新关节位置left_hand_joint_xyz[:, -1] -left_hand_joint_xyz[:, -1] # z轴反转rotation_matrix axangle2mat(np.array([0, 1, 0]), -np.pi * 1 / 2) # 绕y轴旋转left_hand_joint_xyz np.dot(left_hand_joint_xyz, rotation_matrix.T) # 应用旋转矩阵rotation_matrix axangle2mat(np.array([1, 0, 0]), np.pi * 1 / 2) # 绕x轴旋转left_hand_joint_xyz np.dot(left_hand_joint_xyz, rotation_matrix.T) # 应用旋转矩阵rotation_matrix axangle2mat(np.array([0, 0, 1]), -np.pi * 1 / 2) # 绕z轴旋转left_hand_joint_xyz np.dot(left_hand_joint_xyz, rotation_matrix.T) # 应用旋转矩阵left_hand_joint_xyz np.dot(left_hand_joint_xyz, euler2mat(*self.left_hand_ori_offset).T) # 应用欧拉角校正left_hand_joint_xyz np.dot(left_hand_joint_xyz, euler2mat(*delta_ori_accu_left).T) # 应用累积旋转校正left_hand_joint_xyz self.left_hand_offset # 应用平移校正left_hand_joint_xyz delta_movement_accu_left # 应用累积平移校正left_hand_joint_xyz apply_pose_matrix(left_hand_joint_xyz, pose_2) # 应用位姿矩阵 设置左手关节球体和连线 # set joint sphere and linesfor i, sphere in enumerate(self.left_joints): # 遍历左手关节球体transformation np.eye(4) # 创建4x4单位矩阵transformation[:3, 3] left_hand_joint_xyz[i] - sphere.get_center() # 计算平移向量sphere.transform(transformation) # 应用平移变换for i, (x, y) in enumerate(lines): # 遍历连线start self.left_joints[x].get_center() # 获取起点end self.left_joints[y].get_center() # 获取终点create_or_update_cylinder(start, end, cylinder_listself.left_line_set, cylinder_idxi) # 创建或更新圆柱体 加载并处理右手关节数据 # right hand, read from jointright_hand_joint_xyz np.loadtxt(os.path.join(frame_dir, right_hand_joint.txt)) # 读取右手关节位置self.right_hand_joint_xyz right_hand_joint_xyz # 存储右手关节位置right_hand_joint_xyz translate_wrist_to_origin(right_hand_joint_xyz) # 平移手腕到原点right_hand_joint_ori np.loadtxt(os.path.join(frame_dir, right_hand_joint_ori.txt))[0] # 读取右手关节方向self.right_hand_wrist_ori right_hand_joint_ori # 存储右手手腕方向right_rotation_matrix Rotation.from_quat(right_hand_joint_ori).as_matrix().T # 计算旋转矩阵right_joint_xyz_reshaped right_hand_joint_xyz[:, :, np.newaxis] # 重塑关节位置right_transformed_joint_xyz np.matmul(right_rotation_matrix, right_joint_xyz_reshaped) # 应用旋转矩阵right_hand_joint_xyz right_transformed_joint_xyz[:, :, 0] # 更新关节位置right_hand_joint_xyz[:, -1] -right_hand_joint_xyz[:, -1] # z轴反转rotation_matrix axangle2mat(np.array([0, 1, 0]), -np.pi * 1 / 2) # 绕y轴旋转right_hand_joint_xyz np.dot(right_hand_joint_xyz, rotation_matrix.T) # 应用旋转矩阵rotation_matrix axangle2mat(np.array([1, 0, 0]), np.pi * 1 / 2) # 绕x轴旋转right_hand_joint_xyz np.dot(right_hand_joint_xyz, rotation_matrix.T) # 应用旋转矩阵rotation_matrix axangle2mat(np.array([0, 0, 1]), -np.pi * 1 / 2) # 绕z轴旋转right_hand_joint_xyz np.dot(right_hand_joint_xyz, rotation_matrix.T) # 应用旋转矩阵right_hand_joint_xyz np.dot(right_hand_joint_xyz, euler2mat(*self.right_hand_ori_offset).T) # 应用欧拉角校正right_hand_joint_xyz np.dot(right_hand_joint_xyz, euler2mat(*delta_ori_accu).T) # 应用累积旋转校正right_hand_joint_xyz self.right_hand_offset # 应用平移校正right_hand_joint_xyz delta_movement_accu # 应用累积平移校正right_hand_joint_xyz apply_pose_matrix(right_hand_joint_xyz, pose_3) # 应用位姿矩阵 可视化2D图像 if vis_2d: # 如果启用了2D可视化color_image np.asarray(rgbd.color) # 获取彩色图像color_image cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) # 转换颜色空间left_hand_joint_points_homogeneous np.hstack((left_hand_joint_xyz, np.ones((left_hand_joint_xyz.shape[0], 1)))) # 转换为齐次坐标left_hand_transformed_points_homogeneous np.dot(left_hand_joint_points_homogeneous, np.linalg.inv(corrected_pose).T) # 应用逆变换left_hand_points_to_project left_hand_transformed_points_homogeneous[:, :3] / left_hand_transformed_points_homogeneous[:, [3]] # 归一化left_hand_back_projected_points [self._back_project_point(point, o3d_depth_intrinsic.intrinsic_matrix) for point in left_hand_points_to_project] # 反投影for i in range(len(left_hand_back_projected_points)): # 遍历左手关节点u, v left_hand_back_projected_points[i] # 获取投影点坐标if i in [0, 1, 5, 9, 13, 17]:cv2.circle(color_image, (u, v), 10, (0, 255, 0), -1) # 绘制绿色圆点elif i in [4, 8, 12, 16, 20]:cv2.circle(color_image, (u, v), 10, (255, 0, 0), -1) # 绘制蓝色圆点else:cv2.circle(color_image, (u, v), 10, (0, 0, 255), -1) # 绘制红色圆点if i in [1, 2, 3, 4]:cv2.circle(color_image, (u, v), 10, (255, 0, 255), -1) # 绘制紫色圆点right_hand_joint_points_homogeneous np.hstack((right_hand_joint_xyz, np.ones((right_hand_joint_xyz.shape[0], 1)))) # 转换为齐次坐标right_hand_transformed_points_homogeneous np.dot(right_hand_joint_points_homogeneous, np.linalg.inv(corrected_pose).T) # 应用逆变换right_hand_points_to_project right_hand_transformed_points_homogeneous[:, :3] / right_hand_transformed_points_homogeneous[:, [3]] # 归一化right_hand_back_projected_points [self._back_project_point(point, o3d_depth_intrinsic.intrinsic_matrix) for point in right_hand_points_to_project] # 反投影for i in range(len(right_hand_back_projected_points)): # 遍历右手关节点u, v right_hand_back_projected_points[i] # 获取投影点坐标if i in [0, 1, 5, 9, 13, 17]:cv2.circle(color_image, (u, v), 10, (0, 255, 0), -1) # 绘制绿色圆点elif i in [4, 8, 12, 16, 20]:cv2.circle(color_image, (u, v), 10, (255, 0, 0), -1) # 绘制蓝色圆点else:cv2.circle(color_image, (u, v), 10, (0, 0, 255), -1) # 绘制红色圆点if i in [1, 2, 3, 4]:cv2.circle(color_image, (u, v), 10, (255, 0, if vis_2d: # 如果启用了2D可视化color_image np.asarray(rgbd.color) # 获取彩色图像color_image cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) # 转换颜色空间left_hand_joint_points_homogeneous np.hstack((left_hand_joint_xyz, np.ones((left_hand_joint_xyz.shape[0], 1)))) # 转换为齐次坐标left_hand_transformed_points_homogeneous np.dot(left_hand_joint_points_homogeneous, np.linalg.inv(corrected_pose).T) # 应用逆变换left_hand_points_to_project left_hand_transformed_points_homogeneous[:, :3] / left_hand_transformed_points_homogeneous[:, [3]] # 归一化left_hand_back_projected_points [self._back_project_point(point, o3d_depth_intrinsic.intrinsic_matrix) for point in left_hand_points_to_project] # 反投影for i in range(len(left_hand_back_projected_points)): # 遍历左手关节点u, v left_hand_back_projected_points[i] # 获取投影点坐标if i in [0, 1, 5, 9, 13, 17]:cv2.circle(color_image, (u, v), 10, (0, 255, 0), -1) # 绘制绿色圆点elif i in [4, 8, 12, 16, 20]:cv2.circle(color_image, (u, v), 10, (255, 0, 0), -1) # 绘制蓝色圆点else:cv2.circle(color_image, (u, v), 10, (0, 0, 255), -1) # 绘制红色圆点if i in [1, 2, 3, 4]:cv2.circle(color_image, (u, v), 10, (255, 0, 255), -1) # 绘制紫色圆点right_hand_joint_points_homogeneous np.hstack((right_hand_joint_xyz, np.ones((right_hand_joint_xyz.shape[0], 1)))) # 转换为齐次坐标right_hand_transformed_points_homogeneous np.dot(right_hand_joint_points_homogeneous, np.linalg.inv(corrected_pose).T) # 应用逆变换right_hand_points_to_project right_hand_transformed_points_homogeneous[:, :3] / right_hand_transformed_points_homogeneous[:, [3]] # 归一化right_hand_back_projected_points [self._back_project_point(point, o3d_depth_intrinsic.intrinsic_matrix) for point in right_hand_points_to_project] # 反投影for i in range(len(right_hand_back_projected_points)): # 遍历右手关节点u, v right_hand_back_projected_points[i] # 获取投影点坐标if i in [0, 1, 5, 9, 13, 17]:cv2.circle(color_image, (u, v), 10, (0, 255, 0), -1) # 绘制绿色圆点elif i in [4, 8, 12, 16, 20]:cv2.circle(color_image, (u, v), 10, (255, 0, 0), -1) # 绘制蓝色圆点else:cv2.circle(color_image, (u, v), 10, (0, 0, 255), -1) # 绘制红色圆点if i in [1, 2, 3, 4]:cv2.circle(color_image, (u, v), 10, (255, 0, 最后使用Open3D和OpenCV库来处理和现实3D和2D数据 以下是2D可视化部分 将RGBD图像的彩色部分转换为NumPy数组并从RGB格式转换为BGR格式 if vis_2d: # 如果启用了2D可视化color_image np.asarray(rgbd.color) # 将RGBD图像的彩色部分转换为NumPy数组color_image cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) # 将图像从RGB格式转换为BGR格式 处理左手和右手关节数据将其转换为齐次坐标应用校正矩阵转换为 3D坐标并反向投影到图像平面 在图像上绘制不同颜色的圆点表示不同的关节点 # 处理左手关节数据left_hand_joint_points_homogeneous np.hstack((left_hand_joint_xyz, np.ones((left_hand_joint_xyz.shape[0], 1)))) # 将左手关节位置转换为齐次坐标left_hand_transformed_points_homogeneous np.dot(left_hand_joint_points_homogeneous, np.linalg.inv(corrected_pose).T) # 应用校正矩阵left_hand_points_to_project left_hand_transformed_points_homogeneous[:, :3] / left_hand_transformed_points_homogeneous[:, [3]] # 将齐次坐标转换为3D坐标left_hand_back_projected_points [self._back_project_point(point, o3d_depth_intrinsic.intrinsic_matrix) for point in left_hand_points_to_project] # 反向投影到图像平面for i in range(len(left_hand_back_projected_points)): # 遍历所有左手关节点u, v left_hand_back_projected_points[i] # 获取关节点的图像坐标if i in [0, 1, 5, 9, 13, 17]: # 如果关节点是手腕或指根cv2.circle(color_image, (u, v), 10, (0, 255, 0), -1) # 画绿色圆点elif i in [4, 8, 12, 16, 20]: # 如果关节点是指尖cv2.circle(color_image, (u, v), 10, (255, 0, 0), -1) # 画蓝色圆点else: # 其他关节点cv2.circle(color_image, (u, v), 10, (0, 0, 255), -1) # 画红色圆点if i in [1, 2, 3, 4]: # 如果关节点是拇指cv2.circle(color_image, (u, v), 10, (255, 0, 255), -1) # 画紫色圆点# 处理右手关节数据right_hand_joint_points_homogeneous np.hstack((right_hand_joint_xyz, np.ones((right_hand_joint_xyz.shape[0], 1)))) # 将右手关节位置转换为齐次坐标right_hand_transformed_points_homogeneous np.dot(right_hand_joint_points_homogeneous, np.linalg.inv(corrected_pose).T) # 应用校正矩阵right_hand_points_to_project right_hand_transformed_points_homogeneous[:, :3] / right_hand_transformed_points_homogeneous[:, [3]] # 将齐次坐标转换为3D坐标right_hand_back_projected_points [self._back_project_point(point, o3d_depth_intrinsic.intrinsic_matrix) for point in right_hand_points_to_project] # 反向投影到图像平面for i in range(len(right_hand_back_projected_points)): # 遍历所有右手关节点u, v right_hand_back_projected_points[i] # 获取关节点的图像坐标if i in [0, 1, 5, 9, 13, 17]: # 如果关节点是手腕或指根cv2.circle(color_image, (u, v), 10, (0, 255, 0), -1) # 画绿色圆点elif i in [4, 8, 12, 16, 20]: # 如果关节点是指尖cv2.circle(color_image, (u, v), 10, (255, 0, 0), -1) # 画蓝色圆点else: # 其他关节点cv2.circle(color_image, (u, v), 10, (0, 0, 255), -1) # 画红色圆点if i in [1, 2, 3, 4]: # 如果关节点是拇指cv2.circle(color_image, (u, v), 10, (255, 0, 255), -1) # 画紫色圆点 显示带有反向投影点的图像并在按下q键时退出铺环 cv2.imshow(Back-projected Points on Image, color_image) # 显示带有反向投影点的图像# 如果按下q键退出循环if cv2.waitKey(1) 0xFF ord(q):return # 退出函数 以下是3D可视化部分 设置右手关节球体的位置 创建或更新右手关节之问的连线 # 设置关节球体和连线 for i, sphere in enumerate(self.right_joints): # 遍历右手关节球体transformation np.eye(4) # 创建4x4单位矩阵transformation[:3, 3] right_hand_joint_xyz[i] - sphere.get_center() # 计算平移向量sphere.transform(transformation) # 应用平移变换 for i, (x, y) in enumerate(lines): # 遍历连线start self.right_joints[x].get_center() # 获取连线起点end self.right_joints[y].get_center() # 获取连线终点create_or_update_cylinder(start, end, cylinder_listself.right_line_set, cylinder_idxi) # 创建或更新连线的圆柱体return True # 返回True表示成功 第二部分 STEP2_build_dataset // 待更 第三部分 STEP3_train_policy // 待更