Method for determining object poses using weighted features

Abstract

A method for determining a pose of an object in a scene by determining a set of scene features from data acquired of the scene and matching the scene features to model features to generate weighted candidate poses when the scene feature matches one of the model features, wherein the weight of the candidate pose is proportional to the model weight. Then, the pose of the object is determined from the candidate poses based on the weights.

Description

FIELD OF THE INVENTION

This invention relates generally to estimating poses of 3D objects, and more particularly to estimating poses from data acquired by 3D sensors.

BACKGROUND OF THE INVENTION

A frequent problem in computer vision applications is to determine poses of objects in 3D scenes from scene data acquired by 3D sensors based on structured light or time of flight. Pose estimation methods typically require identification and matching of scene measurements with a known model of the object.

Some methods are based on selecting relevant points in a 3D point cloud and using feature representations that can invariantly describe regions near the points. Those methods produce successful results when the shape of the object is detailed, and the scene measurements have a high resolution and little noise. However, under less ideal conditions, the accuracy of those methods decreases rapidly. The 3D measurements can include many hidden surfaces due to imaging from a single viewpoint with the sensor, which makes a detailed region representation unavailable. Noise and background clutter further affect the accuracy of those methods.

A set of pair features can be use for detection and pose estimation. Pairs of oriented points on a surface of an object are used in a voting framework for pose estimation, e.g., see U.S. Publication 20110273442. Even though the descriptor associated with the pair feature is not very discriminative, that method can produce accurate results even when subject to moderate occlusion and background clutters by accumulating measurements for a large number of pairs. That framework can benefit from a hashing and Hough voting scheme.

Other methods model 3D shapes globally by using 2D and 3D contours, shape templates, and feature histograms. In general, global methods require the object to be isolated because those methods are sensitive to occlusion. Also, changes in appearance due to pose variations necessitates the use of a large number of shape templates, which has the drawback of increased memory and processing time. A learning-based keypoint detector that uses range data to decrease processing time is described in U.S. Publication 20100278384.

SUMMARY OF THE INVENTION

The embodiments of the invention provide a method for determining poses of three-dimensional (3D) objects in a scene using weighted model features.

During online operation, scene features are determined from 3D data acquired of the scene by a 3D sensor. The scene features are matched to model features acquired during offline training. The matching generates weighted candidate poses. The weights of candidate poses are proportional to the weights of the model features that match the scene features. Then, the candidate poses are merged by clustering the poses. The pose of the object is determined according to the weights of the merged poses.

In some embodiments, the model and scene features are oriented point pair features. The invention is based on the realization that not all the pairs of points have similar discriminative power, or repeatability. In fact, certain features do not carry any relevant information for pose estimation, hence the model features can be selected sparsely and weighted according to their importance.

BRIEF DESCRIPTION OF THE FIGURES

FIG. 1A is a schematic of a robotic assembly apparatus that can use the embodiments of the invention;

FIG. 1B is a block diagram of a method for pose estimation in the apparatus of FIG. 1A according to the embodiments of the invention;

FIG. 2 is a block diagram of a procedure for learning model features and weights according to some embodiments of the invention;

FIG. 3 is a block diagram of pseudocode of learning model features and weights according to some embodiments of the invention;

FIG. 4 is a schematic of the descriptor constructed from a pair feature according to some embodiments of the invention; and

FIG. 5 is a schematic of the optimized pose estimation using weighted voting scheme according to some embodiments of the invention.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

Robotic Bin Picking Application

FIG. 1A shows a system for estimating a pose of an object. The goal is to pick objects 40 from a bin 50 based on their poses 170. The system includes a 6-axis robotic arm 10 with a gripper 20. A 3D sensor 30 is arranged on the arm to acquire data of the scene. Then, the gripper picks the objects from a bin depending on their poses. It should be noted that the bin can contain different objects.

One example 3D sensor uses structured light generated by a projector. Other sensors, such as stereo cameras and time-of-flight range sensors are also possible. The sensor acquires 3D scene data 100, e.g., a point cloud. The 3D sensor is calibrated with respect to the robot arm. Thus, the poses of the objects estimated in a coordinate system of the 3D sensor can be transformed to a coordinate system of the robotic arm, allowing grasping and picking of the objects by controlling 80 the robotic arm according to the poses. The scene data are processed by a method 71 performed in a processor 70. The processor can include memory and input/output interfaces as known in the art.

Point Pair Features

In some embodiments, the pose estimation method uses point pair features. Each point pair feature can be defined using two points on a surface of the object, and normal directions of the surface at the points, which is called a surface-to-surface (S2S) pair feature. The feature can also be defined using a point on the surface and its normal and another point on an object boundary and direction of this boundary, which is called a surface-to-boundary (S2B) pair feature. The feature can also be defined using two points on the object boundary and their directions, which is called a boundary-to-boundary (B2B) pair feature.

As shown in FIG. 4, let {right arrow over (m)}_r and {right arrow over (m)}_i be the point pair, and {right arrow over (n)}_r and {right arrow over (n)}_i the orientations at the points, e.g., normals or boundary directions, of the two points in the point pair. We refer to the first point {right arrow over (m)}_r as a reference point and the second point {right arrow over (m)}_i as a referred point. A descriptor of the point pair feature is

$\begin{array}{cc}\begin{array}{c}\overrightarrow{F}\left({\overrightarrow{m}}_r,{\overrightarrow{m}}_i\right)=\left(f_1,f_2,f_3,f_4\right)\\ =\left({\overrightarrow{d}}_2,\angle \left({\overrightarrow{n}}_r,\overrightarrow{d}\right),\angle \left({\overrightarrow{n}}_i,\overrightarrow{d}\right),\angle \left({\overrightarrow{n}}_r,\overrightarrow{n_i}\right)\right),\end{array}& \left(1\right)\end{array}$
where {right arrow over (d)}={right arrow over (m)}_i−{right arrow over (m)}_r is the displacement vector between the two points. The 4D descriptor is defined by: the distance (f₁=∥d∥) between the points m_r and m_i and angles (f₂, f₃, f₄). The angle f₂ is between the displacement vector d and the normal vector n_r, the angle f₃ is between d and the vector n_i, and the angle f₃ is between the vectors n_r and n_i. This descriptor is pose invariant.

In other embodiments, the method uses other 3D features such as a spin image, or 3D scale-invariant feature transform (SIFT) image. The spin image is a surface representation that can be used for surface matching and object recognition in 3D scenes. The spin images encodes global properties of the surface in an object-oriented coordinate system, rather than in a viewer-oriented coordinate system. The SIFT image features provide a set of features that are not affected by object scaling and rotation.

Pose Determination

As shown in FIG. 1B, the embodiments of our invention provide the method 71 for determining the pose 170 of the object 40 in a scene 102. The 3D scene data (measurements) 100 are acquired by the sensor 30. Scene features 120 are determined 110 by selecting several point pairs in the scene. For each scene feature, a descriptor is constructed, see Equation above (1).

For the purpose of this description, the features used during online processing are referred to as scene features 120. The features learned during offline training are referred to as model features 180.

As shown in FIG. 2, the model features 180 are determined from a 3D model of the object. Each model feature is associated with a weight which is also learned during training. It is also noted that the way the scene and model features are extracted and constructed in descriptor vectors is identical during offline training and online operation. Hence, when we refer to the feature in general we mean both the scene and model features.

As shown in FIG. 1, the scene feature is matched 140 with the model features 180 using a distance function. The distance function can be any function that determines the distance between the two descriptor vectors, such as a Euclidean distance, an L₁ distance or a Hamming distance on a quantization of features. If the distance of the descriptors is less than a predetermined threshold, then the features match. The matched features and their orientations are aligned in 3D space. Each matched feature generates a weighted candidate pose 150 where the weight is proportional to model feature weight. In some embodiments, the model features and their weights are determined using a training process 230 shown in FIG. 2.

The candidate poses are merged 160 by clustering. If two candidate poses are closer than a clustering threshold, then the candidate poses are combined into a single candidate pose by taking a weighted sum of the two candidate poses. Clustering is repeated until no two candidate poses are closer than the clustering threshold. Then, the object pose 170 is the candidate pose that has a weight larger than a threshold.

Training

FIG. 2 shows a method for learning the model features and their weights 180 using training data 200. The training data include a set of 3D scenes and ground truth poses of the objects in the set of scenes. The training data can be collected by scanning real 3D scenes and manually labeling object poses, or from rendering computer aided design (CAD) models of the objects at various poses (ground truth). For each training scene, training vectors 220 are determined 210 for a training process 230. The method provides a set of model features that are “discriminative” and their weights 180 according to their importance for the pose determination task.

Determining Features and Weights

The pose of the object is the candidate pose with a maximal weight among all the candidate poses. The goal of the learning is to select and weight the model features to ensure that the correct pose receives more weights than other poses.

Weighting Schemes

There are several different ways of weighting the features. The simplest form is based on assigning each model point pair with a different weight, and any scene point pair feature that matches to a given model point pair feature generates a candidate pose equal to this weight. Although this weighting scheme is very general, learning is very underdetermined due to a high dimensional weight space.

Alternatively, we can group sets of features to have the same weights. One such strategy is weighting using quantization of the model feature descriptors. A single weight is defined for all the model point pair features that have the same quantized descriptor m. Any scene point pair feature that is matched to the same quantized descriptor generates a candidate pose that is proportional to this weight. Because the point pairs are grouped into clusters that are mapping to the same quantized descriptor, a dimension of the weight space is reduced to the number of quantization levels M.

An important advantage of this method is that it is possible to learn a weight vector that is sparse. As used herein, sparse is not a relative term. In the art of numerical analysis, sparsity refers to data where most elements are zero. In such cases, the method immediately removes any scene features mapping to a quantized descriptor with zero weight. This reduces processing time significantly.

A second grouping strategy is based on weighting model points. The weight is defined for each model point. Any scene pair that maps to either first or second point of the pair generates candidate poses with this weight. This approach significantly reduces the dimension of the weight space, allowing efficient learning, and directly identifies important points on the model surface.

Given data in the form of a 3D point cloud of a scene containing the object, let S be the set of all scene pair features that are determined from this scene. Without loss of generality, we use weighting using quantization scheme described above. Let yεSE(3) be a candidate pose. The corresponding training vector x_yεR^M is given by a mapping Φ of all the scene pair features that generates a pose y, x=Φ_y(S). When the correspondence is clear from the context, we do not use the subscript and write x instead of x_y.

For weighting using quantization, this mapping is given by the number of times a quantized descriptor m generates a pose y:

$\begin{array}{cc}\begin{array}{c}{x}^m={\Phi }_y^m\left(S\right)\\ =\sum _{s\in S}^{}\sum _{v\in y\left(s\right)}{I}_{\left(h\left(s\right)=m\&y=v\right)},\end{array}& \left(2\right)\end{array}$
where x^m is the m^th dimension of training vector x, Iε{0,1} is an indicator function and, y(s) is the set of poses that the pair s generates, and h(s) is the quantized descriptor of feature s. Note that, a pair s can generate multiple candidate poses.

The weight that a pose receives from all scene pair features can be written using the linear map w^Tx, where w is an M-dimensional weight vector. When w=1_M, this function is equivalent to a uniform weighting function.

Learning Weights

Let I be a 3D scene and y* be the pose of the object in the scene. The goal of learning weights is to determine a non-negative weight vector that satisfies the constraints
w^Tx*>w^Tx,∀y≠y*,∀I.  (3)

This means that for all the 3D scenes I, the true pose of the object y* should have more weight than any other pose.

To solve this problem, we use machine learning because a closed form solution is generally unavailable. Let {(I_i,y_i*)}_{i=1 . . . N} be the training data 200 including N 3D scenes and ground truth poses of the object. For each training scene model, the features are determined. The constraints given in Equation (3) might not define a feasible set. Instead, we reformulate learning the weights as a regularized soft constraint optimization

$\begin{array}{cc}{w}^{*}=\underset{w}{\arg \min }\sum _1{\xi }_i+\lambda R\left(w\right),& \left(4\right)\\ s.t.w^T\left(x_i^{*}-x\right)\ge \Delta \left(y_i^{*},y\right)-{\xi }_i,\forall i,\forall y\ne y_i^{*},& \left(5\right)\\ w_u\ge w\ge 0,{\xi }_i\ge 0,& \left(6\right)\end{array}$
where R(w) is a convex regularizer on the weight vector, λ is a tradeoff between margin and training error, and a loss function Δ(y_i*,y) gives a larger penalty to large pose deviation. We also use an explicit upper bound w_u on the maximal weight of a feature dimension.

We use a cutting plane method to find the weight vector. The cutting-plane method iteratively refines a feasible set or objective function by means of linear inequalities, called cuts.

At each iteration k of the learning process 230, we use the previous set of weights w^(k-1) and solve the pose estimation problem for each scene I_i using the pose estimation method. In addition to the best pose, this method provides the set of candidate poses, which are sorted according to the weights. For all candidate poses, we evaluate a margin constraint
(w^(k-1))^T(x_i*−x)≧Δ(y_i*,y),  (7)
and add the most violated constraint to the selected constraint list. Let y^(1:k) and x^(1:k) be the set of all selected poses and constraints up to iteration k. Then, the optimization problem at iteration k is

$\begin{array}{cc}{w}^{\left(k\right)}=\underset{w}{\arg \min }\sum _i\sum _j{\xi }_{i,j}+\lambda R\left(w\right),& \left(8\right)\\ s.t.w^T\left(x_i^{*}-x_{i,j}^{1:k}\right)\ge \Delta \left(y_i^{*},y_{i,j}^{\left(1:k\right)}\right)-{\xi }_{i,j},\forall i,j,& \left(9\right)\\ w_u\ge w\ge 0,{\xi }_{i,j}\ge 0.& \left(10\right)\end{array}$

This optimization problem is convex and has a finite number of constraints, which we solve optimally. Note that, dimensionality of the training vectors can be large, e.g., M>>10⁵. Fortunately, the training vectors are sparse. Therefore, the optimization can be solved efficiently using convex programming solvers and utilizing sparse matrices and sparse linear algebra.

In general, the method requires fewer iterations if multiple violated constraints are added to the supporting constraint set at a given time, leading to faster operation. We usually solve three or four iterations of the cutting plane method. We initialize with a uniform voting w⁽⁰⁾=1_M, which significantly speeds up the convergence of the method.

Pseudocode for the training method is shown in FIG. 3. The variables and steps used by the pseudo code are described in herein.

Implementation Details

Training Data

Pair features are invariant to the action of the group of 3D rigid motions SE(3). However, we only observe the scene from a single viewpoint, self occlusions, hidden surfaces and measurement noise play a major role in variability of the training vectors x. This variation is largely independent of the 3D translation and rotation of the object along the viewing direction, whereas in general the variation is very sensitive to out-of-plane rotation angles.

Therefore, we sample a set of 3D poses {y_i*}_{i=1 . . . N}, by regularly sampling along two axes of out-of-plane rotation angles on a unit sphere, and appending a random in-plane rotation angle and a translation vector. In addition we add few random objects to the scene to generate background clutter, and render the scene to generate the 3D point cloud.

Alternatively training data can be collected by scanning a real scene containing the target object using a range sensor. The true 6-degrees-of-freedom (DoF) pose of the object can be labeled manually.

Loss Function

We use a loss function
Δ(y,y_i)=1+λ_θθ(y,y_i),  (11)
where θ(y,y_i)=∥log(R_y⁻¹R_yi)∥₂ is the geodesic distance between the two rotation matrices encoded in 6-DoF poses y and y_i. The constant 1 puts a fixed margin between two non identical poses and λ_θ is a weighting factor between fixed margin and its scaled counterpart, based on the orientation difference. We ignore translational component of pose y because the data are largely insensitive to translation as described above.

Regularization

The form of the regularization function plays an important role in accuracy and efficiency of the voting method. In some embodiments, we use a quadratic regularization function R(w)=w^Tw. In other embodiments, we use L₁-norm regularizer ∥w∥₁, which sparsifies the weight vector leading to sparse feature selection and fast operation.

Optimization of Pose Estimation Using Weighted Voting

Pose estimation is optimized using a weighted voting scheme. FIG. 5 shows the voting scheme schematically. As shown in FIG. 4, the voting-based pose estimation uses, for example, point pair features {m_r, m_i}. The 4D descriptor (f₁, f₂, f₃, f₄) is defined above.

As shown in FIG. 5, the model point pair features {a, b, c} 180 are stored in a hash table 501 using their descriptors as the keys. Each scene point pair feature 120 is matched with multiple model point pair features using the hash table. Each matched model point pair feature votes for an entry in a 2D accumulator space 502, corresponding to a particular pose. The weights of the votes, e.g., +w^a, +w^b, and +w^c, are proportional to the weight of the model point pair feature.

The model pair features 180 are stored in the hash table in an offline process for efficiency. The quantized descriptors are served as the hash keys. During an online process, a scene reference point is selected and paired with another scene point to form the scene point pair feature 120. Its descriptor is then used to retrieve matched model point pair features 180 using the hash table. Each of the retrieved model point pair features in the hash bin votes for an entry in the 2D accumulator space.

Each entry in the accumulator space corresponds to a particular object pose. After votes are accumulated for the sampled reference scene point by pairing it with multiple sampled referred scene points, poses supported by a certain number of votes are retrieved. The process is repeated for different reference scene points. Finally, clustering is performed to the retrieved poses to collect support from several reference scene points.

Online Learning of Weights

In one embodiment, training of the model features and their weights are performed online using the robot arm 10. The scene data including the object are acquired using a 3D range sensor. The pose estimation procedure starts with uniform weights. After estimating the pose of the object in the scene, the pose is verified by picking the object from the bin. If the object is successfully picked from the bin, then the scene is added to the training set with the estimated pose as the true pose. If the estimated pose is incorrect, then the scene data are discarded. The scene is altered, e.g., by moving objects with the robot arm, and the process is repeated. The weights of the model features are learned using the generated training set during robot operation.

Application

In one application, as shown in FIG. 1A, the sensor is arranged on the robot arm with a gripper used for bin picking. We use the weighted voting procedure to determine several candidate poses, followed by an iterative closest point (ICP) process to refine each candidate pose. The matching score of the ICP process is used to rank the several candidates and decide how to pick a selected object.

Although the invention has been described by way of examples of preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the invention. Therefore, it is the object of the appended claims to cover all such variations and modifications as come within the true spirit and scope of the invention.

Claims

1. A method for determining a pose of an object in a scene, comprising the steps of:

determining, from a model of the object, model features and a weight associated with each model feature, wherein the model features and the weights are learned using training data by maximizing a difference between a number of votes received by a true pose and a number of votes received by an incorrect pose, and wherein the weights are learned by solving a regularized soft constraint optimization problem;

determining, from scene data acquired of the scene, scene features;

matching the scene features to the model features to obtain a matching scene and matching model features;

generating candidate poses from the matching scene and the matching model features, wherein a weight of each candidate pose is proportional to the weight associated with the matching model feature; and

determining the pose of the object from the candidate poses based on the weights.

2. The method of claim 1, wherein a descriptor is determined for each feature and the matching uses a distance function of two descriptors.

3. The method of claim 2, wherein the descriptor is

4. The method of claim 1, wherein the features are oriented point pair features.

5. The method of claim 1, wherein the pose is determined by clustering the candidate poses.

6. The method of claim 5, wherein the clustering merges two candidate poses by taking a weighted sum of the candidate poses according to the weights associated with the candidate poses.

7. The method of claim 1, wherein the scene data are a 3D point cloud.

8. The method of claim 1, wherein the model features are stored in a hash table, the scene features are matched to the model features using keys into the hash table, and the weights are summed on an accumulator space, wherein each entry of the accumulator space corresponds to a particular object pose.

9. The method of claim 1, further comprising:

picking the object out of a bin according to the pose using a robot arm.

10. The method of claim 9, wherein the model features and the weights are learned using training data acquired during the picking.

11. The method of claim 1, wherein training data are generated from a CAD model of the object and by sampling object poses and rendering the model according to the sampled object poses.

12. The method of claim 1, wherein training data are a 3D point cloud acquired using a range sensor and each object pose is labeled.

13. The method of claim 1, wherein the regularized soft constraint optimization problem uses a loss function to put a fixed margin between two non identical poses.

14. The method of claim 1, wherein the regularized soft constraint optimization problem uses a quadratic regularization function.

15. The method of claim 1, wherein the regularized soft constraint optimization problem uses a L₁-norm regularizer.

16. The method of claim 1, wherein the regularized soft constraint optimization problem has linear constraints.

17. An apparatus for determining a pose of an object in a scene, comprising:

a robot arm;

a sensor arranged on the robot arm, wherein the sensor is configured to acquire scene data; and

a processor configured to

determine scene features from the scene data,

match the scene features to model features to obtain matching scene and model features, wherein each model feature is associated with a weight, wherein the model features and the weights are learned using training data by maximizing a difference between a number of votes received by a true pose and a number of votes received by an incorrect pose, and wherein the weights are learned by solving a regularized soft constraint optimization problem,

generate candidate poses from the matching scene and model features, wherein a weight of each candidate pose is proportional to the weight associated with the matching model feature, and

determine the pose of the object from the candidate poses based on the weights.

18. The apparatus of claim 17, further comprising:

a gripper mounted on the robot arm for picking the object according to the pose.