1.一种基于RGBD相机的6D位姿估计方法,其特征在于:包括,
步骤1:相机成像校准;
步骤2:基于校准后的相机成像对相机参数标定;
步骤3:三维模型重建与数据集构建:对目标物体进行全方位的视频采集,并使用OpenCV将视频文件分解为固定尺寸的单帧RGB图像;提取RGB图像并使用COLMAP工具创建数据集,然后进行SIFT特征提取和匹配;运用SFM算法生成稀疏点云模型,通过PatchMatch算法生成深度图,再进行深度融合生成最终的密集点云模型;对生成的点云模型进行裁剪和标注,构建物体相对于相机的坐标系,从而生成包含6D位姿标注的数据集;
步骤4:基于三维模型与数据集对目标物体分割与实际尺寸计算;
所述步骤4基于三维模型与数据集对目标物体分割与实际尺寸计算包括,机器人操作系统ROS通过消息过滤器来同步接收到的RGB图像、深度图像和点云数据;使用Matplotlib和RectangleSelector工具,让用户通过鼠标交互选择RGB图像中的目标物体所在区域并框选一个矩形区域,记录矩形区域的坐标作为分割的输入;使用预训练的SAM模型进行物体分割,根据用户选择的矩形框输入,使用SAM模型对该区域进行物体分割,得到分割掩码;使用相机的内参矩阵将点云投影到RGB图像坐标系中,计算每个点在图像中的位置,通过相机的内参公式,计算图像坐标系下的投影坐标:其中,fx和fy分别是相机焦距,cx和cy是主点坐标,x,y,z是3D点云的坐标;
通过投影后的坐标与分割掩码进行交叉匹配,筛选出属于分割区域的点云数据,计算并保存它们之间的最大欧氏距离:其中,xi,yi,zi和xj,yj,zj是点云中任意两个点的坐标;
步骤5:基于三维模型与数据集建立6D位姿估计网络模型:将RGB图像和深度图像进行对齐和归一化处理,利用ResNet网络对多尺度RGB图像和深度图像分别进行特征提取;利用不同扩张率的空洞卷积获得RGB图像特征与深度图像特征的不同尺度感受野的特征图,结合位置注意力机制与卷积块注意力机制,最后将特征融合,得到物体的初步估计的姿态;
结合深度图像与初步估计的姿态,利用深度图像中每个像素的深度值更新物体的实际位置,并计算物体与相机之间的距离,对初步估计的姿态进行多次迭代优化,通过调整姿态以最小化误差,从而获得最终的6D位姿,包括物体的平移向量和旋转矩阵;
步骤6:基于实际尺寸进行位姿估计;
所述步骤6基于实际尺寸进行位姿估计具体为,加载步骤4中通过点云计算得到的物体实际尺寸对6D位姿进行估计,估计结果包括物体的平移位置(x,y,z)和旋转角度(Rx,Ry,Rz),结合深度信息和尺寸信息进一步优化位姿估计的准确性,以此调整数据集中参考物体的位姿和尺寸,6D位姿估计网络模型加载调整后的数据集进行位姿估计和姿态细化。
2.根据权利要求1所述的一种基于RGBD相机的6D位姿估计方法,其特征在于:所述步骤1相机成像校准包括,光线穿过透镜投影到成像平面时会产生畸变现象,分为径向畸变和切向畸变,其中径向畸变分为桶形畸变和枕形畸变,而切向畸变是由于在相机的安装过程中,透镜和成像平面不平行导致的,属于安装误差;
径向畸变其归一化坐标计算公式具体为:
xdistorted=x(1+k1r2);
ydistorted=y(1+k1r2)
式中,(xdistorted,ydistorted)是经过矫正的标准坐标,点(x,y)是产生畸变的原始坐标,k1为畸变系数,r表示点(x,y)与图像中心点之间的径向距离;
切向畸变其归一化坐标计算公式具体为:
xdistorted=x+2p1xy+p2(r2+2x2);
ydistorted=y+2p2xy+p1(r2+2y2)
式中,(xdistorted,ydistorted)是经过矫正的标准坐标,点(x,y)是产生畸变的原始坐标,p1、p2为额外的畸变参数;
对于图像上任意一点p,联立上述k1、p1、p2三个畸变参数找到点(x,y)在像素坐标系中的准确坐标,p点归一化坐标具体计算公式为:xdistorted=x(1+k1r2)+2p1xy+p2(r2+2x2);
ydistorted=y(1+k1r2)+2p2xy+p1(r2+2y2)。
3.根据权利要求1所述的一种基于RGBD相机的6D位姿估计方法,其特征在于:所述步骤2基于校准后的相机成像对相机参数标定包括,首先将m张ArUco标定板呈圆周状布置在待检测物体周围,无重复标记且均匀分布,通过机器人操作系统ROS,在不同位置和角度拍摄n张关于ArUco标定板的RGB图像和红外图像,并对数据进行校准;通过校准得到RGB相机、深度相机和红外相机的内参矩阵和外参矩阵;
内参矩阵K公式如下:
其中,fx和fy为RGB相机在水平方向和竖直方向的焦距;cx和cy为主点的像素坐标,即图像中心在像素坐标系中的位置;
外参矩阵[R,T]描述了相机间的相对位置和旋转:
其中,[Xr,Yr,Zr]T为RGB相机坐标系下的三维点坐标;[Xd,Yd,Zd]T为深度相机坐标系下的三维点坐标;R为旋转矩阵;[tx,ty,tz]T为平移向量。
4.根据权利要求1所述的一种基于RGBD相机的6D位姿估计方法,其特征在于:所述步骤3在SFM算法过程中,COLMAP自动估计每帧图像的相机位姿,即通过三角测量和多视角几何分析,利用图像特征匹配计算出相机的外参,从而确定相机在三维空间中的位置和方向。
5.根据权利要求1所述的一种基于RGBD相机的6D位姿估计方法,其特征在于:步骤5中,所述空洞卷积的感受野具体计算公式为:RFi+1=RFi+(k`-1)×Si
式中,RFi+1表示当前层的感受野,RFi表示上一层的感受野,k、表示卷积核的大小,Si表示之前所有层的步长的乘积,Π表示累乘,Stridei表示第i层的步长。
6.根据权利要求1所述的一种基于RGBD相机的6D位姿估计方法,其特征在于:步骤5中,所述位置注意力机制具体包括,首先将特征图A{C*H*W}分别通过3个卷积层得到三个特征图{B,C,D},形状为C*H*W,然后将形状重构为C*N,其中,C代表通道数、H代表矩阵高度、W代表矩阵宽度、N=H×W,代表像素的数量,矩阵C和B的转置相乘,再通过softmax得到注意力特征图S{N*N},矩阵D和S的转置相乘,将结果矩阵的形状重构为C×H×W后再乘以尺度因子α,最后与输入矩阵A相加得到最后的输出矩阵E,计算公式如下:式中,Sji代表S矩阵的每一个元素,表示位置i对位置j的影响,Ej代表E中的每一个元素,α为尺度因子,Di表示特征图D中位置i的特征值,Aj表示特征图A中位置j的特征值,Bi表示经过卷积层提取后得到的特征图B中第i个位置的特征向量,Cj表示特征图C中第i个位置的特征向量;
所述卷积块注意力机制由通道注意力模块和空间注意力模块串联而成,对输入的特征图,先分别经过平均池化和最大池化,再通过共享多层感知机和Sigmoid激活函数来学习通道权重,再将结果分别经过平均池化和最大池化,然后通过一个7×7的空洞卷积和Sigmoid激活函数学习空间各点权重,最终生成权重优化的特征图,根据权重特征图选择具有最高相关性的视角图像作为最终的参考视角。
7.一种基于RGBD相机的6D位姿系统,用于实现如权利要求1-6任一项所述的6D位姿估计方法,其特征在于,包括:存储模块,所述计算机用于存储控制机器人运动的ROS系统、三维模型、6D位姿标注的数据集、预训练的SAM模型和6D位姿估计网络模型;
RGB相机,所述RGB相机对待测物体进行视频拍摄;
图像处理模块,所述图像处理模块包括OpenCV工具将视频文件分解为固定尺寸的单帧RGB图像、COLMAP工具创建数据集、SIFT对RGB图像进行特征提取和匹配;
计算模块,所述计算模块用于存储计算公式以及进行计算;
交互模块,所述交互模块用于让用户通过鼠标交互选择RGB图像中的目标物体所在区域并框选一个矩形区域,以及显示6D位姿;
图像分析模块,所述图像分析模块根据深度图像的深度值进行分析,确定物体与相机之间的距离,通过调整姿态以最小化误差,从而获得最终的6D位姿;
控制模块,所述控制模块用于实现各模块之间的数据传输和信息交互,所述控制模块包括信息接收单元和控制单元,所述信息接收单元用于接收图像处理模块处理后的图像数据,所述控制单元用于调用存储模块中的数据。