Dexcap复当代码数据预处理全流程（一）——replay_human_traj_vis.py ...

此脚本的重要功能是通过视觉化工具和键盘交互，动态调解和校准人体轨迹数据中的点云数据（PCD）和位姿（pose）

• 校准模式下，通过键盘输入动态调解点云与位姿偏移量，并生存调解结果
  
• 非校准模式下，按顺序回放全部帧的点云和位姿数据
  

起首，录制完的数据结构如下：

1. save_data_scenario_1
2. ├── frame_0
3. │   ├── color_image.jpg           # Chest camera RGB image
4. │   ├── depth_image.png           # Chest camera depth image
5. │   ├── pose.txt                  # Chest camera 6-DoF pose in world frame
6. │   ├── pose_2.txt                # Left hand 6-DoF pose in world frame
7. │   ├── pose_3.txt                # Right hand 6_DoF pose in world frame
8. │   ├── left_hand_joint.txt       # Left hand joint positions (3D) in the palm frame
9. │   └── right_hand_joint.txt      # Right hand joint positions (3D) in the palm frame
10. ├── frame_1
11. └── ...

复制代码

为了可视化收集的数据，表现点云和捕获的手部动作，可以运行：

1. cd DexCap/STEP1_collect_data
2. python replay_human_traj_vis.py --directory save_data_scenario_1

复制代码

此外，此脚本还提供了一个用于纠正 SLAM 初始漂移的接口。运行以下脚本，并使用数字键来纠正漂移，校正将应用于整个视频：

1. python replay_human_traj_vis.py --directory save_data_scenario_1 --calib
2. python calculate_offset_vis_calib.py --directory save_data_scenario_1

复制代码

接下来具体解释一下此脚本的代码逻辑

目录
1 库函数引用
2 主函数
3 ReplayDataVisualizer 类
4 键盘交互逻辑
5 pycharm 优化

---

1 库函数引用

1. """
2. 可视化保存的点云（PCD）文件和位姿
4. 使用示例：
5. (1) 可视化所有帧：
6. python replay_human_traj_vis.py --directory ./saved_data/
8. (2) 用于校准：
9. python replay_human_traj_vis.py --directory ./saved_data/ -calib
10. """
12. import argparse  # 用于解析命令行参数
13. import os  # 用于文件和目录操作
14. import copy  # 用于创建对象的副本
15. import zmq  # 用于进程间通信（在本脚本中未直接使用）
16. import cv2  # 用于图像处理（在本脚本中未直接使用）
17. import sys  # 用于系统特定参数和功能
18. import shutil  # 用于文件操作，如删除目录
19. import open3d as o3d  # 用于3D可视化
20. import numpy as np  # 用于数值计算
21. import platform  # 用于判断操作系统
22. from pynput import keyboard  # 用于监听键盘输入
23. from visualizer import *  # 导入自定义可视化工具

复制代码

2 主函数

1. if __name__ == "__main__":
2.     parser = argparse.ArgumentParser(description="可视化保存的帧数据。")  # 创建参数解析器
3.     parser.add_argument("--directory", type=str, default="./saved_data", help="保存数据的目录")  # 数据目录参数
4.     parser.add_argument("--default", type=str, default="default_offset", help="默认校准目录")  # 默认校准偏移目录
5.     parser.add_argument("--calib", action='store_true', help="启用校准模式")  # 校准模式标志
7.     args = parser.parse_args()  # 解析命令行参数
9.     assert os.path.exists(args.directory), f"给定的目录不存在: {args.directory}"  # 检查目录是否存在
10.     visualizer = ReplayDataVisualizer(args.directory)  # 初始化可视化工具
12.     if args.calib:  # 如果启用了校准模式
13.         # 加载校准偏移量
14.         visualizer.right_hand_offset = np.loadtxt(os.path.join(args.default, "calib_offset.txt"))
15.         visualizer.right_hand_ori_offset = np.loadtxt(os.path.join(args.default, "calib_ori_offset.txt"))
16.         visualizer.left_hand_offset = np.loadtxt(os.path.join(args.default, "calib_offset_left.txt"))
17.         visualizer.left_hand_ori_offset = np.loadtxt(os.path.join(args.default, "calib_ori_offset_left.txt"))
19.         # 检查临时校准目录是否存在
20.         if os.path.exists("tmp_calib"):
21.             response = (
22.                 input(
23.                     f"tmp_calib 已存在。是否覆盖？(y/n): "
24.                 )
25.                 .strip()
26.                 .lower()
27.             )
28.             if response != "y":  # 如果用户选择不覆盖
29.                 print("退出程序，未覆盖现有目录。")
30.                 sys.exit()  # 退出程序
31.             else:
32.                 shutil.rmtree("tmp_calib")  # 删除现有目录
33.         os.makedirs("tmp_calib", exist_ok=True)  # 创建临时校准目录
34.         visualizer.replay_keyframes_calibration()  # 启动校准模式
35.     else:  # 如果未启用校准模式
36.         # 加载校准偏移量
37.         visualizer.right_hand_offset = np.loadtxt("{}/calib_offset.txt".format(args.directory))
38.         visualizer.right_hand_ori_offset = np.loadtxt("{}/calib_ori_offset.txt".format(args.directory))
39.         visualizer.left_hand_offset = np.loadtxt("{}/calib_offset_left.txt".format(args.directory))
40.         visualizer.left_hand_ori_offset = np.loadtxt("{}/calib_ori_offset_left.txt".format(args.directory))
41.         visualizer.replay_all_frames()  # 回放所有帧

复制代码

代码入口 if __name__ == "__main__":，分析命令行参数并执行不同模式的逻辑，包括：
1. 检查 --directory 指定的目录是否存在
2. 如果启用校准模式 (--calib):

• 加载默认偏移值文件（如 calib_offset.txt 和 calib_ori_offset.txt）
  
• 检查并处理大概已有的临时校准文件目录（tmp_calib）
  
• 调用 replay_keyframes_calibration 方法启动校准模式
  

否则，加载对应帧数据并调用 replay_all_frames 方法，逐帧回放全部数据
3. 数据校验与文件生存
校验数据目录和默认偏移文件是否存在
确保 tmp_calib 目录可用，如果已有旧文件，会提示用户是否覆盖。
校准结果存储为 .txt 文件，按帧编号定名（如 frame_10.txt）

3 ReplayDataVisualizer 类

1. # 可视化类，用于回放和校准 PCD 数据
2. class ReplayDataVisualizer(DataVisualizer):
3.     def __init__(self, directory):
4.         super().__init__(directory)  # 使用数据目录初始化基类
6.     def replay_keyframes_calibration(self):
7.         """
8.         可视化并逐帧校准。
9.         """
10.         global delta_movement_accu, delta_ori_accu, next_frame, frame
11.         if self.R_delta_init is None:  # 检查是否初始化了基准帧
12.             self.initialize_canonical_frame()
14.         self._load_frame_data(frame)  # 加载当前帧的数据
16.         # 将 3D 对象添加到可视化工具中
17.         self.vis.add_geometry(self.pcd)
18.         self.vis.add_geometry(self.coord_frame_1)
19.         self.vis.add_geometry(self.coord_frame_2)
20.         self.vis.add_geometry(self.coord_frame_3)
21.         for joint in self.left_joints + self.right_joints:
22.             self.vis.add_geometry(joint)
23.         for cylinder in self.left_line_set + self.right_line_set:
24.             self.vis.add_geometry(cylinder)
26.         next_frame = True  # 准备加载下一帧
27.         try:
28.             with keyboard.Listener(on_press=on_press) as listener:  # 启动键盘监听器
29.                 while True:
30.                     if next_frame == True:  # 如果准备好，加载下一帧
31.                         next_frame = False
32.                         frame += 10
33.                     self._load_frame_data(frame)  # 加载帧数据
35.                     self.step += 1  # 步数计数器递增
37.                     # 更新可视化工具中的几何体
38.                     self.vis.update_geometry(self.pcd)
39.                     self.vis.update_geometry(self.coord_frame_1)
40.                     self.vis.update_geometry(self.coord_frame_2)
41.                     self.vis.update_geometry(self.coord_frame_3)
42.                     for joint in self.left_joints + self.right_joints:
43.                         self.vis.update_geometry(joint)
44.                     for cylinder in self.left_line_set + self.right_line_set:
45.                         self.vis.update_geometry(cylinder)
47.                     self.vis.poll_events()  # 处理可视化事件
48.                     self.vis.update_renderer()  # 更新可视化渲染器
49.                 listener.join()  # 等待监听器结束
50.         finally:
51.             print("cumulative_correction ", self.cumulative_correction)  # 打印累计修正值

复制代码

此类继续自 DataVisualizer，实现了点云和轨迹数据的校准与回放功能
1. 初始化
通过 __init__ 方法调用父类构造函数，初始化路径和可视化工具
校准模式下，加载偏移量并初始化临时存储目录
2. 校准方法 replay_keyframes_calibration
核心方法，用于处理校准模式下逐帧的动态调解：

• 如果尚未初始化基准帧（R_delta_init），调用 initialize_canonical_frame
  
• 加载当前帧的点云和姿态数据，并添加到 open3d 的可视化窗口
  
• 启动键盘监听器（keyboard.Listener），及时监听按键调解点云和位姿的偏移量
  
• 每次按下生存键时，更新临时文件中的偏移量，并加载下一帧
  

4 键盘交互逻辑

1. # 脚本中使用的全局变量
2. def on_press(key):
3.     global next_frame, delta_movement_accu, delta_ori_accu, delta_movement_accu_left, delta_ori_accu_left, adjust_movement, adjust_right, frame, step
5.     # 判断当前操作系统类型
6.     os_type = platform.system()
8.     # 根据当前模式（右手/左手，平移/旋转）选择需要调整的数据
9.     if adjust_right:
10.         data_to_adjust = delta_movement_accu if adjust_movement else delta_ori_accu
11.     else:
12.         data_to_adjust = delta_movement_accu_left if adjust_movement else delta_ori_accu_left
14.     if os_type == "Linux":  # Linux 特定的按键绑定
15.         # 根据按键调整平移/旋转偏移量
16.         if key.char == '6':  # 沿负 x 方向移动
17.             data_to_adjust[0] += step
18.         elif key.char == '4':  # 沿正 x 方向移动
19.             data_to_adjust[0] -= step
20.         elif key.char == '8':  # 沿正 y 方向移动
21.             data_to_adjust[1] += step
22.         elif key.char == '2':  # 沿负 y 方向移动
23.             data_to_adjust[1] -= step
24.         elif key.char == "7":  # 沿正 z 方向移动
25.             data_to_adjust[2] += step
26.         elif key.char == "9":  # 沿负 z 方向移动
27.             data_to_adjust[2] -= step
28.         elif key.char == "3":  # 重置所有偏移量
29.             delta_movement_accu *= 0.0
30.             delta_ori_accu *= 0.0
31.             delta_movement_accu_left *= 0.0
32.             delta_ori_accu_left *= 0.0
33.             next_frame = True
34.         elif key.char == "0":  # 保存偏移量并加载下一帧
35.             # 将偏移量保存到临时校准文件
36.             if (delta_movement_accu != np.array([0.0, 0.0, 0.0])).any() or (delta_ori_accu != np.array([0.0, 0.0, 0.0])).any() or (delta_movement_accu_left != np.array([0.0, 0.0, 0.0])).any() or (delta_ori_accu_left != np.array([0.0, 0.0, 0.0])).any():
37.                 frame_dir = "./tmp_calib/"
38.                 np.savetxt(os.path.join(frame_dir, f"frame_{frame}.txt"), delta_movement_accu)
39.                 np.savetxt(os.path.join(frame_dir, f"frame_{frame}_ori.txt"), delta_ori_accu)
40.                 np.savetxt(os.path.join(frame_dir, f"frame_{frame}_left.txt"), delta_movement_accu_left)
41.                 np.savetxt(os.path.join(frame_dir, f"frame_{frame}_ori_left.txt"), delta_ori_accu_left)
43.             # 重置偏移量，准备处理下一帧
44.             delta_movement_accu *= 0.0
45.             delta_ori_accu *= 0.0
46.             delta_movement_accu_left *= 0.0
47.             delta_ori_accu_left *= 0.0
48.             next_frame = True
49.         elif key == keyboard.Key.space:  # 切换平移和旋转调整模式
50.             adjust_movement = not adjust_movement
51.         elif key == keyboard.Key.enter:  # 切换左右手调整模式
52.             adjust_right = not adjust_right
53.         else:
54.             print("Key error", key)  # 对于不支持的按键打印错误
56.     elif os_type == "Windows":  # Windows 特定的按键绑定
57.         # 与 Linux 类似的逻辑，但使用不同的按键（方向键、Page Up/Down 等）
58.         if key == keyboard.Key.right:  # 沿负 x 方向移动
59.             data_to_adjust[0] += step
60.         elif key == keyboard.Key.left:  # 沿正 x 方向移动
61.             data_to_adjust[0] -= step
62.         elif key == keyboard.Key.up:  # 沿正 y 方向移动
63.             data_to_adjust[1] += step
64.         elif key == keyboard.Key.down:  # 沿负 y 方向移动
65.             data_to_adjust[1] -= step
66.         elif key == keyboard.Key.home:  # 沿正 z 方向移动
67.             data_to_adjust[2] += step
68.         elif key == keyboard.Key.page_up:  # 沿负 z 方向移动
69.             data_to_adjust[2] -= step
70.         elif key == keyboard.Key.page_down:  # 重置偏移量
71.             delta_movement_accu *= 0.0
72.             delta_ori_accu *= 0.0
73.             delta_movement_accu_left *= 0.0
74.             delta_ori_accu_left *= 0.0
75.             next_frame = True
76.         elif key == keyboard.Key.insert:  # 保存偏移量并加载下一帧
77.             # 将偏移量保存到临时校准文件
78.             if (delta_movement_accu != np.array([0.0, 0.0, 0.0])).any() or (delta_ori_accu != np.array([0.0, 0.0, 0.0])).any() or (delta_movement_accu_left != np.array([0.0, 0.0, 0.0])).any() or (delta_ori_accu_left != np.array([0.0, 0.0, 0.0])).any():
79.                 frame_dir = "./tmp_calib/"
80.                 np.savetxt(os.path.join(frame_dir, f"frame_{frame}.txt"), delta_movement_accu)
81.                 np.savetxt(os.path.join(frame_dir, f"frame_{frame}_ori.txt"), delta_ori_accu)
82.                 np.savetxt(os.path.join(frame_dir, f"frame_{frame}_left.txt"), delta_movement_accu_left)
83.                 np.savetxt(os.path.join(frame_dir, f"frame_{frame}_ori_left.txt"), delta_ori_accu_left)
85.             # 重置偏移量，准备处理下一帧
86.             delta_movement_accu *= 0.0
87.             delta_ori_accu *= 0.0
88.             delta_movement_accu_left *= 0.0
89.             delta_ori_accu_left *= 0.0
90.             next_frame = True
91.         elif key == keyboard.Key.space:  # 切换平移和旋转调整模式
92.             adjust_movement = not adjust_movement
93.         elif key == keyboard.Key.enter:  # 切换左右手调整模式
94.             adjust_right = not adjust_right
95.         else:
96.             print("Key error", key)  # 对于不支持的按键打印错误

复制代码

键盘输入是代码的交互核心，界说在 on_press 方法中
1. 调解逻辑
（1）判定调解目标：

• 右手（adjust_right=True）或左手（adjust_right=False）
  
• 调解移动偏移量（adjust_movement=True）或姿态偏移量（adjust_movement=False）
  

（2）根据操纵系统选择按键：

• Linux 使用数字键（如 6 表示 x 轴负方向移动）
  
• Windows 使用方向键和功能键（如 Key.right 表示 x 轴负方向移动）
  

2. 特别按键功能

• Space：切换移动和姿态调解模式
  
• Enter：切换左右手调解
  
• 0 / Insert：生存当前偏移量到文件并加载下一帧
  
• 3 / Page Down：重置全部偏移量
  

3. 偏移量存储

• 偏移量存储在 delta_movement_accu 和 delta_ori_accu 等全局变量中
  
• 生存时调用 np.savetxt，将每帧的调解结果存储到临时目录（tmp_calib）
  

5 pycharm 优化

因为想用 pychram 调试，以是简单改一下进入条件，酿成选择模式：

1. # 添加用户交互菜单
2. print("Select mode:")
3. print("1. Calibration Mode")
4. print("2. Visualization Mode")
5. mode = input("Enter your choice (1/2): ").strip()
7. if mode == "1":
8.     print("Entering calibration mode...")
9.     visualizer.right_hand_offset = np.loadtxt(os.path.join(args.default, "calib_offset.txt"))
10.     visualizer.right_hand_ori_offset = np.loadtxt(os.path.join(args.default, "calib_ori_offset.txt"))
11.     visualizer.left_hand_offset = np.loadtxt(os.path.join(args.default, "calib_offset_left.txt"))
12.     visualizer.left_hand_ori_offset = np.loadtxt(os.path.join(args.default, "calib_ori_offset_left.txt"))
14.     # 检查临时校准目录是否存在
15.     if os.path.exists("tmp_calib"):
16.         response = input("tmp_calib already exists. Do you want to override? (y/n): ").strip().lower()
17.         if response != "y":
18.             print("Exiting program without overriding the existing directory.")
19.             sys.exit()
20.         else:
21.             shutil.rmtree("tmp_calib")
22.     os.makedirs("tmp_calib", exist_ok=True)
23.     visualizer.replay_keyframes_calibration()  # 启动校准模式
24. elif mode == "2":
25.     print("Entering visualization mode...")
26.     visualizer.right_hand_offset = np.loadtxt(f"{args.directory}/calib_offset.txt")
27.     visualizer.right_hand_ori_offset = np.loadtxt(f"{args.directory}/calib_ori_offset.txt")
28.     visualizer.left_hand_offset = np.loadtxt(f"{args.directory}/calib_offset_left.txt")
29.     visualizer.left_hand_ori_offset = np.loadtxt(f"{args.directory}/calib_ori_offset_left.txt")
30.     visualizer.replay_all_frames()  # 启动可视化模式
31. else:
32.     print("Invalid choice. Exiting program.")
33.     sys.exit()

复制代码

此时再运行会进入选择：

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。