线段拟合(带拉格朗日乘子，HGL)

线段特征上的扫描点满足 (1)。本文的线段特征定义为：L: [d_L, φ_L, P_Ls, P_Le]^T，如图1所示。其中，d_L为笛卡尔坐标系中原点(激光传感器所在位置)到线段的距离， φ_L为线段特征的倾角，P_Ls为线段特征起点，P_Le为线段特征终点。线段特征在笛卡尔坐标系下方程为：

                                                                                                                                                                          (1)

其中，d_L > 0，-π < φ_L < π，x_i = ρ_icosφ_i，y_i = ρ_isinφ_i。

图1 线段

前几节中，通过统计学方法划分出若干区域，为了准确提取线段特征，采用带约束的最小二乘法对每个划分区域特征进行特征提取，将(1)式转换为如下形式：

                                                                                                                                                                   (2)

式中 S、E——线段端点。

将上式改写为矩阵形式有：

                                                                                                                                                        (3)

其中, , , 。由于α²+β²=1，引入拉格朗日乘子λ，得：

                                                                                                                                                                                             (4)

其中，，满足J^TCJ=1。令，得：

                                                                                                                                                                                                          (5)

解上式的广义特征值与广义特征向量，S为一个正定矩阵，待求特征向量J一定对应最小的特征值λ。当α与β确定以后，可以求得：

                                                                                                                                                                                                     (6)

然后，求出线段特征d_L。最后，转换到全局坐标系下。

文献^[1]给出了一种误差传播方法，通过最小化隐含的关系方程F(I,O)，给出最终误差(I输入误差，O输出误差)。将误差扩展到协方差矩阵，可以通过输入的协方差矩阵Σ_I，得到输出的协方差矩阵Σ_L。则有：

                                                                                                                                        (7)

其中，。，令：

                                                                                                                                                                                      (8)

代入最后计算出Σ_L。

[1] Haralick R M. Propagating covariance in computer vision[M]. Performance Characterization in Computer Vision. Springer Netherlands, 2000: 95-114.