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:
Diperluas untuk dua peta lokal dan ^ X L 2 Saya punya:
Saya tidak yakin penilaian saya di bawah ini benar:
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?