A pose estimation method and system based on key voxel block selection

Abstract

The application provides a pose estimation method and system based on key voxel block selection, the method comprising the following steps: acquiring multi-view images of a three-dimensional model of a target object, extracting two-dimensional feature points in each view image, and projecting the two-dimensional feature points into three-dimensional feature points; performing voxelization processing on all three-dimensional feature points to obtain a voxel block set, calculating the geometric features of the voxel blocks and inputting the Mamba to obtain the visibility frequency of the voxel blocks under multi-view, and taking a plurality of voxel blocks with the highest visibility frequency as key voxel blocks; projecting the center points of the key voxel blocks to an image coordinate system, generating a training set by labeling the projection area to train a target detection model; inputting a video stream containing the target object into the trained target detection model to obtain the relative pose between the target object and the camera; and mapping the relative pose to a world coordinate system according to the absolute pose of the camera in the world coordinate system to obtain the absolute pose of the target object in the world coordinate system.

Description

Technical Field

[0001] This invention belongs to the field of pose estimation technology, specifically relating to a pose estimation method and system based on key voxel block selection. Background Technology

[0002] In the field of computer vision, image-based 3D reconstruction and target pose estimation techniques have wide applications, such as augmented reality (AR), autonomous driving, and smart manufacturing. These techniques typically rely on multi-view images for sparse or dense 3D reconstruction and combine object detection algorithms with 3D-2D point-to-point (PnP) methods to determine the spatial pose of the target in the image, which is of great significance for achieving accurate object localization and interaction.

[0003] Object detection algorithms can accurately identify target objects in images, while the PnP series of algorithms utilize the correspondence between 2D feature points provided by object detection and known 3D model points. Through a series of mathematical transformations and optimization processes, they calculate the pose information of the target object in the image, including parameters such as position and orientation, thereby achieving target pose estimation.

[0004] However, after these methods complete the 3D reconstruction of the target object, they typically perform uniform sampling or randomly select feature points across the entire model space for pose estimation. This operation introduces a large amount of low-value or redundant 3D information, which not only increases the overhead of subsequent processing but also reduces the accuracy of pose estimation. Especially when the target region is occluded, has weak texture, or contains repetitive structures, traditional feature selection methods struggle to effectively focus on the most identifiable key regions. Summary of the Invention

[0005] This invention proposes a pose estimation method and system based on key voxel block selection, which solves the problem that existing pose estimation methods are unable to effectively focus on the most recognizable key regions of the target object.

[0006] To address the aforementioned technical problems, this invention provides a pose estimation method based on key voxel block selection, comprising the following steps:

[0007] Step S1: Construct a three-dimensional model of the target object, obtain multi-view images of the three-dimensional model, extract two-dimensional feature points from each view image, and project the two-dimensional feature points into three-dimensional feature points through ray projection to obtain a set of three-dimensional feature points;

[0008] Step S2: Voxelize the three-dimensional feature point set to obtain a set of voxel blocks. Calculate the geometric features of each voxel block in the set of voxel blocks. Input the geometric features into Mamba, a linear time series model with a selected state space. Mamba predicts the visibility frequency of the voxel blocks under multiple views based on the geometric features. The top N voxel blocks with the highest visibility frequency are taken as key voxel blocks.

[0009] Step S3: Project the center point of the key voxel block onto the image coordinate system, generate a training set by labeling the projection area, and train the target detection model using the training set;

[0010] Step S4: Input the video stream containing the target object into the trained target detection model to obtain the relative pose between the target object and the camera in each frame of the video stream;

[0011] Step S5: Obtain the absolute pose of the target object in the world coordinate system when the camera captures each frame of the image. Map the relative pose to the world coordinate system based on the absolute pose of the camera to obtain the absolute pose of the target object in the world coordinate system when the camera captures each frame of the image.

[0012] Preferably, step S2 involves voxelizing the three-dimensional feature point set, which includes the following steps: taking the bounding box of the three-dimensional model as the root node; if the number of three-dimensional feature points contained in the current node is greater than a set threshold, then dividing the three-dimensional feature points in the current node into eight child nodes, until the number of three-dimensional feature points in all leaf nodes is less than or equal to the threshold, thus obtaining an octree structure voxel block set.

[0013] Preferably, in step S2, the geometric features of all voxel blocks are sorted to obtain a geometric feature sequence. The geometric features of all voxel blocks are used as the geometric feature sequence and input into Mamba. The sorting of the geometric features of all voxel blocks includes the following steps: constructing a geometric feature matrix based on the geometric features of all voxel blocks, performing a depth-first traversal of all leaf nodes of the octree structure, sorting the traversal results using Morton codes to obtain an ordered voxel list, and rearranging the row vectors of the geometric feature matrix according to the ordered voxel list to obtain the geometric feature sequence.

[0014] Preferably, step S4 includes the following steps:

[0015] Step S41: Input the current frame containing the target object into the trained target detection model, and the target detection model outputs a two-dimensional detection box of the key region of the target object;

[0016] Step S42: Match the center point of the key voxel block with the center point of the two-dimensional detection box to obtain the two-dimensional-three-dimensional corresponding point set;

[0017] Step S43: Filter out erroneous matching points in the 2D-3D corresponding point set, and calculate the relative pose between the target object and the camera based on the remaining corresponding points in the 2D-3D corresponding point set.

[0018] Preferably, obtaining the absolute pose of the camera in the world coordinate system in step S5 includes the following steps:

[0019] Step S501: Obtain the absolute pose of the camera in the world coordinate system when it captures the first frame image;

[0020] Step S502: Extract feature points from consecutive frame images, track inter-frame feature points based on optical flow, accumulate the relative pose transformation between adjacent frames according to the optical flow tracking results, and obtain the relative pose of each frame image relative to the first frame image.

[0021] Step S503: Calculate the absolute pose of each frame image in the world coordinate system based on the absolute pose of the first frame image in the world coordinate system and the relative pose of each frame image relative to the first frame image.

[0022] Preferably, after generating the absolute pose of the target object in each frame of the video stream in step S5, error frame verification and rollback are performed based on multi-source information, including the following steps:

[0023] Step S511: Calculate the Euclidean distance and rotation angle difference of the target object in adjacent frames. If the Euclidean distance or rotation angle difference exceeds the set threshold, the adjacent frame is marked as a candidate error frame.

[0024] Step S512: Compare the absolute pose of the target object in the candidate error frame with the global positioning system data and inertial sensor data corresponding to the target object. If either the position deviation or the attitude angle deviation exceeds the set deviation threshold, then the current candidate error frame is taken as an error frame.

[0025] Step S513: Calculate the geometric saliency weights of the three-dimensional feature points in all key voxel blocks in the two-dimensional detection frame of the error frame, and select the four three-dimensional feature points with the highest geometric saliency weights to reconstruct the two-dimensional-three-dimensional corresponding point set;

[0026] Step S514: Calculate the relative pose between the target object and the camera based on the reconstructed 2D-3D corresponding point set. If the position deviation and attitude angle deviation of the recalculated relative pose still exceed the set deviation threshold, discard the current frame.

[0027] Preferably, step S2, in which Mamba predicts the visible frequency of a voxel block from multiple viewpoints based on geometric features, includes the following steps:

[0028] Step S21: Extract the hidden state matrix of the geometric feature sequence using Mamba. The hidden state matrix is ​​then subjected to layer normalization to obtain the layer-normalized hidden state matrix. ;

[0029] Step S22: For Perform a single-layer linear transformation to obtain the visible frequencies of the voxel block from multiple viewpoints. :

[0030] ;

[0031] In the formula, The activation function is Sigmoid; The learnable parameter matrix; For bias terms;

[0032] Step S23: The ratio of the number of times a voxel block is observed in the multi-view image to the total number of views is used as the global visibility of the voxel block, and the global visibility of all voxel blocks is combined as the supervision vector.

[0033] Step S24: Calculate the binary cross-entropy loss of the neural network. By minimizing the binary cross-entropy loss, the visible frequency of the neural network output is optimized to approximate the global visibility. The expression for the binary cross-entropy loss is:

[0034] ;

[0035] In the formula, For binary cross-entropy loss; The number of voxel blocks; voxel blocks Weighting coefficients; voxel blocks Global visibility; voxel blocks Importance score.

[0036] Preferably, the geometric features mentioned in step S2 include the number of voxel points, voxel point density, local curvature, normal vector consistency, and flatness.

[0037] The number of voxel points is the total number of three-dimensional feature points contained in the voxel block;

[0038] The voxel density is the ratio of the number of voxels to the actual volume of space occupied by the voxel block.

[0039] The expression for calculating the local curvature is:

[0040] ;

[0041] In the formula, voxel blocks The local curvature; To obtain the eigenvalues ​​by performing principal component analysis on the position coordinates of all 3D feature points within the voxel block, and ;

[0042] The expression for calculating the consistency of the normal vector is:

[0043] ;

[0044] In the formula, voxel blocks Consistency of normal vectors; voxel blocks The statistical standard deviation of the normal vectors of all three-dimensional feature points within the area;

[0045] The expression for calculating the flatness is:

[0046] ;

[0047] In the formula, voxel blocks Flatness.

[0048] Preferably, the expression for calculating the geometric saliency weights of the three-dimensional feature points in step S513 is as follows:

[0049] ;

[0050] In the formula, Geometric significance weights; , These are adjustable weighting coefficients; voxel blocks The local curvature; voxel blocks Flatness.

[0051] This invention also provides a pose estimation system based on key voxel block selection, which is implemented based on the above-mentioned pose estimation method based on key voxel block selection, including: a 3D reconstruction module, a 3D feature point set construction module, a key voxel extraction module, a relative pose estimation module, and an absolute pose estimation module.

[0052] The 3D reconstruction module: uses a camera to acquire multi-view images of the target object, applies multi-view stereo algorithm and motion structure recovery algorithm to perform 3D reconstruction of the multi-view images, constructs a 3D model of the target object, imports it into the rendering engine, and moves the center point of the model to the origin.

[0053] The three-dimensional feature point set construction module generates a multi-view image set around the target in the rendering engine and records the camera pose simultaneously. It extracts the two-dimensional feature point set in each image and obtains the three-dimensional feature point set corresponding to each two-dimensional feature point set through the ray projection method.

[0054] The key voxel extraction module: divides the three-dimensional feature point set into several voxel blocks, extracts multiple features of each voxel block to construct a feature matrix, inputs the feature matrix into a neural network, and filters out key voxel blocks;

[0055] The relative pose estimation module: projects the center point of the key voxel block onto the image coordinate system, labels the projection area to form a training dataset, uses the training dataset to train the target detection model, inputs the real-time video stream into the trained target detection model, and obtains the relative pose of the target object in the camera coordinate system.

[0056] The absolute pose estimation module calculates the absolute pose of the target object in the world coordinate system by combining the pose of the camera in the world coordinate system and the relative pose of the target object in the camera coordinate system.

[0057] The beneficial effects of the present invention include at least the following:

[0058] 1. By constructing a 3D model of the target object and acquiring multi-view images, the object's positional information is fully covered, effectively avoiding the feature loss problem caused by relying on only a single viewpoint. By extracting 2D feature points and projecting them into 3D feature points, the key shape information of the object's surface can be accurately captured, providing a high-precision feature foundation for subsequent feature analysis and object recognition. Compared with methods based solely on 2D image features, this method can more accurately reflect the object's true geometric structure.

[0059] 2. By using a neural network to score the importance of voxel blocks and selecting the top N voxel blocks with the highest scores as key voxel blocks, the key feature regions of objects are automatically extracted, avoiding the subjectivity and limitations of manual selection of key features and better adapting to the feature distribution of different target objects.

[0060] 3. By calculating the Euclidean distance and rotation angle difference of target objects in adjacent frames and comparing them with a set threshold, candidate error frames are marked, thereby timely detecting abnormal changes between frames and avoiding interference from error frames to subsequent processing; at the same time, cross-validation of candidate error frames is carried out using multi-source information, which improves the accuracy of error frame judgment.

[0061] 4. By reconstructing the corresponding point set by calculating the geometric saliency weights of key voxel blocks in the erroneous frame, the geometric features of the target object can be better reflected, providing a more reliable basis for the subsequent recalculation of relative pose and improving the accuracy of pose calculation. The relative pose is recalculated based on the reconstructed corresponding point set and compared with the deviation threshold again. If it still exceeds the threshold, the current frame is discarded. This dual verification mechanism further ensures the reliability of the output results and enhances the robustness of the method in the face of complex environments and interference. Attached Figure Description

[0062] Figure 1 This is a schematic diagram of the method flow according to an embodiment of the present invention;

[0063] Figure 2 This is a schematic diagram of the system structure according to an embodiment of the present invention. Detailed Implementation

[0064] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the protection scope of the present invention.

[0065] like Figure 1 As shown, this embodiment of the invention provides a pose estimation method based on key voxel block selection, including the following steps:

[0066] Step S1: Construct a three-dimensional model of the target object, obtain multi-view images of the three-dimensional model, extract two-dimensional feature points from each view image, and project the two-dimensional feature points into three-dimensional feature points through ray projection to obtain a set of three-dimensional feature points.

[0067] Specifically, a mobile camera is used to acquire multi-view images of the target object, ensuring that the main structure of the object is covered. The acquired images are then reconstructed into a 3D model using a multi-view stereo (MVS) algorithm and a structure in motion (SFM) algorithm. During this process, SFM is used to recover the camera pose and sparse point cloud information, which is then combined with the MVS algorithm to construct a 3D model of the target object. The constructed 3D model is then imported into the rendering engine, where the bounding box and center point of the 3D model are calculated. Finally, the target 3D model is moved so that its center point is precisely moved to the origin position.

[0068] Step S2: Voxelize the three-dimensional feature point set to obtain a set of voxel blocks. Calculate the geometric features of each voxel block in the set of voxel blocks. Input the geometric features into a linear time series model Mamba with a selected state space. Mamba predicts the visibility frequency of the voxel blocks under multiple views based on the geometric features. The top N voxel blocks with the highest visibility frequencies are selected as key voxel blocks.

[0069] Specifically, the rendering engine generates multi-view images around the target object while simultaneously recording camera poses. A self-supervised feature point detection network, SuperPoint, is used to extract a set of two-dimensional feature points from each image. Based on the image rendering matrix and depth buffer, the ray projection method is used to obtain the 3D feature points corresponding to each 2D feature point, thus obtaining a 3D feature point set. Repeat the above steps to collect all 2D-3D matching point sets in N images. .

[0070] An octree voxel space index structure is constructed for the reconstructed 3D model. The construction rules for the voxel space are as follows: the bounding box of the 3D model is used as the root node. If the number of 3D feature points in the current node exceeds a set threshold T, the 3D feature points in the current node are divided into eight child nodes. This recursive division continues until the maximum depth D is reached or the number of 3D feature points in all leaf nodes is less than or equal to T. In this embodiment, T is set to 10 and D is set to 4. This results in a set of voxel blocks of non-uniform size. Each It contains a certain number of three-dimensional feature points.

[0071] For each voxel block Calculate the geometric features of the voxel block. These features include the number of voxel points, voxel point density, local curvature, normal vector consistency, and flatness.

[0072] Number of voxels This represents the total number of three-dimensional feature points contained in the voxel block.

[0073] voxel point density This is the ratio of the number of voxel points to the actual space volume occupied by the voxel block.

[0074] ;

[0075] In the formula, It represents the volume of space occupied by an object.

[0076] Acquire voxel blocks The coordinates of all three-dimensional feature points within the matrix are determined, and their covariance matrix is ​​calculated using Principal Component Analysis (PCA) to solve for the eigenvalues. , , ,in Calculate local curvature using eigenvalues:

[0077] ;

[0078] In the formula, voxel blocks The local curvature.

[0079] Acquire voxel blocks Calculate the statistical standard deviation between the surface normal vectors of all 3D feature points within the area. The consistency of normal vectors is calculated according to the following expression:

[0080] ;

[0081] In the formula, voxel blocks Consistency of normal vectors; voxel blocks The statistical standard deviation of the normal vectors of all three-dimensional feature points within the area;

[0082] The expression for calculating flatness is:

[0083] ;

[0084] In the formula, voxel blocks Flatness.

[0085] The above features are combined into the feature vector of the voxel block. ,in This represents the dimension of the voxel block's geometric feature vectors. Construct the voxel feature matrix. For each column of the matrix, i.e. each feature dimension, minimum-maximum scaling normalization is applied to linearly map the value of each feature dimension to the interval [0,1].

[0086] An octree structure, conveniently constructed using a depth-first traversal algorithm, is used to visit all leaf node voxel blocks. The visiting order of child nodes is determined according to the Z-order Morton code, ensuring that spatially neighboring voxel blocks are adjacent in the sequence, thus generating an ordered voxel list. The normalized voxel feature matrix is ​​then analyzed based on this ordered voxel list. The row vectors are rearranged to obtain serialized features. .

[0087] Will The input is the lightweight state-space model Mamba, which has 3 encoding layers and 12 feature dimensions. First, the Mamba network is trained multiple times in the cloud, and then optimized using a binary cross-entropy loss function. Mamba outputs the hidden state matrix of each voxel block. ,right Perform layer normalization to obtain the layer-normalized hidden state matrix. During the model training phase, the global visibility of each voxel block is calculated as a supervision signal to generate a supervision vector. ,in voxel blocks The ratio of the number of times observed in a multi-view image to the total number of views, i.e.:

[0088] .

[0089] The importance score of the layer voxel block is calculated by using a single-layer linear transformation and activation function. :

[0090] ;

[0091] In the formula, The activation function is Sigmoid; The learnable parameter matrix; This is a bias term.

[0092] The visibility-weighted binary cross-entropy loss is used as the training loss for the Mamba network:

[0093] ;

[0094] In the formula, For binary cross-entropy loss; The number of voxel blocks; for; voxel blocks Global visibility; voxel blocks Importance score.

[0095] During the inference phase, the Mamba network outputs importance scores for each voxel block. The top 20% of voxel blocks by score were selected as the key voxel block set. .

[0096] Step S3: Project the center point of the key voxel block onto the image coordinate system, generate a training set by labeling the projection area, and train the target detection model using the training set.

[0097] Specifically, the center points of key voxels are mapped onto the image coordinate system through perspective projection transformation. The projection regions of these key voxels in images from various viewpoints are calculated, and the projection regions of voxels in the images are labeled to generate a labeled dataset for training. The Nanodet-plus target detection model is trained using the labeled dataset to obtain a target detection model that can accurately detect key regions.

[0098] Step S4: Input the video stream containing the target object into the trained target detection model to obtain the relative pose between the target object and the camera in each frame of the video stream.

[0099] Specifically, a real-time video stream containing the target object is input into a trained target detection model to obtain two-dimensional detection boxes for key voxel blocks of the target object. The size of the detection boxes is fixed at [size missing]. Pixels. The center of the 2D detection box is matched with the center of a predefined key voxel block to construct a 2D-3D corresponding point set. The Random Sample Consensus (RANSAC) algorithm is used to process the constructed corresponding point set, filtering out erroneous matches. The Maximum Likelihood Pose Estimation (MLPn) method is employed to calculate the 6D pose of the target object relative to the camera based on the filtered correct matches, i.e., the relative position and orientation relationship between the target object and the camera, achieving accurate localization and pose estimation of the target object in the real-time video stream.

[0100] Step S5: Obtain the absolute pose of the target object in the world coordinate system when the camera captures each frame of the image. Map the relative pose to the world coordinate system based on the absolute pose of the camera to obtain the absolute pose of the target object in the world coordinate system when the camera captures each frame of the image.

[0101] Specifically, on the mobile device, each frame of the real-time video stream is processed. The ORB (Object-Related Point Detection) algorithm is used to extract feature points from the video frames, and corresponding points are matched using optical flow. Combining essential matrix estimation and backend graph optimization techniques, camera motion is recovered, and a sparse point cloud map is constructed to obtain the camera's absolute pose in the world coordinate system. Based on this, combined with the target's relative pose, the target's absolute pose in the global environment is calculated through coordinate transformation. Simultaneously, an error frame verification and backoff mechanism based on multi-source information fusion is introduced to improve the stability and accuracy of pose estimation. The process includes the following steps:

[0102] Step S501: When the mobile device acquires the first frame image, it simultaneously records the GPS coordinates and IMU data of the frame, and outputs the absolute pose of the camera relative to the world coordinate system when the first frame image is captured.

[0103] Step S502: On the mobile device, ORB is used to extract feature points from consecutive frames of images, and inter-frame feature point matching is performed based on optical flow.

[0104] Step S503: Use the essential matrix estimation to recover the relative pose transformation between two adjacent frames.

[0105] Step S504: Based on backend graph optimization, the relative poses between consecutive frames are accumulated and fused to obtain the camera pose relative to the first frame when capturing the current frame. At the same time, a sparse point cloud map is constructed to assist in tracking and localization, and finally the absolute pose of the camera in the world coordinate system when capturing each frame image is obtained.

[0106] Step S505: Combining the relative pose of the target object with the absolute pose of the camera in the world coordinate system, the relative pose of the target object is mapped to the world coordinate system through coordinate transformation to obtain the absolute pose of the target object in the world coordinate system.

[0107] During the above process, user errors may render some image frames unusable. For example, a sudden, rapid camera panning can cause excessive inter-frame displacement, significantly altering the camera's pose. In such cases, the translation difference between the absolute pose of the object in the current frame and the previous frame may exceed a set threshold, triggering an anomaly flag. Similarly, if a user's finger accidentally occludes more than 30% of an object, leading to a sharp decrease in the number of detection points and abnormal distribution, or insufficient / incorrect 2D / 3D points causing abrupt pose changes, this also triggers an anomaly flag. To prevent frame skipping caused by factors such as 2D / 3D matching errors and inaccurate feature point selection, this embodiment of the invention introduces a multi-source information fusion error frame verification and rollback mechanism, specifically including the following steps:

[0108] Step S511: Calculate the Euclidean distance and rotation angle difference of the target object in adjacent frames. If the Euclidean distance or rotation angle difference exceeds the set threshold, the adjacent frames are marked as candidate error frames.

[0109] Step S512: Compare the absolute pose of the target object in the candidate error frame with the corresponding GPS data and inertial sensor data. If either the position deviation or the attitude angle deviation exceeds a set deviation threshold, the current candidate error frame is designated as an error frame. In this embodiment, the position deviation is set to 2 cm and the attitude angle deviation to 3°. Alternatively, methods such as sensor data consistency scoring can be used to assess the reliability of the current frame estimation result. If the absolute pose is significantly inconsistent with the motion trend estimated by the sensors, the current candidate error frame is designated as an error frame.

[0110] Step S513: Calculate the geometric saliency weights of the 3D feature points in all key voxel blocks within the 2D detection bounding box of the error frame:

[0111] ;

[0112] In the formula, Geometric significance weights; , As an adjustable weighting coefficient, this embodiment of the invention sets... .

[0113] The four 3D feature points with the highest geometric saliency weights are selected to reconstruct the 2D-3D corresponding point set. If the number of projected points within the 2D detection box is less than four, random sampling is performed from all 2D feature points to supplement the set.

[0114] Step S514: Calculate the relative pose between the target object and the camera based on the reconstructed 2D-3D corresponding point set. If the position deviation and attitude angle deviation of the recalculated relative pose still exceed the set deviation threshold, discard the current frame, use the pose of the previous frame, and trigger a system alarm.

[0115] To verify the effectiveness of the method provided in this embodiment of the invention, a coffee pot with dimensions of approximately 25 cm × 15 cm × 12 cm was selected as the target object. The following are the detailed steps for pose estimation of this object:

[0116] S1: Using a mobile phone, hold the coffee pot and capture 30 RGB images at a resolution of 3024×4032. During the shooting process, ensure that the object's outline is completely covered, including protruding structures such as the spout and handle. The images are input into a multi-view stereo vision (MVS) algorithm and a structure-of-motion (SFM) algorithm to reconstruct the 3D model, restore camera parameters, and construct a sparse point cloud, thereby generating a complete mesh model. After importing the 3D model into the Blender rendering engine, perform centroid normalization processing, that is, move the model's centroid to the origin.

[0117] S2: In Blender, 32 equally spaced multi-view images around the coffee pot were automatically rendered, and the poses of each camera were recorded. Approximately 320 feature points were extracted from each image on average. Combining the rendered depth map and pose matrix, approximately 9200 2D-3D point pairs were matched using ray projection. In the octree voxel space, a maximum depth of 4 and a minimum point threshold of 10 were set, generating 287 voxel blocks. Five-dimensional geometric features were extracted from each voxel block, and these features were input into a pre-trained Mamba voxel scoring network for visibility prediction, achieving a scoring accuracy of 93.1%. Finally, 58 key voxel blocks, representing the top 20% of visibility scores, were selected.

[0118] S3: After projecting the center of the key voxel block onto the rendered image, manually annotated bounding boxes are generated to train the lightweight object detection model NanoDet-Plus. A total of 50 images were used for training, achieving an accuracy of 95.8% for predicted bounding boxes with a deviation of less than 1px on the test set. In actual operation, real-time images captured by a mobile device are input into the model to locate 2D bounding boxes for key regions. A 2D-3D matching set is constructed by matching the center of the bounding box with the center point of the key voxel block. Outliers are removed using RANSAC, and finally, the MLPnP algorithm is used to calculate the relative pose of the coffee pot relative to the camera.

[0119] S4: The phone is slowly moved on the table to capture a 20-second video sequence at 15fps (300 frames total), with each frame linked to GPS and IMU information. Relative pose transformation is calculated using ORB feature point matching and optical flow estimation between consecutive frames, and the camera's global path is recovered through backend graph optimization. The relative pose of the coffee pot is mapped to the world coordinate system to obtain its absolute pose. In testing, pose jumps occurred in some frames when the phone moved rapidly or was occluded, with an error frame rate of approximately 5.6%. By enabling a multi-source information fusion backoff mechanism, approximately 87% of abnormal frames were accurately identified and repaired. The final generated coffee pot trajectory exhibits good continuity in the sparse map without frame jumps. Table 1 compares the pose estimation errors of the method in this invention with other methods, where SuperPoint is a self-supervised feature point detection and description network, PnP is a 2D-3D matching algorithm, YOLOv8 is an object detection algorithm, and EPnP is a 3D-2D matching algorithm.

[0120] Table 1 Comparison of Pose Estimation Results

[0121] method Average position error (cm) Mean attitude error (°) Error Frame Ratio Feature point matching success rate This invention 1.42 2.1 5.6% 92% SuperPoint+PnP 3.87 6.8 21.3% 68% YOLOv8+EPnP 3.25 7.3 32.1% 79%

[0122] As shown in Table 1, the method provided by the embodiments of the present invention exhibits significant advantages in key performance indicators. Regarding the average position error, the embodiment of the present invention has an error of only 1.42 cm, a reduction of 63.3% compared to the SuperPoint+PnP method and 56.3% compared to the YOLOv8+EPnP method. The attitude control accuracy is also outstanding; the average attitude error of the method in the embodiments of the present invention is 2.1°, only 30.9% of the SuperPoint+PnP method and 28.8% of the YOLOv8+EPnP method. In terms of system stability, the error frame ratio of the embodiments of the present invention is controlled at 5.6%, a reduction of 73.7% compared to the SuperPoint+PnP method and 82.6% compared to the YOLOv8+EPnP method. Simultaneously, the feature point matching success rate of the method in the embodiments of the present invention is as high as 92%, significantly outperforming the 68% of SuperPoint+PnP and the 79% of YOLOv8+EPnP.

[0123] In summary, the pose estimation method based on key voxel block selection proposed in this invention first extracts the multi-dimensional geometric structural features of each voxel block through octree voxel partitioning of the 3D model. After serializing and sorting the voxels based on spatial proximity, the lightweight state-space network Mamba is introduced to filter key voxel blocks, achieving accurate focusing and lossless compression of high-information-density regions of the model. This provides an efficient and robust 3D representation for subsequent pose estimation, enabling automatic identification of key structural regions in the 3D model and avoiding redundant feature interference. The 2D-3D point pair construction strategy combining key region detection and the MLPnP algorithm effectively improves the robustness and accuracy of pose estimation. In terms of inter-frame processing, by combining the target pose change trend in continuous visual frames with the physical motion information recorded by IMU and GPS sensors, the system identifies pose estimation jump frames. Once a visually abnormal frame is detected, the system can automatically revert to the detection stage of that frame, reselect feature points, and recalculate the target pose, effectively avoiding pose estimation drift caused by momentary errors. It not only balances accuracy and real-time performance but also adapts to resource-constrained mobile environments, providing a new technical approach for highly reliable pose estimation of target objects.

[0124] like Figure 2 As shown, this embodiment of the invention also provides a pose estimation system based on key voxel block selection, which is implemented based on the above-mentioned pose estimation method based on key voxel block selection, including: a 3D reconstruction module, a 3D feature point set construction module, a key voxel extraction module, a relative pose estimation module, and an absolute pose estimation module.

[0125] 3D Reconstruction Module: Uses a camera to acquire multi-view images of the target object, applies multi-view stereo algorithms and motion structure recovery algorithms to perform 3D reconstruction of the multi-view images, constructs a 3D model of the target object, imports it into the rendering engine, and moves the center point of the model to the origin.

[0126] 3D Feature Point Set Construction Module: Generates multi-view image sets around the target in the rendering engine and records the camera pose simultaneously. Extracts the 2D feature point set from each image and obtains the 3D feature point set corresponding to each 2D feature point set through ray projection method.

[0127] Key voxel extraction module: Divide the 3D feature point set into several voxel blocks, extract multiple features of each voxel block to construct a feature matrix, input the feature matrix into the neural network, and filter out key voxel blocks.

[0128] Relative pose estimation module: Project the center point of the key voxel block onto the image coordinate system, label the projection area to form a training dataset, use the training dataset to train the target detection model, input the real-time video stream into the trained target detection model, and obtain the relative pose of the target object in the camera coordinate system.

[0129] Absolute pose estimation module: Combines the camera pose in the world coordinate system and the relative pose of the target object in the camera coordinate system to calculate the absolute pose of the target object in the world coordinate system.

[0130] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described; only preferred embodiments of the present invention are illustrated. The descriptions are relatively specific and detailed, but they should not be construed as limiting the scope of the present invention. As long as the combination of these technical features does not contradict each other, it should be considered within the scope of this specification.

[0131] It should be noted that those skilled in the art can make various modifications and improvements without departing from the inventive concept, and these all fall within the scope of protection of this invention. Therefore, the scope of protection of this invention should be determined by the appended claims.

Claims

1. A pose estimation method based on key voxel block selection, characterized in that, Includes the following steps: Step S1: Construct a three-dimensional model of the target object, obtain multi-view images of the three-dimensional model, extract two-dimensional feature points from each view image, and project the two-dimensional feature points into three-dimensional feature points through ray projection to obtain a set of three-dimensional feature points; Step S2: Voxelize the three-dimensional feature point set to obtain a set of voxel blocks. Calculate the geometric features of each voxel block in the set of voxel blocks. Input the geometric features into Mamba, a linear time series model with a selected state space. Mamba predicts the visibility frequency of the voxel blocks under multiple views based on the geometric features. The top N voxel blocks with the highest visibility frequency are taken as key voxel blocks. The Mamba method predicts the visibility frequency of voxel blocks in multiple viewpoints based on geometric features, including the following steps: Step S21: Extract the hidden state matrix of the geometric feature sequence using Mamba. The hidden state matrix is ​​then subjected to layer normalization to obtain the layer-normalized hidden state matrix. ; Step S22: For Perform a single-layer linear transformation to obtain the visible frequencies of the voxel block from multiple viewpoints. : ； In the formula, The activation function is Sigmoid; The learnable parameter matrix; For bias terms; Step S23: The ratio of the number of times a voxel block is observed in the multi-view image to the total number of views is used as the global visibility of the voxel block, and the global visibility of all voxel blocks is combined as the supervision vector. Step S3: Project the center point of the key voxel block onto the image coordinate system, generate a training set by labeling the projection area, and train the target detection model using the training set; Step S4: Input the video stream containing the target object into the trained target detection model to obtain the relative pose between the target object and the camera in each frame of the video stream; Step S5: Obtain the absolute pose of the target object in the world coordinate system when the camera captures each frame of the image. Map the relative pose to the world coordinate system based on the absolute pose of the camera to obtain the absolute pose of the target object in the world coordinate system when the camera captures each frame of the image.

2. The pose estimation method based on key voxel block selection according to claim 1, characterized in that: Step S2 involves voxelizing the set of three-dimensional feature points, which includes the following steps: taking the bounding box of the three-dimensional model as the root node; if the number of three-dimensional feature points contained in the current node is greater than the set number threshold, then dividing the three-dimensional feature points in the current node into eight child nodes, until the number of three-dimensional feature points in all leaf nodes is less than or equal to the number threshold, thus obtaining a set of voxel blocks with an octree structure.

3. The pose estimation method based on key voxel block selection according to claim 2, characterized in that: In step S2, the geometric features of all voxel blocks are sorted to obtain a geometric feature sequence. The geometric features of all voxel blocks are used as the geometric feature sequence and input into Mamba. The sorting of the geometric features of all voxel blocks includes the following steps: constructing a geometric feature matrix based on the geometric features of all voxel blocks, performing a depth-first traversal of all leaf nodes of the octree structure, sorting the traversal results using Morton codes to obtain an ordered voxel list, and rearranging the row vectors of the geometric feature matrix according to the ordered voxel list to obtain the geometric feature sequence.

4. The pose estimation method based on key voxel block selection according to claim 1, characterized in that: Step S4 includes the following steps: Step S41: Input the current frame containing the target object into the trained target detection model, and the target detection model outputs a two-dimensional detection box of the key region of the target object; Step S42: Match the center point of the key voxel block with the center point of the two-dimensional detection box to obtain the two-dimensional-three-dimensional corresponding point set; Step S43: Filter out erroneous matching points in the 2D-3D corresponding point set, and calculate the relative pose between the target object and the camera based on the remaining corresponding points in the 2D-3D corresponding point set.

5. The pose estimation method based on key voxel block selection according to claim 1, characterized in that: Step S5, which involves obtaining the absolute pose of the camera in the world coordinate system, includes the following steps: Step S501: Obtain the absolute pose of the camera in the world coordinate system when it captures the first frame image; Step S502: Extract feature points from consecutive frame images, track inter-frame feature points based on optical flow, accumulate the relative pose transformation between adjacent frames according to the optical flow tracking results, and obtain the relative pose of each frame image relative to the first frame image. Step S503: Calculate the absolute pose of each frame image in the world coordinate system based on the absolute pose of the first frame image in the world coordinate system and the relative pose of each frame image relative to the first frame image.

6. The pose estimation method based on key voxel block selection according to claim 4, characterized in that: After generating the absolute pose of the target object in each frame of the video stream in step S5, error frame verification and rollback are performed based on multi-source information, including the following steps: Step S511: Calculate the Euclidean distance and rotation angle difference of the target object in adjacent frames. If the Euclidean distance or rotation angle difference exceeds the set threshold, the adjacent frame is marked as a candidate error frame. Step S512: Compare the absolute pose of the target object in the candidate error frame with the global positioning system data and inertial sensor data corresponding to the target object. If either the position deviation or the attitude angle deviation exceeds the set deviation threshold, then the current candidate error frame is taken as an error frame. Step S513: Calculate the geometric saliency weights of the three-dimensional feature points in all key voxel blocks in the two-dimensional detection frame of the error frame, and select the four three-dimensional feature points with the highest geometric saliency weights to reconstruct the two-dimensional-three-dimensional corresponding point set; Step S514: Calculate the relative pose between the target object and the camera based on the reconstructed 2D-3D corresponding point set. If the position deviation and attitude angle deviation of the recalculated relative pose still exceed the set deviation threshold, discard the current frame.

7. The pose estimation method based on key voxel block selection according to claim 1, characterized in that: Step S2 further includes step S24: calculating the binary cross-entropy loss of the neural network, and optimizing the visible frequency of the neural network output to approximate the global visibility by minimizing the binary cross-entropy loss. The expression for the binary cross-entropy loss is: ； In the formula, For binary cross-entropy loss; The number of voxel blocks; voxel blocks Weighting coefficients; voxel blocks Global visibility; voxel blocks Importance score.

8. The pose estimation method based on key voxel block selection according to claim 1, characterized in that: The geometric features mentioned in step S2 include the number of voxel points, voxel point density, local curvature, normal vector consistency, and flatness. The number of voxel points is the total number of three-dimensional feature points contained in the voxel block; The voxel density is the ratio of the number of voxels to the actual volume of space occupied by the voxel block. The expression for calculating the local curvature is: ； In the formula, voxel blocks Local curvature; To obtain the eigenvalues ​​by performing principal component analysis on the position coordinates of all 3D feature points within the voxel block, and ; The expression for calculating the consistency of the normal vector is: ； In the formula, voxel blocks Consistency of normal vectors; voxel blocks The statistical standard deviation of the normal vectors of all three-dimensional feature points within the area; The expression for calculating the flatness is: ； In the formula, voxel blocks Flatness.

9. The pose estimation method based on key voxel block selection according to claim 6, characterized in that: The expression for calculating the geometric saliency weights of the three-dimensional feature points in step S513 is as follows: ； In the formula, Geometric significance weights; , These are adjustable weighting coefficients; voxel blocks Local curvature; voxel blocks Flatness.

10. A pose estimation system based on key voxel block selection, implemented based on the pose estimation method based on key voxel block selection as described in any one of claims 1-9, characterized in that, include: The module includes a 3D reconstruction module, a 3D feature point set construction module, a key voxel extraction module, a relative pose estimation module, and an absolute pose estimation module. The 3D reconstruction module: uses a camera to acquire multi-view images of the target object, applies multi-view stereo algorithm and motion structure recovery algorithm to perform 3D reconstruction of the multi-view images, constructs a 3D model of the target object, imports it into the rendering engine, and moves the center point of the model to the origin. The three-dimensional feature point set construction module generates a multi-view image set around the target in the rendering engine and records the camera pose simultaneously. It extracts the two-dimensional feature point set in each image and obtains the three-dimensional feature point set corresponding to each two-dimensional feature point set through the ray projection method. The key voxel extraction module: divides the three-dimensional feature point set into several voxel blocks, extracts multiple features of each voxel block to construct a feature matrix, inputs the feature matrix into Mamba, and filters out key voxel blocks; The relative pose estimation module: projects the center point of the key voxel block onto the image coordinate system, labels the projection area to form a training dataset, uses the training dataset to train the target detection model, inputs the real-time video stream into the trained target detection model, and obtains the relative pose of the target object in the camera coordinate system. The absolute pose estimation module calculates the absolute pose of the target object in the world coordinate system by combining the pose of the camera in the world coordinate system and the relative pose of the target object in the camera coordinate system.

Images