FPGA (Field Programmable Gate Array)-based acceleration method for iterative nearest point algorithm

Abstract

The invention discloses an acceleration method of an iterative nearest point algorithm based on an FPGA (Field Programmable Gate Array), which comprises the following steps of: performing voxelization on a three-dimensional space, and after the voxelization is finished, converting nearest neighbor search in the iterative nearest point algorithm from global search to local search for target point cloud by adopting an expansion strategy; and the plurality of calculation units are used for simultaneously calculating the nearest neighbor target points of the plurality of source points, and the parallelism of the FPGA is fully utilized, so that the nearest neighbor search speed is improved, and the effect of accelerating the iterative nearest point algorithm is realized. Besides, on the basis of the expansion strategy, the memory is allocated by utilizing a traversal method in advance, so that the use of the memory is greatly reduced, the point cloud registration can be completed by only using the on-chip memory, and the low-power-consumption implementation of the algorithm is further completed.

Description

Technical Field

[0001] This invention relates to a computer vision processing method, specifically to an iterative nearest-point algorithm acceleration method for point cloud registration. Background Technology

[0002] Point cloud registration is a crucial step in 3D reconstruction. By aligning point cloud data from different perspectives, a more comprehensive information about the 3D model can be obtained. The Iterative Closest Point (ICP) algorithm is currently the mainstream algorithm in the field of point cloud registration. This algorithm achieves the registration of two point clouds through multiple iterations. In each iteration, the algorithm first performs a nearest neighbor search to match corresponding points between the source and target point clouds. Using these corresponding points, the corresponding rotation and translation parameters can be calculated. These two parameters are used to transform the source point cloud, gradually aligning it with the target point cloud. This process is repeated iteratively until a preset iteration termination condition is reached.

[0003] Iterative nearest neighbor algorithms are widely used in fields such as autonomous driving positioning, medical imaging, and architectural design. The nearest neighbor search in particular consumes a lot of time because it requires calculating the Euclidean distance between the source point and all target points. Current methods to accelerate iterative nearest neighbor algorithms either require a large amount of memory, resulting in high power consumption, or the system running speed of the algorithm still needs to be improved, making it difficult to meet the application requirements for real-time processing of point cloud data. Summary of the Invention

[0004] Purpose of the invention: To address the aforementioned issues in the prior art, this invention proposes an accelerated method for the iterative closest point algorithm based on FPGA, which solves the problems of high power consumption, large number of iterations, and long processing time in existing accelerated iterative closest point algorithms.

[0005] Technical solution: An acceleration method for the iterative nearest point algorithm based on FPGA, comprising:

[0006] Step 1: Implement the iterative nearest point algorithm in MATLAB using a fixed-point method to obtain the data bit width required for its implementation on an FPGA;

[0007] Step 2: Voxelize the source point cloud and the target point cloud. Obtain the number of points within each voxel by traversing it in advance. Allocate memory resources for the entire voxel space according to the number of points to reduce the use of storage space.

[0008] Step 3: Perform an expansion operation on the voxelized target point cloud so that the index encoding of the root voxel containing the target point is filled into the empty voxel without the target point, thereby converting the global nearest neighbor search into a local nearest neighbor search.

[0009] Step 4: In the voxel space, the nearest neighbor search of the source point cloud and the target point cloud is performed in parallel through multiple processing units to obtain matrix H;

[0010] Step 5: Perform singular value decomposition on matrix H to obtain rotation and translation parameters, and transform the source point cloud using the rotation and translation parameters;

[0011] Step 6: If the iterative nearest point algorithm has reached the maximum number of iterations, then end the iteration; otherwise, voxelize the transformed source point cloud again and repeat steps 4 and 5 until the maximum number of iterations is reached.

[0012] Furthermore, in step 2, the number of points within each voxel is obtained through pre-traversal, and memory resources are allocated to the entire voxel space according to the number of points. Specifically, this includes:

[0013] a: Define a register set point_reg to store the number of points inside each voxel, where the number of registers is equal to the number of voxel blocks;

[0014] b: Create RAM_voxel and store the addresses of the points in RAM_point in voxel order to facilitate the nearest neighbor search in the voxel block later; where, storing in voxel order satisfies the following: first, it is stored in the order of voxel block index from smallest to largest; second, the storage locations of points belonging to the same voxel block are adjacent.

[0015] Furthermore, step 3 specifically includes: identifying the root voxel, controlling the voxel to expand sequentially in six directions using a counter, completing one direction's expansion in each clock cycle; wherein, for any direction, expanding the root voxel in that direction includes: encoding the indices of all root voxels and passing them to the empty voxels in that direction; if the direction is not empty, then no index needs to be passed; after expansion in that direction, the register values ​​corresponding to some empty voxels become non-zero values, called expanded voxels; all subsequent root voxels and expanded voxels undergo expansion operations in other directions until the register values ​​corresponding to the voxel blocks in the entire space are all non-zero values, at which point the expansion operation is completed; the voxelized source point cloud does not need to undergo expansion operations; after the expansion operation, any voxel containing a source point can find neighboring voxels containing a target point, thereby transforming the nearest neighbor search space from the entire target point cloud to the voxel block.

[0016] Furthermore, step 4 specifically includes: firstly, obtaining the index of the voxel containing the source point through counter control; then, using several processing units to perform a nearest neighbor search of several source points within the voxel in ascending order of index, and finding the nearest neighbor target point that matches the source point.

[0017] Furthermore, in step 2, a counter is used to control the reading of point coordinates. After the coordinates of each point are read, the index of the voxel where the point is located is obtained through an adder, a shift register, and a pipelined stepping operation. After obtaining the index, a multiplexer selects the register corresponding to the voxel in the register group point_reg and increments its value by one. When all the point coordinates have been read, the number of points inside all voxel blocks is obtained. At this time, each register in the register group stores the number of points inside a voxel.

[0018] Beneficial Effects: This invention voxelizes the 3D space. After voxelization, an expansion strategy is applied to the target point cloud, transforming the nearest neighbor search in the iterative nearest point algorithm from a global search to a local search. Furthermore, multiple computing units simultaneously calculate the nearest neighbor target points of multiple source points, fully utilizing the parallelism of the FPGA to improve the speed of the nearest neighbor search and accelerate the iterative nearest point algorithm. In addition, based on the expansion strategy, this invention uses a pre-traversal method to allocate memory, significantly reducing memory usage. This allows the method to complete point cloud registration using only on-chip memory, thus achieving low-power implementation of the algorithm. Attached Figure Description

[0019] Figure 1 This is a flowchart of the method of the present invention;

[0020] Figure 2 This is a schematic diagram of memory allocation in this invention;

[0021] Figure 3 This is the circuit diagram corresponding to the expansion strategy used in this invention;

[0022] Figure 4 This is a schematic diagram illustrating the parallel nearest neighbor search implementation of the present invention;

[0023] Figure 5 This is a hardware architecture diagram of the Cordic algorithm for implementing the arctangent calculator and sine / cosine wave generator of the present invention;

[0024] Figure 6 These are comparison images of the registration of Stanford rabbit point clouds using the present invention, where (a) is the original image and (b) is the image after registration using the present invention. Detailed Implementation

[0025] The invention will now be further explained with reference to the accompanying drawings.

[0026] like Figure 1 As shown, an acceleration method for the iterative nearest point algorithm based on FPGA includes:

[0027] Step 1: First, implement the iterative nearest point algorithm in floating-point format in MATLAB, then implement it in fixed-point format to obtain the data bit width required for the algorithm's implementation on the FPGA. This includes the following sub-steps:

[0028] Step 1-1: Based on the principle of the iterative nearest point algorithm, write code in MATLAB to implement the algorithm in floating-point format.

[0029] Step 1-2: Based on the floating-point implementation of the iterative nearest point algorithm, multiply the floating-point number by 2 to the power of N and round down to the nearest integer. This converts the floating-point calculation into an integer calculation, thus realizing the fixed-point implementation of the algorithm. Record the data bit width N that meets the precision requirements, where N is a positive integer.

[0030] The data bit width N was obtained through debugging and was made as small as possible while meeting the accuracy requirements of the iterative nearest point algorithm.

[0031] Steps 1-3: When implementing the iterative nearest point algorithm on the FPGA, the data bit width N determined in step 1-2 is used.

[0032] In implementing the iterative nearest point algorithm on an FPGA, both storing and calculating the coordinates of points in the point cloud require this data bit width. For storing point coordinates, the floating-point coordinates need to be multiplied by 2 to the power of N, rounded down, and then stored in RAM. For operations involving addition, subtraction, multiplication, and division of point coordinates, the final result needs to retain an N-bit data width. In this embodiment, N = 16.

[0033] Step 2: Store the standard Stanford rabbit point cloud dataset (point coordinates) into BRAM, and then perform voxelization on the point cloud, specifically including the following sub-steps:

[0034] Step 2-1: Read the point cloud data into RAM by text reading. The coordinates of the three dimensions are stored in different RAMs to facilitate subsequent parallel data reading.

[0035] Step 2-2: Divide the voxel blocks according to the resources available to the FPGA, and voxelize the obtained point cloud data. Specifically, this includes: defining a three-dimensional voxel space containing 512 voxels, each with a side length of 1; the input point cloud coordinate range is (-1, 1), which is normalized to make the coordinate range (0, 1), and then the point cloud coordinates are multiplied by 8 to make the coordinate range (0, 8). Thus, each point in the point cloud can be mapped to the corresponding voxel.

[0036] Steps 2-3: Obtain the number of points within each voxel by traversing it in advance, and allocate memory RAM_voxel in the voxel space according to the number of points to reduce memory usage.

[0037] The memory allocation and data reading methods used in this invention are as follows: Figure 2 As shown, the coordinates of the entire point cloud are first traversed to obtain the number of points within each voxel, which is then stored in the register set `point_reg`. Specifically, a counter is used to control the reading of point coordinates. After the coordinates of each point are read, the index of the voxel containing that point is obtained through an adder, a shift register, and a pipelined stepping operation. After obtaining the index, a multiplexer selects the register in the register set `point_reg` for that voxel and increments its value by one. When all point coordinates have been read, the number of points within all voxel blocks is obtained. At this point, each register in the register set stores the number of points within a voxel.

[0038] Then, the addresses of the coordinates of points arranged in voxel order in RAM_voxel are stored in the corresponding RAM_point. The header address of voxel i in RAM_voxel is v_addr[i]. The header address of voxel (i+1) is determined by the header address of voxel i and the number of points inside voxel i. For example, for voxel i, the address of the first point inside voxel in RAM_voxel is v_addr[i], and its corresponding data space stores vi_p0_index. Through vi_p0_index, the coordinates (vi_p0_x, vi_p0_y, vi_p0_z) of point p0 can be read from RAM_point_x, RAM_point_y, and RAM_point_z. It is worth noting that this invention obtains the address of the first point inside voxel (i+1) in RAM_voxel through v_addr[i] and point_reg[i] (which stores the number of points inside voxel i).

[0039] Step 3: After voxelization, the source point within the current voxel only needs to perform a nearest neighbor search within the voxel to find its corresponding target point, i.e., a local search, which greatly reduces the time of the nearest neighbor search. However, for a voxel with a source point, there may not be a target point inside it, which will cause the nearest neighbor search of some source points to become a global search. Therefore, this invention performs an expansion operation on the target point cloud after voxelization, so that voxels without target points in the voxel space can be associated with the voxel containing the target point that is closest to them.

[0040] In three-dimensional space, a voxel is a cube, with six directions representing the top, bottom, left, right, front, and back of the cube (positive x, negative x, positive y, negative y, positive z, and negative z directions). First, the root voxel is found using the register set point_reg, along with selectors and comparators. Then, a counter controls the expansion in each of the six directions sequentially, completing one direction's expansion per clock cycle.

[0041] The circuit diagram for achieving voxel expansion in the positive z-direction in this invention is shown below. Figure 3 As shown, point_reg[a] stores the number of target points inside voxel a. If point_reg[a] is 0, it means that voxel a is an empty voxel and does not need to be expanded, that is, the value of point_reg[a+1] remains unchanged. If point_reg[a] is not 0 and point_reg[a+1] is not 0, it means that both voxel a and voxel a+1 are non-empty voxels, so voxel a does not need to be expanded. If point_reg[a] is not 0 and point_reg[a+1] is 0, then voxel a can be expanded in the positive z direction, that is, point_reg[a+1] can be assigned a value. The default point cloud has no more than 2^16 points. During dilation, it's necessary to check if `point_reg[a]` is less than 2^16. If it is, voxel 'a' is not dilated from other voxels, and `point_reg[a]` contains the number of points containing the target point within voxel 'a'. In this case, the index 'a' of voxel 'a' is added to 2^16, and then assigned to `point_reg[a+1]`. If it's greater than or equal to 2^16, voxel 'a' is dilated from other voxels, and `point_reg[a]` is the sum of the indices of other voxels and 2^16. In this case, the value of `point_reg[a]` is directly assigned to `point_reg[a+1]`. Dilation in other directions is similar to dilation in the positive z-direction. It's worth noting that voxels at the boundaries of the entire 3D voxel space do not need dilation in certain directions. For example, for voxel 7, voxel 8 is not in the positive z-direction of voxel 7; it is in the positive y-direction of voxel 0. There are no other voxels in the positive z-direction of voxel 7, so dilation in the positive z-direction is unnecessary.

[0042] Step 4: After the target point cloud is fully expanded, perform a nearest neighbor search between the source and target point clouds in voxel space. This includes:

[0043] Step 4-1: Obtain the index of the voxel of the internal source point through counter control, and perform nearest neighbor search in sequence.

[0044] Step 4-2: As Figure 4 As shown, at this point, a nearest neighbor search is performed on the source point within the voxel with index 3, and the nearest neighbor target point that matches the source point is found within the voxel.

[0045] To achieve nearest neighbor search within a voxel, this invention uses a basic processing unit (PE) to calculate the nearest neighbor target point of a source point within the voxel. For source point 1, when the PE1 trigger signal is valid, the source point is input into PE1. Simultaneously, all target points within the entire voxel are sequentially input into PE1, one target point per clock cycle. PE1 calculates the square of the Euclidean distance between the current target point and the source point, compares the obtained value with the previous minimum Euclidean distance squared value. If the current value is smaller, the value and the current index of the target point are stored in a register; otherwise, the value of the corresponding register remains unchanged. The search ends when the PE1 output signal is valid. To fully utilize the parallelism of the FPGA, this invention uses 200 PEs, allowing for nearest neighbor searches of up to 200 source points simultaneously, further improving the speed of nearest neighbor search.

[0046] Step 4-3: Multiply the source point coordinates (a 3x1 vector) and the transpose of the matched target point coordinates (a 1x3 vector) to obtain multiple 3x3 matrices. Add these 3x3 matrices together, and then normalize the resulting matrix to obtain a 3x3 matrix H.

[0047] Step 5: Use the matrix H obtained in Step 4 as the input to the singular value decomposition (SVD) module. The SVD module ultimately outputs rotation parameters R and translation parameters T, which are used to transform the source point cloud. The time consumed by SVD is only a small fraction of the entire iterative closest point algorithm; therefore, this invention only uses the conventional Jacobian matrix method to implement the singular value decomposition of the matrix, specifically including:

[0048] Step 5-1: Calculate the left rotation angle α and the right rotation angle β based on the matrix elements of matrix H and the arctangent function.

[0049] Step 5-2: Calculate the left Jacobian matrix J based on the left and right rotation angles. L And the right Jacobian matrix J R .

[0050] This section describes the hardware implementation of an arctangent calculator and a sine / cosine wave generator needed to calculate the Jacobian matrix, using the classic Cordic algorithm. The hardware architecture diagram of the Cordic algorithm is shown below. Figure 5 As shown, only shift registers, adders (subtractors), and lookup tables are used. The core idea is to iteratively approximate the rotation angle θ, and the tangent of the angle for each rotation is defined as a positive power of 2. This converts the multiplication by the tangent in the iterative formula into a shift operation in hardware, i.e., using a shift register. In addition, the angle with a tangent value of an integer power of 2 is calculated in the software in advance and input into the lookup table for easy use in subsequent iterations.

[0051] Step 5-3: Multiply the left side of matrix H by the transpose of the left Jacobian matrix, and multiply the right side of matrix H by the right Jacobian matrix to obtain a new matrix H1.

[0052] Step 5-4: Replace H with H1 and repeat steps 4-1, 4-2 and 4-3 until the off-diagonal elements of the resulting matrix are close to 0.

[0053] The formula for iteration is shown below:

[0054]

[0055] In the formula, H k+1 This represents a matrix that has undergone (k+1) Jacobian rotations. Representation matrix H k The transpose of the left Jacobian matrix, Representation matrix H k The right Jacobian matrix.

[0056] The formula for calculating the matrix U obtained from singular value decomposition is:

[0057]

[0058] In the formula, Let represent the left Jacobian matrix of matrix H after the i-th Jacobian rotation.

[0059] The formula for calculating the matrix V obtained from singular value decomposition is:

[0060]

[0061] In the formula, Let represent the right Jacobian matrix of matrix H after the i-th Jacobian rotation.

[0062] This invention uses a 16-bit fixed-point decimal representation method. After calculation, k is set to 4, that is, 5 iterations. The accuracy of the output result can meet the requirements of point cloud registration.

[0063] Step 5-5: Calculate the rotation parameter R and translation parameter T using the matrices U and V obtained in step 4-4.

[0064] The expressions for R and T are as follows:

[0065] R=VUT

[0066] T = pt – Rps

[0067] Since U and V are both 3x3 square matrices, R is also a 3x3 square matrix. pt and ps are the centroids of the target point cloud and the source point cloud, respectively, both of which are 3x1 vectors. Therefore, T is also a 3x1 vector.

[0068] Step 5-6: Transform the source point cloud using the rotation parameter R and translation parameter T obtained in step 5-5.

[0069] The specific transformation formula for the source point cloud is as follows:

[0070] S'=R*S+T

[0071] S is the coordinates of the source point in the original source point cloud, a vector of size 3x1; S' is the coordinates of the transformed point.

[0072] Step 6: If the iterative nearest point algorithm reaches the pre-set maximum number of iterations, the iteration ends and the system runs out; otherwise, the transformed source point cloud is voxelized again, and steps 4 and 5 are repeated until the maximum number of iterations is reached.

[0073] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. An acceleration method for iterative closest point algorithm based on FPGA, characterized in that: include: Step 1: Implement the iterative closest point algorithm in fixed-point format on Matlab, and then obtain the data bit width required for implementing the algorithm on FPGA; Step 2: Voxelize the source point cloud and the target point cloud, obtain the number of points inside each voxel by traversing in advance, and allocate memory resources for the entire voxel space according to the number of points; Step 3: Dilate the voxelized target point cloud so that the index of the root voxel with the target point inside is encoded and filled into the empty voxel without the target point inside, thereby converting the global nearest neighbor search into a local nearest neighbor search; Step 4: Implement the nearest neighbor search of the source point cloud and the target point cloud in parallel through multiple processing units in the voxel space to obtain the matrix H; Step 5: Perform singular value decomposition on the matrix H to obtain rotation parameters and translation parameters, and transform the source point cloud by the rotation parameters and translation parameters; Step 6: If the iterative closest point algorithm reaches the maximum number of iterations, the iteration is terminated; otherwise, the transformed source point cloud is voxelized again, and steps 4 and 5 are repeated until the maximum number of iterations is reached.

2. The acceleration method according to claim 1, characterized in that: In step 2, the number of points inside each voxel is obtained by traversing in advance, and memory resources are allocated to the entire voxel space according to the number of points, which specifically includes: a: Define a register group point_reg to store the number of points inside each voxel, where the number of registers is equal to the number of divided voxel blocks; b: Create RAM_voxel and store the address of point coordinates in RAM_point in voxel order. The storage in voxel order satisfies the following requirements:

1. Store in ascending order of voxel block index; 2. The storage locations of points belonging to the same voxel block are adjacent.

3. The acceleration method according to claim 2, characterized in that: The step 3 specifically includes: finding the root voxel, controlling the voxel to dilate in six directions in turn through a counter, and completing the dilation in one direction in each clock cycle; wherein, for any direction, dilating the root voxel in that direction includes: encoding the indexes of all root voxels and passing them to the empty voxels in that direction, and if the direction is a non-empty voxel, there is no need to pass the index; after the dilation in that direction, the values ​​of the registers corresponding to some empty voxels become non-zero values, which are called dilated voxels, and all subsequent root voxels and dilated voxels perform dilation operations in other directions until the values ​​of the registers corresponding to the voxel blocks in the entire space are all non-zero values, and the dilation operation is completed; the voxelized source point cloud does not need to be dilated; after the dilation operation, any voxel containing a source point can find a neighboring voxel containing a target point, thereby converting the nearest neighbor search space from the entire target point cloud to the voxel block.

4. The acceleration method according to claim 3, characterized in that: The step 4 specifically includes: firstly, obtaining the index of the voxel containing the source point inside by counter control, and then using several processing units to perform the nearest neighbor search of several source points in the voxel in parallel according to the index order from small to large, to find the nearest neighbor target point matching the source point.

5. The acceleration method according to any one of claims 2 to 4, characterized in that: In step 2, a counter is used to control the reading of point coordinates. After the coordinates of each point are read out, the index of the voxel where the point is located is obtained through an adder, a shift register and a pipeline beat operation; after obtaining the index, a multiplexer is used to select the register corresponding to the voxel in the register group point_reg, and its value is increased by one. When all the point coordinates are read, the number of points inside all the voxel blocks is obtained. At this time, each register in the register group stores the number of points inside a voxel.

Images