最小二乘法——拟合平面方程(深度相机外参标定、地面标定)

2023-10-07 12:59

本文主要是介绍最小二乘法——拟合平面方程(深度相机外参标定、地面标定),希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

1.最小二乘法

最小二乘法(又称最小平方法)是一种数学优化技术。它通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据,并使得这些求得的数据与实际数据之间误差的平方和为最小。最小二乘法还可用于曲线拟合。其他一些优化问题也可通过最小化能量或最大化熵用最小二乘法来表达。

最小二乘法的矩阵形式为:
A x = b Ax=b Ax=b
其中 A A A n ∗ k n * k nk 的矩阵, x x x k ∗ 1 k*1 k1 的列向量, b b b n ∗ 1 n*1 n1 的列向量。如果 n > k n>k n>k(方程的个数大于未知量的个数),这个方程系统称为矛盾方程组 Over Determined System,如果 n < k n<k n<k(方程的个数小于未知量的个数),这个系统就是Under Determined System。

当找到向量 x x x 使得 ∣ ∣ A x − b ∣ ∣ ||Ax-b|| Axb 最小,则 x x x 为该方程的最小二乘解

求解最小二乘的方法有奇异值分解、正规方程、QR分解三种。本文中采用正规方程对平面方程进行拟合,以实现深度相机的外参标定。正规方程组的解为:
x = ( A T A ) − 1 A T b x=(A^TA)^{-1}A^Tb x=(ATA)1ATb

2.平面方程拟合

平面方程的一般表达式为
A x + B y + C z + D = 0 ( C ≠ 0 ) Ax+By+Cz+D=0 (C\neq0) Ax+By+Cz+D=0C=0
将其变换为如下形式
z = − A C x − B C y − D C z=-\frac{A}{C}x-\frac{B}{C}y-\frac{D}{C} z=CAxCByCD
a 0 = − A C ; a_0=-\frac{A}{C}; a0=CA; a 1 = − B C ; a_1=-\frac{B}{C}; a1=CB; a 2 = − D C ; a_2=-\frac{D}{C}; a2=CD;
z = a 0 x + a 1 y + a 2 z=a_0x+a_1y+a_2 z=a0x+a1y+a2
此时对应的最小二乘矩阵形式

A = ( x 1 y 1 1 x 2 y 2 1 . . . x n y n 1 ) ; x = ( a 0 a 1 a 2 ) ; b = ( z 1 z 2 . . . z n ) ; ( n ≥ 3 ) A=\begin{pmatrix} x_1&y_1&1\\x_2&y_2&1\\...\\x_n&y_n&1\end{pmatrix}; x=\begin{pmatrix}a_0\\a_1\\a_2\end{pmatrix};b=\begin{pmatrix} z_1 \\ z_2 \\ ...\\ z_n\end{pmatrix};(n\geq3) A=x1x2...xny1y2yn111;x=a0a1a2;b=z1z2...zn;(n3)

其中 ( x 1 , y 1 , z 1 ) , ( x 2 , y 2 , z 2 ) , . . . , ( x n , y n , z n ) (x_1,y_1,z_1),(x_2,y_2,z_2),...,(x_n,y_n,z_n) (x1,y1,z1),(x2,y2,z2),...,(xn,yn,zn)为输入的三维点坐标。

套用正规方程组的解,即可求得 ( a 0 , a 1 , a 2 ) ; (a_0,a_1,a_2); (a0,a1,a2);

3.标定——构造旋转矩阵

在实际使用中,经常会采用地面作为参照平面,将相机坐标系转化为世界坐标系,本文中使用最小二乘法对地面点云拟合平面方程,将相机坐标系Z轴旋转至垂直地面

如上求解出地面的平面方程系数,则平面方程一般式为:
a 0 x + a 1 y − z + a 2 = 0 a_0x+a_1y-z+a_2=0 a0x+a1yz+a2=0
其法向量为:
( a 0 a 0 2 + a 1 2 + 1 , a 1 a 0 2 + a 1 2 + 1 , − 1 a 0 2 + a 1 2 + 1 ) (\frac{a_0}{\sqrt{a_0^2+a_1^2+1}},\frac{a_1}{\sqrt{a_0^2+a_1^2+1}},\frac{-1}{\sqrt{a_0^2+a_1^2+1}}) (a02+a12+1 a0,a02+a12+1 a1,a02+a12+1 1)
求得平面法向量的单位向量为 n ⃗ \vec{n} n
相机坐标系的Z轴向量 z ⃗ \vec{z} z ( 0 , 0 , 1 ) (0,0,1) (0,0,1)
旋转向量为 r ⃗ \vec{r} r ,其中 r ⃗ \vec{r} r 方向为 n ⃗ × z ⃗ \vec{n}\times\vec{z} n ×z ,旋转角度为 θ = a r c c o s ( n ⃗ ⋅ r ⃗ ) \theta=arccos(\vec{n}\cdot\vec{r}) θ=arccos(n r )

使用 Eigen::AngleAxisd 将旋转向量转化为 Eigen::Matrix3d 的旋转矩阵。

4.代码

	//0.最小二乘拟合平面方程//planePoints存储相机坐标系选择地面区域内的所有三维点云Eigen::MatrixXd A(planePoints.size(), 3);Eigen::VectorXd b(planePoints.size());//将观测点输入矩阵for (int i = 0; i < planePoints.size(); i++){A(i, 0) = planePoints[i].x;A(i, 1) = planePoints[i].y;A(i, 2) = 1;b(i) = planePoints[i].z;}Eigen::MatrixXd AT = A.transpose();//使用最小二乘法求得系数向量Eigen::Vector3d x = (AT*A).inverse()*AT*b;//1.求解旋转矩阵//单位法向量double denominator = sqrt(x(0)*x(0) + x(1)*x(1) + 1);Eigen::Vector3d n(x(0) / denominator, x(1) / denominator, -1 / denominator);n = n.normalized();Eigen::Vector3d zdir(0, 0, 1);//求解两向量的旋转向量,点乘求夹角、叉乘求旋转方向。Eigen::AngleAxisd rotateVector(acos(n.dot(zdir)), n.cross(zdir).normalized());//获取旋转矩阵Eigen::Matrix3d zRotateMatrix = rotateVector.matrix();

5.结果

首先选择一系列三维点云(蓝色代表有点云),如下图:

在这里插入图片描述
未转化的相机坐标系下三维点云如下图:

在这里插入图片描述

用上述构造旋转矩阵进行点云坐标系变换,令Z轴方向垂直地面,结果如下:

在这里插入图片描述
在这里插入图片描述

这篇关于最小二乘法——拟合平面方程(深度相机外参标定、地面标定)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/157908

相关文章

SpringCloud动态配置注解@RefreshScope与@Component的深度解析

《SpringCloud动态配置注解@RefreshScope与@Component的深度解析》在现代微服务架构中,动态配置管理是一个关键需求,本文将为大家介绍SpringCloud中相关的注解@Re... 目录引言1. @RefreshScope 的作用与原理1.1 什么是 @RefreshScope1.

Python 中的异步与同步深度解析(实践记录)

《Python中的异步与同步深度解析(实践记录)》在Python编程世界里,异步和同步的概念是理解程序执行流程和性能优化的关键,这篇文章将带你深入了解它们的差异,以及阻塞和非阻塞的特性,同时通过实际... 目录python中的异步与同步:深度解析与实践异步与同步的定义异步同步阻塞与非阻塞的概念阻塞非阻塞同步

Redis中高并发读写性能的深度解析与优化

《Redis中高并发读写性能的深度解析与优化》Redis作为一款高性能的内存数据库,广泛应用于缓存、消息队列、实时统计等场景,本文将深入探讨Redis的读写并发能力,感兴趣的小伙伴可以了解下... 目录引言一、Redis 并发能力概述1.1 Redis 的读写性能1.2 影响 Redis 并发能力的因素二、

最新Spring Security实战教程之表单登录定制到处理逻辑的深度改造(最新推荐)

《最新SpringSecurity实战教程之表单登录定制到处理逻辑的深度改造(最新推荐)》本章节介绍了如何通过SpringSecurity实现从配置自定义登录页面、表单登录处理逻辑的配置,并简单模拟... 目录前言改造准备开始登录页改造自定义用户名密码登陆成功失败跳转问题自定义登出前后端分离适配方案结语前言

Redis 内存淘汰策略深度解析(最新推荐)

《Redis内存淘汰策略深度解析(最新推荐)》本文详细探讨了Redis的内存淘汰策略、实现原理、适用场景及最佳实践,介绍了八种内存淘汰策略,包括noeviction、LRU、LFU、TTL、Rand... 目录一、 内存淘汰策略概述二、内存淘汰策略详解2.1 ​noeviction(不淘汰)​2.2 ​LR

Python与DeepSeek的深度融合实战

《Python与DeepSeek的深度融合实战》Python作为最受欢迎的编程语言之一,以其简洁易读的语法、丰富的库和广泛的应用场景,成为了无数开发者的首选,而DeepSeek,作为人工智能领域的新星... 目录一、python与DeepSeek的结合优势二、模型训练1. 数据准备2. 模型架构与参数设置3

Java深度学习库DJL实现Python的NumPy方式

《Java深度学习库DJL实现Python的NumPy方式》本文介绍了DJL库的背景和基本功能,包括NDArray的创建、数学运算、数据获取和设置等,同时,还展示了如何使用NDArray进行数据预处理... 目录1 NDArray 的背景介绍1.1 架构2 JavaDJL使用2.1 安装DJL2.2 基本操

最长公共子序列问题的深度分析与Java实现方式

《最长公共子序列问题的深度分析与Java实现方式》本文详细介绍了最长公共子序列(LCS)问题,包括其概念、暴力解法、动态规划解法,并提供了Java代码实现,暴力解法虽然简单,但在大数据处理中效率较低,... 目录最长公共子序列问题概述问题理解与示例分析暴力解法思路与示例代码动态规划解法DP 表的构建与意义动

Go中sync.Once源码的深度讲解

《Go中sync.Once源码的深度讲解》sync.Once是Go语言标准库中的一个同步原语,用于确保某个操作只执行一次,本文将从源码出发为大家详细介绍一下sync.Once的具体使用,x希望对大家有... 目录概念简单示例源码解读总结概念sync.Once是Go语言标准库中的一个同步原语,用于确保某个操

五大特性引领创新! 深度操作系统 deepin 25 Preview预览版发布

《五大特性引领创新!深度操作系统deepin25Preview预览版发布》今日,深度操作系统正式推出deepin25Preview版本,该版本集成了五大核心特性:磐石系统、全新DDE、Tr... 深度操作系统今日发布了 deepin 25 Preview,新版本囊括五大特性:磐石系统、全新 DDE、Tree