# Staircase Detection, Characterization and Approach Pipeline for Search and Rescue Robots
## 2 Materials and Methods
### Staircase Plane Extraction and Characterization
If the angle is below a threshold \(\theta_{th}\) and the distances are below thresholds \(d_{vt}\) and \(d_{ht}\) respectively, then we have obtained two regions belonging to the same staircase riser. After joining these two regions, we remove the neighbor that met the criteria from \(\{P_{c}\}\) and use it as the next seed point and execute the mentioned steps. The above process continues until no more smooth regions (corresponding to the seed point neighbors) meet the criteria. When this state is reached during the joining process, we return to the initial seed point and now begin to search and join neighboring smooth surfaces. By returning to the initial seed point, we are able to search in \(D_{total}\) different directions from the initial seed point, and thus joining all the valid regions for each riser. The complete process continues until all of the points in \(\{P_{c}\}\) have been used as seed points. The steps mentioned above are shown in Algorithm 1. Once the points corresponding to each staircase riser have been joined we use a RANSAC algorithm to find a best fitting plane and obtain the corresponding coefficients. To determine the staircase orientation with respect to the camera frame, we project each plane normal into the \(y=0\) plane in the camera's reference frame (XZ plane). The orthogonal projection of a vector \(\vec{v}\) into a subspace \(H\) of \(\mathbb{R}^{n}\) with an orthonormal base \(\{\vec{u}_{1},\vec{u}_{2},\ldots,\vec{u}_{k}\}\) is given by Equation (1) [30]. \[\mathit{proy}_{H}v=(\vec{v}\cdot\vec{u}_{1})\vec{u}_{1}+(\vec{v}\cdot\vec{u}_{ 2})\vec{u}_{2}+\cdots+(\vec{v}\cdot\vec{u}_{k})\vec{u}_{k} \tag{1}\]  
For the plane given by the equation \(y=0\), the orthonormal base is given by: \( \begin{pmatrix}1\\ 0\\ 0\end{pmatrix} \) and \( \begin{pmatrix}0\\ 0\\ 1\end{pmatrix} \). Therefore, the projection of any plane normal \(\vec{v}\) into plane \(\pi=\left\{ \begin{pmatrix}x\\ y\\ z\end{pmatrix} :y=0\right\}\) is given by:  
\[\mathit{proy}_{\pi}\vec{v}=\left[\vec{v}\cdot \begin{pmatrix}1\\ 0\\ 0\end{pmatrix} \right] \begin{pmatrix}1\\ 0\\ 0\end{pmatrix} +\left[\vec{v}\cdot \begin{pmatrix}0\\ 0\\ 1\end{pmatrix} \right] \begin{pmatrix}0\\ 0\\ 1\end{pmatrix}  \tag{2}\]  
Finally, the angle \(\theta\) between normal \(\vec{v}\) and the unit vector \(\vec{x}= \begin{pmatrix}1\\ 0\\ 0\end{pmatrix} \) (representing the right direction in the camera's reference frame) is given by:  
\[\theta=atan2((\vec{x}\times\mathit{proy}_{\pi}\vec{v})\cdot\vec{y},\vec{x} \cdot\mathit{proy}_{\pi}\vec{v}) \tag{3}\]  
where \(atan2\) is the four-quadrant inverse tangent and \(\vec{y}= \begin{pmatrix}0\\ 1\\ 0\end{pmatrix} \) is the vector perpendicular to \(\vec{x}\) and \(\mathit{proy}_{\pi}\vec{v}\). Through this process we are able to determine the orientation of the staircase even when the risers do not have a completely flat surface or when part of the riser is missing. The angle \(\theta\) gives the angle, with respect to the camera frame. Furthermore, the segmentation of the staircase into planes allowed the measurement of thread and riser size by measuring the vertical and horizontal distance between the centroid of each cluster of points (represented by planes). With this information, stair angle can also be determined. **Inputs:** Point cloud of centroids = \(\{P_{c}\}\), point normals = \(\{N\}\),  
clusters of smooth regions = \(\{C_{p}\}\), neighbor finding function \(\Omega(.