Efficient Merging of Maps and Detection of Changes

Abstract

With the advent of cheap sensors and computing capabilities as well as better algorithms it is now possible to do structure from motion using crowd sourced data. Individual estimates of a map can be obtained using structure from motion (SfM) or simultaneous localization and mapping (SLAM) using e.g. images, sound or radio. However the problem of map merging as used for collaborative SLAM needs further attention. In this paper we study the basic principles behind map merging and collaborative SLAM. We develop a method for merging maps – based on a small memory footprint representation of individual maps – in a way that is computationally efficient. We also demonstrate how the same framework can be used to detect changes in the map. This makes it possible to remove inconsistent parts before merging the maps. The methods are tested on both simulated and real data, using both sensor data from radio sensors and from cameras.

1 Introduction

Structure from motion [5], is the problem of estimating the parameters of a map and of sensor motion using only sensor data. The map is typically a set of 2D or 3D points each consisting of a position and a feature vector. Assuming that feature errors are zero-mean Gaussian, the maximum likelihood estimate is that of minimising the sum of squares of the residuals. Within the field of computer vision this process is denoted bundle adjustment, where bundle refers to the bundle of light rays connecting each camera with each 3D point. For an overview of the literature and theory, see [13].

Fig. 1.

Structure from motion (SfM) is used to estimate a 3D map of scene features using images (or other sensors). In this paper we study the problem of detecting changes and merging maps, given multiple maps estimated by SfM from datasets collected at different occasions.

These optimization techniques are applicable not only to vision, but also to other types of sensors, such as audio, [9, 14] and radio [1]. With the advent of cheaper sensors and computing capabilities as well as better algorithms, it is now possible to gather and use much larger datasets. Instead of mapping a city every 5 years using special measurement cars or aerial photography, it is in principle possible for every car to add to the map of cities as they drive through them. Thus there is an additional need for research on map merging, including the problem of determining what has changed in a map. In this paper we study the basic principles behind map merging and collaborative SLAM. A straight-forward method to merge several individual maps is to take all measurements into account simultaneously. However, non-linear optimization using all data can be prohibitively slow. We will study how a small memory footprint representation of a map can be generated and used to merge maps in a way that is computationally efficient, while still retaining most of the information from each individual bundle adjustment. We also demonstrate how the same framework can be used to detect changes in the map. This makes it possible to remove changing parts before merging the stationary parts of the map. The idea is demonstrated in Fig. 1.

The idea of approximating the result from parts of the data has previously been used in the rotation averaging literature, cf. [2]. These approximate methods can give satisfactory results at a much increased speed. Another example of this idea is the approach of Global Epipolar Adjustment [12], in which a simplified error metric is based on the linear epipolar constraints for image pairs. Another approach is incremental light bundle adjustment, iBLA, [6] in which an error metric based on a combination of epipolar constraints and a variant of the trifocal constraint is used.

The main contributions of this paper are a novel method for computationally efficiently merging of individual maps obtained from bundle adjustment, utilizing a compact representation of the Jacobian matrix, and a change detection method based on a statistical analysis of the residuals.

2 The Separate Bundles - for TOA and Images

Before different maps are merged, the individual map estimates have to be created. In this section we present some of the notations used to understand how the raw data relates to the quality of the map estimates.

For the case of time of arrival (TOA) measurements the feature map consists of a number of receiver positions. Initially, TOA measures between m receivers at positions $x_i \in \mathcal {R}^3$ and n sender positions $y_j \in \mathcal {R}^3$ are given. For each sender-receiver pair this measure can be translated into a distance estimate $d_{ij} = |x_i-y_j| + \varepsilon _{ij}$, where $1\le i \le m$ and $1\le j \le n$ and where $| \cdot |$ denotes the Euclidean norm of a vector in $\mathcal {R}^3$. The measurements errors $\varepsilon _{ij}$ are assumed to be independent, Gaussian with mean zero and standard deviation $\upsigma$.

The final map estimate for a TOA or structure from motion system is usually obtained by non-linear least squares minimization over inlier measurements; this process is referred to as bundle adjustment in computer vision. Here, a few key components from the optimization are presented.

For the TOA data, let $\mathbf {r}$ denote the measurements residuals,

(1)

and denote the parameters of interest, which are optimized, by $\mathbf {z}$. This would typically be the receiver and the sender positions,

(2)

The computer vision case is analogous. Denoting the camera matrices $P_i$ and the 3D points $U_j$, each image point $u_{ij}$ gives a residual $r_{ij}$. The residual vector $\mathbf {r}$ is found by stacking all image feature residuals $r_{ij}$ and the parameters are collected in a parameter vector

(3)

The maximum likelihood estimate of $\mathbf {z}$ is found by minimizing the sum of the squares of the residuals, i.e.

$$\begin{aligned} \mathbf {z}^* = \text {argmin}_{\mathbf {z}} \mathbf {r}^T \mathbf {r}, \end{aligned}$$

(4)

which gives the optimal parameter update

$$\begin{aligned} \Delta \mathbf {z}= -(J^TJ)^{-1}J^T \mathbf {r}. \end{aligned}$$

(5)

For more details on the optimization, see [13]. For the analysis, the estimate of the matrix J (the Jacobian) is containing the derivatives of the residuals with respect to the parameters is of interest, i.e. $\mathbf {r}$ with respect to $\mathbf {z}$, further on denoted $\partial \mathbf {r}/ \partial \mathbf {z}$.

The map points can only be estimated up to a choice of coordinate system. For simplicity we will in the TOA case normalize the coordinate system so that the first receiver is placed in the origin, the second along the x-axis, the third in the xy-plane and so forth. By removing this gauge freedom with dimension $\upphi$ we see that the effective number of degrees of freedom in the problem is $d_{dof}= (m+n) \uprho -\upphi$, where $\uprho$ denotes the dimension. For TOA problems in 3D we have $\uprho =3$ and $\upphi = 6$. The effective degrees of freedom for the computer vision case becomes $d_{dof}= (6m+3n) -\upphi$, with gauge freedom $\upphi =7$ since we are free to choose position, orientation and scale of the coordinate system.

3 Merging Separate Maps

Once the N separate maps are obtained they can be merged to get a single more accurate map. We have investigated three different ways to do this.

3.1 The Full Bundle

One way to add the maps is do one large bundle where all the individual measurements are used simultaneously. Merging all maps through a large bundle is a good way to get an accurate map. However, the method is time consuming and if a new measurement is made after the original merge, the whole map has to be re-bundled. In that sense, there is no way to add new information to the existing, which makes this method unsuitable for online applications.

3.2 The Kalman Filter

A traditional method designed to update parameters gradually is the Kalman filter [8]. The algorithm for the Kalman filter looks as follows:

Then, $H\cdot x_2$ is the new state prediction, and $x_2$ and $P_2$ are the new estimates replacing $x_0$ and $P_0$ for the next iteration. In our case $x_0$ will be the receivers from the first measurement occasion, $x_0= \mathbf {q}^{(1)}$ (superscript denoting measurement occasion), while the observation u will be the receiver values from the following $N-1$ measurements s.t. $u_{k-1}=\mathbf {q}^{(k)}$, $2\le k \le N$. Both the update matrix and the observation matrix are identity matrices, $A=I$, $H=I$ and the covariance of the random excitation is set to $Q=0.1\cdot I$. Finally, $P_0$ and R are measurement uncertainties, $P_0 = \mathbf {C}[\Delta \mathbf {q}^{(1)}]$ and $R_{k-1} = \mathbf {C}[\Delta \mathbf {q}^{(k)}]$, $2\le k \le N$. The covariance $\mathbf {C}[\Delta \mathbf {q}]$ can be extracted from the covariance of $\Delta \mathbf {z}$ from Eq. (5). This is given by

$$\begin{aligned} \mathbf {C}[\Delta \mathbf {z}]=(J^{T}J)^{-1}J^{T}\cdot \mathbf {E}[\mathbf {r}^T\mathbf {r}]\cdot J(J^{T}J)^{-1} = \upsigma ^2(J^{T}J)^{-1}. \end{aligned}$$

(11)

The covariance of the map, $\mathbf {C}[\Delta \mathbf {q}]$, can be retrieved by picking the rows and columns in $\mathbf {C}[\Delta \mathbf {z}]$ that correspond to $\mathbf {q}$ and the variance of $\mathbf {r}$ can be approximated by [7, p. 148]

$$\begin{aligned} \upsigma ^2 \approx \frac{1}{m\cdot n - d_{dof}} \cdot \mathbf {r}^T \mathbf {r}= \frac{1}{m\cdot n - d_{dof}} \cdot \sum _{i=1}^{m\cdot n}r_i^2. \end{aligned}$$

(12)

The Kalman filter is a computationally cheap method. However, it is not as accurate as the full bundle. Also, the parameters need to be tuned for the specific problem and it is not evident either how to detect and handle changes in the map.

3.3 The Linearized Method

The idea of this method is that the optimal residuals from the separate bundles can be linearized – such that all that needs to be saved is a small memory footprint representation – to avoid the large bundles. Having the optimal residuals $\mathbf {r}^{(k)}$ and the optimal Jacobians $J^{(k)}$ from each run k, the residuals can be linearized using a first order Taylor approximation. A key idea here is to divide the unknown parameters in $\mathbf {z}$ into two parts $\mathbf {q}$ and $\mathbf {s}$, where $\mathbf {q}$ are the parameters that exist in several SLAM sessions. The parameters $\mathbf {s}$ can be thought of as auxiliary paramters, e.g. those that are relevant only for one specific bundle session. In the time-of-arrival case, some of the 3D anchors might be constant over several SLAM sessions whereas the measurement points and some of the anchors might be different. For vision based structure from motion, some of the 3D points are the same (these go into $\mathbf {q}$) whereas the rest of the points and camera matrices go into $\mathbf {s}$.

The Compressed Residual. First, the Jacobian is divided into two blocks

$$\begin{aligned} J = \begin{bmatrix} J_a&J_b \end{bmatrix}, \end{aligned}$$

(13)

where $J_a$ contains the columns that correspond to the main parameters $\mathbf {q}$ and $J_b$ contains the columns corresponding to the auxiliary parameters $\mathbf {s}$. The squared Jacobian is

$$\begin{aligned} J^TJ = \begin{bmatrix} J_a^T \\ J_b^ T \end{bmatrix} \cdot \begin{bmatrix} J_a&J_b \end{bmatrix} = \begin{bmatrix} J_a^TJ_a&J_a^TJ_b \\ J_b^TJ_a&J_b^TJ_b \end{bmatrix} = \begin{bmatrix} U&W \\ W^T&V \end{bmatrix}. \end{aligned}$$

(14)

Furthermore, if we insert this in the equation for the optimal update from (5) we get

$$\begin{aligned} \Delta \mathbf {z}= \begin{bmatrix} \Delta \mathbf {q}\\ \Delta \mathbf {s}\end{bmatrix} = -(J^TJ)^{-1}J^T \mathbf {r}\, \qquad \Leftrightarrow \qquad \begin{bmatrix} U&W \\ W^T&V \end{bmatrix} \begin{bmatrix} \Delta \mathbf {q}\\ \Delta \mathbf {s}\end{bmatrix} = -J^T \mathbf {r}. \end{aligned}$$

(15)

The product $-J^T\mathbf {r}$ is zero in an optimal point and so the second row provides a connection between $\mathbf {q}$ and $\mathbf {s}$. This gives a linear constraint on how to adjust the auxiliary parameters $\mathbf {s}$ when the main parameters $\mathbf {q}$ change. Thus the partial derivatives of $\mathbf {s}$ with respect to $\mathbf {q}$ is

$$\begin{aligned} W^T \Delta \mathbf {q}+ V \Delta \mathbf {s}= 0 \quad \Leftrightarrow \quad \Delta \mathbf {s}= -V^{-1}W^T \Delta \mathbf {q}\quad \Rightarrow \quad \frac{\partial \mathbf {s}}{\partial \mathbf {q}}= -V^{-1}W^T. \end{aligned}$$

(16)

We can use this together with the definition $J = \partial \mathbf {r}/\partial \mathbf {z}$ to find how the residuals change if we change the receiver map

$$\begin{aligned} \Delta \mathbf {r}= \begin{bmatrix} J_a&J_b \end{bmatrix}\begin{bmatrix} \Delta \mathbf {q}\\ \Delta \mathbf {s}\end{bmatrix} = \left( J_a+ J_b\cdot \frac{\partial \mathbf {s}}{\partial \mathbf {q}}\right) \Delta \mathbf {q}. \end{aligned}$$

(17)

Thus, $J_a+J_b \frac{\partial \mathbf {s}}{\partial \mathbf {q}}$ will be the Jacobian for the map, further on denoted $J_q$.

Now, denote the residuals as a function of $\Delta \mathbf {q}$. A first order Taylor expansion gives

(18)

Here denotes an optimal point and denotes evaluating an expression at the point . Then, the square of these residuals will be

(19)

In a minimum point is zero. Furthermore, using the QR-decomposition of the Jacobian we get

(20)

Introducing the notation , the squared residuals from (19) can be written shorter as

(21)

and this is our compressed expression for the residuals.

The Merge. Furthermore, this compressed expression can be used to add two separate maps. Assume that we have the residuals for the two maps,

(22)

Adding the two equations and writing $\Delta \mathbf {q}^{(i)}=\mathbf {q}-\mathbf {q}^{(i)}$ for an arbitrary $\mathbf {q}$ gives

(23)

The terms $(a^{(1)}) ^ 2$ and $(a^{(2)}) ^ 2$ are fixed while the third term $\hat{\mathbf {r}}^T\hat{\mathbf {r}}$ can be minimized to minimize the sum of the residuals. Introducing new notations M and b, $\hat{\mathbf {r}}$ can be written

$$\begin{aligned} \hat{\mathbf {r}} = \begin{bmatrix} R^{(1)}(\mathbf {q}-\mathbf {q}^{(1)}) \\ R^{(2)}(\mathbf {q}-\mathbf {q}^{(2)})\end{bmatrix} = \begin{bmatrix} R^{(1)} \\ R^{(2)} \end{bmatrix} \mathbf {q}- \begin{bmatrix} R^{(1)}\mathbf {q}^{(1)} \\ R^{(2)}\mathbf {q}^{(2)}\end{bmatrix}= M\mathbf {q}-b. \end{aligned}$$

(24)

To minimize $\hat{\mathbf {r}}$ and thus $\hat{\mathbf {r}}^T\hat{\mathbf {r}}$ is a least squares problem which can be solved using the pseudo inverse. Denoting the merged map $\mathbf {q}^{*}$ gives

$$\begin{aligned} \mathbf {q}^{(*)} = (M^TM)^{-1}M^Tb. \end{aligned}$$

(25)

We can also compress the final result. Using that a general $\mathbf {q}$ can be written $\mathbf {q}= \Delta \mathbf {q}^{(*)} + \mathbf {q}^{(*)}$, the third term in (23) can be expressed

$$\begin{aligned} \begin{aligned} \hat{\mathbf {r}}^T\hat{\mathbf {r}} =&(M\mathbf {q}-b)^T(M\mathbf {q}-b) = \big (M\mathbf {q}^{(*)} -b + M\Delta \mathbf {q}^{(*)}\big )^T\big (M\mathbf {q}^{(*)} -b + M\Delta \mathbf {q}^{(*)}\big ) \\ =&\big (M\mathbf {q}^{(*)} -b \big )^T\big (M\mathbf {q}^{(*)} -b \big ) +\big (\Delta \mathbf {q}^{(*)}\big )^TM^TM\Delta \mathbf {q}^{(*)}, \end{aligned} \end{aligned}$$

(26)

where the linear term vanishes due to orthogonality. Using this in Eq. (23) gives

$$\begin{aligned} \big (\mathbf {r}^{(*)}\big )^T\mathbf {r}^{(*)} = \big (a^{(1)}\big )^2+\big (a^{(2)}\big )^2+\big (M\mathbf {q}^{(*)} -b\big )^T\big (M\mathbf {q}^{(*)} -b \big )+\big (\Delta \mathbf {q}^{(*)}\big )^TM^TM\Delta \mathbf {q}^{(*)}. \end{aligned}$$

(27)

If M is QR-decomposed in a similar manner as $J_q$ was in (20) this total result can be compressed as

$$\begin{aligned} \big (\mathbf {r}^{(*)}\big )^T\mathbf {r}^{(*)} = \big (a^{(*)}\big )^2 + \big (\Delta \mathbf {q}^{(*)}\big )^T\big (R^{(*)}\big )^TR^{(*)}\Delta \mathbf {q}^{(*)}, \end{aligned}$$

(28)

with $R^{(*)}$ being the triangular matrix from the QR-decomposition of M and

$$\begin{aligned} a^{(*)} = \left( \big (a^{(1)}\big )^2+\big (a^{(2)}\big )^2+\big (M\mathbf {q}^{(*)} -b \big )^T\big (M\mathbf {q}^{(*)} -b \big ) \right) ^{\frac{1}{2}}. \end{aligned}$$

(29)

By this, the representation of the final map is the same as in (21) and the merged map can be treated as one of the original. Furthermore, more maps can be added using the algorithm described above. Thus, to add maps, all we need to save from the separate bundles are the maps $\mathbf {q}^{(i)}$, the squared residuals $a^{(i)}$, and the triangular matrices $R^{(i)}$ from the QR-decompositions of the Jacobians.

In some cases the linearized method is similar to the Kalman filter. However, several maps can be added at once using the linearized model and it also allows for better control. We will also show that this method can be developed to detect map changes.

4 Detection of Changes

Once we know how to merge two or more maps we can also use this to detect whether the map has changed between the measurement occasions. For this, assume that we have two maps $\mathbf {q}^{(1)}$ and $\mathbf {q}^{(2)}$ and their merge $\mathbf {q}^{(*)}$. Furthermore, we have the norms of their residuals, $a^{(1)}$, $a^{(2)}$ and $a^{(*)}$. An approximation for the residual variance is derived in (12). This can be used to find the estimated value of how the squared residuals change when we add maps. Rearranging terms from (12), we get

$$\begin{aligned} \mathbf {E}\big [\big (a^{(i)}\big )^2\big ] =&\mathbf {E}\big [\big (\mathbf {r}^{(i)}\big )^T \big (\mathbf {r}^{(i)}\big )\big ] = \upsigma ^2 (m n -(n\uprho + m\uprho -\upphi )), \qquad i=1,2\, \end{aligned}$$

(30)

$$\begin{aligned} \mathbf {E}\big [\big (a^{(*)}\big )^2\big ] =&\mathbf {E}\big [\big (\mathbf {r}^{(*)}\big )^T \big (\mathbf {r}^{(*)}\big )\big ] = \upsigma ^2 (m n -(N n\uprho + m\uprho -\upphi )), \end{aligned}$$

(31)

and subtracting these – in this case with $N=2$ maps – gives

$$\begin{aligned} \mathbf {E}\big [\big (a^{(*)}\big )^2-\big (a^{(1)}\big )^2-\big (a^{(2)}\big )^2\big ] = \upsigma ^2(N-1)(m \uprho -\upphi ). \end{aligned}$$

(32)

If we use real data, $\upsigma$ is unknown, but it can be estimated from the separate bundles using (12), s.t. $\hat{\upsigma }^2=((\upsigma ^{(1)})^2+(\upsigma ^{(2)})^2)/2$.

The values in (32) can be seen as a sum of $(N-1)(m \uprho -\upphi )$ Gaussian variables, and a sum of $2\nu$ independent Gaussian distributed variables with mean zero and standard deviation $\upsigma _n$ has a $\Gamma$ distribution with density [3, p. 47]

$$\begin{aligned} f_{\upalpha ,\nu }(x) =\frac{1}{\Gamma (v)}\upalpha ^\nu x^{\nu -1}e^{-\upalpha x}, \end{aligned}$$

(33)

with $\upalpha =1/(2\upsigma _n^2)$ and $\Gamma$ being the gamma function. This density will be denoted $\Gamma (\upalpha ,\nu )$ (two parameters). Furthermore, using $\tilde{a}=(a^{(1)})^2+(a^{(2)})^2- (a^{(*)})^2$ and $\upgamma = (N-1)(m\uprho -\upphi )$ we get that $\tilde{a} \sim \Gamma (1/(2\upsigma ^2),\upgamma /2)$. Thus, to know whether a map has changed we can compare the estimated $\tilde{a}$ to the distribution. A reasonable choice is that if the difference $\tilde{a}$ lies within the 99 percentile of $\Gamma (1/(2\upsigma ^2),\upgamma /2)$ there has not been any change in the map, but if $\tilde{a}$ is higher than this limit, a change has probably occured.

If a change between two maps is discovered, we further investigate those maps. By comparing the positions for each map point, we say that if the distance between them is larger than $3\hat{\upsigma }$ the map point has probably moved. This could also be used to decrease the variance even further for the receivers that have not changed, by using information from all maps for these receivers.

5 Experimental Validation

To validate the method suggested in this paper, experiments on simulated TOA data as well as real ultra-wideband (UWB) data have been performed. We have also developed the method to work for, and tried it on, 3D-reconstructions from image data.

5.1 Time of Arrival – Simulated Data

For each of the simulated experiments m receivers in 3D were generated from a uniform distribution, $\mathbf {q}^{(t)} \sim \mathcal {U}(0,10)$, superscript (t) denoting the true value. We simulated N different measurement occasions with n sender positions $\mathbf {s}^{(t)} \sim \mathcal {U}(0,10)$ each and calculated the mn sender-receiver distances. Gaussian noise with standard deviation $\upsigma$ was added to achieve distance measurements. For each measure we performed a separate bundle to get the N maps and the compressed representation explained in Sect. 3.3 and more specifically in (21).

Test of Time and Accuracy. For the first experiment $m=10$, $\upsigma _n=0.3$, $N=2$ and no change occured in the true map. The experiments were run four times with $n=10,\,100,\,1000,\,4000$ respectively. For each case, the merge was computed using the three methods presented in this paper and the runtimes were measured. We computed the error norm $\sqrt{ \sum _{i=1}^m|q_i^{(t)}-q_i|^2 }$ and for the full bundle and the linearized method, we also computed the squared distance residuals per residual $\mathbf {r}^T \mathbf {r}/(m n)=a^2/(m n)$. The results can be seen in Table 1.

Table 1. The results from the experiment explained in Sect. 5.1. The values come from a merge of two maps between which no change has occured. These values are the mean of 10 similar runs.

Even if the runtime is highly dependent on the implementations, the table gives a valid comparison between the methods. The linearized method is almost as accurate as the full bundle. Moreover, when only the sender positions increase, and thus also the number of distances, the runtime for the linearized method and the Kalman filter does not increase notably, while the runtime for the full bundle does. Hence, the linearized method is faster than the full bundle and more accurate than the Kalman filter.

Validating the Detection Threshold. To validate the threshold for detection of changes described in Sect. 4, we tested the distribution of $\tilde{a}$ empirically. Using $m=30$, $n=200$, $N=2$ and $\upsigma _n=0.5$ the distances were computed. The separate bundles as well as the merge using both the full bundle and the linearized method were then conducted. For all of the different maps we computed the compressed representations from (21). We then computed

$$\begin{aligned} \tilde{a}_{full} = \big (a^{(1)}\big )^2 + \big (a^{(2)}\big )^2 -\big (a^{(*)}_{full}\big )^2, \quad \text {and} \quad \tilde{a}_{lin} = \big (a^{(1)}\big )^2 + \big (a^{(2)}\big )^2 -\big (a^{(*)}_{lin}\big )^2, \end{aligned}$$

(34)

where subscript index full and lin denotes the full bundle and the linearized method respectively. This was re-made 2000 times with different noise. The total degrees of freedom were $\upgamma = (N-1)(m\cdot \uprho -\upphi )=30\cdot 3 - 6=84$. The results of $\tilde{a}_{full}$ and $\tilde{a}_{lin}$ were then plotted in a histogram together with a $\Gamma (2,42)$ distribution in Fig. 2. The histograms agree well with the gamma distribution in both cases; hence, this can be used to test the significance.

Fig. 2.

The plots show histograms of the residuals $\tilde{a}_{full}$ (to the left) and $\tilde{a}_{lin}$ (to the right) computed using the full bundle and the linearized method respectively. The curve () shows the $\Gamma$ distribution which we expect $\tilde{a}$ to belong to.

Detection of Changed Maps. Furthermore, we did an experiment where the map actually had changed. This time we used $m=10$, $n=30$, $N=3$ and $\upsigma _n = 0.5$. Four of the ten receivers moved before the last measurement. After running the separate bundles and merging the maps both using a full bundle and our linearized method we investigated the differences in the residuals. The system had $\upgamma = 2\cdot (10\cdot 3-6)=48$ degrees of freedom and thus $\tilde{a}$ should be such that it could come from a $\Gamma (1/(2\hat{\upsigma }^2),24)$ distribution if no changes has occured. Using the estimate $\hat{\upsigma }^2$ the 99-percentile of this was $\tilde{a}=17.7$. In this specific case, the results from the merge gave $\tilde{a}_{full}=603$ and $\tilde{a}_{lin}=749$ and this clearly showed that something had changed. The results from the unsuccessful merge can be seen to the left in Fig. 3. To the right in Fig. 3 are the results from the merge between the first and second map, after the system successfully had detected the change.

5.2 Time of Arrival – Real Data

To test our method $N=9$ experiments were conducted using a Bitcraze Crazyflie quadcopter and their Loco-positioning system which consists of $m=5$ anchors with UWB chips and a flying quadcopter with a mounted UWB chip, giving approximately $n=600$ sender positions for each measurement. The five anchors were positioned around the room and one of them was moved before the last three runs. The experiment was conducted in a MOCAP studio to record the ground truth flightpath as well as the anchor positions. Distance measurements from the quadcopter (sender) to all the anchors (receivers) were measured at a frequency of 30 Hz.

Fig. 3.

An unsuccessful merge of map 1,2 and 3 (left) and a successful merge of map 1 and 2 (right). The stars () show the true receiver positions, the squares the results from full bundle () and the linearized method (). In the right figure, the points for which a change has been detected are (correctly) marked by a diamond (). (Color figure online)

The problem was solved as explained in previous sections, except that the threshold for $\tilde{a}$ now was 10 times the 99 percentile for the $\Gamma$ distribution. This threshold was used for all real data experiments. In Fig. 4 the results from the Kalman filter and the linearized method are shown. While the dynamics of the Kalman filter makes the estimated receivers end up further away from the true positions – on their way to the correct position – for some of the measurements, the linearized method correctly detects when a change has occured. Thereafter, only the similar maps are merged.

Fig. 4.

Results from two of the maps from the experiments with UWB data. The stars ($*$) show the true receiver positions, the circles () the results from the Kalman filter and the squares () from the linearized method. The change between the maps has been correctly detected by the linearized method and changed receivers are marked with a diamond (). (Color figure online)

5.3 Images – Real Data

In this experiment, $N=5$ sets of images were taken of an indoor scene, a bookshelf with a number of toy models, as depicted in Fig. 1. In between set 2 and 3 an R2D2 model was moved, which we wanted to detect. As a first step we used a structure from motion pipeline [11] to obtain a 3D reconstruction for each set. The points in this reconstruction are the feature points in the map, corresponding to the receivers in the TOA experiments.

Unlike the TOA experiments, correspondence between 3D points in the different datasets are not given. Prior to merging, we performed data association by SIFT [10] feature matching and geometric alignment in a RANSAC [4] framework. After this the maps were also in the same coordinate system, which is required for the linearized method and speeds up the full bundling method.

Fig. 5.

Changes detected in merge between dataset 2 and 3. Feature points are maked with blue dots and changed features are circled in cyan. (Color figure online)

Using the same method as in Sect. 5.2 – with detection based on a $\Gamma$ distribution and the feature point distances – the algorithm detected change during the merge of dataset 2 and 3, which is correct. In Fig. 5 we see that the feature points on R2D2 are correctly detected as changed. Note that some features are not present in both datasets and therefore these features on the R2D2 are not marked as changed. Figure 6 shows the 3D reconstruction from above. Here we see that the merged points on R2D2 does not align with either dataset 2 or 3.

Fig. 6.

To the left, the merge between dataset 1 and 2 where no change was detected. The separate maps are marked with dots (,) and the merge by diamonds (). To the right, the merge between dataset 2 and 3, where a change was detected. The points for which a change was detected are marked by squares (). (Color figure online)

6 Conclusions

We have presented a novel and efficient method, with small memory footprint, for merging individual maps obtained from bundle adjustment optimization along with a statistically motivated method for detecting changes in the map. The method has been compared favorably to using full bundle adjustment and the Kalman filter and is shown to be a good compromise between performance and time efficiency. This makes the method suitable for online applications as well as the use of crowd sourced data. The performance has been confirmed on both TOA and vision problems for both simulated and real data. One limitation is that the map points used for the coordinate system normalization need to be consistent for all maps. However, if this problem is solved, we believe that the method could be further developed to a full collaborative SLAM system.