# Detecting and Solving Tube Entanglement in Bin Picking Operations
## 3 Bin Picking Algorithm
### Tube Modeling  
The first phase consists of creating a model for each tube that is present in the bin. This phase uses as input a point cloud of the scene captured by a 3D scanner. These models are used in the subsequent phases to plan the robot's trajectory and grasping pose. The modeling phase assumes that the tube radius is known and, optionally, an upper bound for the tube's length may be provided. Before presenting the main steps for the modeling, it is important to define some keywords that are used in the rest of the article. Definition 1: A **real-life tube** corresponds to a tube that is actually present in the bin. Definition 2: A **virtual tube** corresponds to a tube that was created during the modeling phase. It consists of an ordered list of cylinders and joints. Figure 1: UML activity diagram of the bin picking algorithm. If the modeling is performed correctly, then there is a one-to-one mapping between the real-life and virtual tubes. An **endpoint of a cylinder** is a three-dimensional point at an intersection between the cylinder's central axis and one of its bases. Each cylinder of a virtual tube has two endpoints. A **joint** is an entity that connects two consecutive cylinders of a virtual tube by their closest endpoints. An **endpoint of a virtual tube** is an endpoint of one of its cylinders that is not connected to any other endpoint by a joint. Each virtual tube has two endpoints. The **length of a virtual tube** is the sum of the combined length of its cylinders and joints (whose length is the distance between its endpoints). The four steps of this phase are: filtering, segmentation, cylinder fitting, and tube joining [3]. During the filtering step, all of the cloud points that do not belong to any of the tubes are removed. In addition, the cloud is downsampled randomly to a certain amount of points so that the next steps are performed more efficiently. The segmentation step aims to cluster the point cloud into disjoint piece-wise smooth regions. This is performed using a region-growing algorithm based on the surface normals and curvatures. The normals are estimated using a least-square method that fits a tangent plane to each point using the covariance matrix formed by the neighboring points from the raw point cloud [19]. The curvature of each point is computed using the eigenvalues of this matrix. A **segment** is a subset of the filtered point cloud where the surface normal varies continuously. It corresponds to one of the clusters given by the segmentation step. The cylinder fitting step processes each segment independently. For each segment, the Random Sample Consensus (RANSAC) method is repeatedly used to fit a cylinder to a section of it. A virtual tube is created for each cylinder. The outliers of one call to RANSAC are used as input in the next call for the same segment, until the number of outliers is too low to form a new cylinder. The result of this step is a set of tubes composed of a single cylinder and no joints. Finally, the tube joining step aims to combine smaller tubes into longer virtual tubes. Whenever two tubes are combined, a new joint is created between the closest endpoints of both tubes. To perform this step, all pairs of endpoints of distinct virtual tubes are considered and a cost function that accounts for their Euclidean and angular distances is computed. Then, each pair is processed in ascending order of cost and the virtual tubes to which each of the endpoints belongs are combined if four conditions are satisfied:  
* The cylinder endpoints still belong to distinct tubes. * The pair's Euclidean and angular distances are both below a certain threshold. * There is no visible gap between both endpoints on the acquired point cloud's representation as a depth image, from the sensor's point of view (visibility constraint).