CN110533726A - A kind of laser radar scene 3 d pose point normal estimation modification method - Google Patents
A kind of laser radar scene 3 d pose point normal estimation modification method Download PDFInfo
- Publication number
- CN110533726A CN110533726A CN201910802065.0A CN201910802065A CN110533726A CN 110533726 A CN110533726 A CN 110533726A CN 201910802065 A CN201910802065 A CN 201910802065A CN 110533726 A CN110533726 A CN 110533726A
- Authority
- CN
- China
- Prior art keywords
- normal vector
- point
- angle
- vector
- cluster
- 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.)
- Granted
Links
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
Abstract
The present invention proposes a kind of laser radar scene 3 d pose point normal estimation modification method, and the method includes the steps one: the point normal vector n of each point in a cloud is acquired using the method that part plan is fittedi, calculate the mass center of a normal vectorAnd acquire all niWithAngle thetai；Step 2: according to the distribution of angle between cloud midpoint normal vector and centroid vector, the angle section that frequency f is greater than threshold value λ is selected in angle histogram, the quantity in selected section is the quantity k that clusters；Step 3: preliminary choose represents normal vector；Step 4: it chooses point set to be fitted and is modified to normal vector is represented；Step 5: optimal fitting plane is obtained using random sampling unification algorism, calculates and finally represents normal vector.The present invention is directed to real scene Range Profile, is screened by random sampling unification algorism to the point normal vector in cluster, has modified the deviation for representing normal vector, and acquisition is optimal to represent normal vector, and 3 d pose estimation is more accurate.
Description
Technical field
The invention belongs to laser radar objective Attitude estimation technical fields, more particularly to a kind of laser radar scene
3 d pose point normal estimation modification method.
Background technique
Existing Attitude estimation method mainly has Principal Component Analysis (PCA), Rectangular Bounding Volume method, model matching method and base
In the method for deep learning.PCA method is using relatively broad, but it is more demanding to completeness of target point cloud distribution, due to
What laser radar obtained is the Range Profile data under single visual angle, therefore PCA method and is not suitable for.Rectangular Bounding Volume method is to be based on
The targets such as automobile, tank have the feature of similar rectangular configuration, and this method is more sensitive to blocking, and will have a direct impact on posture
The result of estimation.Model matching method requires target known and has complete attitude mode library, target point cloud feature and model points
The highest attitude mode of cloud characteristic matching degree is target point cloud posture, relatively difficult to achieve in practical applications.Based on deep learning
Attitude estimation method obtains attitude angle data by convolutional neural networks generally directed to two dimensional image.This method needs a large amount of
Sample data is used for training, and to the more demanding of image resolution ratio.The Attitude estimation based on normal vector having proposed is calculated
Method (PDVA), this method make an uproar Range Profile with preferable Attitude estimation effect to the nothing that Lidar Simulation system obtains, but
For actual laser radar scene Range Profile, the representative normal vector acquired can generate biggish offset, lead to Attitude estimation and true
There are certain deviations for real value.
Summary of the invention
The invention aims to solve the problems of the prior art, a kind of laser radar scene 3 d pose point is proposed
Normal estimation modification method.The real scene Range Profile that the present invention is applied to obtain is obtained by the areal model of RANSAC
It is optimal to represent normal vector, reduce 3 d pose evaluated error.The present invention is directed to real scene Range Profile, consistent by random sampling
Algorithm screens the point normal vector in cluster, has modified the deviation for representing normal vector, and acquisition is optimal to represent normal vector, three-dimensional
Attitude estimation is more accurate.
The present invention is achieved by the following technical solutions, and the present invention proposes a kind of laser radar scene 3 d pose point method
Vector estimates modification method, represents the X that normal estimation obtains scene coordinate system SCS according to scene objectss、YsAnd ZsAxis, will
During SCS is converted to earth coordinates GCS, reference axis X of the scene objects around SCSs、YsAnd ZsAxis conversion angle be
Three-dimension altitude angle yaw angle Ψ, pitching angle theta and roll angle Φ；It the described method comprises the following steps:
Step 1: the point normal vector n of each point in a cloud is acquired using the method that part plan is fittedi, calculate a normal direction
The mass center of amountAnd acquire all niWithAngle thetai；
Step 2: it according to the distribution of angle between cloud midpoint normal vector and centroid vector, is selected in angle histogram
Frequency f is greater than the angle section of threshold value λ, and the quantity in selected section is the quantity k that clusters；
Step 3: preliminary choose represents normal vector；
Each cluster δ after sortingi, in i=1 ..., k, to cluster δiIn each normal vector and cluster δiIn its
Angle formed by his all the points normal vector calculates, and sums to formed angle, chooses cluster δiIn with other point
Normal vector angle and the smallest normal vector are to represent normal vector, are denoted as npi；
Point in each cluster is counted, count is denoted asi, choose countiThe maximum representative point normal vector to cluster
It is denoted as n for a direction vector of target-based coordinate system, successively chooses and presses countiThe representative point normal direction meter that clusters of descending arrangement
Itself and n angulation are calculated, angulation is chosen and represents normal vector in [90 ° of-δ, 90 ° of+δ] range as target-based coordinate system
Another direction vector；Wherein δ is a degree threshold value, and size adjusts accordingly according to the actual situation；
Step 4: it chooses point set to be fitted and is modified to normal vector is represented；
Calculate cluster δiMiddle all the points gi∈δiWith method of representatives vector npiThe Euclidean distance ρ of corresponding pointsi, acquire range averaging
ValueSelected distance value is less thanPoint, obtain point set G1:
Calculate cluster δiMiddle all the points normal vector niWith method of representatives vector npiAngle value θpi, angle value is chosen less than 45 °
Point, obtain point set G2:
Wherein, method of representatives vector npiCorresponding points coordinate representation is (xp,yp,zp), put normal vector niCorresponding points coordinate representation is
(xi,yi,zi), finally acquiring point set to be fitted is Gd=G1∩G2；
Step 5: optimal fitting plane is obtained using random sampling unification algorism, calculates and finally represents normal vector.
Further, the step 5 specifically:
Within the scope of the number of iterations K, G is randomly selected every timedIn 3 points obtain fit Plane, acquire a little flat to fitting
Identity distance is from D, according to setting the number of iterations K and distance threshold d, to GdIn point screened, until find best-fitting plane,
It acquires and finally represents normal vector.
Further, the distance of the point to fit Plane is solved as shown in formula (3), and [a, b-1, c] is random choosing in formula
Take the parameter of 3 point building planes；
Detailed description of the invention
Fig. 1 is laser radar scene 3 d pose point normal estimation modification method flow chart of the present invention；
Fig. 2 is scene representations normal vector schematic top plan view.
Specific embodiment
Technical solution in the embodiment of the present invention that following will be combined with the drawings in the embodiments of the present invention carries out clear, complete
Ground description, it is clear that described embodiments are only a part of the embodiments of the present invention, instead of all the embodiments.Based on this
Embodiment in invention, every other reality obtained by those of ordinary skill in the art without making creative efforts
Example is applied, shall fall within the protection scope of the present invention.
In conjunction with Fig. 1, the present invention proposes a kind of laser radar scene 3 d pose point normal estimation modification method, according to field
Scape object representations normal estimation obtains the X of scene coordinate system SCSs、YsAnd ZsSCS is converted to earth coordinates GCS by axis
During (Geodetic Coordinate System), reference axis X of the scene objects around SCSs、YsAnd ZsThe angle of axis conversion
Degree is three-dimension altitude angle yaw angle Ψ, pitching angle theta and roll angle Φ；It the described method comprises the following steps:
Step 1: the point normal vector n of each point in a cloud is acquired using the method that part plan is fittedi, calculate a normal direction
The mass center of amountAnd acquire all niWithAngle thetai；
Step 2: it according to the distribution of angle between cloud midpoint normal vector and centroid vector, is selected in angle histogram
Frequency f is greater than the angle section of threshold value λ, and the quantity in selected section is the quantity k that clusters；
Step 3: preliminary choose represents normal vector；
Each cluster δ after sortingi, in i=1 ..., k, to cluster δiIn each normal vector and cluster δiIn its
Angle formed by his all the points normal vector calculates, and sums to formed angle, chooses cluster δiIn with other point
Normal vector angle and the smallest normal vector are to represent normal vector, are denoted as npi；
Point in each cluster is counted, count is denoted asi, choose countiThe maximum representative point normal vector to cluster
It is denoted as n for a direction vector of target-based coordinate system, successively chooses and presses countiThe representative point normal direction meter that clusters of descending arrangement
Itself and n angulation are calculated, angulation is chosen and represents normal vector in [90 ° of-δ, 90 ° of+δ] range as target-based coordinate system
Another direction vector；Wherein δ is a degree threshold value, and size adjusts accordingly according to the actual situation；
Step 4: it chooses point set to be fitted and is modified to normal vector is represented；
Calculate cluster δiMiddle all the points gi∈δiWith method of representatives vector npiThe Euclidean distance ρ of corresponding pointsi, acquire range averaging
ValueSelected distance value is less thanPoint, obtain point set G1:
Calculate cluster δiMiddle all the points normal vector niWith method of representatives vector npiAngle value θpi, angle value is chosen less than 45 °
Point, obtain point set G2:
Wherein, method of representatives vector npiCorresponding points coordinate representation is (xp,yp,zp), put normal vector niCorresponding points coordinate representation is
(xi,yi,zi), finally acquiring point set to be fitted is Gd=G1∩G2；
Step 5: optimal fitting plane is obtained using random sampling unification algorism, calculates and finally represents normal vector, scene generation
Table normal vector top view is as shown in Figure 2.
The step 5 specifically:
Within the scope of the number of iterations K, G is randomly selected every timedIn 3 points obtain fit Plane, acquire a little flat to fitting
Identity distance is from D, according to setting the number of iterations K and distance threshold d, to GdIn point screened, until find best-fitting plane,
It acquires and finally represents normal vector.
The selection of the random sampling unification algorism parameter is mainly concerned with two parameters, distance threshold d and the number of iterations K
Selection.
In scene objects, building is 0.1875 as range accuracy there are window, the details such as balcony, laser radar actual range
Rice.Therefore, it is 1.5, the as range accuracy of laser radar three times that distance threshold, which chooses d,.Distance of the point to fit Plane
It solves as shown in formula (3), [a, b-1, c] is the parameter for randomly selecting 3 point building planes in formula；
If selecting 3 point estimation planes at random, 3 points are that the probability of correct data is w3, at least 1 in 3 points
Probability for abnormal data is 1-w3, then have:
1-p=(1-w3)K (4)
K is to fix constantly updating, then has in the case where being not more than maximum number of iterations:
Wherein, p is confidence level, generally takes 0.995；W is the ratio of interior point, the number/data set number put in w=；m
For minimum sample number, m=3 required for computation model.
There is large area almost plane region for scene point cloud in the present invention, be fitted and clustered according to part plan
Algorithm realization represents normal vector to scene point cloud and seeks.By setting cluster in each point with represent normal vector distance relation and
Each point normal vector and the angled relationships for representing normal vector carry out optimal screening to the point set for representing normal vector is modified.Using
Random sampling unification algorism carries out the selection of optimal fitting plane to point set, realizes that acquisition is optimal and represents normal vector.The present invention passes through
The laser radar scene 3 d pose point normal estimation modification method can estimate that scene objects are optimal and represent normal vector, obtain field
The accurate 3 d pose of scape target.
Above to a kind of laser radar scene 3 d pose point normal estimation modification method proposed by the invention, carry out
It is discussed in detail, used herein a specific example illustrates the principle and implementation of the invention, above embodiments
Explanation be merely used to help understand method and its core concept of the invention；At the same time, for those skilled in the art,
According to the thought of the present invention, there will be changes in the specific implementation manner and application range, in conclusion in this specification
Appearance should not be construed as limiting the invention.
Claims (3)
1. a kind of laser radar scene 3 d pose point normal estimation modification method, it is characterised in that: according to scene objects generation
Table normal estimation obtains the X of scene coordinate system SCSs、YsAnd ZsAxis, during SCS is converted to earth coordinates GCS, field
Reference axis X of the scape target around SCSs、YsAnd ZsThe angle of axis conversion is three-dimension altitude angle yaw angle Ψ, pitching angle theta and roll angle
Φ；It the described method comprises the following steps:
Step 1: the point normal vector n of each point in a cloud is acquired using the method that part plan is fittedi, calculate the matter of a normal vector
The heartAnd acquire all niWithAngle thetai；
Step 2: according to the distribution of angle between cloud midpoint normal vector and centroid vector, frequency f is selected in angle histogram
Angle section greater than threshold value λ, the quantity in selected section are the quantity k that clusters；
Step 3: preliminary choose represents normal vector；
Each cluster δ after sortingi, in i=1 ..., k, to cluster δiIn each normal vector and cluster δiIn other institute
There is angle formed by a normal vector to be calculated, and sum to formed angle, chooses cluster δiIn with other normal direction
Amount angle and the smallest normal vector are to represent normal vector, are denoted as npi；
Point in each cluster is counted, count is denoted asi, choose countiIt is maximum cluster represent point normal vector as mesh
One direction vector of mark coordinate system is denoted as n, successively chooses and presses countiThe representative point normal vector that clusters of descending arrangement calculates it
With n angulation, chooses angulation and represent normal vector in [90 ° of-δ, 90 ° of+δ] range as the another of target-based coordinate system
A direction vector；Wherein δ is a degree threshold value, and size adjusts accordingly according to the actual situation；
Step 4: it chooses point set to be fitted and is modified to normal vector is represented；
Calculate cluster δiMiddle all the points gi∈δiWith method of representatives vector npiThe Euclidean distance ρ of corresponding pointsi, acquire distance average
Selected distance value is less thanPoint, obtain point set G1:
Calculate cluster δiMiddle all the points normal vector niWith method of representatives vector npiAngle value θpi, point of the angle value less than 45 ° is chosen,
Obtain point set G2:
Wherein, method of representatives vector npiCorresponding points coordinate representation is (xp,yp,zp), put normal vector niCorresponding points coordinate representation is (xi,
yi,zi), finally acquiring point set to be fitted is Gd=G1∩G2；
Step 5: optimal fitting plane is obtained using random sampling unification algorism, calculates and finally represents normal vector.
2. according to the method described in claim 1, it is characterized by: the step 5 specifically:
Within the scope of the number of iterations K, G is randomly selected every timedIn 3 points obtain fit Plane, acquire a little to fit Plane distance
D, according to setting the number of iterations K and distance threshold d, to GdIn point screened, until find best-fitting plane, acquire most
Normal vector is represented eventually.
3. according to the method described in claim 2, it is characterized by: the distance of the point to fit Plane is solved such as formula (3) institute
Show, [a, b-1, c] is the parameter for randomly selecting 3 point building planes in formula；
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 true CN110533726A (en) | 2019-12-03 |
| CN110533726B 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) |
Cited By (5)
| 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 |
| CN113724332A (en) * | 2021-11-04 | 2021-11-30 | 贝壳技术有限公司 | Method for determining relative pose of camera, electronic device and storage medium |
| CN114612665A (en) * | 2022-03-15 | 2022-06-10 | 北京航空航天大学 | Pose estimation and dynamic vehicle detection method based on normal vector histogram features |
Citations (4)
| 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 |
-
2019
- 2019-08-28 CN CN201910802065.0A patent/CN110533726B/en active Active
Patent Citations (4)
| 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 |
Cited By (5)
| 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 |
| CN113724332A (en) * | 2021-11-04 | 2021-11-30 | 贝壳技术有限公司 | Method for determining relative pose of camera, electronic device and storage medium |
| CN114612665A (en) * | 2022-03-15 | 2022-06-10 | 北京航空航天大学 | Pose estimation and dynamic vehicle detection method based on normal vector histogram features |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110533726B (en) | 2021-05-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110533726A (en) | A kind of laser radar scene 3 d pose point normal estimation modification method | |
| CN106548462B (en) | Non-linear SAR image geometric correction method based on thin-plate spline interpolation | |
| CN106199557B (en) | A kind of airborne laser radar data vegetation extracting method | |
| CN110533722A (en) | A kind of the robot fast relocation method and system of view-based access control model dictionary | |
| CN107315171B (en) | Radar networking target state and system error joint estimation algorithm | |
| CN105844276A (en) | Face posture correction method and face posture correction device | |
| CN111145228A (en) | Heterogeneous image registration method based on local contour point and shape feature fusion | |
| CN111273312B (en) | Intelligent vehicle positioning and loop detection method | |
| CN106526593A (en) | Sub-pixel-level corner reflector automatic positioning method based on SAR rigorous imaging model | |
| CN111539422B (en) | Flight target cooperative identification method based on fast RCNN | |
| CN110807781A (en) | Point cloud simplification method capable of retaining details and boundary features | |
| CN109613926A (en) | Multi-rotor unmanned aerial vehicle land automatically it is High Precision Automatic identification drop zone method | |
| CN108680177A (en) | Synchronous superposition method and device based on rodent models | |
| CN113238205A (en) | Unmanned aerial vehicle surveying and mapping point cloud data offset correction method and system based on artificial intelligence | |
| CN115761303A (en) | Ground object classification method based on airborne laser radar point cloud and remote sensing image data | |
| CN110826644B (en) | Distributed power supply time sequence joint output typical scene generation method based on Copula function | |
| CN109931940B (en) | Robot positioning position reliability assessment method based on monocular vision | |
| CN109341682B (en) | Method for improving geomagnetic field positioning accuracy | |
| CN110187337A (en) | A kind of highly maneuvering target tracking and system based on LS and NEU-ECEF time-space relation | |
| CN110688440A (en) | Map fusion method suitable for less sub-map overlapping parts | |
| CN111765883B (en) | Robot Monte Carlo positioning method, equipment and storage medium | |
| CN110160503A (en) | A kind of unmanned plane landscape matching locating method for taking elevation into account | |
| CN110554405B (en) | Normal scanning registration method and system based on cluster combination | |
| CN108595373B (en) | Uncontrolled DEM registration method | |
| CN109508674A (en) | Airborne lower view isomery image matching method based on region division |
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 |