我正在尝试“ SLAM与线性最小二乘问题有多远”中描述的地图连接算法；具体地，公式（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 $是与子图相关的噪声（而不是过程噪声）在EKF中，我曾经制作过子图，该子图可以相同也可以不同）。
向量$ 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，该姿势将位于$ t_0 $的初始帧中；对于地图2，它将位于地图1的框架中。
下一组元素是地图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上。我在做什么错？
#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
在这种情况下，无需调用H（X），因为第一个两个姿势是相同的，因此两个地图共享相同的参考系。函数H只是将状态估计值转换为子图的参考框架。