在SLAM中,线特征可以很好地弥补点特征的不足,因此研究应用也逐渐增多。本文对线特征参数的优化进行一次简单的测试。
线特征优化
线特征可以用六参数向量
[
n
T
d
T
]
T
\begin{bmatrix} \mathbf n^T & \mathbf d^T\end{bmatrix}^T
[nTdT]T来表示,
n
\mathbf n
n为法向量,
d
\mathbf d
d为方向向量。由于线特征本身为四自由度,在实际优化中往往采用最小表示(正交表示)
[
θ
T
ϕ
T
]
T
\begin{bmatrix} \bm \theta^T & \bm \phi^T\end{bmatrix}^T
[θTϕT]T,转换关系如下:
U
=
[
n
d
n
×
d
∣
∣
n
×
d
∣
∣
]
=
e
x
p
(
θ
)
W
=
[
n
∣
∣
n
∣
∣
2
+
∣
∣
d
∣
∣
2
d
∣
∣
n
∣
∣
2
+
∣
∣
d
∣
∣
2
−
d
∣
∣
n
∣
∣
2
+
∣
∣
d
∣
∣
2
n
∣
∣
n
∣
∣
2
+
∣
∣
d
∣
∣
2
]
=
[
c
o
s
(
ϕ
)
s
i
n
(
ϕ
)
−
s
i
n
(
ϕ
)
c
o
s
(
ϕ
)
]
\begin{aligned} \mathbf U &= \begin{bmatrix} \mathbf n & \mathbf d & \frac{\mathbf n \times \mathbf d}{||\mathbf n \times \mathbf d||}\end{bmatrix}=exp(\bm \theta)\\ \mathbf W &= \begin{bmatrix} \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{bmatrix}=\begin{bmatrix}cos(\bm \phi) & sin(\bm \phi) \\ -sin(\bm \phi) & cos(\bm \phi) \end{bmatrix} \end{aligned}
UW=[nd∣∣n×d∣∣n×d]=exp(θ)=[∣∣n∣∣2+∣∣d∣∣2n−∣∣n∣∣2+∣∣d∣∣2d∣∣n∣∣2+∣∣d∣∣2d∣∣n∣∣2+∣∣d∣∣2n]=[cos(ϕ)−sin(ϕ)sin(ϕ)cos(ϕ)]
当一个线特征被多个已知的相机位姿观测到时,可以构建残差:
r
l
=
[
x
s
T
l
l
1
2
+
l
2
2
x
e
T
l
l
1
2
+
l
2
2
]
T
\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
rl=[l12+l22xsTll12+l22xeTl]T
x
s
\mathbf x_s
xs和
x
e
\mathbf x_e
xe为线段的左右端点观测值,
l
\mathbf l
l为图像坐标系下的坐标(实际上就是法向量
n
\mathbf n
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
方向基本优化准确,如果选择将
θ
z
\bm \theta_z
θz和
ϕ
\bm \phi
ϕ固定为真值,优化
θ
x
\bm \theta_x
θx和
θ
y
\bm \theta_y
θy,可以得到如下的残差曲面:
由此可见线特征的初始解很重要,否则容易陷入局部最优或者优化效率不高。