স্বল্প স্কোয়ারের মানচিত্রে যোগদান

Question

এখানে অনেক পটভূমি রয়েছে, প্রশ্নের জন্য নীচে স্ক্রোল করুন

আমি লিনিয়ার সবচেয়ে স্বল্প স্কোয়ার সমস্যা থেকে কতটা দূরে স্ল্যামএমে বর্ণিত বর্ণিত অ্যালগরিদমে যোগদানের মানচিত্রটি চেষ্টা করছি ; বিশেষত, সূত্র (36)। আমি যে কোডটি লিখেছি তা ল্যান্ডমার্ক অবস্থানের জন্য সর্বদা দ্বিতীয় মানচিত্রের মান গ্রহণ করে to আমার প্রশ্নটি হ'ল আমি কি পাঠ্যটি সঠিকভাবে বুঝতে পারছি বা কিছুটা ত্রুটি করছি। আমি সূত্রগুলি বুঝার সাথে সাথে ব্যাখ্যা করার চেষ্টা করব এবং আমার কোড কীভাবে এটি প্রয়োগ করে তা দেখাব। আমি মাত্র দুটি স্থানীয় মানচিত্রে যোগদানের সহজ কেসটি করার চেষ্টা করছি।

কাগজ থেকে (৩)) বলেছে যে দুটি স্থানীয় মানচিত্রে যোগদান করা রাষ্ট্রীয় ভেক্টরকে finding যা ন্যূনতম করে তা খুঁজে পেয়েছে :$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 The ফাংশনটি$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 ম্যাপের মধ্যে কোথাও হওয়া উচিত I'm আমি যথেষ্ট নিশ্চিত যে সমস্যাটি ম্যাট্রিক্স এইচ এর সাথে।

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 এর বর্ণনার সাথে আরও ভালভাবে মিলিয়ে আউটপুট পরিবর্তন করেছি।

মানচিত্র_1 এবং মানচিত্র 2 এর আউটপুট

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

এই ক্ষেত্রে, এইচ (এক্স) কল করার দরকার নেই, কারণ প্রথম দুটি পোজ একই, সুতরাং দুটি মানচিত্র একই রেফারেন্সের ফ্রেম ভাগ করে। এইচ ফাংশনটি কেবলমাত্র রাষ্ট্রের অনুমানকে সাবম্যাপের রেফারেন্সের ফ্রেমে রূপান্তরিত করে।