Least squares map joining

Question

There is a lot of background here, scroll to the bottom for the question

I am trying out the map joining algorithm described in How Far is SLAM From a Linear Least Squares Problem; specifically, formula (36). The code I have written seems to always take the values of the second map for landmark positions. My question is, am I understanding the text correctly or am I making some sort of error. I'll try to explain the formulas as I understand them and show how my code implements that. I'm trying to do the simple case of joining just two local maps.

From the paper (36) says joining two local maps is finding the a state vector $X_{join,rel}$ that minimizes:

$$ \sum_{j=1}^{k}(\hat{X_j^L} - H_{j,rel}(X_{join,rel}))^T(P_j^L)^{-1}(\hat{X_j^L} - H_{j,rel}(X_{join,rel})) $$

Expanded for two local maps $\hat{X_1^L}$ and $\hat{X_2^L}$ I have:

$$ (\hat{X_1^L} - H_{j,rel}(X_{join,rel}))^T(P_1^L)^{-1}(\hat{X_1^L} - H_{j,rel}(X_{join,rel})) + (\hat{X_2^L} - H_{j,rel}(X_{join,rel}))^T(P_2^L)^{-1}(\hat{X_2^L} - H_{j,rel}(X_{join,rel})) $$

As I understand it, a submap can be viewed as an integrated observation for a global map, so $P^L_j$ is noise associated with the submap (as opposed to being the process noise in the EKF I used to make the submap, which may or may not be different).

The vector $X_{join,rel}$ is the pose from the first map, the pose from the second map and the union of the landmarks in both maps.

The function $H_{j,rel}$ is:

$$ \begin{bmatrix} X_{r_{je}}^{r_{(j-1)e}}\\ \phi_{r_{je}}^{r_{(j-1)e}}\\ R(\phi_{r_{(j-1)e}}^{r_{m_{j1}e}}) (X^{r_{m_{j1}e}}_{f_{j1}} - X^{r_{m_{j1}e}}_{r_{(j-1)e}})\\.\\.\\.\\ R(\phi_{r_{(j-1)e}}^{r_{m_{jl}e}}) (X^{r_{m_{jl}e}}_{f_{jl}} - X^{r_{m_{jl}e}}_{r_{(j-1)e}})\\ X_{f_{j(l+1)}}^{r_{j-1e}}\\ .\\.\\.\\ X_{f_{jn}}^{r_{j-1e}} \end{bmatrix} $$

I'm not convinced that my assessment below is correct:

The first two elements are the robot's pose in the reference frame of the previous map. For example, for map 1, the pose will be in initial frame at $t_0$; for map 2, it will be in the frame of map 1.

The next group of elements are those common to map 1 and map 2, which are transformed into map 1's reference frame.

The final rows are the features unique to map 2, in the frame of the first map.

My matlab implementation is as follows:

function [G, fval, output, exitflag] = join_maps(m1, m2)
    x = [m2(1:3);m2];
    [G,fval,exitflag,output] = fminunc(@(x) fitness(x, m1, m2), x, options);
end

function G = fitness(X, m1, m2)
    m1_f = m1(6:3:end);
    m2_f = m2(6:3:end);
    common = intersect(m1_f, m2_f);
    P = eye(size(m1, 1)) * .002;
    r = X(1:2);
    a = X(3);
    X_join = (m1 - H(X, common));
    Y_join = (m2 - H(X, common));
    G = (X_join' * inv(P) * X_join) + (Y_join' * inv(P) * Y_join);
end

function H_j = H(X, com)
    a0 = X(3);
    H_j = zeros(size(X(4:end)));
    H_j(1:3) = X(4:6);
    Y = X(1:2);
    len = length(X(7:end));
    for i = 7:3:len
        id = X(i + 2);
        if find(com == id)
            H_j(i:i+1) = R(a0) * (X(i:i+1) - Y);
            H_j(i+2) = id;
        else  % new lmk
            H_j(i:i+2) = X(i:i+2);
        end
    end
end

function A = R(a)
    A = [cos(a) -sin(a); 
         sin(a)  cos(a)];
end

I am using the optimization toolbox to find the minimum of the fitness function described above. The fitness function itself is pretty straightforward I think. The function H returns the vector H described above.

The result is: When I run join_maps on the two vectors

map_1 = [3.7054;1.0577;-1.9404; %robot x, y, angle
      2.5305;-1.0739;81.0000]; % landmark x, y, id
map_2 = [3.7054;1.0577;-1.9404;
         2.3402;-1.1463;81.0000]; % note the slightly different x,y

[G,fv,output,exitflag] = join_maps(map_1, map_2)

The output is:

Warning: Gradient must be provided for trust-region algorithm;
  using line-search algorithm instead. 
> In fminunc at 341
  In join_maps at 7

Local minimum found.

Optimization completed because the size of the gradient is less than
the default value of the function tolerance.

<stopping criteria details>


Local minimum possible.

fminunc stopped because it cannot decrease the objective function
along the current search direction.

<stopping criteria details>

G = 
      3.7054
      1.0577
     -1.9404
      3.7054
      1.0577
     -1.9404
      2.3402
     -1.1463
      81.0000

 fv =
     1.3136e+07
  output = 
     iterations: 1
      funcCount: 520
       stepsize: 1.0491e-16
  firstorderopt: 1.6200e+05
      algorithm: 'medium-scale: Quasi-Newton line search'
        message: [1x362 char]
  exitflag =
   5

The question:

My program gives map 2 is the minimum of the map joining function. It seems like the minimum should be somewhere between map 1 and map 2. I'm pretty sure the problem is with the matrix H. What am I doing wrong?

Answer 1

This seems to work correctly and is a much simpler solution:

function [X, FVAL, EXITFLAG, OUTPUT, GRAD] = join_maps(m1, m2)
    p = [m1(1:3);m2(1:3)];
    x1 = [p;m1(4:end)];
    x2 = [p;m2(4:end)];
    guess_0 = zeros(size(x1,1),1);
    q = @(x)x'*eye(length(x))*x;
    fit = @(x)q(x1-x)+q(x2-x);
    [X,FVAL,EXITFLAG,OUTPUT,GRAD] = fminunc(fit ,guess_0);
end

I've changed the output to better match the description for fminunc.

The output with map_1 and map_2 is

X =
 3.7054
 1.0577
-1.9404
 3.7054
 1.0577
-1.9404
 2.4353
-1.1101
 81.0000

In this case, there is no need to call H(X), because the first two poses are identical, so the two maps share the same frame of reference. The function H just transforms the state estimate into the frame of reference of the submap.