Mapa de mínimos cuadrados que se une

Hay muchos antecedentes aquí, desplácese hacia abajo para la pregunta

Estoy probando el algoritmo de unión de mapas descrito en Cuán lejos está SLAM de un problema de mínimos cuadrados lineales ; específicamente, fórmula (36). El código que he escrito parece tomar siempre los valores del segundo mapa para las posiciones de referencia. Mi pregunta es, ¿estoy entendiendo el texto correctamente o estoy cometiendo algún tipo de error? Trataré de explicar las fórmulas tal como las entiendo y mostraré cómo mi código implementa eso. Estoy tratando de hacer el caso simple de unir solo dos mapas locales.

Del documento (36) dice que unir dos mapas locales es encontrar un vector 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}))$$

Ampliado para dos mapas locales y Tengo:$\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}))$$

Según tengo entendido, un submapa puede verse como una observación integrada para un mapa global, por lo que es el ruido asociado con el submapa (en lugar de ser el ruido del proceso en el EKF que usé para hacer el submapa, que puede o no puede no ser diferente).$P^L_j$

El vector es la pose del primer mapa, la pose del segundo mapa y la unión de los puntos de referencia en ambos mapas.$X_{join,rel}$

La función es:$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}$$

No estoy convencido de que mi evaluación a continuación sea correcta:

Los dos primeros elementos son la pose del robot en el marco de referencia del mapa anterior. Por ejemplo, para el mapa 1, la pose estará en el cuadro inicial en ; para el mapa 2, estará en el marco del mapa 1.$t_0$

El siguiente grupo de elementos son los comunes al mapa 1 y al mapa 2, que se transforman en el marco de referencia del mapa 1.

Las filas finales son las características exclusivas del mapa 2, en el marco del primer mapa.

Mi implementación de matlab es la siguiente:

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

Estoy usando la caja de herramientas de optimización para encontrar el mínimo de la función de condición física descrita anteriormente. La función de fitness en sí es bastante sencilla, creo. La función H devuelve el vector H descrito anteriormente.

El resultado es: cuando ejecuto join_maps en los dos vectores

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)

El resultado es:

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

La pregunta:

Mi programa proporciona el mapa 2 es el mínimo de la función de unión de mapa. Parece que el mínimo debería estar en algún lugar entre el mapa 1 y el mapa 2. Estoy bastante seguro de que el problema está en la matriz H. ¿Qué estoy haciendo mal?

Esto parece funcionar correctamente y es una solución mucho más simple:

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

He cambiado la salida para que coincida mejor con la descripción de fminunc.

La salida con map_1 y map_2 es

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

En este caso, no es necesario llamar a H (X), porque las dos primeras poses son idénticas, por lo que los dos mapas comparten el mismo marco de referencia. La función H simplemente transforma la estimación del estado en el marco de referencia del submapa.