CN111311679A - Free floating target pose estimation method based on depth camera - Google Patents
Free floating target pose estimation method based on depth camera Download PDFInfo
- Publication number
- CN111311679A CN111311679A CN202010077687.4A CN202010077687A CN111311679A CN 111311679 A CN111311679 A CN 111311679A CN 202010077687 A CN202010077687 A CN 202010077687A CN 111311679 A CN111311679 A CN 111311679A
- Authority
- CN
- China
- Prior art keywords
- pose
- target
- point cloud
- template
- final
- 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
- 238000000034 method Methods 0.000 title claims abstract description 67
- 230000008569 process Effects 0.000 claims abstract description 20
- 238000012549 training Methods 0.000 claims abstract description 12
- DSCFFEYYQKSRSV-KLJZZCKASA-N D-pinitol Chemical compound CO[C@@H]1[C@@H](O)[C@@H](O)[C@H](O)[C@H](O)[C@H]1O DSCFFEYYQKSRSV-KLJZZCKASA-N 0.000 claims abstract description 8
- 230000000007 visual effect Effects 0.000 claims abstract description 5
- 238000001514 detection method Methods 0.000 claims description 25
- 238000009616 inductively coupled plasma Methods 0.000 claims description 22
- 238000004364 calculation method Methods 0.000 claims description 20
- 239000011159 matrix material Substances 0.000 claims description 19
- 238000013519 translation Methods 0.000 claims description 18
- 238000006243 chemical reaction Methods 0.000 claims description 14
- 230000009466 transformation Effects 0.000 claims description 12
- 238000005070 sampling Methods 0.000 claims description 11
- 238000012937 correction Methods 0.000 claims description 10
- 238000009499 grossing Methods 0.000 claims description 7
- 230000002159 abnormal effect Effects 0.000 claims description 6
- 238000007781 pre-processing Methods 0.000 claims description 6
- 238000001914 filtration Methods 0.000 claims description 3
- 230000009467 reduction Effects 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000007792 addition Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000003086 colorant Substances 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005530 etching Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 230000008685 targeting Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/20—Image enhancement or restoration by the use of local operators
- G06T5/30—Erosion or dilatation, e.g. thinning
-
- G06T5/70—
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
- G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20081—Training; Learning
Abstract
The invention relates to the field of visual positioning, and discloses a free floating target pose estimation method based on a depth camera, which comprises the steps of obtaining RGB (red, green and blue) images, depth images and point cloud information of a free floating target; importing a three-dimensional model of a target into OpenGL, and selecting a proper viewpoint acquisition template; detecting the target by using the collected template, RGB image and depth image, and calculating the similarity epsilonsObtaining a set of matching templates; estimating the initial pose of the target by using the training pose information (R, t) contained in the matching template to obtain the initial pose phiinitAnd an initial position pinit(ii) a After point clouds are preprocessed, the initial posture is corrected by utilizing an ICP algorithm to obtain a final postureResult (phi)final,pfinal) And therefore, the pose estimation process of the free floating target is completed.
Description
Technical Field
The invention relates to a free floating target pose estimation method based on a depth camera, and belongs to the technical field of visual positioning.
Background
With the continuous deepening of space exploration, the number of in-orbit spacecrafts is more and more, and fault satellites and orbital garbage are accumulated continuously. The use of space robots for satellite capture of faults, satellite maintenance and orbital refuse removal has become a popular research direction in recent years. Most targets are non-cooperative targets, in a free-floating state. In order for the space robot to correctly capture the target, the pose of the target capture point needs to be accurately estimated. The existing mainstream pose estimation methods comprise an RGB image-based pose estimation method and a depth image-based pose estimation method, wherein the RGB image-based pose estimation method is used for extracting feature points in a color image and calculating a pose, and the depth image-based pose estimation method is used for extracting feature descriptors in a point cloud and calculating and matching the pose. The robustness of the two methods is not high, and the requirements on the environment are high. The method provided by the invention combines the RGB image, the depth image and the point cloud information, improves the accuracy and the calculation efficiency, and reduces the requirement on the environment.
Disclosure of Invention
The invention provides a free floating target pose estimation method based on a depth camera, which combines RGB (red, green and blue) images, depth images and point cloud information, improves the accuracy and the calculation efficiency and reduces the requirement on the environment.
The invention can be realized by the following technical scheme:
a free floating target pose estimation method based on a depth camera is characterized by comprising the following steps:
step 1, target template acquisition, namely importing a three-dimensional model of a target into OpenGL, selecting a viewpoint and acquiring a template;
step 2, detecting the target, and detecting the target by utilizing the acquired template, the RGB image and the depth image and a template matching-based method to obtain a group of matching templates;
step 3, calculating the initial pose of the target, namely calculating the initial pose phi of the target by utilizing the training pose information (R, t) contained in the matching template in the previous stepinitAnd an initial position pinit；
Step 4, correcting the pose of the targetAfter point cloud preprocessing, correcting the pose based on an ICP (inductively coupled plasma) algorithm to obtain a final pose result (phi)final,pfinal)。
In the above free floating target pose estimation method based on the depth camera, the template acquisition process includes two processes of viewpoint sampling and redundancy reduction; the specific operation process of viewpoint sampling is as follows: after a three-dimensional model of a target is imported into OpenGL, an upper hemisphere with 162 vertexes is adopted, and the sampling angle step length of an azimuth angle is 15 degrees; the radius of the enclosing sphere is varied in steps of 10 cm; meanwhile, in order to acquire the template rotating in the plane, the camera rotates around the Z axis of the camera when acquiring each viewpoint, and the step length of the rotation angle is set to be 10 degrees.
In the above method for estimating pose of a free-floating target based on a depth camera, in the detection of the target, a method based on template matching is adopted, the gradient direction in an RGB image and the normal vector direction in a depth image are taken as features, and a template is defined as T ({ O) ═ Om}m∈M,P)；
Wherein O is a template feature representing a gradient direction or a normal vector direction; m is a modality, representing either an RGB image or a depth image; p is a set of doublets (r, m), r being the location of the feature in the template image;
and (3) performing similarity calculation on each template and the image T at the position c in a sliding window mode:
wherein, W represents a window area with c + r as the center; t is the position of the center of the window area; when the similarity of the templates is epsilonsAbove threshold τsThe template is matched.
In the method for estimating the pose of the free-floating target based on the depth camera, in the calculation of the initial pose of the target, because the detection result of the previous step comprises a group of templates, each template comprises training pose information (R, t), and the training pose information is used for calculating the initial pose of the target; the calculation of the attitude is performed first, and then the calculation of the position is performed.
In the free floating target pose estimation method based on the depth camera, during pose calculation, abnormal values in a detection template are eliminated based on channel chromaticity of an RGB image, and an initial pose is obtained;
each detected template comprises a rough estimation of the target pose, pixels positioned on the projection of the object are considered according to the pose estimation value, and the number of pixels with the expected color is calculated; if the difference of the grey values of the channels between the projected pixel and the pixel of the target is less than a specified threshold value, judging that the pixel has the expected color;
the percentage of the pixels with the expected color is less than seventy percent, the detection result is determined to be invalid, and the template is rejected; in the operation process, pixels of the projection boundary are removed by corroding the projection in advance;
after the abnormal values are eliminated, the rest templates are considered to provide enough credibility for the pose of the detection result, and the poses of the templates are averaged to obtain the initial pose phi of the detection resultinit。
In the above free-floating target pose estimation method based on the depth camera, in the position calculation, the initial pose phi is firstly based oninitAnd training the distance d to render a three-dimensional model of the object to obtain a model point cloud and a mask image of the object under the visual angle, and recording the position of the object as prender＝[0,0,d](ii) a Then projecting the mask image into a field depth image, and segmenting an interested area corresponding to the mask; then, converting the region of interest into three-dimensional point cloud called scene point cloud by using camera internal parameters;
calculating a translation vector t from the model point cloud to the scene point cloud; computing geometric centers of model point clouds and scene point clouds
And
then respectively in model point clouds andsearch separation in scene point cloud
And
of closest point
And
and consider that
And
a group of scene-model corresponding points are obtained, and finally, the translation vector t can be obtained by subtracting the two points;
the initial position of the final object is: p is a radical ofinit＝prender+t。
In the free floating target pose estimation method based on the depth camera, the ICP method is utilized to register the point cloud to realize pose correction, and the final pose (phi) is obtainedfinal,pfinal) Based on a given three-dimensional reference point set S ═ S1,s2,...,snThe point cloud registration algorithm is used as a target of the point cloud registration algorithm; given three-dimensional point set M ═ M1,m2,...,mnContinuously carrying out rigid body conversion on M in an iteration process, and gradually approaching S; the objective function is:
wherein R represents a rotation transformation matrix and t represents a translation transformation matrix; the target function represents finding a rigid body conversion, so that the square sum of the position errors of M and S after conversion is minimum; then, the initial pose (phi) of the detection result is updated by using the rotation matrix R and the translation vector t after ICP pose correctioninit,pinit) To obtainFinal pose (phi)final,pfinal) (ii) a The method specifically comprises two processes of point cloud pretreatment and point cloud registration; the point cloud preprocessing comprises three steps of denoising, point cloud smoothing and point cloud downsampling:
the denoising process is to remove discrete isolated points by Gaussian filtering; point cloud smoothing is performed by using a moving least square Method (MLS); the down-sampling is to utilize a voxel grid to down-sample point clouds;
the point cloud registration is to register the preprocessed point cloud by utilizing an ICP (inductively coupled plasma) algorithm to realize pose correction; in the ICP algorithm, a three-dimensional reference point set S is given as S1,s2,...,snThe point cloud registration algorithm is used as a target of the point cloud registration algorithm; given three-dimensional point set M ═ M1,m2,...,mnContinuously carrying out rigid body conversion on M in an iteration process, and gradually approaching S; the objective function is:
wherein R represents a rotation transformation matrix and t represents a translation transformation matrix; the target function represents finding a rigid body conversion, so that the square sum of the position errors of M and S after conversion is minimum; then, the initial pose (phi) of the detection result is updated by using the rotation matrix R and the translation vector t after ICP pose correctioninit,pinit) To obtain the final pose (phi)final,pfinal)。
In the above free-floating target pose estimation method based on the depth camera, the point cloud registration specifically includes:
(1) initializing a point set M;
(2) for each point M in the set of points MiSearching for the closest point S in SjConstructing a set of corresponding points (m)j,sj)；
(3) Using a distance threshold dtMaking a mismatch determination, i.e. if miAnd sjIs greater than dtDeleting the corresponding point of the group;
(4) obtaining a corresponding rotation matrix R and a translation vector t through an objective function of a minimum formula; and (3) the obtained R and t are used for updating S until the error E (R, t) is smaller than a set threshold value or the iteration number is larger than a set number, the iteration is stopped, and otherwise, the step (2) is returned to find the closest point again.
The beneficial technical effects of the invention are as follows:
the template collection is carried out by utilizing the three-dimensional model of the target, the characteristic redundancy of the collected template is reduced after the template collection, the operation speed is improved on the premise of ensuring the identification precision, and the template collection method has high efficiency and timeliness. Then combining the RGB image and the depth image to obtain a group of templates based on a template matching method; when the gradient feature is utilized, the size of the gradient feature is ignored and only the size of the gradient feature is used, so that the method has strong applicability to the space scene background. And then, screening the template according to the intensity characteristics of each channel of the RGB image, and further preliminarily calculating the pose of the target. And finally, the pose is corrected by combining an ICP method to obtain a final pose recognition result, so that the recognition accuracy is improved. The invention is simple and reliable, convenient to operate, easy to realize and convenient to popularize and apply.
Drawings
FIG. 1 is a schematic flow chart of the present invention.
Fig. 2 is a schematic diagram of free floating target capture.
Detailed Description
The invention is further described below with reference to the accompanying drawings. The following examples are only for illustrating the technical solutions of the present invention more clearly, and the protection scope of the present invention is not limited thereby.
As shown in fig. 2, which is a schematic diagram of capturing a free-floating target, a propeller with higher recognition degree is selected as a capture point of the target, and a depth camera is mounted on a mechanical arm base to acquire RGB image, depth image and point cloud information of the target.
As shown in fig. 1, the specific steps are as follows:
step one, target template acquisition: since one template can only represent the observation of an object under a single viewing angle, in order to realize the three-dimensional object detection under any pose, template images must be acquired from multiple viewing angles and at different distances. The template collection comprises the following two steps:
step I, viewpoint sampling: after the three-dimensional model of the target is imported into OpenGL, an upper hemisphere with 162 vertexes is adopted, and the sampling angle step length of the azimuth angle is 15 degrees. The radius of the enclosing sphere is varied in steps of 10 cm. Meanwhile, in order to acquire the template rotating in the plane, the camera rotates around the Z axis of the camera when acquiring each viewpoint, and the step length of the rotation angle is set to be 10 degrees.
Step II, redundancy reduction characteristic: to speed up the computation we use only a part of the collected template features. For the color gradient feature, only the dominant color gradient feature on the target contour is retained, since the target surface we are targeting does not necessarily have texture, and the resulting three-dimensional model of the target does not detail the accompanying texture. For surface normal features, surface normal features are selected inside the object contour, because the surface normal on the projected object boundary is usually not reliably estimated or not estimated at all.
Step two, target detection: the method based on template matching is adopted, the gradient direction in an RGB image and the normal vector direction in a depth image are taken as characteristics, and a template is defined as T ═ ({ O ═ Om}m∈M,P)。
Wherein O is a template feature representing a gradient direction or a normal vector direction; m is a modality, representing either an RGB image or a depth image; p is a set of doublets (r, m) and r is the location of the feature in the template image.
The acquisition of the target image is obtained by a binocular depth camera as shown in fig. 2, which is fixed to the base of the robotic arm.
And (3) performing similarity calculation on each template and the input image I at the position c in a sliding window mode:
wherein, W represents a window area with c + r as the center; t is the position of the center of the window area. When the similarity of the templates is epsilonsAbove threshold τsThe template is matched.
From the above formula, when the similarity of the gradient features is calculated, the absolute value of the difference between the cosine values is calculated, so that the black background has no influence on the detection accuracy of the method, and the method has strong applicability to space scenes.
(1) Calculation of gradient direction: and (4) respectively calculating in three channels of RGB of the image, and only keeping the gradient with the maximum modulus value. For the RGB image I, calculating the gradient direction I at the pixel point xg(x)：
(2) And (3) calculating a normal vector: the depth function d (x) with a first order taylor expansion in the depth image yields:
D(x+dx)-D(x)＝dxT▽D+h.o.t.
within a neighborhood, each offset dx satisfies the above formula, the optimal depth gradient
The result was obtained by the least square method. Based on
One pass X, X can be found1,X2Plane of three points:
X＝v(x)D(x)
where v (x) is a line-of-sight vector of x calculated based on camera intrinsic parameters. Then, for the vector X1-X and X2And normalizing the cross multiplication result of the X to obtain a normal vector of the pixel point X.
Step three, calculating the initial pose of the target: since the detection result of the previous step includes a set of templates, each template contains training pose information (R, t), the training poses are used to calculate the initial pose of the target. The method comprises two steps of calculation of the gesture and calculation of the position.
Step I, posture calculation: and eliminating abnormal values in the detection template based on the gray value of each channel of the RGB image to obtain an initial pose.
Each detected template contains a rough estimate of the pose of the object, considering the pixels located on the object projection based on the pose estimate, and calculating how many of them have the desired color. If the difference between the respective channel gray values between the projected pixel and the pixel of the object is less than a prescribed threshold, it is determined that this pixel has the intended color.
The percentage of pixels with the expected color is less than seventy percent, the detection result is determined to be invalid, and the template is rejected. During operation, pixels at the projection boundaries are removed by etching the projection beforehand.
Processing the black and white components of the capture object maps them to channel gray values of similar colors: black maps to blue and white to yellow. Before calculating the difference between the gray values, the corresponding saturation and value components are checked. If the value component is below the threshold tvThen the hue is set to blue; if the value component is greater than tvAnd the saturation component is less than the threshold tsThe hue is set to yellow.
After the abnormal values are eliminated, the rest templates are considered to provide enough credibility for the pose of the detection result, and the poses of the templates are averaged to obtain the initial pose phi of the detection resultinit。
Step II, position calculation: first based on the initial pose phiinitAnd training the distance d to render a three-dimensional model of the object to obtain a model point cloud and a mask image of the object under the visual angle, and recording the position of the object as prender＝[0,0,d]. And then projecting the mask image into the field depth image, and segmenting an interested area corresponding to the mask. Then the interested region is converted into three by using the camera internal parameterAnd (4) the dimensional point cloud is called as scene point cloud.
And compensating the deviation brought by the center of the scene point cloud as a result by calculating a translation vector t from the model point cloud to the scene point cloud. Computing geometric centers of model point clouds and scene point clouds
And
then searching and separating in the model point cloud and the scene point cloud respectively
And
of closest point
And
and consider that
And
and finally subtracting the two points to obtain the translation vector t.
The initial position of the final object is: p is a radical ofinit＝prender+t。
Step four, in-place pose correction, correcting the pose by utilizing an ICP algorithm, and comprising the following two steps of:
and step I, point cloud preprocessing is carried out, and the point cloud preprocessing comprises three processes of point cloud denoising, point cloud smoothing and point cloud downsampling.
(1) Denoising: and removing discrete isolated points in the point cloud by adopting Gaussian filtering.
(2) Smoothing: point cloud smoothing is performed using a moving least squares Method (MLS).
(3) Down-sampling: the point cloud is downsampled using a voxel grid.
And step II, registering the preprocessed point cloud by utilizing an ICP (inductively coupled plasma) algorithm to realize pose correction. In the ICP algorithm, a three-dimensional reference point set S is given as S1,s2,...,snThe point cloud registration algorithm is used as a target of the point cloud registration algorithm; given three-dimensional point set M ═ M1,m2,...,mnAnd (4) continuously carrying out rigid body conversion on the point set M in the iteration process, and gradually approaching the point set S. The objective function is:
where R represents the rotational transformation matrix and t represents the translational transformation matrix. The objective function representation finds a rigid body transformation such that the sum of squared positional errors of the transformed point set M and the transformed point set S is minimized, comprising the following processes:
(1) initialization: the set of points M must be in a position close to the set of points S.
(2) Finding the closest point: the solution of the rigid body transformation matrix is established on the corresponding relation of the point set S and the point set M. ICP considers that the correspondence is determined by the shortest distance, i.e. M for each point in the set of points MiSearching for the closest point S in SjConstructing a set of corresponding points (m)j,sj)。
(3) Removing mismatching groups: in general, part of the S point set is not located in M, so that the part obtained in the previous step corresponds to the point group (M)i,sj) Is a false match. Using a distance threshold dtMaking mismatching judgment if miAnd sjIs greater than dtThen the set of corresponding points should be excluded.
(4) Solving the optimal rigid body conversion: and solving the corresponding rotation matrix R and translation vector t through the objective function. And (3) the obtained R and t are used for updating the point set S, until the error E (R, t) is smaller than a set threshold value or the iteration frequency is larger than a set frequency, the iteration is stopped, and otherwise, the step (2) is returned to find the closest point again.
Finally, the initial pose (phi) of the detection result is updated by using the rotation matrix R and the translation vector tinit,pinit) To obtain the final pose (phi)final,pfinal)。
The specific embodiments described herein are merely illustrative of the spirit of the invention. Various modifications or additions may be made to the described embodiments or alternatives may be employed by those skilled in the art without departing from the spirit or ambit of the invention as defined in the appended claims.
Claims (8)
1. A free floating target pose estimation method based on a depth camera is characterized by comprising the following steps:
step 1, target template acquisition, namely importing a three-dimensional model of a target into OpenGL, selecting a viewpoint and acquiring a template;
step 2, detecting the target, and detecting the target by utilizing the acquired template, the RGB image and the depth image and a template matching-based method to obtain a group of matching templates;
step 3, calculating the initial pose of the target, namely calculating the initial pose phi of the target by utilizing the training pose information (R, t) contained in the matching template in the previous stepinitAnd an initial position pinit；
Step 4, correcting the pose of the target, after point cloud preprocessing, correcting the pose based on an ICP (inductively coupled plasma) algorithm to obtain a final pose result (phi)final,pfinal)。
2. The free-floating target pose estimation method based on the depth camera is characterized in that the template acquisition process comprises two processes of viewpoint sampling and redundancy reduction; the specific operation process of viewpoint sampling is as follows: after a three-dimensional model of a target is imported into OpenGL, an upper hemisphere with 162 vertexes is adopted, and the sampling angle step length of an azimuth angle is 15 degrees; the radius of the enclosing sphere is varied in steps of 10 cm; meanwhile, in order to acquire the template rotating in the plane, the camera rotates around the Z axis of the camera when acquiring each viewpoint, and the step length of the rotation angle is set to be 10 degrees.
3. The method for estimating the pose of a free-floating target based on a depth camera as claimed in claim 1, wherein in the detection of the target, a method based on template matching is adopted, and the gradient direction in an RGB image and the normal vector direction in the depth image are taken as features, and the template is defined as T ({ O) ═ O }m}m∈M,P)；
Wherein O is a template feature representing a gradient direction or a normal vector direction; m is a modality, representing either an RGB image or a depth image; p is a set of doublets (r, m), r being the location of the feature in the template image;
and (3) performing similarity calculation on each template and the image T at the position c in a sliding window mode:
wherein, W represents a window area with c + r as the center; t is the position of the center of the window area; when the similarity of the templates is epsilonsAbove threshold τsThe template is matched.
4. The method for estimating the pose of a free-floating target based on a depth camera according to claim 1, wherein in the calculation of the initial pose of the target, since the detection result of the previous step comprises a set of templates, each template comprises a training pose information (R, t), the training pose information is used for calculating the initial pose of the target; the calculation of the attitude is performed first, and then the calculation of the position is performed.
5. The free-floating target pose estimation method based on the depth camera as claimed in claim 1, wherein in the pose calculation, the channel chromaticity based on the RGB image is used for eliminating abnormal values in the detection template to obtain an initial pose;
each detected template comprises a rough estimation of the target pose, pixels positioned on the projection of the object are considered according to the pose estimation value, and the number of pixels with the expected color is calculated; if the difference of the grey values of the channels between the projected pixel and the pixel of the target is less than a specified threshold value, judging that the pixel has the expected color;
the percentage of the pixels with the expected color is less than seventy percent, the detection result is determined to be invalid, and the template is rejected; in the operation process, pixels of the projection boundary are removed by corroding the projection in advance;
after the abnormal values are eliminated, the rest templates are considered to provide enough credibility for the pose of the detection result, and the poses of the templates are averaged to obtain the initial pose phi of the detection resultinit。
6. The method for estimating the pose of a free-floating target based on a depth camera as claimed in claim 1, wherein in the position calculation, the initial pose φ is firstly based oninitAnd training the distance d to render a three-dimensional model of the object to obtain a model point cloud and a mask image of the object under the visual angle, and recording the position of the object as prender＝[0,0,d](ii) a Then projecting the mask image into a field depth image, and segmenting an interested area corresponding to the mask; then, converting the region of interest into three-dimensional point cloud called scene point cloud by using camera internal parameters;
calculating a translation vector t from the model point cloud to the scene point cloud; computing geometric centers of model point clouds and scene point clouds
And
then searching and separating in the model point cloud and the scene point cloud respectively
And
of closest point
And
and consider that
And
a group of scene-model corresponding points are obtained, and finally, the translation vector t can be obtained by subtracting the two points;
the initial position of the final object is: p is a radical ofinit＝prender+t。
7. The method for estimating the pose of the free-floating target based on the depth camera as claimed in claim 1, wherein the ICP method is used for registering the point cloud to realize pose correction to obtain the final pose (phi)final,pfinal) Based on a given three-dimensional reference point set S ═ S1,s2,...,snThe point cloud registration algorithm is used as a target of the point cloud registration algorithm; given three-dimensional point set M ═ M1,m2,...,mnContinuously carrying out rigid body conversion on M in an iteration process, and gradually approaching S; the objective function is:
wherein R represents a rotation transformation matrix and t represents a translation transformation matrix; the target function represents finding a rigid body conversion, so that the square sum of the position errors of M and S after conversion is minimum; then, the initial pose (phi) of the detection result is updated by using the rotation matrix R and the translation vector t after ICP pose correctioninit,pinit) To obtain the final pose (phi)final,pfinal) (ii) a Specifically comprises two point cloudsTwo processes of processing and point cloud registration; the point cloud preprocessing comprises three steps of denoising, point cloud smoothing and point cloud downsampling:
the denoising process is to remove discrete isolated points by Gaussian filtering; point cloud smoothing is performed by using a moving least square Method (MLS); the down-sampling is to utilize a voxel grid to down-sample point clouds;
the point cloud registration is to register the preprocessed point cloud by utilizing an ICP (inductively coupled plasma) algorithm to realize pose correction; in the ICP algorithm, a three-dimensional reference point set S is given as S1,s2,...,snThe point cloud registration algorithm is used as a target of the point cloud registration algorithm; given three-dimensional point set M ═ M1,m2,...,mnContinuously carrying out rigid body conversion on M in an iteration process, and gradually approaching S; the objective function is:
wherein R represents a rotation transformation matrix and t represents a translation transformation matrix; the target function represents finding a rigid body conversion, so that the square sum of the position errors of M and S after conversion is minimum; then, the initial pose (phi) of the detection result is updated by using the rotation matrix R and the translation vector t after ICP pose correctioninit,pinit) To obtain the final pose (phi)final,pfinal)。
8. The depth camera-based free-floating target pose estimation method of claim 7, wherein point cloud registration specifically comprises:
(1) initializing a point set M;
(2) for each point M in the set of points MiSearching for the closest point S in SjConstructing a set of corresponding points (m)j,sj)；
(3) Using a distance threshold dtMaking a mismatch determination, i.e. if miAnd sjIs greater than dtDeleting the corresponding point of the group;
(4) obtaining a corresponding rotation matrix R and a translation vector t through an objective function of a minimum formula; and (3) the obtained R and t are used for updating S until the error E (R, t) is smaller than a set threshold value or the iteration number is larger than a set number, the iteration is stopped, and otherwise, the step (2) is returned to find the closest point again.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010077687.4A CN111311679B (en) | 2020-01-31 | 2020-01-31 | Free floating target pose estimation method based on depth camera |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010077687.4A CN111311679B (en) | 2020-01-31 | 2020-01-31 | Free floating target pose estimation method based on depth camera |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111311679A true CN111311679A (en) | 2020-06-19 |
| CN111311679B CN111311679B (en) | 2022-04-01 |
Family
ID=71148278
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010077687.4A Active CN111311679B (en) | 2020-01-31 | 2020-01-31 | Free floating target pose estimation method based on depth camera |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111311679B (en) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111784770A (en) * | 2020-06-28 | 2020-10-16 | 河北工业大学 | Three-dimensional attitude estimation method in disordered grabbing based on SHOT and ICP algorithm |
| CN111814792A (en) * | 2020-09-04 | 2020-10-23 | 之江实验室 | Feature point extraction and matching method based on RGB-D image |
| CN112465903A (en) * | 2020-12-21 | 2021-03-09 | 上海交通大学宁波人工智能研究院 | 6DOF object attitude estimation method based on deep learning point cloud matching |
| CN112651944A (en) * | 2020-12-28 | 2021-04-13 | 哈尔滨工业大学（深圳） | 3C component high-precision six-dimensional pose estimation method and system based on CAD model |
| CN112686950A (en) * | 2020-12-04 | 2021-04-20 | 深圳市优必选科技股份有限公司 | Pose estimation method and device, terminal equipment and computer readable storage medium |
| CN113239771A (en) * | 2021-05-07 | 2021-08-10 | 中国科学院深圳先进技术研究院 | Attitude estimation method, system and application thereof |
| CN113393522A (en) * | 2021-05-27 | 2021-09-14 | 湖南大学 | 6D pose estimation method based on monocular RGB camera regression depth information |
| CN114049399A (en) * | 2022-01-13 | 2022-02-15 | 上海景吾智能科技有限公司 | Mirror positioning method combining RGBD image |
| CN114781056A (en) * | 2022-04-13 | 2022-07-22 | 南京航空航天大学 | Aircraft complete machine shape measuring method based on feature matching |
| CN115063483A (en) * | 2022-06-14 | 2022-09-16 | 广东天太机器人有限公司 | Template posture correction method and system based on 2d image recognition |
Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101726296A (en) * | 2009-12-22 | 2010-06-09 | 哈尔滨工业大学 | Vision measurement, path planning and GNC integrated simulation system for space robot |
| CN103777220A (en) * | 2014-01-17 | 2014-05-07 | 西安交通大学 | Real-time and accurate pose estimation method based on fiber-optic gyroscope, speed sensor and GPS |
| DE102014005181A1 (en) * | 2014-04-03 | 2015-10-08 | Astrium Gmbh | Position and orientation of objects |
| CN105976353A (en) * | 2016-04-14 | 2016-09-28 | 南京理工大学 | Spatial non-cooperative target pose estimation method based on model and point cloud global matching |
| CN106251353A (en) * | 2016-08-01 | 2016-12-21 | 上海交通大学 | Weak texture workpiece and the recognition detection method and system of three-dimensional pose thereof |
| CN107392845A (en) * | 2017-07-31 | 2017-11-24 | 芜湖微云机器人有限公司 | A kind of method of 3D point cloud imaging and positioning |
| CN109255813A (en) * | 2018-09-06 | 2019-01-22 | 大连理工大学 | A kind of hand-held object pose real-time detection method towards man-machine collaboration |
| CN109636854A (en) * | 2018-12-18 | 2019-04-16 | 重庆邮电大学 | A kind of augmented reality three-dimensional Tracing Registration method based on LINE-MOD template matching |
| CN109801337A (en) * | 2019-01-21 | 2019-05-24 | 同济大学 | A kind of 6D position and orientation estimation method of Case-based Reasoning segmentation network and iteration optimization |
| CN109885073A (en) * | 2019-01-15 | 2019-06-14 | 西北工业大学 | A kind of prediction technique for the free float state of space non-cooperative target |
| CN110223348A (en) * | 2019-02-25 | 2019-09-10 | 湖南大学 | Robot scene adaptive bit orientation estimation method based on RGB-D camera |
| CN110340891A (en) * | 2019-07-11 | 2019-10-18 | 河海大学常州校区 | Mechanical arm positioning grasping system and method based on cloud template matching technique |
| CN110702091A (en) * | 2019-07-24 | 2020-01-17 | 武汉大学 | High-precision positioning method for moving robot along subway rail |
-
2020
- 2020-01-31 CN CN202010077687.4A patent/CN111311679B/en active Active
Patent Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101726296A (en) * | 2009-12-22 | 2010-06-09 | 哈尔滨工业大学 | Vision measurement, path planning and GNC integrated simulation system for space robot |
| CN103777220A (en) * | 2014-01-17 | 2014-05-07 | 西安交通大学 | Real-time and accurate pose estimation method based on fiber-optic gyroscope, speed sensor and GPS |
| DE102014005181A1 (en) * | 2014-04-03 | 2015-10-08 | Astrium Gmbh | Position and orientation of objects |
| CN105976353A (en) * | 2016-04-14 | 2016-09-28 | 南京理工大学 | Spatial non-cooperative target pose estimation method based on model and point cloud global matching |
| CN106251353A (en) * | 2016-08-01 | 2016-12-21 | 上海交通大学 | Weak texture workpiece and the recognition detection method and system of three-dimensional pose thereof |
| CN107392845A (en) * | 2017-07-31 | 2017-11-24 | 芜湖微云机器人有限公司 | A kind of method of 3D point cloud imaging and positioning |
| CN109255813A (en) * | 2018-09-06 | 2019-01-22 | 大连理工大学 | A kind of hand-held object pose real-time detection method towards man-machine collaboration |
| CN109636854A (en) * | 2018-12-18 | 2019-04-16 | 重庆邮电大学 | A kind of augmented reality three-dimensional Tracing Registration method based on LINE-MOD template matching |
| CN109885073A (en) * | 2019-01-15 | 2019-06-14 | 西北工业大学 | A kind of prediction technique for the free float state of space non-cooperative target |
| CN109801337A (en) * | 2019-01-21 | 2019-05-24 | 同济大学 | A kind of 6D position and orientation estimation method of Case-based Reasoning segmentation network and iteration optimization |
| CN110223348A (en) * | 2019-02-25 | 2019-09-10 | 湖南大学 | Robot scene adaptive bit orientation estimation method based on RGB-D camera |
| CN110340891A (en) * | 2019-07-11 | 2019-10-18 | 河海大学常州校区 | Mechanical arm positioning grasping system and method based on cloud template matching technique |
| CN110702091A (en) * | 2019-07-24 | 2020-01-17 | 武汉大学 | High-precision positioning method for moving robot along subway rail |
Non-Patent Citations (2)
| Title |
|---|
| HAOXIANG CAO等: "The 3D map building of the mobile robot", 《2016 IEEE INTERNATIONAL CONFERENCE ON MECHATRONICS AND AUTOMATION》 * |
| 王月 等: "基于模型的增强现实无标识三维注册追踪方法", 《上海交通大学学报》 * |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111784770A (en) * | 2020-06-28 | 2020-10-16 | 河北工业大学 | Three-dimensional attitude estimation method in disordered grabbing based on SHOT and ICP algorithm |
| CN111784770B (en) * | 2020-06-28 | 2022-04-01 | 河北工业大学 | Three-dimensional attitude estimation method in disordered grabbing based on SHOT and ICP algorithm |
| CN111814792A (en) * | 2020-09-04 | 2020-10-23 | 之江实验室 | Feature point extraction and matching method based on RGB-D image |
| CN111814792B (en) * | 2020-09-04 | 2020-12-29 | 之江实验室 | Feature point extraction and matching method based on RGB-D image |
| CN112686950A (en) * | 2020-12-04 | 2021-04-20 | 深圳市优必选科技股份有限公司 | Pose estimation method and device, terminal equipment and computer readable storage medium |
| CN112686950B (en) * | 2020-12-04 | 2023-12-15 | 深圳市优必选科技股份有限公司 | Pose estimation method, pose estimation device, terminal equipment and computer readable storage medium |
| CN112465903A (en) * | 2020-12-21 | 2021-03-09 | 上海交通大学宁波人工智能研究院 | 6DOF object attitude estimation method based on deep learning point cloud matching |
| CN112651944B (en) * | 2020-12-28 | 2023-08-22 | 哈尔滨工业大学（深圳） | 3C component high-precision six-dimensional pose estimation method and system based on CAD model |
| CN112651944A (en) * | 2020-12-28 | 2021-04-13 | 哈尔滨工业大学（深圳） | 3C component high-precision six-dimensional pose estimation method and system based on CAD model |
| CN113239771A (en) * | 2021-05-07 | 2021-08-10 | 中国科学院深圳先进技术研究院 | Attitude estimation method, system and application thereof |
| CN113393522A (en) * | 2021-05-27 | 2021-09-14 | 湖南大学 | 6D pose estimation method based on monocular RGB camera regression depth information |
| CN114049399A (en) * | 2022-01-13 | 2022-02-15 | 上海景吾智能科技有限公司 | Mirror positioning method combining RGBD image |
| CN114049399B (en) * | 2022-01-13 | 2022-04-12 | 上海景吾智能科技有限公司 | Mirror positioning method combining RGBD image |
| CN114781056B (en) * | 2022-04-13 | 2023-02-03 | 南京航空航天大学 | Aircraft complete machine shape measuring method based on feature matching |
| CN114781056A (en) * | 2022-04-13 | 2022-07-22 | 南京航空航天大学 | Aircraft complete machine shape measuring method based on feature matching |
| CN115063483A (en) * | 2022-06-14 | 2022-09-16 | 广东天太机器人有限公司 | Template posture correction method and system based on 2d image recognition |
| CN115063483B (en) * | 2022-06-14 | 2023-04-11 | 广东天太机器人有限公司 | Template posture correction method and system based on 2d image recognition |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111311679B (en) | 2022-04-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111311679B (en) | Free floating target pose estimation method based on depth camera | |
| CN109544456B (en) | Panoramic environment sensing method based on two-dimensional image and three-dimensional point cloud data fusion | |
| CN111062990B (en) | Binocular vision positioning method for underwater robot target grabbing | |
| Pizarro et al. | Toward large-area mosaicing for underwater scientific applications | |
| CN108597009B (en) | Method for detecting three-dimensional target based on direction angle information | |
| CN111721259B (en) | Underwater robot recovery positioning method based on binocular vision | |
| CN111862201B (en) | Deep learning-based spatial non-cooperative target relative pose estimation method | |
| CN109903313B (en) | Real-time pose tracking method based on target three-dimensional model | |
| CN108225319B (en) | Monocular vision rapid relative pose estimation system and method based on target characteristics | |
| CN110569861B (en) | Image matching positioning method based on point feature and contour feature fusion | |
| CN110021029B (en) | Real-time dynamic registration method and storage medium suitable for RGBD-SLAM | |
| CN111784655B (en) | Underwater robot recycling and positioning method | |
| CN110222661B (en) | Feature extraction method for moving target identification and tracking | |
| CN110245566B (en) | Infrared target remote tracking method based on background features | |
| CN113658337A (en) | Multi-mode odometer method based on rut lines | |
| CN114549549B (en) | Dynamic target modeling tracking method based on instance segmentation in dynamic environment | |
| CN115641367A (en) | Infrared and visible light image registration method based on multi-stage feature matching | |
| CN107944350B (en) | Monocular vision road identification method based on appearance and geometric information fusion | |
| CN104156933A (en) | Image registering method based on optical flow field | |
| CN111260736A (en) | In-orbit real-time calibration method for internal parameters of space camera | |
| CN116152068A (en) | Splicing method for solar panel images | |
| CN112767481B (en) | High-precision positioning and mapping method based on visual edge features | |
| CN109242910B (en) | Monocular camera self-calibration method based on any known plane shape | |
| CN113313116A (en) | Vision-based accurate detection and positioning method for underwater artificial target | |
| CN111882589A (en) | Image-based monocular vision SLAM initialization method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |