3D点云处理:用SVD分解法和最小二乘法拟合平面点云,求解平面方程

这篇具有很好参考价值的文章主要介绍了3D点云处理:用SVD分解法和最小二乘法拟合平面点云,求解平面方程。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

学习目标:

本文主要介如何用SVD分解法和最小二乘法拟合平面点云,包含原理推导和代码


1.SVD分解法求解平面点云

1.1问题描述

  1. 将空间中的离散点拟合为一个平面,就是使离散点到某个平面距离和最小的问题,可以将求解过程看作最优化的过程。
  2. 一个先验知识为拟合平面一定经过离散点的质心(离散点坐标的平均值)。平面方程可以通过求解求解平面的法向量来获得。根据协方差矩阵的SVD变换,最小奇异值对应的奇异向量就是平面的方向。
    注意:这个方法是直接的计算方法,没办法解决数值计算遇到的病态矩阵问题.在公式转化代码之前必须对空间点坐标进行近似归一化!

1.2问题建模:

已知若干三维点坐标 ( x i , y i , z i ) (x_{i},y_{i},z_{i}) (xi,yi,zi),拟合出平面方程 a x + b y + c z = d ax+by+cz=d ax+by+cz=d (1)
约束条件为 a 2 + b 2 + c 2 = 1 a^{2}+b^{2}+c^{2}=1 a2+b2+c2=1 (2)
目标为使该平面到所有点的距离之和最小

1.3推导:

所有点的平均坐标为 ( x ˉ , y ˉ , z ˉ ) (\bar{x},\bar{y},\bar{z}) (xˉ,yˉ,zˉ),则由先验知识(拟合平面一定经过离散点的质心),可以到以下方程 a x ˉ + b y ˉ + c z ˉ = d . a\bar{x}+b\bar{y}+c\bar{z}=d. axˉ+byˉ+czˉ=d.(3)

式(1)与式(3)相减,得 a ( x i − x ˉ ) + b ( y i − y ˉ ) + c ( z i − z ˉ ) = 0 a(x_{i}-\bar{x})+b(y_{i}-\bar{y})+c(z_{i}-\bar{z})=0 a(xixˉ)+b(yiyˉ)+c(zizˉ)=0(4)

将所有点数据带入式4,整理可以得到矩阵 A = [ x 1 − x ˉ , y 1 − y ˉ , z 1 − z ˉ x 2 − x ˉ , y 2 − y ˉ , z 2 − z ˉ x 3 − x ˉ , y 3 − y ˉ , z 3 − z ˉ . . . x n − x ˉ , y n − y ˉ , z n − z ˉ ] A=\begin{bmatrix} x_{1}-\bar{x},y_{1}-\bar{y},z_{1}-\bar{z}\\ x_{2}-\bar{x},y_{2}-\bar{y},z_{2}-\bar{z}\\ x_{3}-\bar{x},y_{3}-\bar{y},z_{3}-\bar{z}\\ ...\\ x_{n}-\bar{x},y_{n}-\bar{y},z_{n}-\bar{z} \end{bmatrix} A= x1xˉ,y1yˉ,z1zˉx2xˉ,y2yˉ,z2zˉx3xˉ,y3yˉ,z3zˉ...xnxˉ,ynyˉ,znzˉ ,列矩阵 X = [ a b c ] X = \begin{bmatrix} a\\ b\\ c \end{bmatrix} X= abc ,则式(4)等价与AX=0 (5)

理想情况下所有点都在平面上,式(5)成立;实际情况下有部分点在平面外,拟合的目的为平面距离所有点的距离之和尽量小,所以目标函数为 m i n ∥ A X ∥ min\left \| AX \right \| minAX (6)

约束条件为 ∥ X ∥ = 1 \left \| X \right \|=1 X=1 (7)

若A可做奇异值分解: A = U D V T A = UDV^{T} A=UDVT (8)

其中,D是对角矩阵,U和V均为酉矩阵。

∥ A X ∥ = ∥ U D V T X ∥ = ∥ D V T X ∥ \left \| AX \right \|=\left \| UDV^{T}X \right \|=\left \| DV^{T}X \right \| AX= UDVTX = DVTX (9)

其中为列矩阵,并且 ∥ V T X ∥ = ∥ X ∥ = 1 \left \| V^{T}X \right \|=\left \|X \right \|=1 VTX =X=1 (10)

因为D的对角元素为奇异值,假设最后一个对角元素为最小奇异值,则当且仅当
V T X = [ 0 0 0 . . . 1 ] V^{T}X=\begin{bmatrix} 0\\ 0\\ 0\\ ...\\ 1 \end{bmatrix} VTX= 000...1 (11)时,式(9)可以取得最小值,即式(6)成立。

此时 X = V [ 0 0 0 . . . 1 ] = [ v 1 v 2 v 3 . . . v n ] [ 0 0 0 . . . 1 ] = v n X=V\begin{bmatrix} 0\\ 0\\ 0\\ ...\\ 1 \end{bmatrix}=\begin{bmatrix} v_{1} &v_{2} &v_{3} &... &v_{n} \end{bmatrix}\begin{bmatrix} 0\\ 0\\ 0\\ ...\\ 1 \end{bmatrix}=v_{n} X=V 000...1 =[v1v2v3...vn] 000...1 =vn (12)

所以,目标函数(6)在约束条件(7)下的最优解为 X = ( a , b , c ) = ( v n , 1 , v n , 2 , v n , 3 ) X=(a,b,c)=(v_{n,1},v_{n,2},v_{n,3}) X=(a,b,c)=(vn,1,vn,2,vn,3) (13)

综上:对矩阵A做奇异值分解,最小奇异值对应的特征向量就是拟合平面的系数向量。

1.4 伪代码

1 读取点云数据
2 求解点云质心(x0,y0,z0)
3 将原始点云坐标减去点云质心,构建矩阵A
4 对矩阵A进行SVD分解,A = U * S * VT
5 V最后一列对应为(A,B,C); D=-(Ax0+By0+C*z0)

1.5 使用opencv进行求解

void fitPlane(cv::Mat &points,cv::Mat &plane)
//points输入点云
//plane输出平面方程系数
{
	cv::Mat centor = cv::Mat::zeros(1,points.cols,CV_32FC1);
	for(int i =0;i < points.cols;++i)
	{
		for(int j =0;j < points.rows;++j)
		{
			centor.at<float>(0,i) = centor.at<float>(0,i) + points.at<float>(j,i);  
		}
		centor.at<float>(0.i) = centor.at<float>(0.i) / points.rows;
	}
	cv::Mat pointC = cv::Mat::ones(points.rows,points.cols,CV_32FC1);
	for(int i =0;i < points.cols;++i)
	{
		for(int j =0;j < points.rows;++j)
		{
			pointC.at<float>(j,i) =  points.at<float>(j,i) - centor.at<float>(0.i);
		}
	}
	//SVD分解
	cv::Mat A,W,U,V;
	//构建奇异值矩阵(gemm矩阵相乘)
	SVD::compute(pointC,W,U,V);
	// 提取最小奇异值和相应的向量
    double min_sing_val = W.at<double>(svd.w.rows-1); // 最后一个元素为最小奇异值
    Mat min_sing_vec = V.row(V.rows-1); // 最后一行为最小奇异值对应的右奇异向量
	plane[0]=min_sing_vec[0]
	plane[1]=min_sing_vec[1]
	plane[2]=min_sing_vec[2]
	plane[4]= -(plane[0]*centor.at<float>(0,0)+plane[1]*centor.at<float>(0,1)+plane[2]*centor.at<float>(0,2)
}

1.6 使用Eigen进行求解

void FitPlaneSVD::compute()
{
	// 1、计算质心
	Eigen::RowVector3d centroid = m_cloud.colwise().mean();
	// 2、去质心
	Eigen::MatrixXd demean = m_cloud;
	demean.rowwise() -= centroid;
	// 3、SVD分解求解协方差矩阵的特征值特征向量
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(demean, Eigen::ComputeThinU | Eigen::ComputeThinV);
	Eigen::Matrix3d V = svd.matrixV();
	Eigen::MatrixXd U = svd.matrixU();
	Eigen::Matrix3d S = U.inverse() * demean * V.transpose().inverse();
	// 5、平面的法向量a,b,c
	Eigen::RowVector3d normal;
	normal << V(0,2), V(1,2), V(2,2);
	// 6、原点到平面的距离d
	double d = -normal * centroid.transpose();
	// 7、获取拟合平面的参数a,b,c,d和质心x,y,z。
	m_planeparameters << normal, d, centroid
}

2.最小二乘法求解平面点云

2.1问题描述

找到一个平面
Z=Ax+By+C
根据最小二乘法,使各个点到这个平面的距离最近:
S=∑(Axi + Byi + C - Zi) 2

2.2问题描述

3D点云处理:用SVD分解法和最小二乘法拟合平面点云,求解平面方程

2.3问题使用opencv进行求解

void CaculateLaserPlane(std::vector<cv::Point3f> Points3ds,vector<double> &res)
{
	//最小二乘法拟合平面
	//获取cv::Mat的坐标系以纵向为x轴,横向为y轴,而cvPoint等则相反
	//系数矩阵
	cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1);
	//
	cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1);
	//结果矩阵
	cv::Mat X = cv::Mat::zeros(3, 1, CV_64FC1);
	double x2 = 0, xiyi = 0, xi = 0, yi = 0, zixi = 0, ziyi = 0, zi = 0, y2 = 0;
	for (int i = 0; i < Points3ds.size(); i++)
	{
		x2 += (double)Points3ds[i].x * (double)Points3ds[i].x;
		y2 += (double)Points3ds[i].y * (double)Points3ds[i].y;
		xiyi += (double)Points3ds[i].x * (double)Points3ds[i].y;
		xi += (double)Points3ds[i].x;
		yi += (double)Points3ds[i].y;
		zixi += (double)Points3ds[i].z * (double)Points3ds[i].x;
		ziyi += (double)Points3ds[i].z * (double)Points3ds[i].y;
		zi += (double)Points3ds[i].z;
	}
	A.at<double>(0, 0) = x2;
	A.at<double>(1, 0) = xiyi;
	A.at<double>(2, 0) = xi;
	A.at<double>(0, 1) = xiyi;
	A.at<double>(1, 1) = y2;
	A.at<double>(2, 1) = yi;
	A.at<double>(0, 2) = xi;
	A.at<double>(1, 2) = yi;
	A.at<double>(2, 2) = Points3ds.size();
	B.at<double>(0, 0) = zixi;
	B.at<double>(1, 0) = ziyi;
	B.at<double>(2, 0) = zi;
	//计算平面系数
	X = A.inv() * B;
	//A
	res.push_back(X.at<double>(0, 0));
	//B
	res.push_back(X.at<double>(1, 0));
	//Z的系数为1
	res.push_back(1.0);
	//C
	res.push_back(X.at<double>(2, 0));
	return;
}


参考链接

OpenCV最小二乘法拟合空间平面
最小二乘法拟合平面
点云拟合—平面拟合
SVD解决平面拟合问题文章来源地址https://www.toymoban.com/news/detail-437147.html

到了这里,关于3D点云处理:用SVD分解法和最小二乘法拟合平面点云,求解平面方程的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 3D点云处理:圆柱侧面点云展开为平面 凹凸缺陷检测(附源码)

    订阅说明:如果要订阅,先看链接内容 看链接内容 看链接内容:订阅先看此内容 文章目录: 3D视觉个人学习目录 目标:对采集的圆柱面点云展开为平面; 应用:可用于检测圆柱侧面的凹凸缺陷;       圆柱的侧面展开原理是将一个圆柱体(或柱体)的侧面展开成一个矩

    2024年02月09日
    浏览(138)
  • 3D点云处理:拟合球(附源码)

    订阅说明:如果要订阅,先看链接内容 看链接内容 看链接内容:订阅先看此内容 文章目录: 3D视觉个人学习目录       球面拟合是一种用于找到最佳拟合球体的方法,通常使用最小二乘法进行拟合。为了使用最小二乘法拟合球,我们首先需要定义一个损失函数,并通过最

    2024年02月10日
    浏览(32)
  • 矩阵:采用奇异值分解(SVD)对n个点进行平面拟合

    奇异值分解(Singular Value Decomposition, SVD),是线性代数中一种重要的矩阵分解,在信号处理、统计学等领域有重要应用。奇异值分解在某些方面与对称矩阵或厄米矩阵基于特征向量的对角化类似。对称矩阵特征向量分解的基础是谱分析,而奇异值分解则是谱分析理论在任意矩

    2023年04月08日
    浏览(40)
  • PCL点云处理之最小二乘直线拟合(2D| 方法2)(❤亲测可用❤)(二百零一)

    在二百章中,我们介绍了一种最小二乘拟合直线点云(2D)的方法,可以获取直线方程系数k,b,这里介绍另一种拟合直线点云的方法,更为简单方便,结果与前者一致,主要内容直接复制代码使用即可,原理简单看代码即可,下面是具体的实现和拟合结果展示 离散点云中拟合规

    2024年02月16日
    浏览(75)
  • 拟合算法之最小二乘法

    与插值问题不同,在拟合问题中不需要曲线一定经过给定的点。拟合问题的目标是追求一个函数(曲线),使得该曲线在某种准测下与所有的数据点最为接近,即曲线拟合最好(最小化损失函数)。 插值算法中,得到的多项式f(x)要经过所有的样本点。但是如果样本点太多,

    2024年02月04日
    浏览(59)
  • 数值分析——曲线拟合的最小二乘法

    拟合曲线定义:求近似函数 φ(x), 使之 “最好” 的逼近f(x) ,无需满足插值原则. 这就是曲线拟合问题。 (时间紧迫直接看例子就行,智慧交通专业的补修课,可能理论学的不那么深入,主要是方法。) 超定方程组 是指方程个数大于未知量个数的方程组 。 最小二乘解 : 对于

    2024年02月09日
    浏览(44)
  • Matlab 最小二乘法 拟合平面 (PCL PCA拟合平面)

    最小二乘法 拟合平面是我们最常用的拟合平面的方法,但是有特殊的情况是用这种方法是不能拟合的,后续会加上这种拟合方法(RANSAC)。 matlab 最小二乘拟合平面(方法一) - 灰信网(软件开发博客聚合) 平面方程:Ax+By+Cz+D=0;   1、随机出来一些离散的点    2、将其写成

    2024年02月16日
    浏览(48)
  • 【Matlab】最小二乘法拟合多项式

    在最近的电机项目中,有遇到有传感器数据并不线性的问题,然后想要用最小二乘法做个曲线拟合,反过来去校准不线性的传感器的数据,因此记录一下使用最小二乘法来拟合多项式的曲线的步骤。本篇从最小二乘法的原始公式入手编写M文件,目的是方便使用单片机实现,或

    2023年04月22日
    浏览(46)
  • 最小二乘法的几种拟合函数

    目录 1.最小二乘法的原理和解决的问题 2.最小二乘法的公式解法 2.1  拟合h(x) = a * x 2.2 拟合 h(x) = a0 + a1*x 2.3拟合 h(x) = a0 + a1 *x + a3 * x^3  因为采用矩阵法来进行最小二乘法的函数拟合时,会出现系数矩阵的逆矩阵不存在的情况有一定的局限性,所以本篇对公式法进行简单说明

    2024年02月13日
    浏览(42)
  • C语言编程:最小二乘法拟合直线

    本文研究通过C语言实现最小二乘法拟合直线。 最小二乘法,简单来说就是根据一组观测得到的数值,寻找一个函数,使得函数与观测点的误差的平方和达到最小。在工程实践中,这个函数通常是比较简单的,例如一次函数或二次函数。 汽车上的毫米波雷达可以探测到其他目

    2024年02月12日
    浏览(40)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包