## Problem statement

Find $$R,t$$ such that $q_i = Rp_i + t$
IROS 2013. Vikas Dhiman, SUNY Buffalo

## Problem statement

### From robots point of view

IROS 2013. Vikas Dhiman, SUNY Buffalo

## Related work

Single observation
External landmarks
Egomotion
• No mutual observation.
• Example: ARToolKit (Kato et al. 1999)
• Controlled environment.
• Corner features are error prone.
• Example: Bundler (Snavely et al. 2006)
• Needs accurate egomotion estimates.

IROS 2013. Vikas Dhiman, SUNY Buffalo

## Mutual localization

### A generalized Perspective-N-Point algorithm

IROS 2013. Vikas Dhiman, SUNY Buffalo

## Mutual localization

### Using minimum fiducials

• Fiducials can easily be installed on Robots.
• Can fall back to single robot localization.
• No need of egomotion estimates.
IROS 2013. Vikas Dhiman, SUNY Buffalo

## Mathematical formulation

$\newcommand{\vect}[1]{\mathbf{#1}} \newcommand{\hvect}[1]{\bar{\vect{#1}}} \newcommand{\uvect}[1]{\hat{\vect{#1}}} \newcommand{\field}[1]{\mathbb{#1}}$
\begin{align} \|s_1\uvect{p}_1-s_2\uvect{p}_2\| &=\| \vect{q_1}- \vect{q_2}\| \\ \end{align}
\begin{align} \|s_2\uvect{p}_2- \vect{p_3}\| &=\| \vect{q_2}-s_3\uvect{q}_3\| \\ \| \vect{p_3}-s_1\uvect{p}_1\| &=\|s_3\uvect{q}_3- \vect{q_1}\| \end{align}
IROS 2013. Vikas Dhiman, SUNY Buffalo

## Solving for $$s_1$$ $$s_2$$ and $$s_3$$

### Expanding to polynomials

\begin{align} s_1^2 + s_2^2 - 2s_1s_2\uvect{p}_1^\top\uvect{p}_2 - \|\vect{q}_1 - \vect{q}_2\|^2 &= 0 \\ % s_2^2 - s_3^2 - 2s_2\uvect{p}_2^\top\vect{p}_3 + 2s_3\vect{q}_2^\top\uvect{q}_3 + \|\vect{p}_3\|^2 - \|\vect{q}_2\|^2 &= 0 \\ % s_1^2 - s_3^2 - 2s_1\uvect{p}_1^\top\vect{p}_3 + 2s_3\vect{q}_1^\top\uvect{q}_3 + \|\vect{p}_3\|^2 - \|\vect{q}_1\|^2 &= 0 \label{eq31} \end{align}

\begin{align} s_2^2 + \underbrace{(-2s_1\uvect{p}_1^\top\uvect{p}_2)}_{a_1}s_2 + \underbrace{(s_1^2 - \|\vect{q}_1 - \vect{q}_2\|^2)}_{a_0} = 0 \\ s_2^2 + \underbrace{(-2\uvect{p}_2^\top\vect{p}_3)}_{b_1}s_2 - \underbrace{(s_3^2 - 2s_3\vect{q}_2^\top\uvect{q}_3 - \|\vect{p}_3\|^2 + \|\vect{q}_2\|^2)}_{b_0} = 0 \end{align}

IROS 2013. Vikas Dhiman, SUNY Buffalo

## Eliminating $$s_2$$

\begin{align} s_2^2 + \underbrace{(-2s_1\uvect{p}_1^\top\uvect{p}_2)}_{a_1}s_2 + \underbrace{(s_1^2 - \|\vect{q}_1 - \vect{q}_2\|^2)}_{a_0} = 0 \\ s_2^2 + \underbrace{(-2\uvect{p}_2^\top\vect{p}_3)}_{b_1}s_2 - \underbrace{(s_3^2 - 2s_3\vect{q}_2^\top\uvect{q}_3 - \|\vect{p}_3\|^2 + \|\vect{q}_2\|^2)}_{b_0} = 0 \end{align}

• \begin{align} r(s_1, s_3) = \begin{vmatrix} 1 & a_1 & a_0 & 0 \\ 0 & 1 & a_1 & a_0 \\ 1 & b_1 & b_0 & 0 \\ 0 & 1 & b_1 & b_0 \end{vmatrix} = 0 \label{eq:sylvester} \end{align}
IROS 2013. Vikas Dhiman, SUNY Buffalo

## Eliminating $$s_3$$

$$\begin{split} r(s_1, s_3) = c_4s_3^4 + c_3s_3^3 + c_2s_3^2 + c_1s_3 + c_0 &= 0\\ -s_3^2 + \underbrace{(2\vect{q}_1^\top\uvect{q}_3)}_{d_1}s_3 + \underbrace{s_1^2 - 2s_1\uvect{p}_1^\top\vect{p}_3 + \|\vect{p}_3\|^2 - \|\vect{q}_1\|^2}_{d_0} &= 0 \end{split}$$

• \begin{align} r_2(s_1) = \begin{vmatrix} c_4 & c_3 & c_2 & c_1 & c_0 & 0 \\ 0 & c_4 & c_3 & c_2 & c_1 & c_0\\ 1 & d_1 & d_0 & 0 & 0 & 0 \\ 0 & 1 & d_1 & d_0 & 0 & 0 \\ 0 & 0 & 1 & d_1 & d_0 & 0 \\ 0 & 0 & 0 & 1 & d_1 & d_0 \end{vmatrix} = 0. \label{eq:finalsylvester} \end{align}
IROS 2013. Vikas Dhiman, SUNY Buffalo

## Too many roots

Out of the eight roots, only one is right. Which one?

IROS 2013. Vikas Dhiman, SUNY Buffalo

## Choosing the optimal root

$$\uvect{p}_1^\top\vect{p}_3$$
\begin{align*} s_2 - \uvect{p}_2^\top\vect{p}_3 &> 0\\ s_3 - \vect{q}_2^\top\uvect{q}_3 &> 0\\ s_1 - \uvect{p}_1^\top\vect{p}_3 &> 0\\ \end{align*}
\begin{align} (s_1 + s_2)^2& - 2s_1s_2(1 + \uvect{p}_1^\top\uvect{p}_2) - \|\vect{q}_1 - \vect{q}_2\|^2 = 0 \end{align} \begin{align} (s_2 - \uvect{p}_2^\top\vect{p}_3)^2 - (s_3 - \vect{q}_2^\top\uvect{q}_3)^2 + (\vect{p}_3 - \uvect{p}_2)^\top\vect{p}_3 - \vect{q}_2^\top(\vect{q}_2 - \uvect{q}_3) = 0\\ \end{align} \begin{align} (s_1 - \uvect{p}_1^\top\vect{p}_3)^2 - (s_3 - \vect{q}_1^\top\uvect{q}_3)^2 + (\vect{p}_3 - \uvect{p}_1)^\top\vect{p}_3 - \vect{q}_1^\top(\vect{q}_1 - \uvect{q}_3) = 0\\ \end{align}

IROS 2013. Vikas Dhiman, SUNY Buffalo

• For four markers solve polynomial .
• For more than 5 markers, we can use the homography method. \begin{align*} \vect{q}_i^\top E \uvect{p}_i &= 0 & \forall i \in {1, 2}\\ \uvect{q}_i^\top E \vect{p}_i &= 0 & \forall i \in {3, 4, 5} \label{eq:essentialmatrix} \end{align*}
• For more than 8 markers, the system becomes linear.
IROS 2013. Vikas Dhiman, SUNY Buffalo

## Stability against noise

### Experimental setup

IROS 2013. Vikas Dhiman, SUNY Buffalo

## Stability against noise

### Results

IROS 2013. Vikas Dhiman, SUNY Buffalo

## Experimental setup: Fiducial markers on turtlebot

IROS 2013. Vikas Dhiman, SUNY Buffalo

## Experimental setup

### Robot placement

IROS 2013. Vikas Dhiman, SUNY Buffalo

## Experimental results

### Translation error

IROS 2013. Vikas Dhiman, SUNY Buffalo

## Experimental setup

### Realtime mutual localization

IROS 2013. Vikas Dhiman, SUNY Buffalo

## Conclusion

• Localization using fiducial without egomotion
• Generalization of Perspective-N-point algorithm
• Compared accuracy against state of art methods
• Source code and data is available at buffalo.edu/~vikasdhi/
IROS 2013. Vikas Dhiman, SUNY Buffalo

## Questions?

### Acknowledgements

This material is based upon work partially supported by the Federal Highway Administration under Cooperative Agreement No. DTFH61-07-H-00023, the Army Research Office (W911NF-11-1-0090) and the National Science Foundation CAREER grant (IIS-0845282). Any opinions, findings, conclusions or recommendations are those of the authors and do not necessarily reflect the views of the FHWA, ARO, or NSF.