# In vision based localization, is it possible to make multiple vehicles cooperate to improve the estimation of each other?

I am currently working on a project that involves structure from motion using multiple cameras on multiple aerial vehicles (each vehicle has a monocular camera: think of it as a distributed stereo), and I am trying to extend this to include localization as well. My pipeline currently goes: robots at known locations -> take pictures -> reconstruct.

When it comes to localizing the vehicles as well using this incrementally built map, the standard approach that comes to mind is to apply the PNP algorithm on each camera (assuming the reconstructed scene is visible to all cameras) which results in the 3D pose: but this doesn't necessarily take advantage of the fact that multiple cameras exist, apart from the fact that they are used in reconstructing the environment. Is there anything I can exploit using multiple cameras/vehicles that would result in enhanced localization accuracy of all of the vehicles as compared to a "single vehicle performing PNP on a known map" scenario?

• Isn't this called stereo vision? I'm not sure if I'm missing something. It sounds to me like you're talking about building a depth map with multiple cameras (stereo vision), and then using that data to simultaneously map the terrain and localize the robot - this would be a stereo vision SLAM technique. What differentiates your scenario from stereo SLAM? – Chuck Oct 31 '16 at 14:36
• Correct me if I'm wrong: isn't the term 'stereo vision' used exclusively for rigid camera rigs? When I say multiple cameras, they are on multiple vehicles: each vehicle has one. Perhaps I didn't word the question right. – Unattainable Oct 31 '16 at 15:24
• are you doing stuff real time? or is this being post processed ? – holmeski Oct 31 '16 at 15:25
• Post processed for now. – Unattainable Oct 31 '16 at 15:26
• Edited question to remove ambiguity – Unattainable Oct 31 '16 at 15:29

You can do it by fusion using a Kalman filter:

You have a process model model:

$$x_t = g(x_{t-1},u_t)$$

Now, you have multiple measurements of the same process model from different perspectives:

$$z1_t = h_1(x_{t}) \leftarrow \text{camera 1} \\ z2_t = h_2(x_{t}) \leftarrow \text{camera 2} \\ \cdots \\ zn_t = h_n(x_{t}) \leftarrow \text{camera n} \\$$

In the prediction step of the Kalman filter, nothing changes you have only one process:

$$\bar{x}_t = g(x_{t-1},u_t) \\ \bar{\Sigma}_t = G_t \Sigma_{t-1} G_t^\intercal + R_t$$

And for the update step, you have to include all the measurements:

$$K_t = \bar{\Sigma}_t H_t^\intercal ( H_t \bar{\Sigma}_t H_t^\intercal + Q_t )^{-1} \\ x_t = \bar{x}_t + K_t ( z_t - h( \bar{x}_t ) ) \\ \Sigma_t = ( I - K_t H_t ) \bar{\Sigma}_t$$

The trick is defining how you are building the matrices in the update step:

$$z_t = \left[ \begin{array}{c} z1_t \\ z2_t \\ \cdots \\ zn_t \end{array} \right]$$

$$h(\bar{x}_t) = \left[ \begin{array}{c} h_1(\bar{x}_t) \\ h_2(\bar{x}_t) \\ \cdots \\ h_n(\bar{x}_t) \end{array} \right]$$

$$H_t = \left[ \begin{array}{c} H1_t \\ H2_t \\ \cdots \\ Hn_t \end{array} \right]$$

$$Q_t = \left[ \begin{array}{cccc} Q1_t & 0 & 0 & 0 \\ 0 & Q2_t & 0 & 0 \\ \cdots \\ 0 & 0 & 0 & Qn_t \end{array} \right]$$

• Thanks for the reply, but I'm confused: doesn't this solve the problem for one vehicle with multiple.. perspectives? Perhaps the edit I made to the question would make it clearer. I don't have multiple cameras localizing the same vehicle, I have one camera on each vehicle and I want to localize all of them assuming a common set of features are visible. Sorry about any confusion. – Unattainable Oct 31 '16 at 15:28
• The cameras on each aerial vehicle give you a pose. You said all the cameras can see the same scene. So for example, if all cameras see the same point, each camera will give you a pose of that point. That point has a process, here I called it g. The Kalman update can optimize all the measurements (views) of that point. You need something to detect that two or more cameras are indeed looking at the same point. – Luis Oct 31 '16 at 15:51
• Ah, I see it better now. Hope you wouldn't mind a couple more questions: As of now, the reconstruction of the scene doesn't happen continuously. Let's say my map contains only one point P at time instant 0. From 0 to another instant k, I re-use this 3D coordinate, and the only 'new' information coming in is where P is located in the image 2D coordinates. PNP(3D, 2D) gives me the pose of the vehicle. So to adapt this to your approach, would you suggest calculating let's say, the reprojection of that point P? In the world space, as P doesn't move. what would the process look like? – Unattainable Oct 31 '16 at 16:01
• That re projection is the measurement of each camera. You can use it in pixel coordinates, before the re projection. For the process model you can use a velocity model on the point to optimize. And the measurement model can be the camera pinhole model. – Luis Oct 31 '16 at 16:12
• I see. I am going to try to implement this idea. Just one last thing: let's say I am observing a scene from two vehicles, and the vehicles start moving in opposite directions, hence the 2D projection of the point I am observing is moving in opposite directions in the two frames. Would this cause a problem? – Unattainable Nov 1 '16 at 0:19