CN110243390A - The determination method, apparatus and odometer of pose - Google Patents
The determination method, apparatus and odometer of pose Download PDFInfo
- Publication number
- CN110243390A CN110243390A CN201910620817.1A CN201910620817A CN110243390A CN 110243390 A CN110243390 A CN 110243390A CN 201910620817 A CN201910620817 A CN 201910620817A CN 110243390 A CN110243390 A CN 110243390A
- Authority
- CN
- China
- Prior art keywords
- plane
- point
- pose
- matching
- 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
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
- G01C22/02—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers by conversion into electric waveforms and subsequent integration, e.g. using tachometer generator
- G01C22/025—Differential odometers
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Image Analysis (AREA)
Abstract
The present invention provides the determination method, apparatus and odometer of a kind of pose, the program includes: according to cromogram and depth map, a cloud is calculated, cromogram and depth map are obtained by acquisition same target, extract the plane in point cloud, according to matching plane pair or, match plane to and matching double points, determine pose, wherein, any one matching plane centering includes matched first plane and the second plane, and the first plane is the plane of first frame data reduction, and the second plane is the plane of the second frame data reduction；It include matched first point and second point in any one matching double points, first point is the point in first frame point cloud, and second point is the point in the second frame point cloud.It can be seen that two frame point clouds are calculated according to two frame cromograms and depth map, and the plane respectively extracted according to two frame point clouds determines pose, because plane is compared to point, it is not easy to be influenced by spot noise, the accuracy of pose can be effectively improved in the present invention.
Description
Technical field
The present invention relates to image processing algorithm technical field more particularly to the determination method, apparatus and mileage of a kind of pose
Meter.
Background technique
The development of science and technology is maked rapid progress, and the autonomous robot as frontier area of scientific and technological development product gradually comes into the visual field of people
In.Effective sensor of the odometer as autonomous robot relative positioning provides real-time posture information for autonomous robot,
Ensure that autonomous robot can learn the scene structure of surrounding in real time.Positioning and map structuring (simultaneous
Localization and mapping, SLAM) technology as the core technology in odometer, can be in a unknown environment
The three-dimensional structure of middle real-time reconstruction environment simultaneously simultaneously positions robot itself.In SLAM technology, the tool of SLAM technology
The purpose of body be in order to and meanwhile recover the corresponding camera motion parameter of every frame image and scene three-dimensional structure.Wherein, camera is transported
3 × 3 spin matrix and three-dimensional position variable included in dynamic parameter are exactly the posture information determined needed for odometer.
Currently, the point of the ambient enviroment generally provided using camera is made when existing odometer is calculated using SLMA technology
For reference parameter, carries out three-dimensional space modeling and pose is calculated.However, point is easy to be influenced and generated not by spot noise
The error that can be estimated eventually leads to the obtained pose of calculating and relatively large deviation occurs.
Summary of the invention
In view of this, the embodiment of the present invention provides the determination method, apparatus and odometer of a kind of pose, it is therefore intended that solve
The problem for the pose inaccuracy that existing odometer provides.
To achieve the above object, the embodiment of the present invention provides the following technical solutions:
A kind of determination method of pose, comprising:
According to cromogram and depth map, a cloud, the cromogram and the depth map is calculated by acquisition with a pair of
As obtaining；
Extract the plane in described cloud；
According to matching plane pair or, the matching plane to and matching double points, determine pose, any one of
Trim includes matched first plane and the second plane in facing, and first plane is the plane of first frame data reduction, institute
State the plane that the second plane is the second frame data reduction；In any one matching double points include matched first point and second point,
Described first point is the point in the first frame point cloud, and the second point is the point in the second frame point cloud.
Above-mentioned method, optionally, after the determining pose, further includes:
By using default first object function, the distance between true point and estimation point are minimized, and using default
Second objective function minimizes the geometric error between true planar and estimation plane, the pose after being optimized；
Wherein, the coordinate really put is the actual coordinate for constituting the 3D point of object under world coordinate system；Institute
The coordinate for stating estimation point is the point in the point cloud being calculated according to the cromogram and the depth map, is become by the pose
Change to the coordinate under the world coordinate system；The equation of the true planar is the actual equations of the plane under world coordinate system；
The equation of the estimation plane is the plane extracted in a cloud, transforms to the side under the world coordinate system by the pose
Journey.
Above-mentioned method, optionally, the plane extracted in described cloud, comprising:
Determine the corresponding plane of default screening areas of point cloud, the corresponding plane of any one screening areas are as follows: the screening
The plane that the preset quantity point chosen in region determines；
The plane that the quantity of interior point is met to preset condition, as the plane in described cloud, any one plane it is interior
Point are as follows: belong to the point of the plane in the corresponding screening areas of the plane, the point for belonging to the plane is at a distance from the plane
Less than the point of preset threshold.
Above-mentioned method, optionally, the number of the Similarity matching plane pair are three；
The foundation matching plane pair, determines pose, comprising:
Obtain the plane V of the first frame data reductionj, the plane V' of the second frame data reductionj, wherein j=1,
2,3；
Based on the plane VjWith the plane V'j, and the goal-selling function by minimizing spin matrix R obtains institute
State the estimated value of spin matrix R；
Based on the plane VjWith the plane V'j, and the predetermined linear equation by solving three-dimensional position variable t obtains
The estimated value of the three-dimensional position variable t；
According to the estimated value of the spin matrix R and the estimated value of the three-dimensional position variable t, the pose is determined.
Above-mentioned method, optionally, the number of the matching plane pair are less than three；
It is described according to matching plane to and matching double points, determine pose, comprising:
Obtain the plane V of the first frame data reductionj, the plane V' of the second frame data reductionj, the first frame
Point p in point cloudi, point p' in the second frame point cloudi, wherein i=1,2, j=1 or i=1, j=1,2；
Based on the point pi, the point p'i, the plane VjWith the plane V'j, and by minimizing spin matrix R's
Goal-selling function obtains the estimated value of the spin matrix R；
Based on the point pi, the point p'i, the plane VjWith the plane V'j, in conjunction with the estimation of the spin matrix R
Value, and the goal-selling function by minimizing three-dimensional position variable t obtains the estimated value of the three-dimensional position variable t；
According to the estimated value of the spin matrix R and the estimated value of the three-dimensional position variable t, the pose is determined.
Above-mentioned method optionally determines first plane and the matched process of the second plane, comprising:
Determining first plane and second plane the first preset condition of satisfaction and first plane and institute
In the case where stating the second plane the second preset condition of satisfaction, first plane and second plane matching are determined；
Wherein, first preset condition includes: the normal vector of first plane and the normal vector of second plane
In parallel；
Second preset condition include: any point in first plane with it is any one in second plane
Line between point is vertical with the normal vector of first plane or second plane.
Above-mentioned method optionally determines and the matched process of the second point at described first point, comprising:
Extract the characteristic point in the characteristic point and the second frame cromogram in first frame cromogram；
Determine described first point corresponding fisrt feature point and the corresponding second feature point of the second point；
According to the fisrt feature point, it is calculated the first description, and according to the second feature point, is calculated the
Two description；
In the case that distance between the first description and the second description is less than preset threshold, institute is determined
State fisrt feature point and the matching of second feature point；
Under the fisrt feature point and the matched situation of second feature point, and described second are determined at described first point
Point matching.
A kind of determining device of pose, comprising:
Point cloud acquiring unit, for a cloud, the cromogram and the depth to be calculated according to cromogram and depth map
Figure is obtained by acquisition same target；
Plane determination unit, for extracting the plane in described cloud；
Pose determination unit, for according to matching plane pair or, the matching plane to and matching double points, determine position
Appearance, any one of matching plane centering include matched first plane and the second plane, and first plane is first frame
The plane of data reduction, second plane are the plane of the second frame data reduction；It include matching in any one matching double points
First point and second point, described first point is the point in the first frame point cloud, and the second point is the second frame point cloud
In point.
Above-mentioned device, optionally, further includes:
Pose optimizes unit, for minimizing between true point and estimation point by using default first object function
Distance, and using default second objective function, minimize the geometric error between true planar and estimation plane, optimized
Pose afterwards；Wherein, the coordinate really put is the actual coordinate for constituting the 3D point of object under world coordinate system；Institute
The coordinate for stating estimation point is the point in the point cloud being calculated according to the cromogram and the depth map, is become by the pose
Change to the coordinate under the world coordinate system；The equation of the true planar is the practical side of the plane under the world coordinate system
Journey；The equation of the estimation plane is the plane extracted in a cloud, is transformed under the world coordinate system by the pose
The equation of plane.
Above-mentioned device, optionally, the plane extracted in described cloud, comprising:
Determine the corresponding plane of default screening areas of point cloud, the corresponding plane of any one screening areas are as follows: the screening
The plane that the preset quantity point chosen in region determines；The plane that the quantity of interior point is met to preset condition, as the point
Plane in cloud, the interior point of any one plane are as follows: belong to the point of the plane in the corresponding screening areas of the plane, it is described to belong to
The point of the plane is the point for being less than preset threshold at a distance from the plane.
Above-mentioned device, optionally, the number of the Similarity matching plane pair are three；
The foundation matching plane pair, determines pose, comprising:
Obtain the plane V of the first frame data reductionj, the plane V' of the second frame data reductionj, wherein j=1,
2,3；Based on the plane VjWith the plane V'j, and the goal-selling function by minimizing spin matrix R obtains the rotation
The estimated value of torque battle array R；Based on the plane VjWith the plane V'j, and the predetermined linear by solving three-dimensional position variable t
Equation obtains the estimated value of the three-dimensional position variable t；Estimated value and the three-dimensional position variable according to the spin matrix R
The estimated value of t determines the pose.
Above-mentioned device, optionally, the number of the matching plane pair are less than three；
It is described according to matching plane to and matching double points, determine pose, comprising:
Obtain the plane V of the first frame data reductionj, the plane V' of the second frame data reductionj, the first frame
Point p in point cloudi, point p' in the second frame point cloudi, wherein i=1,2, j=1 or i=1, j=1,2；Based on the point
pi, the point p'i, the plane VjWith the plane V'j, and the goal-selling function by minimizing spin matrix R obtains institute
State the estimated value of spin matrix R；Based on the point pi, the point p'i, the plane VjWith the plane V'j, in conjunction with the rotation
The estimated value of torque battle array R, and the goal-selling function by minimizing three-dimensional position variable t obtains the three-dimensional position variable t
Estimated value；According to the estimated value of the spin matrix R and the estimated value of the three-dimensional position variable t, the pose is determined.
Above-mentioned device, optionally, the pose determination unit are also used to:
Determining first plane and second plane the first preset condition of satisfaction and first plane and institute
In the case where stating the second plane the second preset condition of satisfaction, first plane and second plane matching are determined；Wherein, institute
State the first preset condition include: first plane normal vector it is parallel with the normal vector of second plane；Described second is pre-
If condition include: line between any point on any point and second plane in first plane with it is described
The normal vector of first plane or second plane is vertical.
Above-mentioned device, optionally, the pose determination unit are also used to:
Extract the characteristic point in the characteristic point and the second frame cromogram in first frame cromogram；Determine first point of correspondence
Fisrt feature point and the corresponding second feature point of the second point；According to the fisrt feature point, the first description is calculated
Son, and according to the second feature point, the second description is calculated；Son is described in the first description and described second
Between distance be less than preset threshold in the case where, determine the fisrt feature point and the second feature point matching；Described
Under one characteristic point and the matched situation of second feature point, and second point matching are determined at described first point.
A kind of odometer, including processor and memory, the memory is for storing program, and the processor is for transporting
Line program, to realize the determination method of above-mentioned pose.
The determination method, apparatus and odometer of a kind of pose provided by the invention are calculated according to cromogram and depth map
To a cloud, cromogram and depth map are obtained by acquisition same target, extract the plane in point cloud, according to matching plane to or
Person, matching plane to and matching double points, determine pose, wherein any one matching plane centering include matched first plane
With the second plane, the first plane is the plane of first frame data reduction, and the second plane is the plane of the second frame data reduction；Arbitrarily
It include matched first point and second point in one matching double points, first point is the point in first frame point cloud, second point second
Point in frame point cloud.It can be seen that two frame point clouds are calculated according to two frame cromograms and depth map, and according to 2 frame points in the present invention
The plane that cloud respectively extracts determines pose, because plane is compared to point, it is not easy to be influenced by spot noise, can effectively improve position
The accuracy of appearance.
Detailed description of the invention
In order to more clearly explain the embodiment of the invention or the technical proposal in the existing technology, to embodiment or will show below
There is attached drawing needed in technical description to be briefly described, it should be apparent that, the accompanying drawings in the following description is only this
The embodiment of invention for those of ordinary skill in the art without creative efforts, can also basis
The attached drawing of offer obtains other attached drawings.
Fig. 1 is a kind of flow diagram of the determination method of pose provided in an embodiment of the present invention；
Fig. 2 is the flow diagram of the determination method of another pose provided in an embodiment of the present invention；
Fig. 3 is a kind of process signal of specific implementation for extracting the plane in point cloud provided in an embodiment of the present invention
Figure；
Fig. 4 is a kind of flow diagram of method for calculating pose provided in an embodiment of the present invention；
Fig. 5 is the flow diagram of another method for calculating positioning appearance provided in an embodiment of the present invention；
Fig. 6 is a kind of flow diagram of judgment method for matching plane pair provided in an embodiment of the present invention；
Fig. 7 is a kind of flow diagram of the judgment method of matching double points provided in an embodiment of the present invention；
Fig. 8 is the flow diagram of the determination method of another pose provided in an embodiment of the present invention；
Fig. 9 is a kind of structural schematic diagram of the determining device of pose provided in an embodiment of the present invention；
Figure 10 is a kind of structural schematic diagram of odometer provided in an embodiment of the present invention.
Specific embodiment
Following will be combined with the drawings in the embodiments of the present invention, and technical solution in the embodiment of the present invention carries out clear, complete
Site preparation description, it is clear that described embodiments are only a part of the embodiments of the present invention, instead of all the embodiments.It is based on
Embodiment in the present invention, it is obtained by those of ordinary skill in the art without making creative efforts every other
Embodiment shall fall within the protection scope of the present invention.
As shown in Figure 1, being a kind of flow diagram of the determination method of pose provided in an embodiment of the present invention, including as follows
Step:
S101: according to cromogram and depth map, a cloud is calculated.
Wherein, cromogram and depth map are obtained by acquisition same target, and therefore, cromogram and depth map are corresponding frame by frame
, a frame point cloud can be calculated in corresponding frame cromogram and a frame depth map.
It should be noted that depth map refers to: the distance dependent on the surface comprising the scenario objects with photograph viewpoint
The image or image channel of information.Wherein, depth map is similar to gray level image, and only each pixel value of depth map is in camera
Sensor distance object actual range.
It should be noted that in embodiments of the present invention, the acquisition modes of cromogram and depth map include but is not limited to: logical
The shooting of RGBD camera is crossed to be obtained.For example, shooting any one object in reality using RGBD camera, RGBD phase chance synchronizes life
At the cromogram and depth map of the object.
It should be noted that point cloud refer specifically to: by camera acquire map environment and record in dots
In the picture, each point includes the three-dimensional coordinate based on 3D coordinate system, and a other point may contain colouring information, distance letter
Breath or Reflection intensity information.In embodiments of the present invention, it can be calculated from cromogram and depth map by aperture camera model
A cloud is obtained, specific calculation is as follows:
Z=z (3)
Wherein, X, Y, Z are the current any point of cromogram object collected in reality in world coordinate system
Coordinate, the i.e. coordinate of point in point cloud.X, y is coordinate of the current point in image coordinate system in cromogram.Z is depth a little
Value, obtains from depth map.Cx, cy are coordinate of the camera photocentre in image coordinate system.Fx, fy be camera focus horizontal and
The length of vertical direction.
It should be noted that world coordinate system refers to: after vision SLAM is initialized successfully, first frame cromogram and depth
Scheme corresponding camera coordinates system.The frame of reference of the coordinate system as current pose.
It should be noted that camera coordinates system refers to: camera using the focusing center of itself as origin, build for Z axis by optical axis
Vertical three-dimensional cartesian coordinate system, the X-axis and Y-axis for the image that the X-axis and Y-axis of the three-dimensional cartesian coordinate system are generated with camera respectively are flat
Row, the position of any pixel on image is described using the three-dimensional cartesian coordinate system, which is known as camera seat
Mark system.
S102: the plane in point cloud is extracted.
Wherein, since the number of the point in cloud is more huge, pass through the number for the plane that three-dimensional planar equation calculation obtains
Mesh also can be more.In embodiments of the present invention, optimal planar can be selected from multiple planes, as the plane in cloud.
It should be noted that optimal planar refers specifically to: calculating the plane of each point and extraction in present frame point cloud
Distance, then think that the point is the interior point of the plane less than preset threshold.And compare each plane of present frame data reduction
Interior quantity, the interior most plane of quantity is exactly optimal planar.
It should be noted that the specific number of optimal planar can be selected according to the actual situation by technical staff, this hair
Bright embodiment is without limitation.
S103: according to matching plane pair or matching plane to and matching double points, determine pose.
Wherein, any one of matching plane centering includes matched first plane and the second plane, and described first is flat
Face is the plane of first frame data reduction, and second plane is the plane of the second frame data reduction；Any one matching double points
In include matched first point and second point, described first point is the point in the first frame point cloud, and the second point is described
Point in second frame point cloud.
Specifically, in the case where the quantity for matching plane pair is not less than three, according to matching plane to determining pose, tool
Body implementation process can be found in shown in Fig. 4.Match plane pair quantity less than three in the case where, according to matching plane to
With point pair, pose is determined, specific implementation flow can be found in shown in Fig. 5.
In embodiments of the present invention, according to cromogram and depth map, a cloud, cromogram and depth map is calculated by adopting
Collect same target obtain, extract point cloud in plane, according to matching plane pair or, matching plane to and matching double points, determine
Pose.Wherein, any one matching plane centering includes matched first plane and the second plane, and the first plane is first frame point
The plane that cloud extracts, the second plane are the plane of the second frame data reduction；It include matched first in any one matching double points
Point and second point, first point is the point in first frame point cloud, and second point is the point in the second frame point cloud.It can be seen that in the present invention, root
Two frame point clouds are calculated according to two frame cromograms and depth map, and the plane respectively extracted according to two frame point clouds determines pose, because
Plane is compared to point, it is not easy to be influenced by spot noise, can effectively improve the accuracy of pose.
It should be noted that for determining the demand of pose in real time, it can be by coloured silk collected before current time
A cloud and plane is calculated as process object in chromatic graph and depth map, with further acquisitions match plane to and matching double points,
And determine the pose at current time.
Compared to Figure 1, the embodiment of the present invention can also further be optimized for obtained pose in Fig. 1, be obtained
The higher pose of accuracy.
Optionally, Fig. 2 is the flow diagram of the determination method of another pose provided in an embodiment of the present invention, including such as
Lower step:
S201: according to cromogram and depth map, a cloud is calculated.
Wherein, the specific implementation procedure and realization principle of S201 and the specific implementation procedure of above-mentioned S101 shown in fig. 1 and
Realization principle is consistent, and which is not described herein again.
S202: the plane in point cloud is extracted.
Wherein, the specific implementation procedure and realization principle of S202 and the specific implementation procedure of above-mentioned S102 shown in fig. 1 and
Realization principle is consistent, and which is not described herein again.
S203: according to matching plane pair or, matching plane to and matching double points, determine pose.
Wherein, the specific implementation procedure and realization principle of S203 and the specific implementation procedure of above-mentioned S103 shown in fig. 1 and
Realization principle is consistent, and which is not described herein again.
S204: by using default first object function, the distance between true point and estimation point are minimized, and is used
Default second objective function, minimizes the geometric error between true planar and estimation plane, the pose after being optimized.
Wherein, the coordinate really put is the actual coordinate for constituting the 3D point of object under world coordinate system.Estimation point
Coordinate be calculated according to cromogram and depth map point cloud in point, the seat under world coordinate system is transformed to by pose
Mark.The equation of true planar is the actual equations of the plane under world coordinate system.The equation of estimation plane is extracted in a cloud
Plane transforms to the equation under world coordinate system by pose.
First object function specifically:
pi=(xi,yi,zi)TFor true point, xi,yi,ziReally to put the actual coordinate in world coordinate system,To estimate point, RkFor the spin matrix of kth frame,For really point piIn kth frame point cloud
Obtained measured value is calculated according to kth frame cromogram and depth map,It is the point in kth frame point cloud in camera coordinates
Coordinate in system, tkThe three-dimensional position variable of camera when to carry out kth frame shooting, i are index corresponding to 3D point, and k is any
Frame cromogram and the corresponding frame number of depth map, m are the corresponding index of point in point cloud, k=1,2,3 ..., i=1,2,3 ..., m=
1,2,3…。
Second objective function specifically:
It should be noted that πj=(aj,bj,cj,dj)TFor true planar, aj,bj,cj,djIt is sat for true planar in the world
The actual equations of plane in mark system,For estimation plane,It is any in the plane of kth frame data reduction
One sampled point, j be true planar corresponding to index, s be sampled point corresponding to index, j=1,2,3 ..., s=1,2,
3….It should be noted that in the embodiment of the present invention, not calculated between true planar and estimation plane to reduce error
Distance, but linear equation is constructed according to the distance between sampled point and true planar, to obtain true planar and estimation
Geometric error between plane.
Specifically, using the obtained pose of S203 as the initial pose of first object function and the second objective function, benefit
With the sparsity resolver of Eigen math library and Gauss-Newton iterative method to first object function and the second objective function into
Row calculates, and obtains RkOptimal value and tkOptimal value, and by RkOptimal value and tkOptimal value as optimization after pose.
In embodiments of the present invention, according to cromogram and depth map, a cloud, cromogram and depth map is calculated by adopting
Collect same target to obtain.Extract point cloud in plane, according to matching plane pair or, matching plane to and matching double points, determine
Pose.By using default first object function, the distance between true point and estimation point are minimized, and uses default second
Objective function minimizes the geometric error between true planar and estimation plane, the pose after being optimized.It can be seen that the present invention
In, two frame point clouds are calculated according to two frame cromograms and depth map, and the plane respectively extracted according to two frame point clouds determines position
Appearance, because plane is compared to point, it is not easy to be influenced by spot noise, the accuracy of pose can be effectively improved.In addition, also passing through
Default first object function and default second objective function further optimize pose, effectively improve the accurate of pose
Property.
Optionally, Fig. 3 shows the specific implementation for extracting the plane in point cloud, includes the following steps:
S301: the corresponding plane of default screening areas of point cloud is determined.
Wherein, the corresponding plane of any one screening areas are as follows: the preset quantity point chosen in the screening areas determines
Plane.Further, interior quantity of the corresponding plane of any one screening areas meets preset requirement.Any one plane
Interior point are as follows: the point for belonging to the plane in the corresponding screening areas of the plane, belong to the plane point be at a distance from the plane
Less than the point of preset threshold.
It should be noted that the specific size of preset threshold is configured according to the actual situation by technical staff, the present invention
Embodiment is without limitation.
The point and the calculation of the distance between the plane include but is not limited to: putting as (x0,y0,z0), plane Ax+
By+Cz+D=0 obtains the distance between the point and plane d based on formula (6)0.If d0Less than preset threshold, then point (x0,
y0,z0) be the plane interior point.
Formula (6) specifically:
It should be noted that interior quantity of the corresponding plane of any one screening areas meets preset requirement includes but not
Be limited to: this process of plane is calculated in the preset quantity point that circulation is repeatedly chosen from the screening areas, obtains multiple flat
Interior quantity of face, acquisition and more each plane, using the most plane of interior quantity as the corresponding plane of the screening areas.
Specifically, handling using aperture camera model cromogram and depth map, a cloud is calculated.Use 10
The sliding window of × 10 pixels traverses the depth map of 100 × 100 pixels, obtains the identical screening areas of 10 elemental areas.From
Three points are randomly selected in the point cloud that any one screening areas is covered, solve three-dimensional planar equation ax+by+ based on three points
Cz=d obtains plane π1.It for single screening areas, recycles 10 times and randomly selects three points, and solve three-dimensional planar equation ax
+ by+cz=d obtains plane πi, i=1,2,3 ... 10.Obtain interior quantity of each plane, wherein plane π1Interior quantity
It is 20, plane π2Interior quantity be 19, plane π3Interior quantity be 18, plane π4Interior quantity be 17, plane π5It is interior
Point quantity is 16, plane π6Interior quantity be 15, plane π7Interior quantity be 14, plane π8Interior quantity be 13, plane
π9Interior quantity be 12, plane π10Interior quantity be 11.Compare the interior quantity of 10 planes, it is known that plane π1Interior point
Quantity is most, therefore by plane π1As the corresponding plane of the screening areas.Similarly, it is successively selected from remaining nine screening areas
Nine planes.
It should be noted that above-mentioned specific implementation process is used only for for example, π is used only for characterization plane.
It should be noted that in embodiments of the present invention, depth map is traversed using the sliding window of M × M pixel, thus
To multiple screening areas.Wherein, specific size of the M greater than 3, M is configured according to the actual situation by technical staff.
Depth map is being traversed using the sliding window of M × M pixel, is being obtained in multiple screening areas, each pixel conduct
An individual point.The N number of pixel randomly selected from single screening areas carries out solving three-dimensional planar equation, thus
To single plane.Wherein, N is greater than 2 and is less than M × M, and the specific size of N is configured according to the actual situation by technical staff, this
Inventive embodiments are without limitation.
S302: interior quantity is met to the plane of preset condition, as the plane in cloud.
Wherein, the corresponding plane of each screening areas selects optimal planar to make according to preset condition from multiple planes
For the plane in cloud.
It should be noted that the preset condition includes but is not limited to be: in each corresponding plane of screening areas, depositing
The case where plane repeats, duplicate plane is filtered out, remaining mutually different multiple planes are respectively right according to remaining plane
The frequency of occurrence answered is ranked up remaining plane, selects previous position or front two or front three in sequence
Plane as the plane for meeting preset condition.
Specifically, traversing the depth map of 100 × 100 pixels using the sliding window of 10 × 10 pixels, obtaining 10 pixels
The identical screening areas of area, and an optimal planar is obtained from 10 screening areas from respective, respectively obtain plane π1, plane
π2, plane π3, plane π4, plane π5, plane π6, plane π7, plane π8, plane π9With plane π10.Wherein, plane π1, plane π2、
Plane π3With plane π4For identical plane (n1 T, d1)T, plane π5, plane π6With plane π7For identical plane (n2 T, d2)T, put down
Face π8With plane π9For identical plane (n3 T, d3)T, plane π10For (n4 T, d4)T, nTThe normal vector of plane is characterized, d characterizes coordinate
Distance of the origin to plane.Filter out the plane repeated, remaining plane (n1 T, d1)T, plane (n2 T, d2)T, plane (n3 T, d3)T
With plane (n4 T, d4)T.Wherein, plane (n1 T, d1)TThe number repeated is 4 times, plane (n2 T, d2)TThe number repeated
It is 3 times, plane (n3 T, d3)TThe number repeated is 2 times, plane (n4 T, d4)TOnly occurred primary.It as a result, will be remaining each
A plane is ranked up, and obtaining sequence is plane (n1 T, d1)T, plane (n2 T, d2)T, plane (n3 T, d3)TWith plane (n4 T, d4)T。
It is plane (n respectively as the plane in point cloud from the plane of front three is selected in sequence1 T, d1)T, plane (n2 T, d2)TAnd plane
(n3 T, d3)T。
It should be noted that above-mentioned specific implementation process is used only for illustrating.
In embodiments of the present invention, the corresponding plane of default screening areas for determining point cloud will meet the flat of preset condition
Face, as the plane in cloud.Wherein, the corresponding plane of any one screening areas are as follows: that chooses in the screening areas is default
The plane that quantity point determines, the interior point of any one plane are as follows: belong to the point of the plane in the corresponding screening areas of the plane,
The point for belonging to the plane is the point for being less than preset threshold at a distance from the plane.It can be seen that in the present invention, according to default screening areas
Multiple planes are calculated, and according to preset condition, filters out out optimal planar as the plane in point cloud, guarantees to finally screen
Plane representativeness out is prominent.
Optionally, the situation that the number for matching plane pair is three, Fig. 4 show the specific implementation side of above-mentioned S103
The flow diagram of formula, includes the following steps:
S401: the plane V of first frame data reduction is obtainedj, the plane V' of the second frame data reductionj。
Wherein, j=1,2,3.In embodiments of the present invention, first frame point cloud and the second frame point cloud are two frames of arbitrary continuation
Cromogram and depth map corresponding to point cloud.
S402: it is based on plane VjWith plane V'j, and the goal-selling function by minimizing spin matrix R obtains spin moment
The estimated value of battle array R.
Wherein, the goal-selling function of spin matrix R is specifically as follows:
It should be noted that njFor plane VjNormal vector, n'jFor plane V'jNormal vector.
It should be noted that minimizing to the objective function about spin matrix R, it is calculated spin matrix R's
Estimated value
S403: it is based on plane VjWith plane V'j, and the predetermined linear equation by solving three-dimensional position variable t obtains three-dimensional
The estimated value of location variable t.
Wherein, the predetermined linear equation of three-dimensional position variable t is specifically as follows:
n'j TT=dj-d'j (8)
It should be noted that djFor plane VjIn distance of the coordinate origin to plane, d'jFor plane V'jIn coordinate it is former
Point arrives the distance of plane, and T is for characterizing transposed matrix.
It should be noted that solving to the linear equation about three-dimensional position variable t, three-dimensional position variable t is obtained
Estimated value
S404: according to the estimated value of spin matrix R and the estimated value of three-dimensional position variable t, pose is determined.
Wherein, according to the estimated value of spin matrix RWith the estimated value of three-dimensional position variable tDetermine pose
In embodiments of the present invention, pass through the plane V of acquisition first frame data reductionj, the plane of the second frame data reduction
V'j.Based on plane VjWith plane V'j, and the goal-selling function by minimizing spin matrix R obtains the estimation of spin matrix R
Value.Based on plane VjWith plane V'j, and the predetermined linear equation by solving three-dimensional position variable t obtains three-dimensional position variable t
Estimated value.According to the estimated value of spin matrix R and the estimated value of three-dimensional position variable t, pose is determined.It can be seen that in the present invention,
The plane respectively extracted according to two frame point clouds determines pose, because plane is compared to point, it is not easy to it is influenced by spot noise, it can
Effectively improve the accuracy of pose.
Optionally, the number for matching plane pair is less than three situations, and Fig. 5 shows the specific implementation of above-mentioned S103
The flow diagram of mode, includes the following steps:
S501: the plane V of first frame data reduction is obtainedj, the plane V' of the second frame data reductionj, in first frame point cloud
Point pi, point p' in the second frame point cloudi。
Wherein, i=1,2, j=1 or i=1, j=1,2.In embodiments of the present invention, first frame point cloud and the second frame point cloud
Point cloud corresponding to two frame cromograms and depth map for arbitrary continuation.
S502: it is based on point pi, point p'i, plane VjWith plane V'j, and the goal-selling letter by minimizing spin matrix R
Number obtains the estimated value of spin matrix R.
Wherein, the goal-selling function of spin matrix R is specifically as follows:
It should be noted that wjRefer to pre-set weight between plane that two frame point clouds respectively extract, weight
Specific value can be configured according to the actual situation by technical staff.
It should be noted that minimizing to the objective function about spin matrix R, it is calculated spin matrix R's
Estimated value
S503: it is based on point pi, point p'i, plane VjWith plane V'j, in conjunction with the estimated value of spin matrix R, and pass through minimum
The goal-selling function of three-dimensional position variable t obtains the estimated value of three-dimensional position variable t.
Wherein, the goal-selling function of three-dimensional position variable t is specifically as follows:
It should be noted that M is the number of corresponding point,
It should be noted that minimizing to the objective function about three-dimensional position variable t, three-dimensional position variable is obtained
The estimated value of t
S504: according to the estimated value of spin matrix R and the estimated value of three-dimensional position variable t, pose is determined.
Wherein, according to the estimated value of spin matrix RWith the estimated value of three-dimensional position variable tDetermine pose
In embodiments of the present invention, pass through the plane V of acquisition first frame data reductionj, the plane of the second frame data reduction
V'j, point p in first frame point cloudi, point p' in the second frame point cloudi.Based on point pi, point p'i, plane VjWith plane V'j, and lead to
The goal-selling function for crossing minimum spin matrix R obtains the estimated value of spin matrix R.Based on point pi, point p'i, plane VjPeace
Face V'j, in conjunction with the estimated value of spin matrix R, and the goal-selling function by minimizing three-dimensional position variable t obtains three-dimensional position
Set the estimated value of variable t.According to the estimated value of spin matrix R and the estimated value of three-dimensional position variable t, pose is determined.It can be seen that
In invention, the point in plane and two frames point cloud respectively extracted according to two frame point clouds determines pose, because plane is compared to point, does not allow
Vulnerable to the influence of spot noise, the accuracy of pose can be effectively improved.
Optionally, for the determination process of the above-mentioned matching plane pair referred to, the first plane of determination and that Fig. 6 is shown
The flow diagram of the matched specific implementation of two planes, includes the following steps:
S601: judge whether the first plane and the second plane meet the first preset condition.
Wherein, the first preset condition includes: that the normal vector of the first plane is parallel with the normal vector of the second plane.If first is flat
Face and the second plane meet the first preset condition, execute S602.
S602: judge whether the first plane and the second plane meet the second preset condition.
Wherein, the second preset condition includes: between any point on any point and the second plane in the first plane
Line it is vertical with the normal vector of the first plane or the second plane.If the first plane and the second plane meet the second preset condition,
Execute S603.
S603: the first plane and the matching of the second plane are determined.
Wherein, in the same plane according to two planes known to the first preset condition and the second preset condition, so can
To determine two plane matchings.
In embodiments of the present invention, determining that the first plane and the second plane meet the first preset condition and first flat
In the case that face and the second plane meet the second preset condition, determines the first plane and the second plane is approximately the same plane, first
Plane and the matching of the second plane.Wherein, the first preset condition includes: that the normal vector of the first plane and the normal vector of the second plane are put down
Row；Second preset condition includes: the line and between any point on any point and the second plane in the first plane
The normal vector of one plane or the second plane is vertical.It can be seen that in the present invention, it can be according to the first preset condition and the second preset condition
Judge whether match between two planes.
Optionally, for the determination process of the above-mentioned matching double points referred to, first point of judgement and second point that Fig. 7 is shown
Whether the flow diagram of matched specific implementation, include the following steps:
S701: the characteristic point in the characteristic point and the second frame cromogram in first frame cromogram is extracted.
Wherein, the specific method for extracting the characteristic point in cromogram includes but is not limited to: by OpenCV computer vision
Extract the characteristic point in cromogram in library.
It should be noted that characteristic point refers specifically to: gray value of image occurs the point of acute variation or on image side
The biggish point of curvature on edge.
S702: first point of corresponding fisrt feature point and the corresponding second feature point of second point are determined.
Wherein, in embodiments of the present invention, cromogram and depth map are all via after registration process.Therefore, according to colour
Figure and depth map calculate obtained point Yun Zhongdian, and from there are corresponding relationships between characteristic point extracted in cromogram.
S703: according to fisrt feature point, being calculated the first description, and according to second feature point, is calculated second
Description.
Wherein, according to the common approach that the calculating process that description is calculated in characteristic point is in art technology, here
It repeats no more.
It should be noted that description refers to: portraying a data structure of characteristic point, wherein each characteristic point is all
Description is corresponded to.
S704: judge that the first description and second describes whether the distance between son is less than preset threshold.
Wherein, the specific size of preset threshold can be configured according to the actual situation by technical staff, the embodiment of the present invention
Without limitation.If the first description and second describes the distance between son less than preset threshold, S705 is executed.
S705: fisrt feature point and the matching of second feature point are determined.
Wherein, after determining fisrt feature point and the matching of second feature point, S706 is continued to execute.
S706: and second point matching are determined at first point.
In embodiments of the present invention, the characteristic point in the characteristic point and the second frame cromogram in first frame cromogram is extracted,
It determines first point of corresponding fisrt feature point and the corresponding second feature point of second point, according to fisrt feature point, is calculated
One description, and according to second feature point, the second description is calculated.First description son second description son between away from
In the case where being less than preset threshold, fisrt feature point and the matching of second feature point are determined.In fisrt feature point and second feature
In the matched situation of point, and second point matching are determined at first point.It can be seen that in the present invention, according to fisrt feature point and second feature point
Between match condition, to determine the match condition of and second point at first point, if fisrt feature point is matched with second feature point,
Determine and second point matching at first point.
Compared with the determination method of pose shown in fig. 1, in embodiments of the present invention, it is understood that there may be of matching plane pair
The case where number is zero.
Optionally, as shown in figure 8, the process signal of the determination method for another pose provided in an embodiment of the present invention
Figure, includes the following steps:
S801: according to cromogram and depth map, a cloud is calculated.
Wherein, the specific implementation procedure and realization principle of S801 and the specific implementation procedure of above-mentioned S101 shown in fig. 1 and
Realization principle is consistent, and which is not described herein again.
S802: the plane in point cloud is extracted.
Wherein, the specific implementation procedure and realization principle of S802 and the specific implementation procedure of above-mentioned S102 shown in fig. 1 and
Realization principle is consistent, and which is not described herein again.
S803: judge match plane pair number whether non-zero.
Wherein, if the number non-zero of matching plane pair, executes S804, otherwise execute S805.
S804: according to matching plane pair or, matching plane to and matching double points, determine pose.
Wherein, the specific implementation procedure and realization principle of S804 and the specific implementation procedure of above-mentioned S103 shown in fig. 1 and
Realization principle is consistent, and which is not described herein again.
S805: according to the point in two frame point clouds, pose is determined.
Wherein, based on the point p in first frame point cloudi, point p' in the second frame point cloudi, construct the target letter of spin matrix R
Several and three-dimensional position variable t linear equation.
The objective function of spin matrix R are as follows:
Wherein,I=1,2,3 ... M, M
For positive integer.By minimizing the objective function of spin matrix R, the estimated value of spin matrix R is obtained
The linear equation of three-dimensional position variable t are as follows:
Wherein, by solving the linear equation of three-dimensional position variable t, the estimated value of three-dimensional position variable t is obtained
Further, according to the estimated value of spin matrix RWith the estimated value of three-dimensional position variable tDetermine pose
In embodiments of the present invention, according to cromogram and depth map, a cloud, cromogram and depth map is calculated by adopting
Collect same target to obtain, extracts the plane in point cloud.According to matching plane pair or, matching plane to and matching double points or
Person, matching double points determine pose.Wherein, any one matching plane centering includes matched first plane and the second plane, the
One plane is the plane of first frame data reduction, and the second plane is the plane of the second frame data reduction；Any one matching double points
In include matched first point and second point, first point is the point in first frame point cloud, and second point is the point in the second frame point cloud.
It can be seen that two frame point clouds are calculated according to two frame cromograms and depth map in the present invention, and respectively extracted according to two frame point clouds
Plane determines pose, because plane is compared to point, it is not easy to be influenced by spot noise, can effectively improve the accuracy of pose.
Optionally, a kind of determination method of the pose provided in conjunction with the embodiments of the present invention, the embodiment of the present invention are also right
A kind of determining device of pose should be provided, as shown in figure 9, being a kind of determining device of pose provided in an embodiment of the present invention
Structural schematic diagram, the device include:
Point acquiring unit 100, for a cloud being calculated, cromogram and depth map pass through according to cromogram and depth map
Same target is acquired to obtain.
Plane determination unit 200, for extracting the plane in some clouds.
Pose determination unit 300, for according to matching plane pair or, matching plane to and matching double points, determine position
Appearance, any one matching plane centering include matched first plane and the second plane, and the first plane is first frame data reduction
Plane, the second plane be the second frame data reduction plane.It include matched first point and the in any one matching double points
2 points, first point is the point in first frame point cloud, and second point is the point in the second frame point cloud.
Pose optimizes unit 400, for minimizing between true point and estimation point by using default first object function
Distance, and using default second objective function, minimize the geometric error between true planar and estimation plane, obtain excellent
Pose after change.Wherein, the coordinate really put is the actual coordinate for constituting the 3D point of object under world coordinate system.Estimation
The coordinate of point is the point in the point cloud being calculated according to cromogram and depth map, is transformed under world coordinate system by pose
Coordinate.The equation of true planar is the actual equations of the plane under world coordinate system.The equation of estimation plane is extracted in a cloud
Plane, the equation under world coordinate system is transformed to by pose.
Wherein, the specific implementation that plane determination unit 200 extracts the plane in point cloud comprises determining that the default of a cloud
The corresponding plane of screening areas, the corresponding plane of any one screening areas are as follows: the preset quantity chosen in the screening areas
The determining plane of point.The plane that the quantity of interior point is met to preset condition, as the plane in cloud, any one plane it is interior
Point are as follows: the point for belonging to the plane in the corresponding screening areas of the plane, the point for belonging to the plane are to be less than at a distance from the plane
The point of preset threshold.
Pose determination unit 300 determines that the specific implementation of pose includes: to obtain first frame point according to matching plane pair
The plane V that cloud extractsj, the plane V' of the second frame data reductionj, wherein j=1,2,3.Based on plane VjWith plane V'j, and lead to
The goal-selling function for crossing minimum spin matrix R obtains the estimated value of spin matrix R.Based on plane VjWith plane V'j, and lead to
The predetermined linear equation for crossing solution three-dimensional position variable t obtains the estimated value of three-dimensional position variable t；According to estimating for spin matrix R
The estimated value of evaluation and three-dimensional position variable t, determines pose.
Pose determination unit 300 according to matching plane to and matching double points, determine that the specific implementation of pose includes: to obtain
Take the plane V of first frame data reductionj, the plane V' of the second frame data reductionj, point p in first frame point cloudi, the second frame point cloud
In point p'i, wherein i=1,2, j=1 or i=1, j=1,2；Based on point pi, point p'i, plane VjWith plane V'j, and by most
The goal-selling function of smallization spin matrix R obtains the estimated value of spin matrix R；Based on point pi, point p'i, plane VjAnd plane
V'j, in conjunction with the estimated value of spin matrix R, and the goal-selling function by minimizing three-dimensional position variable t obtains three-dimensional position
The estimated value of variable t；According to the estimated value of spin matrix R and the estimated value of three-dimensional position variable t, pose is determined.
Pose determination unit 300 determine the specific implementation of the first plane and the matched process of the second plane include:
Determine that the first plane and the second plane meet the first preset condition and the first plane and the second plane meets the second preset condition
In the case where, determine the first plane and the matching of the second plane.Wherein, the first preset condition include: the first plane normal vector with
The normal vector of second plane is parallel.Second preset condition include: any point in the first plane with it is any in the second plane
Line between a little is vertical with the normal vector of the first plane or the second plane.
Pose determination unit 300 determines that the specific implementation of and the matched process of second point includes: extraction the at first point
The characteristic point in characteristic point and the second frame cromogram in one frame cromogram；Determine first point of corresponding fisrt feature point and second
The corresponding second feature point of point.According to fisrt feature point, the first description is calculated, and according to second feature point, calculates
To the second description.In the case that distance between the first description and the second description is less than preset threshold, first is determined
Characteristic point and the matching of second feature point；Under fisrt feature point and the matched situation of second feature point, and second are determined at first point
Point matching.
In embodiments of the present invention, according to cromogram and depth map, a cloud, cromogram and depth map is calculated by adopting
Collect same target obtain, extract point cloud in plane, according to matching plane pair or, matching plane to and matching double points, determine
Pose.Wherein, any one matching plane centering includes matched first plane and the second plane, and the first plane is first frame point
The plane that cloud extracts, the second plane are the plane of the second frame data reduction；It include matched first in any one matching double points
Point and second point, first point is the point in first frame point cloud, and second point is the point in the second frame point cloud.It can be seen that in the present invention, root
Two frame point clouds are calculated according to two frame cromograms and depth map, and the plane respectively extracted according to two frame point clouds determines pose, because
Plane is compared to point, it is not easy to be influenced by spot noise, can effectively improve the accuracy of pose.
Optionally, the present invention also provides a kind of odometer, which includes processor 500 and memory 600, storage
Device 600 is for storing program, and processor 500 is for running program, to realize the determination method of pose in the embodiment of the present invention.
All the embodiments in this specification are described in a progressive manner, the portion of identical match between each embodiment
Dividing may refer to each other, and each embodiment focuses on the differences from other embodiments.Especially for system or
For system embodiment, since it substantially matches embodiment of the method, so describing fairly simple, related place is referring to method
The part of embodiment illustrates.System and system embodiment described above is only schematical, wherein the conduct
The unit of separate part description may or may not be physically separated, component shown as a unit can be or
Person may not be physical unit, it can and it is in one place, or may be distributed over multiple network units.It can root
According to actual need that some or all of the modules therein is selected to achieve the purpose of the solution of this embodiment.Ordinary skill
Personnel can understand and implement without creative efforts.
Professional further appreciates that, unit described in conjunction with the examples disclosed in the embodiments of the present disclosure
And algorithm steps, can be realized with electronic hardware, computer software, or a combination of the two, in order to clearly demonstrate hardware and
The interchangeability of software generally describes each exemplary composition and step according to function in the above description.These
Function is implemented in hardware or software actually, the specific application and design constraint depending on technical solution.Profession
Technical staff can use different methods to achieve the described function each specific application, but this realization is not answered
Think beyond the scope of this invention.
The foregoing description of the disclosed embodiments enables those skilled in the art to implement or use the present invention.
Various modifications to these embodiments will be readily apparent to those skilled in the art, as defined herein
General Principle can be realized in other embodiments without departing from the spirit or scope of the present invention.Therefore, of the invention
It is not intended to be limited to the embodiments shown herein, and is to fit to and the principles and novel features disclosed herein phase one
The widest scope of cause.
Claims (15)
1. a kind of determination method of pose characterized by comprising
According to cromogram and depth map, a cloud, the cromogram and the depth map is calculated and is obtained by acquiring same target
?；
Extract the plane in described cloud；
According to matching plane pair or, the matching plane to and matching double points, determine pose, any one of matching is put down
It include matched first plane and the second plane in facing, first plane is the plane of first frame data reduction, described the
Two planes are the plane of the second frame data reduction；It is described including matched first point and second point in any one matching double points
First point is the point in the first frame point cloud, and the second point is the point in the second frame point cloud.
2. the method according to claim 1, wherein after the determining pose, further includes:
By using default first object function, the distance between true point and estimation point are minimized, and uses default second
Objective function minimizes the geometric error between true planar and estimation plane, the pose after being optimized；
Wherein, the coordinate really put is the actual coordinate for constituting the 3D point of object under world coordinate system；It is described to estimate
The coordinate for calculating point is the point in the point cloud being calculated according to the cromogram and the depth map, is transformed to by the pose
Coordinate under the world coordinate system；The equation of the true planar is the actual equations of the plane under world coordinate system；It is described
The equation of estimation plane is the plane extracted in a cloud, transforms to the equation under the world coordinate system by the pose.
3. the method according to claim 1, wherein the plane extracted in described cloud, comprising:
Determine the corresponding plane of default screening areas of point cloud, the corresponding plane of any one screening areas are as follows: the screening areas
The plane that the preset quantity point of middle selection determines；
The plane that the quantity of interior point is met to preset condition, as the plane in described cloud, the interior point of any one plane are as follows:
Belong to the point of the plane in the corresponding screening areas of the plane, the point for belonging to the plane is to be less than in advance at a distance from the plane
If the point of threshold value.
4. the method according to claim 1, wherein the number of the Similarity matching plane pair is three；
The foundation matching plane pair, determines pose, comprising:
Obtain the plane V of the first frame data reductionj, the plane V' of the second frame data reductionj, wherein j=1,2,3；
Based on the plane VjWith the plane V'j, and the goal-selling function by minimizing spin matrix R obtains the rotation
The estimated value of torque battle array R；
Based on the plane VjWith the plane V'j, and the predetermined linear equation by solving three-dimensional position variable t obtain it is described
The estimated value of three-dimensional position variable t；
According to the estimated value of the spin matrix R and the estimated value of the three-dimensional position variable t, the pose is determined.
5. the method according to claim 1, wherein the number of the matching plane pair is less than three；
It is described according to matching plane to and matching double points, determine pose, comprising:
Obtain the plane V of the first frame data reductionj, the plane V' of the second frame data reductionj, the first frame point cloud
In point pi, point p' in the second frame point cloudi, wherein i=1,2, j=1 or i=1, j=1,2；
Based on the point pi, the point p'i, the plane VjWith the plane V'j, and by minimizing the default of spin matrix R
Objective function obtains the estimated value of the spin matrix R；
Based on the point pi, the point p'i, the plane VjWith the plane V'j, in conjunction with the estimated value of the spin matrix R, and
Goal-selling function by minimizing three-dimensional position variable t obtains the estimated value of the three-dimensional position variable t；
According to the estimated value of the spin matrix R and the estimated value of the three-dimensional position variable t, the pose is determined.
6. the method according to claim 1, wherein determining that first plane and second plane are matched
Process, comprising:
Determining that first plane and second plane meet the first preset condition and first plane and described the
In the case that two planes meet the second preset condition, first plane and second plane matching are determined；
Wherein, first preset condition includes: that the normal vector of first plane is parallel with the normal vector of second plane；
Second preset condition include: any point in first plane and any point in second plane it
Between line it is vertical with the normal vector of first plane or second plane.
7. the method according to claim 1, wherein determining and the matched mistake of the second point at first point
Journey, comprising:
Extract the characteristic point in the characteristic point and the second frame cromogram in first frame cromogram；
Determine described first point corresponding fisrt feature point and the corresponding second feature point of the second point；
According to the fisrt feature point, the first description is calculated, and according to the second feature point, is calculated second and retouches
State son；
In the case that distance between the first description and the second description is less than preset threshold, described the is determined
One characteristic point and the matching of second feature point；
Under the fisrt feature point and the matched situation of second feature point, and the second point are determined at described first point
Match.
8. a kind of determining device of pose characterized by comprising
Point cloud acquiring unit, for a cloud according to cromogram and depth map, to be calculated, the cromogram and the depth map are logical
Cross the acquisition of acquisition same target；
Plane determination unit, for extracting the plane in described cloud；
Pose determination unit, for according to matching plane pair or, the matching plane to and matching double points, determine pose, times
One matching plane centering of meaning includes matched first plane and the second plane, and first plane is that first frame point cloud mentions
The plane taken, second plane are the plane of the second frame data reduction；It include matched first in any one matching double points
Point and second point, described first point is the point in the first frame point cloud, and the second point is the point in the second frame point cloud.
9. device according to claim 8, which is characterized in that further include:
Pose optimizes unit, for minimizing the distance between true point and estimation point by using default first object function,
And using default second objective function, the geometric error between true planar and estimation plane is minimized, after being optimized
Pose；Wherein, the coordinate really put is the actual coordinate for constituting the 3D point of object under world coordinate system；It is described to estimate
The coordinate for calculating point is the point in the point cloud being calculated according to the cromogram and the depth map, is transformed to by the pose
Coordinate under the world coordinate system；The equation of the true planar is the actual equations of the plane under the world coordinate system；
The equation of the estimation plane is the plane extracted in a cloud, transforms to the plane under the world coordinate system by the pose
Equation.
10. device according to claim 8, which is characterized in that the plane extracted in described cloud, comprising:
Determine the corresponding plane of default screening areas of point cloud, the corresponding plane of any one screening areas are as follows: the screening areas
The plane that the preset quantity point of middle selection determines；The plane that the quantity of interior point is met to preset condition, as in described cloud
Plane, the interior point of any one plane are as follows: belong to the point of the plane in the corresponding screening areas of the plane, described to belong to this flat
The point in face is the point for being less than preset threshold at a distance from the plane.
11. device according to claim 8, which is characterized in that the number of the Similarity matching plane pair is three；
The foundation matching plane pair, determines pose, comprising:
Obtain the plane V of the first frame data reductionj, the plane V' of the second frame data reductionj, wherein j=1,2,3；
Based on the plane VjWith the plane V'j, and the goal-selling function by minimizing spin matrix R obtains the spin moment
The estimated value of battle array R；Based on the plane VjWith the plane V'j, and the predetermined linear equation by solving three-dimensional position variable t
Obtain the estimated value of the three-dimensional position variable t；Estimated value and the three-dimensional position variable t according to the spin matrix R
Estimated value determines the pose.
12. device according to claim 8, which is characterized in that the number of the matching plane pair is less than three；
It is described according to matching plane to and matching double points, determine pose, comprising:
Obtain the plane V of the first frame data reductionj, the plane V' of the second frame data reductionj, the first frame point cloud
In point pi, point p' in the second frame point cloudi, wherein i=1,2, j=1 or i=1, j=1,2；Based on the point pi, institute
State point p'i, the plane VjWith the plane V'j, and the goal-selling function by minimizing spin matrix R obtains the rotation
The estimated value of torque battle array R；Based on the point pi, the point p'i, the plane VjWith the plane V'j, in conjunction with the spin moment
The estimated value of battle array R, and the goal-selling function by minimizing three-dimensional position variable t obtains estimating for the three-dimensional position variable t
Evaluation；According to the estimated value of the spin matrix R and the estimated value of the three-dimensional position variable t, the pose is determined.
13. device according to claim 8, which is characterized in that the pose determination unit is also used to:
Determining that first plane and second plane meet the first preset condition and first plane and described the
In the case that two planes meet the second preset condition, first plane and second plane matching are determined；Wherein, described
One preset condition includes: that the normal vector of first plane is parallel with the normal vector of second plane；The second default item
Part includes: the line and described first between any point on any point and second plane in first plane
The normal vector of plane or second plane is vertical.
14. device according to claim 8, which is characterized in that the pose determination unit is also used to:
Extract the characteristic point in the characteristic point and the second frame cromogram in first frame cromogram；Determine described first point corresponding
One characteristic point and the corresponding second feature point of the second point；According to the fisrt feature point, the first description is calculated, and
According to the second feature point, the second description is calculated；Between the first description and the second description
In the case that distance is less than preset threshold, the fisrt feature point and the matching of second feature point are determined；It is special described first
Under sign point and the matched situation of the second feature point, and second point matching are determined at described first point.
15. a kind of odometer, which is characterized in that including processor and memory, the memory is for storing program, the place
Reason device requires the determination method of pose described in any one of 1-7 with perform claim for running program.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910620817.1A CN110243390B (en) | 2019-07-10 | 2019-07-10 | Pose determination method and device and odometer |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910620817.1A CN110243390B (en) | 2019-07-10 | 2019-07-10 | Pose determination method and device and odometer |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110243390A true CN110243390A (en) | 2019-09-17 |
| CN110243390B CN110243390B (en) | 2021-07-27 |
Family
ID=67891724
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910620817.1A Active CN110243390B (en) | 2019-07-10 | 2019-07-10 | Pose determination method and device and odometer |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110243390B (en) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111402328A (en) * | 2020-03-17 | 2020-07-10 | 北京图森智途科技有限公司 | Pose calculation method and device based on laser odometer |
| CN111598930A (en) * | 2020-05-19 | 2020-08-28 | 北京数字绿土科技有限公司 | Color point cloud generation method and device and terminal equipment |
| CN112422848A (en) * | 2020-11-17 | 2021-02-26 | 深圳市歌华智能科技有限公司 | Video splicing method based on depth map and color map |
| CN112752028A (en) * | 2021-01-06 | 2021-05-04 | 南方科技大学 | Pose determination method, device and equipment of mobile platform and storage medium |
| CN113739786A (en) * | 2021-07-30 | 2021-12-03 | 国网江苏省电力有限公司电力科学研究院 | Indoor environment sensing method, device and equipment for quadruped robot |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2014147863A1 (en) * | 2013-03-21 | 2014-09-25 | 日本電気株式会社 | Three-dimensional information measuring/displaying device, three-dimensional information measuring/displaying method, and program |
| CN107341804A (en) * | 2016-04-29 | 2017-11-10 | 成都理想境界科技有限公司 | Determination method and device, image superimposing method and the equipment of cloud data midplane |
| CN107590827A (en) * | 2017-09-15 | 2018-01-16 | 重庆邮电大学 | A kind of indoor mobile robot vision SLAM methods based on Kinect |
| CN108564600A (en) * | 2018-04-19 | 2018-09-21 | 北京华捷艾米科技有限公司 | Moving object attitude tracking method and device |
| CN109034017A (en) * | 2018-07-12 | 2018-12-18 | 北京华捷艾米科技有限公司 | Head pose estimation method and machine readable storage medium |
| CN109308737A (en) * | 2018-07-11 | 2019-02-05 | 重庆邮电大学 | A kind of mobile robot V-SLAM method of three stage point cloud registration methods |
| CN109813335A (en) * | 2017-11-21 | 2019-05-28 | 武汉四维图新科技有限公司 | Scaling method, device, system and the storage medium of data collection system |
| CN109903330A (en) * | 2018-09-30 | 2019-06-18 | 华为技术有限公司 | A kind of method and apparatus handling data |
-
2019
- 2019-07-10 CN CN201910620817.1A patent/CN110243390B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2014147863A1 (en) * | 2013-03-21 | 2014-09-25 | 日本電気株式会社 | Three-dimensional information measuring/displaying device, three-dimensional information measuring/displaying method, and program |
| CN107341804A (en) * | 2016-04-29 | 2017-11-10 | 成都理想境界科技有限公司 | Determination method and device, image superimposing method and the equipment of cloud data midplane |
| CN107590827A (en) * | 2017-09-15 | 2018-01-16 | 重庆邮电大学 | A kind of indoor mobile robot vision SLAM methods based on Kinect |
| CN109813335A (en) * | 2017-11-21 | 2019-05-28 | 武汉四维图新科技有限公司 | Scaling method, device, system and the storage medium of data collection system |
| CN108564600A (en) * | 2018-04-19 | 2018-09-21 | 北京华捷艾米科技有限公司 | Moving object attitude tracking method and device |
| CN109308737A (en) * | 2018-07-11 | 2019-02-05 | 重庆邮电大学 | A kind of mobile robot V-SLAM method of three stage point cloud registration methods |
| CN109034017A (en) * | 2018-07-12 | 2018-12-18 | 北京华捷艾米科技有限公司 | Head pose estimation method and machine readable storage medium |
| CN109903330A (en) * | 2018-09-30 | 2019-06-18 | 华为技术有限公司 | A kind of method and apparatus handling data |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111402328A (en) * | 2020-03-17 | 2020-07-10 | 北京图森智途科技有限公司 | Pose calculation method and device based on laser odometer |
| CN111402328B (en) * | 2020-03-17 | 2023-11-10 | 北京图森智途科技有限公司 | Pose calculation method and device based on laser odometer |
| CN111598930A (en) * | 2020-05-19 | 2020-08-28 | 北京数字绿土科技有限公司 | Color point cloud generation method and device and terminal equipment |
| CN111598930B (en) * | 2020-05-19 | 2021-07-30 | 北京数字绿土科技有限公司 | Color point cloud generation method and device and terminal equipment |
| CN112422848A (en) * | 2020-11-17 | 2021-02-26 | 深圳市歌华智能科技有限公司 | Video splicing method based on depth map and color map |
| CN112422848B (en) * | 2020-11-17 | 2024-03-29 | 深圳市歌华智能科技有限公司 | Video stitching method based on depth map and color map |
| CN112752028A (en) * | 2021-01-06 | 2021-05-04 | 南方科技大学 | Pose determination method, device and equipment of mobile platform and storage medium |
| CN113739786A (en) * | 2021-07-30 | 2021-12-03 | 国网江苏省电力有限公司电力科学研究院 | Indoor environment sensing method, device and equipment for quadruped robot |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110243390B (en) | 2021-07-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110243390A (en) | The determination method, apparatus and odometer of pose | |
| JP6681729B2 (en) | Method for determining 3D pose of object and 3D location of landmark point of object, and system for determining 3D pose of object and 3D location of landmark of object | |
| CN110276317B (en) | Object size detection method, object size detection device and mobile terminal | |
| CN107808407A (en) | Unmanned plane vision SLAM methods, unmanned plane and storage medium based on binocular camera | |
| CN108303994B (en) | Group control interaction method for unmanned aerial vehicle | |
| CN108510535A (en) | A kind of high quality depth estimation method based on depth prediction and enhancing sub-network | |
| CN107833270A (en) | Real-time object dimensional method for reconstructing based on depth camera | |
| CN108401461A (en) | Three-dimensional mapping method, device and system, cloud platform, electronic equipment and computer program product | |
| CN106529538A (en) | Method and device for positioning aircraft | |
| CN108871349A (en) | A kind of deep space probe optical guidance pose weight determination method | |
| CN108474658A (en) | Ground Morphology observation method and system, unmanned plane landing method and unmanned plane | |
| CN105258680B (en) | A kind of object pose measuring method and device | |
| CN109359514A (en) | A kind of gesture tracking identification federation policies method towards deskVR | |
| CN113850865A (en) | Human body posture positioning method and system based on binocular vision and storage medium | |
| JP2017532695A (en) | Method and system for scanning an object using an RGB-D sensor | |
| CN105760809A (en) | Method and apparatus for head pose estimation | |
| CN109785373A (en) | A kind of six-freedom degree pose estimating system and method based on speckle | |
| CN108320310B (en) | Image sequence-based space target three-dimensional attitude estimation method | |
| EP3599588A1 (en) | Rendering an object | |
| CN108061879A (en) | Space-location method, device, electronic equipment and system | |
| CN114972646A (en) | Method and system for extracting and modifying independent ground objects of live-action three-dimensional model | |
| CN106767841A (en) | Vision navigation method based on self adaptation volume Kalman filtering and single-point random sampling | |
| CN110517323A (en) | 3 D positioning system and method based on manipulator one camera multi-vision visual | |
| CN108053481B (en) | Method and device for generating three-dimensional point cloud normal vector and storage medium | |
| CN108446595A (en) | A kind of space-location method, device, system and storage medium |
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 |