CN119850434A

CN119850434A - Acceleration method of iterative nearest point algorithm based on FPGA

Info

Publication number: CN119850434A
Application number: CN202411958580.5A
Authority: CN
Inventors: 王启晶; 李谦; 常穹; 李昀; 施毅
Original assignee: Nanjing University
Current assignee: Nanjing University
Priority date: 2024-12-30
Filing date: 2024-12-30
Publication date: 2025-04-18
Anticipated expiration: 2044-12-30
Also published as: CN119850434B

Abstract

The present invention discloses an acceleration method for an iterative closest point algorithm based on FPGA, which voxelizes the three-dimensional space. After the voxelization is completed, an expansion strategy is used for the target point cloud to convert the nearest neighbor search in the iterative closest point algorithm from a global search to a local search, and multiple computing units are used to simultaneously calculate the nearest neighbor target points of multiple source points, which fully utilizes the parallelism of the FPGA, thereby improving the speed of the nearest neighbor search and achieving the effect of accelerating the iterative closest point algorithm. In addition, based on the expansion strategy, the present invention uses a pre-traversal method to allocate memory, which greatly reduces the use of memory, so that the method can complete point cloud registration using only on-chip memory, thereby completing the low-power implementation of the algorithm.

Description

Acceleration method of iterative nearest point algorithm based on FPGA

Technical Field

The invention relates to a computer vision processing method, in particular to an iterative nearest point algorithm acceleration method for point cloud registration.

Background

The point cloud registration is a key link in the three-dimensional reconstruction process, and the more comprehensive information of the three-dimensional model is finally obtained by aligning the point cloud data under different view angles. The iterative closest point (ICP, ITERATIVE CLOSEST POINT) algorithm is the dominant algorithm in the current point cloud registration field. The algorithm realizes the registration of two point clouds in a multi-iteration mode. 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 the corresponding points, corresponding rotation parameters and translation parameters can be calculated, and the source point cloud is transformed through the two parameters so as to be aligned with the target point cloud gradually. This process is iterated until a predetermined iteration termination condition is reached.

The iterative nearest point algorithm is widely applied to the fields of automatic driving positioning, medical imaging, building design and the like, and because the nearest neighbor search needs to calculate Euclidean distances between a source point and all target points, a great amount of time is consumed, the current method for accelerating the iterative nearest point algorithm either needs to occupy a great amount of memory to cause higher power consumption, or the running speed of a system for realizing the algorithm is still to be improved, and the requirement of application on real-time processing of point cloud data is difficult to meet.

Disclosure of Invention

Aiming at the prior art, the invention provides an acceleration method of an iteration closest point algorithm based on an FPGA, which solves the problems of high power consumption, more times of iteration and long time consumption of the existing acceleration method of the iteration closest point algorithm.

The technical scheme is that the method for accelerating the iterative closest point algorithm based on the FPGA comprises the following steps:

the method comprises the steps of 1, carrying out fixed-point implementation on an iterative nearest point algorithm on matlab, and further obtaining a data bit width required by implementation of the algorithm on an FPGA;

step 2, voxelizing the source point cloud and the target point cloud, obtaining the points in each voxel in a traversing way in advance, and distributing memory resources for the whole voxel space according to the points so as to reduce the use of the memory space;

Step 3, expanding the voxelized target point cloud to enable indexes of root voxels with target points inside to be filled into empty voxels without target points inside after being coded, and further converting global nearest neighbor searching into local nearest neighbor searching;

Step 4, realizing nearest neighbor search of a source point cloud and a target point cloud in parallel in a voxel space through a plurality of processing units to obtain a matrix H;

step 5, singular value decomposition is carried out on the matrix H to obtain rotation parameters and translation parameters, and the source point cloud is transformed through the rotation parameters and the translation parameters;

And 6, judging that if the iteration closest point algorithm reaches the maximum iteration times, ending the iteration, otherwise, carrying out voxelization on the transformed source point cloud again, and repeatedly executing the step 4 and the step 5 until the maximum iteration times are reached.

Further, in the step 2, the number of points in each voxel is obtained by a traversing manner in advance, and memory resources are allocated to the whole voxel space according to the number of points, which specifically includes:

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;

the method comprises the steps of creating a RAM_voxel, and storing addresses of point coordinates in the RAM_point according to a voxel sequence to facilitate the follow-up nearest search in the voxel blocks, wherein the storage according to the voxel sequence is simultaneously satisfied, the storage according to the voxel block index sequence from small to large is firstly performed, and secondly, the storage positions of points belonging to the same voxel block are adjacent.

Further, the step 3 specifically includes finding out a root voxel, sequentially performing expansion in six directions through a counter control voxel, and completing expansion in one direction every clock period, wherein for any direction, performing expansion operation on the root voxel in the direction includes encoding indexes of all root voxels and then transmitting the indexes to null voxels in the direction, if the direction is a non-null voxel, no indexes need to be transmitted, when the values of registers corresponding to the null voxels become non-0 values after expansion in the direction, the values of registers corresponding to the null voxels are called expansion voxels, all the root voxels and the expansion voxels in the subsequent directions are performed until the values of registers corresponding to voxel blocks in the whole space are all non-0 values, the expansion operation is completed, the source point cloud after the voxelization does not need to perform expansion operation, and after the expansion operation, any voxel containing a source point can find adjacent voxels containing a target point, so that the nearest neighbor search space is converted from the whole target point cloud to the voxel block.

Further, the step 4 specifically includes obtaining indexes of voxels with source points inside through counter control, and then using a plurality of processing units to search nearest neighbor of a plurality of source points in the voxels in parallel according to the index sequence from small to large to find nearest neighbor target points matched with the source points.

Further, in the step 2, a counter is used to control the reading of the coordinates of the points, 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 beating operation, after the index is obtained, the corresponding register of the voxel in the register group point_reg is selected through a multiplexer, the value of the register is added by one, after all the coordinates of the points are read out, the points in all the voxel blocks are obtained, and at this time, each register in the register group stores the points in one voxel.

The method has the advantages that the three-dimensional space is voxelized, after the voxelization is finished, the nearest neighbor search in the iterative nearest point algorithm is converted from global search to local search by adopting the expansion strategy on the target point cloud, a plurality of computing units are used for simultaneously computing the nearest neighbor target points of a plurality of source points, the parallelism of the FPGA is fully utilized, the speed of the nearest neighbor search is further improved, and the effect of accelerating the iterative nearest point algorithm is realized. In addition, the memory is allocated by using a method of traversing in advance on the basis of the expansion strategy, so that the use of the memory is greatly reduced, the method can finish point cloud registration by using the on-chip memory, and further, the low-power-consumption implementation of the algorithm is finished.

Drawings

FIG. 1 is a flow chart of the method of the present invention;

FIG. 2 is a diagram illustrating memory allocation according to the present invention;

FIG. 3 is a circuit diagram corresponding to the expansion strategy used in the present invention;

FIG. 4 is a schematic diagram of the present invention implementing parallel nearest neighbor searching;

FIG. 5 is a hardware architecture diagram of a Cordic algorithm for implementing an arctangent calculator and a sine and cosine wave generator according to the present invention;

Fig. 6 is a comparison graph of the present invention registering a point cloud of a stent rabbit, wherein (a) is an original image and (b) is an image registered by the present invention.

Detailed Description

The invention is further explained below with reference to the drawings.

As shown in fig. 1, an acceleration method of an iterative closest point algorithm based on an FPGA includes:

Step 1, firstly floating point implementation is carried out on an iterative nearest point algorithm on matlab, then fixed point implementation is carried out on the algorithm, and further the data bit width required by the algorithm for realizing on an FPGA is obtained, and the method specifically comprises the following sub-steps:

And 1-1, writing codes on matlab according to the principle of iterative nearest point algorithm, and floating the algorithm.

Step 1-2, based on the implementation of floating point of the iterative closest point algorithm, floating point number is multiplied by the power of 2 to form a whole downwards, floating point number calculation can be converted into integer calculation, the fixed point of the algorithm is realized, and the data bit width N meeting the precision requirement is recorded, wherein N is a positive integer.

The data bit width N is obtained through debugging, and is made as small as possible under the condition of meeting the requirement of iterative nearest point algorithm precision.

And step 1-3, when an iterative closest point algorithm is realized on the FPGA, adopting the data bit width N determined in the step 1-2.

In the process of realizing the iterative nearest point algorithm on the FPGA, the data bit width is needed to be used for storing and calculating the coordinates of the points in the point cloud. For the storage of point coordinates, floating point coordinates need to be rounded down after being multiplied by the power of 2 to the power of N, and then stored into RAM. For the addition, subtraction, multiplication and division operation related to the coordinates of the point, the final operation result needs to keep the data bit width of N bits. In this embodiment, n=16.

Step 2, storing a standard Stanford rabbit point cloud data set (point coordinates) into a BRAM, and then voxelizing the point cloud, wherein the method specifically comprises the following substeps:

and 2-1, reading the point cloud data into the RAM in a text reading mode, and storing coordinates of three dimensions into different RAMs to facilitate subsequent parallel reading of the data.

The method comprises the steps of 2-2 dividing the number of voxel blocks according to the resources owned by an FPGA, and voxelizing the obtained point cloud data, wherein a three-dimensional voxel space is defined, the three-dimensional voxel space comprises 512 voxels, the side length of each voxel is 1, the input point cloud coordinate range is (-1, 1), the coordinate range is (0, 1) through normalization processing, then the point cloud coordinates are multiplied by 8, the coordinate range is (0, 8), and therefore each point in the point cloud can be mapped into the corresponding voxel.

And 2-3, obtaining the number of points in each voxel in a traversing way in advance, and distributing the memory RAM_volume of the voxel space according to the number of points so as to reduce the use of the memory.

The memory allocation and data reading method used in the invention is as shown in fig. 2, firstly traversing the coordinates of the whole point cloud to obtain the point number in each voxel, and storing the point number in the register group point_reg. Specifically, a counter is used for controlling the reading of the coordinates of points, 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 beating operation, after the index is obtained, a register of the voxel in a register group point_reg is selected through a multiplexer, the value of the register is added by one, after the coordinates of all points are read out, the points in all voxel blocks are obtained, and at the moment, each register in the register group stores the points in one voxel.

Then, the addresses of coordinates of points arranged in the voxel sequence in the corresponding RAM_point are stored in the RAM_voxel, the head address of the voxel i in the RAM_voxel is v_addr [ i ], and the head address of the voxel (i+1) is determined by the head address of the voxel i and the number of points inside the voxel i. For example, for voxel i, the address of the first point inside in RAM_voxel is v_addr [ i ], and the corresponding data space stores vi_p0_index, through which the coordinates (vi_p0_x, vi_p0_y, vi_p0_z) of point p0 can be read in RAM_point_x, RAM_point_y, and RAM_point_z. Notably, the present invention obtains the address of the first point inside voxel (i+1) in RAM_volume through v_addr [ i ] and point_reg [ i ] (the points inside voxel i are stored).

And 3, after voxelization, the source point in the current voxel only needs to perform nearest neighbor search in the voxel to find a target point corresponding to the source point, namely local search, so that the time of nearest neighbor search is greatly reduced. However, for the voxels of the current active point, there is not necessarily a target point in the voxels, which can make the nearest neighbor search of part of the source points become global search, so the invention performs expansion operation on the voxelized target point cloud, so that the voxels without target points in the voxel space can be associated with the voxels closest to the voxels containing the target points.

In three-dimensional space, a voxel block is a cube, with six directions referring to the top, bottom, left, right, front and back (positive x, negative x, positive y, negative y, positive z, negative z directions) of the cube, respectively. Firstly, a root voxel is found out through a register group point_reg, a selector and a comparator, then expansion in six directions is sequentially carried out through counter control, and expansion in one direction is completed in each clock cycle.

The circuit diagram for realizing the expansion of the voxel in the positive z direction is shown in fig. 3, the quantity of target points in the voxel a is stored in the point_reg [ a ], if the point_reg [ a ] is 0, the point_reg [ a+1] is indicated to be an empty voxel, the expansion is not required, that is, the value of the point_reg [ a+1] is kept unchanged, if the point_reg [ a ] is not 0, and the point_reg [ a+1] is also not 0, the voxel a is not required to be expanded, if the point_reg [ a ] is not 0, and the point_reg [ a+1] is 0, the expansion in the positive z direction of the voxel a, that is, the point_reg [ a+1] can be assigned. The default point cloud has a point number not exceeding 2-16, and if the point number is less than 2-16, it is necessary to determine whether the point_reg [ a ] is less than 2-16, if the point number is less than 2-16, it is indicated that the voxel a is not expanded from other voxels, the value of the point_reg [ a ] is the point number of the target point contained in the voxel a, at this time, the index a of the voxel a is added with 2-16, and then the point_reg [ a+1] is assigned, if the point number is greater than or equal to 2-16, it is indicated that the voxel a is expanded from other voxels, the value of the point_reg [ a ] is the sum of the index of other voxels and 2-16, and at this time, the point_reg [ a ] is directly assigned to the point_reg [ a+1]. The expansion in the other direction is similar to the expansion in the positive z direction. Notably, the voxels at the boundaries of the entire three-dimensional voxel space do not need to be inflated in a particular direction, e.g. 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, the positive z-direction of voxel 7 has no other voxels, and therefore no positive z-direction inflation thereof is required.

And 4, after the target point cloud expansion is completed, performing nearest neighbor search of the source point cloud and the target point cloud in the voxel space. The method specifically comprises the following steps:

and 4-1, controlling and acquiring indexes of voxels of the internal active points through a counter, and sequentially performing nearest neighbor searching.

And 4-2, as shown in fig. 4, performing nearest neighbor search on the source point in the voxel with index 3, and finding a nearest neighbor target point matched with the source point in the voxel.

In order to achieve a nearest neighbor search within a voxel, the present invention calculates the nearest neighbor target point of one source point within the voxel by means of the basic processing unit PE. For the source point 1, when the trigger signal of the PE1 is valid, the source point is input into the PE1, meanwhile, all target points in the whole voxel are sequentially input into the PE1, one target point is input every clock period, the PE1 calculates the square of the Euclidean distance between the current target point and the source point, the obtained numerical value is compared with the minimum Euclidean distance square value before, if the numerical value is smaller at the moment, the numerical value and the index of the target point at the moment are stored in a register, otherwise, the value of the corresponding register is kept unchanged until the output valid signal of the PE1 is valid, and the search is ended. In order to fully utilize the parallelism of the FPGA, 200 PEs are used, and nearest neighbor searching of 200 source points can be simultaneously performed at most, so that the speed of nearest neighbor searching is further improved.

And 4-3, multiplying the source point coordinates (vector with the form of 3x 1) and the transpose of the matched target point coordinates (vector with the form of 1x 3) to obtain a plurality of matrices with the size of 3x3, adding the matrices with the size of 3x3, and normalizing the obtained matrices to obtain a matrix H with the size of 3x 3.

And 5, taking the matrix H obtained in the step 4 as input of a singular value decomposition module, and finally outputting a rotation parameter R and a translation parameter T by the singular value decomposition module, and transforming the source point cloud through the rotation parameter R and the translation parameter T. The singular value decomposition only occupies a small part of the whole iterative nearest point algorithm, so the invention only adopts a conventional jacobian matrix method to realize the singular value decomposition of the matrix, and specifically comprises the following steps:

And 5-1, calculating a left rotation angle alpha and a right rotation angle beta according to matrix elements and an arctangent function of the matrix H.

And 5-2, calculating a left jacobian matrix J ^L and a right jacobian matrix J ^R according to the left and right rotation angles.

The arctangent calculator and the sine and cosine wave generator needed for calculating the jacobian matrix are realized on hardware through a classical Cordic algorithm. The hardware architecture of Cordic algorithm is shown in figure 5, only uses shift register, adding (subtracting) method and lookup table, its core idea is that through iteration successive approximation rotation angle theta, tangent value of angle of every rotation is agreed to be positive power of 2, so that multiplication tangent value in iteration formula can be converted into shift operation in hardware, i.e. implemented by shift register, in addition, angle of tangent value of integer power of 2 is calculated in software in advance, and inputted into lookup table, so that it is convenient for subsequent iteration use.

And 5-3, multiplying the left side of the matrix H by the transpose of the left jacobian matrix, and multiplying the right side of the matrix H by the right jacobian matrix to obtain a new matrix H1.

And 5-4, replacing H with H1, and repeating the steps 4-1, 4-2 and 4-3 until the off-diagonal element of the obtained matrix is close to 0.

The formula for performing the iteration is as follows:

wherein H _k+1 represents a matrix in which (k+1) Jacobian rotations are performed, Representing the transpose of the left jacobian of matrix H _k,Representing the right jacobian of matrix H _k.

The calculation formula of the matrix U decomposed by singular values is as follows:

In the formula, The left jacobian matrix, in which matrix H performs the ith jacobian rotation, is represented.

The calculation formula of the matrix V decomposed by the singular values is as follows:

In the formula, The right jacobian matrix, in which matrix H performs the ith jacobian rotation, is represented.

According to the invention, a 16-bit fixed point small number representation method is adopted, through calculation, k is taken to be 4, namely, iteration is carried out for 5 times, and the precision of an output result can meet the requirement of point cloud registration.

And 5-5, calculating a rotation parameter R and a translation parameter T through the matrix U and the matrix V obtained in the step 4-4.

The expressions of R and T are as follows:

R=VUT

T=pt–Rps

In the formula, since U and V are square matrixes with the size of 3x3, R is also square matrix with the size of 3x3, pt and ps are respectively the mass centers of the target point cloud and the source point cloud and are vectors with the size of 3x1, and T is also a vector with the size of 3x 1.

And 5-6, transforming the source point cloud through the rotation parameter R and the translation parameter T obtained in the step 5-5.

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, the size is a vector of 3x1, and S' is the transformed point coordinate.

And step 6, judging that if the iteration closest point algorithm reaches the preset maximum iteration times, ending the iteration and ending the system operation, otherwise, carrying out voxelization on the transformed source point cloud again, and repeatedly executing the step 4 and the step 5 until the maximum iteration times are reached.

The foregoing is merely a preferred embodiment of the present invention and it should be noted that modifications and adaptations to those skilled in the art may be made without departing from the principles of the present invention, which are intended to be comprehended within the scope of the present invention.

Claims

1. An acceleration method of an iterative closest point algorithm based on an FPGA is characterized by comprising the following steps:

Step 2, voxelizing the source point cloud and the target point cloud, obtaining the points in each voxel in a traversing mode in advance, and distributing memory resources for the whole voxel space according to the points;

2. The acceleration method according to claim 1, wherein in the step 2, the number of points in each voxel is obtained by a traversal method in advance, and memory resources are allocated to the whole voxel space according to the number of points, specifically including:

The method comprises the steps of creating a RAM_voxel, and storing addresses of point coordinates in the RAM_point according to the voxel sequence, wherein the storage according to the voxel sequence is simultaneously satisfied, the storage according to the voxel block index sequence from small to large is firstly performed, and the storage positions of points belonging to the same voxel block are adjacent.

3. The acceleration method according to claim 2, wherein the step 3 specifically includes finding out a root voxel, sequentially performing expansion in six directions by a counter control voxel, completing expansion in one direction every clock cycle, wherein for any direction, performing expansion operation on the root voxel includes encoding indexes of all root voxels and transmitting the encoded indexes to null voxels in the direction, if the direction is a non-null voxel, no index is required to be transmitted, when the direction is expanded, a value of a register corresponding to a partial null voxel becomes a non-0 value, called an expansion voxel, performing expansion operation on all root voxels and expansion voxels in other directions until a value of a register corresponding to a voxel block in the whole space is a non-0 value, performing expansion operation on a source point cloud after the voxelization, and after the expansion operation, any voxel containing a source point can find an adjacent voxel containing a target point, thereby converting a search space of the nearest neighbor voxel from the whole cloud to a voxel block.

4. An acceleration method according to claim 3, characterized in that step 4 comprises, firstly, obtaining the index of the voxels with source points inside by means of counter control, then, using several processing units to search for nearest neighbor target points matching with the source points in parallel with several source points in the voxels in order of index from small to large.

5. The acceleration method of any one of claims 2-4, wherein in the step 2, a counter is used to control the reading of coordinates of points, 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 the index is obtained, a register corresponding to the voxel in the register group point_reg is selected through a multiplexer, the value is added by one, after all the coordinates of the points are read out, the number of points inside all the voxel blocks is obtained, and at this time, each register in the register group stores the number of points inside one voxel.