A Framework for Depth Estimation and Relative Localization of Ground Robots using Computer Vision

TL;DR

This paper presents a continuous estimation framework for depth and relative localization of ground robots using computer vision, considering robot dynamics and validated through simulations and experiments.

Contribution

It introduces a novel continuous estimation framework that jointly estimates depth and relative pose, incorporating robot dynamics for ground robot coordination.

Findings

Framework effectively estimates depth and relative pose in simulations.

Experimental validation confirms the framework's practical applicability.

Convergence analysis demonstrates robustness of the proposed method.

Abstract

The 3D depth estimation and relative pose estimation problem within a decentralized architecture is a challenging problem that arises in missions that require coordination among multiple vision-controlled robots. The depth estimation problem aims at recovering the 3D information of the environment. The relative localization problem consists of estimating the relative pose between two robots, by sensing each other's pose or sharing information about the perceived environment. Most solutions for these problems use a set of discrete data without taking into account the chronological order of the events. This paper builds on recent results on continuous estimation to propose a framework that estimates the depth and relative pose between two non-holonomic vehicles. The basic idea consists in estimating the depth of the points by explicitly considering the dynamics of the camera mounted on a ground robot, and feeding the estimates of 3D points observed by both cameras in a filter that computes the relative pose between the robots. We evaluate the convergence for a set of simulated scenarios and show experimental results validating the proposed framework.

Tables

Table 1. TABLE I: List of Notations for the relative pose estimation filter.

Variable	Description
$A$, $B$	Vehicles
$\left\{A\right\}$, $\left\{B\right\}$	Body-fixed coordinate frames
${𝐮}_A$, ${𝐮}_B$	Control input of camera A and B
${\left({𝐬}_i^A\right)}_{i=1}^m$, ${\left({𝐬}_i^B\right)}_{i=1}^m$	Normalized coordinates of the 3D points
${\left(Z_i^A\right)}_{i=1}^m$, ${\left(Z_i^B\right)}_{i=1}^m$	Depth of the 3D points
$𝝃$	Pose of $B$ w.r.t $A$ described in $\left\{A\right\}$

Equations

$$\dot{\mathbf{p}}=-\mathbf{w}\times \mathbf{p}-\mathbf{v}=\{\begin{array}{c}\dot{\bar{x}}=\bar{y}{w}_z-\bar{z}{w}_y-v_x\\ \dot{\bar{y}}=\bar{z}{w}_x-\bar{x}{w}_z-v_y\\ \dot{\bar{z}}=\bar{x}{w}_y-\bar{y}{w}_x-v_z\end{array},$$

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right]=\left[\begin{array}{cccccc}-\frac{1}{\bar{z}}& 0& \frac{x}{\bar{z}}& xy& -(1+x^2)& y\\ 0& -\frac{1}{\bar{z}}& \frac{y}{\bar{z}}& 1+y^2& -xy& -x\end{array}\right]\left[\begin{array}{c}\mathbf{v}\\ \mathbf{w}\end{array}\right].$$

$$\dot{\mathbf{s}}$$

$$\dot{\chi }$$

$$\{\begin{array}{ccc}{f}_m(\mathbf{s},\mathbf{u},t)& =& w_{\theta }\left[\begin{array}{c}(1+x^2)\\ xy\end{array}\right]\\ \Omega (t)& =& v_d\left[\begin{array}{cc}x& y\end{array}\right]\\ f_u(\mathbf{s},\mathbf{u},\chi ,t)& =& v_d{\chi }^2+x{w}_{\theta }\chi \end{array}.$$

$$\dot{\hat{\mathbf{s}}}$$

$$\dot{\hat{\chi }}$$

$${\int }_t^{t+T}\Omega (\tau ){\Omega }^T(\tau )d\tau \ge \gamma I>0,t\ge 0,$$

$$\Vert \Omega (t){\Vert }^2=(x^2+y^2)v_d^2.$$

$$\{\begin{array}{rl}{\dot{r}}_{A,x}& =v_{A,d}\cos {\theta }_A\\ {\dot{r}}_{A,x}& =v_{A,d}\sin {\theta }_A\\ {\dot{\theta }}_A& =w_{A,\theta }\end{array}\text{ and }\{\begin{array}{rl}{\dot{r}}_{B,x}& =v_{B,d}\cos {\theta }_B\\ {\dot{r}}_{B,y}& =v_{B,d}\sin {\theta }_B\\ {\dot{\theta }}_B& =w_{B,\theta }\end{array}.$$

$$\mathbf{\xi }=\left[\begin{array}{c}\mathbf{r}\\ \theta \end{array}\right]=\left[\begin{array}{c}{R}^T({\theta }_A)({\mathbf{r}}_B-{\mathbf{r}}_A)\\ {\theta }_B-{\theta }_A\end{array}\right],$$

$$\dot{\mathbf{\xi }}=\left[\begin{array}{c}{\dot{R}}^T({\theta }_A)({\mathbf{r}}_B-{\mathbf{r}}_A)+R^T({\theta }_A)({\dot{\mathbf{r}}}_B-{\dot{\mathbf{r}}}_A)\\ {\dot{\theta }}_B-{\dot{\theta }}_A\end{array}\right],$$

$$R(a)=\left[\begin{array}{cc}\cos a& -\sin a\\ \sin a& \cos a\end{array}\right],\text{ and }S(\dot{a})=\left[\begin{array}{cc}0& -\dot{a}\\ \dot{a}& 0\end{array}\right].$$

$$\dot{\mathbf{\xi }}=\left[\begin{array}{c}-S({\dot{\theta }}_A)\mathbf{r}+R^T({\theta }_A)({\dot{\mathbf{r}}}_B-{\dot{\mathbf{r}}}_A)\\ {\dot{\theta }}_B-{\dot{\theta }}_A\end{array}\right].$$

$$\dot{\mathbf{\xi }}=f(\mathbf{\xi },{\mathbf{u}}_A,{\mathbf{u}}_B)=\left[\begin{array}{c}{v}_{B,d}\cos \theta -v_{A,d}+w_{A,\theta }{r}_y\\ v_{B,d}\sin \theta -w_{A,\theta }{r}_x\\ w_{B,\theta }-w_{A,\theta }\end{array}\right].$$

$$\mathbf{r}={\bar{z}}_i^A{\mathbf{s}}_i^A-R(\theta ){\bar{z}}_i^B{\mathbf{s}}_i^B,$$

$$\dot{\mathbf{r}}={\dot{\bar{z}}}_i^A{\mathbf{s}}_i^A+{\bar{z}}_i^A{\dot{\mathbf{s}}}_i^A-R(\theta )[S(\dot{\theta }){\bar{z}}_i^B{\mathbf{s}}_i^B+{\dot{\bar{z}}}_i^B{\mathbf{s}}_i^B+{\bar{z}}_i^B{\dot{\mathbf{s}}}_i^B].$$

$$\dot{\mathbf{r}}=-\frac{{\dot{\chi }}_i^A}{({\chi }_i^A{)}^2}{\mathbf{s}}_i^A+\frac{{\dot{\mathbf{s}}}_i^A}{{\chi }_i^A}-R(\theta )\left[S(\dot{\theta })\frac{{\mathbf{s}}_i^B}{{\chi }_i^B}-\frac{{\dot{\chi }}_i^B}{({\chi }_i^B{)}^2}{\mathbf{s}}_i^B+\frac{{\dot{\mathbf{s}}}_i^B}{{\chi }_i^B}\right].$$

$$h_i(\mathbf{\xi })=R^T(\theta )[{\dot{\bar{z}}}_i^A{\mathbf{s}}_i^A+{\bar{z}}_i^A{\dot{\mathbf{s}}}_i^A].$$

$${\dot{\hat{\mathbf{\xi }}}}^{-}=f(\dot{\hat{\mathbf{\xi }}},{\mathbf{u}}^A,{\mathbf{u}}^B)$$

$$\dot{\hat{\mathbf{\xi }}}={\dot{\hat{\mathbf{\xi }}}}^{-}+K[{\pi }_i(t)-h_i({\dot{\hat{\mathbf{\xi }}}}^{-})],$$

$${\pi }_i(t)=S(\dot{\theta }){\bar{z}}_i^B{\mathbf{s}}_i^B+{\dot{\bar{z}}}_i^B{\mathbf{s}}_i^B+{\bar{z}}_i^B{\dot{\mathbf{s}}}_i^B$$

Full text

I INTRODUCTION

The decentralized coordinated control of multiple mobile robots has several applications, including exploration [1], coverage [2], sampling and patrolling [3], search & rescue [4], and manipulation [5]. Achieving coordination within a decentralized architecture requires each agent in the formation to have some knowledge of the pose of its neighboring agents. The relative pose information allows to locally optimize a goal function, re-plan trajectories, and avoid collisions. However, for some mission scenarios such as mine caves or catastrophic areas, it is particularly challenging to obtain an accurate estimate of the relative pose due to the constrained communication channel bandwidth and impossibility to install or employ a high-precision positioning system, e.g., geolocation devices (GPS and Galileo) or Motion Capture systems. This paper aims at studying the problem of relative localization for two autonomous non-holonomic ground robots using 3D points observed by an onboard visual system (See Fig. 1). Since cameras observe 3D points up to an unknown scale, the formulation considers the problem of concurrently estimating the unknown depth of the 3D points.

In monocular computer vision, the mapping task aims at estimating the inherent 3D structure of the world from the 2D pixels in the image frame [6]. Considering two or more sequences of images taking from different views in a static environment, the depth of the points is computed by estimating the 3D points where the correspondent projection rays intersect. More sophisticated solutions have been proposed [7, 8, 9]. However, these works do not take into account the motion imparted to the camera. Instead it is considered a sequence of images captured at a known pose in the environment. Some authors study the estimation of depth maps from a single image (e.g. [10, 11, 12]). However, these works assume some high level knowledge of the environment. Recently, continuous 3D depth estimation has gained considerable attention (e.g. [13, 14, 15, 16, 17]). The goal is to minimize the estimation error of the 3D features over a continuous sequence of images ordered in time, by accounting for the dynamics of the camera. In contrast to other solutions that aim at algebraically estimating the unknown parameters as a fitting problem, continuous estimation considers the error dynamics and its convergence to the origin over time. This strategy benefits from: 1) being able to provide a metric about the quality of their estimates, e.g. by measuring the error’s covariance or convergence rate; and 2) having robust performance by continuously improving the estimation based on new views. These advantages are particularly important for image based visual servoing schemes [18, 19], in which the depth of the 3D features are fed in the control loop.

Relative pose estimation is another active topic in the robotics and computer vision communities. From [20], geometrically one can see that the minimum required number of 2D point correspondences is five. The number of required point correspondences is reduced even more if the motion is constrained in a plane. In this case, only two 2D point correspondences are required [21]. One of the disadvantages of these methods is that they estimate the translation parameters up to a scale. This means that one needs to get additional information about the observed environment to get the complete relative pose. Over the years, there have been many approaches to solve the relative pose problem, such as [22, 23, 24]. However, all of these methods were built on the assumption that the data (2D images) were discretely acquired over time, without any continuously time assumption. In [25], the authors propose a continuous perception strategy to estimate the relative localization of a set of robots in formation. Their method is based on the bearing-rigidity of the formation, and requires that the at least a pair robots should be in mutual visibility. In our work, we also aim at estimating the relative pose of the robots as in [25].

The scenario considered in this paper includes two mobile robots, navigating autonomously in an unknown environment. Both robots are equipped with perspective cameras. While previous solutions for this problem consider a set of two or more images from the environment or use some special fleet configuration (e.g. the robots are in each others’ fields of view or have the ability of sensing the bearing-information about each others’ positions), in this paper we propose a framework that shares a set of common observations of the environment in the respective local frame of each robot (3D point features are employed). A graphical representation of the problem is shown in Fig. 1. In this scenario, as will be shown in the experimental results, the proposed solution is able to estimate the 3 degrees of freedom (DoF) relative pose between the two robots using 3D points which are being continuously estimated by the two robotic agents.

To compute the relative pose, we use the fact that the two agents are tracking in the image plane two point correspondences. The proposed pipeline estimates the depth of the points seen by both robots while computing their relative pose. In contrast to [21], by taking account a continuous estimation of the relative transformation, our method is able to get the complete relative pose (the translation is not estimated up to a scale). As the robots navigate in the environment, they will be estimating the depth of the points being tracked in their respective image frames using a depth estimation filter. Then, an Extended Kalman Filter (EKF) receives the estimated depths to estimate the relative pose of the robots.

This paper is organized as follows. The next section presents an overview of the system proposed in this paper. In Sec. III we describe the proposed method for depth and relative pose estimation. Results with simulated and real data are shown in Sec. IV, and in Sec. V we discuss and conclude the paper.

II SYSTEM OVERVIEW

This section presents the problem studied in this paper, an overview of the proposed system architecture, and the basic mathematical notation that holds for the remainder.

The problem discussed here is two-fold: 1) each agent has to compute the unknown depth of the points to recover the 3D structure of the world; and 2) each agent has to estimate the relative pose of its neighboring agent. The robot and camera configuration is shown in Fig. 2, and the proposed pipeline solution is shown in Fig. 3.

The cameras are mounted at the front of a non-holonomic vehicle, in which the optical axis of the sensor is aligned with the forward moving direction (see Fig. 2). Without loss of generality, we assume that the camera lies in the origin of the vehicle. The robot actuation, denoted as $\mathbf{u}=[v_{d},w_{\theta}]^{T}\in\mathbb{R}^{2}$, consists of a forward and an angular speed command. This translates into a linear speed along the optical axis of the camera and an angular speed around the $y$-axis of the camera.

A depth estimation filter runs in each agent independently, recovering the depth of the 3D points. The depth filter, discussed in Sec. III-A, is an particularization of [14], constrained for cameras mounted on non-holonomic planar vehicles. Then, an Extended Kalman Filter (EKF) gathers the motion of each camera and the coordinates of the observed points to estimate the relative pose between the vehicles. This requires that one vehicle sends its 3D points estimation and actuation commands to its neighboring vehicle through the network. This is discussed in Sec. III-B. The EKF allows to capture the uncertainty inherent to the problem: depths converge online and measurements and actuation commands are subjected to noise.

The proposed pipeline includes two additional modules: image processing and network. The former is related with the detection and tracking of a 3D target on the image plane. For that purpose, one can use techniques such as SIFT [26], SURF [27], or ORB [28]. As a proof of concept, a color scheme detection is employed. It also eases the match of the correspondences between the points detected by the two robots. The network module enable the robots to communicate with each other. In practice, the vehicles are equipped with WiFi.

The mathematical notation adopted in this document is as follows: vectors are written in lower case bold font; matrices are written in upper case font; coordinate frames are denoted within curly brackets; and a subscript indicates the coordinate frame that a vector is described in (e.g. $\mathbf{u}_{A}$ stands for the control input of camera $A$).

III METHODS

This section presents the methods proposed for the simultaneous depth and relative pose localization of ground robots. We start by describing the used depth filter (Sec. III-A) for a single vehicle/point. Then, the relative pose filter is presented (Sec. III-B).

III-A Depth Filter

This method is based on the framework proposed in [13], adapted to the motion of ground vehicles. Consider a camera moving freely in the 3D space, the dynamic of a point $\mathbf{p}=[\bar{x},\bar{y},\bar{z}]^{T}$ relative to the camera frame is

[TABLE]

where $\mathbf{v}=[v_{x},v_{y},v_{z}]^{T}\in\mathbb{R}^{3}$ and $\mathbf{w}=[w_{x},w_{y},w_{z}]^{T}\in\mathbb{R}^{3}$ are the camera linear and angular velocity described in the camera frame. Since only the direction of the point is known, it is common practice to use the normalized image coordinates $\mathbf{s}=[\bar{x}/\bar{z},\bar{y}/\bar{z}]^{T}=[x,y]^{T}$. The previous equation can be re-stated as

[TABLE]

As discussed in Sec. II, the vehicle input $\mathbf{u}=[v_{d},w_{\theta}]^{T}$ consists in a forward speed ($v_{d}$) and an angular rate ($w_{\theta}$). The forward direction of the vehicle is aligned with the optical axis of the camera, i.e., the linear velocity expressed in the camera frame satisfies $v_{x}=v_{y}=0$ and $v_{z}=v_{d}$. The rotational axis of the vehicle is aligned with the y–axis of the camera such that $w_{x}=w_{z}=0$ and $w_{y}=-w_{\theta}$. The reader is referred to Fig. 2 for a graphical illustration of the coordination frames at stake. Substituting $\chi=1/\bar{z}$ to obtain a linear expression and simplifying the previous equation by removing the components multiplied by [math], the following dynamic system is obtained:

[TABLE]

where

[TABLE]

In order to estimate the unknown depth $\bar{z}$, the estimation variables $\hat{\mathbf{s}}$ and $\hat{\chi}$ are introduced, as well as the associated estimation errors $\tilde{\bm{s}}=\mathbf{s}-\hat{\mathbf{s}}$ and $\tilde{\chi}={\chi}-\hat{{\chi}}$. For that purpose, the following observer is considered:

[TABLE]

where $\Lambda\in\mathbb{R}_{++}$ is a positive gain and $H,Q=\alpha I_{2}\in\mathbb{S}^{2}$ are symmetric positive matrices with $\alpha>0$. Since for non-null $v_{d}$ the camera moves along the optical axis, $\dot{\chi}\neq 0$. For this setup, the dynamics of the error can be proven to be locally exponentially stable. For a formal proof of the convergence of error to the origin, the reader is referred to [13]. The basic idea consists in guarantee that a Persistency of Excitation conditions holds. More precisely, this requires that the matrix $\Omega(t)\Omega(t)^{T}$ is full rank throughout time, that is

[TABLE]

where $\gamma>0$ and $I$ is an identity matrix of appropriate dimension. In particular, for a single point, this can be done by ensuring that $\|\Omega(t)\|^{2}>0$. From (1), we have

[TABLE]

Therefore, the filter converges, as long as the projected point is not in the center of the image frame, i.e., $x\neq 0$ or $y\neq 0$ must hold for convergence. Geometrically, this constraint is easy to verify: if $x=y=0$ and the camera moves along the $z$–axis, the motion will be along along the projection ray meaning and it would be impossible to recover the depth (not enough information to do triangulation).

In the next section, the recovered 3D points are employed for online relative pose estimation.

III-B Relative Pose Filter

This section presents relative pose filter, which takes as input the depth estimation and the control inputs of the robots. First, consider that each robot has a body-fixed coordinate frame, denoted as $\{A\}$ for robot $A$ and $\{B\}$ for robot $B$. The transformation from the body-fixed to the camera fixed frame is known for each robot. Now, instead of a single point, the robots observe $m$–points in the scenario. For each observed 3D point, each robot runs an instance of the depth estimation filter, as presented before. The pose of $B$ with respect to $A$ described in $\{A\}$ is denoted as $\bm{\xi}=[\mathbf{r}^{T},\theta]^{T}\in\mathit{SE}(2)$, where $\mathbf{r}=[r_{x},r_{y}]^{T}$ is a 2D translation and $\theta$ an orientation angle. In accordance with the definition presented in Sec. II, the control input for each vehicle is defined as $\mathbf{u}_{A}=[v_{A,d},w_{A,\theta}]^{T}$ and $\mathbf{u}_{B}=[v_{d,B},w_{B,\theta}]^{T}$. The list of notations is summarized in Table I.

In order to derive the dynamic model of the relative pose between the robots, let $\bm{\xi}_{A}=[\mathbf{r}_{A}^{T},\theta_{A}]^{T}$ and $\bm{\xi}_{B}=[\mathbf{r}_{B}^{T},\theta_{B}]^{T}$ be the pose of robot $A$ and $B$, respectively, described in an inertial coordinate frame. The dynamic of each robot is

[TABLE]

The relative pose between the two non-holonomic vehicles described in $\{A\}$ is given by

[TABLE]

where the rotation matrix $R(\cdot)$ will be soon defined. The dynamics of the relative pose is

[TABLE]

where $\dot{R}(a)=R(a)S(\dot{a})$,

[TABLE]

Substituting (5) in (6) yields

[TABLE]

Finally, applying (4) and (7) in the previous equation leads to the dynamic model

[TABLE]

The output model considers the relationship between the pose of the robots and the observed 3D points in the body-fixed frame. From the geometric perspective, the following equation holds for each of the $m$–points:

[TABLE]

for which the corresponding time derivative is

[TABLE]

By replacing $\dot{\bar{z}}=-\dot{\chi}/{\chi}^{2}$, it is possible to describe the relationship between the dynamics of 3D points, stated in (2) and (3), and the dynamics of the relative pose:

[TABLE]

If vehicle $A$ periodically estimates $(\mathbf{s}_{i}^{A},{\chi}_{i}^{A})$ and their dynamics, and receives the corresponding estimates from vehicle $B$, the following observation model holds:

[TABLE]

The relative pose filter consists in an Extended Kalman Filter that assumes that the input of the vehicles and the observations are perturbed by Gaussian noise with zero mean. First, in the prediction step, the prior is obtained according to (9)

[TABLE]

Then, for each pair of measurements, the update model is:

[TABLE]

where K is the Kalman gain and

[TABLE]

is computed from the measurements received from the neighboring vehicle. The Kalman gain and the covariance matrix are updated following the regular EKF framework.

With this we conclude the relative pose filter description. This setup allows us to obtain an estimate of the unknown state. We stress that this method builds on the assumption that at least two 3D points correspondences are required to align two coordinate systems [29]. Although we focus on the critical case with only two points, in general, the EKF framework benefits from multiple observations and increase robustness against occlusions.

IV Results

In this section, we discuss simulated (Sec.  IV-A ) and experimental results (IV-B). Simulations were performed using the Robot Operating System (ROS) and Gazebo/Turtlebot simulator at $20$ [Hz]. Similarly, for the experimental results, ROS and two TurtleBots were employed. A footage of this experiment is attached.

IV-A Simulation

The first simulated scenario illustrates the depth filter properties presented in Sec. III-A. A single non-holonomic ground robot observes four 3D points. At $t=0$, the points coordinates w.r.t. to the camera frame are $\mathbf{p}_{1}=[0,0,5]^{T}$, $\mathbf{p}_{2}=[0,5,5]^{T}$, $\mathbf{p}_{3}=[5,0,5]^{T}$, and $\mathbf{p}_{4}=[5,5,5]^{T}$. For each point, a filter was initialized with 1 [m] depth estimation error. The robot actuation is constant, set to $v_{d}=$ 0.1 [m/s] and $w_{\theta}=$ 0 [rad], that is, the robot moves straight ahead in the direction of the optical axis. Figure 4 shows the results for the three points using the same filter parameters. The estimation error converges to zero when the point does not rest at the origin of the image frame.

In fact, the further the point is from the center of the image, the faster is the convergence due to the larger value of $\Omega(t)$. However, this also amplifies numerical errors and noise. Figure 4 at the right depicts the impact of different gains on the depth estimation, $\mathbf{p}_{3}$ was selected. Larger values of $H$ in (2) slow down the convergence rate, while higher values of $\alpha\Lambda$ increase the convergence rate. Here, once again, the designer must take into consideration that faster convergence leads to large steady-state errors.

Figure 5 shows the performance of the proposed depth estimation and relative pose estimation framework (method proposed in Sec. III-B) altogether. Only the points $\mathbf{p}_{1}$ and $\mathbf{p}_{4}$ from the previous experience were considered and an additional robot was inserted in the scenario. For each set of robot/point, the initial depth estimation error was set to $1$ [m] and the initial relative pose estimation error was set to $1.5$ [m] in each transnational axis and $15^{o}$ in orientation. The depth estimation error for each point follows a convergence behaviour similar to the one shown in Fig. 4. Therefore, as shown in Fig. 5, the translation estimation error converges to zero as the depth estimation error converges to zero. The same applies for the orientation error.

IV-B Experimental Results

Experiments were conducted at the Smart Mobility Lab111https://www.kth.se/dcs/research/control-of-transport/smart-mobility-lab/smart-mobility-lab-1.441539 [accessed ]., from KTH. We employed two TurtleBots (differential drive robots) named here Robot_A and Robot_B, respectively. Each robot was equipped with a perspective camera pointing towards the motion of the robot. The scenario contained two 3D targets (red and blue targets). For evaluation purpose, a motion capture system installed at the ceiling of the arena provides ground truth for the poses of the robots and the 3D points. Fig. 6 shows the proposed setup. The following velocities were sent to the robots: $v_{d}^{A}$ = 0.1[m/s] & $w_{\theta}^{A}$ = 0.02[rad/s], and $v_{d}^{B}$ = 0.1[m/s] & $w_{\theta}^{B}$ = -0.03[rad/s], for Robot_A and Robot_B, respectively. Through all the trajectory, we make ensure that the robots are observing the target points (around 40 [s]). The depth filter parameters were $H=2.5I$ and $\alpha\Lambda=120$ for both robots.

The results are shown in Fig. 7. The initial error for the depth filters were approximately 1 [m]. The depth of both points converge after $13$ [s] for Robot_A and $25$ [s] for Robot_B. The offset between the initial guess and the real values for the relative pose filter was 1.5 [m] in each of the translation axis and $60^{o}$ in orientation. As one can see, after a transient period, the errors converge to zero, proving that the proposed pipeline is able to correctly estimate the relative pose between the two robots for the evaluated setup.

V DISCUSSION

In this paper we presented a framework for depth and relative pose estimation for two vision-based robots. The depth estimation filter was built on top of recent results on continuous structure from motion methods. In particular, we constrained the problem to a camera mounted on a non-holonomic ground vehicle. The depth estimation and robot input commands are fed into an Extended Kalman Filter that estimates the relative pose between the ground robots. Simulations and experiments in a real scenario shown the convergence of both filters for several scenarios and set of initial conditions. In future work, the active control of the robots will be considered as well as the interplay between the gains and the position of the point in the image frame.

ACKNOWLEDGEMENT

The authors are grateful for the invaluable support provided by Pedro Roque (KTH) in the experimental setup.