กำลังเข้าร่วมแผนที่น้อยที่สุด

ที่นี่มีพื้นหลังมากมายเลื่อนไปที่ด้านล่างสุดของคำถาม

ฉันกำลังพยายามออกแผนที่ร่วมงานกับอัลกอริทึมที่อธิบายไว้ในHow Far เป็นสแลมจากเชิงเส้นสี่เหลี่ยมน้อยปัญหา ; สูตรเฉพาะ (36) รหัสที่ฉันเขียนดูเหมือนจะใช้ค่าของแผนที่ที่สองเสมอสำหรับตำแหน่งที่สำคัญ คำถามของฉันคือฉันเข้าใจข้อความอย่างถูกต้องหรือทำผิดพลาด ฉันจะพยายามอธิบายสูตรตามที่ฉันเข้าใจและแสดงให้เห็นว่าโค้ดของฉันใช้งานอย่างไร ฉันกำลังพยายามทำกรณีง่าย ๆ ในการเข้าร่วมเพียงสองแผนที่ท้องถิ่น

จากบทความ (36) กล่าวว่าการเข้าร่วมแผนที่ท้องถิ่นสองแห่งคือการค้นหาเวกเตอร์สถานะที่ย่อให้เล็กสุด:$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}))$$

ขยายสำหรับแผนที่สองแห่งและฉันมี:$\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}))$$

ตามที่ฉันเข้าใจแล้วมันสามารถมองเห็น submap เป็นการสังเกตแบบบูรณาการสำหรับแผนที่โลกดังนั้นเป็นเสียงที่เกี่ยวข้องกับ submap (ตรงข้ามกับเสียงรบกวนของกระบวนการใน EKF ที่ฉันใช้ในการทำ submap ซึ่งอาจหรือ อาจจะไม่แตกต่างกัน)$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}$$

ฉันไม่เชื่อว่าการประเมินของฉันด้านล่างถูกต้อง:

องค์ประกอบสองประการแรกคือการวางตัวของหุ่นยนต์ในกรอบอ้างอิงของแผนที่ก่อนหน้า ตัวอย่างเช่นสำหรับแผนที่ 1 ท่าทางจะอยู่ในเฟรมเริ่มต้นที่ ; สำหรับแผนที่ 2 มันจะอยู่ในเฟรมของแผนที่ 1$t_0$

องค์ประกอบกลุ่มถัดไปคือองค์ประกอบทั่วไปของแผนที่ 1 และแผนที่ 2 ซึ่งถูกเปลี่ยนเป็นกรอบอ้างอิงของแผนที่ 1

แถวสุดท้ายเป็นคุณลักษณะเฉพาะของแผนที่ 2 ในกรอบของแผนที่แรก

การใช้งาน matlab ของฉันเป็นดังนี้:

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

ฉันกำลังใช้กล่องเครื่องมือปรับให้เหมาะสมเพื่อค้นหาฟังก์ชันการออกกำลังกายขั้นต่ำที่อธิบายไว้ข้างต้น ฟังก์ชั่นการออกกำลังกายนั้นค่อนข้างตรงไปตรงมา ฟังก์ชัน H ส่งคืนเวกเตอร์ H ที่อธิบายข้างต้น

ผลลัพธ์คือ: เมื่อฉันเรียกใช้ join_maps กับเวกเตอร์สองตัว

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)

ผลลัพธ์คือ:

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

คำถาม:

โปรแกรมของฉันให้แผนที่ 2 เป็นฟังก์ชั่นการเข้าร่วมแผนที่ขั้นต่ำ ดูเหมือนว่าค่าต่ำสุดควรอยู่ระหว่างแผนที่ 1 และแผนที่ 2 ฉันค่อนข้างแน่ใจว่าปัญหาเกิดขึ้นกับเมทริกซ์เอชฉันกำลังทำอะไรผิด

ดูเหมือนว่าจะทำงานได้อย่างถูกต้องและเป็นวิธีที่ง่ายกว่ามาก:

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

ฉันเปลี่ยนผลลัพธ์ให้ตรงกับคำอธิบายสำหรับ fminunc ดีกว่า

ผลลัพธ์ด้วย map_1 และ map_2 คือ

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

ในกรณีนี้ไม่จำเป็นต้องโทรหา H (X) เพราะทั้งสองโพสท่าแรกเหมือนกันดังนั้นแผนที่ทั้งสองแบ่งปันเฟรมอ้างอิงเดียวกัน ฟังก์ชัน H เพิ่งแปลงการประมาณสถานะเป็นกรอบอ้างอิงของ submap