Jointure de la carte des moindres carrés

Il y a beaucoup de fond ici, faites défiler vers le bas pour la question

J'essaie l'algorithme de jointure de carte décrit dans À quelle distance est SLAM d'un problème de moindres carrés linéaires ; en particulier, la formule (36). Le code que j'ai écrit semble toujours prendre les valeurs de la deuxième carte pour les positions de points de repère. Ma question est, est-ce que je comprends bien le texte ou est-ce que je fais une sorte d'erreur. Je vais essayer d'expliquer les formules telles que je les comprends et montrer comment mon code implémente cela. J'essaie de faire le cas simple de joindre seulement deux cartes locales.

D'après l'article (36), joindre deux cartes locales revient à trouver un vecteur d'état qui minimise:$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}))$$

Développé pour deux cartes locales et ^ X L 2 J'ai:$\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}))$$

$P^L_j$

$X_{join,rel}$

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

Je ne suis pas convaincu que mon évaluation ci-dessous soit correcte:

$t_0$

Le prochain groupe d'éléments sont ceux communs à la carte 1 et à la carte 2, qui sont transformés en cadre de référence de la carte 1.

Les dernières lignes sont les caractéristiques propres à la carte 2, dans le cadre de la première carte.

Mon implémentation matlab est la suivante:

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

J'utilise la boîte à outils d'optimisation pour trouver le minimum de la fonction fitness décrite ci-dessus. Je pense que la fonction fitness en elle-même est assez simple. La fonction H renvoie le vecteur H décrit ci-dessus.

Le résultat est: lorsque j'exécute join_maps sur les deux vecteurs

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)

La sortie est:

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 question:

Mon programme donne la carte 2 est le minimum de la fonction de jonction de carte. Il semble que le minimum devrait se situer quelque part entre la carte 1 et la carte 2. Je suis sûr que le problème vient de la matrice H. Qu'est-ce que je fais mal?

Cela semble fonctionner correctement et est une solution beaucoup plus 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

J'ai changé la sortie pour mieux correspondre à la description de fminunc.

La sortie avec map_1 et map_2 est

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

Dans ce cas, il n'est pas nécessaire d'appeler H (X), car les deux premières poses sont identiques, donc les deux cartes partagent le même cadre de référence. La fonction H transforme simplement l'estimation d'état en cadre de référence de la sous-carte.