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

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

相关文章

深度解析Java DTO(最新推荐)

《深度解析JavaDTO(最新推荐)》DTO(DataTransferObject)是一种用于在不同层(如Controller层、Service层)之间传输数据的对象设计模式,其核心目的是封装数据,... 目录一、什么是DTO?DTO的核心特点:二、为什么需要DTO?(对比Entity)三、实际应用场景解析

深度解析Java项目中包和包之间的联系

《深度解析Java项目中包和包之间的联系》文章浏览阅读850次,点赞13次,收藏8次。本文详细介绍了Java分层架构中的几个关键包:DTO、Controller、Service和Mapper。_jav... 目录前言一、各大包1.DTO1.1、DTO的核心用途1.2. DTO与实体类(Entity)的区别1

深度解析Python装饰器常见用法与进阶技巧

《深度解析Python装饰器常见用法与进阶技巧》Python装饰器(Decorator)是提升代码可读性与复用性的强大工具,本文将深入解析Python装饰器的原理,常见用法,进阶技巧与最佳实践,希望可... 目录装饰器的基本原理函数装饰器的常见用法带参数的装饰器类装饰器与方法装饰器装饰器的嵌套与组合进阶技巧

深度解析Spring Boot拦截器Interceptor与过滤器Filter的区别与实战指南

《深度解析SpringBoot拦截器Interceptor与过滤器Filter的区别与实战指南》本文深度解析SpringBoot中拦截器与过滤器的区别,涵盖执行顺序、依赖关系、异常处理等核心差异,并... 目录Spring Boot拦截器(Interceptor)与过滤器(Filter)深度解析:区别、实现

深度解析Spring AOP @Aspect 原理、实战与最佳实践教程

《深度解析SpringAOP@Aspect原理、实战与最佳实践教程》文章系统讲解了SpringAOP核心概念、实现方式及原理,涵盖横切关注点分离、代理机制(JDK/CGLIB)、切入点类型、性能... 目录1. @ASPect 核心概念1.1 AOP 编程范式1.2 @Aspect 关键特性2. 完整代码实

SpringBoot开发中十大常见陷阱深度解析与避坑指南

《SpringBoot开发中十大常见陷阱深度解析与避坑指南》在SpringBoot的开发过程中,即使是经验丰富的开发者也难免会遇到各种棘手的问题,本文将针对SpringBoot开发中十大常见的“坑... 目录引言一、配置总出错?是不是同时用了.properties和.yml?二、换个位置配置就失效?搞清楚加

Python中文件读取操作漏洞深度解析与防护指南

《Python中文件读取操作漏洞深度解析与防护指南》在Web应用开发中,文件操作是最基础也最危险的功能之一,这篇文章将全面剖析Python环境中常见的文件读取漏洞类型,成因及防护方案,感兴趣的小伙伴可... 目录引言一、静态资源处理中的路径穿越漏洞1.1 典型漏洞场景1.2 os.path.join()的陷

Spring Boot拦截器Interceptor与过滤器Filter深度解析(区别、实现与实战指南)

《SpringBoot拦截器Interceptor与过滤器Filter深度解析(区别、实现与实战指南)》:本文主要介绍SpringBoot拦截器Interceptor与过滤器Filter深度解析... 目录Spring Boot拦截器(Interceptor)与过滤器(Filter)深度解析:区别、实现与实

MyBatis分页插件PageHelper深度解析与实践指南

《MyBatis分页插件PageHelper深度解析与实践指南》在数据库操作中,分页查询是最常见的需求之一,传统的分页方式通常有两种内存分页和SQL分页,MyBatis作为优秀的ORM框架,本身并未提... 目录1. 为什么需要分页插件?2. PageHelper简介3. PageHelper集成与配置3.

Maven 插件配置分层架构深度解析

《Maven插件配置分层架构深度解析》:本文主要介绍Maven插件配置分层架构深度解析,本文给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价值,需要的朋友参考下吧... 目录Maven 插件配置分层架构深度解析引言:当构建逻辑遇上复杂配置第一章 Maven插件配置的三重境界1.1 插件配置的拓扑