कम से कम वर्ग के नक्शे में शामिल होने

Question

यहाँ पृष्ठभूमि बहुत है, प्रश्न के लिए नीचे स्क्रॉल करें

मैं एल्गोरिथ्म में शामिल होने के नक्शे की कोशिश कर रहा हूं कि कैसे हायर फार एसएलएएम फ्रॉम ए लाइनियर लिस्टर स्क्वेयर प्रॉब्लम ; विशेष रूप से, सूत्र (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}))$$

जैसा कि मैं इसे समझता हूं, एक उपमैप को वैश्विक मानचित्र के लिए एक एकीकृत अवलोकन के रूप में देखा जा सकता है, इसलिए के साथ जुड़ा हुआ शोर है (जैसा कि ईकेएफ में प्रक्रिया के शोर होने के विरोध में मैंने उपमैप बनाया था, जो हो सकता है) अलग नह) ं हो सकता।$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 नक्शे के लिए अद्वितीय विशेषताएं हैं।

मेरा मतलाब कार्यान्वयन इस प्रकार है:

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

मैं ऊपर वर्णित फिटनेस फ़ंक्शन का न्यूनतम पता लगाने के लिए अनुकूलन टूलबॉक्स का उपयोग कर रहा हूं । मुझे लगता है कि फिटनेस फंक्शन अपने आप में बहुत सीधा है। फ़ंक्शन एच ऊपर वर्णित वेक्टर एच लौटाता है।

परिणाम है: जब मैं दो वैक्टर पर 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 के बीच कहीं होना चाहिए। मुझे पूरा यकीन है कि समस्या मैट्रिक्स एच के साथ है। मैं क्या गलत कर रहा हूं?

Answer 1

यह सही ढंग से काम करने लगता है और एक बहुत सरल उपाय है:

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

इस मामले में, एच (एक्स) को कॉल करने की कोई आवश्यकता नहीं है, क्योंकि पहले दो पोज़ समान हैं, इसलिए दो नक्शे संदर्भ के एक ही फ्रेम को साझा करते हैं। फ़ंक्शन एच बस उप-संदर्भ के फ्रेम में राज्य के अनुमान को बदल देता है।