CN107958466A - 一种Slam算法优化基于模型的跟踪方法 - Google Patents
一种Slam算法优化基于模型的跟踪方法 Download PDFInfo
- Publication number
- CN107958466A CN107958466A CN201711249812.XA CN201711249812A CN107958466A CN 107958466 A CN107958466 A CN 107958466A CN 201711249812 A CN201711249812 A CN 201711249812A CN 107958466 A CN107958466 A CN 107958466A
- Authority
- CN
- China
- Prior art keywords
- image
- mrow
- slam
- edge
- point
- 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.)
- Granted
Links
Classifications
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/13—Edge detection
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
-
- 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)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种Slam算法优化基于模型的跟踪方法，包括步骤S1、对目标物进行初始化，得到目标物在图像中的精确位姿，确定目标物的边沿；S2、进行边沿追踪，确定目标物运动之后的位姿；S3、使用SLAM算法对目标物所在新的图像位置进行点云提取；S4、点云匹配，确定目标物的精确位姿；本发明中，相对于传统AR技术中使用人工标志点或者自然特征点方式进行视点定位，创新性的针对风机舱内环境特征纹理比较贫乏的问题将半稠密边缘SLAM技术应用到AR视点定位中。半稠密SLAM技术利用图像中的边缘特征同时完成场景三维重建和视点定位，非常适合风机舱内由线和面等元素构成的场景，具有新意。
Description
技术领域
本发明属于AR相关技术设计开发领域或属于海上风电场可视化运维决策系统领域，尤其是涉及一种Slam算法优化基于模型的跟踪方法。
背景技术
风电场一般采用预防性维护与纠错性维护相结合的运维策略，海上风电的工况复杂性、交通运输的可达性与安全性、故障处理可完成性等都会成为影响运维质量的重要因素。本发明针对海上风电运维体系所存在的管理经验欠缺，人员素质参差不齐，海上风电场天气及作业环境复杂，人员、设备和船只在海上作业危险性高等问题，基于AR(AugmentedReality增强现实)相关技术设计开发海上风电场可视化运维决策系统，利用图像识别、智能检测和数据挖掘技术等技术，帮助操作人员及时准确的定位并排除问题，根据现场情况制定相应的运维策略。AR技术依据实际现场环境与设备状态设计触发点，将实时运行数据嵌入AR显示界面中，实现集控数据与现实场景并存，从而获得人机交互的体感。在基于AR的可视化运维决策系统中现场维护人员通过所佩戴的AR设备对所要维修的对象进行识别，将相关的结构信息和实时运行数据附加在真实场景之上，制造出虚拟数据与现实场景共存的场景，帮助维护人员在现场进行运维问题决策与操作。
有鉴于此，特提出本发明。
发明内容
本发明要解决的技术问题在于克服现有技术的不足，提供一种Slam算法优化基于模型的跟踪方法，创新性的针对风机舱内环境特征纹理比较贫乏的问题将半稠密边缘SLAM技术应用到AR视点定位中。半稠密SLAM技术利用图像中的边缘特征同时完成场景三维重建和视点定位，非常适合风机舱内由线和面等元素构成的场景，具有新意。
为解决上述技术问题，本发明采用技术方案的基本构思是：
一种Slam算法优化基于模型的跟踪方法,包括以下步骤：
S1、对目标物进行初始化，得到目标物在图像中的精确位姿，确定目标物的边沿；
S2、进行边沿追踪，确定目标物运动之后的位姿；
S3、使用SLAM算法对目标物所在新的图像位置进行点云提取；
S4、点云匹配，确定目标物的精确位姿。
在上述方案中，创新性的针对风机舱内环境特征纹理比较贫乏的问题将半稠密边缘SLAM技术应用到AR视点定位中。半稠密SLAM技术利用图像中的边缘特征同时完成场景三维重建和视点定位，非常适合风机舱内由线和面等元素构成的场景，具有新意。
优选的，在步骤S1中，所述的对目标物进行初始化包括：根据已知目标物的模型，预先建立其点云数据库，然后确定目标物在图像中的位置，提取目标物在图像中的点云数据，最后将得到的点云数据与所建立的点云数据库进行点云匹配，得到点云集间的精确变换关系。
优选的，在进行目标物初始化后，得到目标物的位姿，并根据已知的目标物三维模型，确定目标物的边沿在图像中的坐标，将三维模型与实际图像拟合。
优选的，在步骤S2中，包括实时检测目标物的边沿以确定目标物的位置，其中检测目标物边沿的步骤为：
S101、对相机采集的图像做高斯平滑处理；
S102、计算得到图像的全局梯度；
S103、对图形进行保留局部梯度最大的点，而抑制非极大值；
S104、用双阈值算法检测和连接图像边缘；
S105、获得目标物新的轮廓位置，更新目标物位姿信息。
优选的，在步骤S101中，对图像进行高斯平滑处理采用高斯平滑函数为：
令g(x，y)为平滑后的图像，用h(x，y，σ)对图像f(x，y)进行平滑处理，即：
g(x，y)＝h(x，y，σ)*f(x，y)。
优选的，在步骤S102中，还包括以下步骤：
S1021、使用一阶有限差分计算x和y方向的偏导数f′x(x，y)和f′y(x，y)，由此得到偏导数矩阵Gx(x，y)和Gy(x，y)，公式如下：
f′x(x，y)≈Gx＝[f(x+1，y)-f(x，y)+f(x+1，y+1)-f(x，y+1)]/2；
f′y(x，y)≈Gy＝[f(x，y+1)-f(x，y)+f(x+1，y+1)-f(x+1，y)]/2；
S1022、进一步求有限差分的均值，以在图像中的同一点计算x和y的偏导数梯度，其中，幅值和方位角可用直角坐标到极坐标的坐标转化公式来计算：
θ[x，y]＝arctan(Gx(x，y)/Gy(x，y))；
其中，M[x，y]反映了图像的边缘强度；θ[x，y]反映了边缘的方向，使得M[x，y]取得局部最大值的方向θ[x，y]，就反映了边缘的方向。
优选的，步骤S103中对图形进行保留局部梯度最大的点，而抑制非极大值包括：比较每一点上的领域的中心像素M[x，y]与沿着梯度线的两个像素相比，如果M[x，y]的梯度值不比沿梯度线的两个相邻像素梯度值大，则令M[x，y]＝0，从而得到非极大值抑制图像。
优选的，步骤S104中用双阈值算法检测和连接图像边缘包括：对步骤S103中得到的非极大值抑制图像作用两个阈值th1和th2，其中，th1＝0.4th2；
把梯度值小于th1的像素的灰度值设为0，得到图像1，然后把梯度值小于th2的像素的灰度值设为0，得到图像2，以图像2为基础，以图像1为补充来连结图像的边缘。
优选的，所述连接图像的边缘的具体步骤如下：
S1041、对图像2进行扫描，当遇到一个非零灰度的像素p(x，y)时，跟踪以p(x，y)为开始点的轮廓线，直到轮廓线的终点q(x,y)；
S1042、考察图像1中与图像2中q(x,y)点位置对应的点s(x,y)的8邻近区域，如果在s(x,y)点的8邻近区域中有非零像素s(x,y)存在，则将其包括到图像2中，作为r(x,y)点；
S1043、从r(x,y)开始，重复上述步骤，直到在图像1和图像2中都无法继续为止；
S1044、完成对包含p(x,y)的轮廓线的连结之后，将这条轮廓线标记为已经访问，进入步骤S1041，寻找下一条轮廓线，重复上述步骤，直到图像2中找不到新轮廓线为止。
优选的，步骤S3中所述的SLAM算法为LSD-SLAM算法、ORB-SLAM算法、RGBD-SLAM2算法、ElasticFusion算法中的任意一种算法；
优选的,SLAM算法选用ORB-SLAM算法。
采用上述技术方案，本发明具有以下有益效果：
本发明的Slam算法优化基于模型的跟踪方法，包括步骤S1、对目标物进行初始化，得到目标物在图像中的精确位姿，确定目标物的边沿；S2、进行边沿追踪，确定目标物运动之后的位姿；S3、使用SLAM算法对目标物所在新的图像位置进行点云提取；S4、点云匹配，确定目标物的精确位姿；本发明中，相对于传统AR技术中使用人工标志点或者自然特征点方式进行视点定位，创新性的针对风机舱内环境特征纹理比较贫乏的问题将半稠密边缘SLAM技术应用到AR视点定位中。半稠密SLAM技术利用图像中的边缘特征同时完成场景三维重建和视点定位，非常适合风机舱内由线和面等元素构成的场景，具有新意。风电场一般采用预防性维护与纠错性维护相结合的运维策略，海上风电的工况复杂性、交通运输的可达性与安全性、故障处理可完成性等都会成为影响运维质量的重要因素。本发明针对海上风电运维体系所存在的管理经验欠缺，人员素质参差不齐，海上风电场天气及作业环境复杂，人员、设备和船只在海上作业危险性高等问题，基于AR(Augmented Reality增强现实)相关技术设计开发海上风电场可视化运维决策系统，利用图像识别、智能检测和数据挖掘技术等技术，帮助操作人员及时准确的定位并排除问题，根据现场情况制定相应的运维策略。AR技术依据实际现场环境与设备状态设计触发点，将实时运行数据嵌入AR显示界面中，实现集控数据与现实场景并存，从而获得人机交互的体感。在基于AR的可视化运维决策系统中现场维护人员通过所佩戴的AR设备对所要维修的对象进行识别，将相关的结构信息和实时运行数据附加在真实场景之上，制造出虚拟数据与现实场景共存的场景，帮助维护人员在现场进行运维问题决策与操作。
下面结合附图对本发明的具体实施方式作进一步详细的描述。
附图说明
附图作为本发明的一部分，用来提供对本发明的进一步的理解，本发明的示意性实施例及其说明用于解释本发明，但不构成对本发明的不当限定。显然，下面描述中的附图仅仅是一些实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其他附图。在附图中：
图1是本发明的跟踪方法执行步骤图；
图2是实施例一中计算得到图像的全局梯度示意图。
需要说明的是，这些附图和文字描述并不旨在以任何方式限制本发明的构思范围，而是通过参考特定实施例为本领域技术人员说明本发明的概念。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对实施例中的技术方案进行清楚、完整地描述，以下实施例用于说明本发明，但不用来限制本发明的范围。
在本发明的描述中，需要说明的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
实施例一
参见图1所示，本实施例提供一种Slam算法优化基于模型的跟踪方法,包括以下步骤：
S1、对目标物进行初始化，得到目标物在图像中的精确位姿，确定目标物的边沿；
S2、进行边沿追踪，确定目标物运动之后的位姿；
S3、使用SLAM算法对目标物所在新的图像位置进行点云提取；
S4、点云匹配，确定目标物的精确位姿。
在上述方案中，创新性的针对风机舱内环境特征纹理比较贫乏的问题将半稠密边缘SLAM技术应用到AR视点定位中。半稠密SLAM技术利用图像中的边缘特征同时完成场景三维重建和视点定位，非常适合风机舱内由线和面等元素构成的场景，具有新意。
优选的，在步骤S1中，所述的对目标物进行初始化包括：根据已知目标物的模型，预先建立其点云数据库，然后确定目标物在图像中的位置，提取目标物在图像中的点云数据，最后将得到的点云数据与所建立的点云数据库进行点云匹配，得到点云集间的精确变换关系。
优选的，在进行目标物初始化后，得到目标物的位姿，并根据已知的目标物三维模型，确定目标物的边沿在图像中的坐标，将三维模型与实际图像拟合。
优选的，在步骤S2中，包括实时检测目标物的边沿以确定目标物的位置，其中检测目标物边沿的步骤为：
S101、对相机采集的图像做高斯平滑处理；
S102、计算得到图像的全局梯度；
S103、对图形进行保留局部梯度最大的点，而抑制非极大值；
S104、用双阈值算法检测和连接图像边缘；
S105、获得目标物新的轮廓位置，更新目标物位姿信息。
优选的，在步骤S101中，对图像进行高斯平滑处理采用高斯平滑函数为：
令g(x，y)为平滑后的图像，用h(x，y，σ)对图像f(x，y)进行平滑处理，即：
g(x，y)＝h(x，y，σ)*f(x，y)。
优选的，在步骤S102中，还包括以下步骤：
S1021、使用一阶有限差分计算x和y方向的偏导数f′x(x，y)和f′y(x，y)，由此得到偏导数矩阵Gx(x，y)和Gy(x，y)，公式如下：
f′x(x，y)≈Gx＝[f(x+1，y)-f(x，y)+f(x+1，y+1)-f(x，y+1)]/2；
f′y(x，y)≈Gy＝[f(x，y+1)-f(x，y)+f(x+1，y+1)-f(x+1，y)]/2；
S1022、进一步求有限差分的均值，以在图像中的同一点计算x和y的偏导数梯度，其中，幅值和方位角可用直角坐标到极坐标的坐标转化公式来计算：
θ[x，y]＝arctan(Gx(x，y)/Gy(x，y))；
其中，M[x，y]反映了图像的边缘强度；θ[x，y]反映了边缘的方向，使得M[x，y]取得局部最大值的方向θ[x，y]，就反映了边缘的方向，参见图2所示。
优选的，步骤S103中对图形进行保留局部梯度最大的点，而抑制非极大值包括：比较每一点上的领域的中心像素M[x，y]与沿着梯度线的两个像素相比，如果M[x，y]的梯度值不比沿梯度线的两个相邻像素梯度值大，则令M[x，y]＝0，从而得到非极大值抑制图像。
优选的，步骤S104中用双阈值算法检测和连接图像边缘包括：对步骤S103中得到的非极大值抑制图像作用两个阈值th1和th2，其中，th1＝0.4th2；
把梯度值小于th1的像素的灰度值设为0，得到图像1，然后把梯度值小于th2的像素的灰度值设为0，得到图像2,以图像2为基础，以图像1为补充来连结图像的边缘。
优选的，所述连接图像的边缘的具体步骤如下：
S1041、对图像2进行扫描，当遇到一个非零灰度的像素p(x,y)时，跟踪以p(x,y)为开始点的轮廓线，直到轮廓线的终点q(x,y)；
S1042、考察图像1中与图像2中q(x,y)点位置对应的点s(x,y)的8邻近区域，如果在s(x,y)点的8邻近区域中有非零像素s(x,y)存在，则将其包括到图像2中，作为r(x,y)点；
S1043、从r(x,y)开始，重复上述步骤，直到在图像1和图像2中都无法继续为止；
S1044、完成对包含p(x,y)的轮廓线的连结之后，将这条轮廓线标记为已经访问，进入步骤S1041，寻找下一条轮廓线，重复上述步骤，直到图像2中找不到新轮廓线为止。
优选的，步骤S3中所述的SLAM算法为LSD-SLAM算法、ORB-SLAM算法、RGBD-SLAM2算法、ElasticFusion算法中的任意一种算法；
优选的,SLAM算法选用ORB-SLAM算法。
实施例二
实施例二在实施例一的基础上进一步对目标物进行初始化过程进行展开描述：
该目标物初始化过程，包括以下步骤：
S1001、根据已知目标物的模型，预先建立其点云数据库；
S1002、确定目标物在相机采集的图像中的位置，提取目标物在图像中的点云数据；
S1003、将得到的点云数据与所建立的点云数据库进行点云匹配，得到点云集间的精确变换关系。
优选的，所述的目标物的模型为3D模型，通过选择该3D模型的不同视角，生成多视角点云数据库，并记录各视角下目标物的位姿。可以采用slam软件库或PCL等实现。
优选的，将相机采集的图像与预先准备好的目标物的图像做ORB匹配，得到目标物在图像中的大致位置。
优选的，对预先准备好的目标物的图像进行ORB特征点提取并与相机采集的图像进行匹配，包括：分别获取该两图像的特征点，得到特征描述子，并判断两图像的特征描述子之间的欧式距离是否小于设定阈值，若判断结果为是，则判断两图像相匹配，否则判断两图像不匹配；
所述的ORB特征点提取包括以下步骤：
S1、将预先准备好的目标物的图像生成图像高斯金字塔；
S2、根据步骤S1中得到的图像生成DOG金字塔；
S3、对步骤S2得到的图像进行空间极值点检测，得到在尺度空间和二维图像空间上都是局部极值点的若干关键点；
S4、在步骤S3中得到的关键点中，以每一个关键点像素p为中心，半径为3做圆，圆形上有16个像素点分别为：p1、p2、...p16；
S5、定义一个阈值，计算p1、p9与中心p的像素差，若它们绝对值都小于设定阈值，则判断p点不可能是特征点，除去，否则，p点为候选点，需进一步判断；
S6、若p是候选点，则计算p1、p9、p5、p13与中心p的像素差，若它们的绝对值有至少3个超过阈值，则当做候选点，再进行下一步考察；
S7、计算p1到p16这16个点与中心p的像素差，若它们有至少9个超过阈值，则p为特征点；
S8、对图像进行非极大值抑制：计算特征点处的得分值，判断以特征点p为中心的一个邻域内，若有多个特征点，则判断每个特征点的s值，若p是邻域所有特征点中响应值最大的，则保留；
其中得分计算公式为：
其中p表示中心点的像素值，value表示以p为中心的领域内的特征点的像素值，S表示得分，t表示阈值；所述的特征点的s值为16个点与中心差值的绝对值总和；
S9、以步骤S8中所保留的特征点为中心，取SxS的邻域窗口，在窗口内随机选取一对点，比较二者像素的大小，进行如下二进制赋值；
其中，p(x)，p(y)分别是随机点x＝(u1,v1),y＝(u2,v2)的像素值；
S10、在窗口中随机选取N对随机点，重复二进制赋值，得到特征描述子；
S11、对步骤S8中筛选后的每一特征点，均得到一个256bit的二进制编码。
优选的，步骤S1包括以下步骤：
S101、将预先准备好的目标物的图像扩大一倍，作为高斯金字塔的第一组第一层，将第一组第一层图像经高斯卷积得到第一组第二层，高斯卷积的公式为：
其中，(x,y)为像素点的坐标，σ为正态分布的标准偏差，优选设定为1.6；
S102、将σ乘以一个比例系数k，得到新的σ，用它来平滑第一组第二层的图像，重复该步骤，最后得到L层图像，在同一组中，每一层图像的尺寸都是一样的，只是平滑系数不一样；
S103、将第一组倒数第三层图像作比例因子为2的降采样，得到的图像作为第二组第一层，然后进行步骤S102和S103，得到第二组的L层图像；
S104、根据上述过程，反复执行，得到一共O组，每组L层，共计O*L个图像；
在步骤S2中，由步骤S1中，得到的图像高斯金字塔中的第一组第二层减第一组第一层得到DOG金字塔第一组第一层，高斯金字塔中的第一组第三层减第一组第二层得到DOG金字塔第一组第二层，以此类推，逐组逐层生成每一个差分图像，所有的差分图像构成差分DOG金字塔，即DOG金字塔的第O组第I层图像是高斯金字塔的第O组I+1层减第O组I层得到的；
步骤S3中，还包括以下步骤：
S301、在DOG金字塔图像中，所有像素点与它3*3邻域内的8个点做比较；
S302、各像素点再和上下相邻两层图像中相同位置的像素点的3*3领域内的2*9个点做比较；
S303、保证关键点在尺度空间和二维图像空间上都是局部极值点。
优选的，所述的提取目标物在图像中的点云数据，包括在确定目标物在图像中的位置后，使用SLAM算法提起物体点云；
所述的SLAM算法采用LSD-SLAM算法、ORB-SLAM算法、RGBD-SLAM2算法、ElasticFusion算法中的任意一种算法；
优选的,SLAM算法选用ORB-SLAM算法。
优选的，当采用单目SLAM算法时，单目SLAM提取的特征点开始为二维点，需要使用三角化的方法求取特征点的深度信息，得到特征点的深度信息后，从而得到点云数据。
优选的，所述将得到的点云数据与所建立的点云数据库进行点云匹配，得到点云集间的精确变换关系包括采用点云匹配的算法，得到精确的物体位姿信息：
该点云匹配的算法包括以下步骤：
S401、点云特征点选取过程：
S402、计算特征描述子过程；
S403、匹配特征点，对点云进行粗匹配，得到粗匹配的坐标变化T和尺度变换S；
S404、迭代优化过程。
优选的，步骤S401还包括以下步骤：
S411、查询一次点云获取得到的数据中每一个点pi半径ri内所有点，并计算权值：
wij为三维点pi领域内任一点pj的权值，公式中pi,pj分别代表两点的三维坐标向量；
S412、根据权值计算方差矩阵
其中，T为求转值；
S413、计算方差矩阵的特征值{λi 1,λi 2,λi 3}，特征值按照从大到小顺序排列；
S414、设置阈值ε1和ε2保留满足λi 2/λi 1≤ε1和λi 3/λi 2≤ε2的点为关键点；
步骤S402还包括以下步骤：
S421、寻找满足步骤S414的关键点pi的r半径范围内所有点，假设点的个数为ni；
S422、计算ni个点的法向量；
S423、根据ni个点计算关键点pi的特征描述子；
其中，计算任意两点Ds和Dt以及它们对应的法线ns和nt之间的特征如下：
α＝V·nt；
θ＝arctan(W·ns，U·nt)；
d＝||Dt-Ds||；
其中，U,V,W分别代表三维直角坐标系中的三个坐标轴的单位向量，其中ns与U方向相同，φ为U方向和Ds与Dt连线方向间的夹角，α为nt与V方向的夹角，θ为nt在U‐V平面上的投影与U方向的夹角，d为Ds与Dt两点间的欧氏距离，计算关键点pi的r半径领域内的任意两点间的α、φ、θ、d为作为关键点pi的特征。
优选的，步骤S404包括以下步骤：
S441、设pi＝(xi,yi,zi),qj＝(xj,yj,zj)为三维空间中两个3D点，他们的欧氏距离为：
为求解任意两个点云集P和Q的旋转矩阵R和坐标变换T，对于点云P中的任一特征点pi,和Q中与pi所对应的特征点qj，有qj＝Rpi+T，利用最小二乘法求最优解得误差E为：
其中N代表两个点云中匹配的特征点总量，使用最小二乘法求解使误差E最小的R与T；
S442、平行移动和旋转分离：先对坐标变换T进行初始估算，分别得到点集P和Q的中心：
S443、构造点集P和Q的协方差矩阵：
其中代表点云中心化矩阵，qi T为向量的转置；
S444、由协方差矩阵构造4x4对称矩阵：
其中，I3是3x3单位矩阵；
其中，Δ为指代符号，代表Q(∑P,Q)；
S445、计算Q(∑P,Q)的特征值和特征向量，其最大特征值对应的特征向量即为最佳旋转向量qR＝[q0q1q2q3]T；
S446、计算最佳平移向量：
S447、将旋转矩阵和平移向量叠加在点云Q上，之后再带入式：
其中，若误差小于设定阈值，则迭代结束，否则继续重复以上步骤；迭代结束之后，所得到的旋转矩阵与平移向量即是目标的初始位置，初始化过程结束。
需要注意的是，实施例二中各步骤的序号与实施例一中的各步骤的序号相互独立，没有相关关系。
以上所述仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明，任何熟悉本专利的技术人员在不脱离本发明技术方案范围内，当可利用上述提示的技术内容做出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明方案的范围内。
Claims (10)
1.一种Slam算法优化基于模型的跟踪方法，其特征在于，包括以下步骤：
S1、对目标物进行初始化，得到目标物在图像中的精确位姿，确定目标物的边沿；
S2、进行边沿追踪，确定目标物运动之后的位姿；
S3、使用SLAM算法对目标物所在新的图像位置进行点云提取；
S4、点云匹配，确定目标物的精确位姿。
2.根据权利要求1所述的一种Slam算法优化基于模型的跟踪方法，其特征在于，在步骤S1中，所述的对目标物进行初始化包括：根据已知目标物的模型，预先建立其点云数据库，然后确定目标物在图像中的位置，提取目标物在图像中的点云数据，最后将得到的点云数据与所建立的点云数据库进行点云匹配，得到点云集间的精确变换关系。
3.根据权利要求1或2所述的一种Slam算法优化基于模型的跟踪方法，其特征在于，在进行目标物初始化后，得到目标物的位姿，并根据已知的目标物三维模型，确定目标物的边沿在图像中的坐标，将三维模型与实际图像拟合。
4.根据权利要求1-3任一所述的一种Slam算法优化基于模型的跟踪方法，其特征在于，在步骤S2中，包括实时检测目标物的边沿以确定目标物的位置，其中检测目标物边沿的步骤为：
S101、对相机采集的图像做高斯平滑处理；
S102、计算得到图像的全局梯度；
S103、对图形进行保留局部梯度最大的点，而抑制非极大值；
S104、用双阈值算法检测和连接图像边缘；
S105、获得目标物新的轮廓位置，更新目标物位姿信息。
5.根据权利要求4所述的一种Slam算法优化基于模型的跟踪方法，其特征在于，在步骤S101中，对图像进行高斯平滑处理采用高斯平滑函数为：
<mrow>
<mi>h</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>,</mo>
<mi>y</mi>
<mo>,</mo>
<mi>&sigma;</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mrow>
<mn>2</mn>
<msup>
<mi>&pi;&sigma;</mi>
<mn>2</mn>
</msup>
</mrow>
</mfrac>
<msup>
<mi>e</mi>
<mrow>
<mo>-</mo>
<mfrac>
<mrow>
<msup>
<mi>x</mi>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mi>y</mi>
<mn>2</mn>
</msup>
</mrow>
<mrow>
<mn>2</mn>
<msup>
<mi>&sigma;</mi>
<mn>2</mn>
</msup>
</mrow>
</mfrac>
</mrow>
</msup>
<mo>;</mo>
</mrow>
令g(x，y)为平滑后的图像，用h(x，y，σ)对图像f(x，y)进行平滑处理，即：
g(x，y)＝h(x，y，σ)*f(x，y)。
6.根据权利要求5所述的一种Slam算法优化基于模型的跟踪方法，其特征在于，在步骤S102中，还包括以下步骤：
S1021、使用一阶有限差分计算x和y方向的偏导数f′x(x，y)和f′y(x，y)，由此得到偏导数矩阵Gx(x，y)和Gy(x，y)，公式如下：
f′x(x，y)≈Gx＝[f(x+1，y)-f(x，y)+f(x+1，y+1)-f(x，y+1)]/2；
fy(x，y)≈Gy＝[f(x，y+1)-f(x，y)+f(x+1，y+1)-f(x+1，y)]/2；
S1022、进一步求有限差分的均值，以在图像中的同一点计算x和y的偏导数梯度，其中，幅值和方位角可用直角坐标到极坐标的坐标转化公式来计算：
<mrow>
<mi>M</mi>
<mo>&lsqb;</mo>
<mi>x</mi>
<mo>,</mo>
<mi>y</mi>
<mo>&rsqb;</mo>
<mo>=</mo>
<msqrt>
<mrow>
<msub>
<mi>G</mi>
<mi>x</mi>
</msub>
<msup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>,</mo>
<mi>y</mi>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msub>
<mi>G</mi>
<mi>y</mi>
</msub>
<msup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>,</mo>
<mi>y</mi>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</msqrt>
<mo>;</mo>
</mrow>
θ[x，y]＝arctan(Gx(x，y)/Gy(x，y))；
其中，M[x，y]反映了图像的边缘强度；θ[x，y]反映了边缘的方向，使得M[x，y]取得局部最大值的方向θ[x，y]，就反映了边缘的方向。
7.根据权利要求4-6任一所述的一种Slam算法优化基于模型的跟踪方法，其特征在于，步骤S103中对图形进行保留局部梯度最大的点，而抑制非极大值包括：比较每一点上的领域的中心像素M[x，y]与沿着梯度线的两个像素相比，如果M[x，y]的梯度值不比沿梯度线的两个相邻像素梯度值大，则令M[x，y]＝0，从而得到非极大值抑制图像。
8.根据权利要求4-7任一所述的一种Slam算法优化基于模型的跟踪方法，其特征在于，步骤S104中用双阈值算法检测和连接图像边缘包括：对步骤S103中得到的非极大值抑制图像作用两个阈值th1和th2，其中，th1＝0.4th2；
把梯度值小于th1的像素的灰度值设为0，得到图像1，然后把梯度值小于th2的像素的灰度值设为0，得到图像2,以图像2为基础，以图像1为补充来连结图像的边缘。
9.根据权利要求8所述的一种Slam算法优化基于模型的跟踪方法，所述连接图像的边缘的具体步骤如下：
S1041、对图像2进行扫描，当遇到一个非零灰度的像素p(x,y)时，跟踪以p(x,y)为开始点的轮廓线，直到轮廓线的终点q(x,y)；
S1042、考察图像1中与图像2中q(x,y)点位置对应的点s(x,y)的8邻近区域，如果在s(x,y)点的8邻近区域中有非零像素s(x,y)存在，则将其包括到图像2中，作为r(x,y)点；
S1043、从r(x,y)开始，重复上述步骤，直到在图像1和图像2中都无法继续为止；
S1044、完成对包含p(x,y)的轮廓线的连结之后，将这条轮廓线标记为已经访问，进入步骤S1041，寻找下一条轮廓线，重复上述步骤，直到图像2中找不到新轮廓线为止。
10.根据权利要求1-9任一所述的一种Slam算法优化基于模型的跟踪方法，其特征在于，步骤S3中所述的SLAM算法为LSD-SLAM算法、ORB-SLAM算法、RGBD-SLAM2算法、ElasticFusion算法中的任意一种算法；
优选的,SLAM算法选用ORB-SLAM算法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711249812.XA CN107958466B (zh) | 2017-12-01 | 2017-12-01 | 一种Slam算法优化基于模型的跟踪方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711249812.XA CN107958466B (zh) | 2017-12-01 | 2017-12-01 | 一种Slam算法优化基于模型的跟踪方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107958466A true CN107958466A (zh) | 2018-04-24 |
| CN107958466B CN107958466B (zh) | 2022-03-29 |
Family
ID=61962280
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711249812.XA Active CN107958466B (zh) | 2017-12-01 | 2017-12-01 | 一种Slam算法优化基于模型的跟踪方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107958466B (zh) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109887003A (zh) * | 2019-01-23 | 2019-06-14 | 亮风台（上海）信息科技有限公司 | 一种用于进行三维跟踪初始化的方法与设备 |
| CN111311684A (zh) * | 2020-04-01 | 2020-06-19 | 亮风台（上海）信息科技有限公司 | 一种进行slam初始化的方法与设备 |
| CN111368932A (zh) * | 2020-03-16 | 2020-07-03 | 北京伟杰东博信息科技有限公司 | 一种图像比对的方法及系统 |
| CN112258575A (zh) * | 2020-10-13 | 2021-01-22 | 浙江大学 | 一种同步定位和地图构建中物体的快速识别方法 |
| CN112837225A (zh) * | 2021-04-15 | 2021-05-25 | 浙江卡易智慧医疗科技有限公司 | 一种立位全脊柱图像自动无缝拼接的方法及装置 |
| CN112862803A (zh) * | 2021-02-26 | 2021-05-28 | 中国人民解放军93114部队 | 基于边缘和特征点融合的红外成像slam方法和装置 |
| CN113688819A (zh) * | 2021-08-23 | 2021-11-23 | 南通大学 | 一种基于标记点的目标物期望点跟踪匹配方法 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102903108A (zh) * | 2012-09-25 | 2013-01-30 | 哈尔滨工程大学 | 基于水下图像统计特性的边缘检测方法 |
| CN104933718A (zh) * | 2015-06-23 | 2015-09-23 | 广东省自动化研究所 | 一种基于双目视觉的物理坐标定位方法 |
| CN105212418A (zh) * | 2015-11-05 | 2016-01-06 | 北京航天泰坦科技股份有限公司 | 基于红外夜视功能的增强现实智能头盔研制 |
| US20170008650A1 (en) * | 2013-12-28 | 2017-01-12 | Huazhong University Of Science And Technology | Attitude estimation method and system for on-orbit three-dimensional space object under model restraint |
| CN106441151A (zh) * | 2016-09-30 | 2017-02-22 | 中国科学院光电技术研究所 | 一种基于视觉和主动光学融合的三维目标欧式空间重建的测量系统 |
| CN106548519A (zh) * | 2016-11-04 | 2017-03-29 | 上海玄彩美科网络科技有限公司 | 基于orb‑slam和深度相机的真实感的增强现实方法 |
| CN106709878A (zh) * | 2016-11-30 | 2017-05-24 | 长沙全度影像科技有限公司 | 一种快速图像融合方法 |
| CN106780326A (zh) * | 2016-11-30 | 2017-05-31 | 长沙全度影像科技有限公司 | 一种提高全景图像清晰度的融合方法 |
-
2017
- 2017-12-01 CN CN201711249812.XA patent/CN107958466B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102903108A (zh) * | 2012-09-25 | 2013-01-30 | 哈尔滨工程大学 | 基于水下图像统计特性的边缘检测方法 |
| US20170008650A1 (en) * | 2013-12-28 | 2017-01-12 | Huazhong University Of Science And Technology | Attitude estimation method and system for on-orbit three-dimensional space object under model restraint |
| CN104933718A (zh) * | 2015-06-23 | 2015-09-23 | 广东省自动化研究所 | 一种基于双目视觉的物理坐标定位方法 |
| CN105212418A (zh) * | 2015-11-05 | 2016-01-06 | 北京航天泰坦科技股份有限公司 | 基于红外夜视功能的增强现实智能头盔研制 |
| CN106441151A (zh) * | 2016-09-30 | 2017-02-22 | 中国科学院光电技术研究所 | 一种基于视觉和主动光学融合的三维目标欧式空间重建的测量系统 |
| CN106548519A (zh) * | 2016-11-04 | 2017-03-29 | 上海玄彩美科网络科技有限公司 | 基于orb‑slam和深度相机的真实感的增强现实方法 |
| CN106709878A (zh) * | 2016-11-30 | 2017-05-24 | 长沙全度影像科技有限公司 | 一种快速图像融合方法 |
| CN106780326A (zh) * | 2016-11-30 | 2017-05-31 | 长沙全度影像科技有限公司 | 一种提高全景图像清晰度的融合方法 |
Non-Patent Citations (3)
| Title |
|---|
| 李鹏等: ""基于ORB特征点匹配的多目标跟踪算法"", 《湖 南 大 学 学 报 （自 然 科 学 版 ）》 * |
| 林汀等: ""序列图像特征提取与匹配算法的改进"", 《计算机工程与应用》 * |
| 胡汉平: ""双目立体测距关键技术研究"", 《中国博士学位论文全文数据库信息科技辑》 * |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109887003A (zh) * | 2019-01-23 | 2019-06-14 | 亮风台（上海）信息科技有限公司 | 一种用于进行三维跟踪初始化的方法与设备 |
| CN111368932A (zh) * | 2020-03-16 | 2020-07-03 | 北京伟杰东博信息科技有限公司 | 一种图像比对的方法及系统 |
| CN111368932B (zh) * | 2020-03-16 | 2021-05-28 | 赢技科技发展(杭州)有限公司 | 一种图像比对的方法及系统 |
| CN111311684A (zh) * | 2020-04-01 | 2020-06-19 | 亮风台（上海）信息科技有限公司 | 一种进行slam初始化的方法与设备 |
| CN111311684B (zh) * | 2020-04-01 | 2021-02-05 | 亮风台（上海）信息科技有限公司 | 一种进行slam初始化的方法与设备 |
| CN112258575A (zh) * | 2020-10-13 | 2021-01-22 | 浙江大学 | 一种同步定位和地图构建中物体的快速识别方法 |
| CN112862803A (zh) * | 2021-02-26 | 2021-05-28 | 中国人民解放军93114部队 | 基于边缘和特征点融合的红外成像slam方法和装置 |
| CN112862803B (zh) * | 2021-02-26 | 2023-09-26 | 中国人民解放军93114部队 | 基于边缘和特征点融合的红外成像slam方法和装置 |
| CN112837225A (zh) * | 2021-04-15 | 2021-05-25 | 浙江卡易智慧医疗科技有限公司 | 一种立位全脊柱图像自动无缝拼接的方法及装置 |
| CN112837225B (zh) * | 2021-04-15 | 2024-01-23 | 浙江卡易智慧医疗科技有限公司 | 一种立位全脊柱图像自动无缝拼接的方法及装置 |
| CN113688819A (zh) * | 2021-08-23 | 2021-11-23 | 南通大学 | 一种基于标记点的目标物期望点跟踪匹配方法 |
| CN113688819B (zh) * | 2021-08-23 | 2023-08-01 | 南通大学 | 一种基于标记点的目标物期望点跟踪匹配方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107958466B (zh) | 2022-03-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107958466A (zh) | 一种Slam算法优化基于模型的跟踪方法 | |
| CN107993287A (zh) | 一种目标跟踪的自动初始化方法 | |
| CN107679537B (zh) | 一种基于轮廓点orb特征匹配的无纹理空间目标姿态估计算法 | |
| CN104376552B (zh) | 一种3d模型与二维图像的虚实配准方法 | |
| Sanchez et al. | Planar 3D modeling of building interiors from point cloud data | |
| Ding et al. | Automatic registration of aerial imagery with untextured 3d lidar models | |
| CN104408732B (zh) | 一种基于全向结构光的大视场深度测量系统及方法 | |
| CN104867126B (zh) | 基于点对约束和三角形网的有变化区域的合成孔径雷达图像配准方法 | |
| CN108090572A (zh) | 一种海上风电场增强现实系统及其控制方法 | |
| CN111126304A (zh) | 一种基于室内自然场景图像深度学习的增强现实导航方法 | |
| Bonanni et al. | 3-d map merging on pose graphs | |
| Zhou et al. | FVNet: 3D front-view proposal generation for real-time object detection from point clouds | |
| CN105046710A (zh) | 基于深度图分割与代理几何体的虚实碰撞交互方法及装置 | |
| CN108765489A (zh) | 一种基于组合靶标的位姿计算方法、系统、介质及设备 | |
| Apollonio et al. | Evaluation of feature-based methods for automated network orientation | |
| CN104346608A (zh) | 稀疏深度图稠密化方法和装置 | |
| Urban et al. | Finding a good feature detector-descriptor combination for the 2D keypoint-based registration of TLS point clouds | |
| CN105654023B (zh) | 识别物体危险性的方法和装置 | |
| CN108109208A (zh) | 一种海上风电场增强现实方法 | |
| CN109741240A (zh) | 一种基于层次聚类的多平面图像拼接方法 | |
| CN108416385A (zh) | 一种基于改进图像匹配策略的同步定位与建图方法 | |
| Girard et al. | End-to-end learning of polygons for remote sensing image classification | |
| CN114782628A (zh) | 基于深度相机的室内实时三维重建方法 | |
| Tang et al. | Sonar image mosaic based on a new feature matching method | |
| Weinmann et al. | Geometric point quality assessment for the automated, markerless and robust registration of unordered TLS point clouds |
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 |