最小二乘图加入


10

这里有很多背景,请滚动到底部以查找问题

我正在尝试“ 线性最小二乘问题SLAM有多远”中描述的地图连接算法;具体地,公式(36)。我写的代码似乎总是采用第二张地图的值作为地标位置。我的问题是,我是正确理解文本还是在犯某种错误。我将在理解公式的同时尝试说明它们,并说明我的代码如何实现这些公式。我正在尝试仅合并两个本地地图的简单情况。

从论文(36)中可以看出,将两个局部地图是在寻找一个状态向量,该状态向量将最小化:Xjoin,rel

j=1k(XjL^Hj,rel(Xjoin,rel))T(PjL)1(XjL^Hj,rel(Xjoin,rel))

扩展为两个本地地图和我有:X1L^X2L^

(X1L^Hj,rel(Xjoin,rel))T(P1L)1(X1L^Hj,rel(Xjoin,rel))+(X2L^Hj,rel(Xjoin,rel))T(P2L)1(X2L^Hj,rel(Xjoin,rel))

据我了解,子图可以看作是全局图的综合观察,因此是与子图相关联的噪声(而不是我用来制作子图的EKF中的过程噪声。可能不会有所不同)。PjL

向量是第一个地图的姿态,第二个地图的姿态以及两个地图中地标的并集。Xjoin,rel

函数为:Hj,rel

[Xrjer(j1)eϕrjer(j1)eR(ϕr(j1)ermj1e)(Xfj1rmj1eXr(j1)ermj1e)...R(ϕr(j1)ermjle)(XfjlrmjleXr(j1)ermjle)Xfj(l+1)rj1e...Xfjnrj1e]

我不相信我的以下评估是正确的:

前两个元素是机器人在上一张地图的参考框中的姿势。例如,对于地图1,该姿势将位于初始帧中;对于地图2,它将位于地图1的框架中。t0

下一组元素是地图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

问题:

我的程序给出map 2是map join功能的最小值。似乎最小值应该在图1和图2之间。我很确定问题出在矩阵H上。我在做什么错?

Answers:


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只是将状态估计值转换为子图的参考框架。

By using our site, you acknowledge that you have read and understand our Cookie Policy and Privacy Policy.
Licensed under cc by-sa 3.0 with attribution required.