CN110533726B - Laser radar scene three-dimensional attitude point normal vector estimation correction method - Google Patents

Laser radar scene three-dimensional attitude point normal vector estimation correction method Download PDF

Info

Publication number
CN110533726B
CN110533726B CN201910802065.0A CN201910802065A CN110533726B CN 110533726 B CN110533726 B CN 110533726B CN 201910802065 A CN201910802065 A CN 201910802065A CN 110533726 B CN110533726 B CN 110533726B
Authority
CN
China
Prior art keywords
normal vector
point
points
angle
vector
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910802065.0A
Other languages
Chinese (zh)
Other versions
CN110533726A (en
Inventor
孙剑峰
张楠
刘迪
周鑫
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN201910802065.0A priority Critical patent/CN110533726B/en
Publication of CN110533726A publication Critical patent/CN110533726A/en
Application granted granted Critical
Publication of CN110533726B publication Critical patent/CN110533726B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/77—Determining position or orientation of objects or cameras using statistical methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Evolutionary Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Probability & Statistics with Applications (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Image Analysis (AREA)

Abstract

The invention provides a laser radarA scene three-dimensional attitude point normal vector estimation correction method comprises the following steps: method for solving point normal vector n of each point in point cloud by using local plane fittingiCalculating the centroid of the normal vector of the point
Figure DDA0002182598800000011
And find all niAnd
Figure DDA0002182598800000012
angle of (theta)i(ii) a Step two: selecting an included angle interval with the frequency f larger than a threshold value lambda from an included angle histogram according to the distribution of an included angle between a normal vector and a centroid vector of points in the point cloud, wherein the number of the selected intervals is the clustering number k; step three: preliminarily selecting a representative normal vector; step four: selecting a point set to be fitted to correct the representative normal vector; step five: and obtaining an optimal fitting plane by adopting a random sampling consistency algorithm, and calculating a final representative normal vector. According to the method, aiming at the distance image of the real scene, the point normal vectors in the cluster are screened through a random sampling consistency algorithm, the deviation of the representative normal vector is corrected, the optimal representative normal vector is obtained, and the three-dimensional attitude estimation is more accurate.

Description

Laser radar scene three-dimensional attitude point normal vector estimation correction method
Technical Field
The invention belongs to the technical field of laser radar three-dimensional target attitude estimation, and particularly relates to a method for estimating and correcting a normal vector of a three-dimensional attitude point of a laser radar scene.
Background
The existing attitude estimation method mainly comprises a Principal Component Analysis (PCA), a rectangular bounding box method, a model matching method and a deep learning-based method. The PCA method is widely applied, but the requirement on the completeness of target point cloud distribution is high, and the PCA method is not suitable because the laser radar obtains range profile data under a single visual angle. The rectangular bounding box method is based on the characteristic that targets such as automobiles, tanks and the like have similar rectangular structures, and is sensitive to shielding and can directly influence the result of attitude estimation. The model matching method requires that the target is known and has a complete attitude model library, and the attitude model with the highest matching degree of the target point cloud feature and the model point cloud feature is the attitude of the target point cloud, so that the method is difficult to realize in practical application. The attitude estimation method based on deep learning generally acquires attitude angle data through a convolutional neural network for a two-dimensional image. The method needs a large amount of sample data for training and has higher requirements on image resolution. The proposed attitude estimation algorithm (PDVA) based on the point normal vector has a good attitude estimation effect on a noise-free range profile obtained by a laser radar simulation system, but for a real laser radar scene range profile, the obtained representative normal vector generates large offset, so that a certain deviation exists between attitude estimation and a real value.
Disclosure of Invention
The invention aims to solve the problems in the prior art and provides a method for estimating and correcting a normal vector of a three-dimensional attitude point of a laser radar scene. The method is applied to the acquired real scene range profile, and the optimal representative normal vector is obtained through the plane model of RANSAC, so that the three-dimensional attitude estimation error is reduced. According to the method, aiming at the distance image of the real scene, the point normal vectors in the cluster are screened through a random sampling consistency algorithm, the deviation of the representative normal vector is corrected, the optimal representative normal vector is obtained, and the three-dimensional attitude estimation is more accurate.
The invention is realized by the following technical scheme that the invention provides a method for estimating and correcting the normal vector of the three-dimensional attitude point of the laser radar scene, and the X of a scene coordinate system SCS is estimated according to the normal vector represented by a scene targets、YsAnd ZsAxis, X coordinate axis of scene object around SCS in process of converting SCS into geodetic coordinate system GCSs、YsAnd ZsThe shaft conversion angles are a three-dimensional attitude angle yaw angle psi, a pitch angle theta and a roll angle phi; the method comprises the following steps:
the method comprises the following steps: method for solving point normal vector n of each point in point cloud by using local plane fittingiCalculating the centroid of the normal vector of the point
Figure BDA0002182598780000011
And find all niAnd
Figure BDA0002182598780000012
angle of (theta)i；
Step two: selecting an included angle interval with the frequency f larger than a threshold value lambda from an included angle histogram according to the distribution of an included angle between a normal vector and a centroid vector of points in the point cloud, wherein the number of the selected intervals is the clustering number k;
step three: preliminarily selecting a representative normal vector;
each cluster after classification deltaiIn 1.. k, the pair is clustered by δiNormal vector and cluster delta of each point in the vectoriCalculating the included angles formed by the normal vectors of all other points, summing the included angles, and selecting a cluster deltaiThe normal vector of the point with the minimum angle sum with the normal vectors of other points is taken as a representative normal vector and is marked as npi；
The points in each cluster are counted and denoted as countiChoose countiThe normal vector of the representative point of the largest cluster is taken as a direction vector of the target coordinate system and is recorded as n, and the normal vector is sequentially selected according to countiCalculating the angle between the normal vector of the cluster representative point in descending order and n, and selecting the angle between the normal vector and the n as [ 90-delta, 90-delta + delta ]]The representative normal vector in the range is the other direction vector of the target coordinate system; wherein, delta is a degree threshold value, and the size of the threshold value is correspondingly adjusted according to the actual situation;
step four: selecting a point set to be fitted to correct the representative normal vector;
computing cluster deltaiAll points g ini∈δiAnd represents a normal vector npiEuclidean distance ρ of the corresponding pointiTo find the average value of the distances
Figure BDA0002182598780000021
Selecting a distance value less than
Figure BDA0002182598780000022
To obtain a point set G1：
Figure BDA0002182598780000023
Computing cluster deltaiNormal vector n of all points iniAnd represents a normal vector npiAngle of inclusion value thetapiSelecting points with included angle value less than 45 degrees to obtain a point set G2：
Figure BDA0002182598780000024
Wherein, represents a normal vector npiThe corresponding point coordinate is expressed as (x)p,yp,zp) Normal vector n of pointsiThe corresponding point coordinate is expressed as (x)i,yi,zi) Finally, the point set to be fitted is obtained as Gd＝G1∩G2；
Step five: and obtaining an optimal fitting plane by adopting a random sampling consistency algorithm, and calculating a final representative normal vector.
Further, the fifth step is specifically:
randomly extracting G each time within the range of the iteration number KdObtaining a fitting plane from 3 points in the step (A), obtaining the distance D between the points and the fitting plane, and comparing G according to the set iteration number K and the distance threshold value DdAnd (5) screening the points until a best fitting plane is found, and solving a final representative normal vector.
Further, the distance solution from the points to the fitting plane is shown in the formula (3), wherein [ a, b-1, c ] is a parameter for randomly selecting 3 points to construct the plane;
Figure BDA0002182598780000031
drawings
FIG. 1 is a flow chart of a method for estimating and correcting a normal vector of a three-dimensional attitude point of a laser radar scene according to the invention;
FIG. 2 is a schematic diagram of a scene representation of a normal vector top view.
Detailed Description
The technical solutions in the embodiments of the present invention will be described clearly and completely with reference to the accompanying drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
With reference to fig. 1, the invention provides a method for estimating and correcting a normal vector of a three-dimensional attitude point of a laser radar scene, which estimates and obtains an X of a scene coordinate system SCS according to a scene target representative normal vectors、YsAnd ZsAxis, the scene object is around the Coordinate axis X of SCS in the process of converting SCS into geodetic Coordinate system GCS (geodetic Coordinate System)s、YsAnd ZsThe shaft conversion angles are a three-dimensional attitude angle yaw angle psi, a pitch angle theta and a roll angle phi; the method comprises the following steps:
the method comprises the following steps: method for solving point normal vector n of each point in point cloud by using local plane fittingiCalculating the centroid of the normal vector of the point
Figure BDA0002182598780000032
And find all niAnd
Figure BDA0002182598780000033
angle of (theta)i；
Step two: selecting an included angle interval with the frequency f larger than a threshold value lambda from an included angle histogram according to the distribution of an included angle between a normal vector and a centroid vector of points in the point cloud, wherein the number of the selected intervals is the clustering number k;
step three: preliminarily selecting a representative normal vector;
each cluster after classification deltaiIn 1.. k, the pair is clustered by δiNormal vector and cluster delta of each point in the vectoriCalculating the included angles formed by the normal vectors of all other points, summing the included angles, and selecting a cluster deltaiThe normal vector of the point with the minimum angle sum with the normal vectors of other points is taken as a representative normal vector and is marked as npi；
The points in each cluster are counted and denoted as countiChoose countiThe normal vector of the representative point of the largest cluster is taken as a direction vector of the target coordinate system and is recorded as n, and the normal vector is sequentially selected according to countiCalculating the angle between the normal vector of the cluster representative point in descending order and n, and selecting the angle between the normal vector and the n as [ 90-delta, 90-delta + delta ]]The representative normal vector in the range is the other direction vector of the target coordinate system; wherein, delta is a degree threshold value, and the size of the threshold value is correspondingly adjusted according to the actual situation;
step four: selecting a point set to be fitted to correct the representative normal vector;
computing cluster deltaiAll points g ini∈δiAnd represents a normal vector npiEuclidean distance ρ of the corresponding pointiTo find the average value of the distances
Figure BDA0002182598780000041
Selecting a distance value less than
Figure BDA0002182598780000042
To obtain a point set G1：
Figure BDA0002182598780000043
Computing cluster deltaiNormal vector n of all points iniAnd represents a normal vector npiAngle of inclusion value thetapiSelecting points with included angle value less than 45 degrees to obtain a point set G2：
Figure BDA0002182598780000044
Wherein, represents a normal vector npiThe corresponding point coordinate is expressed as (x)p,yp,zp) Normal vector n of pointsiThe corresponding point coordinate is expressed as (x)i,yi,zi) Finally, the point set to be fitted is obtained as Gd＝G1∩G2；
Step five: and (3) obtaining a best fit plane by adopting a random sampling consistency algorithm, calculating a final representative normal vector, and showing a scene representative normal vector top view as shown in figure 2.
The fifth step is specifically as follows:
randomly extracting G each time within the range of the iteration number KdObtaining a fitting plane from 3 points in the step (A), obtaining the distance D between the points and the fitting plane, and comparing G according to the set iteration number K and the distance threshold value DdAnd (5) screening the points until a best fitting plane is found, and solving a final representative normal vector.
The random sampling consistency algorithm parameter selection mainly relates to the selection of two parameters, namely a distance threshold value d and iteration times K.
In the scene target, the building has details such as windows and balconies, and the actual range image of laser radar has a range finding precision of 0.1875 m. Therefore, the distance threshold value d is 1.5, which is the distance measurement precision three times of that of the laser radar. The distance solution from the points to the fitting plane is shown in the formula (3), wherein [ a, b-1, c ] is a parameter for randomly selecting 3 points to construct the plane;
Figure BDA0002182598780000051
let the probability of randomly selecting 3 points to estimate the plane and 3 points all being correct data be w3And the probability that at least 1 of the 3 points is abnormal data is 1-w3Then, there are:
1-p＝(1-w3)K (4)
if K is not greater than the maximum number of iterations, and is constantly updated rather than fixed, then:
Figure BDA0002182598780000052
wherein, p is the confidence coefficient, and is generally 0.995; w is the ratio of interior points, w is the number of interior points/number of datasets; m is the minimum number of samples required for calculating the model, and m is 3.
The method aims at the characteristic that the scene point cloud has a large-area approximate plane area, and the normal vector represented by the scene point cloud is solved according to local plane fitting and a clustering algorithm. And optimally screening the point set which is corrected to represent the normal vector by setting the distance relation between each point and the representative normal vector in the clustering and the included angle relation between each point normal vector and the representative normal vector. And (4) selecting the optimal fitting plane of the point set by adopting a random sampling consistency algorithm to obtain the optimal representative normal vector. According to the method for estimating and correcting the normal vector of the three-dimensional attitude point of the laser radar scene, the optimal representative normal vector of the scene target can be estimated, and the accurate three-dimensional attitude of the scene target can be obtained.
The method for estimating and correcting the normal vector of the three-dimensional attitude point of the laser radar scene provided by the invention is described in detail, a specific example is applied in the method to explain the principle and the implementation mode of the invention, and the description of the embodiment is only used for helping to understand the method and the core idea of the invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, there may be variations in the specific embodiments and the application scope, and in summary, the content of the present specification should not be construed as a limitation to the present invention.

Claims (3)

1. A laser radar scene three-dimensional attitude point normal vector estimation correction method is characterized by comprising the following steps: estimating and obtaining X of scene coordinate system SCS according to scene target representation normal vectors、YsAnd ZsAxis, X coordinate axis of scene object around SCS in process of converting SCS into geodetic coordinate system GCSs、YsAnd ZsThe shaft conversion angles are a three-dimensional attitude angle yaw angle psi, a pitch angle theta and a roll angle phi; the method comprises the following steps:
the method comprises the following steps: method for solving point normal vector n of each point in point cloud by using local plane fittingiCalculating the centroid of the normal vector of the point
Figure FDA0002881964720000011
And find all niAnd
Figure FDA0002881964720000012
angle of (theta)i；
Step two: selecting an included angle interval with the frequency f larger than a threshold value lambda from an included angle histogram according to the distribution of an included angle between a normal vector and a centroid vector of points in the point cloud, wherein the number of the selected intervals is the clustering number k;
step three: preliminarily selecting a representative normal vector;
each cluster after classification deltaiIn 1.. k, the pair is clustered by δiNormal vector and cluster delta of each point in the vectoriCalculating the included angles formed by the normal vectors of all other points, summing the included angles, and selecting a cluster deltaiThe normal vector of the point with the minimum angle sum with the normal vectors of other points is taken as a representative normal vector and is marked as npi；
The points in each cluster are counted and denoted as countiChoose countiThe representative normal vector of the largest cluster is a direction vector of a scene coordinate system SCS and is marked as n, and the direction vector is sequentially selected according to countiThe cluster arranged in descending order represents the angle formed by the normal vector and n, and the angle is selected to be [ 90-delta, 90-delta + delta ]]The representative normal vector in the range is another direction vector of the scene coordinate system SCS; wherein, delta is a degree threshold value, and the size of the threshold value is correspondingly adjusted according to the actual situation;
step four: selecting a point set to be fitted to correct the representative normal vector;
computing cluster deltaiAll points g ini∈δiAnd represents a normal vector npiEuclidean distance ρ of the corresponding pointiTo find the average value of the distances
Figure FDA0002881964720000013
Selecting a distance value less than
Figure FDA0002881964720000014
To obtain a point set G1：
Figure FDA0002881964720000015
Computing cluster deltaiNormal vector n of all points iniAnd represents a normal vector npiAngle of inclusion value thetapiSelecting points with included angle value less than 45 degrees to obtain a point set G2：
Figure FDA0002881964720000016
Wherein, represents a normal vector npiThe corresponding point coordinate is expressed as (x)p,yp,zp) Normal vector n of pointsiThe corresponding point coordinate is expressed as (x)i,yi,zi) Finally, the point set to be fitted is obtained as Gd＝G1∩G2；
Step five: and obtaining an optimal fitting plane by adopting a random sampling consistency algorithm, and calculating a final representative normal vector.
2. The method of claim 1, wherein: the fifth step is specifically as follows:
randomly extracting G each time within the range of the iteration number KdObtaining a fitting plane from 3 points in the step (A), obtaining the distance D between the points and the fitting plane, and comparing G according to the set iteration number K and the distance threshold value DdAnd (5) screening the points until a best fitting plane is found, and solving a final representative normal vector.
3. The method of claim 2, wherein: the distance solution from the points to the fitting plane is shown in the formula (3), wherein [ a, b-1, c ] is a parameter for randomly selecting 3 points to construct the plane;
Figure FDA0002881964720000021
CN201910802065.0A 2019-08-28 2019-08-28 Laser radar scene three-dimensional attitude point normal vector estimation correction method Active CN110533726B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910802065.0A CN110533726B (en) 2019-08-28 2019-08-28 Laser radar scene three-dimensional attitude point normal vector estimation correction method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910802065.0A CN110533726B (en) 2019-08-28 2019-08-28 Laser radar scene three-dimensional attitude point normal vector estimation correction method

Publications (2)

Publication Number Publication Date
CN110533726A CN110533726A (en) 2019-12-03
CN110533726B true CN110533726B (en) 2021-05-04

Family

ID=68664806

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910802065.0A Active CN110533726B (en) 2019-08-28 2019-08-28 Laser radar scene three-dimensional attitude point normal vector estimation correction method

Country Status (1)

Country Link
CN (1) CN110533726B (en)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112085773A (en) * 2020-09-07 2020-12-15 深圳市凌云视迅科技有限责任公司 Plane fitting method and device for removing local outliers
CN112614186A (en) * 2020-12-28 2021-04-06 上海汽车工业(集团)总公司 Target pose calculation method and calculation module
CN113188570A (en) * 2021-04-27 2021-07-30 西南石油大学 Attitude error calibration method of inclinometer while drilling based on support vector classifier and K-proximity method
CN113724332B (en) * 2021-11-04 2022-01-18 贝壳技术有限公司 Method for determining relative pose of camera, electronic device and storage medium
CN114612665B (en) * 2022-03-15 2022-10-11 北京航空航天大学 Pose estimation and dynamic vehicle detection method based on normal vector histogram features

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2012146253A1 (en) * 2011-04-29 2012-11-01 Scape Technologies A/S Pose estimation and classification of objects from 3d point clouds
CN103236064A (en) * 2013-05-06 2013-08-07 东南大学 Point cloud automatic registration method based on normal vector
CN104657968A (en) * 2013-11-25 2015-05-27 武汉海达数云技术有限公司 Automatic vehicle-mounted three-dimensional laser point cloud facade classification and outline extraction method
CN108090960A (en) * 2017-12-25 2018-05-29 北京航空航天大学 A kind of Object reconstruction method based on geometrical constraint

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2012146253A1 (en) * 2011-04-29 2012-11-01 Scape Technologies A/S Pose estimation and classification of objects from 3d point clouds
CN103236064A (en) * 2013-05-06 2013-08-07 东南大学 Point cloud automatic registration method based on normal vector
CN104657968A (en) * 2013-11-25 2015-05-27 武汉海达数云技术有限公司 Automatic vehicle-mounted three-dimensional laser point cloud facade classification and outline extraction method
CN108090960A (en) * 2017-12-25 2018-05-29 北京航空航天大学 A kind of Object reconstruction method based on geometrical constraint

Also Published As

Publication number Publication date
CN110533726A (en) 2019-12-03

Similar Documents

Publication Publication Date Title
CN110533726B (en) Laser radar scene three-dimensional attitude point normal vector estimation correction method
CN110097093B (en) Method for accurately matching heterogeneous images
CN113139453B (en) Orthoimage high-rise building base vector extraction method based on deep learning
CN109523595B (en) Visual measurement method for linear angular spacing of building engineering
CN108428220B (en) Automatic geometric correction method for ocean island reef area of remote sensing image of geostationary orbit satellite sequence
CN111145228A (en) Heterogeneous image registration method based on local contour point and shape feature fusion
CN112484746B (en) Monocular vision auxiliary laser radar odometer method based on ground plane
CN104732546B (en) The non-rigid SAR image registration method of region similitude and local space constraint
JP2014523572A (en) Generating map data
CN110187337B (en) LS and NEU-ECEF space-time registration-based high maneuvering target tracking method and system
CN111508008A (en) Point cloud registration method, electronic equipment and storage medium
CN111008964A (en) Component surface defect detection method
CN110688440B (en) Map fusion method suitable for less sub-map overlapping parts
CN111275748A (en) Point cloud registration method based on laser radar in dynamic environment
CN107765257A (en) A kind of laser acquisition and measuring method based on the calibration of reflected intensity accessory external
CN104484647B (en) A kind of high-resolution remote sensing image cloud height detection method
CN114529615A (en) Radar calibration method, device and storage medium
CN103337080A (en) Registration technology of infrared image and visible image based on Hausdorff distance in gradient direction
CN110554405B (en) Normal scanning registration method and system based on cluster combination
CN108595373B (en) Uncontrolled DEM registration method
CN117269977A (en) Laser SLAM implementation method and system based on vertical optimization
CN109375160B (en) Angle measurement error estimation method in pure-azimuth passive positioning
CN109375159B (en) Pure orientation weighting constraint total least square positioning method
CN116819561A (en) Point cloud data matching method, system, electronic equipment and storage medium
CN112017108B (en) Satellite image color relative correction method based on adjustment of independent model method

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant