Method and system for synchronized visualization of point cloud and model inferred trajectory collected by robot

Abstract

The application provides a kind of robot collection point cloud and model inference trajectory synchronous visualization method and system, to collect the camera timestamp in data as benchmark, the point cloud of current frame, robot TCP pose and end force data are aligned, and are transformed to world coordinate system uniformly;Based on current frame, construct inference observation, initiate model inference request in independent thread, and asynchronously superimpose the latest returned prediction trajectory on current three-dimensional scene.The camera timestamp in inference observation records the moment when the request is initiated, which is used to represent the inference request time.The system reduces the point cloud processing load by voxel downsampling, avoids blocking the rendering main loop by asynchronous inference, and runs the bounding box parameter dialog box in a separate subprocess to isolate the graphical interface event loop.The main rendering callback updates the bounding box and continues rendering after the subprocess returns, to improve the evaluability of the collected data and strategy trajectory, the debugging and labeling efficiency.

Description

Technical Field

[0001] This invention relates to the field of robot vision and 3D visualization technology, specifically to a method and system for synchronously visualizing point clouds acquired by a robot and waypoint trajectories inferred from a model. More particularly, it relates to a method, apparatus, and system for real-time synchronous 3D visualization in a robot system comprised of an RGB-D vision sensor, a single-arm or dual-arm robotic arm, an end effector, and an imitation learning (IL) inference service, unifying the real-world point cloud reconstructed from the acquired dataset, the robotic arm's TCP coordinate system, the end effector force vector, and the waypoint trajectory inferred in real-time by the neural network strategy into the same world coordinate system.

[0002] This invention can be applied to robot systems consisting of RGB-D vision sensors (including a global camera and an inhand camera), single-arm or dual-arm robotic arms, end effectors (grippers), six-dimensional force sensors, imitation learning inference servers, and graphics rendering units (Open3D windows). It is suitable for scenarios that require visual evaluation, debugging, and annotation of the quality of collected data and strategy trajectories, such as manipulating flexible objects like folding towels, sorting parts, loading and unloading materials, and assembly. Background Technology

[0003] In robot operating systems based on deep imitation learning, engineers need to repeatedly collect teaching / running datasets (RGB-D image sequences, proprioception, torque, etc.) and train vision-action policies to drive the robotic arm to complete tasks. To evaluate the "quality of collected data" and "quality of policy inference," it is necessary to intuitively present the real scene seen by the sensors and the actions predicted by the policy. However, the collected data is multi-source, multi-coordinate system, and high-volume, while policy inference has a fluctuating latency of tens to hundreds of milliseconds. How to present "real point cloud + robotic arm pose + force feedback + predicted trajectory" in the same 3D scene in real time, smoothly, and synchronously is an engineering challenge.

[0004] Existing technical solution 1: Offline / segmented window 2D data playback.

[0005] The most common approach is to use an image viewer to review the RGB and depth maps frame by frame, or to use separate curve tools to view the TCP and torque curves. This approach is simple to implement, but it has the following drawbacks: RGB, depth, TCP, and torque are scattered across different views, making it difficult for humans to determine the true relative relationship between depth pixels and the robotic arm's end effector and the manipulated object in 3D space. The 2D view cannot align the 3D waypoint trajectory output by the neural network with the real scene, making it difficult to determine whether the strategy is "aligned" with the target object (such as the edge of a towel). The global camera, the inhand camera, and the coordinate systems of each arm are different, and offline tools generally do not perform a unified coordinate transformation, resulting in a mismatch between the point cloud and the pose.

[0006] Existing technical solution 2: Simple real-time 3D rendering (without downsampling and synchronous blocking).

[0007] Another approach directly feeds the raw, dense point cloud into the 3D rendering window and simultaneously invokes strategy inference and pop-up interaction within the main rendering loop. While this achieves 3D display, it suffers from the following drawbacks: The raw RGB-D backprojection yields hundreds of thousands of points, which are then uploaded to the GPU without voxel downsampling, resulting in high memory consumption, rendering stutters, and difficulty in smooth playback. Simultaneously waiting for the inference service to return within the main rendering loop (tens to hundreds of milliseconds) can cause screen freezes and unresponsive interactions. Directly popping up the graphics parameter dialog box within the main rendering loop often leads to window freezes or crashes due to conflicts between the event loop and the Global Interpreter Lock (GIL), making interactive adjustment of the clipping area difficult. Without TCP pose transformation, the hand-eye camera point cloud becomes misaligned with the global camera point cloud and the robotic arm pose, failing to form a unified visualization scene.

[0008] Patent application CN119141528A discloses an integrated communication and positioning method for a swarm of intelligent machines, comprising: intelligent robots constructing query vectors from currently perceived radar point cloud data, calculating spatial importance information, and extracting intermediate features for communication interaction through a mid-term feature extraction and interaction module; a base station clustering intelligent robots through an optimal communication link construction module, allocating optimal communication links between robots within each cluster, and then completing the communication links at the inter-cluster boundaries; and intelligent robots processing the mid-term feature time sequence sent by the cooperating positioning object and the feature map sequence they themselves collect through a temporal relative positioning module to obtain the relative positional relationship between robots. However, this patent cannot completely solve the existing technical problems, nor can it meet the needs of this invention.

[0009] Explanation of terminology in this invention: Point cloud: A discrete geometric representation composed of a large number of three-dimensional points, each point containing three-dimensional coordinates (x, y, z) (unit: m) and optional colors (r, g, b). This invention uses a point cloud reconstruction module to reconstruct a point cloud from an RGB-D image and camera intrinsic parameters via backprojection, for use in the three-dimensional visualization of real-world scene data acquisition.

[0010] RGB-D Image: A color image (RGB) and a depth image simultaneously output by an RGB-D vision camera. The depth image is represented as an integer and needs to be divided by a depth scaling factor (1000.0 in this invention, i.e., millimeters to meters) to restore it to the true depth in meters.

[0011] Camera Intrinsics: A parameter matrix describing the camera's imaging geometry, containing focal lengths fx and fy (per unit pixel) and principal points (optical centers) cx and cy (per unit pixel). This invention uses it to back-project depth pixels (u, v, d) into three-dimensional points in the camera coordinate system.

[0012] Camera Extrinsics: The 4×4 homogeneous transformation matrix M from the camera coordinate system to the world / robot coordinate system. The extrinsics of the global camera are given directly by calibration; the extrinsics of the inhand camera are defined in the TCP coordinate system and need to be multiplied by the real-time TCP pose of the arm to transform to the base coordinate system.

[0013] In-hand camera: A camera mounted on the wrist / endpoint of the robotic arm that moves with the endpoint, with its extrinsic parameters given in the TCP coordinate system. For visualization, the point cloud needs to be transformed from the TCP coordinate system to the world coordinate system using the arm's current TCP pose matrix (first multiply the extrinsic parameters by the TCP pose matrix on the left, then transform the point cloud); and the corresponding arm's TCP should be selected according to the left / right indication included in the camera type identifier.

[0014] Voxel Down-Sample: The 3D space is divided into cubic voxels of a given side length. Points within each voxel are replaced by their centroids, thereby sparsifying the point cloud. This invention uses a voxel side length of 0.01m (1cm), which significantly reduces the number of points while maintaining the scene structure, thus reducing memory / GPU memory usage and rendering load.

[0015] 7-D Pose Vector: This invention uses a 7-dimensional vector to represent a rigid body pose, namely a three-dimensional translation (x,y,z) (unit m) and a unit quaternion pose (qw,qx,qy,qz); a 4×4 homogeneous transformation matrix can be obtained from the pose-matrix transformation, and the 7-D pose can be restored from the matrix in reverse.

[0016] Quaternion: A non-singular, compact three-dimensional rotation representation, denoted as q = (qw, qx, qy, qz) and satisfying ||q|| = 1. This invention uses a standard method for converting between quaternions and 3×3 rotation matrices.

[0017] TCP (Tool Center Point): The tool center point at the end of the robotic arm. This invention describes its position and orientation in the arm base coordinate system in 7-dimensional pose, and after being transformed to the world coordinate system by the transformation matrix from the arm base to the world coordinate system, it is rendered as a coordinate system (three-color arrows on the coordinate axes).

[0018] End effector state (end_effector): The state quantity of the actuator such as the gripper at the end of the robotic arm. In this invention, it is described by a scalar (such as gripper width / opening degree) and is used as the input observation (data field end_effector) for inference service along with proprioception.

[0019] Force / Torque: The end effector's six-dimensional force sensor outputs high-frequency force and torque data, with each arm having six dimensions. The first three dimensions represent forces fx, fy, and fz, in N, while the latter three dimensions represent torques mx, my, and mz, in N·m. During inference observation, this invention applies a rotation matrix to the arm base for both the force and torque components. During 3D rendering, only the first three-dimensional force components are extracted and rotated to the world coordinate system before being displayed as adaptive arrows; arrows are not drawn for the latter three-dimensional torque components.

[0020] Axis-Aligned Bounding Box (AABB): A cuboid bounding box with each edge parallel to the coordinate axes, defined by the minimum corner point (x_min, y_min, z_min) and the maximum corner point (x_max, y_max, z_max). This invention uses it as a region of interest (ROI) to clip the point cloud, retaining only the points within the bounding box.

[0021] Imitation Learning Policy: A deep neural network policy trained on teaching data, taking visual and proprioceptive observations as input, and outputting a sequence of waypoint actions for a future time period. This invention invokes the service via HTTP inference, returning the data fields action_arm, action_ee, and termination.

[0022] Predicted Trajectory: A sequence of waypoints output by a single inference iteration of the imitation learning strategy (data field action_arm), with a shape of (traj_len, num_arms, dim). This invention renders each waypoint as a small sphere, colors it according to the termination signal (red / green), and overlays it onto the acquired point cloud.

[0023] Termination signal: A scalar value (data field termination) output by the policy for each waypoint. A value greater than 0 indicates that the task / subsegment at that waypoint should be terminated. Based on this, the inference trajectory sphere is colored: red for termination > 0, and green otherwise, to visually identify the termination segment of the trajectory.

[0024] Open3D Visualization Window: A real-time 3D rendering window based on Open3D (supports keyboard callbacks). It refreshes and updates the screen through periodic event polling and rendering, and registers keyboard callbacks to enable interactive functions such as playback, frame-by-frame rendering, framing, and cropping.

[0025] Inference Latency: The time elapsed from initiating a policy inference request to obtaining a new waypoint sequence. This invention uses an independent inference visualization thread to asynchronously poll the inference service at a fixed period (approximately 0.5 seconds), ensuring that inference latency does not block the Open3D rendering main loop.

[0026] Process Isolation: This mechanism places the GUI event loop of the bounding box parameter dialog box into a separate child process and returns the results via a cross-process queue, thus isolating the dialog box from the Open3D window's graphical interface resources. The main rendering callback waits for this child process to finish during user input. Therefore, this mechanism is used to reduce the risk of direct conflicts between different GUI event loops, rather than to keep the parameter input process continuously rendered. Summary of the Invention

[0027] To address the shortcomings of existing technologies, the purpose of this invention is to provide a method and system for synchronously visualizing robot point cloud acquisition and model inference trajectories.

[0028] The method for synchronously visualizing robot point cloud acquisition and model inference trajectory according to the present invention includes: Step 1: Obtain the RGB image, depth image, and camera parameters of the current frame captured by the RGB-D vision camera; obtain the center point of the end effector and the state of the end effector, including the pose of the tool center point and the end force data; back-project the depth image to generate a point cloud; transform the point cloud to the world coordinate system according to the camera parameters and the pose of the tool center point of the end effector to obtain point cloud data. Step 2: Based on the RGB image and depth image of the current frame, as well as the center point of the robotic arm end effector and the state of the end effector, construct inference observations, asynchronously send inference requests to the model inference service, and receive the predicted trajectory returned by the model inference service; Step 3: Load the point cloud data, the tool center point pose of the robotic arm, and the predicted trajectory into the 3D scene, and perform visualization rendering; align the point cloud data, the tool center point pose of the robotic arm, and the end effector force data in time based on the camera timestamp; update the point cloud data in the 3D scene only when the frame index of the current frame changes; Step 4: Run the parameter input dialog box in a child process independent of the main process to obtain the bounding box parameters input by the user; the main process pauses rendering while the parameter input dialog box is running, and obtains the bounding box parameters through inter-process communication after the child process ends, and updates the axis-aligned bounding box in the 3D scene.

[0029] Preferably, in step 1, the point cloud generated by back projection is subjected to voxel downsampling. The voxel downsampling divides the three-dimensional space into cubic elements with a preset side length, and the points in each voxel are replaced by their centroids.

[0030] Preferably, in step 1, the point cloud is transformed to the world coordinate system according to the type identifier of the RGB-D vision camera: For a global camera, the point cloud is transformed from the camera coordinate system to the world coordinate system based on the camera extrinsic parameters of the global camera. For the hand-eye camera, the point cloud is first transformed from the camera coordinate system to the tool center point coordinate system based on the camera extrinsic parameters of the hand-eye camera. Then, based on the tool center point pose of the robotic arm associated with the hand-eye camera, the point cloud is transformed from the tool center point coordinate system to the world coordinate system.

[0031] Preferably, based on the type identifier of the RGB-D vision camera, when the type identifier is right arm, the tool center point at the end of the right arm is taken as the tool center point pose of the robotic arm associated with the hand-eye camera; otherwise, the tool center point at the end of the left arm is taken as the tool center point pose of the robotic arm associated with the hand-eye camera.

[0032] Preferably, in step 2, in an inference thread independent of the main rendering loop, an inference request is sent to the model inference service at a preset period. Based on the waypoint positions and termination signals in the predicted trajectory returned by the model inference service, the position and color of the constructed trackball are updated. Specifically, when the termination signal is greater than zero, the corresponding trackball is colored red, and when the termination signal is less than or equal to zero, the corresponding trackball is colored green.

[0033] Preferably, in step 2, the camera timestamp in the inference observation records the time when the inference request is initiated, which is used to characterize the time of the inference request.

[0034] Preferably, in step 3, the tool center point pose of the robotic arm is visualized and rendered to generate a coordinate system indicator graphic representing the position and attitude of the robotic arm's end effector in the three-dimensional scene; the end effector force data of the robotic arm is visualized and rendered to generate a force vector indicator graphic from the tool center point position of the robotic arm in the three-dimensional scene. The length of the force vector indicator graphic is related to the magnitude of the force. When the magnitude of the force is greater than a preset threshold, the force vector indicator graphic is generated.

[0035] Preferably, the end force data is six-dimensional force / torque data, and the first three-dimensional force components are extracted to generate the force vector indicator graphic.

[0036] Preferably, in step 4, the point cloud is cropped according to the updated axis-aligned bounding box, retaining only the point cloud within the axis-aligned bounding box.

[0037] The synchronous visualization system for robot point cloud acquisition and model inference trajectory provided by the present invention includes: Data reading and point cloud reconstruction module: acquires the RGB image, depth image and camera parameters of the current frame captured by the RGB-D vision camera, acquires the center point of the end tool and the state of the end effector of the robotic arm, including the pose of the tool center point and the end force data of the robotic arm, back-projects the depth image to generate a point cloud, and transforms the point cloud to the world coordinate system according to the camera parameters and the pose of the tool center point of the robotic arm to obtain point cloud data; Inference module: It is communicatively connected to the data reading and point cloud reconstruction module, constructs inference observations based on the RGB image and depth image of the current frame, as well as the center point of the robotic arm end tool and the state of the end effector, asynchronously sends inference requests to the model inference service, and receives the predicted trajectory returned by the model inference service; The rendering module communicates with the data reading and point cloud reconstruction module and the inference module. It loads the point cloud data, the tool center point pose of the robotic arm, and the predicted trajectory into the 3D scene for visualization rendering. The rendering module uses the camera timestamp as a reference to perform time alignment on the point cloud data, the tool center point pose of the robotic arm, and the end effector force data. The rendering module only updates the point cloud data in the 3D scene when the frame index of the current frame changes. The parameter configuration module includes a subprocess that runs independently of the main process of the rendering module. The subprocess runs a parameter input dialog box to obtain bounding box parameters input by the user. The main process pauses rendering while the parameter input dialog box is running, and obtains the bounding box parameters through inter-process communication after the subprocess ends, and updates the axis-aligned bounding box in the 3D scene.

[0038] Compared with the prior art, the present invention has the following beneficial effects: (1) This invention compresses the dense point cloud generated by back projection through voxel downsampling, divides the three-dimensional space into cubes of a given side length and replaces all points in the voxel with the centroid, greatly reducing the amount of point cloud data, thereby significantly reducing the use of video memory and memory; at the same time, the rendering module only updates the point cloud data in the three-dimensional scene when the frame index of the current frame changes, avoiding unnecessary back projection and GPU upload overhead, and combined with axis-aligned bounding box clipping to remove background and irrelevant points, further reducing the number of points drawn per frame, thereby solving the technical problem of rendering stuttering and low frame rate caused by dense point clouds and improving rendering smoothness.

[0039] (2) This invention transforms the TCP pose of each arm from the arm base coordinate system to the world coordinate system through the transformation matrix of each arm base. For the global camera, the point cloud is transformed from the camera coordinate system to the world coordinate system according to the camera extrinsic parameters. For the hand-eye camera, the point cloud is first transformed from the camera coordinate system to the tool center point coordinate system according to the extrinsic parameters, and then transformed to the world coordinate system according to the current tool center point pose of the arm. This unifies the multi-path point cloud, the tool center point pose of the robotic arm, and the end effector force data into the same world coordinate system. Time alignment is performed based on the camera timestamp in the acquired data, thereby eliminating the misalignment of multiple coordinate systems and the posture timing deviation. This solves the technical problem that it is difficult to form a unified three-dimensional scene when RGB-D point cloud, TCP pose, and torque data are scattered in different coordinate systems.

[0040] (3) This invention places the model inference request in an inference thread independent of the main rendering loop and sends the request to the inference service asynchronously at a fixed period, so that the network wait and computation time of the inference request will not block the main rendering loop, thus solving the technical problem of screen freezing and unresponsive interaction caused by synchronous call to the inference service in the main rendering loop; at the same time, the in-situ position and color of the constructed trackball are updated according to the waypoint position and termination signal returned by the inference service, without adding or deleting geometry, thus avoiding rendering jitter caused by frequent addition and deletion of geometry, and realizing asynchronous overlay display of the inference trajectory and the real scene.

[0041] (4) This invention solves the technical problem of window freezing or crashing due to conflict between the graphical interface event loop and the global interpreter lock when the parameter dialog box is directly popped up in the main rendering loop. This is achieved by placing the bounding box parameter input dialog box in a subprocess independent of the main rendering process and using cross-process communication to return the bounding box boundary values ​​input by the user. The graphical interface event loop of the dialog box is isolated from the Open3D rendering window. This allows the bounding box parameters to be stably and interactively adjusted, and the point cloud is clipped based on the updated axis-aligned bounding box to focus the region of interest.

[0042] (5) This invention extracts the first three-dimensional force components from the six-dimensional force / torque data, rotates the force vector from the arm base coordinate system to the world coordinate system, and generates a force vector indicator graphic in the three-dimensional scene from the tool center point position of the robotic arm. The direction is along the force direction and the length is related to the force magnitude. The graphic is generated only when the force magnitude is greater than a preset threshold. This solves the technical problems of the lack of intuitive three-dimensional spatial presentation of force feedback data and the difficulty in qualitatively judging whether the contact force is normal. It makes the magnitude and direction of the gripping / contact force intuitively assessable. Attached Figure Description

[0043] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings: Figure 1 This is a system framework diagram of the synchronous visualization system of the present invention; Figure 2 This is a flowchart of the synchronous visualization method of the present invention; Figure 3 This is a schematic diagram illustrating the operating principle of the present invention; Figure 4 This is a graph showing the relationship between the acquisition frame time and the asynchronous inference request time in this invention. Figure 5 This is a schematic diagram illustrating the principle of selecting and rotating the end force arrow data in this invention. Detailed Implementation

[0044] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.

[0045] Example 1 The core technical problem this invention aims to solve is: under the condition of multiple data acquisition sources and policy inference with fluctuating latency, how to unify the real point cloud aligned with the acquisition frame timestamp, the robotic arm TCP coordinate system, and the end effector force vector into the same world coordinate system, and asynchronously overlay and display the latest model inference trajectory initiated based on the current frame; at the same time, it reduces the use of video memory and memory by voxel downsampling, avoids blocking the rendering main loop by using an independent inference thread, isolates the bounding box dialog box from the Open3D GUI event loop by using an independent subprocess, and resumes rendering after the dialog box returns, thereby improving the evaluability, debugging, and annotation efficiency of the acquired data and policy trajectory.

[0046] This invention provides a synchronous visualization system for acquiring point cloud data and real-time model inference trajectories. The system execution entity includes: RGB-D vision camera and data reading module: Provides color image (rgb) and depth image according to camera timestamp, and provides camera intrinsic parameters (fx, fy, cx, cy) and camera extrinsic parameters (4×4 matrix M); the camera is divided into global camera and wrist hand-eye camera, which are distinguished by the camera type identifier in the camera configuration information.

[0047] Single-arm or dual-arm robotic arm (the number of arms is determined by whether the robot is a dual-arm configuration, i.e., 2 or 1): provides the pose of each arm base to the world coordinate system (7 dimensions per arm, forming the transformation matrix of each arm base), and the TCP pose of each arm read in time-stamp alignment.

[0048] End effector and six-dimensional force sensor: Provides end effector status (data field end_effector) and high-frequency force / torque (data field force_torque, taking the most recent historical window).

[0049] Imitation learning inference service: Receives observations and natural language instructions via HTTP inference service, and returns the data fields action_arm, action_ee, and termination.

[0050] Graphics rendering and interaction unit: Based on the Open3D rendering window, register keyboard callbacks (space, play / pause, left / right arrow keys, frame-by-frame, ESC to exit, B to set the bounding box, C to clip); inference is performed in a separate inference visualization thread; the parameter dialog box pops up in a separate subprocess, and the main rendering callback waits for the subprocess to return parameters before updating the bounding box and continuing rendering.

[0051] System Input: Acquired dataset, including RGB-D image sequences, proprioceptive data (TCP pose, end-effector state), and optional high-frequency force / torque data (force_torque), as well as natural language instructions. System Output: Real-time rendered point cloud of the scene in the same world coordinate system, TCP coordinate systems of each arm, end-effector force adaptive arrows generated from the first three-dimensional force components of force_torque, and a sequence of waypoint trajectory balls for model inference. The latter three-dimensional torque components can be used as inference history observations but are not plotted as arrows.

[0052] The overall data flow is as follows: Step 1: The data reading module reads RGB-D according to the current frame index → ​​reconstructs the point cloud by backprojecting from the intrinsic parameters and downsampling from voxels → unifies the point cloud with the TCP pose T of the hand-eye camera and the extrinsic parameters M to the world coordinate system; Step 2: The inference visualization thread asynchronously calls the inference service to overlay the predicted trajectory onto the same scene; Step 3: Users can set the bounding box in a dialog box that runs in a separate subprocess; the main rendering callback updates the bounding box after the dialog box returns parameters, and clips the ROI when clipping is enabled; Step 4: Render the main loop, aligning the TCP and torque replays by timestamp, and reload the point cloud only when the frame changes; Step 5: Extract the first three-dimensional force components from the six-dimensional force_torque data of each arm and render them as arrows with adaptive direction and magnitude; do not render the second three-dimensional torque components as arrows.

[0053] In step 1, the hardware execution components are an RGB-D camera, a robot controller, a data acquisition device, and a processor. The RGB-D camera acquires RGB images and depth maps according to timestamps. The processor reads the camera's intrinsic and extrinsic parameters and corresponding TCP poses, generates point clouds by back-projecting depth pixels, performs 0.01m voxel downsampling, and... Transform to the world coordinate system and output the color point cloud of the current frame; where, Let be the homogeneous coordinates of the point in the camera coordinate system. Here, M represents the coordinates in the world / base coordinate system, M represents the camera extrinsic parameters, and T represents the TCP pose matrix. In step 2, the hardware execution entities are the processor, the network interface, and the graphics processor or inference server running the imitation learning model. The processor encodes the current frame visual data, proprioception, and optional force / torque history into observations and sends them via HTTP. The graphics processor or inference server returns action_arm, action_ee, and termination. The processor updates the position and color of each waypoint sphere in an independent inference thread and outputs the inference trajectory superimposed on the point cloud. In step 3, the hardware execution entities are the processor, display, graphics rendering unit, and input device. The processor responds to keyboard input and displays the bounding box parameter dialog box in an independent subprocess. The main rendering callback waits for the subprocess to finish, receives boundary values ​​through the cross-process queue, and updates the axis-aligned bounding box. Subsequently, the graphics rendering unit continues to refresh the point cloud, TCP coordinate system, and force arrows according to the acquisition frame timestamp, and only displays the point cloud within the region of interest when clipping is enabled, outputting the synchronous visualization result.

[0054] In step 1: The execution unit consists of a data reading module and a point cloud reconstruction module. The inputs are the RGB image, depth map, camera intrinsic parameters, camera extrinsic parameters M corresponding to the timestamp of the current frame index, and the real-time TCP pose of the arm corresponding to the hand-eye camera. The output is a color point cloud unified to the world coordinate system, voxel downsampled and optionally cropped.

[0055] The first step is RGB-D point cloud reconstruction. The depth map is first divided by a depth scaling factor to restore the true depth, and then each pixel is back-projected into a 3D point in the camera coordinate system using the intrinsic parameters of the pinhole camera. Z=D(u,v) / s X=(u cx)·Z / fx Y=(v cy)·Z / fy Where X, Y, and Z are the 3D point coordinates in the camera coordinate system, u and v are the column / row coordinates of pixels; D(u,v) is the integer reading of the depth map; s is the depth scaling factor (in this invention, it is set to 1000.0, meaning the depth is stored in millimeters, and dividing by 1000 yields Z in meters); fx and fy are the camera focal lengths (pixels), cx and cy are the principal points / optical centers (pixels); (X,Y,Z) are the 3D coordinates in the camera coordinate system (unit: m). This step is completed by the point cloud reconstruction module based on Open3D RGB-D back projection.

[0056] The second step is to reduce the load by voxel downsampling. Voxel downsampling (voxel side length 0.01m) is performed on the dense point cloud obtained by backprojection. Points within each 1cm cube are merged into centroids, which significantly reduces the number of points, thereby reducing memory / video memory usage and improving the rendering frame rate.

[0057] The third step is to unify coordinate transformation. This invention uses 7-dimensional pose and a 4×4 homogeneous matrix to describe rigid body transformation. The method for constructing the homogeneous matrix from the 7-dimensional pose is as follows: M=[R(q)t;0001], where R(q)=Rot(qw,qx,qy,qz), t=(x,y,z)^T Where q=(qw,qx,qy,qz) is a unit quaternion, R(q)∈SO(3) is its corresponding 3×3 rotation matrix (obtained by converting a quaternion to a rotation matrix); t∈R^3 is a translation (unit m); here M is a 4×4 homogeneous transformation matrix; its inverse operation is to perform a matrix-quaternion transformation on the upper left 3×3 rotation block of M to restore the 7-dimensional pose by t=M[:3,3]; Rot(qw,qx,qy,qz) is a function to construct a rotation matrix from quaternions; here T is the matrix transpose symbol.

[0058] The uniform transformation applied to the point cloud is p_world = T·M·p_cam, with two cases depending on the camera type: Global camera: The point cloud is already in the camera coordinate system, and the extrinsic parameter M is directly given by the calibration. Let T=I (identity matrix), and transform it into p_world=M·p_cam, that is, transform the point cloud from the camera coordinate system to the world / base coordinate system.

[0059] Wrist-in-hand camera: Its extrinsic parameter M is defined in the TCP coordinate system, so it needs to be multiplied by the current TCP pose matrix T (constructed from the current TCP 7-dimensional pose of the arm) to transform it into p_world=(T·M)·p_cam; and select the corresponding arm TCP according to the camera type identifier: if the identifier is the right arm, take the right arm TCP; otherwise, take the left arm TCP by default.

[0060] p_world = T·M·p_cam, where global:T = I; inhand:T = the corresponding arm TCP pose matrix; Where p_cam represents the homogeneous coordinates of a point in the camera coordinate system, and p_world represents the coordinates in the world / base coordinate system; M represents the camera extrinsic parameters (camera → its reference frame); and T represents the TCP pose matrix (hand-eye camera reference frame TCP → base coordinate system). This unifies the multi-path point clouds of the global camera and each wrist camera, as well as the poses of each arm, into the same world coordinate system, resolving the problem of mismatched coordinate systems.

[0061] When clipping is enabled, bounding box clipping is applied to the transformed point cloud, retaining only the ROI within the bounding box; to save computing power, the point cloud is only reconstructed and uploaded when the frame index changes.

[0062] In step 2: The execution is carried out by an independent inference visualization daemon thread and inference service. The thread reads RGB-D data, intrinsic and extrinsic parameters, proprioception, optional high-frequency force / torque history, and natural language instructions according to the current frame index, and constructs inference observations. The image, proprioception, and force / torque data are taken from the acquisition timestamp corresponding to the current frame, while the timestamp in the cameras field of the observation records the time the inference request was initiated. The output is a sequence of the latest inference waypoint trajectory balls superimposed on the currently acquired point cloud, with its position and color updated in-situ as the inference results are obtained. This superposition is used for spatial alignment evaluation and does not imply that the inference result has the same timestamp as the acquisition frame.

[0063] The first step is to construct the observation. The observation consists of three parts: Cameras (Vision): Encapsulates the camera's timestamp, RGB, depth, type, intrinsics, extrinsics, and depth_scale. RGB is a string encoded as a JPEG image and then Base64 encoded; depth is a string encoded as a 16-bit PNG image and then Base64 encoded; depth_scale is 1000.0; timestamp is a timestamp, taken from the system time when the inference request was initiated, in milliseconds, used to identify the request time, and is not equivalent to the timestamp of the acquired data used when reading the RGB-D frame; intrinsics are intrinsic parameters, extrinsics are extrinsics, and type is the camera type identifier.

[0064] proprio (proprioception): tcp—Transforms each arm's TCP from the arm base coordinate system to the world coordinate system through the transformation matrix of each arm base, and then flattens it into one dimension; end_effector—End effector status (default is filled with zero according to the number of arms).

[0065] history (historical data): When force visualization is enabled, it includes high-frequency torque (force_torque). It is first rotated to the base coordinate system, then padded to a fixed length according to the time sequence, and then flattened and fed in.

[0066] The second step is to call and parse the inference service. A request is initiated using the HTTP POST method (the request body is the serialized observation, along with command parameters); if the service returns a non-200 status, an exception is thrown, carrying the server's error information. The returned body is parsed into three tensors: action_arm: The waypoint motion of the robotic arm, rearranged into a shape of (traj_len, num_arms, dim), where the first 3 dimensions are the target translation coordinates used to determine the position of each trackball; traj_len is the trajectory length, num_arms is the number of robotic arms, and dim is the motion dimension.

[0067] action_ee: The target action sequence of the end effector (such as the opening and closing of a gripper).

[0068] termination: The termination scalar for each waypoint, used for trackball coloring.

[0069] The third step is rendering and asynchronous updating. During initialization, a small sphere with a radius of 0.005m is created for each arm and each waypoint. This sphere is then translated to the target's translation coordinates for that waypoint and colored according to the termination signal. The color (color(t) of the trackball at the t-th waypoint is determined by the termination signal (termination[t]): when termination[t] > 0, color(t) = red (1, 0, 0), indicating that the waypoint is a terminated segment; otherwise, color(t) = green (0, 1, 0), indicating that the waypoint is a normal progression segment. Here, t is the waypoint number; termination[t] is the termination scalar for that waypoint; red indicates a terminated segment, and green indicates a normal progression segment, making it easy to see at a glance where the strategy prediction ends.

[0070] The inference visualization thread repeatedly requests inference at fixed intervals (approximately 0.5 seconds) and updates the position of existing spheres by translating them in place (without adding or deleting geometry, and without blocking the main rendering): translate_t=target_t center_t Here, `translate_t` is the translation increment, `target_t` is the new target position of the sphere (m, taken from the previous 3D translation coordinates of the waypoint in `action_arm`), and `center_t` is the current geometric center of the sphere. The difference between the two is the translation increment, which moves the sphere in place rather than rebuilding it, avoiding rendering jitter caused by adding or deleting geometry. The color is also updated synchronously with the latest termination. Inference is performed in a separate inference visualization thread. Even if a single inference takes tens to hundreds of milliseconds, the main rendering loop still refreshes frame by frame, and the screen does not freeze. The thread ends with the exit flag.

[0071] In step 3: The execution entity consists of the main rendering unit process and a parameter input dialog box within an independent subprocess. Inputs include the six boundary values ​​of the current bounding box (min / max of x / y / z) and new boundary values ​​entered by the user in the dialog box. Outputs are the updated axis-aligned bounding box (AABB) and (when clipping is enabled) a point cloud containing only the ROI.

[0072] When the B key is pressed, this invention does not directly pop up a dialog box within the thread containing the Open3D rendering main loop, but instead: (a) Pack the current bounding box with the axes aligned to the minimum / maximum boundary values ​​(x_min, y_min, z_min, x_max, y_max, z_max, six boundary values) of the bounding box; (b) Create a cross-process queue and start an independent child process to run the bounding box input dialog box; (c) The main process waits for the child process to finish and retrieves the result through the cross-process queue (carrying six floating-point boundaries on success and exception information on failure). (d) On success, update the minimum / maximum bounding box and refresh it in real time (red AABB wireframe); use a flag to prevent repeated pop-ups.

[0073] The above method places the GUI event loop of the Tk dialog box from the Tkinter graphical user interface library into a separate child process, preventing it from sharing the same GUI event loop as the Open3D window. Because the main rendering callback calls `join` to wait for the child process to finish, the 3D rendering pauses during parameter input; after the user closes the dialog box, the main process retrieves the result from the queue and resumes rendering. This method reduces the risk of abnormal exits due to direct conflicts with GUI resources, but it does not describe the parameter input process as non-blocking rendering.

[0074] Press the C key to toggle the clipping switch and force the point cloud to be reloaded in the next frame; during reloading, apply AABB clipping to the point cloud after the unified coordinate transformation: cloud_roi={p∈cloud|x_min≤p_x≤x_max,y_min≤p_y≤y_max,z_min≤p_z≤z_max} Where p = (p_x, p_y, p_z) represents the coordinates of a point in the world coordinate system (m); (x_min, x_max, y_min, y_max, z_min, z_max) represents the bounding box boundary (m); cloud_roi represents the cropped point cloud, and cloud represents the original point cloud. After cropping, only the working area (such as the desktop area for folding towels) is retained, while the background and irrelevant points are removed. This focuses on the area of ​​interest and further reduces the number of rendering points, improving rendering smoothness and subsequent annotation efficiency.

[0075] In step 4: The execution body consists of the main rendering loop and keyboard callbacks. The inputs are the camera timestamp sequence of the acquired data, the current frame index, the TCP coordinates of each arm aligned with the acquisition timestamp, and the optional force data. The outputs are the point cloud corresponding to the current acquisition frame, the TCP coordinate system of each arm, the force arrows, and the latest inference trajectory updated asynchronously by an independent inference thread.

[0076] The first step is to align the point cloud, TCP, and force data based on the camera timestamp in the acquired data. This invention uses the acquisition timestamp corresponding to the current frame index as the base time, reads the aligned TCP and optional force data at that time, ensuring that the point cloud, robotic arm pose, and force feedback correspond to the same acquisition frame. The timestamp in the inference observation is taken from the inference request initiation time, and the latest returned trajectory is asynchronously overlaid and displayed; therefore, this trajectory is not considered measurement data with a strictly identical timestamp to the acquisition frame.

[0077] The second step is to render each arm's TCP as a coordinate system. For the i-th arm, after transforming its TCP from the arm-based coordinate system to the world coordinate system, its position and orientation are displayed using a coordinate system mesh (size 0.1m): M_i = B_i·Pose(tcp_i) Coordinate system mesh ← Transformed to world system via M_i Where tcp_i is the 7D TCP pose of the i-th arm in the arm base coordinate system; Pose(·) represents the construction of a 4×4 homogeneous matrix from the 7D pose; B_i is the 4×4 transformation matrix from the i-th arm's base to the world coordinate system; M_i is the pose matrix of the arm's TCP in the world coordinate system; the three axes of the coordinate system (red / green / blue corresponding to x / y / z) after transformation by M_i indicate the real-time orientation of the arm's end in the world coordinate system. The vertices and vertex colors of the coordinate system geometry are updated and refreshed in-situ to avoid adding or deleting geometry.

[0078] Step 3, interactive playback control (keyboard callback): Spacebar: Toggle between autoplay and pause; during playback, the frame index increments by 1 for each main loop cycle (up to the last frame).

[0079] Left / Right Arrow Keys: Manually rewind / forward frame by frame, decrement / increase the frame index by 1 and clamp it to the valid range.

[0080] ESC key: Sets the exit flag, closes the window, and exits the main loop; B / C triggers the frame setting and cropping respectively.

[0081] The fourth step involves reloading on demand to save computational resources. Point clouds are reconstructed and uploaded, and TCP / Force is refreshed only when the frame index changes; the main loop controls the refresh rate at approximately 0.05 seconds per round (approximately 20Hz). When the camera is a wrist-based hand-eye camera, the reloaded point cloud needs to be passed to the corresponding arm TCP (if the camera type is right arm, take the right arm; otherwise, take the left arm) for correct transformation; reconstruction is skipped for unchanged frames to avoid unnecessary backprojection and GPU upload overhead.

[0082] In step 5: The execution body is the force visualization branch (force vector rendering module) of the main rendering loop. The input is high-frequency force / torque (force_torque, 6-dimensional per arm: 3-dimensional force + 3-dimensional torque), rotation matrix of each arm base, and current TCP position. The output is a magenta arrow starting from each arm TCP, oriented along the force direction, and whose length adapts to the magnitude of the force.

[0083] The first step is force vector coordinate transformation. The sensor force is defined in the arm base / sensor coordinate system, and needs to be rotated to the world / base coordinate system using the rotation matrix of each arm base (rotation only, no translation): f_world=R_i·f_sensor R_i=B_i[:3,:3] Where f_sensor∈R^3 is the force (in N) in the sensor coordinate system, R_i is the 3×3 rotation matrix from the i-th arm base to the world (taking the top left 3×3 block of the transformation matrix B_i of the arm base), and f_world is the force vector in the world coordinate system. Both the force and torque 3D components are processed using this rotation during observation construction.

[0084] The second step is to adaptively generate arrows based on the force magnitude. The force magnitude `force_mag` is calculated, and arrows are only drawn when it exceeds a threshold (0.5N). The length of both the arrow shaft and the arrowhead is proportional to the force magnitude. force_mag=||f_world||_2 Arrow shaft length = force_mag·k·0.8 Arrow length = force_mag·k·0.2 Where force_mag is the magnitude of the force (N); k is the force-length calibration coefficient (0.05 in this invention, in m / N, that is, the force magnitude is linearly mapped to the arrow length, 1N force corresponds to 0.05m total length); the arrow shaft accounts for 80% of the total length and the arrowhead (cone) accounts for 20%. When force_mag≤0.5N, the vertices and vertex colors of the arrow geometry are set to empty (zero row), thus "clearing" the arrow and indicating that the force is negligible.

[0085] The third step is to align the arrow with the force direction. The arrow defaults to the +z axis; the z-axis needs to be rotated to the unit vector of the force direction, and then translated to TCP. Rotation is represented by axis-angle, and special handling is given for the degenerate case of near-collinearity: force_dir=f_world / force_mag,z=(0,0,1) If |z·force_dir|>0.99, then rot_axis=(1,0,0) Otherwise, rot_axis=(z×force_dir) / ||z×force_dir|| rot_angle=arccos(clip(z·force_dir, 1,1)) Where force_dir is the unit vector of force direction; z is the initial direction of the arrow (+z); rot_axis is the axis of rotation, and rot_angle is the rotation angle (rad, obtained from the angle between z and force_dir; clip prevents numerical out-of-bounds values ​​from causing arccos anomalies); when z and force_dir are approximately parallel (absolute value of the dot product > 0.99), the cross product tends to zero and the direction is unstable, so rot_axis is degenerately set to (1,0,0). arccos() is the arccosine function, and clip() is the cutoff function.

[0086] When rot_angle > 1e-6, a rotation matrix is ​​generated according to axis-angle (rot_axis·rot_angle) to rotate the arrow. Then, it is translated to the TCP position of the arm and colored magenta (1,0,1). The vertices and vertex colors of the arrow geometry are updated and refreshed in place to avoid additions and deletions. When the force is less than the threshold, it is cleared to avoid noise forces causing jittery small arrows that interfere with observation.

[0087] Example 2: Scene of two arms folding towels (three cameras, complete operation process) Execution Unit: A dual-arm robot (two-arm configuration); three RGB-D vision cameras—one global camera fixed above the head / overhead for viewing the entire workbench and reconstructing the global scene point cloud; two inhand cameras mounted on the wrists / endpoints of the left and right robotic arms respectively, moving with the arms and used for close-up observation of local details of the towels held by each arm; an inference service running an imitation learning strategy; and a gripper end effector equipped with a six-dimensional force sensor. Task: The two arms work together to fold a towel laid flat on a table. Typical setup is as follows: Data and Cameras: The data reading module loads a dataset collected during a towel-folding exercise, with the instruction "fold the towel" enabled, and force visualization is activated. RGB-D data from all three cameras participates in point cloud reconstruction.

[0088] Three-camera point cloud fusion: Each camera's RGB-D back-projection is followed by voxel downsampling (voxel side length 0.01m), and then unified to the same world / base coordinate system by the point cloud reconstruction module—the global camera uses its fixed extrinsic parameters for direct transformation; the poses of the two wrist cameras are given in the TCP coordinate system of the corresponding arm, requiring real-time TCP pose matrix transformation of that arm first, followed by superposition (left multiplication) of the extrinsic parameters, and finally the three point clouds are fused into the same coordinate system for display (and superimposed with the real-time inference trajectory); press C to crop to the desktop ROI (e.g., x∈[0,1], y∈[...]). 1,1],z∈[ [0.5, 0.5]m) to remove the ground and background.

[0089] Position and force: The left and right arms TCP are each displayed as a coordinate system (0.1m in size); when the gripper grabs the edge of the towel and the contact force increases to more than 0.5N, a magenta arrow appears at the TCP, and its length increases with the increase of the gripping force, which intuitively reflects the magnitude and direction of the force.

[0090] Inference Trajectory: The inference visualization thread calls the inference service every 0.5 seconds and returns action_arm (e.g., traj_len=40, num_arms=2); 40 green balls on each of the left and right arms outline the predicted path of "grab the edge first, then lift up, fold in half, and flatten", and waypoints near the termination (termination>0) are displayed in red.

[0091] A complete operation process: (1) Visualization system initialization: Read metadata (robot configuration, three-camera configuration information, transformation of each arm base to the world), create an Open3D window and register keyboard callbacks, load the first frame point cloud, world coordinate system, AABB and each arm TCP coordinate system.

[0092] (2) Because the instruction was provided, the action_arm was obtained for the first reasoning and a series of trackballs were built for each of the left and right arms; the reasoning visualization daemon thread was started to refresh asynchronously.

[0093] (3) Enter the main rendering loop: Press the space bar to play, the frame index increments and advances frame by frame; each frame is aligned with the timestamp to retrieve TCP and torque, the point clouds of the three cameras are transformed into unified coordinates and then merged and refreshed, and the TCP coordinate system and force arrow are refreshed.

[0094] (4) The engineer observes whether the strategy trackball fits the edge of the towel, whether the folding path is reasonable, and whether the gripping force is too large; if the background is cluttered, press B to set the frame and press C to crop and focus on the desktop.

[0095] (5) Press ESC to exit, close the window, and the inference visualization daemon thread will end with the exit flag.

[0096] With this embodiment, engineers can simultaneously evaluate "the quality of the collected data point cloud + the pose / force of the robotic arm + the predicted trajectory of the strategy" in the same three-dimensional scene without having to look through the two-dimensional image frame by frame, and quickly locate data or strategy problems (such as missing depth, misaligned coordinates, or the strategy not being aligned with the edge of the towel).

[0097] like Figure 1 The diagram shows the system framework of the synchronous visualization system of the present invention, illustrating the data flow between the RGB-D camera and data reading module, the robot proprioception and force / torque acquisition module, the imitation learning inference service, the graphics rendering and interaction unit, as well as the relationship between the inference thread, the parameter dialog box subprocess, and the main rendering callback waiting to return.

[0098] like Figure 2 The flowchart of the synchronous visualization method of the present invention shows the steps of RGB-D back projection, voxel downsampling, T·M coordinate transformation, asynchronous inference trajectory update, axis-aligned bounding box clipping, and timestamp-based rendering refresh.

[0099] like Figure 3 The diagram illustrates the operating principle of this invention, showing a multi-way point cloud in a unified world coordinate system, a single-arm or double-arm TCP coordinate system, an end force arrow, an axis-aligned bounding box region of interest, and a predicted waypoint trajectory colored by termination.

[0100] like Figure 4 The diagram shows the relationship between the acquisition frame time and the asynchronous inference request time of this invention. It shows that the point cloud, TCP, and end force are aligned using the acquisition timestamp t[k] corresponding to the current frame index, while the timestamp in the inference observation records the request initiation time τ_req. The latest inference trajectory is asynchronously superimposed on the current 3D scene after returning, and t[k] and τ_req have different time meanings.

[0101] like Figure 5 This is a schematic diagram of the end force arrow data selection and direction rotation principle of the present invention. It shows the extraction of the first three-dimensional force components from the six-dimensional force_torque data for arrow rendering, the second three-dimensional torque components only for inference history observation, and the relationship between the coordinate rotation of the force vector, the 0.5N drawing threshold, the arrow length calculation, and the rotation axis selection.

[0102] Change plan Application scenario changes: In addition to folding towels, it can be used for scenarios such as bin-picking of parts, loading and unloading, and assembly; only the data collection dataset and instructions need to be changed, and the point cloud reconstruction, coordinate unification, trajectory superposition, and force arrow logic remain unchanged.

[0103] The number of cameras is configurable: the number of cameras can be configured as needed, and a single camera or multiple cameras can be used (such as one global camera + two wrist hand-eye cameras in this embodiment); the global camera and wrist camera can be switched or enabled simultaneously, and multiple point clouds can be fused and superimposed in the same world coordinate system.

[0104] Camera configuration changes: The wrist-eye camera automatically performs real-time TCP pose transformation according to the corresponding arm (if the camera type is right arm, the right arm is used; otherwise, the left arm is used), and the global camera transforms according to fixed extrinsic parameters, supporting the superposition of point clouds from multiple cameras in the same coordinate system.

[0105] Single-arm / dual-arm variation: The number of arms is determined by whether the robot is a dual-arm configuration. In the case of a single arm, only one TCP coordinate system, a series of trackballs and a force arrow are rendered; in the case of multiple arms, each is maintained independently.

[0106] Enable force visualization: The force visualization switch controls whether to read high-frequency torque and draw force arrows; the force-length calibration coefficient and threshold of 0.5N can be adjusted according to the sensor range calibration.

[0107] Pure playback (no inference) mode: When the instruction is empty, the inference thread is skipped, and only the temporal alignment playback of point cloud / pose / force is performed, which is used for pure data quality inspection and annotation.

[0108] Downsampling / clipping parameters are adjustable: voxel side length and AABB boundary can be adjusted according to the scene and hardware memory, balancing "detail preservation" and "smooth rendering"; the inference refresh cycle (0.5s) can also be adjusted according to the inference latency.

[0109] The rendering backend is replaceable: Open3D can be replaced with other 3D rendering engines; the parameter dialog box can be replaced with other GUIs, while the process isolation concept remains unchanged.

[0110] The "point cloud reconstruction - unified coordinate transformation - real-time synchronous rendering" framework of this invention can be ported to different data reading modules and robot platforms; the mutual conversion between pose and homogeneous matrix, and the mutual conversion between quaternion and rotation matrix are all general tools and do not depend on specific hardware models.

[0111] Asynchronous inference threads, process isolation dialog boxes, voxel downsampling parameters, bounding box boundaries, force calibration coefficients, refresh cycles, etc. are all configurable items that can be flexibly adjusted according to hardware memory, inference latency, and scenario requirements, making them easy to reuse across different robots and tasks.

[0112] The reasoning trajectory overlay and termination coloring visualization method of the present invention can be used independently as an online evaluation / debugging tool for imitation learning strategies, for comparative observation of different strategies and different checkpoints.

[0113] Those skilled in the art will understand that, in addition to implementing the system, apparatus, and their modules provided by this invention in purely computer-readable program code, the same program can be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system, apparatus, and their modules provided by this invention can be considered a hardware component, and the modules included therein for implementing various programs can also be considered structures within the hardware component; alternatively, modules for implementing various functions can be considered both software programs implementing the method and structures within the hardware component.

[0114] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A method for synchronously visualizing point cloud data collected by a robot and the inference trajectory of a model, characterized in that, include: Step 1: Obtain the RGB image, depth image, and camera parameters of the current frame captured by the RGB-D vision camera; obtain the center point of the end effector and the state of the end effector, including the pose of the tool center point and the end force data; back-project the depth image to generate a point cloud; transform the point cloud to the world coordinate system according to the camera parameters and the pose of the tool center point of the end effector to obtain point cloud data. Step 2: Based on the RGB image and depth image of the current frame, as well as the center point of the robotic arm end effector and the state of the end effector, construct inference observations, asynchronously send inference requests to the model inference service, and receive the predicted trajectory returned by the model inference service; Step 3: Load the point cloud data, the tool center point pose of the robotic arm, and the predicted trajectory into the 3D scene, and perform visualization rendering; align the point cloud data, the tool center point pose of the robotic arm, and the end effector force data in time based on the camera timestamp; update the point cloud data in the 3D scene only when the frame index of the current frame changes; Step 4: Run the parameter input dialog box in a child process independent of the main process to obtain the bounding box parameters input by the user; the main process pauses rendering while the parameter input dialog box is running, and obtains the bounding box parameters through inter-process communication after the child process ends, and updates the axis-aligned bounding box in the 3D scene; In step 1, the point cloud is transformed to the world coordinate system according to the type identifier of the RGB-D vision camera: For a global camera, the point cloud is transformed from the camera coordinate system to the world coordinate system based on the camera extrinsic parameters of the global camera. For the hand-eye camera, the point cloud is first transformed from the camera coordinate system to the tool center point coordinate system based on the camera extrinsic parameters of the hand-eye camera. Then, based on the tool center point pose of the robotic arm associated with the hand-eye camera, the point cloud is transformed from the tool center point coordinate system to the world coordinate system.

2. The method for synchronous visualization of robot point cloud acquisition and model inference trajectory according to claim 1, characterized in that, In step 1, the point cloud generated by back projection is subjected to voxel downsampling. The voxel downsampling divides the three-dimensional space into cubic elements with a preset side length, and the points in each voxel are replaced by their centroids.

3. The method for synchronous visualization of robot point cloud acquisition and model inference trajectory according to claim 1, characterized in that, According to the type identifier of the RGB-D vision camera, when the type identifier is right arm, the tool center point at the end of the right arm is taken as the tool center point pose of the robotic arm associated with the hand-eye camera; otherwise, the tool center point at the end of the left arm is taken as the tool center point pose of the robotic arm associated with the hand-eye camera.

4. The method for synchronous visualization of robot point cloud acquisition and model inference trajectory according to claim 1, characterized in that, In step 2, in an inference thread independent of the main rendering loop, an inference request is sent to the model inference service at a preset period. Based on the waypoint positions and termination signals in the predicted trajectory returned by the model inference service, the position and color of the constructed trackball are updated. Specifically, when the termination signal is greater than zero, the corresponding trackball is colored red, and when the termination signal is less than or equal to zero, the corresponding trackball is colored green.

5. The method for synchronous visualization of robot point cloud acquisition and model inference trajectory according to claim 1, characterized in that, In step 2, the camera timestamp in the inference observation records the time when the inference request is initiated, which is used to characterize the time of the inference request.

6. The method for synchronous visualization of robot point cloud acquisition and model inference trajectory according to claim 1, characterized in that, In step 3, the tool center point pose of the robotic arm is visualized and rendered, and a coordinate system indicator graphic representing the position and attitude of the robotic arm end effector is generated in the three-dimensional scene; the end effector force data of the robotic arm is visualized and rendered, and a force vector indicator graphic is generated from the tool center point position of the robotic arm in the three-dimensional scene. The length of the force vector indicator graphic is related to the magnitude of the force. When the magnitude of the force is greater than a preset threshold, the force vector indicator graphic is generated.

7. The method for synchronous visualization of robot point cloud acquisition and model inference trajectory according to claim 6, characterized in that, The end force data is six-dimensional force / torque data, and the first three-dimensional force components are extracted to generate the force vector indicator graphic.

8. The method for synchronous visualization of robot point cloud acquisition and model inference trajectory according to claim 1, characterized in that, In step 4, the point cloud is cropped according to the updated axis-aligned bounding box, retaining only the point cloud within the axis-aligned bounding box.

9. A synchronous visualization system for robot point cloud acquisition and model inference trajectory, characterized in that, The method for synchronously visualizing robot point cloud acquisition and model inference trajectory according to any one of claims 1 to 8 includes: Data reading and point cloud reconstruction module: acquires the RGB image, depth image and camera parameters of the current frame captured by the RGB-D vision camera, acquires the center point of the end tool and the state of the end effector of the robotic arm, including the pose of the tool center point and the end force data of the robotic arm, back-projects the depth image to generate a point cloud, and transforms the point cloud to the world coordinate system according to the camera parameters and the pose of the tool center point of the robotic arm to obtain point cloud data; Inference module: It is communicatively connected to the data reading and point cloud reconstruction module, constructs inference observations based on the RGB image and depth image of the current frame, as well as the center point of the robotic arm end tool and the state of the end effector, asynchronously sends inference requests to the model inference service, and receives the predicted trajectory returned by the model inference service; The rendering module communicates with the data reading and point cloud reconstruction module and the inference module. It loads the point cloud data, the tool center point pose of the robotic arm, and the predicted trajectory into the 3D scene for visualization rendering. The rendering module uses the camera timestamp as a reference to perform time alignment on the point cloud data, the tool center point pose of the robotic arm, and the end effector force data. The rendering module only updates the point cloud data in the 3D scene when the frame index of the current frame changes. The parameter configuration module includes a subprocess that runs independently of the main process of the rendering module. The subprocess runs a parameter input dialog box to obtain bounding box parameters input by the user. The main process pauses rendering while the parameter input dialog box is running, and obtains the bounding box parameters through inter-process communication after the subprocess ends, and updates the axis-aligned bounding box in the 3D scene.

Images