精华内容
下载资源
问答
  • 2020-12-22 14:43:48

    一种基于三维曲线匹配的点云相对位姿估计方法

    【专利摘要】本发明公开了一种基于三维曲线匹配的点云相对位姿估计方法,它的步骤是提取两帧输入点云的边缘轮廓,得到代表三维边缘轮廓曲线的三维点集;针对三维点集提取得到有序点集的集合;将每个有序点集进行分解,分解后的每个子集表示一个圆弧段,建立每段圆弧的特征描述向量;匹配两帧点云边缘曲线圆弧段的特征描述向量,建立匹配关系,计算两帧点云的相对位姿变换矩阵。本发明方法不依赖初始位姿,仅通过环境的轮廓进行匹配,估计相对位姿;仅处理边缘点集,降低了运算量;可适应点云中存在的干扰物。

    【专利说明】一种基于三维曲线匹配的点云相对位姿估计方法【技术领域】

    [0001]本发明涉及三维环境重构领域,尤其涉及一种基于三维曲线匹配的点云相对位姿估计方法。

    【背景技术】

    [0002]已有的点云相对位姿估计方法主要分为两类。一类基于点云特征匹配,缺点是受点云密度变化影响很大,密度差异会直接导致错误概率提高。另一类基于点云的直接匹配,缺点是依赖初始位姿,且运算量大。

    [0003]对于三维曲线的匹配,已有方法也主要基于卷积,难以适用于多曲线对多曲线的匹配。

    【发明内容】

    [0004]本发明的目的是克服现有技术的不足,提供一种基于三维曲线匹配的点云相对位姿估计方法。

    [0005]基于三维曲线匹配的点云相对位姿估计方法是:

    O分别提取两帧输入点云的边缘轮廓,得到代表三维边缘轮廓曲线的三维点集

    L1^L2 ;

    2)分别针对三维点集ΖΛΙ2提取得到有序点集的集合,每个有序点集L丨或句均表不了一条二维曲线;

    j

    3)将每个有序点集Li或L2进行分解,分解后的每个子集表示一个圆弧段,建立每段圆弧的特征描述向量;

    4)匹配两帧点云边缘曲线圆弧段的特征描述向量,建立匹配关系,计算两帧点云的相对位姿变换矩阵为旋转矩阵,t为平移矩阵。

    [0006]所述的步骤I)为:a)对于无序点云,搜索每个点周围半径小于r范围内的数量不超过n的所有最近邻点,得到点集N,为点集N拟合平面G,以点集N在平面G内的投影点位置为自变量,点集iV到平面G的距离为函数值,拟合二元二次函数f,得到Hessian矩阵H,计算Hessian矩阵If的特征值I1和I,假定λ1 >λ2 若λ1 > thresh且

    λ2 < thresh2 ,其中thresh1、thresh2别为相应阈值,则认为该点为边缘点;b)对于有序点云,即深度图,利用Canny算法提取边缘点集。

    [0007]所述的为点集N拟合平面G的方法为:计算点集N的均值,得到平面G的中心

    ;计算的特征向量,其最小特征值对应的特征向量即为平面G的法向量NG;平面G的中心和法向量1?即表示了一个经过中心Ce、法向量为的平面。

    [0008]所述的拟合二元二次函数/并得到Hessian矩阵孖的方法为:对于点集i¥中每一

    点q,假定NTN最大的两个特征值对应的特征向量分别为αG和βG,计算一个以x、y为自

    变量,左(.v,y)为值的键值对,

    【权利要求】

    1.一种基于三维曲线匹配的点云相对位姿估计方法,其特征在于它的步骤如下: 1)分别提取两帧输入点云的边缘轮廓,得到代表三维边缘轮廓曲线的三维点集IM2 ; 2)分别针对三维点集Hi2提取得到有序点集的集合辽丨,每个有序点集if或 均表不了一条二维曲线; 3)将每个有序点集或g进行分解,分解后的每个子集表示一个圆弧段,建立每段圆弧的特征描述向量; 4)匹配两帧点云边缘曲线圆弧段的特征描述向量,建立匹配关系,计算两帧点云的相对位姿变换矩阵(I?, T),Λ为旋转矩阵,T为平移矩阵。

    2.根据权利要求1所述的基于三维曲线匹配的点云相对位姿估计方法,其特征在于,所述的步骤I)为:a)对于无序点云,搜索每个点周围半径小于r范围内的数量不超过的所有最近邻点,得到点集iV ,为点集iV拟合平面G,以点集Λτ在平面G内的投影点位置为自变量,点集N到平面G的距离为函数值,拟合二元二次函数/,得到Hessian矩阵if ,计算Hessian矩阵/f的特征值1,和I,假定i >厶,若A1 > threslb,且A7 < thresiu,其中f/rres/iyrtres/i^、别为相应阈值,则认为该点为边缘点;b)对于有序点云,即深度图,利用Canny算法提取边缘点集。

    3.根据权利要求2所述的基于三维曲线匹配的点云相对位姿估计方法,其特征在于,所述的为点集i¥拟合平面G的方法为:计算点集AT的均值,得到平面G的中心Ce ;计算iV'V的特征向量,其最小特征值对应的特征向量即为平面G的法向量《5 ;平面G的中心Ce和法向量11(3即表示了一个经过中心Ci5、法向量为的平面。

    4.根据权利要求2所述的基于三维曲线匹配的点云相对位姿估计方法,其特征在于,所述的拟合二元二次函数/并得到Hessian矩阵嚴的方法为:对于点集JV中每一点1|,假定N7N最大的两个特征值对应的特征向量分别为和!?^,计算一个以Λ; 'V为自变量,A-0^3,)为值的键值对,

    5.根据权利要求1所述的基于三维曲线匹配的点云相对位姿估计方法,其特征在于,所述的步骤2)为:使用区域增长算法,以每次从三维点集I2或L2中随机选取的区域增长种子点为起始,以点集所表示的曲线一阶连续为条件,通过不断吸纳符合增长条件的新点,有序地拓展曲线点集,若不能再拓展,则另取种子点并拓展另一个曲线点集,直至所有点被拓展完毕。

    6.根据权利要求5所述的基于三维曲线匹配的点云相对位姿估计方法,其特征在于,所述的曲线一阶连续条件为:

    7.根据权利要求1所述的基于三维曲线匹配的点云相对位姿估计方法,其特征在于,所述的步骤3)为:对于每一个表示三维曲线的有序点集Li或M,依次计算每个点P,—处曲

    线的曲率IC:和扭率h ,

    8.根据权利要求1所述的基于三维曲线匹配的点云相对位姿估计方法,其特征在于,所述的步骤4)为:针对两帧点云边缘曲线圆弧段的特征描述向量,建立k-d树,在k-d树中,建立两帧点云边缘曲线圆弧段之间的匹配关系,运用随机抽样一致性算法,去除部分错误匹配,建立代价函数

    【文档编号】G06T17/00GK103729882SQ201310742491

    【公开日】2014年4月16日 申请日期:2013年12月30日 优先权日:2013年12月30日

    【发明者】熊蓉, 李千山 申请人:浙江大学

    更多相关内容
  • 针对无人机自动空中加油过程中近距离高精度相对位姿估计问题,提出了一种基于特征跟踪的单目视觉方法。在加油机机身预先设置多个辅助识别标志点,安装在无人机机头上的摄像机获取加油机图像,将提取出的图像特征点与...
  • 为解决对空间未知目标的相对位置、姿态估计问题,以激光成像雷达作为测量敏感器,提出了一种基于扩展Kalman 滤波(EKF)的相对位姿估计算法。采用迭代最近点算法(Iterative Closest Point, ICP)对激光雷达的点云...
  • 相机标定源代码校准相机相对位姿估计的算法集 介绍 该存储库包含以下相对姿态估计求解器,采用 C++ 和 Matlab API: 传统的 5 点算法 (5P),包装自 Hartley 的著名实现。 已知旋转角度的 4 点算法 (4P-RA) 平面运动...
  • 文稿整理者:小萝卜审稿&修改:赵季博士本文总结自12月4日赵季博士关于【相对位姿估计的进展和新方法】公开课。位姿估计是多视图几何的重要方向。赵博士在公开课第一部分首先概述了位姿估计...

    文稿整理者:小萝卜

    审稿&修改:赵季博士


    本文总结自12月4日赵季博士关于【相对位姿估计的进展和新方法】公开课。

    位姿估计是多视图几何的重要方向。赵博士在公开课第一部分首先概述了位姿估计的背景,然后回顾了经典的最小配置问题、多项式方程组的解法,介绍了目前学术界的研究方向,最后给出学习资料和相关软件资源。

    第二部分中,赵博士介绍了经典的最小配置问题(5点法&6点法)的新解法。这部分包括赵博士的工作(把rotation和translation解耦,构造只包含rotation的方程组)。

    第三部分,赵博士介绍了从新的几何特征“射线-点-射线”中估计位姿。第四部分,赵博士介绍了N点法的全局最优位姿估计。

    01

    位姿估计背景介绍和经典方法介绍

    位姿估计是三维视觉的重要基础,有着非常广泛的应用,例如无人机定位,无人车定位,三维重建等。

    按照给定的观测不同,位姿估计可以分为三大类。根据点云求位姿,属于3D-3D位姿估计,称作点云配准问题,代表算法包括ICP,NDT,LOAM等。根据图像点和地图点求位姿,属于3D-2D位姿估计,称作绝对位姿估计,代表算法包括P3P,PnP等。根据具有公共视野的图像求位姿,属于2D-2D位姿估计,称作相对位姿估计,代表算法包括5点法、8点法、单应矩阵法等。本公开课重点关注2D-2D相对位姿估计方法。

    赵博士首先介绍了经典的最小配置问题以及其最小配置解,即至少需要多少个点(观测)来求相对位姿。对于单目相机,最少需要5个点,对于多目相机或者广义相机,最少需要6个点。参考论文[C. Longuet-Higgins 1981], [D.Nister 2004], [H. Stewenius et al 05,06].

    随后,赵博士介绍了定制化最小配置问题的三个研究方向,分别是:

    1.利用先验降低问题的自由度,例如已知旋转角、已知旋转轴、平面运动约束、阿克曼运动模型等

    2.将其他变量和位姿同时估计,例如相机焦距、主点、畸变参数等。

    3.处理非标准的位姿估计问题,例如使用新型的特征匹配恢复位姿(仿射匹配,射线-点-射线匹配)、处理新型相机或场景等。

    最小配置问题的核心是求解多项式方程组。接下来,赵博士介绍了多项式方程组求解的方法以及工具。常见方法包括closed-form solution, companion matrix, Sturm sequence, Gröbner basis (格罗布纳基), resultant method (结式法), Wu’s method (吴文俊方法), homotopy continuation (同伦连续)等。工具包括Macaulay2, Maple,以及方程求解器的自动生成程序。赵博士在公开课中着重介绍了针对多元高阶方程的Gröbner basis方法。Gröbner basis方法当前分为离线和在线两部分,离线构造出消元模板,在线填充消元模板中的非零元素,便于提高计算效率。感兴趣的同学可以看原公开课视频。

    第一部分最后,赵博士介绍了最优位姿估计的研究方向,包括设计具有鲁棒性和全局最优性的算法,解的最优性认证,从两视图的位姿估计扩展到多视图,基于机器学习的位姿估计方法等。

    02

    5点法 & 6点法

    赵博士在第二部分中介绍了他和合作者的工作,主要是对经典最小配置问题提出新的解法。在最小配置问题中,对于单目相机,最少需要5个点可以恢复位姿。而对于一般的多目相机,最少需要6个点。

    572982887f6d9621386db55c7a0f2e6e.png

    (图1)左:单目相机 右:多目相机

    单目相机和多目相机的位姿恢复都可以根据方向向量和平移的共面性建立几何约束。

    (1)单目相机

    对于单目相机,经典的对极几何刻画了两个方向向量、平移向量t之间的共面关系。具体做法是建立约束57e2525dd4f30f6dfad2450e2c011005.png,即1b5db3baebd0e4f4bc08e6a9fe665115.png,令7b503c5a60bc165f86283bdc2f979c69.png,称为本质矩阵,则e914061d662e0e61d8627ca005d7d46a.png。也可以不引入本质矩阵,使用更加直接的方式建模。例如,在[Kneip et al 2012 ECCV]中潜在地使用了一种表示形式:

    c4524939faecb45f3f35e31e9f63e13e.png

    此时可以去掉t,令矩阵不满秩即可,任取矩阵3行构造的行列式应为0。这样就得到了只与R相关的约束,实现了R和t的解耦。当R求出后,对相应的5*3矩阵做SVD分解求t。

    (2)多目相机

    对于多目相机,不同相机的光心之间还有一个相对偏置,因此多目相机的对极几何变得复杂,为:

    37a8ac9ce9a99f2789fec27da69e3a27.png

    可以看出,单目情况的3维方向向量变成了6维的普吕克直线表示,单目情况的3*3的本质矩阵变成了6*6的广义本质矩阵。单目的五点法[D. Nister 2004]没法推广到多目情况。为了解决这个问题,[Stewenius, Astrom, Nister 2005]提出了一种匹配点深度参数化的解法,成为多年来几乎唯一的解法。

    我们的旋转-平移解耦方法可以轻易地推广到多目相机。区别在于单目的5*3矩阵变成了6*4矩阵,再任取其中的4*4矩阵,令它门的行列式为0。此外,如果存在三个匹配在两个视图中都被相同的单相机成像,则需要增加3*3子矩阵的行列式为0的约束。

    需要对旋转R给出合适的参数化方法,用于构建多项式方程。R有多种表示方法,赵博士列举了Cayley,Quaternion,Direction cosine matrix(DCM)三种方法。

    1b073025101ca24c3f2ca92a7e9a1d5d.png

    如何去选择旋转的表示方法呢?好的表示方法要使方程组的阶次尽量低,未知变量尽量少,对称性尽量少。

    赵博士工作用Cayley和quaternion表示方法来求解5点法[1]和6点法[3]中旋转矩阵R。Cayley表示旋转的具体5点/6点法求解步骤在视频中有讲解,感兴趣的同学请看原公开课,在此不赘述。

    赵博士总结了R和t解耦方法的一般步骤[3]:

    1. 用Rt写出约束,R建议用Cayley或quaternion表示。

    2. 将约束表示为M*t=0,其中M只和R相关。单相机的t是3维,多相机系统的t是4维齐次坐标。

    3. 让M子矩阵的行列式为0,构造多项式方程。

    4. 对于多相机,如果存在三个匹配在两个视图中都被相同的单相机成像,则需要增加3*3子矩阵的行列式为0的约束。

    5. 对于步骤3和4的多项式方程,除以Cayley或quaternion相关的尺度因子。

    6. 使用多项式方程求解的工具链求出R,再求出t

    本部分最后,赵博士介绍了其工作的实验结果。5点法对纯旋转比较友好,扩展性强。6点法相比于之前方法具有更高的效率,更好的数值稳定性,扩展性更强,可以方便地推广到各种复杂的定制化问题。

    03

    从射线-点-射线(RPR)中恢复位姿

    c7cb499da68d0f61c9e96470cddc36da.png

    (图2)室内RPR几何结构

    结构化环境中建图和定位时,注意到室内有很多线段结构。但是,理论上可以证明无法从两视图的线段匹配中恢复位姿。因此,提出了将射线-点-射线组成角进行匹配,这种结构称为RPR (ray-point-ray)。如果知道这种角结构在3D空间中的真实角度,我们就可以建立约束来估计位姿。如图三所示,为角结构的几何约束示意图。

    60f27a6096579f19d99b79ebee67aa12.png

    (图3) 二视图中的RPR约束

    每个ray的观测引入了一个平面,可以求出平面的法向量。由于3D空间中的ray同时位于两个平面上,它的方向向量正比于两个法向量的叉乘。因此我们可以建立约束。

    07969470412b2c319b953b00cb665c83.png

    对于90度角:

    95957a14639ccf6a856bb41ff9ad7bd2.png

    对于一般角:

    2a8f8b0f63ec6aa181f5e294599b8786.png

    赵博士的实验结果如下。其中消元模板的尺寸是求解效率相关的指标,模板尺寸越小越好。可以看出,Cayley表示的求解效率是最高的。这种RPR约束具有理论价值,也为结构化场景的视觉定位提供了新思路。

    f4aa43ac8fa1e22638ec5a25c5f7008b.png

    (图4)不同R表示方法对RPR约束求解对比

    04

    N点法的全局最优位姿估计

    第四部分中,赵博士介绍了用多点法求全局最优位姿。在实际工作中,冗余观测较多,而且存在噪声。因此,多点法相比于最小配置解法,具有更高的精度。

    赵博士首先介绍了一些经典工作和相关论文。

    1.代数误差

    ·LMI optimization [Chesi 2009 PAMI]

    ·local optimization, BnB [Kneip & Lynen 2013 ICCV]

    ·SDP + R-T representation [Briales et al 2018 CVPR]

    2.几何误差(例如重投影误差)

    ·Gauss-Newton, Levenberg-Marquardt.

    3.DLT method

    ·8 point method + normalization [Hartley 1997 PAMI]

    4.Certifiable solvers for related tasks

    ·geometric perception [H. Yang & Luca Carlone ICCV’19, RAL’20, CVPR’20, NIPS’20]

    ·certifiable solvers [Briales et al IROS’16, ICRA’17, CVPR’17, Garcia-Salguero et al IVC’21]

    ·rotation averaging [Rosen et al IJRR’19, Eriksson et al CVPR’18, Dellaert et al ECCV’20]

    然后赵博士介绍了通过几何约束建立约束模型、和最小配置解不同的是,多点法需要考虑噪声,因此从解方程问题变成了优化问题,优化目标是使方程的残差尽可能接近0。

    使用代数误差时,优化问题为95a022d0c4b9f0ac79dd4fdbdf413cf4.png

    其中,归一化本质矩阵的集合定义为:

    92224b325f90eace1f1052d7b592d30f.png

    对目标函数进行整理,变为标准的二次型

    3e0aa39a86d50a7097993652edd3d7e3.png

    由于归一化本质矩阵的充分必要条件为

    6b441335f74b957774af5f4a73a71651.png

    可以减少约束中的未知数,把问题重新整理为标准QCQP问题:

    5c8b953f1937526c09f515e78d25351d.png

    我们需要对上述优化问题进行求最优值,下图为非凸QCQP问题的求解框架。核心思想是把原来的非凸优化通过半正定松弛转化为凸优化。求解完毕后再将最优解返回到原问题。

    d757225f412579a95e8f8e5b6c8d348b.png

    最后,赵博士介绍了一些相关的主题:

    1. 松弛紧性(tightness)的判断、松弛的局部稳定性证明;

    2. 把N点法嵌入到鲁棒框架,得到鲁棒N点法;

    3. 实验结果,包括效率评估、鲁棒N点法的精度和鲁棒性、匹配点的个数与位姿精度的关系等。

    05

    推荐的学习资源

    Tutorial & Survey

    1).The art of solving minimal problems

    -http://cmp.felk.cvut.cz/minimal-iccv-2015/

    -http://cmp.felk.cvut.cz/minimal-cvpr-2019/

    2).Minimal Problems in Computer Vision

    -http://aag.ciirc.cvut.cz/minimal/

    6).Global Optimization for Geometric Understanding with Provable Guarantees 

    -https://mit-spark.github.io/GlobalOptimization-ICCV2019/

    CAS Software

    1).Macaulay2:

    - http://www2.macaulay2.com/Macaulay2/

    2).Maple software

    Automatic solver generator程序

    1).Automatic generator for minimal solvers. 

    - http://people.inf.ethz.ch/vlarsson/

    2).Automatic generator. 

    - https://github.com/PavelTrutman/Automatic-Generator

    3).Polyjam. 

    - https://github.com/laurentkneip/polyjam

    4).Gaps. 

    - https://github.com/prclibo/gaps

    教材

    1).D.Cox et al. Ideals, Varieties, and Algorithms. Springer, 2013.

    2).D.Cox et al. Using Algebraic Geometry. Springer, 2006.

    中文资料

    1).王东明, 等. 计算机代数(第二版), 清华大学出版社, 2007.

    2).李超, 等. 计算机代数系统的数学原理. 清华大学出版社, 2010.

    3).北航牟老师. 计算机代数课程视频:

     https://www.bilibili.com/video/BV1p7411c7mz?p=1

    06

    参考文献

    [1] Ji Zhao, Laurent Kneip, Yijia He, and Jiayi Ma. Minimal Case Relative Pose Computation using Ray-Point-Ray Features.

    IEEE Transactions on Pattern Analysis and Machine Intelligence, 42(5): 1176 - 1190, 2020.

    [2] Ji Zhao. An Efficient Solution to Non-Minimal Case Essential Matrix Estimation. IEEE Transactions on Pattern Analysis and Machine Intelligence. DOI: 10.1109/TPAMI.2020.3030161.

    [3] Ji Zhao, and Banglei Guan. On Relative Pose Recovery for Multi-Camera Systems. ArXiv:2102.11996.

    Webpage: 

    https://sites.google.com/site/drjizhao/

    Github: 

    https://github.com/jizhaox

    
     
    
     
    
     
    
     
    
     
    往期精彩回顾
    
    
    
    
    适合初学者入门人工智能的路线及资料下载机器学习及深度学习笔记等资料打印机器学习在线手册深度学习笔记专辑《统计学习方法》的代码复现专辑
    AI基础下载黄海广老师《机器学习课程》视频课黄海广老师《机器学习课程》711页完整版课件

    本站qq群955171419,加入微信群请扫码:

    5a302a5be0b15789158980953c9b2a03.png

    展开全文
  • 云计算-基于INSGPS视觉的自主空中加油相对位姿估计算法研究.pdf
  • 位姿估计

    千次阅读 2019-11-28 19:44:49
    计算机视觉-位姿估计 1. 简述仿射变换和透视变换的基本概念,并用实例说明。 仿射变换: 计算机视觉中的仿射变换一般指:平移,比例缩放,旋转变换都是一种称为仿射变换的特殊情况。 仿射变换的性质: 1.仿射变换有6...

    计算机视觉-位姿估计

    1. 简述仿射变换和透视变换的基本概念,并用实例说明。

    仿射变换:
    计算机视觉中的仿射变换一般指:平移,比例缩放,旋转变换都是一种称为仿射变换的特殊情况。
    仿射变换的性质:
    1.仿射变换有6个自由度(对应变换中的6个系数:空间平移xyz,以及旋转变换三轴)应此仿射变换后互相平行直线仍然为平行直线,三角形映射后仍是三角形。但却不能保证将四边形上的多边形映射为等变数的多边形。
    2.仿射变换的乘积和逆变换仍是仿射变换*
    3.仿射变换能够实现平移、旋转、缩放等几何变换。
    举例
    旋转:
    在这里插入图片描述
    平移:
    在这里插入图片描述
    仿射变换一般表达式:
    在这里插入图片描述
    透视变换
    透视变换(Perspective Transformation)是指利用透视中心、像点、目标点三点共线的条件,按透视旋转定律使承影面(透视面)绕迹线(透视轴)旋转某一角度,破坏原有的投影光线束,仍能保持承影面上投影几何图形不变的变换。
    简单来说透视变换就是把三维图像变为二位表示的过程,其表达式为
    在这里插入图片描述
    1.透视变换也是一种平面映射 ,并且可以保证任意方向上的直线经过透视变换后仍然保持是直线。
    2.透视变换具有9个自由度(其变换系数为9个),故可以实现平面四边形到四边形的映射。
    举例
    在这里插入图片描述
    对于透视投影,一束平行于投影面的平行线的投影可保持平行,而不平行于投影面的平行线的投影会聚集到一个点,该点称为灭点

    2. 结合图简述图像坐标系、相机坐标系和世界坐标系的定义,并说明三者之间的变换关系。

    接下来了解相机标定中要用到的各种坐标系:它们分别为世界坐标系,相机坐标系,图像坐标系,像素坐标系。
    世界坐标系(world coordinate system)
    用户定义的三维世界的坐标系,未来描述目标物在真实世界里的位置而被引入。单位为m。
    相机坐标系(camera coordinate system)
    在相机上建立的坐标系,为了从相机的角度描述物体位置而定义,作为沟通世界坐标系的图像/像素坐标系的中间一环。单位为m。
    图像坐标系(image coordinate system)
    为了描述成像过程中物体从相机坐标系到图像坐标系的投影投射关系而引入,方便进一步得到像素坐标系下的坐标。单位为m。
    像素坐标系(pixel coordinate system)
    为了描述物体成像后像点在数字图像上(相片)的坐标而引入,是我们真正从相机读取信息所在的坐标系。单位为m。
    在这里插入图片描述
    上面这张图上可能更易于理解他们之间的关系
    世界坐标系:Xw、Yw、Zw。相机坐标系: Xc、Yc、Zc。图像坐标系:x、y。像素坐标系:u、v。
    其中,相机坐标系的轴与光轴重合,且垂直于图像坐标系平面并通过图像坐标系的原点,相机坐标系与图像坐标系之间的距离为焦距f(也即图像坐标系原点与焦点重合)。像素坐标系平面u-v和图像坐标系平面x-y重合,但像素坐标系原点位于图中左上角(之所以这么定义,目的是从存储信息的首地址开始读写)。
    变换关系
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    3. 什么是相机的内、外参数矩阵?实际中你能结合身边的实例(如电脑摄像头、手机镜头)说明这些参数的大致值么?

    相机的内参
    相机的内参是:1/dx、1/dy、r、u0、v0、f
    opencv中的内参是4个,分别为fx、fy、u0、v0。其实opencv中的fx也就是FSx,其中F是焦距上面的f,Sx是像素/每毫米也就是上面的1/dx。
    dx和dy表示x方向和y方向的一个像素分别占多少个单位,是反映现实中的图像物理坐标关系与像素坐标系转换的关键(我理解的是可以反映像元密度)。
    u0,v0代表图像的中心像素坐标和图像原点像素坐标之间相差的横向和纵向像素数
    相机的外参
    相机的外参是6个,三个轴的旋转参数分别是(w、δ、θ),把每个轴的33旋转矩阵进行组合(矩阵之间相乘),得到集合三个轴旋转信息的R,其大小还是33;T的三个轴的平移参数(Tx、Ty、Tz)。R、T组合成3
    4的矩阵,其是转换到标定纸坐标的关键。
    例子
    佳能 70D相机:
    参数:
    采用焦距f​:50 mm
    分辨率采用:1920x1080
    传感器尺寸:22.5x15 mm
    计算内参数:
    u0=1920/2=960​、v0=1080/2=540​、dx=22.5/1920=0.0117​、dy=15/1080=0.0139
    ax=fdx=4,273、ay=fdy=3,597

    4. 说明使用线性法求解相对位姿时的要求输入和输出,以及求解的基本思想。

    (1)基于空间多点的相对位姿估计测量算法
    相对位姿估计的基本问题
    已知:相机内参数;多个空间上的特征点(非共面)在目标坐标系(3D)和相平面坐标系(2D)坐标
    输出:目标坐标系相对相机坐标系的位置和姿态
    求解基本思路:
    已知相机的内参数,写出内参数矩阵,对于单个特征点来说通过线性求解计算相机的外参数矩阵的平移向量和旋转向量。拓展到多个特征点,不在同一平面上的点就要计算出关于平移向量和旋转向量的向量矩阵,利用矩阵的QR分解,得到最终的旋转矩阵非奇异矩阵P的正交三角分解:P=QR,其中Q(维数n*s):正交阵;R:上三角阵,得到最终的旋转矩阵和平移矩阵,最后通过旋转矩阵计算旋转角,使得相机坐标系和世界坐标系完全平行。
    在实现过程中,一般使用solvePnP方法进行计算,传入参数:目标坐标系的3D点,图像平面点坐标,相机内参数,畸变系数,最后就可以得到旋转向量和平移向量。
    (2)基于平面特征点的位姿测量
    已知:相机内参数;多个平面上的特征点在目标坐标系(3D)和相平面坐标系(2D)坐标;
    输出:目标坐标系相对相机坐标系的位置和姿态
    基本思想:
    基本思想与空间多点的位姿测量相似;要注意的是,在已知至少四个平面点三维点坐标的条件下,可通过点的图像坐标及相对位姿估计算法计算相对位姿

    5. 说明使用Zhang方法进行相机标定需要的输入条件和得到的具体输出量,以及Zhang方法的主要步骤

    输入条件
    使用张氏标定法,拿标定棋盘格来举例,需要输入30张不同角度拍摄的棋盘格照片,然后进行棋盘格标定
    具体输出量
    相机标定相机的没参数,输出相机的内外参数矩阵
    zhang方法主要步骤
    (1) 对一个pose,计算单应矩阵(类似M矩阵)
    求解单应矩阵—基本方程
    在这里插入图片描述
    求解单应矩阵—建立内参数方程
    在这里插入图片描述
    (2) 有三个以上Pose,根据各单应矩阵计算线性相机参数;
    求解内参数—建立方程
    在这里插入图片描述
    求解内参数—建立内参数方程
    在这里插入图片描述
    求解内参数—内参数求解
    在这里插入图片描述
    求解内参数—外参数求解
    在这里插入图片描述
    (3) 使用非线性优化方法计算非线性参数
    非线性畸变参数求解
    在这里插入图片描述

    展开全文
  • 功能:实时检测棋盘格相对于摄像头的距离以及位姿。为此主要步骤可分为以下三个步骤:标定图片的拍摄、相机的标定、以及棋盘格位姿的实时解算。

    写在前面:

        这次要实现的功能:实时检测棋盘格相对于摄像头的距离以及位姿。为此主要步骤可分为以下三个步骤:标定图片的拍摄、相机的标定、以及棋盘格位姿的实时解算。


    目录

    1. 标定图片的拍摄

    2. 相机的标定

    3. 棋盘格位姿的实时解算

    4. 需要注意的点

    5. 运行效率问题


    1. 标定图片的拍摄

    棋盘格图片

            打印上面的图片,尽量铺满一张A4纸,边缘留出一定的空白以方便握持。这里使用的是10×7的棋盘格,内部有9×6个角点。

            然后使用摄像头来随意拍摄棋盘格的15-20张照片。这里笔者踩了一个小坑,笔者一开始用的是电脑的相机应用拍摄了20张照片,分辨率为1280×720。进行后面的步骤都没什么问题,但是测得的结果怎么都不准。这是因为这颗摄像头录像和拍照的分辨率不一致,因此编写一个小程序来获得标定所用的照片。

    import cv2
    camera = cv2.VideoCapture(0)
    i = 1
    while i < 50:
        _, frame = camera.read()
        cv2.imwrite("E:/images/"+str(i)+'.png', frame, [int(cv2.IMWRITE_PNG_COMPRESSION), 0]) 
        cv2.imshow('frame', frame)
        i += 1
        if cv2.waitKey(200) & 0xFF == 27: # 按ESC键退出
            break
    cv2.destroyAllWindows()

            保存的图片经过手动筛选,挑选出清晰度和完整度较好的20张,如下所示:

    利用摄像头拍摄的20张棋盘格图片

    2. 相机的标定

            相机标定的原理此处略去,网上可以找到大量的相关资料。

            有一点需要注意,标定中将世界坐标系的建在标定板上,所有的z坐标均为0。但是x和y坐标需要经过测量得出。笔者所打印的棋盘格一格的边长为2.6厘米,因此每一个角点在世界坐标系中的坐标都需要以2.6厘米为倍数。

            相机标定部分的主要代码如下:

    objp = np.zeros((6 * 9, 3), np.float32)
    objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)  # 将世界坐标系建在标定板上,所有点的Z坐标全部为0,所以只需要赋值x和y
    objp = 2.6 * objp   # 打印棋盘格一格的边长为2.6cm
    obj_points = []     # 存储3D点
    img_points = []     # 存储2D点
    images=glob.glob("E:/image/*.png")  #黑白棋盘的图片路径
    
    for fname in images:
        img = cv2.imread(fname)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        size = gray.shape[::-1]
        ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
        if ret:
            obj_points.append(objp)
            corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001))  
            if [corners2]:
                img_points.append(corners2)
            else:
                img_points.append(corners)
            cv2.drawChessboardCorners(img, (9, 6), corners, ret)  # 记住,OpenCV的绘制函数一般无返回值
            cv2.waitKey(1)
    _, mtx, dist, _, _ = cv2.calibrateCamera(obj_points, img_points, size, None, None)
    
    # 内参数矩阵
    Camera_intrinsic = {"mtx": mtx,"dist": dist,}

            后面解算位姿所需要的参数为内参数矩阵mtx和畸变系数dist。据此求得的内参数矩阵和畸变系数如下:

            内参矩阵的具体表达式如下:

    M=\left[\begin{matrix}\frac{1}{\text{d}x}&0&{​{u}_{0}}\\0&\frac{1}{\text{d}y}&{​{v}_{0}}\\0&0&1\\\end{matrix}\right]\left[\begin{matrix}f&0&0\\0&f&0\\0&0&1\\\end{matrix}\right]=\left[\begin{matrix}{​{f}_{x}}&0&{​{u}_{0}}\\0&{​{f}_{y}}&{​{v}_{0}}\\0&0&1 \\\end{matrix}\right]

            其中,\text{d}x\text{d}y分别是每个像素在图像平面xy方向上的物理尺寸,({​{u}_{0}},{​{v}_{0}})是图像坐标系原点在像素坐标系中的坐标,f为摄像头的焦距,{​{f}_{x}}{​{f}_{y}}为焦距f与像素物理尺寸的比值,单位为个(像素数目)。 

            据此可以得到,这台摄像头的{​{f}_{x}}\approx {​{f}_{y}}\approx 450,说明焦距f约等于450个像素的物理尺寸。{​{u}_{0}}\approx 376{​{v}_{0}}\approx 234。这台摄像头的像素为640×480,因此{​{u}_{0}}的理论值应为320,v_0的理论值应为240。误差主要是因为摄像头的分辨率太低,实际角点在像素坐标系中显示不准;此外,目标坐标系的测量时也会带来误差。

    3. 棋盘格位姿的实时解算

            利用solvePnP函数可以实时解算出每一帧的旋转矢量rvec和平移矢量tvec。旋转矢量虽然简洁,但是作为结果显示不够直观,故需要将其转换为欧拉角。

            在欧拉角中,俯仰角(pitch)代表绕x轴旋转的角度, 偏航角(yaw)代表绕y轴旋转的角度,滚转角(roll)代表绕z轴旋转的角度。其中,默认逆时针选择为正,顺时针旋转为负。

            该部分的主要代码如下:

    obj_points = objp   # 存储3D点
    img_points = []     # 存储2D点
    
    #从摄像头获取视频图像
    camera = cv2.VideoCapture(0)
    
    while True:
        _, frame = camera.read()
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        size = gray.shape[::-1]
        ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
        if ret:    # 画面中有棋盘格
            img_points = np.array(corners)
            cv2.drawChessboardCorners(frame, (9, 6), corners, ret)
            # rvec: 旋转向量 tvec: 平移向量
            _, rvec, tvec = cv2.solvePnP(obj_points, img_points, Camera_intrinsic["mtx"], Camera_intrinsic["dist"])    # 解算位姿
            distance = math.sqrt(tvec[0]**2+tvec[1]**2+tvec[2]**2)  # 计算距离
            rvec_matrix = cv2.Rodrigues(rvec)[0]    # 旋转向量->旋转矩阵
            proj_matrix = np.hstack((rvec_matrix, tvec))    # hstack: 水平合并
            eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6]  # 欧拉角
            pitch, yaw, roll = eulerAngles[0], eulerAngles[1], eulerAngles[2]
            cv2.putText(frame, "dist: %.2fcm, yaw: %.2f, pitch: %.2f, roll: %.2f" % (distance, yaw, pitch, roll), (10, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
            cv2.imshow('frame', frame)
            if cv2.waitKey(1) & 0xFF == 27: # 按ESC键退出
                break
        else:   # 画面中没有棋盘格
            cv2.putText(frame, "Unable to Detect Chessboard", (20, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 1.3, (0, 0, 255), 3) 
            cv2.imshow('frame', frame)
            if cv2.waitKey(1) & 0xFF == 27: # 按ESC键退出
                break
    cv2.destroyAllWindows()

            当画面中检测不到棋盘格,或距离过远无法检测棋盘格的角点时,显示结果如下:

            当画面中能正常检测棋盘格的角点时,显示结果如下:

            左上角第一个红点为标定时所确定的世界坐标系的原点,沿棋盘格向右为x轴正方向,沿棋盘格向下为y轴正方向。 

            此时该棋盘格的坐标原点与摄像头的距离为43.18cm,偏航角为-1.3°,俯仰角为-26.48°,滚转角为3.92°。经过验证,该结果与实际的误差在1%以内,证明了结果的正确性。

    4. 需要注意的点

            相对位姿估计的基本问题

    • 输入:相机内参数;多个空间上的特征点在目标坐标系(3D)和相平面坐标系(2D)坐标
    • 输出:目标坐标系相对相机坐标系的位置和姿态

            也就是说,solvePnP函数求解的是目标坐标系相对相机坐标系的位置和姿态。为了提高结果的可读性,最好将初始位置的目标坐标系与相机坐标系的方向同一化。

            相机坐标系的x轴和y轴对应着相平面坐标系的u轴和v轴,因此在实际操作中,确定目标坐标系时按照像素坐标系的方向来确定即可。具体如下:

            此处目标坐标系的坐标原点确定在第一个角点处,目的是为了在编程中简化目标坐标系的设置。实际上将目标坐标系的坐标原点确定在棋盘格的中心更为合理。

            为了验证输出结果为目标坐标系相对于相机坐标系的位姿(顺序很重要),将棋盘格绕x轴逆时针旋转45°,输出俯仰角pitch也约为45°;将棋盘格绕y轴逆时针旋转45°,输出偏航角yaw也约为45°,将棋盘格绕z轴逆时针旋转45°,输出滚转角roll也约为45°。由此证明了输出结果为目标坐标系相对于相机坐标系的位姿。(如果结果符号不对,说明在编程中目标坐标系的设置有误)

    5. 运行效率问题

            在程序中加入时钟检测代码,可以得到每运行一帧所需要的计算时间。

    逐帧运行时间

            该程序运行于i7-8550U低压CPU平台,性能没有很高。当正常解算位姿时,一帧的平均运行时间在0.01秒以内,可以支持60Hz或90Hz的摄像头运行。但是当画面中没有棋盘格时,一帧的平均运行时间更长,原因是findChessboardCorners在寻找不到角点的情况下运算量更大。但是我们关注的只是正常解算位姿时的运行效率,无法检测棋盘格时的运行效率可以不予考虑。

    展开全文
  • 视觉SLAM十四讲学习5 位姿估计(6)ICP前言SVD方法BA方法 前言 本篇继续学习3D-3D的方法,ICP SVD方法 基于RGBD相机或者Lidar的SLAM前端常用ICP。 已知匹配点在上一帧的世界坐标P′P'P′,以及在当前帧的相机坐标PPP...
  • 相机位姿估计1:根据四个特征点估计相机姿态 随文Demo 使用opencv基于特征点估计位姿
  • 位姿估计_1

    万次阅读 多人点赞 2017-12-19 17:11:43
    姿态估计(Pose estimation)在计算机视觉领域扮演着十分重要的角色。这里主要是求解相机和目标物体的相对位姿
  • 基于深度学习的位姿估计方法

    千次阅读 2020-11-19 20:03:58
    A Multi-task Learning ...1.端到端网络结构:目标检测与分割网络+位姿估计网络 1)目标检测与分割网络:采用Mask R-CNN网络 2)位姿估计网络:先使用VGG16进行特征提取,再与Mask R-CNN网络输出的语义分割掩
  • 基于视觉信息的空间目标相对位姿估计问题是未来空间操作的关键所在,正交迭代算法是一种具有实时性好且全局收敛特点的单目位姿估计算法。为了有效利用多个摄像机获取的数据,进一步提高位姿估计算法的综合性能,提出...
  • 1. 相对位姿误差(RPE) 用于计算两个相同时间戳上相机位姿的真实值与SLAM系统的估计值之间每隔一段时间位姿变化量之间的差值,也可以理解为位姿真实值与与估计值的实时比较。该标准适合于估计系统的漂移。 2. 绝对...
  • 视觉SLAM十四讲学习5 位姿估计(2)本质矩阵的计算前言本质矩阵的性质本质矩阵计算五点法八点法位姿计算对极几何计算本质矩阵的问题后记 前言 本篇详细推导对极几何中,本质矩阵的计算。 本质矩阵的性质 根据上篇的...
  • 相机位姿估计

    2021-04-05 13:31:42
    相机位姿估计 今天,在查资料时发现一个博主的位姿估计分栏,里面的内容我觉得很实用,适合初学者,推荐给大家,也给自己留个记录,方便查阅。 1、相机位姿估计0:基本原理之如何解PNP问题 2、相机位姿估计1:根据四...
  • 鉴于此,结合惯性测量单元的测量数据和平面场景存在的单应性约束,提出了基于点线特征结合的单目相对位姿估计算法。先利用惯性测量单元提供的旋转角信息将2帧连续图像校正为正下视图像;再对图像中的点线特征进行检测和...
  • 本章介绍计算机视觉中位姿估计相关的知识。
  • 关键词:位姿估计 OpenCV::solvePnP 用途:各种位姿估计 文章类型:原理、流程、Demo示例 @Author:VShawn(singlex@foxmail.com) @Date:2016-11-18 @Lab: CvLab202@C
  • SLAM中位姿估计的图优化方法比较

    千次阅读 2021-11-15 01:23:44
    这些相对位姿测量通常是使用自运动估计、扫描匹配、迭代最近点 (ICP) 或某种形式的最小化视觉重投影误差从 IMU、激光传感器、相机或车轮里程计获得的。值得注意的是,[7] 和 [8] 中的方法提出了基于滤波器的位姿 ...
  • 刚体6D位姿估计方法综述

    万次阅读 多人点赞 2019-07-31 20:40:49
    方法分类 (Correspondences-based method)(Template-based method)...姿态估计方法 已知信息 核心思路 代表方法 适用场景 基于2D-3D对应的方法 3D模型3D点与以及投影后的带纹理图像 将3D模型投影得到RG...
  • 提出了一种广义正交迭代算法把所有摄像机获取的全部图像作为整体计算得到相对位姿参数, 是通用的多目视觉位姿估计算法。算法先把所有摄像机数据进行统一表达, 再把所有摄像机观测到的全部特征点的目标空间共线性误差...
  • [149]-[152]拓扑度量地图中有的度量信息仅仅包含每个位置节点的相对位姿。[1][2][26][140][153]–[156]则在每个节点内存储了路标或物体的姿态,[2][7][76]中这些内部度量信息存为稀疏路标地图,[134]中存为密集网格...
  • 这篇博客主要是对基于特征点匹配的slam中的位姿估计方法进行总结。 2d-2d的位姿估计 一般用在单目slam的初始阶段,系统还没有地图,此时只有输入的2D的图片序列。一般会使用如下方法进行位姿估计。而RGBD与双目...
  • 绝对轨迹误差直接计算相机位姿的...相对位姿误差用于计算相同两个时间戳上的位姿变化量的差, 同样, 在用时间戳对齐之后, 真实位姿和估计位姿均每隔一段相同时间计算位姿的变化量, 然后对该变化量做差, 以获得相...
  • 《十四讲》基础篇还剩下非线性优化,不过我在数值计算中已经把牛顿法、LM法这些记录过了,因此先跳过非线性优化,从位姿估计开始。 ORB特征 通过图像还原相机相邻位姿之间的变化,首先需要确定相邻图像上哪一对点是...
  • 视觉SLAM位姿估计(总结)

    千次阅读 2019-04-29 23:59:57
    计算机视觉对极几何之FEH 同时,还要根据匹配的特征点和计算出的相对位姿进行三角化,恢复出 3D空间点。 计算机视觉对极几何之Triangulate(三角化) 在单目视觉SLAM中,以上过程主要用于SLAM的初始化:计算第二关键...
  • 介绍了基于单目摄像机采集到的多帧图像,实现相机的位姿估计。介绍了対级几何,三角测量,八点法,P3P,ICPSVD分解。对于上述方法的理论推导完善且详尽,对于求解问题用到的算法进行了简明的整理。是学习SLAM前端...
  • Kinect 标定, 位姿估计

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 3,842
精华内容 1,536
关键字:

相对位姿估计