JP2020086941A - Position deviation correction device and program - Google Patents

Abstract

To suppress an error mapping in an initial step in a positioning and increase accuracy in the positioning.SOLUTION: In the case where each data point included in a data point group is corresponded to a model point included in a model point group, and a plurality of data points included in the data point group is corresponded to the same model point on the basis of a coordinate of the data point and the coordinate of the model point until it is determined that a determination part 160 satisfies a prescribed condition, a prescribed weight is set to each of the correspondence of the same model point and the plurality of data points. In the case of converting the coordinate of the data point included in the data point group on the basis of the correspondence with the data point group and the model point group and the prescribed weight to be set, a coordinate conversion matrix in which a position deviation of the data point group and the model point group after the conversion becomes the minimum is calculated, and the conversion of the coordinate of the data point included in the data point group is repeated on the basis of the coordinate conversion matrix.SELECTED DRAWING: Figure 1

Description

The present invention relates to a positional deviation correction device and a program.

Conventionally, white line information and the like detected by a vehicle positioning device using a GNSS (Global Positioning Satellite System, such as GPS) and an external sensor (camera or LIDAR) mounted on the vehicle is collected as traveling data to generate a map of the traveling environment. Has been done.

When a plurality of pieces of travel data are aggregated to improve the accuracy of the map and expand the generation range, the travel locus is displaced due to GPS positioning errors, etc., so it is necessary to correct the relative displacement of different travel data. is there.

In order to correct the relative displacement, the acquired 3D point group P is superposed on the point group Q modeling the 3D environment to determine the position/orientation of the object and the observer's position/orientation. An ICP (Iterative Closest Point) method (Non-Patent Document 1) is often used in the estimating technique (Patent Documents 1 and 2).

In the ICP method, (1) the points included in the point group P and the point group Q are associated with each other, and (2) the coordinate transformation matrix (rotation, translation) is repeatedly calculated so that the distance between the corresponding points becomes small, and the optimum point is obtained. This is a method of estimating the coordinate transformation matrix (FIG. 12).

Although the ICP method is guaranteed to have a convergent property, it may be possible to fall into a local solution if an incorrect correspondence is made. Therefore, it is important to make a correct correspondence.

In Patent Document 1, with respect to each point of the point group P, a point having the smallest distance in the point group Q is searched and associated. At this time, if the distance is larger than a specified value, it is invalidated as a wrong correspondence.

Further, there is also used a method of making correspondence in the opposite direction and validating only when they match (patent document 3 and non-patent document 2).

In general, the point cloud Q is obtained densely, and it can be expected that correct association is gradually performed by iterative processing by the ICP method. As described above, when the point group is dense, the correspondence to the initial erroneous points is gradually eliminated by the iterative processing by the ICP method, and in many cases, the positioning is performed with a small error due to the correct correspondence.

"Iterative closest point", [online], 2017, arXiv:1706.00227, [Search on October 5, 2018], Internet <https://en.wikipedia.org/wiki/Iterative_closest_point>. Congcong Jin, Jihua Zhu, Yaochen Li, Shaoyi Du, Zhongyu Li, Huimin Lu, "An Effective Approach for Point Clouds Registration Based on the Hard and Soft Assignments", [online], 2017, arXiv:1706.00227, [2018 10 5th month search], Internet <https://arxiv.org/abs/1706.00227>.

JP, 2013-072857, A JP, 2008-250905, A JP, 2011-209116, A

However, when the point cloud has noise or omissions, there is a problem that the initial incorrect correspondence cannot be resolved, and the possibility of convergence to a registration result with a large error due to the incorrect correspondence increases.

Further, when the point group Q is sparse, there is a problem that the erroneous correspondence in the initial state is not resolved and the possibility of falling into a local solution becomes high (FIG. 13).

In addition, the method of Patent Document 3 has a problem that erroneous correspondence may remain depending on how the point group P and the point group Q are displaced or arranged.

The present invention has been made to solve the above problems, and provides a misalignment correction device and a program capable of suppressing incorrect association in the initial stage of alignment and improving alignment accuracy. The purpose is to

In order to achieve the above-mentioned object, the positional deviation correction device of the present invention includes an input unit that receives an input of a data point group that is a set of coordinates of data points to be aligned, and a model that serves as a reference for alignment. An acquisition unit that acquires a model point group that is a set of point coordinates, and based on the coordinates of the data points and the coordinates of the model points, for each of the data points included in the data point group, in the model point group. When a plurality of data points included in the data point group are associated with the same model point as the model point corresponding unit that associates with the included model point, the same model point and the A weight setting unit that sets a predetermined weight for each correspondence with each of the plurality of data points, a correspondence between the data point group and the model point group, and the set predetermined weight. Based on the above, when the coordinates of the data points included in the data point group are converted, a conversion matrix calculation for calculating a coordinate conversion matrix that minimizes the positional deviation between the converted data point group and the model point group. Unit, based on the coordinate transformation matrix, a coordinate transformation unit that transforms the coordinates of the data points included in the data point group, and the transformation by the coordinate transformation unit until it is determined that a predetermined condition is satisfied, The model point correspondence unit includes a determination unit that repeats the association by the model point correspondence unit, the setting by the weight setting unit, and the calculation by the conversion matrix calculation unit.

In addition, the program of the present invention causes a computer to input an input of a data point group, which is a set of coordinates of data points to be aligned, and a model which is a set of coordinates of model points to be a reference for alignment. Based on the acquisition unit that acquires a point group, the coordinates of the data points, and the coordinates of the model points, each of the data points included in the data point group is associated with the model point included in the model point group. Correspondence between the same model point and each of the plurality of data points when a plurality of data points included in the data point group are associated with the same model point A weight setting unit that sets a predetermined weight for each of the data points, the correspondence between the data point group and the model point group, and based on the set predetermined weight, the data points included in the data point group. When the coordinates of the data points are converted, a conversion matrix calculation unit that calculates a coordinate conversion matrix that minimizes the positional deviation between the converted data point group and the model point group, based on the coordinate conversion matrix, A coordinate conversion unit that converts the coordinates of the data points included in the data point group, and conversion by the coordinate conversion unit, association by the model point correspondence unit, and setting of the weight until it is determined that a predetermined condition is satisfied. It is a program for functioning as a determination unit that repeats the setting by the unit and the calculation by the conversion matrix calculation unit.

According to the state prediction apparatus and the program of the present invention, the input unit receives an input of a data point group that is a set of coordinates of data points to be aligned, and the acquisition unit is a model point to be a reference for alignment. A model point group, which is a set of coordinates, and the model point corresponding unit, based on the coordinates of the data points and the coordinates of the model points, for each of the data points included in the data point group, Correlate with the model points included in the group.

When a plurality of data points included in the data point group are associated with the same model point, the weight setting unit associates the same model point with each of the plurality of data points. A predetermined weight is set for each of them, and the conversion matrix calculation unit includes the data point group based on the correspondence between the data point group and the model point group and the set predetermined weight in the data point group. When the coordinates of the data points to be converted are converted, a coordinate conversion matrix that minimizes the positional deviation between the converted data point group and the model point group is calculated, and the coordinate conversion unit calculates the coordinate conversion matrix based on the coordinate conversion matrix. The coordinates of the data points included in the data point group are converted, and the determination unit performs conversion by the coordinate conversion unit and associates the model point correspondence unit until it is determined that a predetermined condition is satisfied, The setting by the weight setting unit and the calculation by the conversion matrix calculation unit are repeated.

As described above, until it is determined that the predetermined condition is satisfied, the model points included in the model point group and the model points included in the data point group are determined based on the coordinates of the data points and the coordinates of the model points. Of the same model point, when a plurality of data points included in the data point group are associated, for each of the correspondence of the same model point and each of the plurality of data points When a predetermined weight is set, the data point group and the model point group are associated with each other, and the coordinates of the data point included in the data point group are converted based on the set predetermined weight. The coordinate conversion matrix that minimizes the positional deviation between the data point group and the model point group is calculated, and the coordinates of the data points included in the data point group are converted based on the coordinate conversion matrix, thereby performing the alignment. It is possible to suppress erroneous correspondence in the initial stage and improve the accuracy of alignment.

Further, the conversion matrix calculation unit of the misalignment correction device of the present invention is configured such that a distance between a coordinate after conversion of the associated data point and a coordinate of the model point, and correspondence between the data point and the model point. The coordinate transformation matrix can be calculated so that the sum of squares of the weighted distances, which is obtained by using the weights for weighting, is minimized.

Further, the weight setting unit of the misalignment correction apparatus of the present invention, when only one of the data points is associated with one of the model points, the model point and the data point are associated with each other. A fixed weight is set for the same model point, and when a plurality of data points are associated with each other, for each of the association of the same model point and each of the plurality of data points. Thus, a weight lower than the fixed weight can be set.

Further, the misalignment correction device of the present invention, based on the coordinates of the model points and the coordinates of the data points, with respect to each of the model points included in the model point group, with the data points included in the data point group. The weight setting unit further includes a corresponding data point correspondence unit, and the plurality of model points included in the model point group are associated with the same data point, the same data point and the plurality of data points. A predetermined weight is set for each of the correspondence with each of the model points, and the determination unit performs the conversion by the coordinate conversion unit and the model point correspondence unit until it is determined that a predetermined condition is satisfied. It is possible to repeat the associating, the data point associating unit, the setting by the weight setting unit, and the calculation by the conversion matrix calculating unit.

Further, the positional deviation correction device unit of the present invention corresponds to the case where the distance between the coordinates of the data points and the coordinates of the model points associated by the model point association unit is equal to or greater than a first threshold value set in advance. A first effective distance determination unit that invalidates the attachment may be further included.

Further, the positional deviation correction device unit of the present invention corresponds to the case where the distance between the coordinates of the model point and the coordinates of the data point associated by the data point correspondence unit is equal to or greater than a second threshold value set in advance. A second effective distance determination unit that invalidates the attachment may be further included.

Further, when the weight setting unit of the positional deviation correcting device of the present invention is configured such that the positional deviation between the associated data points of the data point group and the model points of the model point group is equal to or less than a predetermined third threshold value. Alternatively, when the number of repetitions by the determination unit is equal to or larger than a predetermined fourth threshold value, the weight setting method can be changed.

As described above, according to the misalignment correction apparatus and the program of the present invention, it is possible to suppress an incorrect association in the initial stage of alignment and improve the alignment accuracy.

It is a block diagram which shows the schematic structure in the 1st Embodiment of this invention. It is an image figure which shows the example of matching of a point cloud in the 1st Embodiment of this invention. 6 is a flowchart showing the contents of a positional deviation correction processing routine according to the first embodiment of the present invention. It is a flowchart which shows the content of the matching processing routine of the point cloud in the 1st Embodiment of this invention. It is a block diagram which shows the schematic structure in the 2nd Embodiment of this invention. It is an image figure which shows the example of the position alignment of the three-dimensional point group in the 2nd Embodiment of this invention. It is a flowchart which shows the content of the point cloud matching process routine in the 2nd Embodiment of this invention. It is a block diagram which shows the schematic structure in the 3rd Embodiment of this invention. It is a flowchart which shows the content of the matching process routine of the point cloud in the 3rd Embodiment of this invention. It is a block diagram which shows schematic structure in the 4th Embodiment of this invention. It is a flowchart which shows the content of the matching processing routine of the point cloud in the 4th Embodiment of this invention. It is an image figure which shows the example of the position alignment of the conventional three-dimensional point group. It is an image figure which shows the example which the incorrect correspondence by the position alignment of the conventional three-dimensional point group remains. It is an image figure which shows the basic composition of the alignment of a three-dimensional point group. It is an image figure which shows the outline|summary of the alignment of the three-dimensional point cloud in embodiment of this invention.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

<Outline of Embodiment of the Present Invention>
In the conventional alignment between point groups, the figure represented by the point sequence of the three-dimensional data point group P (FIG. 12A) is superimposed on the figure represented by the point sequence of the model point group Q (see FIG. 12(b)). This figure is an arbitrary figure such as a line figure, a plane, or a solid.

Coordinate conversion is performed between the data point group P and the model point group Q such that the distance between the coordinates of the corresponding data point p _{i and} the coordinate of the model point q _j is minimized. For example, the point groups are associated using the ICP method or the like. The data point group P is assumed to be in the same coordinate system as the model point group Q.

The basic structure of the ICP method is shown in FIG. In the search and association of the nearest neighbor point performed by the basic configuration of the ICP method, incorrect association may be performed.

Further, FIG. 14B shows a configuration based on the association in the opposite direction as in Non-Patent Document 2. Even in this case, if the positional deviation between the data point group P and the model point group Q is large in the initial stage of the iterative processing of the ICP method, an erroneous correspondence is likely to occur depending on the arrangement of the point groups.

The embodiment of the present invention relates to the alignment of the point cloud having such noise or omission.

In the present embodiment, in order to suppress the occurrence of erroneous correspondence, when the correspondence between the data point group P and the model point group Q is ambiguous, the correspondence is invalidated, or the correspondence is small. A weight is set (FIG. 15C). As a criterion for ambiguity, in addition to the distance between the model point and the data point as in the prior art, duplicate correspondence (correspondence from a plurality of data points to the same model point) Etc.) is used.

In the application of the conventional ICP method, it is assumed that the model point group Q is a state in which noise and the like are removed. However, in the present embodiment, the model point group Q serving as a reference for alignment also includes noise and omissions. Even in such a case, it is possible to suppress alignment due to incorrect association (FIG. 15D).

<Configuration of misregistration correction device 10 according to the first embodiment of the present invention>
A positional deviation correction device 10 according to an embodiment of the present invention will be described with reference to FIG.

The positional deviation correction device 10 includes a CPU, a RAM, and a ROM that stores a program for executing a positional deviation correction processing routine described later, and is functionally configured as shown below.

As shown in FIG. 1, the misalignment correction apparatus 10 according to the embodiment of the present invention includes an input unit 100, a model point group storage unit 110, an acquisition unit 120, a model point correspondence unit 130, and a weight setting unit. 140, a conversion matrix calculation unit 150, a determination unit 160, a coordinate conversion unit 170, and an output unit 180.

The input unit 100 receives an input of a data point group P that is a set of coordinates of data points p _i to be aligned. The data point group P includes n data points p _i {i=1,..., N}. The data point p _i is a three-dimensional point, and the coordinates of the data point p _i are represented by (x _i , y _i , z _i ). The data point group P input to the input unit 100 has the same coordinate system as the model point group Q described later, or has been converted into the same coordinate system in advance.

Then, the input unit 100 passes the data point group P to the acquisition unit 120.

The model point group storage unit 110 stores a model point group Q, which is a set of coordinates of model points q _{j serving} as a reference for alignment. The model point group Q includes m model points q _j {j=1,..., M}. The model point q _j is a three-dimensional point, and the coordinates of the model point q _j are represented by (x _j , y _j , z _j ).

The acquisition unit 120 acquires a model point group Q, which is a set of coordinates of model points q _{j serving} as a reference for alignment.

Then, the acquisition unit 120 passes the data point group P and the model point group Q to the model point corresponding unit 130.

Model points corresponding unit 130, based on the coordinates of the coordinates and the model point q _j of the data points p _i, for each of the data points p _i included in the data points P, the model points q _j in the model point group Q Corresponds to.

Specifically, the model point correspondence unit 130 searches the model point group Q for a model point q _j that is the closest point for each of the data points p _i included in the data point group P (the following formula (1)). ..

Then, for each of the data points p _i included in the data point group P, the model point correspondence unit 130 associates the corresponding data point p _i with the model point q _j that is the closest point to the data point p _i (p. _i , q _j ) is passed to the weight setting unit 140.

Weight setting unit 140, for the same model points q _j, when a plurality of data points p _i included in the data points P are associated, same model points q _j and the plurality of data points p _i A predetermined weight w _i is set for each association (p _i , q _j ) with each

Specifically, the weight setting unit 140 first sets, for each data point p _i included in the data point group P, the data point p _i and the model point q _j that is the closest point to the data point p _i . Predetermined weight w _i =W1 is given to the association (p _i , q _j ) to obtain a weighted association (p _i , q _j , w _i ). Here, W1 is a predetermined constant.

Next, the weight setting unit 140 determines whether or not a plurality of data points p _i are associated with the same model point q _j .

When a plurality of data points p _i are associated with the same model point q _j , the weight setting unit 140 causes the weighted association (p _i , q _j , w _i ) including the model point q _j. With respect to, the weight w _i =W2. W2 is a predetermined constant and has a lower weight than W1.

Then, the weight setting unit 140 passes the weighted association (p _i , q _j , w _i ) for each of the data points p _i included in the data point group P to the conversion matrix calculation unit 150.

The conversion matrix calculation unit 150 converts the coordinates of the data points p _i included in the data point group P based on the correspondence between the data point group P and the model point group Q and the set weight w _i. , The coordinate transformation matrix Ts that minimizes the positional deviation between the transformed data point group and the model point group Q is calculated.

Specifically, the conversion matrix calculation unit 150 first initializes the coordinate conversion matrix T to T=14 in advance. Here, l4 is a 4×4 identity matrix.

Next, the conversion matrix calculation unit 150, based on the weighted correspondence (p _i , q _j , w _i ) for each of the data points p _i included in the data point group P, the coordinates that minimize the positional deviation. The conversion matrix Ts is calculated. Here, s is a counter indicating the number of times of alignment, and the initial value is s=1.

Specifically, the conversion matrix calculation unit 150 calculates the distance between the coordinates of the converted point p _i ′ of the associated data point p _i and the coordinates of the model point q _j , and the data point p _i and the model point q. _The coordinate transformation matrix Ts is calculated so that the sum of squared weighted distances (Equation (2) below), which is obtained using the weight w _i associated with _j , is minimized.

However, the coordinate X′ of the converted point p _i ′ is represented by the following equation (3).

Here, X is a vector using the coordinates (x, y, z) of the data point p _i before conversion,

It is represented by.

As a specific method of calculating the coordinate conversion matrix Ts, for example, the method of Reference Document 1 can be used.
[Reference 1] Olga Sorkine-Hornung, Michael Rabinovich, "Least-Squares Rigid Motion Using SVD", [online], Department of Computer Science, ETH Zurich, January 16, 2017, [Search on October 5, 2018] , Internet <https://igl.ethz.ch/projects/ARAP/svd_rot.pdf>.

The conversion matrix calculation unit 150 updates the coordinate conversion matrix T by the following formula (4) using the calculated coordinate conversion matrix Ts.

Further, the conversion matrix calculation unit 150 adds 1 to s.

Then, the conversion matrix calculation unit 150 determines the weighted association (p _i , q _j , w _i ) for each of the data points p _i included in the data point group P, s, and the coordinate conversion matrix T. It is passed to the section 160.

The determination unit 160 determines whether or not a predetermined condition is satisfied.

The condition is, for example, that the average value of the positional deviations of the weighted correspondences (p _i , q _j , w _i ) for each of the data points p _i included in the data point group P is equal to or less than a predetermined third threshold value. In this case, the counter s is equal to or larger than a predetermined fourth threshold value.

When it is determined that the predetermined condition is not satisfied, the determination unit 160 passes the data point group P and the coordinate conversion matrix T to the coordinate conversion unit 170.

On the other hand, when determining that the predetermined condition is satisfied, the determining unit 160 passes the coordinate conversion matrix T to the output unit 180.

The coordinate conversion unit 170 converts the coordinates of the data points p _i included in the data point group P based on the coordinate conversion matrix T.

Specifically, the coordinate transformation unit 170 transforms the coordinates of the data points p _i included in the data point group P using the following equation (4).

Here, the transformation matrix T is

And the rotation matrix R in the coordinate transformation is

And the translation vector t is

Is.

Then, the coordinate conversion unit 170 passes the coordinate-converted data point group P′ to the model point correspondence unit 130.

Until the determination unit 160 determines that the predetermined condition is satisfied, the model point correspondence unit 130 associates the data point group P′ after the coordinate conversion by the coordinate conversion unit 170 with the setting by the weight setting unit 140. And the calculation by the conversion matrix calculation unit 150 is repeated.

The output unit 180 outputs the coordinate conversion matrix T.

As described above, by assigning the weight w _i , ambiguous association can be removed or the influence of the ambiguous association can be reduced, so that the positional deviation can be reduced.

For example, if the weight w _i is W1=1.0 and W2=0.0, the association (p _i , q _j ) for which the duplication determination is made is processed as invalid.

FIG. 2 shows an example when W1=1.0 and W2=0.0. FIG. 2A shows a case where both the data point group P and the model point group Q are partially missing.

In this case, the correspondence (p _i , q _j ) determined to be duplicated is treated as invalid as shown in FIG. Then, as shown in FIG. 2C, only the effective association (p _i , q _j ) is used for the alignment.

Further, for example, if W1=1.0 and W2=0.1 for the weight w _i , the correspondence (p _i , q _j ) for which the duplication determination is made contributes to the calculation of the above formula (2). Becomes smaller.

Then, as the positional deviation between the point groups becomes smaller, the ambiguity is eliminated, the number of pairs of points that can be associated with each other is increased, and highly accurate alignment becomes possible.

<Operation of the positional deviation correction device 10 according to the first embodiment of the present invention>
Next, with reference to FIG. 3, a positional deviation correction processing routine of the positional deviation correction device 10 of the present embodiment will be described.

First, in step S100, the input unit 100 receives an input of a data point group P, which is a set of coordinates of data points p _i to be aligned.

In step S110, the acquisition unit 120 acquires a model point group Q, which is a set of coordinates of model points q _{j serving} as a reference for alignment.

In step S120, the conversion matrix calculation unit 150 initializes the coordinate conversion matrix T to T=14 in advance.

In step S130, the conversion matrix calculation unit 150 sets s=1. In addition, s is a counter.

In step S140, the model point correspondence unit 130 and the weight setting unit 140 perform point cloud correspondence processing.

In step S150, the conversion matrix calculation unit 150 associates the data point group P with the model point group Q, and based on the set predetermined weight w _i , determines the data points p _i included in the data point group P. When the coordinates are converted, the coordinate conversion matrix Ts that minimizes the positional deviation between the data point group after the coordinate conversion and the model point group Q is calculated, and the coordinate conversion matrix Ts is used to calculate the coordinate conversion matrix T using the above equation. Update by (3).

In step S160, the determination unit 160 determines whether or not a predetermined condition is satisfied.

If the predetermined condition is not satisfied (NO in step S160), the coordinate conversion unit 170 converts the coordinates of the data points p _i included in the data point group P based on the coordinate conversion matrix T in step S170.

In step S180, the conversion matrix calculation unit 150 adds 1 to s and returns to step S140.

If the predetermined condition is satisfied (YES in step S160), the output unit 180 outputs the coordinate conversion matrix T in step S190, and the process ends.

Next, referring to FIG. 4, the point cloud association processing in step S140 will be described.

In step S141, the model point corresponding unit 130 is included in the model point group Q for each of the data points p _i included in the data point group P based on the coordinates of the data points p _{i and} the coordinates of the model points q _{j. Correlation} with the model point q _j is performed.

In step S142, the weight setting unit 140 associates each data point p _i included in the data point group P with the data point p _i and the model point q _j that is the closest point to the data point p _i ( A constant weight w _i =W1 is given to p _i , q _j ) to make a weighted association (p _i , q _j , w _i ).

In step S143, the weight setting unit 140 sets the same model point q _j to the same model point q _{j in} the weighted correspondence (p _i , q _j , w _i ) for each data point p _i included in the data point group P. It is determined whether or not a plurality of data points p _i are associated.

When a plurality of data points p _i are associated with the same model point q _j (YES in step S143), in step S144, the weight setting unit 140 causes the weighted association including the model point q _j. For (p _i , q _j , w _i ), the weight w _i =W2 is set, and the process returns.

When a plurality of data points p _i are not associated with the same model point q _j (NO in step S143), the process returns.

As described above, according to the positional deviation correction device according to the first embodiment of the present invention, based on the coordinates of data points and the coordinates of model points, it is determined that a predetermined condition is satisfied. For each of the data points included in the data point group, in correspondence with the model point included in the model point group, when a plurality of data points included in the data point group are associated with the same model point. , A predetermined weight is set for each correspondence between the same model point and each of the plurality of data points, and the correspondence between the data point group and the model point group and the set predetermined weight are set. Then, when the coordinates of the data points included in the data point group are converted, a coordinate conversion matrix that minimizes the positional deviation between the converted data point group and the model point group is calculated, and based on the coordinate conversion matrix, By converting the coordinates of the data points included in the data point group, it is possible to suppress erroneous correspondence in the initial stage of the alignment and improve the alignment accuracy.

<Configuration of misregistration correction device 20 according to second embodiment of the present invention>
The configuration of the positional deviation correction device 20 according to the second embodiment of the present invention will be described with reference to FIG. The same components as those of the positional deviation correction device 10 according to the first embodiment are designated by the same reference numerals, and detailed description thereof will be omitted.

The positional deviation correction device 10 includes a CPU, a RAM, and a ROM that stores a program for executing a positional deviation correction processing routine described later, and is functionally configured as shown below.

As shown in FIG. 5, the positional deviation correction device 20 according to the embodiment of the present invention includes an input unit 100, a model point group storage unit 110, an acquisition unit 220, a model point correspondence unit 130, and data point correspondence. The configuration includes a unit 235, a weight setting unit 240, a conversion matrix calculation unit 150, a determination unit 160, a coordinate conversion unit 270, and an output unit 180.

The acquisition unit 220 acquires a model point group Q, which is a set of coordinates of model points q _{j serving} as a reference for alignment.

Then, the acquisition unit 220 passes the data point group P and the model point group Q to the model point corresponding unit 130 and the data point corresponding unit 235.

Data points corresponding unit 235, based on the coordinates of the coordinates and the data points p _i model points q _j, for each of the model points q _j in the model point group Q, the data points p _i included in the data point group P Corresponds to.

Specifically, the data point correspondence unit 235 searches the data point group P for the data point p _i that is the closest point for each model point q _j of the model point group Q.

Then, for each of the model points q _j included in the model point group Q, the data point correspondence unit 235 associates the model point q _j with the data point p _i that is the closest point to the model point q _j (p. _i , q _j ) to the weight setting unit 240.

For each data point p _i included in the data point group P, the weight setting unit 240 associates the corresponding data point p _i with the model point q _j that is the closest point to the data point p _i (p _i , q _j ) is given a predetermined weight w _i =W1 to make a weighted correspondence (p _i , q _j , w _i ), and for each model point q _j included in the model point group Q, A weight w _j =W1 is assigned to the association (p _i , q _j ) between the model point q _j and the data point p _i that is the closest point to the model point q _j , and the weighted association (q _j , p _i , w _j ).

In addition, when a plurality of data points p _i are associated with the same model point q _j , the weight setting unit 240 includes a weighted association (p _i , q _j , w) including the model point q _j. For each _i ), let the weight w _i =W2. When a plurality of model points q _j included in the model point group Q are associated with the same data point p _i , the weight setting unit 240 further assigns a weighted association (including the data point p _i ). A weight w _i =W2 is given to each of q _j , p _i , and w _j ).

Then, the weight setting unit 240 passes each of the weighted associations (p _i , q _j , w _j ) to the conversion matrix calculation unit 150.

The coordinate conversion unit 270 converts the coordinates of the data points p _i included in the data point group P based on the coordinate conversion matrix T.

Specifically, the coordinate conversion unit 270 performs coordinate conversion of the data point group P into the data point group P'using the above equation (4).

Then, the coordinate conversion unit 270 passes the data point group P′ to the model point corresponding unit 130 and the data point corresponding unit 235.

In this way, by adding the weight w _j , ambiguous associations can be removed or the influence of ambiguous associations can be reduced, so that the positional deviation can be reduced.

Similar to the first embodiment, FIG. 6 shows an example in which W1=1.0 and W2=0.0. FIG. 6A shows a case where both the data point group P and the model point group Q are partially missing.

In this case, the correspondence (p _i , q _j ) determined to be duplicated is treated as invalid as shown in FIG. Then, as shown in FIG. 6C, only the effective association (p _i , q _j ) is used for the alignment.

Therefore, even when the matching in the opposite direction is performed, as the positional deviation between the point groups becomes smaller, the ambiguity is eliminated, the number of point pairs that can be matched is increased, and highly accurate positioning becomes possible.

<Operation of the positional deviation correction device 20 according to the second embodiment of the present invention>
FIG. 7 is a flowchart showing a point cloud association processing routine according to the second embodiment of the present invention. The same processes as those in the point cloud association process routine according to the first embodiment are designated by the same reference numerals, and detailed description thereof will be omitted.

In step S300, the data point corresponding unit 235 is included in the data point group P for each of the model points q _j included in the model point group Q based on the coordinates of the model point q _{j and} the coordinates of the data point p _{i. Correlate} with the data point p _i .

In step S305, the weight setting unit 240, for each of the data points p _i included in the data points P, correspondence between model points q _j is the closest point of the data points p _i and the data point p _i ( p _i ,q _j ) is given a predetermined weight w _i =W1 to make a weighted association (p _i ,q _j ,w _i ), and model points q _j included in the model point group Q. For each of the above, weight w _j =W1 is given to the association (p _i , q _j ) between the model point q _j and the data point p _i that is the closest point of the model point q _j , and weighting is performed. Associating (q _j , p _i , w _j ).

In step S310, the weight setting unit 240 associates a plurality of data points p _i with the same model point q _j in the weighted association (p _i , q _j , w _i ), or the same. It is determined whether or not a plurality of model points q _j are associated with the data points p _i of.

When there are no multiple associations including the same model point q _j and there are no multiple associations including the same data point p _i (NO in step S310 above), the process returns.

On the other hand, when there are a plurality of associations including the same model point q _j or a plurality of associations including the same data point p _i (YES in step S310 above), in step S244, the weight setting unit 240 is set. a plurality of weighted correlation including the same model points _{q j (p i, q j} , w i), or with a plurality of weights, including the same data points _{p i} association _{(p i, q} j, w _j ), weights w _i =W2 and w _j =W2 are set, and the process returns.

As described above, according to the positional deviation correction device according to the second embodiment of the present invention, until it is determined that the predetermined condition is satisfied,

Based on the coordinates of the data points and the coordinates of the model points, each of the data points included in the data point group is associated with the model point included in the model point group, and the data point group is assigned to the same model point. When a plurality of included data points are associated with each other, a predetermined weight is set for each association of the same model point and each of the plurality of data points, and the coordinates of the model points and the data points Based on the coordinates, each model point included in the model point group is associated with a data point included in the data point group, and the same data point is associated with a plurality of model points included in the model point group. When attached, a predetermined weight is set for each correspondence between the same data point and each of the plurality of model points, and the correspondence between the data point group and the model point group is set. When the coordinates of the data points included in the data point group are converted based on the predetermined weight, the coordinate conversion matrix that minimizes the positional deviation between the converted data point group and the model point group is calculated, and By converting the coordinates of the data points included in the data point group based on the coordinate conversion matrix, it is possible to suppress erroneous association at the initial stage of alignment and improve the alignment accuracy.

<Configuration of misregistration correction device 30 according to third embodiment of the present invention>
The configuration of the positional deviation correction device 30 according to the second embodiment of the present invention will be described with reference to FIG. The same components as those of the positional deviation correction device 10 according to the first embodiment are designated by the same reference numerals, and detailed description thereof will be omitted.

The positional deviation correction device 30 includes a CPU, a RAM, and a ROM that stores a program for executing a positional deviation correction processing routine described later, and is functionally configured as shown below.

As shown in FIG. 8, the positional deviation correction device 30 according to the embodiment of the present invention includes an input unit 100, a model point group storage unit 110, an acquisition unit 120, a model point correspondence unit 130, and a first effective point. The distance determination unit 332, the weight setting unit 140, the conversion matrix calculation unit 150, the determination unit 160, the coordinate conversion unit 170, and the output unit 180 are included.

In the present embodiment, the model points corresponding unit 130, for each of the data points p _i included in the data points P, corresponding to the model point q _j Recently a neighbor of the data points p _i and the data point p _i The attachment (p _i , q _j ) is passed to the first effective distance determination unit 332.

The first effective distance determination unit 332 associates the distance between the coordinate of the data point p _{i and} the coordinate of the model point q _j associated by the model point association unit 130 with a predetermined first threshold value or more. Invalidate.

Specifically, the first effective distance determination unit 332 first, for each correspondence (p _i , q _j ) between the data point p _i and the model point q _j , the data point p _i and the model point q. Calculate the distance from _j .

Next, the first effective distance determination unit 332 determines whether the distance between the data point p _i and the model point q _j for each of the associations (p _i , q _j ) is the first threshold value or more. To judge.

If the distance is greater than or equal to the first threshold, the first effective distance determination unit 332 invalidates the association (p _i , q _j ) and does not use it in the subsequent processing.

On the other hand, when the distance is not greater than or equal to the first threshold, the first effective distance determination unit 332 makes the association (p _i , q _j ) valid.

Then, the first effective distance determination unit 332 associates each data point p _i included in the data point group P with the data point p _i and the model point q _j that is the closest point to the data point p _{i. (p} i, _{q j)} effective mapping of the _(p i, _{q j),} and passes to the weight setting unit 140.

<Operation of the positional deviation correction device 30 according to the third embodiment of the present invention>
FIG. 9 is a flowchart showing a point cloud association processing routine according to the third embodiment of the present invention. The same processes as those in the point cloud association process routine according to the first embodiment are designated by the same reference numerals, and detailed description thereof will be omitted.

In step S400, the first effective distance determination unit 332 associates the data point p _i with the model point q _j for each association (p _i , q _j ) between the data point p _i and the model point q _j . Calculate the distance.

In step S410, the first effective distance determination unit 332 determines whether the distance between the data point p _i and the model point q _j for each of the associations (p _i , q _j ) is the first threshold value or more. To determine.

When the distance is equal to or greater than the first threshold value (YES in step S410), in step S420, the first effective distance determination unit 332 makes the association (p _i , q _j ) invalid, and the process proceeds to step S142.

On the other hand, if the distance is not equal to or more than the first threshold value (YES in step S410), the process proceeds to step S142.

As described above, according to the positional deviation correction device according to the third embodiment of the present invention, the distance between the coordinate of the associated data point and the coordinate of the model point is equal to or larger than the first threshold value set in advance. In some cases, since the association is invalidated, it is possible to suppress erroneous association in the initial stage of alignment and further improve the alignment accuracy.

<Structure of the positional deviation correction device 40 according to the fourth embodiment of the present invention>
The configuration of the positional deviation correction device 40 according to the fourth embodiment of the present invention will be described with reference to FIG. In addition, regarding the same configurations as the positional deviation correction device 10 according to the first embodiment, the positional deviation correction device 20 according to the second embodiment, and the positional deviation correction device 30 according to the third embodiment, The same reference numerals are given and detailed description is omitted.

The positional deviation correction device 40 includes a CPU, a RAM, and a ROM that stores a program for executing a positional deviation correction processing routine described later, and is functionally configured as shown below.

As shown in FIG. 10, the positional deviation correction device 40 according to the embodiment of the present invention includes an input unit 100, a model point group storage unit 110, an acquisition unit 220, a model point correspondence unit 130, and data point correspondence. Unit 235, first effective distance determination unit 332, second effective distance determination unit 437, weight setting unit 240, conversion matrix calculation unit 150, determination unit 160, coordinate conversion unit 270, output unit 180. It is configured with.

In the present embodiment, the model points corresponding unit 130, for each of the data points p _i included in the data points P, corresponding to the model point q _j Recently a neighbor of the data points p _i and the data point p _i The attachment (p _i , q _j ) is passed to the first effective distance determination unit 332.

In addition, the data point correspondence unit 235 associates each model point q _j of the model point group Q with the corresponding data point p _i and the model point q _j that is the closest point to the data point p _i (p _i , Q _j ) is passed to the second effective distance determination unit 437.

The second effective distance determination unit 437 associates the distance between the coordinate of the model point p _{i and} the coordinate of the data point p _i associated by the data point association unit 235 with a predetermined second threshold value or more. Invalidate.

Specifically, the second effective distance determination unit 437 firstly, for each of the correspondence (p _i , q _j ) between the model point q _j and the data point p _i , the model point q _j and the data point p. Calculate the distance from _i .

Next, the second effective distance determination unit 437 determines whether or not the distance between the model point q _j and the data point p _i for each of the associations (p _i , q _j ) is the second threshold value or more. To judge.

If the distance is equal to or greater than the second threshold value, the second effective distance determination unit 437 invalidates the association (p _i , q _j ) and does not use it in the subsequent processing.

On the other hand, when the distance is not greater than or equal to the second threshold value, the second effective distance determination unit 437 makes the association (p _i , q _j ) valid.

Then, the second effective distance determination unit 437 associates each model point p _i included in the model point group Q with the model point q _j and the data point p _i that is the closest point to the model point q _{j. (p} i, _{q j)} effective mapping of the _(p i, _{q j),} and passes to the weight setting unit 240.

<Operation of the positional deviation correction device 40 according to the fourth embodiment of the present invention>
FIG. 11 is a flowchart showing a point cloud association processing routine according to the fourth embodiment of the present invention. It should be noted that the point cloud association processing routine according to the first embodiment, the point cloud association processing routine according to the second embodiment, and the point cloud association processing routine according to the third embodiment. The same processing as the above is denoted by the same reference numeral, and detailed description thereof will be omitted.

In step S510, the second effective distance determination unit 437 associates the model point q _j with the data point p _i for each of the correspondence (p _i , q _j ) between the model point q _j and the data point p _i . Calculate the distance.

In step S520, the second effective distance determination unit 437 determines whether or not the distance between the model point q _j and the data point p _i for each of the associations (p _i , q _j ) is the second threshold value or more. To determine.

When the distance is equal to or greater than the second threshold value (YES in step S520), in step S530, the second effective distance determination unit 437 invalidates the association (p _i , q _j ) and proceeds to step S305.

On the other hand, if the distance is not greater than or equal to the second threshold value (YES in step S520 above), the process advances to step S305.

As described above, according to the misregistration correction device according to the fourth embodiment of the present invention, each model point included in the model point group is associated with the model point and the model point. It is determined whether or not the distance to the data point is greater than or equal to a predetermined second threshold value, and if the distance is greater than or equal to the second threshold value, the correspondence with the model point is invalidated. It is possible to suppress erroneous correspondence of the above and further improve the accuracy of alignment.

The present invention is not limited to the above-described embodiments, and various modifications and applications can be made without departing from the scope of the present invention.

For example, in the above embodiment, the case of a three-dimensional point group has been described, but the present invention is not limited to this. A configuration using a two-dimensional point group may be used.

Further, the weight setting unit 140 (240), when the positional deviation between the data point p _i of the associated data point group P and the model point q _j of the model point group Q is equal to or less than a predetermined third threshold value, Alternatively, when the number of repetitions by the determination unit 160 is equal to or larger than the predetermined fourth threshold value, the weight setting method is changed.

Further, when the determination unit 160 determines that the predetermined condition is not satisfied, the weight setting unit 140 (240) may be instructed to change the weight setting method based on the determination result. .

Specifically, the determination unit 160 determines that the average value of the positional deviations of the weighted associations (p _i , q _j , w _i ) for each of the data points p _i included in the data point group P is predetermined. When the value is equal to or less than 3 threshold values or when the counter s is equal to or higher than a predetermined fourth threshold value, the condition that is not the predetermined condition is satisfied, the weight setting unit 140 is configured to set the weight ( Instruct to change (values of W1 and W2). For example, you may change so that the value of W2 may be enlarged.

This is because when there is a missing point group, the association may remain invalidated, so changing the weight value changes the contribution to alignment and improves accuracy. .

Further, although the weight setting unit 140 and the weight setting unit 240 are configured to change the weight to the predetermined W2, the present invention is not limited to this.

For example, when a plurality of data points p _i correspond to the same model point q _j , or when a plurality of model points q _j correspond to the same data point p _i , w _It is also possible to adopt a configuration in which _i is divided by 10, a value is divided by a number corresponding to the duplication number, or the like.

In addition, the weight setting unit 240 determines that a plurality of data points p _i correspond to the same model point q _j , or a plurality of model points q _j correspond to the same data point p _i. The weight is changed only when the weight is changed, but the present invention is not limited to this.

For example, the weight setting unit 240 associates each of the data points p _i included in the data point group P with the model point q _j, and as a result, the data point p _i and the nearest point of the data point p _i . in a correspondence between model points _{q j (p i, q j} ) if there is, for each of the model points q _j in the model point group Q, results of correspondence between the data points p _i, When the data point associated with the model point q _j is different from the data point p _i, it is determined that there is a disagreement in the association, and the weight setting unit 240 causes the weight setting unit 240 to associate (p _i , q _j). ), a weight w _i =W3 may be given and weighted association (p _i , q _j , w _i ) may be performed. Here, W3 is a predetermined constant.

Further, in the specification of the present application, the embodiment in which the program is preinstalled has been described, but the program can be stored in a computer-readable recording medium and provided.

10 correction device 20 correction device 30 correction device 40 correction device 100 input unit 110 model point group storage unit 120 acquisition unit 130 model point correspondence unit 140 weight setting unit 150 conversion matrix calculation unit 160 determination unit 170 coordinate conversion unit 180 output unit 220 acquisition Unit 235 Data point correspondence unit 240 Weight setting unit 270 Coordinate conversion unit 332 First effective distance determination unit 437 Second effective distance determination unit

Claims (8)

An input unit that receives an input of a data point group, which is a set of coordinates of data points to be aligned,
An acquisition unit that acquires a model point group, which is a set of coordinates of model points serving as a reference for alignment,
Based on the coordinates of the data points and the coordinates of the model points, for each of the data points included in the data point group, a model point corresponding unit that associates with the model points included in the model point group,
When a plurality of data points included in the data point group are associated with the same model point, a predetermined value is set for each association of the same model point and each of the plurality of data points. A weight setting section for setting the weight of
When the coordinates of the data points included in the data point group are converted based on the correspondence between the data point group and the model point group, and the set predetermined weight, the converted data A conversion matrix calculation unit that calculates a coordinate conversion matrix that minimizes the positional deviation between the point group and the model point group;
A coordinate conversion unit that converts the coordinates of the data points included in the data point group based on the coordinate conversion matrix;
A determination unit that repeats the conversion by the coordinate conversion unit, the association by the model point correspondence unit, the setting by the weight setting unit, and the calculation by the conversion matrix calculation unit until it is determined that a predetermined condition is satisfied,
Misregistration correction device including. The conversion matrix calculation unit is obtained by using the distance between the coordinates of the converted data points and the coordinates of the model points associated with each other, and the weight for the association between the data points and the model points. The positional deviation correction device according to claim 1, wherein the coordinate conversion matrix is calculated so that the sum of squares of the attached distances is minimized. The weight setting unit, when only one of the data points is associated with one of the model points, sets a certain weight for the association of the model point and the data point, and the same. When a plurality of data points are associated with the model point of, a lower weight than the constant weight is assigned to each of the association of the same model point and each of the plurality of data points. The position shift correction device according to claim 1 or 2, which is set. A data point correspondence unit that associates each model point included in the model point group with a data point included in the data point group, based on the coordinates of the model point and the coordinates of the data point. ,
The weight setting unit associates the same data point with each of the plurality of model points when a plurality of model points included in the model point group are associated with the same data point. Set a predetermined weight for each of
Until the determination unit determines that a predetermined condition is satisfied, conversion by the coordinate conversion unit, association by the model point correspondence unit, association by the data point correspondence unit, setting by the weight setting unit, and The positional deviation correction device according to claim 1, wherein the calculation by the conversion matrix calculation unit is repeated. A first effective distance determination unit that invalidates the association when the distance between the coordinates of the data points and the coordinates of the model points associated by the model point association unit is equal to or greater than a first threshold value set in advance. The position shift correction device according to any one of items 1 to 3. A second effective distance determination unit that invalidates the association when the distance between the coordinates of the model point and the coordinates of the data point associated by the data point association unit is equal to or greater than a second threshold value set in advance. Item 4. The position shift correction device according to item 4. The weight setting unit, when the positional deviation between the associated data points of the data point group and the model points of the model point group is equal to or less than a predetermined third threshold value, or the number of repetitions by the determination unit is set in advance. The positional deviation correction device according to claim 1, wherein a setting method of the weight is changed when the value is equal to or more than the determined fourth threshold value. Computer,
An input unit that receives an input of a data point group that is a set of coordinates of data points to be aligned,
An acquisition unit that acquires a model point group that is a set of coordinates of model points serving as a reference for alignment,
A model point corresponding unit that associates each of the data points included in the data point group with a model point included in the model point group based on the coordinates of the data points and the coordinates of the model points,
When a plurality of data points included in the data point group are associated with the same model point, a predetermined value is set for each association of the same model point and each of the plurality of data points. A weight setting unit that sets the weight of
When the coordinates of the data points included in the data point group are converted based on the correspondence between the data point group and the model point group, and the set predetermined weight, the converted data A conversion matrix calculation unit that calculates a coordinate conversion matrix that minimizes the positional deviation between the point group and the model point group,
A coordinate conversion unit for converting the coordinates of the data points included in the data point group based on the coordinate conversion matrix, and conversion by the coordinate conversion unit until the predetermined condition is satisfied, the model point A program that functions as a determination unit that repeats the association by the association unit, the setting by the weight setting unit, and the calculation by the conversion matrix calculation unit.