An acceleration method of iterative closest point algorithm based on FPGA

Abstract

The application discloses an acceleration method of an iterative closest point algorithm based on FPGA, voxelizes three-dimensional space, adopts an inflation strategy for a target point cloud after voxelization, converts a nearest neighbor search in the iterative closest point algorithm from global search into local search, and simultaneously calculates nearest neighbor target points of multiple source points by using multiple calculation units, fully utilizes parallelism of the FPGA, further improves the speed of the nearest neighbor search, and realizes the effect of accelerating the iterative closest point algorithm. In addition, the application allocates memory by using a prior traversal method on the basis of the inflation strategy, greatly reduces the use of the memory, and enables the method to complete point cloud registration by using only on-chip memory, and further realizes low-power consumption of the algorithm.

Description

Technical Field

[0001] The present invention relates to a computer vision processing method, and in particular to an iterative closest point algorithm acceleration method for point cloud registration. Background Art

[0002] Point cloud registration is a key step in the 3D reconstruction process. By aligning point cloud data from different perspectives, more comprehensive information about the 3D model can be obtained. The iterative closest point (ICP) algorithm is the mainstream algorithm in the current field of point cloud registration. This algorithm achieves the registration of two point clouds through multiple iterations. In each iteration, the algorithm first needs to perform a nearest neighbor search to match the corresponding points between the source point cloud and the target point cloud. Through these corresponding points, the corresponding rotation parameters and translation parameters can be calculated. These two parameters are used to transform the source point cloud so that it is gradually aligned with the target point cloud. This process will be iterated repeatedly until the preset iteration termination condition is reached.

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

[0004] Purpose of the invention: In view of the above-mentioned existing technologies, an acceleration method of the iterative closest point algorithm based on FPGA is proposed to solve the problems of the existing accelerated iterative closest point algorithm methods having high power consumption, requiring many iterations and thus taking a long time.

[0005] Technical solution: An FPGA-based acceleration method for the iterative closest point algorithm, including:

[0006] Step 1: Implement the iterative closest point algorithm in MATLAB to obtain the data bit width required for implementing the algorithm on FPGA.

[0007] Step 2: Voxelize the source and target point clouds, obtain the number of points inside each voxel by traversing in advance, and allocate memory resources to the entire voxel space according to the number of points to reduce storage space usage;

[0008] 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;

[0009] Step 4: performing nearest neighbor search of the source point cloud and the target point cloud in the voxel space by a plurality of processing units to obtain a matrix H;

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

[0011] Step 6: judging whether the iterative closest point algorithm reaches a maximum iteration number, and if so, ending the iteration, otherwise voxelizing the transformed source point cloud again, and repeating steps 4 and 5 until the maximum iteration number is reached.

[0012] Further, in step 2, the number of points in each voxel is obtained by prior traversal, and memory resources are allocated to the entire voxel space according to the number of points, specifically including:

[0013] a: defining a register group point_reg for storing the number of points in each voxel, wherein the number of registers is equal to the number of divided voxel blocks;

[0014] b: creating RAM_voxel to store the address of the point coordinates in RAM_point in voxel order, facilitating subsequent nearest neighbor search in the voxel block; wherein storing in voxel order satisfies: one, storing in the order of small to large voxel block index; two, the storage locations of points belonging to the same voxel block are adjacent.

[0015] Further, step 3 specifically includes: finding a root voxel, and controlling the voxel to perform inflation in six directions in turn by a counter, and completing inflation in one direction per clock cycle; wherein for any direction, the inflation operation of the root voxel in the direction includes: encoding the index of all root voxels and passing it to the empty voxel in the direction, and if the direction is a non-empty voxel, the index does not need to be passed; after inflation in the direction, the value of the register corresponding to part of the empty voxels becomes a non-0 value, which is called an inflation voxel, and all subsequent root voxels and inflation voxels perform inflation operations in other directions until the values of the registers corresponding to all voxel blocks in the entire space are non-0 values, and the inflation operation is completed; the voxelized source point cloud does not need to perform inflation operation; after the inflation operation, any voxel containing a source point can find a neighboring voxel containing a target point, thereby converting the search space of the nearest neighbor from the entire target point cloud to the voxel block.

[0016] Further, step 4 specifically includes: first obtaining the index of the voxel containing the source point inside by a counter, and then using a plurality of processing units to perform nearest neighbor search of a plurality of source points in the voxel in parallel according to the order of small to large index to find the nearest neighbor target point matching 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 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 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 three-dimensional space. After voxelization is complete, an expansion strategy is applied to the target point cloud, transforming the nearest neighbor search in the iterative closest point algorithm from a global search to a local search. Multiple computing units are used to simultaneously calculate the nearest neighbor target points of multiple source points, fully utilizing the parallelism of the FPGA, thereby increasing the speed of the nearest neighbor search and achieving the effect of accelerating the iterative closest point algorithm. Furthermore, based on the expansion strategy, this invention utilizes 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, thereby achieving a low-power implementation of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 is a flow chart of the method of the present invention;

[0020] Figure 2 It is a memory allocation schematic diagram of the present invention;

[0021] Figure 3 A circuit diagram corresponding to the expansion strategy used in the present invention;

[0022] Figure 4 A schematic diagram of implementing parallel nearest neighbor search in the present invention;

[0023] Figure 5 The hardware architecture diagram of the Cordic algorithm for implementing the inverse tangent calculator and sine-cosine wave generator of the present invention;

[0024] Figure 6 This is a comparison diagram of the Stanford rabbit point cloud registration performed by the present invention, where (a) is the original image and (b) is the image after registration by the present invention. DETAILED DESCRIPTION

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

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

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

[0028] Step 1-1: Based on the principle of iterative closest point algorithm, write code on 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 raised to the power of N and then round it down. This converts the floating-point calculation into an integer calculation, implements the fixed-point implementation of the algorithm, and records the data bit width N that meets the accuracy requirements, where N is a positive integer.

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

[0031] Step 1-3: When implementing the iterative closest point algorithm on the FPGA, use the data bit width N determined in step 1-2.

[0032] When implementing the iterative closest point algorithm on an FPGA, this data bit width is required for both storing and calculating the coordinates of points in the point cloud. To store point coordinates, the floating-point coordinates must first be multiplied by 2 raised to the Nth power, rounded down, and then stored in RAM. For addition, subtraction, multiplication, and division operations involving the point coordinates, the final result must retain a data bit width of N bits. In this embodiment, N = 16.

[0033] Step 2: Store the standard Stanford Rabbit point cloud dataset (point coordinates) into BRAM, and then voxelize the point cloud. This includes 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 reading of the data.

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

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

[0037] The memory allocation and data reading method used in the present invention is as follows Figure 2 As shown, first traverse the coordinates of the entire point cloud to obtain the number of points inside each voxel and store it in the register group point_reg. Specifically, 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 of 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 voxel blocks is obtained. At this time, each register in the register group stores the number of points inside a voxel.

[0038] Then, the coordinates of the points arranged in voxel order are stored in the corresponding RAM_point in RAM_voxel. The head address of voxel i in RAM_voxel is v_addr[i]. The head address of voxel (i+1) is determined by the head address of voxel i and the number of points inside voxel i. For example, for voxel i, the address of the first point inside in RAM_voxel is v_addr[i], and its corresponding data space stores vi_p0_index. Through vi_p0_index, the coordinates of point p0 (vi_p0_x, vi_p0_y, vi_p0_z) can be read in RAM_point_x, RAM_point_y, and RAM_point_z. It is worth noting that the present 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 the corresponding target point, i.e., a local search, which greatly reduces the nearest neighbor search time. However, the voxel containing the current source point may not contain a target point, which will cause the nearest neighbor search for some source points to become a global search. Therefore, the present invention performs an expansion operation on the voxelized target point cloud, so that voxels without target points in the voxel space can be associated with the nearest voxel containing the target point.

[0040] In three-dimensional space, a voxel block is a cube, and the six directions refer to 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 group point_reg, selectors, and comparators. Then, the counter controls the expansion in each of the six directions, completing one expansion direction per clock cycle.

[0041] The circuit diagram of the present invention for realizing the expansion of voxels in the positive z direction is as follows: 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 voxel a and voxel a+1 are both non-empty voxels, then there is no need to expand voxel a; 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. By default, the number of points in a point cloud does not exceed 2^16. During expansion, it is necessary to determine whether point_reg[a] is less than 2^16. If it is less than 2^16, it means that voxel a is not expanded from other voxels. The value of point_reg[a] is the number of points within voxel a that contain the target point. In this case, the index a of voxel a is added with 2^16 and then assigned to point_reg[a+1]. If it is greater than or equal to 2^16, it means that voxel a is expanded from other voxels. The value of point_reg[a] is the sum of the index of the other voxels and 2^16. In this case, the value of point_reg[a] is directly assigned to point_reg[a+1]. Expansion in other directions is similar to the expansion in the positive z direction. It is worth noting that voxels at the boundary of the entire 3D voxel space do not need to be expanded in specific directions. For example, for voxel 7, voxel 8 is not in the positive z direction of voxel 7, but in the positive y direction of voxel 0. There are no other voxels in the positive z direction of voxel 7, so it does not need to be expanded in the positive z direction.

[0042] Step 4: After the expansion of the target point cloud is completed, the nearest neighbor search of the source point cloud and the target point cloud is performed in the voxel space. Specifically, it includes:

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

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

[0045] In order to realize the nearest neighbor search in the voxel, the application calculates the nearest neighbor target point of one source point in the voxel by the basic processing element PE. For the source point 1, when the PE1 trigger signal is valid, the source point is input into the PE1, at the same time, all the target points in the whole voxel are input into the PE1 in sequence, one target point is input every clock cycle, the PE1 will calculate the square of the Euclidean distance of the current target point and the source point, compare the obtained value with the minimum Euclidean distance square value before, if the value at this time is smaller, then save the value and the index of the target point at this time in the register, otherwise, the value of the corresponding register remains unchanged, until the output valid signal of the PE1 is valid, the search is ended. In order to make full use of the parallelism of the FPGA, 200 PEs are used, the nearest neighbor search of 200 source points can be carried out at the same time, and the speed of the nearest neighbor search is further improved.

[0046] Step 4-3: multiply the source point coordinates (in the form of a 3x1 vector) and the transpose of the matched target point coordinates (in the form of a 1x3 vector) to obtain a plurality of 3x3 matrices, add the 3x3 matrices, and then normalize the obtained matrix to obtain a 3x3 matrix H.

[0047] Step 5: take the matrix H obtained in step 4 as the input of the singular value decomposition module, and finally output the rotation parameter R and the translation parameter T, and transform the source point cloud through the rotation parameter R and the translation parameter T. The time consumed by the singular value decomposition only accounts for a small part of the whole iterative closest point algorithm, therefore, the application only uses the conventional Jacobian matrix method to realize the singular value decomposition of the matrix, which specifically includes:

[0048] Step 5-1: calculate the left rotation angle α and the right rotation angle β according to the matrix elements of the matrix H and the arctangent function.

[0049] Step 5-2: calculate the left Jacobian matrix J L and the right Jacobian matrix J R .

[0050] The arctangent calculator and the sine and cosine wave generator required for calculating the Jacobian matrix are realized on the hardware through the classic Cordic algorithm. The hardware architecture diagram of the Cordic algorithm is as shown in Figure 5 , only shift registers, adders (subtractors) and lookup tables are used, and the core idea is to gradually approach the rotation angle θ through iteration, and the tangent value of the rotation angle is stipulated to be the positive integer power of 2 every time, so that the multiplication of the tangent value in the iteration formula can be converted into the shift operation in the hardware, that is, realized by using the shift register; in addition, the angles with the tangent value of the integer power of 2 are calculated in advance in the software, and input into the lookup table, which is convenient for subsequent iteration.

[0051] Step 5-3: multiply the transpose of the left Jacobian matrix on the left of the matrix H and multiply the right Jacobian matrix on the right of the matrix H 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 non-diagonal elements of the obtained matrix are close to 0.

[0053] The formula for iteration is as follows:

[0054]

[0055] In the formula, H k+1 represents the matrix after (k+1) times of Jacobian rotation, represents the transpose of the left Jacobian matrix of the matrix H k , and represents the right Jacobian matrix of the matrix H k .

[0056] The calculation formula of the matrix U decomposed by singular value is as follows:

[0057]

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

[0059] The calculation formula of the matrix V decomposed by singular value is as follows:

[0060]

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

[0062] The present application adopts 16-bit fixed-point decimal representation method, and through measurement and calculation, k is 4, that is, iteration is 5 times, and the accuracy of the output result can meet the requirements of point cloud registration.

[0063] Step 5-5: calculate the rotation parameter R and the translation parameter T through the matrix U and the matrix V obtained in step 4-4.

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

[0065] R = VUT

[0066] T = pt-Rps

[0067] In the formula, since U and V are both 3x3 square matrices, R is also a 3x3 square matrix; pt and ps are the center of mass of the target point cloud and the center of mass of the source point cloud respectively, and are both 3x1 vectors, so 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] Among them, the specific transformation formula of the source point cloud is as follows:

[0070] S'=R*S+T

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

[0072] Step 6: If the iterative closest point algorithm reaches the pre-set maximum number of iterations, the iteration is terminated and the system ends; 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 is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as 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 MATLAB to 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: Perform the nearest neighbor search of the source point cloud and the target point cloud in parallel in the voxel space through multiple processing units 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. 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, specifically including: 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 the point coordinates in RAM_point in voxel order. The storage in voxel order also meets the following requirements:

1. Store in ascending order of voxel block index; 2. Points belonging to the same voxel block are stored adjacently. The step 3 specifically includes: finding the root voxel, controlling the voxel to dilate in six directions in sequence 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; if the direction is a non-empty voxel, no index needs to be passed; after the dilation in that direction, the values ​​of the registers corresponding to some empty voxels become non-zero, which are called dilated voxels, and all subsequent root voxels and dilated voxels are dilated in other directions until the values ​​of the registers corresponding to the voxel blocks in the entire space are all non-zero, 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; The step 5 specifically includes: Step 5-1: Calculate the left rotation angle α and the right rotation angle β according to the matrix elements of the matrix H and the inverse tangent function; 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 ; Step 5-3: Multiply the left side of the matrix H by the transpose of the left Jacobian matrix, and multiply the right side of the matrix H by the right Jacobian matrix to obtain the new matrix H1; Step 5-4: Replace H with H1 and repeat steps 5-1, 5-2, and 5-3 until the off-diagonal elements of the resulting matrix are close to 0; The formula for iteration is as follows: Where, H k+1 Indicates that ( k +1) Jacobian rotation matrix, Representation matrix H k The transpose of the left Jacobian matrix of , Representation matrix H k The right Jacobian matrix of ; The calculation formula of the matrix U obtained by singular value decomposition is: Where, Represents the left Jacobian matrix of the matrix H after the i-th Jacobian rotation; The calculation formula of the matrix V obtained by singular value decomposition is: Where, Represents the right Jacobian matrix of the matrix H after the i-th Jacobian rotation; Step 5-5: Calculate the rotation parameter R and translation parameter T by obtaining the matrix U and matrix V through step 5-4; The expressions for R and T are as follows: R = VU T T = pt – Rps pt and ps are the centroid of the target point cloud and the source point cloud respectively; Step 5-6: Transform the source point cloud using the rotation parameter R and translation parameter T obtained in step 5-5; Among them, the specific transformation formula of the source point cloud is as follows: S'= R*S + T S is the point coordinate of the source point in the original source point cloud; S' is the point coordinate after transformation.

2. The acceleration method according to claim 1, characterized in that: The step 4 specifically includes: first, obtaining the index of the voxel containing the source point through counter control, and then using several processing units to perform a nearest neighbor search for several source points in the voxel in parallel according to the index order from small to large, to find the nearest neighbor target point that matches the source point.

3. The acceleration method according to claim 1 or 2, 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 voxel blocks is obtained. At this time, each register in the register group stores the number of points inside a voxel.

Images