State Estimation and Localization for Self-Driving Cars 第四周作业利用点云进行平面拟合

最新推荐文章于 2023-01-04 21:53:03 发布

置顶恋日

最新推荐文章于 2023-01-04 21:53:03 发布

阅读量341

点赞数

分类专栏：状态估计学习笔记

本文链接：https://blog.csdn.net/yahhqy/article/details/105139158

版权

状态估计学习笔记专栏收录该内容

4 篇文章 2 订阅

订阅专栏

1.平面拟合算法

三维平面方程为 $z = a x + b y + c$ 。希望通过LIDAR测得的多组 $(x, y, z)$ 来拟合 $(a, b, c)$ 。

测量误差为 $\begin{aligned} e_{j} &=\hat{z}_{j}-z_{j} \\ &=\left(\hat{a}+\hat{b}{x_{j}}+\hat{c} y_{j}\right)-z_{j} \quad j=1 \ldots n \end{aligned}$

将多组测量数据进行罗列，得到相应的矩阵形式。
$\underbrace{\left[\begin{array}{c} e_{1} \\ e_{2} \\ \vdots \\ e_{n} \end{array}\right]}_{e}=\underbrace{\left[\begin{array}{ccc} 1 & x_{1} & y_{1} \\ 1 & x_{2} & y_{2} \\ \vdots & \vdots & \vdots \\ 1 & x_{n} & y_{n} \end{array}\right]}_{\mathbf{A}} \underbrace{\left[\begin{array}{c} a \\ b \\ c \end{array}\right]}_{\mathbf{x}}-\underbrace{\left[\begin{array}{c} z_{1} \\ z_{2} \\ \vdots \\ z_{n} \end{array}\right]}_{\mathbf{b}}$

这是一个最小二乘问题，其解为
$\hat{\mathbf{x}}=\left(\mathbf{A}^{T} \mathbf{A}\right)^{-1} \mathbf{A}^{T} \mathbf{b}$

2. 相关程序

from numpy import *
import math
import numpy as np
def estimate_params§:
“”"
Estimate parameters from sensor readings in the Cartesian frame.
Each row in the P matrix contains a single 3D point measurement;
the matrix P has size n x 3 (for n points). The format is:

P = [[x1, y1, z1],
[x2, x2, z2], …]

where all coordinate values are in metres. Three parameters are
required to fit the plane, a, b, and c, according to the equation

z = a + bx + cy

The function should retrn the parameters as a NumPy array of size
three, in the order [a, b, c].
“”"
param_est = zeros(3)

A = np.hstack([ones([len§, 1]), P[:, :2]])
b = P[:, 2:]
cc = linalg.inv(A.T.dot(A)).dot(A.T).dot(b)
param_est[0] = cc[0,0]
param_est[1] = cc[1,0]
param_est[2] = cc[2,0]

return param_est

注释：python 语言中，将两个矩阵左右拼接可以用np.hstack。