# US 12,591,985 B1 · Point cloud registration state determination based on directional weighting

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 12,591,985 B1
- Document ID: US12591985B1
- Status: Granted
- Kind: B1
- Application number: 19/371,867
- Filed: 2025-10-28
- Granted: 2026-03-31
- Earliest claimed: 2025-10-28
- Assignees: VisionNav Robotics USA Inc.
- Applicants: VisionNav Robotics USA Inc.
- Inventors: Shuai Jia; Yujie Lu; Fan Zheng; Jianhong Xu
- Web page: https://patentgrep.com/patent/US12591985B1

## CPC

- G06T 7/33: using feature-based methods
- B66F 9/063: {Automatically guided}
- B66F 9/0755: {Position control; Position detectors}
- G01S 17/89: for mapping or imaging
- G06T 2207/10028: Range image; Depth image; 3D point clouds
- G06T 2207/30252: Vehicle exterior; Vicinity of vehicle
- G06T: IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- B66F: HOISTING, LIFTING, HAULING OR PUSHING, NOT OTHERWISE PROVIDED FOR, e.g. DEVICES WHICH APPLY A LIFTING OR PUSHING FORCE DIRECTLY TO THE SURFACE OF A LOAD ({invalid lifting devices A61G7/10; } mounting artificial islands on piles or like supports E02B17/00; scaffolding combined with lifting devices E04G1/22; lifting devices for sliding forms E04G11/24; lifting of buildings E04G23/06; shores or struts E04G25/00; lifts or other hoisting devices on ladders E06C7/12; props for mining E21D15/00)
- G01S: RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES

## Abstract

An autonomous mobile machine, a controller and a method are provided. The method includes: transforming an original point cloud to obtain a transformed point cloud; determining a first point cloud set; determining a weight value w1; dividing the original point cloud into a second and third point cloud sets; and determining a registration state of the original point cloud based on a distance between a point in the first point cloud set and a corresponding point in a target point cloud, a distance between a point in a transformed second point cloud set and a corresponding point in the target point cloud, a distance between a point in a transformed third point cloud set and a corresponding point in the target point cloud, w1, a weight value w2, and a weight value w3.

## Claims

### Claim 1

An autonomous mobile machine, comprising: a controller that is configured to execute program instructions to implement the following actions:
obtaining an original point cloud and a target point cloud;
registering the original point cloud and the target point cloud, and obtaining a HESSIAN matrix and a registered pose upon convergence of the registration;
transforming the original point cloud based on the registered pose, to obtain a transformed point cloud;
determining a first point cloud set based on the HESSIAN matrix and the transformed point cloud;
determining a first weight value w1 of the first point cloud set based on a first eigenvalue and a second eigenvalue from the HESSIAN matrix, wherein the first eigenvalue is associated with a first direction and the second eigenvalue is associated with a second direction, wherein the first direction is perpendicular to the second direction;
dividing the original point cloud into a second point cloud set and a third point cloud set;
transforming the second point cloud set and the third point cloud set based on the registered pose;
determining a second weight value w2 of the transformed second point cloud set and a third weight value w3 of the transformed third point cloud set; and
determining a registration state of the original point cloud based on a distance between a point in the first point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed second point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed third point cloud set and a corresponding point in the target point cloud, the first weight value w1, the second weight value w2, and the third weight value w3.

### Claim 2 (depends on 1)

The autonomous mobile machine according to claim 1, wherein said determining a first point cloud set based on the HESSIAN matrix and the transformed point cloud comprises:
extracting a sub-matrix associated with the first direction and the second direction from the HESSIAN matrix, wherein the second direction is a forward direction of the autonomous mobile machine, the first direction is perpendicular to the second direction and is located in a same horizontal plane as the second direction;
performing eigenvalue decomposition on the sub-matrix, to obtain the first eigenvalue and the second eigenvalue; and
performing dimensionality augmentation on an eigenvector V(a, b) corresponding to a smallest one of the first eigenvalue and the second eigenvalue, to obtain V_ext(a, b, 0), wherein a is a projection component of the eigenvector in the second direction, b is a projection component of the eigenvector in the first direction, 0 is a projection component of the eigenvector in a third direction, and the third direction is perpendicular to the plane in which the first direction and the second direction are located.

### Claim 3 (depends on 2)

The autonomous mobile machine according to claim 2, wherein said determining a first point cloud set based on the HESSIAN matrix and the transformed point cloud further comprises:
determining a plane normal vector n of each point in the transformed point cloud;
calculating an absolute value |n·V_ext| of a dot product of the plane normal vector n and the V_ext;
determining whether the absolute value is greater than or equal to a first threshold, and
in accordance with a determination that the absolute value is greater than or equal to the first threshold, retaining a corresponding point; or
in accordance with a determination that the absolute value is less than the first threshold, discarding a corresponding point; and
determining, for the retained corresponding point, whether a coordinate value of the retained corresponding point in the third direction is greater than a second threshold, and
in accordance with a determination that the coordinate value of the retained corresponding point in the third direction is greater than the second threshold, setting the retained corresponding point as an upper degenerate feature point; or
in accordance with a determination that the coordinate value of the retained corresponding point in the third direction is not greater than the second threshold, setting the retained corresponding point as a lower degenerate feature point,
wherein the first point cloud set comprises the upper degenerate feature point and the lower degenerate feature point.

### Claim 4 (depends on 1)

The autonomous mobile machine according to claim 1, wherein said determining a first weight value w1 of the first point cloud set based on a first eigenvalue and a second eigenvalue from the HESSIAN matrix, wherein the first eigenvalue is associated with a first direction and the second eigenvalue is associated with a second direction, comprises:
determining that N=the second eigenvalue÷the first eigenvalue, wherein the second eigenvalue is greater than the first eigenvalue; and
determining whether N is greater than a third threshold,
in accordance with a determination that N is greater than the third threshold, setting the first weight value w1 to be equal to the third threshold; and
in accordance with a determination that N is not greater than the third threshold, setting the first weight value w1 to be equal to N.

### Claim 5 (depends on 1)

The autonomous mobile machine according to claim 1, wherein said dividing the original point cloud into a second point cloud set and a third point cloud set comprises:
segmenting the original point cloud based on a height threshold, to obtain a roof point cloud, an upper feature point cloud, a lower feature point cloud, and a ground point cloud, wherein,
the second point cloud set comprises the upper feature point cloud and the lower feature point cloud, and the third point cloud set comprises the roof point cloud and the ground point cloud.

### Claim 6 (depends on 5)

The autonomous mobile machine according to claim 5, the controller is further configured to execute program instructions to implement the following actions:
determining a total quantity of the roof point cloud and the ground point cloud in the third point cloud set; and
determining whether the total quantity is greater than or equal to a fourth threshold, and
in accordance with a determination that the total quantity is greater than or equal to the fourth threshold, performing downsampling processing on the roof point cloud and the ground point cloud, such that the total quantity after downsampling approaches the fourth threshold.

### Claim 7 (depends on 5)

The autonomous mobile machine according to claim 5, wherein said transforming the second point cloud set and the third point cloud set based on the registered pose comprises:
transforming the roof point cloud, the upper feature point cloud, the lower feature point cloud, and the ground point cloud separately by using the registered pose, such that the transformed roof point cloud, the transformed upper feature point cloud, the transformed lower feature point cloud, and the transformed ground point cloud are in the same coordinate system as the target point cloud.

### Claim 8 (depends on 1)

The autonomous mobile machine according to claim 1, wherein the first weight value w1 is greater than the second weight value w2.

### Claim 9 (depends on 8)

The autonomous mobile machine according to claim 8, wherein the second weight value w2 is greater than the third weight value w3.

### Claim 10 (depends on 1)

The autonomous mobile machine according to claim 1, wherein said determining a registration state of the original point cloud based on a distance between a point in the first point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed second point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed third point cloud set and a corresponding point in the target point cloud, the first weight value w1, the second weight value w2, and the third weight value w3 comprises:
inputting each point in the first point cloud set and the first weight value w1 corresponding to each point, each point in the transformed second point cloud set and the second weight value w2 corresponding to each point, and each point in the transformed third point cloud and the third weight value w3 corresponding to each point; and
traversing each inputted point to determine whether a current point is a last point, and
in accordance with a determination that the current point is not the last point, calculating a distance dis_temp between the current point and a corresponding nearest neighbor point in the target point cloud, and
determining whether the current point is a valid point;
in accordance with a determination that the current point is the valid point, outputting a total distance all_dis and a total weight cout_num, wherein,
the total distance all_dis is a sum of the products of the distance dis_temp and the corresponding weight value for each of the valid points, wherein the corresponding weight value is determined based on the following rule: when a calculated object is a point in the first point cloud set, the corresponding weight value is the first weight value w1; when a calculated object is a point in the second point cloud set, the corresponding weight value is the second weight value w2; and when a calculated object is a point in the third point cloud set, the third point cloud set corresponds to the third weight value w3; and
the total weight cout_num is a sum of the weight values of all of the valid points.

### Claim 11 (depends on 10)

The autonomous mobile machine according to claim 10, wherein said determining whether the current point is a valid point comprises:
determining whether the distance dis_temp of the current point is greater than a fifth threshold, and
in accordance with a determination that the distance dis_temp of the current point is not greater than the fifth threshold, determining that the current point is the valid point; or
in accordance with a determination that the distance dis_temp of the current point is greater than the fifth threshold,
counting a quantity cout_outline of points having the distance dis_temp greater than the fifth threshold; and
determining whether the quantity cout_outline is greater than a sixth threshold, and
in accordance with a determination that the quantity cout_outline is greater than the sixth threshold,
for all points counted in the point cloud type to which the current point belongs, based on a descending order of their distances dis_temp, deleteing a portion of the points ranked top to satisfy the quantity of points defined by the sixth threshold, and setting points not deleted as the valid points; or
in accordance with a determination that the quantity cout_outline is not greater than the sixth threshold,
determining that the current point is the valid point.

### Claim 12 (depends on 11)

The autonomous mobile machine according to claim 11, wherein the sixth threshold for the upper degenerate feature point, the upper feature point cloud, and the roof point cloud is a first percentage of a total quantity of corresponding point clouds, the sixth threshold for the lower degenerate feature point, the lower feature point cloud, and the ground point cloud is a second percentage of the total quantity of the corresponding point clouds, and the first percentage is less than the second percentage.

### Claim 13 (depends on 10)

The autonomous mobile machine according to claim 10, wherein said determining a registration state of the original point cloud based on a distance between a point in the first point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed second point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed third point cloud set and a corresponding point in the target point cloud, the first weight value w1, the second weight value w2, and the third weight value w3 further comprises:
adding, after the valid points are determined, a product of the distance dis_temp and the corresponding weight value for each of the valid points to the total distance all_dis;
adding the corresponding weight value of each of the valid points to the total weight cout_num; and
returning to the action of traversing each inputted point to determine whether a current point is a last point, until all points are processed completely.

### Claim 14 (depends on 10)

The autonomous mobile machine according to claim 10, wherein said determining a registration state of the original point cloud based on a distance between a point in the first point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed second point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed third point cloud set and a corresponding point in the target point cloud, the first weight value w1, the second weight value w2, and the third weight value w3 comprises:
determining that a registration error is an average weighted distance of the valid points, and the average weighted distance is equal to the total distance all_dis divided by the total weight cout_num.

### Claim 15 (depends on 14)

The autonomous mobile machine according to claim 14, wherein said determining a registration state of the original point cloud based on a distance between a point in the first point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed second point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed third point cloud set and a corresponding point in the target point cloud, the first weight value w1, the second weight value w2, and the third weight value w3 comprises:
comparing the registration error with a first range, to evaluate the registration state,
wherein, if the registration error falls within the first range, determining that the registration state is normal;
if the registration error does not fall within the first range:
determining that the registration state is abnormal; and
controlling the autonomous mobile machine to suspend work, and sending information to give a prompt of manual intervention.

### Claim 16 (depends on 15)

The autonomous mobile machine according to claim 15, wherein the first range is greater than or equal to 0.06 and less than or equal to 0.12.

### Claim 17 (depends on 2)

The autonomous mobile machine according to claim 2, wherein the autonomous mobile machine works in an environment of a corridor, the first direction corresponds to a direction perpendicular to a wall of the corridor, and the first direction corresponds to a direction associated with the eigenvector corresponding to the smallest one of the first eigenvalue and the second eigenvalue.

### Claim 18 (depends on 1)

The autonomous mobile machine according to claim 1, wherein said registering the original point cloud and the target point cloud, and obtaining a HESSIAN matrix and a registered pose upon convergence of the registration comprises:
registering the original point cloud and the target point cloud by using a generalized iterative closest point (GICP) algorithm or an iterative closest point (ICP) algorithm;
performing iteration by using a Levenberg-Marquardt algorithm or a Gauss-Newton algorithm and by using a point-to-plane distance as a residual, to optimize registration of the original point cloud and the target point cloud; and
determining, when an absolute value of a change of a pose between two adjacent iterations at each degree of freedom is less than an eighth threshold, that the registration converges, to obtain the HESSIAN matrix and the registered pose upon convergence of the registration.

### Claim 19

A controller, configured to execute program instructions to implement the following actions:
obtaining an original point cloud and a target point cloud;
registering the original point cloud and the target point cloud, and obtaining a HESSIAN matrix and a registered pose upon convergence of the registration;
transforming the original point cloud based on the registered pose, to obtain a transformed point cloud;
determining a first point cloud set based on the HESSIAN matrix and the transformed point cloud;
determining a first weight value w1 of the first point cloud set based on a first eigenvalue and a second eigenvalue from the HESSIAN matrix, wherein the first eigenvalue is associated with a first direction and the second eigenvalue is associated with a second direction, wherein the first direction is perpendicular to the second direction;
dividing the original point cloud into a second point cloud set and a third point cloud set;
transforming the second point cloud set and the third point cloud set based on the registered pose;
determining a second weight value w2 of the transformed second point cloud set and a third weight value w3 of the transformed third point cloud set; and
determining a registration state of the original point cloud based on a distance between a point in the first point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed second point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed third point cloud set and a corresponding point in the target point cloud, the first weight value w1, the second weight value w2, and the third weight value w3.

### Claim 20

A point cloud matching state determining method for an autonomous mobile machine, comprising:
obtaining an original point cloud and a target point cloud;
registering the original point cloud and the target point cloud, and obtaining a HESSIAN matrix and a registered pose upon convergence of the registration;
transforming the original point cloud based on the registered pose, to obtain a transformed point cloud;
determining a first point cloud set based on the HESSIAN matrix and the transformed point cloud;
determining a first weight value w1 of the first point cloud set based on a first eigenvalue and a second eigenvalue from the HESSIAN matrix, wherein the first eigenvalue is associated with a first direction and the second eigenvalue is associated with a second direction, wherein the first direction is perpendicular to the second direction;
dividing the original point cloud into a second point cloud set and a third point cloud set;
transforming the second point cloud set and the third point cloud set based on the registered pose;
determining a second weight value w2 of the transformed second point cloud set and a third weight value w3 of the transformed third point cloud set; and
determining a registration state of the original point cloud based on a distance between a point in the first point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed second point cloud set and a corresponding point in the target point cloud, a distance between a point in the transformed third point cloud set and a corresponding point in the target point cloud, the first weight value w1, the second weight value w2, and the third weight value w3.