直线拟合(支持任意维空间的直线拟合，附代码)

一、问题描述

  给定一系列的三维空间点$(x_i,y_i,z_i),i=1,2,...,n$，拟合得到直线的方程。本文的直线拟合方法适用于任意维空间的直线拟合，不失一般性，这里以三维空间的直线拟合为例。本文的直线拟合方法的基本思想参考博文：最小二乘法三维(k维)直线拟合。

二、推导步骤

  设直线的点向式方程为：
$$\frac{x-x_0}{a}=\frac{y-y_0}{b}=\frac{z-z_0}{c}=s\text{\hspace{1em}(1)}$$
  由式(1)，得到直线的参数方程为：
$$\{\begin{array}{c}x=x_0+as\\ y=y_0+bs\\ z=z_0+cs\end{array}\text{\hspace{1em}(2)}$$
  式(2)写成向量形式为：
$$\mathbf{L}={\mathbf{L}}_0+\mathbf{v}s\text{\hspace{1em}(3)}$$
  其中，$\mathbf{L}=[x,y,z{]}^T$，${\mathbf{L}}_0=[x_0,y_0,z_0{]}^T$为直线上任意一点，$\mathbf{v}=[a,b,c{]}^T$为直线的单位方向向量。
  如下图，红色点$L_i(x_i,y_i,z_i)$为给定的一系列三维空间点，根据给定三维空间点，拟合直线方程(3)，也就是计算${\mathbf{L}}_0$和$\mathbf{v}$，使得在某种“距离”的度量下，达到最佳的直线拟合效果。

  点$L_i$到直线距离的平方为：
$$||{\mathbf{Q}}_{\mathbf{i}}{\mathbf{L}}_{\mathbf{i}}|{|}^2=||{\mathbf{L}}_0{\mathbf{L}}_{\mathbf{i}}|{|}^2-||{\mathbf{L}}_0{\mathbf{Q}}_{\mathbf{i}}|{|}^2\text{\hspace{1em}(4)}$$
  ${\mathbf{L}}_0{\mathbf{L}}_{\mathbf{i}}$在直线的投影的平方为：
$$||{\mathbf{L}}_0{\mathbf{Q}}_{\mathbf{i}}|{|}^2=({\mathbf{L}}_0{\mathbf{L}}_{\mathbf{i}}\cdot \mathbf{v}{)}^2\text{\hspace{1em}(5)}$$
  令向量${\mathbf{Y}}_{\mathbf{i}}={\mathbf{L}}_0{\mathbf{L}}_{\mathbf{i}}={\mathbf{L}}_{\mathbf{i}}-{\mathbf{L}}_0$，式(4)写成：
$$||{\mathbf{Q}}_{\mathbf{i}}{\mathbf{L}}_{\mathbf{i}}|{|}^2=||{\mathbf{Y}}_{\mathbf{i}}|{|}^2-({\mathbf{Y}}_{\mathbf{i}}\cdot \mathbf{v}{)}^2={{\mathbf{Y}}_{\mathbf{i}}}^T{\mathbf{Y}}_{\mathbf{i}}-({\mathbf{v}}^T{\mathbf{Y}}_{\mathbf{i}}{)}^2\text{\hspace{1em}(6)}$$
  在最小二乘准则下，可以建立直线拟合的优化模型目标函数：
$$f=\sum _{i=1}^n||{\mathbf{Q}}_{\mathbf{i}}{\mathbf{L}}_{\mathbf{i}}|{|}^2=\sum _{i=1}^n[{{\mathbf{Y}}_{\mathbf{i}}}^T{\mathbf{Y}}_{\mathbf{i}}-({\mathbf{v}}^T{\mathbf{Y}}_{\mathbf{i}}{)}^2]\text{\hspace{1em}(7)}$$
  计算${\mathbf{L}}_0$:
  目标函数$f$对向量${\mathbf{Y}}_{\mathbf{i}}$求偏导数：
$$\frac{\partial f}{\partial {\mathbf{Y}}_{\mathbf{i}}}=\sum _{i=1}^n(2{\mathbf{Y}}_{\mathbf{i}}-2{\mathbf{v}}^T{\mathbf{Y}}_{\mathbf{i}}\mathbf{v})=\sum _{i=1}^n(2{\mathbf{Y}}_{\mathbf{i}}-2\mathbf{v}{\mathbf{v}}^T{\mathbf{Y}}_{\mathbf{i}})=\sum _{i=1}^{n}2(\mathbf{I}-\mathbf{v}{\mathbf{v}}^T){\mathbf{Y}}_{\mathbf{i}}\text{\hspace{1em}(8)}$$
  式(8)中，利用了恒等式${\mathbf{v}}^T{\mathbf{Y}}_{\mathbf{i}}\mathbf{v}\equiv \mathbf{v}{\mathbf{v}}^T{\mathbf{Y}}_{\mathbf{i}}$，简单进行验算可以证明该恒等式。
  由于$\mathbf{v}$为单位向量，可以证明$\mathbf{I}-\mathbf{v}{\mathbf{v}}^T\ne 0$。
  因此
$$\sum _{i=1}^n{\mathbf{Y}}_{\mathbf{i}}=\sum _{i=1}^n({\mathbf{L}}_{\mathbf{i}}-{\mathbf{L}}_0)=\sum _{i=1}^n{\mathbf{L}}_{\mathbf{i}}-n{\mathbf{L}}_0=0\text{\hspace{1em}(9)}$$
$${\mathbf{L}}_0=\frac{1}{n}\sum _{i=1}^n{\mathbf{L}}_{\mathbf{i}}\text{\hspace{1em}(10)}$$
  可以得到结论：待拟合的直线经过一个点${\mathbf{L}}_0$，该点的坐标为所有给定点的坐标平均值。如下图所示，一旦确定直线的单位方向向量$\mathbf{v}$，则直线的方程便确定。

  计算$\mathbf{v}$:
  对于单位向量$\mathbf{v}$，${\mathbf{v}}^T\mathbf{v}=1$，可以证明：${{\mathbf{Y}}_{\mathbf{i}}}^T{\mathbf{Y}}_{\mathbf{i}}\equiv {\mathbf{v}}^{\mathbf{T}}({{\mathbf{Y}}_{\mathbf{i}}}^T{\mathbf{Y}}_{\mathbf{i}})\mathbf{v}$，$({\mathbf{v}}^T{\mathbf{Y}}_{\mathbf{i}}{)}^2\equiv {\mathbf{v}}^T({\mathbf{Y}}_{\mathbf{i}}{\mathbf{Y}}_{\mathbf{i}}^{\mathbf{T}})\mathbf{v}$

  式(7)可改写成：
$$f=\sum _{i=1}^n[{{\mathbf{Y}}_{\mathbf{i}}}^T{\mathbf{Y}}_{\mathbf{i}}-({\mathbf{v}}^T{\mathbf{Y}}_{\mathbf{i}}{)}^2]=\sum _{i=1}^n[{\mathbf{v}}^{\mathbf{T}}({{\mathbf{Y}}_{\mathbf{i}}}^T{\mathbf{Y}}_{\mathbf{i}})\mathbf{v}-{\mathbf{v}}^T({\mathbf{Y}}_{\mathbf{i}}{\mathbf{Y}}_{\mathbf{i}}^{\mathbf{T}})\mathbf{v}]={\mathbf{v}}^{\mathbf{T}}\sum _{i=1}^n[({{\mathbf{Y}}_{\mathbf{i}}}^T{\mathbf{Y}}_{\mathbf{i}})\mathbf{I}-{\mathbf{Y}}_{\mathbf{i}}{\mathbf{Y}}_{\mathbf{i}}^{\mathbf{T}}]\mathbf{v}\text{\hspace{1em}(11)}$$
  令矩阵$S=\sum _{i=1}^n[({{\mathbf{Y}}_{\mathbf{i}}}^T{\mathbf{Y}}_{\mathbf{i}})\mathbf{I}-{\mathbf{Y}}_{\mathbf{i}}{\mathbf{Y}}_{\mathbf{i}}^{\mathbf{T}}]$,式(11)可写成：

$$f={\mathbf{v}}^{\mathbf{T}}S\mathbf{v}\text{\hspace{1em}(12)}$$
  $f$的最小值为矩阵$S$最小特征值对应的特征向量。直线方向向量$v$的求解问题转化为矩阵最小特征值对应的特征向量的求解问题！

三、$MATLAB$代码

%{
Function: line_fitting
Description: 直线拟合
Input: 任意维直线点数据points，行数为点个数，列数为点的维数
Output: 拟合得到的直线经过的一点L0,直线的单位方向向量v
Author: Marc Pony(marc_pony@163.com)
%}
function [L0, v] = line_fitting(points)
n = size(points, 1);
x = points(:, 1);
y = points(:, 2);
z = points(:, 3);

L0 = [mean(x); mean(y); mean(z)];
S = zeros(3,3);
for i = 1 : n
    Yi = [x(i) - L0(1); y(i) - L0(2); z(i) - L0(3)];
    S = S + (Yi' * Yi * eye(3, 3) - Yi * Yi');
end
[V, ~] = eig(S);

v = V(:, 1); %矩阵S最小特征值对应的特征向量
end

%{
Function: generate_line_points
Description: 直线路径点生成
Input: 直线经过的一点L0,直线的单位方向向量v,点个数n，路径标量最小值minS，路径标量最大值maxS
Output: 任意维直线点数据points，行数为点个数，列数为点的维数
Author: Marc Pony(marc_pony@163.com)
%}
function points = generate_line_points(L0, v, n, minS, maxS)
points = zeros(n, length(v));
s = linspace(minS, maxS, n);
for i = 1 : n
    points(i, :) = (L0 + v * s(i))';
end
end

clear
clc
close all

%% 验证恒等式: v'*Yi*v = v*v'*Yi
syms v1 v2 v3 y1 y2 y3 real
v = [v1; v2; v3];
Yi = [y1; y2; y3];
res1 = simplify(v'*Yi*v - v*v'*Yi)

%% 验证恒等式: Yi'*Yi = v'*(Yi'*Yi)*v, 其中v'*v=1
res2 = [Yi'*Yi; simplify(v'*(Yi'*Yi)*v)]

%% 验证恒等式: (v'*Yi)^2 = v'*(Yi*Yi')*v
res3 = simplify((v'*Yi)^2 - v'*(Yi*Yi')*v)

% points = [1 0 0
%     1 10 0
%     1 20 0
%     ];
% points = [0 1 0
%     10 1 0
%     200 1 0
%     ];
% points = [1 1 1
%     2 1 2
%     ];

figure
axis([-10, 10, -10, 10])
hold on
pointCount = 6;
points = zeros(pointCount, 3);
for i = 1 : pointCount
    [points(i, 1), points(i, 2)] = ginput(1);
    plot(points(i, 1), points(i, 2), '+')
end

[L0, v] = line_fitting(points)

n = 100;
len = sqrt((max(points(:,1)) - min(points(:,1)))^2 + (max(points(:,2)) - min(points(:,2)))^2 + (max(points(:,3)) - min(points(:,3)))^2);
minS = -0.6 * len;
maxS = 0.6 * len;
p = generate_line_points(L0, v, n, minS, maxS);
plot3(p(:,1), p(:,2), p(:,3), '-')