União dos mapas dos mínimos quadrados

Há muitos antecedentes aqui, role até o final da pergunta

Estou testando o algoritmo de junção de mapas descrito em Quão longe está o SLAM de um problema linear de mínimos quadrados ; especificamente, fórmula (36). O código que escrevi parece levar sempre os valores do segundo mapa para posições de referência. Minha pergunta é: estou entendendo o texto corretamente ou estou cometendo algum tipo de erro. Vou tentar explicar as fórmulas como as entendo e mostrar como meu código implementa isso. Estou tentando fazer o simples caso de juntar apenas dois mapas locais.

No artigo (36), diz que juntar dois mapas locais é encontrar o vetor de estado que minimiza:$X_{join,rel}$

$$\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}))$$

Expandido para dois mapas locais e que tenho:$\hat{X_1^L}$$\hat{X_2^L}$

$$(\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}))$$

Pelo que entendi, um submapa pode ser visto como uma observação integrada para um mapa global, então é um ruído associado ao submapa (em vez de ser o ruído do processo no EKF que usei para fazer o submapa, que pode pode não ser diferente).$P^L_j$

O vetor é a pose do primeiro mapa, a pose do segundo mapa e a união dos pontos de referência nos dois mapas.$X_{join,rel}$

A função é:$H_{j,rel}$

$$\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}$$

Não estou convencido de que minha avaliação abaixo esteja correta:

Os dois primeiros elementos são a pose do robô no quadro de referência do mapa anterior. Por exemplo, no mapa 1, a pose estará no quadro inicial em ; para o mapa 2, ele estará no quadro do mapa 1.$t_0$

O próximo grupo de elementos é o comum no mapa 1 e no mapa 2, que são transformados no quadro de referência do mapa 1.

As linhas finais são os recursos exclusivos do mapa 2, no quadro do primeiro mapa.

Minha implementação do matlab é a seguinte:

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

Estou usando a caixa de ferramentas de otimização para encontrar o mínimo da função de condicionamento físico descrita acima. A função de fitness em si é bem direta, eu acho. A função H retorna o vetor H descrito acima.

O resultado é: Quando executo join_maps nos dois vetores

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)

A saída é:

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

A questão:

Meu programa fornece o mapa 2 é o mínimo da função de junção de mapas. Parece que o mínimo deve estar entre o mapa 1 e o mapa 2. Tenho certeza de que o problema está na matriz H. O que estou fazendo de errado?

Isso parece funcionar corretamente e é uma solução muito mais simples:

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

Alterei a saída para corresponder melhor à descrição do fminunc.

A saída com map_1 e map_2 é

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

Nesse caso, não há necessidade de chamar H (X), porque as duas primeiras poses são idênticas, portanto os dois mapas compartilham o mesmo quadro de referência. A função H apenas transforma a estimativa de estado no quadro de referência do submapa.