Python中的 SciPy 最小二乘法 leastsq 拟合平面

1. 问题:

已知三维空间的一些点集，求拟合出来的平面； 也就是求出所有点到平面的距离最短的平面参数；

2. 最优化的问题：

依旧是说，使用最小二乘法，得到平面的参数；

3. 函数说明：

直接使用 python scipy optimize： leastsq 方法的用法
leastaq 官方函数说明
简化说明：

• scipy.optimize.leastsq(func, x0, args=(), Dfun=None, full_output=0, col_deriv=0, ftol=1.49012e-08, xtol=1.49012e-08, gtol=0.0, maxfev=0, epsfcn=None, factor=100, diag=None)
• func 表示估计函数
• x0 表示初始的估计参数
• args 表示函数的参数

4. 代码：

import numpy as np
from scipy.optimize import leastsq


def fit_func(p, x, y):
    """ 数据拟合函数 """
    a, b, c = p
    return a * x + b * y + c


def residuals(p, x, y, z):
    """ 误差函数 """
    return z - fit_func(p, x, y)


def estimate_plane_with_leastsq(pts):
    """ 根据最小二乘拟合出平面参数 """
    p0 = [1, 0, 1]
    np_pts = np.array(pts)
    plsq = leastsq(residuals, p0, args=(np_pts[:, 0], np_pts[:, 1], np_pts[:, 2]))
    return plsq[0]


def get_proper_plane_params(p, pts):
    """ 根据拟合的平面的参数，得到实际显示的最佳的平面 """
    assert isinstance(pts, list), r'输入的数据类型必须依赖 list'
    np_pts = np.array(pts)

    np_pts_mean = np.mean(np_pts, axis=0)
    np_pts_min = np.min(np_pts, axis=0)
    np_pts_max = np.max(np_pts, axis=0)

    plane_center_z = fit_func(p, np_pts_mean[0], np_pts_mean[1])
    plane_center = [np_pts_mean[0], np_pts_mean[1], plane_center_z]

    plane_origin_z = fit_func(p, np_pts_min[0], np_pts_min[1])
    plane_origin = [np_pts_min[0], np_pts_min[1], plane_origin_z]

    if np.linalg.norm(p) < 1e-10:
        print(r'plsq 的 norm 值为 0 {}'.format(p))
    plane_normal = p / np.linalg.norm(p)

    plane_pt_1 = [np_pts_max[0], np_pts_min[1], fit_func(p, np_pts_max[0], np_pts_min[1])]
    plane_pt_2 = [np_pts_min[0], np_pts_max[1], fit_func(p, np_pts_min[0], np_pts_max[1])]
    return plane_center, plane_normal, plane_origin, plane_pt_1, plane_pt_2


if __name__ == '__main__':

    pts = [
        [0, 0, 128],
        [256, 0, 128],
        [256, 256, 128],
        [0, 256, 128],
    ]

    p = estimate_plane_with_leastsq(pts)
    center, normal, origin, plane_x_pt, plane_y_pt = get_proper_plane_params(p, pts)
    print(r'estimate plane center: {}, normal: {}, origin: {}, plane_x_pt: {}, plane_y_pt: {}'.format(
        center, normal, origin, plane_x_pt, plane_y_pt))

5. 结果：

estimate plane 
center: [128.0, 128.0, 128.0], 
normal: [-1.70384540e-14  1.41639579e-18  1.00000000e+00], 
origin: [0, 0, 128.00000000027913], 
plane_x_pt: [256, 0, 127.99999999972081],
plane_y_pt: [0, 256, 128.0000000002792]

输出结果与预期的结果是一致的；