Penggabungan peta kuadrat terkecil

Ada banyak latar belakang di sini, gulir ke bawah untuk pertanyaan itu

Saya mencoba algoritma penggabungan peta yang dijelaskan dalam Seberapa Jauh SLAM Dari Masalah Kuadrat Linear ; khususnya, formula (36). Kode yang saya tulis tampaknya selalu mengambil nilai peta kedua untuk posisi tengara. Pertanyaan saya adalah, apakah saya memahami teks dengan benar atau apakah saya membuat semacam kesalahan. Saya akan mencoba menjelaskan formula ketika saya memahaminya dan menunjukkan bagaimana kode saya mengimplementasikannya. Saya mencoba melakukan kasus sederhana dengan menggabungkan hanya dua peta lokal.

Dari makalah tersebut (36) mengatakan bergabung dengan dua peta lokal adalah menemukan vektor keadaan yang meminimalkan:$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}))$$

Diperluas untuk dua peta lokal dan ^ X L 2 Saya punya:$\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}$$

Saya tidak yakin penilaian saya di bawah ini benar:

$t_0$

Kelompok elemen berikutnya adalah yang umum untuk peta 1 dan peta 2, yang diubah menjadi kerangka referensi peta 1.

Baris terakhir adalah fitur unik untuk memetakan 2, dalam bingkai peta pertama.

Implementasi matlab saya adalah sebagai berikut:

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

Saya menggunakan kotak optimasi untuk menemukan minimum fungsi kebugaran yang dijelaskan di atas. Fungsi kebugaran itu sendiri cukup mudah, saya pikir. Fungsi H mengembalikan vektor H yang dijelaskan di atas.

Hasilnya adalah: Ketika saya menjalankan join_maps pada dua vektor

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)

Outputnya adalah:

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

Pertanyaan:

Program saya memberi peta 2 adalah minimum dari fungsi penggabungan peta. Sepertinya minimum harus berada di suatu tempat antara peta 1 dan peta 2. Saya cukup yakin masalahnya adalah dengan matriks H. Apa yang saya lakukan salah?

Ini tampaknya berfungsi dengan benar dan merupakan solusi yang jauh lebih sederhana:

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

Saya telah mengubah output agar lebih cocok dengan deskripsi untuk fminunc.

Output dengan map_1 dan map_2 adalah

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

Dalam hal ini, tidak perlu memanggil H (X), karena dua pose pertama identik, sehingga dua peta memiliki kerangka referensi yang sama. Fungsi H hanya mentransformasikan estimasi negara ke dalam kerangka referensi submap.