CN112381884B - 基于rgbd相机的空间圆目标位姿测量方法 - Google Patents

基于rgbd相机的空间圆目标位姿测量方法 Download PDF

Info

Publication number
CN112381884B
CN112381884B CN202011262357.9A CN202011262357A CN112381884B CN 112381884 B CN112381884 B CN 112381884B CN 202011262357 A CN202011262357 A CN 202011262357A CN 112381884 B CN112381884 B CN 112381884B
Authority
CN
China
Prior art keywords
rgb
coordinate system
matrix
camera
plane
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011262357.9A
Other languages
English (en)
Other versions
CN112381884A (zh
Inventor
孟偲
李兆玺
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202011262357.9A priority Critical patent/CN112381884B/zh
Publication of CN112381884A publication Critical patent/CN112381884A/zh
Application granted granted Critical
Publication of CN112381884B publication Critical patent/CN112381884B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/02—Picture taking arrangements specially adapted for photogrammetry or photographic surveying, e.g. controlling overlapping of pictures
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/04—Interpretation of pictures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G06F18/232—Non-hierarchical techniques
    • G06F18/2321—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions
    • G06F18/23213—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions with fixed number of clusters, e.g. K-means clustering
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Multimedia (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Biology (AREA)
  • Probability & Statistics with Applications (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了一种基于RGBD相机的空间圆目标位姿测量方法，首先从RGBD深度相机中获取RGB图像、深度数据、内参和外参数据；然后检测目标空间圆在RGB相机拍摄的RGB图像中投影形成的目标椭圆，并得到对应候选区域的像素点坐标集合；然后根据内参数据解算出两组平面法向向量解；再利用内参数据、外参数据和像素点坐标集合获得对应的空间三维点云数据；之后根据三维点云数据和两组平面法向向量解算出精确的空间圆的真实法向向量；最后利用真实法向向量结合椭圆的拟合矩阵对空间圆位置进行精确定位。

Description

基于RGBD相机的空间圆目标位姿测量方法
技术领域
本发明涉及计算机视觉技术领域，更具体的说是涉及一种基于RGBD相机的空间圆目标位姿测量方法。
背景技术
空间圆的位姿测量一直是计算机视觉中的关键研究问题，主要任务是解算出目标圆在相机坐标系下的空间位置和法向。位姿估计可以广泛应用于空间卫星的对接环对接，发动机尾部跟踪，无人机导航，空地协同等多种前瞻性应用。空间圆的位姿测量主要分为单目测量和多目测量两种情况。其中，单目测量往往要求目标提供少许合作信息，比如目标实际半径或直线、圆的位置等信息。此外由于单目位姿精度直接与目标椭圆精度相关，因此会导致位姿跟踪过程中存在位姿不稳定的问题。而基于多目的测量则可以缓解上述问题，并且不需要目标位置尺度等合作信息。
多目相机分为多目和结构光相机，统称为RGBD深度相机。多数基于多目相机的圆位姿估计方法主要是在每个相机上检测目标圆，之后利用双目之间的变换矩阵去除圆位姿二义性并确定中心点。但这种方法要求两边均检测出椭圆且形状相似。因此这类方法尽管精度较高，但是很容易丢失目标。
因此，如何提高空间圆位姿测量的准确性和稳定性是本领域技术人员亟需解决的问题。
发明内容
有鉴于此，本发明提供了一种基于RGBD相机的空间圆目标位姿测量方法，首先从RGBD深度相机中采集RGB图像、深度数据、内参和外参数据，内参数据包括红外IR相机下的深度数据DIR和相机标定后得到的内参矩阵KRGB、KIR，外参数据包括红外IR相机到RGB相机的旋转矩阵
Figure BDA0002775050950000011
与平移向量
Figure BDA0002775050950000021
然后检测目标空间圆在RGB相机拍摄的RGB图像中投影形成的目标椭圆，并得到对应候选区域的像素点坐标集合；然后根据内参数据解算出两组平面法向向量虚解；再利用内参数据、外参数据和像素点坐标集合获得对应的空间三维点云数据；之后根据三维点云数据和两组平面法向向量解算出精确的空间圆的最终法向向量；最后利用最终法向向量结合椭圆的拟合矩阵对空间圆位置进行精确定位。
为了实现上述目的，本发明采用如下技术方案：
一种基于RGBD相机的空间圆目标位姿测量方法，包括以下步骤：
步骤1：从RGBD深度相机采集数据，所述数据包括RGB相机下的RGB图像IRGB、左红外IR相机下的深度数据DIR和相机标定后得到的内参矩阵KRGB、KIR，以及所述左红外IR相机到所述RGB相机的旋转矩阵
Figure BDA0002775050950000022
与平移向量
Figure BDA0002775050950000023
定义RGB图像坐标系为{I}RGB，RGB相机坐标系为{C}RGB，定义左IR图像坐标系为{I}IR，左IR相机坐标系为{C}IR；定义坐标系{I}RGB下的坐标为(uRGB,vRGB)，坐标系{C}RGB下的坐标为(xRGB,yRGB,zRGB)，坐标系{I}IR下的坐标为(uIR,vIR)，坐标系{C}IR下的坐标为(xIR,yIR,zIR)；
步骤2：空间圆在RGB图像坐标系{I}RGB下的投影为目标椭圆
Figure BDA0002775050950000024
在所述RGB图像IRGB上检测所述目标椭圆
Figure BDA0002775050950000025
并得到所述目标椭圆
Figure BDA0002775050950000026
在所述RGB图像坐标系{I}RGB下对应的拟合矩阵
Figure BDA0002775050950000027
和候选区域的像素点坐标集合
Figure BDA0002775050950000028
步骤3：利用所述内参矩阵KRGB和所述目标椭圆
Figure BDA00027750509500000217
解算出所述空间圆所在支撑平面在RGB相机坐标系{C}RGB下的两组平面法向向量
Figure BDA0002775050950000029
其中一组解为真实解，一组为虚假解；
步骤4：利用所述内参矩阵KRGB和KIR、所述旋转矩阵
Figure BDA00027750509500000210
所述平移向量
Figure BDA00027750509500000211
将所述深度数据DIR对齐到所述RGB图像IRGB上，从所述步骤2中得到的候所述像素点坐标集合
Figure BDA00027750509500000212
中，提取出对应的在左IR图像坐标系{I}IR下空间点集
Figure BDA00027750509500000213
步骤5：利用所述空间点集
Figure BDA00027750509500000214
和两组解算出的所述平面法向向量
Figure BDA00027750509500000215
提取所述目标椭圆
Figure BDA00027750509500000216
所在平面，排除虚假解并确定精确的所述空间圆的真实法向向量nC；
步骤6：利用所述真实法向向量nC和所述步骤2中的所述拟合矩阵
Figure BDA0002775050950000031
对所述空间圆的位置进行精确解算。
优选的，所述步骤2的具体实现过程为：
步骤21：所述空间圆在所述RGB图像坐标系{I}RGB下的投影为椭圆，利用椭圆检测算法得到所述目标椭圆
Figure BDA0002775050950000032
其中心点为(x0,y0)，半长短轴为(a,b),a＞b，旋转角为θ；
步骤22：设置所述候选区域为圆环，其宽度为Tring，则所述目标椭圆
Figure BDA0002775050950000033
的最大外接圆的中心点为(x0,y0)，半径为
Figure BDA0002775050950000034
最小内接圆中心点为(x0,y0)，半径为
Figure BDA0002775050950000035
步骤23：查找所有满足
Figure BDA0002775050950000036
的像素点，其中所述RGB图像坐标系{I}RGB下的坐标为(uRGB,vRGB)，并构成所述像素点坐标集合
Figure BDA0002775050950000037
其中ci表示在所述RGB图像坐标系{I}RGB下所述候选区域内的坐标。
优选的，所述步骤3的具体实现过程为：
步骤31：根据所述目标椭圆
Figure BDA00027750509500000311
的中心点(x0,y0)、半长短轴(a,b)和旋转角θ，得到椭圆二次型方程为
Figure BDA0002775050950000038
其中所述RGB相机坐标系{C}RGB下的坐标为(xRGB,yRGB,zRGB)，其中ai的计算方式为：
Figure BDA0002775050950000039
步骤32：以所述RGB相机坐标系{C}RGB的原点为顶点，根据所述目标椭圆
Figure BDA00027750509500000310
确定空间椭圆锥在所述RGB相机坐标系{C}RGB的椭圆锥方程，利用所述椭圆二次型方程和所述内参矩阵KRGB确定所述空间椭圆锥，所述椭圆锥方程表示为
Figure BDA0002775050950000041
其中p＝(x,y,z)T；
步骤33：所述椭圆锥方程的二次型矩阵表示为
Figure BDA0002775050950000042
Figure BDA0002775050950000043
计算矩阵QΓ的特征值λ1，λ2，λ3和特征向量e1,e2,e3，满足|λ1|≥|λ2|,e3|Z＞0，e3*(0,0,1)＞0，则两组所述平面法向向量为
Figure BDA0002775050950000044
其中P＝[e1,e2,e3]。
优选的，所述步骤4的具体实现过程为：
步骤41：在所述左IR图像坐标系{I}IR下，利用所述深度数据DIR得到所述左IR图像坐标系{I}IR下的坐标(uIR,vIR)对应的深度值zc＝d，利用所述内参矩阵KIR得到左IR相机坐标系{C}IR下的坐标(xIR,yIR,zIR)和所述坐标(uIR,vIR)之间的变换关系：
Figure BDA0002775050950000045
步骤42：利用所述旋转矩阵
Figure BDA0002775050950000046
和所述平移向量
Figure BDA0002775050950000047
将所述坐标(xIR,yIR,zIR)变换到所述RGB相机坐标系{C}RGB下，得到新坐标(xRGB,yRGB,zRGB)：
Figure BDA0002775050950000048
步骤43：利用所述内参矩阵KRGB将所述新坐标(xRGB,yRGB,zRGB)投影到所述RGB图像坐标系{I}RGB下，得到像素点坐标
Figure BDA0002775050950000049
步骤44：根据步骤41-步骤43获得所述RGB图像坐标系{I}RGB下像素点坐标(uRGB,vRGB)和所述左IR图像坐标系{I}IR下坐标(uIR,vIR)之间的变换关系，结合所述候选区域的所述像素点坐标集合
Figure BDA0002775050950000051
获得对应的在所述左IR图像坐标系{I}IR下的空间点集
Figure BDA0002775050950000052
优选的，所述步骤5的具体实现过程为：
步骤51：利用中值滤波算法对所述空间点集
Figure BDA0002775050950000053
进行滤波，去除噪声深度，得到修正后的点集
Figure BDA0002775050950000054
点集
Figure BDA0002775050950000055
个数也为K2个；
将所述空间点集
Figure BDA0002775050950000056
投影到所述左IR图像坐标系{I}IR的图像平面形成投影图像，然后对所述投影图像进行中值滤波，这样改变的是每个点的深度，去除噪声深度，直接就可以得到修正后的点集；
步骤52：对滤波后的所述点集
Figure BDA0002775050950000057
使用K-means算法进行聚类，得到K组深度点集合{pi,i＝1,2,...,K}，其中K值较大，以保证聚类片较小以去除错误平面片；对每组深度点集合pi利用最小距离方法拟合平面，采用最小化误差函数
Figure BDA0002775050950000058
约束
Figure BDA0002775050950000059
得到每个深度点对应的平面参数ai及单位法向ni；
步骤53：对K组所述深度点集pi构建邻接矩阵M，所述邻接矩阵M为对称矩阵，对于点集pi和点集pj，i表示矩阵行数，j表示矩阵列数，当满足所述点集pi和所述点集pj之间最短距离接近于0，且所述单位法向ni≈nj，则所述邻接矩阵M中表示所述点集pi和所述点集pj的邻接关系的Mi,j＝1，否则Mi,j＝-1；
步骤54：设定一个在IR相机坐标系下的椭圆法向量nIR，计算每个所述深度点集pi的法向量ni与所述椭圆法向量nIR的角度差异
Figure BDA00027750509500000510
若所述角度差异大于设定阈值，则对应的所述深度点集pi为无效点集，并将所述邻接矩阵M的第i行和第i列所有元素设置为-1；否则为有效点集；
步骤55：查找在所述目标椭圆
Figure BDA00027750509500000511
上的有效点集{pki,i＝1,2,...,m}，对每个有效点集
Figure BDA00027750509500000512
进行平面生长，生长过程中不断更新当前点集的平面法向，若更新前的点集法向和更新后的点集法向之间的所述角度差异小于所述阈值，则使用更新后的点集继续生长，否则使用更新前的点集继续搜索新的平面，直至无法组合新的平面，最终每个所述有效点集
Figure BDA0002775050950000061
对应的生长区域为
Figure BDA0002775050950000062
步骤56：构建所述有效点集
Figure BDA0002775050950000063
之间的邻接矩阵C，若点集
Figure BDA0002775050950000064
和
Figure BDA0002775050950000065
重合的平面点集超过
Figure BDA0002775050950000066
生长点集的个数的一半，则Ci,j＝1，否则Ci,j＝0；
步骤57：对每个所述点集
Figure BDA0002775050950000067
进行生长，统计最终生长点集
Figure BDA0002775050950000068
的个数Mi，并拟合出对应的平面法向
Figure BDA0002775050950000069
和平面方程的常数项Ti，计算最终法向得分
Figure BDA00027750509500000610
选择最高得分
Figure BDA00027750509500000611
的平面法向
Figure BDA00027750509500000612
作为输入椭圆法向量
Figure BDA00027750509500000613
的最终结果，并提取出对应平面方程的常数项
Figure BDA00027750509500000614
步骤58：根据所述步骤33计算出的所述法向向量
Figure BDA00027750509500000615
和所述步骤43给出的相机之间的转换关系得到所述法向向量
Figure BDA00027750509500000616
在所述IR相机下的椭圆法向量
Figure BDA00027750509500000617
其中
Figure BDA00027750509500000618
根据当前所述椭圆法向量
Figure BDA00027750509500000619
重复所述步骤54至所述步骤57，获得平面法向
Figure BDA00027750509500000620
及对应的最高得分
Figure BDA00027750509500000621
选择最大分数对应的所述平面法向
Figure BDA00027750509500000622
作为最终法向结果
Figure BDA00027750509500000623
获得精确的所述空间圆的所述真实法向向量
Figure BDA00027750509500000624
和所述空间圆所在所述平面方程的常数项
Figure BDA00027750509500000625
优选的，所述步骤6的具体实现过程为：
步骤61：令所述空间圆的所述中心点为世界坐标系原点，所述空间圆的法向为坐标系的Z轴的方向，则所述空间圆在所述RGB图像坐标系{I}RGB下的方程表达式如下：
Figure BDA00027750509500000626
其中，R为所述世界坐标系到所述RGB相机坐标系{C}RGB的旋转矩阵，(x0,y0,z0)为所述世界坐标系下相机中心点坐标；
根据步骤5算出的所述真实法向向量nC，构造一个所述旋转矩阵R，
Figure BDA0002775050950000071
其中l＝nC|x，m＝nC|y，n＝nC|z；
步骤62：结合所述内参矩阵KRGB和KIR，令A＝RK，则得到约束矩阵B如下：
Figure BDA0002775050950000072
步骤63：利用椭圆对应的所述拟合矩阵
Figure BDA0002775050950000073
构建新拟合矩阵
Figure BDA0002775050950000074
计算所述新拟合矩阵S*的最大特征值对应的特征向量，得到
Figure BDA0002775050950000075
对应的拟合值u1,u2,u3,u4，其中u1＞0；
步骤64：在所述步骤63的基础上，计算出空间圆中心点在所述RGB相机坐标系{C}RGB下过原点的空间直线，直线方向向量为
Figure BDA0002775050950000076
其中
Figure BDA0002775050950000077
步骤65：根据所述步骤57得到的所述空间圆所在的所述平面方程的常数项TC和所述步骤64计算出的所述直线方向向量lc，计算出对应的所述空间圆的圆心坐标
Figure BDA0002775050950000078
经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种基于RGBD相机的空间圆目标位姿测量方法，基于RGBD深度相机实现空间圆在线位姿测量，融合目标空间圆的深度信息，实现更高精度的测量。使用RGB彩色图像和深度图像检测空间圆的位姿，利用彩色图确定空间圆，利用深度数据对姿态进行进一步修正与优化，以提高位姿的准确性。此外，本发明仅利用空间圆算位姿，无需目标实际半径和直线等合作信息，因此降低了测量数据采集的要求，提高了位姿测算的稳定性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
图1附图为本发明提供的基于RGBD相机的空间圆目标位姿测量方法流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例公开了一种基于RGBD相机的空间圆目标位姿测量方法，包括以下步骤：
S1：从RGBD深度相机采集的数据包含以下几个内容，彩色相机下的RGB图像IRGB、左红外IR相机下的深度数据DIR和相机标定后得到的内参矩阵KRGB、KIR和左IR相机到RGB相机的旋转矩阵
Figure BDA0002775050950000081
与平移向量
Figure BDA0002775050950000082
S2：空间圆在RGB图像坐标系{I}RGB下的投影为目标椭圆
Figure BDA0002775050950000083
在RGB图像IRGB上检测目标椭圆
Figure BDA0002775050950000084
并得到目标椭圆
Figure BDA0002775050950000085
在RGB图像坐标系{I}RGB下对应的拟合矩阵
Figure BDA0002775050950000086
和候选区域的像素点坐标集合
Figure BDA0002775050950000087
S21：空间圆在RGB图像坐标系{I}RGB下的投影为椭圆，因此利用椭圆检测算法得到目标椭圆
Figure BDA0002775050950000088
其中心点为(x0,y0)，半长短轴为(a,b),a＞b，旋转角为θ；
S22：设置候选区域为圆环，其宽度为Tring，则目标椭圆
Figure BDA0002775050950000089
的最大外接圆的中心点为(x0,y0)，半径为
Figure BDA00027750509500000810
最小内接圆中心点为(x0,y0)，半径为
Figure BDA00027750509500000811
S23：查找所有满足
Figure BDA0002775050950000091
的像素点，其中RGB图像坐标系{I}RGB下的坐标为(uRGB,vRGB)，并构成像素点坐标集合
Figure BDA0002775050950000092
其中ci表示在RGB图像坐标系{I}RGB下候选区域内的坐标；因为拟合出的椭圆误差较大，此刻旋转角基本无用，只能确定最大的一个圆环区域，因此不需要考虑倾角；
S3：利用内参矩阵KRGB和目标椭圆
Figure BDA0002775050950000093
解算出空间圆所在支撑平面在RGB相机坐标系{C}RGB下的两组平面法向向量
Figure BDA0002775050950000094
其中一组解为真实解，一组为虚假解；
S31：根据目标椭圆
Figure BDA0002775050950000095
的中心点(x0,y0)、半长短轴(a,b)和旋转角θ，得到椭圆二次型方程为
Figure BDA0002775050950000096
其中RGB相机坐标系{C}RGB下的坐标为(xRGB,yRGB,zRGB)，其中ai的计算方式如下所示：
Figure BDA0002775050950000097
S32：以RGB相机坐标系{C}RGB的原点为顶点，根据目标椭圆
Figure BDA0002775050950000098
确定空间椭圆锥在RGB相机坐标系{C}RGB的椭圆锥方程，利用椭圆二次型方程和内参矩阵KRGB确定空间椭圆锥，椭圆锥方程表示为
Figure BDA0002775050950000099
其中p＝(x,y,z)T；
S33：椭圆锥方程的二次型矩阵表示为
Figure BDA00027750509500000910
计算矩阵QΓ的特征值λ1，λ2，λ3和特征向量e1,e2,e3，满足|λ1|≥|λ2|,e3|Z＞0，e3*(0,0,1)＞0，则两组平面法向向量为
Figure BDA00027750509500000911
其中P＝[e1,e2,e3]；每个向量对应的元素用l，m，n表示，i取值范围为1和2，1表示公式中正号的情况，2表示公式中负号的情况；
S4：利用相机内参KRGB和KIR、外参
Figure BDA0002775050950000101
将深度数据DIR对齐到RGB图像IRGB上，从S2中得到的候像素点坐标集合
Figure BDA0002775050950000102
中，提取出对应的在左IR图像坐标系{I}IR下空间点集
Figure BDA0002775050950000103
S41：在左IR图像坐标系{I}IR下，利用深度数据DIR得到左IR图像坐标系{I}IR下的坐标(uIR,vIR)对应的深度值zc＝d，利用内参矩阵KIR得到左IR相机坐标系{C}IR下的坐标(xIR,yIR,zIR)和坐标(uIR,vIR)之间的变换关系：
Figure BDA0002775050950000104
S42：利用旋转矩阵
Figure BDA0002775050950000105
和平移向量
Figure BDA0002775050950000106
将坐标(xIR,yIR,zIR)变换到RGB相机坐标系{C}RGB下，得到新坐标(xRGB,yRGB,zRGB)：
Figure BDA0002775050950000107
S43：利用内参矩阵KRGB将新坐标(xRGB,yRGB,zRGB)投影到RGB图像坐标系{I}RGB下，得到像素点坐标
Figure BDA0002775050950000108
S44：根据S41-S43获得RGB图像坐标系{I}RGB下像素点坐标(uRGB,vRGB)和左IR图像坐标系{I}IR下坐标(uIR,vIR)之间的变换关系，结合候选区域的像素点坐标集合
Figure BDA0002775050950000109
获得对应的在左IR图像坐标系{I}IR下的空间点集
Figure BDA00027750509500001010
S5：利用空间点集
Figure BDA00027750509500001011
和两组解算出的平面法向向量
Figure BDA00027750509500001012
提取目标椭圆
Figure BDA00027750509500001013
所在平面，排除虚假解并确定精确的空间圆的真实法向向量nC；
S51：利用中值滤波算法对空间点集
Figure BDA00027750509500001014
进行滤波，去除噪声深度，得到修正后的点集
Figure BDA00027750509500001015
点集
Figure BDA00027750509500001016
个数也为K2个；
将空间点集
Figure BDA0002775050950000111
投影到左IR图像坐标系{I}IR的图像平面形成投影图像，然后对投影图像进行中值滤波，这样改变的是每个点的深度，去除噪声深度，直接就可以得到修正后的点集；
S52：对滤波后的点集
Figure BDA0002775050950000112
使用K-means算法进行聚类，得到K组深度点集合{pi,i＝1,2,...,K}，其中K值较大，以保证聚类片较小以去除错误平面片。对每组深度点集合利用最小距离的方法拟合平面，即最小化误差函数
Figure BDA0002775050950000113
约束
Figure BDA0002775050950000114
得到每个深度点对应的平面参数ai及其单位法向ni；
S53：对K组深度点集pi构建邻接矩阵M，邻接矩阵M为对称矩阵，对于点集pi和pj，i表示矩阵行数，j表示矩阵列数，当其满足点集pi和pj之间最短距离接近于0，且单位法向ni≈nj，则邻接矩阵M中表示点集pi和点集pj的邻接关系的Mi,j＝1，否则Mi,j＝-1；
S54：设定一个在IR相机坐标系下的椭圆法向量nIR，计算每个深度点集pi的法向量ni与椭圆法向量nIR的角度差异
Figure BDA0002775050950000115
若角度差异过大，大于设定阈值
Figure BDA0002775050950000116
则认为对应的深度点集pi为无效点集，将邻接矩阵M的第i行和第i列所有元素设置为-1；否则为有效点集；
S55：查找在目标椭圆
Figure BDA0002775050950000117
上的有效点集
Figure BDA0002775050950000118
对每个有效点集
Figure BDA0002775050950000119
进行平面生长，生长过程中不断更新当前点集的平面法向，若若更新前的点集法向和更新后的点集法向之间的角度差异小于阈值，则使用更新后的点集继续生长，否则使用更新前的点集继续搜索新的平面，直至无法组合新的平面，最终每个有效点集
Figure BDA00027750509500001110
对应的生长区域为
Figure BDA00027750509500001111
S56：构建有效点集
Figure BDA00027750509500001112
之间的邻接矩阵C，若点集
Figure BDA00027750509500001113
和
Figure BDA00027750509500001114
重合的平面点集超过
Figure BDA00027750509500001115
生长点集的个数的一半，则Ci,j＝1，否则Ci,j＝0；
S57：对每个点集
Figure BDA00027750509500001116
进行生长，统计最终生长点集
Figure BDA00027750509500001117
的个数Mi，并拟合出对应的平面法向
Figure BDA00027750509500001118
和平面方程的常数项Ti，计算最终法向得分
Figure BDA0002775050950000121
选择最高得分
Figure BDA0002775050950000122
的平面法向
Figure BDA0002775050950000123
作为输入椭圆法向量
Figure BDA0002775050950000124
的最终结果，并提取出对应平面方程的常数项
Figure BDA0002775050950000125
S58：根据步骤33计算出的法向向量
Figure BDA0002775050950000126
和步骤43给出的相机之间的转换关系得到法向向量
Figure BDA0002775050950000127
在IR相机下的椭圆法向量
Figure BDA0002775050950000128
其中
Figure BDA0002775050950000129
根据当前椭圆法向量
Figure BDA00027750509500001210
重复步骤54至步骤57，获得平面法向
Figure BDA00027750509500001211
及对应的最高得分
Figure BDA00027750509500001212
选择最大分数对应的平面法向
Figure BDA00027750509500001213
作为最终法向结果
Figure BDA00027750509500001214
获得精确的空间圆的真实法向向量
Figure BDA00027750509500001215
和空间圆所在平面方程的常数项
Figure BDA00027750509500001216
S6：利用精炼后的真实法向向量nC和步骤2中的拟合矩阵
Figure BDA00027750509500001217
对空间圆的位置进行精确解算；
S61：令空间圆的中心点为世界坐标系原点，空间圆的法向为坐标系的Z轴的方向，则空间圆在RGB图像坐标系{I}RGB下的方程表达式如下：
Figure BDA00027750509500001218
其中，R为世界坐标系到RGB相机坐标系{C}RGB的旋转矩阵，(x0,y0,z0)为世界坐标系下相机中心点坐标；
根据S5算出的真实法向向量nC，构造一个旋转矩阵R，
Figure BDA00027750509500001219
其中l＝nC|x，m＝nC|y，n＝nC|z；
S62：结合内参矩阵KRGB和KIR，令A＝RK，则得到约束矩阵B如下：
Figure BDA00027750509500001220
S63：利用椭圆对应的拟合矩阵
Figure BDA0002775050950000131
构建新拟合矩阵
Figure BDA0002775050950000132
计算新拟合矩阵S*的最大特征值对应的特征向量，得到
Figure BDA0002775050950000133
对应的拟合值u1,u2,u3,u4，其中u1＞0；
S64：在S63的基础上，计算出空间圆中心点在RGB相机坐标系{C}RGB下过原点的空间直线，直线方向向量为
Figure BDA0002775050950000134
其中
Figure BDA0002775050950000135
S65：根据S57得到的空间圆所在的平面方程的常数项TC和S64计算出的直线方向向量lc，计算出对应的空间圆的圆心坐标
Figure BDA0002775050950000136
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (6)

1.一种基于RGBD相机的空间圆目标位姿测量方法，其特征在于，包括以下步骤：
步骤1：从RGBD深度相机采集数据，所述数据包括RGB相机下的RGB图像IRGB、左红外IR相机下的深度数据DIR、相机标定后得到的内参矩阵KRGB和KIR、所述左红外IR相机到所述RGB相机的旋转矩阵
Figure FDA0002775050940000011
与平移向量
Figure FDA0002775050940000012
步骤2：空间圆在RGB图像坐标系{I}RGB下的投影为目标椭圆
Figure FDA0002775050940000013
在所述RGB图像IRGB上检测所述目标椭圆
Figure FDA0002775050940000014
并得到所述目标椭圆
Figure FDA0002775050940000015
在所述RGB图像坐标系{I}RGB下对应的拟合矩阵
Figure FDA0002775050940000016
和候选区域的像素点坐标集合
Figure FDA0002775050940000017
步骤3：利用所述内参矩阵KRGB和所述目标椭圆
Figure FDA0002775050940000018
解算出所述空间圆所在支撑平面在RGB相机坐标系{C}RGB下的两组平面法向向量
Figure FDA0002775050940000019
步骤4：利用所述内参矩阵KRGB和KIR、所述旋转矩阵
Figure FDA00027750509400000110
所述平移向量
Figure FDA00027750509400000111
将所述深度数据DIR对齐到所述RGB图像IRGB上，从所述步骤2中得到的所述像素点坐标集合
Figure FDA00027750509400000112
中提取出对应的在左IR图像坐标系{I}IR下空间点集
Figure FDA00027750509400000113
步骤5：利用所述空间点集
Figure FDA00027750509400000114
和两组解算出的所述平面法向向量
Figure FDA00027750509400000115
提取所述目标椭圆
Figure FDA00027750509400000116
所在平面，确定所述空间圆的真实法向向量nC；
步骤6：利用所述真实法向向量nC和所述步骤2中的所述拟合矩阵
Figure FDA00027750509400000117
对所述空间圆的位置进行精确解算。
2.根据权利要求1所述的一种基于RGBD相机的空间圆目标位姿测量方法，其特征在于，所述步骤2的具体实现过程为：
步骤21：所述空间圆在所述RGB图像坐标系{I}RGB下的投影为椭圆，利用椭圆检测算法得到所述目标椭圆
Figure FDA00027750509400000118
其中心点为(x0,y0)，半长短轴为(a,b),a＞b，旋转角为θ；
步骤22：设置所述候选区域为圆环，宽度为Tring，则所述目标椭圆
Figure FDA0002775050940000021
的最大外接圆的中心点为(x0,y0)，半径为
Figure FDA0002775050940000022
最小内接圆中心点为(x0,y0)，半径为
Figure FDA0002775050940000023
步骤23：查找所有满足
Figure FDA0002775050940000024
的像素点，其中所述RGB图像坐标系{I}RGB下的坐标为(uRGB,vRGB)，并构成所述像素点坐标集合
Figure FDA0002775050940000025
其中ci表示在所述RGB图像坐标系{I}RGB下所述候选区域内的坐标。
3.根据权利要求2所述的一种基于RGBD相机的空间圆目标位姿测量方法，其特征在于，所述步骤3的具体实现过程为：
步骤31：根据所述目标椭圆
Figure FDA0002775050940000026
的中心点(x0,y0)、半长短轴(a,b)和旋转角θ，得到椭圆二次型方程为
Figure FDA0002775050940000027
其中所述RGB相机坐标系{C}RGB下的坐标为(xRGB,yRGB,zRGB)，ai的计算方式为：
Figure FDA0002775050940000028
步骤32：以所述RGB相机坐标系{C}RGB的原点为顶点，根据所述目标椭圆
Figure FDA0002775050940000029
确定空间椭圆锥在所述RGB相机坐标系{C}RGB的椭圆锥方程，利用所述椭圆二次型方程和所述内参矩阵KRGB确定所述空间椭圆锥，所述椭圆锥方程表示为
Figure FDA00027750509400000210
其中p＝(xRGB,yRGB,zRGB)；
步骤33：所述椭圆锥方程的二次型矩阵表示为
Figure FDA00027750509400000211
Figure FDA00027750509400000212
计算矩阵QΓ的特征值λ1，λ2，λ3和特征向量e1,e2,e3，满足|λ1|≥|λ2|,e3|Z＞0，e3*(0,0,1)＞0，则两组所述平面法向向量为
Figure FDA0002775050940000031
其中P＝[e1,e2,e3]。
4.根据权利要求3所述的一种基于RGBD相机的空间圆目标位姿测量方法，其特征在于，所述步骤4的具体实现过程为：
步骤41：在所述左IR图像坐标系{I}IR下，利用所述深度数据DIR得到所述左IR图像坐标系{I}IR下的坐标(uIR,vIR)对应的深度值zc＝d，利用所述内参矩阵KIR得到左IR相机坐标系{C}IR下的坐标(xIR,yIR,zIR)和所述坐标(uIR,vIR)之间的变换关系：
Figure FDA0002775050940000032
步骤42：利用所述旋转矩阵
Figure FDA0002775050940000033
和所述平移向量
Figure FDA0002775050940000034
将所述坐标(xIR,yIR,zIR)变换到所述RGB相机坐标系{C}RGB下，得到新坐标(xRGB,yRGB,zRGB)，变换关系如下：
Figure FDA0002775050940000035
步骤43：利用所述内参矩阵KRGB将所述新坐标(xRGB,yRGB,zRGB)投影到所述RGB图像坐标系{I}RGB下，得到像素点坐标
Figure FDA0002775050940000036
步骤44：根据步骤41-步骤43获得所述RGB图像坐标系{I}RGB下像素点坐标(uRGB,vRGB)和所述左IR图像坐标系{I}IR下坐标(uIR,vIR)之间的变换关系，结合所述候选区域的所述像素点坐标集合
Figure FDA0002775050940000037
获得对应的在所述左IR图像坐标系{I}IR下的空间点集
Figure FDA0002775050940000038
5.根据权利要求4所述的一种基于RGBD相机的空间圆目标位姿测量方法，其特征在于，所述步骤5的具体实现过程为：
步骤51：利用中值滤波算法对所述空间点集
Figure FDA0002775050940000039
进行滤波，去除噪声深度，得到修正后的点集
Figure FDA00027750509400000310
步骤52：对滤波后的所述点集
Figure FDA0002775050940000041
使用K-means算法进行聚类，得到K组深度点集合{pi,i＝1,2,...,K}；对每组深度点集合pi利用最小距离方法拟合平面，采用最小化误差函数
Figure FDA0002775050940000042
约束
Figure FDA0002775050940000043
得到每个深度点对应的平面参数ai及单位法向ni；
步骤53：对K组所述深度点集pi构建邻接矩阵M，所述邻接矩阵M为对称矩阵，对于点集pi和点集pj，i表示矩阵行数，j表示矩阵列数，当满足所述点集pi和所述点集pj之间最短距离接近于0，且所述单位法向ni≈nj，则所述邻接矩阵M中表示所述点集pi和所述点集pj的邻接关系的Mi,j＝1，否则Mi,j＝-1；
步骤54：设定一个在IR相机坐标系下的椭圆法向量nIR，计算每个所述深度点集pi的法向量
Figure FDA0002775050940000044
与椭圆法向量nIR的角度差异
Figure FDA0002775050940000045
若所述角度差异大于设定阈值，则对应的所述深度点集pi为无效点集，并将所述邻接矩阵M的第i行和第i列所有元素设置为-1；否则为有效点集；
步骤55：查找在所述目标椭圆
Figure FDA0002775050940000046
上的有效点集
Figure FDA0002775050940000047
对每个有效点集
Figure FDA0002775050940000048
进行平面生长，生长过程中不断更新当前点集的平面法向，若更新前的点集法向和更新后的点集法向之间的所述角度差异小于所述阈值，则使用更新后的点集继续生长，否则使用更新前的点集继续搜索新的平面，直至无法组合新的平面，最终每个所述有效点集
Figure FDA0002775050940000049
对应的生长区域为
Figure FDA00027750509400000410
步骤56：构建所述有效点集
Figure FDA00027750509400000411
之间的邻接矩阵C，若点集
Figure FDA00027750509400000412
和
Figure FDA00027750509400000413
重合的平面点集超过
Figure FDA00027750509400000414
生长点集的个数的一半，则Ci,j＝1，否则Ci,j＝0；
步骤57：对每个所述点集
Figure FDA00027750509400000415
进行生长，统计最终生长点集
Figure FDA00027750509400000416
的个数Mi，并拟合出对应的平面法向
Figure FDA00027750509400000417
和平面方程的常数项Ti，计算最终法向得分
Figure FDA00027750509400000418
选择最高得分
Figure FDA00027750509400000419
的平面法向
Figure FDA00027750509400000420
作为输入所述椭圆法向量
Figure FDA00027750509400000421
的最终结果，并提取出对应平面方程的常数项
Figure FDA00027750509400000422
步骤58：根据所述步骤33计算出的所述法向向量
Figure FDA0002775050940000051
和所述步骤43给出的相机之间的转换关系得到所述法向向量
Figure FDA0002775050940000052
在所述IR相机下的椭圆法向
Figure FDA0002775050940000053
其中
Figure FDA0002775050940000054
根据当前所述椭圆法向
Figure FDA0002775050940000055
重复所述步骤54至所述步骤57，获得平面法向
Figure FDA0002775050940000056
及对应的最高得分
Figure FDA0002775050940000057
选择最大分数对应的所述平面法向
Figure FDA0002775050940000058
作为最终法向结果
Figure FDA0002775050940000059
获得精确的所述空间圆的真实法向向量
Figure FDA00027750509400000510
和所述空间圆所在所述平面方程的常数项
Figure FDA00027750509400000511
6.根据权利要求5所述的一种基于RGBD相机的空间圆目标位姿测量方法，其特征在于，所述步骤6的具体实现过程为：
步骤61：令所述空间圆的所述中心点为世界坐标系原点，所述空间圆的法向为坐标系的Z轴的方向，则所述空间圆在所述RGB图像坐标系{I}RGB下的方程表达式如下：
Figure FDA00027750509400000512
其中，R为所述世界坐标系到所述RGB相机坐标系{C}RGB的旋转矩阵，(x0,y0,z0)为所述世界坐标系下相机中心点坐标；
根据算出的所述真实法向向量nC，构造一个所述旋转矩阵R，
Figure FDA00027750509400000513
其中l＝nC|x，m＝nC|y，n＝nC|z；
步骤62：结合所述内参矩阵KRGB和KIR，令A＝RK，则得到约束矩阵B如下：
Figure FDA00027750509400000514
步骤63：利用椭圆对应的所述拟合矩阵
Figure FDA0002775050940000061
构建新拟合矩阵
Figure FDA0002775050940000062
计算所述新拟合矩阵S*的最大特征值对应的特征向量，得到
Figure FDA0002775050940000063
-x0z0,-y0z0,
Figure FDA0002775050940000064
对应的拟合值u1,u2,u3,u4，其中u1＞0；
步骤64：根据所述拟合值计算出空间圆中心点在所述RGB相机坐标系{C}RGB下过原点的空间直线，直线方向向量为
Figure FDA0002775050940000065
其中
Figure FDA0002775050940000066
步骤65：根据所述步骤57得到的所述空间圆所在的所述平面方程的常数项TC和所述步骤64计算出的所述直线方向向量lc，计算出对应的所述空间圆的圆心坐标
Figure FDA0002775050940000067
CN202011262357.9A 2020-11-12 2020-11-12 基于rgbd相机的空间圆目标位姿测量方法 Active CN112381884B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011262357.9A CN112381884B (zh) 2020-11-12 2020-11-12 基于rgbd相机的空间圆目标位姿测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011262357.9A CN112381884B (zh) 2020-11-12 2020-11-12 基于rgbd相机的空间圆目标位姿测量方法

Publications (2)

Publication Number Publication Date
CN112381884A CN112381884A (zh) 2021-02-19
CN112381884B true CN112381884B (zh) 2022-04-19

Family

ID=74583381

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011262357.9A Active CN112381884B (zh) 2020-11-12 2020-11-12 基于rgbd相机的空间圆目标位姿测量方法

Country Status (1)

Country Link
CN (1) CN112381884B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113180709B (zh) * 2021-07-01 2021-09-07 晓智未来(成都)科技有限公司 一种基于摄影测量的人体待检测部位姿态识别方法
CN113453009B (zh) * 2021-07-04 2023-02-14 西北工业大学 基于拟合平面几何误差最小的点云空间可伸缩编码几何重构方法
CN113888611B (zh) * 2021-09-03 2023-03-31 北京三快在线科技有限公司 一种确定图像深度的方法、装置及存储介质
CN113744338A (zh) * 2021-10-29 2021-12-03 青岛影创信息科技有限公司 一种深度视频空间平面检测方法及系统
CN116664394B (zh) * 2023-08-01 2023-10-03 博奥生物集团有限公司 一种三维人眼图像生成方法及装置、电子设备、存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104517291A (zh) * 2014-12-15 2015-04-15 大连理工大学 基于目标同轴圆特征的位姿测量方法
CN105509733A (zh) * 2015-11-30 2016-04-20 上海宇航系统工程研究所 非合作空间圆目标的相对位姿测量方法
CN109163657A (zh) * 2018-06-26 2019-01-08 浙江大学 一种基于双目视觉三维重建的圆形目标位姿检测方法
CN109405835A (zh) * 2017-08-31 2019-03-01 北京航空航天大学 基于非合作目标直线与圆单目图像的相对位姿测量方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104517291A (zh) * 2014-12-15 2015-04-15 大连理工大学 基于目标同轴圆特征的位姿测量方法
CN105509733A (zh) * 2015-11-30 2016-04-20 上海宇航系统工程研究所 非合作空间圆目标的相对位姿测量方法
CN109405835A (zh) * 2017-08-31 2019-03-01 北京航空航天大学 基于非合作目标直线与圆单目图像的相对位姿测量方法
CN109163657A (zh) * 2018-06-26 2019-01-08 浙江大学 一种基于双目视觉三维重建的圆形目标位姿检测方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Satellite Pose Estimation via Single Perspective Circle and Line;MENG C. 等;《IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS》;20181231;第54卷(第6期);全文 *

Also Published As

Publication number Publication date
CN112381884A (zh) 2021-02-19

Similar Documents

Publication Publication Date Title
CN112381884B (zh) 基于rgbd相机的空间圆目标位姿测量方法
CN109300162B (zh) 一种基于精细化雷达扫描边缘点的多线激光雷达和相机联合标定方法
CN109785379B (zh) 一种对称物体尺寸及重量的测量方法及测量系统
CN113436260B (zh) 基于多传感器紧耦合的移动机器人位姿估计方法和系统
CN106919944B (zh) 一种基于orb算法的大视角图像快速识别方法
CN106683137B (zh) 基于人工标志的单目多目标识别与定位方法
CN110246159A (zh) 基于视觉和雷达信息融合的3d目标运动分析方法
CN109211198B (zh) 一种基于三目视觉的智能目标检测和测量系统和方法
CN109523595B (zh) 一种建筑工程直线棱角间距视觉测量方法
CN109579825B (zh) 基于双目视觉和卷积神经网络的机器人定位系统及方法
WO2021098079A1 (zh) 一种利用双目立体相机构建栅格地图的方法
CN112270698B (zh) 基于最邻近曲面的非刚性几何配准方法
CN107818598B (zh) 一种基于视觉矫正的三维点云地图融合方法
CN108921864A (zh) 一种光条中心提取方法及装置
CN103278138A (zh) 一种复杂结构薄部件三维位置及姿态的测量方法
CN113834625B (zh) 一种飞行器模型表面压力测量方法及系统
WO2023178925A1 (zh) 一种基于icp与多特征数据关联的障碍物预测与跟踪方法
CN106204557A (zh) 一种基于扩展高斯球与m估计的非完备数据对称特征的提取方法
CN112946679B (zh) 一种基于人工智能的无人机测绘果冻效应检测方法及系统
CN112947526B (zh) 一种无人机自主降落方法和系统
CN105957107A (zh) 行人检测与跟踪方法及装置
CN109086350B (zh) 一种基于WiFi的混合图像检索方法
CN115144828B (zh) 一种智能汽车多传感器时空融合的自动在线标定方法
CN113884002A (zh) 基于二三维信息融合的受电弓滑板上表面检测系统及方法
CN113034614B (zh) 一种五圆标定板的五圆心标定方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant