线特征优化测试

在SLAM中，线特征可以很好地弥补点特征的不足，因此研究应用也逐渐增多。本文对线特征参数的优化进行一次简单的测试。

线特征优化

线特征可以用六参数向量${\left[\begin{array}{cc}{\mathbf{n}}^T& {\mathbf{d}}^T\end{array}\right]}^T$来表示，$\mathbf{n}$为法向量，$\mathbf{d}$为方向向量。由于线特征本身为四自由度，在实际优化中往往采用最小表示(正交表示)${\left[\begin{array}{cc}{\mathbf{\theta }}^T& {\mathbf{\varphi }}^T\end{array}\right]}^T$，转换关系如下：
$$\begin{array}{rl}\mathbf{U}& =\left[\begin{array}{ccc}\mathbf{n}& \mathbf{d}& \frac{\mathbf{n}\times \mathbf{d}}{||\mathbf{n}\times \mathbf{d}||}\end{array}\right]=exp(\mathbf{\theta })\\ \mathbf{W}& =\left[\begin{array}{cc}\frac{\mathbf{n}}{||\mathbf{n}|{|}^2+||\mathbf{d}|{|}^2}& \frac{\mathbf{d}}{||\mathbf{n}|{|}^2+||\mathbf{d}|{|}^2}\\ -\frac{\mathbf{d}}{||\mathbf{n}|{|}^2+||\mathbf{d}|{|}^2}& \frac{\mathbf{n}}{||\mathbf{n}|{|}^2+||\mathbf{d}|{|}^2}\end{array}\right]=\left[\begin{array}{cc}cos(\mathbf{\varphi })& sin(\mathbf{\varphi })\\ -sin(\mathbf{\varphi })& cos(\mathbf{\varphi })\end{array}\right]\end{array}$$
当一个线特征被多个已知的相机位姿观测到时，可以构建残差：
$${\mathbf{r}}_l={\left[\begin{array}{ll}\frac{{\mathbf{x}}_s^T\mathbf{l}}{\sqrt{l_1^2+l_2^2}}& \frac{{\mathbf{x}}_e^T\mathbf{l}}{\sqrt{l_1^2+l_2^2}}\end{array}\right]}^T$$
${\mathbf{x}}_s$和${\mathbf{x}}_e$为线段的左右端点观测值，$\mathbf{l}$为图像坐标系下的坐标（实际上就是法向量$\mathbf{n}$的投影）。
对应的雅可比矩阵参考线特征学习。

Matlab测试

设置一个线段被11帧相机观测到，每个量测真值上都加上高斯白噪声，并用高斯牛顿法进行优化，仿真代码如下：

noise = 1;

% 线段设置
endPoints = [0  5  2;
             0  5 -2]; % 垂直于x-y平面
line_n = cross(endPoints(1,:)', endPoints(2,:)');
line_v = endPoints(2,:)' - endPoints(1,:)';
line_n = line_n / norm(line_n);
line_w = [line_n; line_v];
line_orth = plk_to_orth(line_w);

% 相机位姿设置
K = [460    0   320;
       0  460   320;
       0    0     1];
Rcw = [1  0 0;
       0  0 1;
       0 -1 0];

tcw = [-10 0 0;
        -8 0 0;
        -6 0 0;
        -4 0 0;
        -2 0 0;
         0 0 0;
         2 0 0;
         4 0 0;
         6 0 0;
         8 0 0;
        10 0 0];
camStates = cell(size(tcw, 1));
for i = 1:size(tcw, 1)
    camStates{i}.R = Rcw;
    camStates{i}.t = tcw(i,:)';
end

% 添加噪声
obs_noise = genObs(camStates, endPoints, noise, K);

% 优化估计
[line_w_est, endPoints_final] = calcGNLine(camStates, obs_noise, noise, K);

优化得到线特征参数如下：
line_G: -0.9806 0.0000 0.0002 -0.0000 0.0000 -0.1961
真值为:
line_G_gt: -1 0 0 0 0 -4
方向基本优化准确，如果选择将${\mathbf{\theta }}_z$和$\mathbf{\varphi }$固定为真值，优化${\mathbf{\theta }}_x$和${\mathbf{\theta }}_y$，可以得到如下的残差曲面：

由此可见线特征的初始解很重要，否则容易陷入局部最优或者优化效率不高。