Bản đồ hình vuông nhỏ nhất tham gia

Có rất nhiều nền tảng ở đây, cuộn xuống phía dưới cho câu hỏi

Tôi đang thử thuật toán nối bản đồ được mô tả trong Làm thế nào xa SLAM từ một vấn đề bình phương tuyến tính nhỏ nhất ; cụ thể, công thức (36). Mã tôi đã viết dường như luôn lấy các giá trị của bản đồ thứ hai cho các vị trí mốc. Câu hỏi của tôi là, tôi hiểu văn bản chính xác hay tôi đang mắc một số lỗi. Tôi sẽ cố gắng giải thích các công thức khi tôi hiểu chúng và chỉ ra cách mã của tôi thực hiện điều đó. Tôi đang cố gắng thực hiện trường hợp đơn giản là chỉ tham gia hai bản đồ địa phương.

Từ bài báo (36) cho biết việc tham gia hai bản đồ địa phương là tìm ra một vectơ trạng thái để giảm thiểu: $X_{join,rel}$

Σ_{j = = 1}^{k} (\hat{X_{j}^{L}} - H_{j, r e tôi} (X_{j o Tôi n, r e tôi}))^{T} (P_{j}^{L})^{- 1} (\hat{X_{j}^{L}} - H_{j, r e tôi} (X_{j o Tôi n, r e tôi}))

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

Mở rộng cho hai bản đồ địa phương và Tôi có: $\hat{X_1^L}$ $\hat{X_2^L}$

(\hat{X_{1}^{L}} - H_{j, r e tôi} (X_{j o Tôi n, r e tôi}))^{T} (P_{1}^{L})^{- 1} (\hat{X_{1}^{L}} - H_{j, r e tôi} (X_{j o Tôi n, r e tôi})) + (\hat{X_{2}^{L}} - H_{j, r e tôi} (X_{j o Tôi n, r e tôi}))^{T} (P_{2}^{L})^{- 1} (\hat{X_{2}^{L}} - H_{j, r e tôi} (X_{j o Tôi n, r e tôi}))

$(\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{matrix} X_{r_{j e}}^{r_{(j - 1) e}} \\ φ_{r_{j e}}^{r_{(j - 1) e}} \\ R (φ_{r_{(j - 1) e}}^{r_{m_{j 1} e}}) (X_{f_{j 1}}^{r_{m_{j 1} e}} - X_{r_{(j - 1) e}}^{r_{m_{j 1} e}}) \\ . \\ . \\ . \\ R (φ_{r_{(j - 1) e}}^{r_{m_{j tôi} e}}) (X_{f_{j tôi}}^{r_{m_{j tôi} e}} - X_{r_{(j - 1) e}}^{r_{m_{j tôi} e}}) \\ X_{f_{j (tôi + 1)}}^{r_{j - 1 e}} \\ . \\ . \\ . \\ X_{f_{j n}}^{r_{j - 1 e}} \end{matrix}]

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

Tôi không tin rằng đánh giá của tôi dưới đây là chính xác:

$t_0$

Nhóm các yếu tố tiếp theo là những yếu tố phổ biến cho bản đồ 1 và bản đồ 2, được chuyển thành khung tham chiếu của bản đồ 1.

Các hàng cuối cùng là các tính năng duy nhất cho bản đồ 2, trong khung của bản đồ đầu tiên.

Việc thực hiện MATLAB của tôi như sau:

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

Tôi đang sử dụng hộp công cụ tối ưu hóa để tìm mức tối thiểu của chức năng tập thể dục được mô tả ở trên. Bản thân chức năng tập thể dục khá đơn giản. Hàm H trả về vectơ H được mô tả ở trên.

Kết quả là: Khi tôi chạy jo_maps trên hai vectơ

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)

Đầu ra là:

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

Câu hỏi:

Chương trình của tôi cho bản đồ 2 là mức tối thiểu của chức năng nối bản đồ. Có vẻ như mức tối thiểu phải nằm ở đâu đó giữa bản đồ 1 và bản đồ 2. Tôi khá chắc chắn rằng vấn đề xảy ra với ma trận H. Tôi đang làm gì sai?

slam

— munk
nguồn

Điều này dường như hoạt động chính xác và là một giải pháp đơn giản hơn nhiều:

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

Tôi đã thay đổi đầu ra để phù hợp hơn với mô tả cho fminunc.

Đầu ra với map_1 và map_2 là

Trong trường hợp này, không cần gọi H (X), vì hai tư thế đầu tiên giống hệt nhau, vì vậy hai bản đồ chia sẻ cùng một khung tham chiếu. Hàm H chỉ biến đổi ước tính trạng thái thành khung tham chiếu của biểu đồ con.

— munk
nguồn