CN103729882A - Point cloud relative pose estimation method based on three-dimensional curve matching - Google Patents

Point cloud relative pose estimation method based on three-dimensional curve matching Download PDF

Info

Publication number
CN103729882A
CN103729882A CN201310742491.2A CN201310742491A CN103729882A CN 103729882 A CN103729882 A CN 103729882A CN 201310742491 A CN201310742491 A CN 201310742491A CN 103729882 A CN103729882 A CN 103729882A
Authority
CN
China
Prior art keywords
point
curve
dimensional
point set
cloud
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
Application number
CN201310742491.2A
Other languages
Chinese (zh)
Other versions
CN103729882B (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.)
Which Hangzhou science and Technology Co Ltd
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN201310742491.2A priority Critical patent/CN103729882B/en
Publication of CN103729882A publication Critical patent/CN103729882A/en
Application granted granted Critical
Publication of CN103729882B publication Critical patent/CN103729882B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Abstract

The invention discloses a point cloud relative pose estimation method based on three-dimensional curve matching. The method includes the steps that the edge contours of two frames of input point clouds are extracted, and three-dimensional point sets (img file='dest_path_image 002. TIF' wi='13' he='16'/) and (img file='dest_path_image 004. TIF' wi='14' he='16'/) on behalf of the three-dimensional edge contour curve are obtained; according to the three-dimensional point sets (img file='915081 dest_path_image 002. TIF' wi='14' he='17'/) and (img file='301063 dest_path_image 004. TIF' wi='14' he='16'/), orderly point sets (img file='dest_path_image 006. TIF' wi='26' he='20'/) are obtained through extraction; each orderly point set (img file='dest_path_image 008. TIF' wi='14' he='20'/) or (img file='dest_path_image 010. TIF' wi='15' he='21'/) is decomposed, each subset is on behalf of one arc section after decomposition, and a feature description vector of each arc section is established; the feature description vectors of the edge curve arc sections of the two frames of point clouds are matched to establish a matching relation, and the relative pose transformational matrixes (img file='dest_path_image 012. TIF' wi='39' he='18'/) of the two frames of point clouds are calculated. The point cloud relative pose estimation method based on three-dimensional curve matching does not depend on an initial pose, only an environmental contour is used for matching, and the relative pose is estimated; only the edge point sets are processed, and calculation is lowered; the method can be suitable for chaff interferent in which point clouds exist.

Description

A kind of some cloud Relative attitude and displacement estimation method based on three-dimensional curve coupling
Technical field
The present invention relates to three-dimensional environment reconstruction field, relate in particular to a kind of some cloud Relative attitude and displacement estimation method based on three-dimensional curve coupling.
Background technology
Existing some cloud Relative attitude and displacement estimation method is mainly divided into two classes.One class is based on a cloud characteristic matching, and shortcoming is that the impact of receptor site cloud variable density is very large, and the density variation probability that can directly lead to errors improves.The another kind of direct coupling based on a cloud, shortcoming is to rely on initial pose, and operand is large.
For the coupling of three-dimensional curve, existing method, also mainly based on convolution, is difficult to be applicable to the coupling of many curves to many curves.
Summary of the invention
The object of the invention is to overcome the deficiencies in the prior art, a kind of some cloud Relative attitude and displacement estimation method based on three-dimensional curve coupling is provided.
Point cloud Relative attitude and displacement estimation method based on three-dimensional curve coupling is:
1) extract respectively the edge contour of two frame input point clouds, obtain representing the three-dimensional point set of three-dimensional edges contour curve
Figure 2013107424912100002DEST_PATH_IMAGE001
,
Figure 2013107424912100002DEST_PATH_IMAGE002
;
2) respectively for three-dimensional point set
Figure 518551DEST_PATH_IMAGE001
,
Figure 904533DEST_PATH_IMAGE002
extraction obtains the set of orderly point set
Figure 2013107424912100002DEST_PATH_IMAGE003
,
Figure DEST_PATH_IMAGE004
, each orderly point set
Figure DEST_PATH_IMAGE005
or
Figure DEST_PATH_IMAGE006
all represented a three-dimensional curve;
3) by each orderly point set or
Figure 735272DEST_PATH_IMAGE006
decompose, each subset after decomposition represents an arc section, sets up the feature description vectors of every section of circular arc;
4) the feature description vectors of coupling two frame point cloud boundary curve arc sections, sets up matching relationship, calculates the relative pose transformation matrix of two frame point clouds
Figure DEST_PATH_IMAGE007
,
Figure DEST_PATH_IMAGE008
for rotation matrix,
Figure DEST_PATH_IMAGE009
for translation matrix.
Described step 1) is: a) for unordered some cloud, search for each point around radius be less than
Figure DEST_PATH_IMAGE010
quantity in scope is no more than
Figure DEST_PATH_IMAGE011
all nearest neighbor points, obtain point set , be point set
Figure 706901DEST_PATH_IMAGE012
fit Plane , with point set
Figure 998205DEST_PATH_IMAGE012
in plane interior subpoint position is independent variable, point set
Figure 932980DEST_PATH_IMAGE012
to plane
Figure 70700DEST_PATH_IMAGE013
distance be functional value, matching Binary quadratic functions
Figure DEST_PATH_IMAGE014
, obtain Hessian matrix
Figure DEST_PATH_IMAGE015
, calculate Hessian matrix
Figure 782173DEST_PATH_IMAGE015
eigenwert
Figure DEST_PATH_IMAGE016
with
Figure DEST_PATH_IMAGE017
, suppose if, and
Figure DEST_PATH_IMAGE020
, wherein
Figure DEST_PATH_IMAGE021
, be respectively respective threshold, think that this point is marginal point; B), for orderly some cloud, depth map, utilizes Canny algorithm to extract edge point set.
Described is point set
Figure 640232DEST_PATH_IMAGE012
fit Plane
Figure 377244DEST_PATH_IMAGE013
method be: calculate point set
Figure 369471DEST_PATH_IMAGE012
average, obtain plane
Figure 799315DEST_PATH_IMAGE013
center
Figure DEST_PATH_IMAGE023
; Calculate
Figure DEST_PATH_IMAGE024
proper vector, its minimal eigenvalue characteristic of correspondence vector is plane
Figure 509782DEST_PATH_IMAGE013
normal vector
Figure DEST_PATH_IMAGE025
; Plane
Figure 503015DEST_PATH_IMAGE013
center
Figure 615327DEST_PATH_IMAGE023
and normal vector
Figure 950493DEST_PATH_IMAGE025
represented that one through center
Figure 148257DEST_PATH_IMAGE023
, normal vector is
Figure 227071DEST_PATH_IMAGE025
plane.
Described matching Binary quadratic functions
Figure 725048DEST_PATH_IMAGE014
and obtain Hessian matrix
Figure 965537DEST_PATH_IMAGE015
method be: for point set
Figure 916175DEST_PATH_IMAGE012
middle every bit
Figure DEST_PATH_IMAGE026
, suppose
Figure 18255DEST_PATH_IMAGE024
two maximum eigenwert characteristic of correspondence vectors are respectively
Figure DEST_PATH_IMAGE027
with , calculate one with
Figure DEST_PATH_IMAGE029
,
Figure DEST_PATH_IMAGE030
for independent variable,
Figure DEST_PATH_IMAGE031
for the key-value pair of value,
One group of final formation
Figure DEST_PATH_IMAGE033
to
Figure 495372DEST_PATH_IMAGE031
key assignments mapping, and utilize least square method to ask for Hessian matrix
Figure 437921DEST_PATH_IMAGE015
Figure DEST_PATH_IMAGE034
，
Figure 344697DEST_PATH_IMAGE014
can be expressed as
Figure DEST_PATH_IMAGE035
。
Described step 2) be: use region growing algorithm, with at every turn from three-dimensional point set
Figure 234155DEST_PATH_IMAGE001
or
Figure 706725DEST_PATH_IMAGE002
in the region growing Seed Points chosen be at random initial, the represented curve single order of the point set of take is condition continuously, by constantly receiving the new point that meets condition of growth, expand in an orderly manner curve point set, if can not expand again, separately get Seed Points and expand another curve point set, until institute is a little complete by expansion.
The described curve single order condition of continuity is:
Figure DEST_PATH_IMAGE036
Wherein
Figure 505660DEST_PATH_IMAGE037
for two Neighbor Points in a cloud
Figure DEST_PATH_IMAGE038
,
Figure 2013107424912100002DEST_PATH_IMAGE039
between distance,
Figure DEST_PATH_IMAGE040
, be respectively at 2 from the distance of true origin,
Figure DEST_PATH_IMAGE042
for threshold value.
Described step 3) is: for each, represent the orderly point set of three-dimensional curve
Figure 368574DEST_PATH_IMAGE005
or
Figure 61724DEST_PATH_IMAGE006
, calculate successively each point
Figure DEST_PATH_IMAGE043
locate bent curvature of a curve
Figure DEST_PATH_IMAGE044
with turn round rate
Figure DEST_PATH_IMAGE045
,
Figure DEST_PATH_IMAGE046
Figure DEST_PATH_IMAGE047
Wherein
Figure DEST_PATH_IMAGE048
Figure 2013107424912100002DEST_PATH_IMAGE049
for point is concentrated in order
Figure 467428DEST_PATH_IMAGE043
next point, subsequently the orderly point set that represents three-dimensional curve is split into some subsets, the point in each subset has identical curvature of curve, and the rate of turning round is zero, this subset has represented a circular arc, remembers that this circular arc curvature is , arc length is
Figure 2013107424912100002DEST_PATH_IMAGE051
, centre coordinate is
Figure DEST_PATH_IMAGE052
, according to right-handed coordinate system rule, take arc method direction, circular arc starting point cutting method to and the direction vertical with this both direction as three axles, set up coordinate system, remember that this coordinate system and world coordinate system angulation vector are
Figure 2013107424912100002DEST_PATH_IMAGE053
, define the feature description vectors of this circular arc for
Figure DEST_PATH_IMAGE055
。
Described step 4) is: for the feature description vectors of two frame point cloud boundary curve arc sections, set up k-d tree, in k-d tree, set up the matching relationship between two frame point cloud boundary curve arc sections, use random sampling consistency algorithm, remove part erroneous matching, set up cost function
Figure DEST_PATH_IMAGE056
Wherein
Figure DEST_PATH_IMAGE057
be one group of matching relationship in the matching relationship between two frame point cloud boundary curve arc sections,
Figure DEST_PATH_IMAGE058
for the matching relationship of all existence,
Figure DEST_PATH_IMAGE059
,
Figure DEST_PATH_IMAGE060
be respectively the feature description vectors of one section of arc section in two frame point clouds,
Figure 345255DEST_PATH_IMAGE061
Figure DEST_PATH_IMAGE062
Figure DEST_PATH_IMAGE063
it is assessment feature description vectors
Figure 695464DEST_PATH_IMAGE059
,
Figure 723463DEST_PATH_IMAGE060
between the function of distance, with the relative pose transition matrix of two frame point clouds
Figure 905046DEST_PATH_IMAGE007
it is relevant, be defined as
Figure DEST_PATH_IMAGE064
Wherein
Figure DEST_PATH_IMAGE065
,
Figure DEST_PATH_IMAGE066
for the parameter of specified weight,
Figure 148869DEST_PATH_IMAGE067
Figure DEST_PATH_IMAGE068
Finally, Optimization Solution
Obtain the relative pose transformation matrix of two frame point clouds
Figure 485490DEST_PATH_IMAGE007
.
The present invention compared with prior art, the beneficial effect having:
1. do not rely on initial bit appearance, only the profile by environment mates, and estimates relative pose;
2. only process edge point set, reduced operand;
3. adapt to the coupling of many curves to many curves, can resist the impact that in a cloud, chaff interference causes.
Accompanying drawing explanation
Fig. 1 is the some cloud Relative attitude and displacement estimation method operation steps schematic diagram based on three-dimensional curve coupling;
Fig. 2 is the some cloud Relative attitude and displacement estimation method implementation result figure based on three-dimensional curve coupling;
Fig. 3 is the some cloud Relative attitude and displacement estimation method application example figure based on three-dimensional curve coupling.
Embodiment
Point cloud Relative attitude and displacement estimation method based on three-dimensional curve coupling is by being some arc sections by the edge contour curve separating of three-dimensional point cloud, definition be take arc section as basic three-dimensional curve vector quantization description, for two frame cloud datas, comparing and mate the vector quantization of its edge contour curve describes, the relative pose of asking for two frame cloud datas, concrete steps are as follows:
1) extract respectively the edge contour of two frame input point clouds, obtain representing the three-dimensional point set of three-dimensional edges contour curve
Figure 846064DEST_PATH_IMAGE001
,
Figure 951292DEST_PATH_IMAGE002
;
2) respectively for three-dimensional point set
Figure 321093DEST_PATH_IMAGE001
,
Figure 680530DEST_PATH_IMAGE002
extraction obtains the set of orderly point set
Figure 477585DEST_PATH_IMAGE003
,
Figure 555263DEST_PATH_IMAGE004
, each orderly point set
Figure 728755DEST_PATH_IMAGE005
or
Figure 739436DEST_PATH_IMAGE006
all represented a three-dimensional curve;
3) by each orderly point set
Figure 176234DEST_PATH_IMAGE005
or
Figure 272366DEST_PATH_IMAGE006
decompose, each subset after decomposition represents an arc section, sets up the feature description vectors of every section of circular arc;
4) the feature description vectors of coupling two frame point cloud boundary curve arc sections, sets up matching relationship, calculates the relative pose transformation matrix of two frame point clouds
Figure 203544DEST_PATH_IMAGE007
,
Figure 68732DEST_PATH_IMAGE008
for rotation matrix,
Figure 676431DEST_PATH_IMAGE009
for translation matrix.
Described step 1) is: a) for unordered some cloud, search for each point around radius be less than
Figure 259859DEST_PATH_IMAGE010
quantity in scope is no more than
Figure 509574DEST_PATH_IMAGE011
all nearest neighbor points, obtain point set
Figure 963690DEST_PATH_IMAGE012
, be point set fit Plane
Figure 813014DEST_PATH_IMAGE013
, with point set
Figure 866421DEST_PATH_IMAGE012
in plane
Figure 424310DEST_PATH_IMAGE013
interior subpoint position is independent variable, point set
Figure 904970DEST_PATH_IMAGE012
to plane distance be functional value, matching Binary quadratic functions , obtain Hessian matrix
Figure 217636DEST_PATH_IMAGE015
, calculate Hessian matrix
Figure 338039DEST_PATH_IMAGE015
eigenwert
Figure 383355DEST_PATH_IMAGE016
with
Figure 247406DEST_PATH_IMAGE017
, suppose
Figure 796199DEST_PATH_IMAGE018
if,
Figure 100885DEST_PATH_IMAGE019
and
Figure 367919DEST_PATH_IMAGE020
, wherein , be respectively respective threshold, think that this point is marginal point; B) for orderly some cloud, it is depth map, utilize Canny algorithm (Canny J. A computational approach to edge detection[J]. Pattern Analysis and Machine Intelligence, IEEE Transactions on, 1986 (6): 679-698.) extract edge point set.
Described is point set
Figure 697903DEST_PATH_IMAGE012
fit Plane
Figure 921074DEST_PATH_IMAGE013
method be: calculate point set
Figure 658086DEST_PATH_IMAGE012
average, obtain plane
Figure 650312DEST_PATH_IMAGE013
center
Figure 80157DEST_PATH_IMAGE023
; Calculate
Figure 56203DEST_PATH_IMAGE024
proper vector, its minimal eigenvalue characteristic of correspondence vector is plane
Figure 315015DEST_PATH_IMAGE013
normal vector
Figure 958486DEST_PATH_IMAGE025
; Plane
Figure 762494DEST_PATH_IMAGE013
center
Figure 491415DEST_PATH_IMAGE023
and normal vector
Figure 39071DEST_PATH_IMAGE025
represented that one through center , normal vector is plane.
Described matching Binary quadratic functions
Figure 728176DEST_PATH_IMAGE014
and obtain Hessian matrix
Figure 610681DEST_PATH_IMAGE015
method be: for point set
Figure 182739DEST_PATH_IMAGE012
middle every bit
Figure 125287DEST_PATH_IMAGE026
, suppose
Figure 32063DEST_PATH_IMAGE024
two maximum eigenwert characteristic of correspondence vectors are respectively
Figure 452680DEST_PATH_IMAGE027
with
Figure 394092DEST_PATH_IMAGE028
, calculate one with
Figure 241962DEST_PATH_IMAGE029
, for independent variable,
Figure 860342DEST_PATH_IMAGE031
for the key-value pair of value,
Figure DEST_PATH_IMAGE070
One group of final formation
Figure 905527DEST_PATH_IMAGE033
to
Figure 189878DEST_PATH_IMAGE031
key assignments mapping, and utilize least square method to ask for Hessian matrix
Figure 71246DEST_PATH_IMAGE015
，
Figure 484090DEST_PATH_IMAGE014
can be expressed as
Figure 939342DEST_PATH_IMAGE035
。
Described step 2) be: use region growing algorithm (Adams R, Bischof L. Seeded region growing[J]. Pattern Analysis and Machine Intelligence, IEEE Transactions on, 1994,16 (6): 641-647.), with at every turn from three-dimensional point set
Figure 839165DEST_PATH_IMAGE001
or
Figure 874117DEST_PATH_IMAGE002
in the region growing Seed Points chosen be at random initial, the represented curve single order of the point set of take is condition continuously, by constantly receiving the new point that meets condition of growth, expand in an orderly manner curve point set, if can not expand again, separately get Seed Points and expand another curve point set, until institute is a little complete by expansion.
The described curve single order condition of continuity is:
Figure 910206DEST_PATH_IMAGE036
Wherein
Figure 577223DEST_PATH_IMAGE037
for two Neighbor Points in a cloud ,
Figure 537406DEST_PATH_IMAGE039
between distance,
Figure 693581DEST_PATH_IMAGE040
,
Figure 959477DEST_PATH_IMAGE041
be respectively at 2 from the distance of true origin, for threshold value.
Described step 3) is: for each, represent the orderly point set of three-dimensional curve
Figure 741805DEST_PATH_IMAGE005
or
Figure 221328DEST_PATH_IMAGE006
, calculate successively each point
Figure 189284DEST_PATH_IMAGE043
locate bent curvature of a curve
Figure 3525DEST_PATH_IMAGE044
with turn round rate
Figure 449550DEST_PATH_IMAGE045
,
Figure DEST_PATH_IMAGE071
Figure 49159DEST_PATH_IMAGE047
Wherein
Figure DEST_PATH_IMAGE072
Figure 656857DEST_PATH_IMAGE049
for point is concentrated in order
Figure 240285DEST_PATH_IMAGE043
next point, subsequently the orderly point set that represents three-dimensional curve is split into some subsets, the point in each subset has identical curvature of curve, and the rate of turning round is zero, this subset has represented a circular arc, remembers that this circular arc curvature is , arc length is
Figure 944116DEST_PATH_IMAGE051
, centre coordinate is
Figure 473449DEST_PATH_IMAGE052
, according to right-handed coordinate system rule, take arc method direction, circular arc starting point cutting method to and the direction vertical with this both direction as three axles, set up coordinate system, remember that this coordinate system and world coordinate system angulation vector are
Figure 544173DEST_PATH_IMAGE053
, define the feature description vectors of this circular arc
Figure 597580DEST_PATH_IMAGE054
for
Figure 906201DEST_PATH_IMAGE055
。
Described step 4) is: for the feature description vectors of two frame point cloud boundary curve arc sections, set up k-d tree, in k-d tree, set up the matching relationship between two frame point cloud boundary curve arc sections, use random sampling consistency algorithm (Fischler M A, Bolles R C. Random sample consensus:a paradigm for model fitting with applications to image analysis and automated cartography[J]. Communications of the ACM, 1981, 24 (6): 381-395.), remove part erroneous matching, set up cost function
Figure DEST_PATH_IMAGE073
Wherein
Figure 855703DEST_PATH_IMAGE057
be one group of matching relationship in the matching relationship between two frame point cloud boundary curve arc sections, for the matching relationship of all existence,
Figure 474083DEST_PATH_IMAGE059
, be respectively the feature description vectors of one section of arc section in two frame point clouds,
Figure 538040DEST_PATH_IMAGE062
it is assessment feature description vectors
Figure 447407DEST_PATH_IMAGE059
,
Figure 996200DEST_PATH_IMAGE060
between the function of distance, with the relative pose transition matrix of two frame point clouds
Figure 553083DEST_PATH_IMAGE007
it is relevant,
Figure 820117DEST_PATH_IMAGE063
be defined as
Wherein
Figure 222279DEST_PATH_IMAGE065
,
Figure 891158DEST_PATH_IMAGE066
for the parameter of specified weight,
Figure DEST_PATH_IMAGE076
Finally, Optimization Solution
Obtain the relative pose transformation matrix of two frame point clouds
Figure 324337DEST_PATH_IMAGE007
.

Claims (8)

1. a some cloud Relative attitude and displacement estimation method of mating based on three-dimensional curve, is characterized in that its step is as follows:
1) extract respectively the edge contour of two frame input point clouds, obtain representing the three-dimensional point set of three-dimensional edges contour curve
Figure 15624DEST_PATH_IMAGE001
,
Figure 993682DEST_PATH_IMAGE002
;
2) respectively for three-dimensional point set
Figure 961638DEST_PATH_IMAGE001
,
Figure 526612DEST_PATH_IMAGE002
extraction obtains the set of orderly point set
Figure 972636DEST_PATH_IMAGE003
,
Figure 572245DEST_PATH_IMAGE004
, each orderly point set
Figure 711102DEST_PATH_IMAGE005
or
Figure 294530DEST_PATH_IMAGE006
all represented a three-dimensional curve;
3) by each orderly point set
Figure 13088DEST_PATH_IMAGE005
or
Figure 998361DEST_PATH_IMAGE006
decompose, each subset after decomposition represents an arc section, sets up the feature description vectors of every section of circular arc;
4) the feature description vectors of coupling two frame point cloud boundary curve arc sections, sets up matching relationship, calculates the relative pose transformation matrix of two frame point clouds
Figure 776961DEST_PATH_IMAGE007
,
Figure 847686DEST_PATH_IMAGE008
for rotation matrix,
Figure 369934DEST_PATH_IMAGE009
for translation matrix.
2. the some cloud Relative attitude and displacement estimation method based on three-dimensional curve coupling according to claim 1, is characterized in that, described step 1) is: a) for unordered some cloud, search for each point around radius be less than
Figure 209714DEST_PATH_IMAGE010
quantity in scope is no more than
Figure 159215DEST_PATH_IMAGE011
all nearest neighbor points, obtain point set
Figure 451656DEST_PATH_IMAGE012
, be point set fit Plane
Figure 471882DEST_PATH_IMAGE013
, with point set
Figure 123443DEST_PATH_IMAGE012
in plane
Figure 147855DEST_PATH_IMAGE013
interior subpoint position is independent variable, point set to plane distance be functional value, matching Binary quadratic functions
Figure 648741DEST_PATH_IMAGE014
, obtain Hessian matrix
Figure 384615DEST_PATH_IMAGE015
, calculate Hessian matrix
Figure 317936DEST_PATH_IMAGE015
eigenwert
Figure 455657DEST_PATH_IMAGE016
with
Figure 714600DEST_PATH_IMAGE017
, suppose if,
Figure 674783DEST_PATH_IMAGE019
and , wherein
Figure 831274DEST_PATH_IMAGE021
,
Figure 72900DEST_PATH_IMAGE022
be respectively respective threshold, think that this point is marginal point; B), for orderly some cloud, depth map, utilizes Canny algorithm to extract edge point set.
3. the some cloud Relative attitude and displacement estimation method based on three-dimensional curve coupling according to claim 2, is characterized in that, described is point set fit Plane method be: calculate point set
Figure 529923DEST_PATH_IMAGE012
average, obtain plane center
Figure 337659DEST_PATH_IMAGE023
; Calculate
Figure 803013DEST_PATH_IMAGE024
proper vector, its minimal eigenvalue characteristic of correspondence vector is plane
Figure 309081DEST_PATH_IMAGE013
normal vector
Figure 994140DEST_PATH_IMAGE025
; Plane
Figure 876646DEST_PATH_IMAGE013
center and normal vector
Figure 374940DEST_PATH_IMAGE025
represented that one through center
Figure 812875DEST_PATH_IMAGE023
, normal vector is plane.
4. the some cloud Relative attitude and displacement estimation method based on three-dimensional curve coupling according to claim 2, is characterized in that described matching Binary quadratic functions
Figure 174903DEST_PATH_IMAGE014
and obtain Hessian matrix
Figure 757194DEST_PATH_IMAGE015
method be: for point set
Figure 682425DEST_PATH_IMAGE012
middle every bit
Figure 375574DEST_PATH_IMAGE026
, suppose
Figure 702650DEST_PATH_IMAGE024
two maximum eigenwert characteristic of correspondence vectors are respectively
Figure 455843DEST_PATH_IMAGE027
with
Figure 602790DEST_PATH_IMAGE028
, calculate one with
Figure 630789DEST_PATH_IMAGE029
,
Figure 281213DEST_PATH_IMAGE030
for independent variable,
Figure 736465DEST_PATH_IMAGE031
for the key-value pair of value,
Figure 603665DEST_PATH_IMAGE032
One group of final formation
Figure 169775DEST_PATH_IMAGE033
to
Figure 674706DEST_PATH_IMAGE031
key assignments mapping, and utilize least square method to ask for Hessian matrix
Figure 300860DEST_PATH_IMAGE015
Figure 422399DEST_PATH_IMAGE034
，
Figure 261042DEST_PATH_IMAGE014
be expressed as
Figure 417217DEST_PATH_IMAGE035
。
5. the some cloud Relative attitude and displacement estimation method based on three-dimensional curve coupling according to claim 1, is characterized in that described step 2) be: region growing algorithm used, with at every turn from three-dimensional point set
Figure 683113DEST_PATH_IMAGE001
or
Figure 291949DEST_PATH_IMAGE002
in the region growing Seed Points chosen be at random initial, the represented curve single order of the point set of take is condition continuously, by constantly receiving the new point that meets condition of growth, expand in an orderly manner curve point set, if can not expand again, separately get Seed Points and expand another curve point set, until institute is a little complete by expansion.
6. the some cloud Relative attitude and displacement estimation method based on three-dimensional curve coupling according to claim 5, is characterized in that, the described curve single order condition of continuity is:
Figure 668704DEST_PATH_IMAGE036
Wherein
Figure DEST_PATH_IMAGE037
for two Neighbor Points in a cloud
Figure 148227DEST_PATH_IMAGE038
,
Figure DEST_PATH_IMAGE039
between distance,
Figure 116183DEST_PATH_IMAGE040
,
Figure 681156DEST_PATH_IMAGE041
be respectively at 2 from the distance of true origin, for threshold value.
7. the some cloud Relative attitude and displacement estimation method based on three-dimensional curve coupling according to claim 1, is characterized in that, described step 3) is: for each, represent the orderly point set of three-dimensional curve
Figure 992369DEST_PATH_IMAGE005
or
Figure 131226DEST_PATH_IMAGE006
, calculate successively each point
Figure 416452DEST_PATH_IMAGE043
locate bent curvature of a curve
Figure 931747DEST_PATH_IMAGE044
with turn round rate
Figure 385862DEST_PATH_IMAGE045
,
Figure 695621DEST_PATH_IMAGE046
Figure 500766DEST_PATH_IMAGE047
Wherein
Figure 23014DEST_PATH_IMAGE048
Figure DEST_PATH_IMAGE049
for point is concentrated in order next point, subsequently the orderly point set that represents three-dimensional curve is split into some subsets, the point in each subset has identical curvature of curve, and the rate of turning round is zero, this subset has represented a circular arc, remembers that this circular arc curvature is
Figure 281137DEST_PATH_IMAGE050
, arc length is , centre coordinate is
Figure 307999DEST_PATH_IMAGE052
, according to right-handed coordinate system rule, take arc method direction, circular arc starting point cutting method to and the direction vertical with this both direction as three axles, set up coordinate system, remember that this coordinate system and world coordinate system angulation vector are
Figure DEST_PATH_IMAGE053
, define the feature description vectors of this circular arc
Figure 368359DEST_PATH_IMAGE054
for
Figure 797066DEST_PATH_IMAGE055
。
8. the some cloud Relative attitude and displacement estimation method based on three-dimensional curve coupling according to claim 1, it is characterized in that, described step 4) is: for the feature description vectors of two frame point cloud boundary curve arc sections, set up k-d tree, in k-d tree, set up the matching relationship between two frame point cloud boundary curve arc sections, use random sampling consistency algorithm, remove part erroneous matching, set up cost function
Wherein
Figure 195741DEST_PATH_IMAGE057
be one group of matching relationship in the matching relationship between two frame point cloud boundary curve arc sections,
Figure 590950DEST_PATH_IMAGE058
for the matching relationship of all existence,
Figure 405322DEST_PATH_IMAGE059
,
Figure 696627DEST_PATH_IMAGE060
be respectively the feature description vectors of one section of arc section in two frame point clouds,
Figure 963660DEST_PATH_IMAGE061
Figure 237963DEST_PATH_IMAGE063
it is assessment feature description vectors ,
Figure 454498DEST_PATH_IMAGE060
between the function of distance, with the relative pose transition matrix of two frame point clouds
Figure 457089DEST_PATH_IMAGE007
it is relevant,
Figure 449316DEST_PATH_IMAGE063
be defined as
Wherein
Figure 324048DEST_PATH_IMAGE065
,
Figure 864751DEST_PATH_IMAGE066
for the parameter of specified weight,
Figure 210019DEST_PATH_IMAGE067
Figure 810765DEST_PATH_IMAGE068
Finally, Optimization Solution
Figure 742949DEST_PATH_IMAGE069
Obtain the relative pose transformation matrix of two frame point clouds
Figure 87343DEST_PATH_IMAGE007
.
CN201310742491.2A 2013-12-30 2013-12-30 A kind of some cloud relative pose estimation method based on three-dimensional curve coupling Active CN103729882B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201310742491.2A CN103729882B (en) 2013-12-30 2013-12-30 A kind of some cloud relative pose estimation method based on three-dimensional curve coupling

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201310742491.2A CN103729882B (en) 2013-12-30 2013-12-30 A kind of some cloud relative pose estimation method based on three-dimensional curve coupling

Publications (2)

Publication Number Publication Date
CN103729882A true CN103729882A (en) 2014-04-16
CN103729882B CN103729882B (en) 2016-09-28

Family

ID=50453943

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201310742491.2A Active CN103729882B (en) 2013-12-30 2013-12-30 A kind of some cloud relative pose estimation method based on three-dimensional curve coupling

Country Status (1)

Country Link
CN (1) CN103729882B (en)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104123546A (en) * 2014-07-25 2014-10-29 黑龙江省科学院自动化研究所 Multi-dimensional feature extraction based bullet trace comparison method
CN105021124A (en) * 2015-04-16 2015-11-04 华南农业大学 Planar component three-dimensional position and normal vector calculation method based on depth map
CN105509748A (en) * 2015-12-29 2016-04-20 深圳先进技术研究院 Navigation method and apparatus for robot
CN109613543A (en) * 2018-12-06 2019-04-12 深圳前海达闼云端智能科技有限公司 Method and device for correcting laser point cloud data, storage medium and electronic equipment
CN110186467A (en) * 2018-02-23 2019-08-30 通用汽车环球科技运作有限责任公司 Group's sensing points cloud map
CN113763560A (en) * 2021-08-02 2021-12-07 纵目科技（上海）股份有限公司 Method, system and equipment for generating point cloud data and computer readable storage medium
CN114943058A (en) * 2022-04-18 2022-08-26 哈尔滨理工大学 Orthodontic arch wire error fluctuation degree evaluation method based on position error judgment

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130009954A1 (en) * 2010-05-20 2013-01-10 Microsoft Corporation Spatially registering user photographs
CN103150747A (en) * 2012-11-16 2013-06-12 佳都新太科技股份有限公司 Point cloud registration algorithm based on topological characteristic
CN103383776A (en) * 2013-07-14 2013-11-06 浙江大学 Progressive type three-dimensional matching algorithm based on sectional matching and bayes estimation

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130009954A1 (en) * 2010-05-20 2013-01-10 Microsoft Corporation Spatially registering user photographs
CN103150747A (en) * 2012-11-16 2013-06-12 佳都新太科技股份有限公司 Point cloud registration algorithm based on topological characteristic
CN103383776A (en) * 2013-07-14 2013-11-06 浙江大学 Progressive type three-dimensional matching algorithm based on sectional matching and bayes estimation

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
刘洁 等: "基于空问曲线重建的点云模型精化方法", 《测绘通报》 *
吴禄慎: "基于特征点的改进ICP三维点云配准技术", 《南昌大学学报·工科版》 *
牛丹华 等: "改进的多视图点云数据配准方法", 《工具技术》 *
葛宝臻: "基于曲率图的三维点云数据配准", 《天津大学学报(自然科学与工程技术版)》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104123546A (en) * 2014-07-25 2014-10-29 黑龙江省科学院自动化研究所 Multi-dimensional feature extraction based bullet trace comparison method
CN105021124A (en) * 2015-04-16 2015-11-04 华南农业大学 Planar component three-dimensional position and normal vector calculation method based on depth map
CN105021124B (en) * 2015-04-16 2018-02-16 华南农业大学 A kind of planar part three-dimensional position and normal vector computational methods based on depth map
CN105509748A (en) * 2015-12-29 2016-04-20 深圳先进技术研究院 Navigation method and apparatus for robot
CN110186467A (en) * 2018-02-23 2019-08-30 通用汽车环球科技运作有限责任公司 Group's sensing points cloud map
CN109613543A (en) * 2018-12-06 2019-04-12 深圳前海达闼云端智能科技有限公司 Method and device for correcting laser point cloud data, storage medium and electronic equipment
CN113763560A (en) * 2021-08-02 2021-12-07 纵目科技（上海）股份有限公司 Method, system and equipment for generating point cloud data and computer readable storage medium
CN113763560B (en) * 2021-08-02 2024-02-09 纵目科技（上海）股份有限公司 Method, system, equipment and computer readable storage medium for generating point cloud data
CN114943058A (en) * 2022-04-18 2022-08-26 哈尔滨理工大学 Orthodontic arch wire error fluctuation degree evaluation method based on position error judgment

Also Published As

Publication number Publication date
CN103729882B (en) 2016-09-28

Similar Documents

Publication Publication Date Title
CN103729882A (en) Point cloud relative pose estimation method based on three-dimensional curve matching
CN110443836B (en) Point cloud data automatic registration method and device based on plane features
CN111508074B (en) Three-dimensional building model simplification method based on roof contour line
CN103727930B (en) A kind of laser range finder based on edge matching and camera relative pose scaling method
CN108010081B (en) RGB-D visual odometer method based on Census transformation and local graph optimization
CN109685080B (en) Multi-scale plane extraction method based on Hough transformation and region growth
TWI497450B (en) Visual object tracking method
CN107818598B (en) Three-dimensional point cloud map fusion method based on visual correction
CN105261047A (en) Docking ring circle center extraction method based on close-range short-arc image
CN109146001B (en) Multi-view ISAR image fusion method
CN105160686B (en) A kind of low latitude various visual angles Remote Sensing Images Matching Method based on improvement SIFT operators
CN108932734B (en) Monocular image depth recovery method and device and computer equipment
CN111998862B (en) BNN-based dense binocular SLAM method
CN104050681A (en) Road vanishing point detection method based on video images
CN103279955B (en) Image matching method and system
CN111709450B (en) Point cloud normal vector estimation method and system based on multi-scale feature fusion
CN104657986A (en) Quasi-dense matching extension method based on subspace fusion and consistency constraint
CN104881029A (en) Mobile robot navigation method based on one point RANSAC and FAST algorithm
CN104463953A (en) Three-dimensional reconstruction method based on inertial measurement unit and RGB-D sensor
CN108805915A (en) A kind of close-range image provincial characteristics matching process of anti-visual angle change
CN105469110A (en) Non-rigid transformation image characteristic matching method based on local linear transfer and system
CN114202632A (en) Grid linear structure recovery method and device, electronic equipment and storage medium
CN110967020B (en) Simultaneous drawing and positioning method for port automatic driving
Zhu et al. An improved RANSAC algorithm based on similar structure constraints
CN111664845B (en) Traffic sign positioning and visual map making method and device and positioning system

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20180806

Address after: 310052 208, room 6, 1197 Binan Road, Binjiang District, Hangzhou, Zhejiang.

Patentee after: Which Hangzhou science and Technology Co Ltd

Address before: 310027 No. 38, Zhejiang Road, Hangzhou, Zhejiang, Xihu District

Patentee before: Zhejiang University