# RGB-D-Based Robotic Grasping in Fusion Application Environments
## 2 Materials and Methods
### Plane Extraction, Grasping Pose Calculation and Contact Point Chosen  
Since all the components to be grasped in this application scenario are very regular aggregates, there are very standard planes in these structured data, and the Random Sample Consensus (RANSAC) [19] plane extraction algorithm achieves good results in this case. The RANSAC-based plane extraction in this paper is shown in Algorithm 2. Once the top plane of the object has been extracted, we can easily calculate the grasping pose.  
As shown in Figure 4, the grasping pose has several elements, such as \(\textbf{P},W\) and \(\theta\), \(\{P_{a},P_{b},P_{c},P_{d}\}\) are the four corner points of the extracted plane. For each corner point, \(P=\{x,y,z,u,v\}\), \(\{u,v\}\) are the pixel coordinates of the images, and \(\{x,y,z\}\) are the 3D coordinates calculated through Formula (1).  
```
0: Point clouds \(P=\{p_{1},p_{2},...,p_{n}\}\), initialise the parameter \(n=3\), iteration times \(k=1000\) and the distance threshold \(t=3 mm\)
1: Initializing \(iteration=0\), \(Plane_{best}=Null\), \(InlierSet_{best}=\emptyset\), \(OutlierSet_{best}=\emptyset\), \(SumDistance_{best}=0\)
2:for\(iteration<k\)do
3: Randomly pick \(n\) points set \(P_{s}\) from \(P\), calculate the \(Plane_{cur}\) and initializing \(InlierSet_{cur}=\emptyset\), \(OutlierSet_{cur}=\emptyset\), \(SumDistance_{cur}=0\)
4:for each \(p_{m}\in(P-P_{s})\)do
5: Calculate the distance \(d\) from point \(p_{m}\) to the \(Plane_{cur}\)
6:if\(d<t\)then
7: Updating \(InlierSet_{cur}=InlierSet_{cur}\cup p_{m}\), \(SumDistance_{cur}=SumDistance_{cur}+d\)
8:else
9: Updating \(OutlierSet_{cur}=OutlierSet_{cur}\cup p_{m}\)
10:endif
11:endfor
12:if\(|InlierSet_{cur}|>|InlierSet_{best}|\)then
13:\(Plane_{best}=Plane_{cur}\)
14:\(InlierSet_{best}=InlierSet_{cur}\)
15:\(OutlierSet_{best}=OutlierSet_{cur}\)
16:\(SumDistance_{best}=SumDistance_{cur}\)
17:elseif\(|InlierSet_{cur}|=|InlierSet_{best}|\) & \(SumDistance_{best}>SumDistance_{cur}\)then
18:\(Plane_{best}=Plane_{cur}\)
19:\(InlierSet_{best}=InlierSet_{cur}\)
20:\(OutlierSet_{best}=OutlierSet_{cur}\)
21:\(SumDistance_{best}=SumDistance_{cur}\)
22:endif
23:\(iteration=iteration+1\)
24:endfor
25:\(Plane_{best}\), \(InlierSet_{best},OutlierSet_{best}\)
```  
**Algorithm 2** RANSAC plane extraction  
Figure 4: The elements contained within the grasping pose.  
\(\mathbf{P}=\{x_{c},y_{c},z_{c},V\}\), \(\{x_{c},y_{c},z_{c}\}\) are the 3D coordinates of the centre point of the extracted plane, which could easily be calculated. \(V\) is the rotation vector which could be calculated as:  
\[\begin{split}& V_{ab}=(x_{b}-x_{a},y_{b}-y_{a},z_{b}-z_{a})\\ & V_{ac}=(x_{c}-x_{a},y_{c}-y_{a},z_{c}-z_{a})\\ & V=V_{ab}\times V_{ac}\end{split} \tag{2}\]  
It should be noted that, according to the right-hand rule, the direction of \(V\) is pointing inside the screen, which is in accordance with the pose of the end-effector in the robot's coordinate system. Since we already have the extracted plane, we can easily obtain the four corner points: \(P_{a}\), \(P_{b}\), \(P_{c}\) and \(P_{d}\). Thus, the \(\theta\) can be calculated as:  
\[\theta=arctan(\frac{y_{a}-y_{b}}{x_{a}-x_{b}}) \tag{3}\]  
\(W\) is the distance between \(P_{a}\) and \(P_{c}\).  
We use the three-finger gripper as the end-effector to grasp the rectangular part of the object, so the three contact points are on the two long sides of the rectangle. The method we propose (called HUDR) is a hybrid one combining UOIS with DBSCAN and RANSAC.