CN108717712A - 一种基于地平面假设的视觉惯导slam方法 - Google Patents
一种基于地平面假设的视觉惯导slam方法 Download PDFInfo
- Publication number
- CN108717712A CN108717712A CN201810532344.5A CN201810532344A CN108717712A CN 108717712 A CN108717712 A CN 108717712A CN 201810532344 A CN201810532344 A CN 201810532344A CN 108717712 A CN108717712 A CN 108717712A
- Authority
- CN
- China
- Prior art keywords
- camera
- ground
- matrix
- point
- equation
- 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
- 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
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
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方法,从图像中提取特征点进行IMU预积分,建立相机投影模型,并进行相机内参标定、IMU和相机之间的外参标定;系统初始化,将视觉观测到的点云和相机位姿对齐到IMU预积分上,恢复地面方程和相机位姿;对地面初始化得到地面方程,确定当前相机位姿下的地面方程,并反投影到图像坐标系上,获取更加准确的地面区域;基于状态估计,推导出各传感器观测模型,将相机观测、IMU观测和地面特征观测融合在一起做状态估计,使用图优化模型来做状态估计,使用稀疏图优化和梯度下降法来实现整个优化。本发明相较之前的算法有较大的精度上的提升,可在全局限制相机位姿的估计,使得准确度大大提升。
Description
技术领域
本发明涉及一种定位及建图技术,具体为一种基于地平面假设的视觉惯导SLAM方法。
背景技术
SLAM全称是同时定位和地图构建,由摄像头实时采集图像,通过一帧帧的图像估计相机的运动轨迹,并重建相机运动场景的地图。传统视觉SLAM使用场景中颜色变化明显的点、线作为地图的路标点,没有任何实际含义,也没有任何的上下文语义,在商场环境中受光照和行人遮挡影响严重。为了让机器人在室内外环境自由移动,为了AR应用能够更加真实的与场景融为一体,SLAM成为近年研究的热点,而单目相机体积小、成本低,能够方便的嵌入其他设备中等优点逐渐成为关注的焦点。但是传统的基于纯视觉的SLAM受到光照变化和遮挡后精度和连贯性上面都会明显下降。
发明内容
针对现有技术中基于纯视觉的SLAM受到光照变化和遮挡后精度和连贯性明显下降等不足,本发明要解决的问题是提供一种定位鲁棒性、连续性更强,可恢复真实尺度的基于地平面假设的视觉惯导SLAM方法。
为解决上述技术问题,本发明采用的技术方案是:
本发明一种基于地平面假设的视觉惯导SLAM方法,包括以下步骤:
1)从一图像中提取特征点,对上述特征点进行IMU预积分,建立相机投影模型,并进行相机内参标定、IMU和相机之间的外参标定;
2)基于地面标识物的Visual-Inertial SLAM系统的初始化,将视觉观测到的点云和相机位姿对齐到IMU预积分上,并且用地面上的已知尺寸、坐标的标识物来恢复地面方程和相机位姿;
3)通过Visual-Inertial Odometry得到相机相对地面的旋转和高度,对步骤2)中的地面进行初始化,得到地面方程,确定当前相机位姿下的地面方程,并把这个地面方程反投影到7图像坐标系上,使用图像处理方法获取更加准确的地面区域;
4)基于先验地面特征点深度的Visual-Inertial状态估计,详细推导出各种传感器的观测模型,将相机观测、IMU观测和地面特征观测融合在一起来做状态估计,使用图优化模型来做状态估计,使用稀疏图优化和梯度下降法来实现整个优化。
步骤1)中建立相机投影模型为:
101)世界坐标系和相机坐标系转换公式为
其中为相机的外参,描述相机相对世界原点旋转和平移的量,Xc、Yc、Zc为相机坐标系,Xw、Yw、Zw为世界坐标系;
图像坐标系到像素坐标系转换公式为
X、Y、Z代表相机坐标系,x、y代表图像平面坐标系,f表示一个线性倍数;
102)将相机坐标在u轴上放大fx倍,在v轴上放大fy倍,原点平移cx,cy,得到像素坐标系:
103)整理世界坐标系到像素坐标系的转换,其中包含一个其次坐标到非齐次坐标的转换,如公式:
其中为相机内参K矩阵,1/Zc为尺度因子,fx和fy表示放大的倍数,cx和cy表示原点平移,R表示旋转矩阵,t表示平移矩阵;
104)对每张图像,首先使用Shi-Tomasi检测特征点,利用滑动窗口在各个方向上变化,确定是否为角点;
105)将关键帧之间IMU测量值积分分成两帧之间相对运动的约束,消除初始条件变化造成的重复积分,在每个关键帧到来之前对IMU数据积分;
106)利用“张正友标定法”对相机进行标定。
3.根据权利要求2所述的基于地平面假设的视觉惯导SLAM方法,其特征在于:步骤1)中,相机投影模型中具有一个归一化平面,在相机坐标系下,用来表示相机坐标,有公式:
归一化平面的归一化坐标是在z的方向上,当z=1时用一个平面截断,光心与3D点的连线在此平面上的点为该3d点的归一化点,表示为公式:
106)归一化坐标经过内参的左乘之后得到相机坐标,如公式所示:
其中,K为其中相机内参K矩阵为
步骤2)中,基于地面标识物的Visual-Inertial SLAM系统的初始化,将视觉观测到的点云和相机位姿对齐到IMU观测上,并且用地面上的已知尺寸、坐标的标识物来恢复地面方程和相机位姿,包括以下步骤:
201)从两张图片中,得到一对配对好的特征点,利用对极约束,给出两个匹配点的空间位置关系:平移和旋转;
202)用八点法求解本质矩阵E:
E=t^R
E为一个3*3的矩阵,内有9个未知数;t为平移矩阵,r为旋转矩阵;
上述一对特征点的归一化坐标为x1=[u1,v1,1]T,x2=[u2,v2,1]T,根据对极约束,有公式:
(u1,v1,1)和是做为一对匹配点的两组归一化坐标是本质矩阵E;
利用八对匹配点组成的矩阵满足秩为8的条件,本质矩阵E根据上述方程求得;
203)利用SVD分解求出两个关键帧之间的相对旋转和平移设本质矩阵E的SVD分解为公式:
E=U Σ VT
其中∑为奇异值矩阵,U和V为对角矩阵,根据E的内在性质,在SVD分解中,对于任意一个E,存在四个可能的R,t与它对应,其中只有一组解中P矩阵即R和t组成的矩阵在两个相机中都具有正的深度,取那个解;
204)恢复位姿后,利用三角化恢复特征点深度;
设x1,x2为两个特征点的归一化坐标,满足公式:
s1x1=s2Rx2+t
求解的是两个特征点的深度s1,s2。对公式等式两边左乘x1^,左侧为零,右侧可以看成是s2的一个方程,可以直接求出s2,有了s2,s1也自然能求出,于是,得到了两帧下的点的深度,确定了它们的空间坐标;
205)将视觉和惯导紧耦合对齐;
206)使用SUFT角点作为图像中特征点,搜索实时图像中标识物的位置,并使用SUFT描述子来寻找匹配的特征点,当匹配的特征点超过10个,再使用RANSAC算法,使得取出任意四对儿匹配的特征点,满足同一个单应性矩阵;
207)假定这个平面标识物的中心为世界坐标的原点,它的四个顶点都在同一个平面上,它的四个顶点的真实世界坐标为(-a,-b,0),(-a,b,0),(a,-b,0)和(a,b,0);已知3D,2D的匹配点对儿,使用PnP算法来求解相机相对这个标识物的旋转和平移矩阵。
步骤3)中,对预选定的地面进行初始化,得到地面方程,确定当前相机位姿下的地面方程,并把这个地面方程反投影到图像坐标系上,使用图像处理方法获取更加准确的地面区域,具体为:
301)通过Visual-Inertial Odometry的迭代求解获取带有累积误差的相机位姿系统初始化时,世界坐标系下的地面方程为:
nw TPw+dw=0
其中nw代表地面方程的法向量,Pw=(Xw,Yw,Zw)代表地面上任意一点,dw是世界坐标系原点到地面平面的距离;
302)准备一个地面模型图像,假设M是模型直方图数据、I是图像直方图数据,j为像素BIN的个数,直方图交叉描述为:
303)根据直方图图像的每个像素点I(x,y)的像素值获得相应的直方图分布概率,对得到分布概率图像做卷积,求取局部最大值,即得到已知物体位置信息。
步骤4)中,将相机观测、IMU观测和地面特征观测融合在一起来做状态估计包括以下步骤:
401)使用一个紧耦合滑动窗口将图像帧和IMU观测融合在一起,保证计算量保持在可控范围内;
402)定义惯导观测模型为:
其中,第一行代表位置残差,第二行代表速度残差,第三行代表朝向残差,第四行代表加速度计Bias残差,第五行代表陀螺仪Bias残差; 代表在k时刻IMU在世界坐标系下的旋转和平移,gw代表重力加速度,代表k时刻IMU相对世界坐标系下的速度,代表k时刻加速度计的Bias,代表k时刻陀螺仪的Bias;表示带噪声的加速度计预积分结果,表示带噪声的陀螺仪观测数据的预积分结果。表示两帧之间的旋转约束。
403)建立视觉差模型,用三角化的带深度的特征点云,依据当前估计的相机位姿投影到相机的像素坐标系上,然后算投影特征点和与它匹配的真实特征点位置的偏移量;
404)建立地面观测模型,得到地面特征点深度的观测残差;
405)融合视觉差模型、惯导观测模型以及面观测模型,对三个模型的特征点深度进行捆集优化;
406)对相机的运动模型和观测模型构成的图进行优化;
407)采用梯度下降方法对上述图优化构造的公式求解最优解;
408)将构造的图进行稀疏性和边缘化处理,得到稀疏图。
步骤404)中,建立地面观测模型,得到地面特征点深度的观测残差包括以下步骤:
404A)假设一帧图像中地面特征点的集合为g,则地面观测残差为公式:
其中为第一次观测到地面特征点g为利用当时估计的相机位姿三角化之后的特征点深度,为地面特征点g通过当前相机位姿投影到地面之后的深度;x为参数;
404B)将图像上归一化坐标系的一点对应到相机坐标系下,如公式:
得到
最终结果如公式:
k1=T11(T22T34-T24T32)
+T12(T24T31-T21T34)
+T14(T21T32-T22T31)
k2=T11T22-T12T21
k3=T21T32-T22T31
得到地面特征点深度的观测残差;
其中ki是由中参数构成的,是一个4*4矩阵,描述了相机到地面的旋转和平移,此处ki只是中间变量;
404C)构建一个最小二乘来求解最优的后验状态估计,即代价函数:
通过上式找到一个最优的参数,使得代价函数无限接近于零,则得到地面特征点深度的观测残差。
步骤407)中,梯度下降法为采用梯度下降对构造的方程求最优解为:
407A)通过信赖域法,结合普通梯度下降和高斯牛顿法优化方法,利用局部数据对整体函数进行近似,求得局部最小值;
407B)使用立文伯格马夸特测定法算法进行梯度下降:
给定初始值x0,以及初始优化半径μ;
对于第k次迭代求解,得公式:
计算ρ;
若ρ>3/4,则μ=2μ;
若ρ<1/4,则μ=0.5μ;
如果ρ大于某阈值,则认为近似可行,令xk+1=xk+Δxk
判断是否收敛,如果不收敛则返回第二步,否则结束。
步骤408)中,稀疏图优化是将构造的图进行稀疏性和边缘化处理,得到稀疏图,步骤为:
408A)假设一个场景有两个相机位姿(c1,c2)和六个路标(P1,P2,P3,P4,P5,P6),相机位姿和特征点所对应的变量为Ti,i=1,2以及xj,j=1,2,相机C1观测到路标点P1,P2,P3,P4,相机C2观测到路标点P3,P4,P5,P6;
408B)场景下的目标函数为:
408C)e11描述了在C1观测到P1,与其他的相机位姿和路标无关;令J11为e11所对应的雅可比矩阵,这里把所有变量以(T1,T2,x1,...,x6)T的顺序摆放,有式:
408D)将这些Jij按照顺序列为向量,得到整体雅可比矩阵及相应的H矩阵的稀疏;
408E)在H矩阵中,将左上角维度与相机维度相等的对角矩块记为B,右下角与路标点维度相等的对角矩阵块记为C,其余的非对角矩阵块,左下角的非对角矩阵块记为ET,右上角的非对角矩阵块记为E,得到与对应的线性方程组,将其变为公式:
其中Δxc指代相机位姿的最优变化量,Δxp指代特征点三维位置的最优变化量;U,W代表整个代价函数在当前状态下对相机位姿的偏导数和对路标点的偏导数,也就是雅可比矩阵分块之后左右两个区域;
408F)对线形方程组进行高斯消元,消去右上角的非对角部分E,得公式:
把左上角维度与相机维度相等的对角矩块记为B,右下角与路标点维度相等的对角矩阵块记为C,其余的非对角矩阵块,左下角的非对角矩阵块记为ET,右上角的非对角矩阵块记为E;
经过消元之后,方程组第一行变成和Δxp无关的项,单独取出,得到关于位姿部分的增量方程为式:
[B-EC-1ET]Δxc=v-EC-1w
408G)通过上述方程,快速计算出Δxc的值,然后把解得的Δxc带入到原方程中,求解Δxp。
本发明具有以下有益效果及优点:
1.本发明基于地面平面假设的Visual-Inertial SLAM算法,因为可以实时的获取地面点真实深度,每一帧的误差都会被全局的进行校正,消除位置以及角度的累积误差,所以相较之前的算法有较大的精度上的提升,几乎与真值重合。
2.本发明方法加入了惯性传感器,把IMU的预积分数据也加入到优化框架里,可以在全局限制相机位姿的估计,使得准确度大大提升。
3.本发明方法在重建的三维地图中,能够清晰的构建出地面区域,为后续的AR应用或者机器人应用提供更丰富的信息。
附图说明
图1为本发明中基于地面假设的Visual-Inertial SLAM示意图;
图2为本发明涉及的相机模型图;
图3为本发明中Shi-Tomasi特征点检测过程示意图;
图4为本发明中对极几何约束示意图;
图5为本发明中视觉和惯导紧耦合对齐过程示意图;
图6为本发明中实际场景和标识物模版特征点匹配图示;
图7为本发明中不同相机位姿下的世界坐标轴对比图;
图8为本发明方法中初始化时的反投影地面区域图片;
图9为本发明中半图像帧和IMU观测融合在一起的紧耦合滑动窗口示意图;
图10为本发明中整体的雅可比矩阵及相应的H矩阵的稀疏情况图示;
图11为本发明中真实室内场景测试环境图片;
图12为本发明中人工制作的平面标识物图示;
图13为本发明中将平面路标放置在场景中示意图;
图14为本发明中初始化世界坐标系原点示意图;
图15为本发明中初始化点云图;
图16为本发明中初始化相机位置的性能图;
图17为本发明中地面预选区域图示;
图18为本发明中地面模版和与之对应的颜色概率直方图;
图19为本发明中进行直方图反投影后的地面区域(白色区域);
图20为本发明中的测试场景平面图;
图21为本发明中真实轨迹平面图;
图22为本发明中的VINS轨迹图;
图23为本发明中的OKVIS轨迹图;
图24为本发明中的VIGO轨迹图。
具体实施方式
下面结合说明书附图对本发明作进一步阐述。
本发明提出了基于地面平面假设的Visual-Inertial SLAM方法(VIGO),加入地面上特征点以及平面路标上的特征点作为地图特征实现SLAM。为了使定位的鲁棒性、连续性更强,恢复真实尺度,本文加入了惯性传感器,把IMU的预积分数据也加入到优化框架里。这样可以在全局限制相机位姿的估计,使得准确度大大提升。此外在重建的三维地图中,能够清晰的构建出地面区域,为后续的AR应用或者机器人应用提供更丰富的信息。整体框架如图1所示。
本发明基于地平面假设的视觉惯导SLAM方法,括以下步骤:
1)从一图像中提取特征点,对上述特征点进行IMU预积分,建立相机投影模型,并进行相机内参标定、IMU和相机之间的外参标定;
2)基于地面标识物的Visual-Inertial SLAM系统的初始化,将视觉观测到的点云和相机位姿对齐到IMU预积分上,并且用地面上的已知尺寸、坐标的标识物来恢复地面方程和相机位姿;
3)通过Visual-Inertial Odometry得到相机相对地面的旋转和高度,对步骤2)中的地面进行初始化,得到地面方程,确定当前相机位姿下的地面方程,并把这个地面方程反投影到7图像坐标系上,使用地面模板动态更新、直方图反向投影匹配等图像处理方法获取更加准确的地面区域;
4)基于先验地面特征点深度的Visual-Inertial状态估计,详细推导出各种传感器的观测模型,将相机观测、IMU观测和地面特征观测融合在一起来做状态估计,使用图优化模型来做状态估计,使用稀疏图优化和梯度下降法来实现整个优化。
步骤1)中,建立相机投影模型为:
101)世界坐标系和相机坐标系转换公式为
相机模型如图2所示,其中为相机的外参,描述相机相对世界原点旋转和平移的量,相机坐标系为X、Y、Z,图像平面坐标系为x,y;
图像坐标系到像素坐标系转换公式为
X、Y、Z代表相机坐标系,x、y代表图像平面坐标系,f表示一个线性倍数;
102)将相机坐标在u轴上放大fx倍,在v轴上放大fy倍,原点平移cx,cy,得到像素坐标系:
103)整理世界坐标系到像素坐标系的转换,其中包含一个其次坐标到非齐次坐标的转换,如公式:
其中为相机内参K矩阵,1/Zc为尺度因子,fx和fy表示放大的倍数,cx和cy表示原点平移,R表示旋转矩阵,t表示平移矩阵。
104)对每张图像,首先使用Shi-Tomasi检测特征点,利用滑动窗口在各个方向上变化,确定是否为角点;
Shi-Tomasi特征点检测过程如图3所示。想象一个窗口在图像上移动,在平滑区域中,窗口在各个方向没有变化。在边缘区域中,窗口在边缘的方向上没有变化。在角点处窗口在各个方向上都具有变化。Shi-Tomasi角点检测正是利用了这个物理现象,通过窗口在各个方向上变化,决定是否为角点。
105)将关键帧之间IMU测量值积分分成两帧之间相对运动的约束,消除初始条件变化造成的重复积分,在每个关键帧到来之前对IMU数据积分;
106)利用“张正友标定法”对相机进行标定。
步骤2)中,基于地面标识物的Visual-Inertial SLAM系统的初始化,将视觉观测到的点云和相机位姿对齐到IMU观测上,并且用地面上的已知尺寸、坐标的标识物来恢复地面方程和相机位姿,包括以下步骤:
201)从两张图片中,得到一对配对好的特征点,利用对极约束,给出两个匹配点的空间位置关系:平移和旋转;
如图4所示,对极约束包含了平移和旋转。T为平移,R为旋转。
E=T^R
202)用八点法求解本质矩阵E:
E=t^R
E为一个3*3的矩阵,内有9个未知数;
上述一对特征点的归一化坐标为x1=[u1,v1,1]T,x2=[u2,v2,1]T,根据对极约束,有公式:
利用八对匹配点组成的矩阵满足秩为8的条件,本质矩阵E根据上述方程求得;
203)利用SVD分解求出两个关键帧之间的相对旋转和平移
设本质矩阵E的SVD分解为公式:
E=U Σ VT
其中∑为奇异值矩阵,U和V为对角矩阵。根据E的内在性质,在SVD分解中,对于任意一个E,存在四个可能的R,t与它对应,其中只有一组解中P矩阵即R和t组成的矩阵在两个相机中都具有正的深度,取那个解;
204)恢复位姿后,利用三角化恢复特征点深度;
设x1,x2为两个特征点的归一化坐标,满足公式:
s1x1=s2Rx2+t
求解的是两个特征点的深度s1,s2。对公式等式两边左乘x1^,左侧为零,右侧可以看成是s2的一个方程,可以直接求出s2,有了s2,s1也自然能求出,于是,得到了两帧下的点的深度,确定了它们的空间坐标;
205)将视觉和惯导紧耦合对齐;如图5所示。
206)使用SUFT角点作为图像中特征点,搜索实时图像中标识物的位置,并使用SUFT描述子来寻找匹配的特征点,当匹配的特征点超过10个,再使用RANSAC算法,使得取出任意四对儿匹配的特征点,满足同一个单应性矩阵;如图6所示,为实际场景和标识物模版特征点匹配情况:
207)假定这个平面标识物的中心为世界坐标的原点,它的四个顶点都在同一个平面上,它的四个顶点的真实世界坐标为(-a,-b,0),(-a,b,0),(a,-b,0)和(A,b,0);已知3D,2D的匹配点对儿,使用PnP算法来求解相机相对这个标识物的旋转和平移矩阵。
如图7所示,相机相对世界坐标原点的位姿,其中黑线包围区域区域为地面区域,地面上的A4纸区域为标识物平面,三条直线中的灰线和白线分别代表x轴和y轴,黑线代表z轴:
步骤3)中,对预选定的地面进行初始化,得到地面方程,确定当前相机位姿下的地面方程,并把这个地面方程反投影到图像坐标系上,使用图像处理方法获取更加准确的地面区域,具体为:
301)通过Visual-Inertial Odometry的迭代求解获取带有累积误差的相机位姿系统初始化时,世界坐标系下的地面方程为:
nw TPw+dw=0
其中nw=(aw,bw,cw)代表地面方程的法向量,Pw=(XwYwZw)代表地面上任意一点,dw/||n||是世界坐标系原点到地面平面的距离;
图8为初始化地面预选区域,其中黑线框为初始化时地面在相机图像坐标系下的投影。
302)准备一个地面模型图像,假设M是模型直方图数据、I是图像直方图数据,j为像素BIN的个数,直方图交叉描述为:
303)根据直方图图像的每个像素点I(x,y)的像素值获得相应的直方图分布概率,对得到分布概率图像做卷积,求取局部最大值,即得到已知物体位置信息。
步骤4)中,将相机观测、IMU观测和地面特征观测融合在一起来做状态估计包括以下步骤:
401)使用一个紧耦合滑动窗口将图像帧和IMU观测融合在一起,保证计算量保持在可控范围内;如图9所示.
402)定义惯导观测模型为:
其中,第一行代表位置残差,第二行代表速度残差,第三行代表朝向残差,第四行代表加速度计Bias残差,第五行代表陀螺仪Bias残差; 代表在k时刻IMU在世界坐标系下的旋转和平移,gw代表重力加速度,代表k时刻IMU相对世界坐标系下的速度,代表k时刻加速度计的Bias,代表k时刻陀螺仪的Bias;
403)建立视觉差模型,用三角化的带深度的特征点云,依据当前估计的相机位姿投影到相机的像素坐标系上,然后算投影特征点和与它匹配的真实特征点位置的偏移量;
404)建立地面观测模型,得到地面特征点深度的观测残差;
假设一帧图像中地面特征点的集合为g,则地面观测残差为公式:
其中为第一次观测到地面特征点g为利用当时估计的相机位姿三角化之后的特征点深度,为地面特征点g通过当前相机位姿投影到地面之后的深度;x为参数。
404B)将图像上归一化坐标系的一点对应到相机坐标系下,如公式:
得到
最终结果如公式:
k1=T11(T22T34-T24T32)
+T12(T24T31-T21T34)
+T14(T21T32-T22T31)
k2=T11T22-T12T21
k3=T21T32-T22T31
得到地面特征点深度的观测残差;
其中ki是由中参数构成的,是一个4*4矩阵,描述了相机到地面的旋转和平移,此处ki只是中间变量;
404C)构建一个最小二乘来求解最优的后验状态估计,即代价函数:
通过上式找到一个最优的参数,使得代价函数无限接近于零,则得到地面特征点深度的观测残差。
405)融合视觉差模型、惯导观测模型以及面观测模型,对三个模型的特征点深度进行捆集优化;
406)对相机的运动模型和观测模型构成的图进行优化;
406A)假设在时刻k,机器人在位置xk处,用传感器进行了一次测量,得到数据zk。
406B)传感器的观测方程为式:
zk=h(xk)
406C)由于噪声就有了误差,所以真正的观测误差如式
ek=zk-h(xk)
406D)这里以xk为优化变量,以minxF(xk)=||ek||为目标函数,求得xk的估计值。以IMU观测方程为例,为直接从传感器获得的观测数据,相当于zk,相当于h(xk),待估计的状态为
步骤407)中,梯度下降法为采用梯度下降对构造的方程求最优解为:
407A)通过信赖域法,结合普通梯度下降和高斯牛顿法优化方法,利用局部数据对整体函数进行近似,求得局部最小值;
407B)使用LM(Levenberg–Marquardt)(立文伯格马夸特测定法)算法进行梯度下降:
给定初始值x0,以及初始优化半径μ;
对于第k次迭代求解,得公式:
计算ρ。
若ρ>3/4,则μ=2μ。
若ρ<1/4,则μ=0.5μ。
如果ρ大于某阈值,则认为近似可行,令xk+1=xk+Δxk
判断是否收敛。如果不收敛则返回第二步,否则结束。
步骤408),将构造的图进行稀疏性和边缘化处理,得到稀疏图,具体为:
408A)假设一个场景有两个相机位姿(C1,C2)和六个路标(P1,P2,P3,P4,P5,P6),相机位姿和特征点所对应的变量为Ti,i=1,2以及xj,j=1,2,相机C1观测到路标点P1,P2,P3,P4,相机C2观测到路标点P3,P4,P5,P6;
408B)场景下的目标函数为:
408C)e11描述了在C1观测到P1,与其他的相机位姿和路标无关;令J11为e11所对应的雅可比矩阵,这里把所有变量以(T1,T2,x1,...,x6)T的顺序摆放,有式:
408D)将这些Jij按照顺序列为向量,得到整体雅可比矩阵及相应的H矩阵的稀疏情况,如图10所示:雅可比矩阵和H矩阵的稀疏性,填色代表有数据,无填色代表零;
408E)在H矩阵中,将左上角维度与相机维度相等的对角矩块记为B,右下角与路标点维度相等的对角矩阵块记为C,其余的非对角矩阵块,左下角的非对角矩阵块记为ET,右上角的非对角矩阵块记为E,得到与对应的线性方程组,将其变为公式:
其中Δxc指代相机位姿的最优变化量,Δxp指代特征点三维位置的最优变化量。U,W代表整个代价函数在当前状态下对相机位姿的偏导数和对路标点的偏导数。也就是雅可比矩阵分块之后左右两个区域;
408F)对线形方程组进行高斯消元,消去右上角的非对角部分E,得公式:
把左上角维度与相机维度相等的对角矩块记为B,右下角与路标点维度相等的对角矩阵块记为C,其余的非对角矩阵块,左下角的非对角矩阵块记为ET,右上角的非对角矩阵块记为E。
经过消元之后,方程组第一行变成和Δxp无关的项,单独取出,得到关于位姿部分的增量方程为式:
[B-EC-1ET]Δxc=v-EC-1w
408G)通过上述方程,快速计算出Δxc的值,然后把解得的Δxc带入到原方程中,求解Δxp。
本实施例中,图像传感器采用的是全局曝光的黑白图像传感器MT9V034,IMU选用ICM20602。图像传感器输出640*480尺寸的图像,帧率可调,本实验使用每秒20帧的图像帧率。图像传感器和IMU之间的同步通过板载的FPGA绝对同步的外触发方式驱动,IMU在每次图像传感器曝光时采集数据,保证图像每帧之间50ms内一共采集10个IMU数据,即IMU的采样率为200HZ。实验使用的传感器把相机和IMU做在一块PCB板子上,节省了空间。板子的尺寸为9*2.2cm。可以方便的嵌入机器人或者AR眼镜中。图像传感器的镜头采用M12广角镜头,可观测角度为150度,使得机器视觉的感受范围更宽,采集到更多的特征点。
实验用的机器配置为Intel i5-4250U处理器,4G DDR3内存,Linux系统。
选择东北大学信息楼一楼休息区作为实验场地,如图11所示;测试场景还需要一个或多个平面标识物进行世界坐标的确定以及累积误差的修正,制作的平面标识物,如图12所示;这个标识物需要提前布置在场景中,如图13所示。取这个路标的中心点为世界坐标原点,那么四个角点的坐标分别为(-0.21,-0.29),(0.21,-0.29),(-0.21,0.29),(0.21,0.29)。
初始化阶段:
在初始化阶段,用户在休息区测试环境中前后移动摄像头,使用环境中的特征和平面路标点上的特征来构建SFM初始点云,并在跟IMU数据对齐之后根据提示,对准平面路标进行世界坐标的确立。如图14所示,为初始化的世界坐标系可视化。
图15为初始化点云:
初始化点云数据可视化,其中2曲线为相机轨迹,1框为相机,白色点为识别出的地面点云,深灰色点为场景内普通点云。
为了测试初始化的性能,进行了十次相同位置的对平面路标的观测,如图所示,其中横轴为相机三维坐标轴,纵轴为坐标值,单位是米。十次测试的相机真实位置均为(-0.5,0.5,1.5),图16中用浅色的加号表示。深色的加号表示十次初始化的相机位置,可以看到,x,y轴的误差在±0.01m。z轴的误差在±0.02cm。
实验结果证明算法初始化的精度足够进行初始位置的确定,而且初始化过程完全是自动进行的,不需要用户选取初始点位置,满足实时性要求。地面检测模块:
从初始化时获得的相机位姿,地面方程是在初始化时确定的,地面方程与地面平面路标在同一平面。
基于图17中的预选区域进行反向直方图投影,在进行直方图匹配之前需要对图像进行预先处理,计算反向投影直方图的过程如下:
基于图像中的预选区域进行反向直方图投影,在进行直方图匹配之前需要对图像进行预先处理,计算反向投影直方图的过程如下:
(1)从地面模版的归一化直方图中读取概率值,概率图如图18所示。
(2)把原图像中的每一个像素替换成与之对应的归一化中直方图中的概率值。
(3)把替换成概率值的像素值从0~1替换成0~255的值。
(4)灰度值越大的像素越有可能是目标像素点。
如图19所示为根据模版直方图反向投影得到的与地面模版匹配的地面区域。
非线性优化模块:
实验数据使用第一节介绍的图像传感器和IMU,采集的数据来源于东北大学浑南校区信息楼A座一楼休息区。
场景平面图如图20所示,场地占地面积为150平方米,平面图中边缘标注的数字的单位是米,考虑到实际场景中沙发是稳定的参考物,所以只把沙发画在了平面图中:
图21为实验人员的真实行走路径:VINS也是使用了Visual-Inertial紧耦合的方式进行非线性优化,所以转角的角度误差很小,虽然VINS也通过初始化恢复了相机的真实尺度,但是在实时跟踪当中,受限于IMU有很大的累积误差,导致最终优化的直线部分会存在尺度问题,表现在实验结果中即为直线部分的累积误差。
如图22~24所示,三幅图展示了不同方法在平面图上的轨迹图。如图22所示,其中VINS也是使用了Visual-Inertial紧耦合的方式进行非线性优化,所以转角的角度误差很小,虽然VINS也通过初始化恢复了相机的真实尺度,但是在实时跟踪当中,受限于IMU有很大的累积误差,导致最终优化的直线部分会存在尺度问题,表现在实验结果中即为直线部分的累积误差。
如图23所示,相对于VINS,OKVIS表现的不够好,虽然和VINS一样,是Visual-Inertial紧耦合的方式进行非线性估计,但是在转弯处的估计会有一些偏差,这个可能跟OKVIS的边缘化策略有关。
如图24所示,可以看到VINS和OKVIS的计算结果都有一定的累积误差,VINS使用了IMU进行尺度恢复,所以误差更小一些,在一些局部达到了很高的精度,但是在长时间直线移动后,还是有较大的累积误差。而本文提出的基于地面平面假设的Visual-InertialSLAM算法,因为可以实时的获取地面点真实深度,每一帧的误差都会被全局的进行校正,消除位置以及角度的累积误差,所以相较之前的算法有较大的精度上的提升,几乎与真值重合。
本发明提出的基于地面平面假设的Visual-Inertial SLAM算法,因为可以实时的获取地面点真实深度,每一帧的误差都会被全局的进行校正,消除位置以及角度的累积误差,所以相较之前的算法有较大的精度上的提升,几乎与真值重合。
Claims (9)
1.一种基于地平面假设的视觉惯导SLAM方法,其特征在于包括以下步骤:
1)从一图像中提取特征点,对上述特征点进行IMU预积分,建立相机投影模型,并进行相机内参标定、IMU和相机之间的外参标定;
2)基于地面标识物的Visual-Inertial SLAM系统的初始化,将视觉观测到的点云和相机位姿对齐到IMU预积分上,并且用地面上的已知尺寸、坐标的标识物来恢复地面方程和相机位姿;
3)通过Visual-Inertial Odometry得到相机相对地面的旋转和高度,对步骤2)中的地面进行初始化,得到地面方程,确定当前相机位姿下的地面方程,并把这个地面方程反投影到图像坐标系上,使用图像处理方法获取更加准确的地面区域;
4)基于先验地面特征点深度的Visual-Inertial状态估计,详细推导出各种传感器的观测模型,将相机观测、IMU观测和地面特征观测融合在一起来做状态估计,使用图优化模型来做状态估计,使用稀疏图优化和梯度下降法来实现整个优化。
2.根据权利要求1所述的基于地平面假设的视觉惯导SLAM方法,其特征在于:步骤1)中建立相机投影模型为:
101)世界坐标系和相机坐标系转换公式为
其中为相机的外参,描述相机相对世界原点旋转和平移的量,Xc、Yc、Zc为相机坐标系,Xw、Yw、Zw为世界坐标系;
图像坐标系到像素坐标系转换公式为
X、Y、Z代表相机坐标系,x、y代表图像平面坐标系,f表示一个线性倍数;
102)将相机坐标在u轴上放大fx倍,在v轴上放大fy倍,原点平移cx,cy,得到像素坐标系:
103)整理世界坐标系到像素坐标系的转换,其中包含一个其次坐标到非齐次坐标的转换,如公式:
其中为相机内参K矩阵,1/Zc为尺度因子,fx和fy表示放大的倍数,cx和cy表示原点平移,R表示旋转矩阵,t表示平移矩阵;
104)对每张图像,首先使用Shi-Tomasi检测特征点,利用滑动窗口在各个方向上变化,确定是否为角点;
105)将关键帧之间IMU测量值积分分成两帧之间相对运动的约束,消除初始条件变化造成的重复积分,在每个关键帧到来之前对IMU数据积分;
106)利用“张正友标定法”对相机进行标定。
3.根据权利要求2所述的基于地平面假设的视觉惯导SLAM方法,其特征在于:步骤1)中,相机投影模型中具有一个归一化平面,在相机坐标系下,用来表示相机坐标,有公式:
归一化平面的归一化坐标是在z的方向上,当z=1时用一个平面截断,光心与3D点的连线在此平面上的点为该3d点的归一化点,表示为公式:
106)归一化坐标经过内参的左乘之后得到相机坐标,如公式所示:
其中,K为其中相机内参K矩阵为
4.根据权利要求1所述的基于地平面假设的视觉惯导SLAM方法,其特征在于:步骤2)中,基于地面标识物的Visual-Inertial SLAM系统的初始化,将视觉观测到的点云和相机位姿对齐到IMU观测上,并且用地面上的已知尺寸、坐标的标识物来恢复地面方程和相机位姿,包括以下步骤:
201)从两张图片中,得到一对配对好的特征点,利用对极约束,给出两个匹配点的空间位置关系:平移和旋转;
202)用八点法求解本质矩阵E:
E=t^R
E为一个3*3的矩阵,内有9个未知数;t为平移矩阵,r为旋转矩阵;
上述一对特征点的归一化坐标为x1=[u1,v1,1]T,x2=[u2,v2,1]T,根据对极约束,有公式:
(u1,v1,1)和是做为一对匹配点的两组归一化坐标是本质矩阵E;
利用八对匹配点组成的矩阵满足秩为8的条件,本质矩阵E根据上述方程求得;
203)利用SVD分解求出两个关键帧之间的相对旋转和平移设本质矩阵E的SVD分解为公式:
E=U∑VT
其中∑为奇异值矩阵,U和V为对角矩阵,根据E的内在性质,在SVD分解中,对于任意一个E,存在四个可能的R,t与它对应,其中只有一组解中P矩阵即R和t组成的矩阵在两个相机中都具有正的深度,取那个解;
204)恢复位姿后,利用三角化恢复特征点深度;
设x1,x2为两个特征点的归一化坐标,满足公式:
s1x1=s2Rx2+t
求解的是两个特征点的深度s1,s2;对公式等式两边左乘x1 ∧,左侧为零,右侧可以看成是s2的一个方程,可以直接求出s2,有了s2,s1也自然能求出,于是,得到了两帧下的点的深度,确定了它们的空间坐标;
205)将视觉和惯导紧耦合对齐;
206)使用SUFT角点作为图像中特征点,搜索实时图像中标识物的位置,并使用SUFT描述子来寻找匹配的特征点,当匹配的特征点超过10个,再使用RANSAC算法,使得取出任意四对儿匹配的特征点,满足同一个单应性矩阵;
207)假定这个平面标识物的中心为世界坐标的原点,它的四个顶点都在同一个平面上,它的四个顶点的真实世界坐标为(-a,-b,0),(-a,b,0),(a,-b,0)和(a,b,0);已知3D,2D的匹配点对儿,使用PnP算法来求解相机相对这个标识物的旋转和平移矩阵。
5.根据权利要求1所述的基于地平面假设的视觉惯导SLAM方法,其特征在于:步骤3)中,对预选定的地面进行初始化,得到地面方程,确定当前相机位姿下的地面方程,并把这个地面方程反投影到图像坐标系上,使用图像处理方法获取更加准确的地面区域,具体为:
301)通过Visual-Inertial Odometry的迭代求解获取带有累积误差的相机位姿系统初始化时,世界坐标系下的地面方程为:
nw TPw+dw=0
其中nw代表地面方程的法向量,Pw=(Xw,Yw,Zw)代表地面上任意一点,dw是世界坐标系原点到地面平面的距离;
302)准备一个地面模型图像,假设M是模型直方图数据、I是图像直方图数据,j为像素BIN的个数,直方图交叉描述为:
303)根据直方图图像的每个像素点I(x,y)的像素值获得相应的直方图分布概率,对得到分布概率图像做卷积,求取局部最大值,即得到已知物体位置信息。
6.根据权利要求1所述的基于地平面假设的视觉惯导SLAM方法,其特征在于:步骤4)中,将相机观测、IMU观测和地面特征观测融合在一起来做状态估计包括以下步骤:
401)使用一个紧耦合滑动窗口将图像帧和IMU观测融合在一起,保证计算量保持在可控范围内;
402)定义惯导观测模型为:
其中,第一行代表位置残差,第二行代表速度残差,第三行代表朝向残差,第四行代表加速度计Bias残差,第五行代表陀螺仪Bias残差; 代表在k时刻IMU在世界坐标系下的旋转和平移,gw代表重力加速度,代表k时刻IMU相对世界坐标系下的速度,代表k时刻加速度计的Bias,代表k时刻陀螺仪的Bias;表示带噪声的加速度计预积分结果,表示带噪声的陀螺仪观测数据的预积分结果;表示两帧之间的旋转约束;
403)建立视觉差模型,用三角化的带深度的特征点云,依据当前估计的相机位姿投影到相机的像素坐标系上,然后算投影特征点和与它匹配的真实特征点位置的偏移量;
404)建立地面观测模型,得到地面特征点深度的观测残差;
405)融合视觉差模型、惯导观测模型以及面观测模型,对三个模型的特征点深度进行捆集优化;
406)对相机的运动模型和观测模型构成的图进行优化;
407)采用梯度下降方法对上述图优化构造的公式求解最优解;
408)将构造的图进行稀疏性和边缘化处理,得到稀疏图。
7.根据权利要求6所述的基于地平面假设的视觉惯导SLAM方法,其特征在于步骤404)中,建立地面观测模型,得到地面特征点深度的观测残差包括以下步骤:
404A)假设一帧图像中地面特征点的集合为则地面观测残差为公式:
其中为第一次观测到地面特征点g为利用当时估计的相机位姿三角化之后的特征点深度,为地面特征点g通过当前相机位姿投影到地面之后的深度;x为参数;
404B)将图像上归一化坐标系的一点对应到相机坐标系下,如公式:
得到
最终结果如公式:
k1=T11(T22T34-T24T32)
+T12(T24T31-T21T34)
+T14(T21T32-T22T31)
k2=T11T22-T12T21
k3=T21T32-T22T31
得到地面特征点深度的观测残差;
其中ki是由中参数构成的,是一个4*4矩阵,描述了相机到地面的旋转和平移,此处ki只是中间变量;
404C)构建一个最小二乘来求解最优的后验状态估计,即代价函数:
通过上式找到一个最优的参数,使得代价函数无限接近于零,则得到地面特征点深度的观测残差。
8.根据权利要求1所述的基于地平面假设的视觉惯导SLAM方法,其特征在于步骤407)中,梯度下降法为采用梯度下降对构造的方程求最优解为:
407A)通过信赖域法,结合普通梯度下降和高斯牛顿法优化方法,利用局部数据对整体函数进行近似,求得局部最小值;
407B)使用立文伯格马夸特测定法算法进行梯度下降:
给定初始值x0,以及初始优化半径μ;
对于第k次迭代求解,得公式:
计算ρ;
若ρ>3/4,则μ=2μ;
若ρ<1/4,则μ=0.5μ;
如果ρ大于某阈值,则认为近似可行,令xk+1=xk+Δxk
判断是否收敛,如果不收敛则返回第二步,否则结束。
9.根据权利要求1所述的基于地平面假设的视觉惯导SLAM方法,其特征在于步骤408)中,稀疏图优化是将构造的图进行稀疏性和边缘化处理,得到稀疏图,步骤为:
408A)假设一个场景有两个相机位姿(C1,C2)和六个路标(P1,P2,P3,P4,P5,P6),相机位姿和特征点所对应的变量为Ti,i=1,2以及xj,j=1,2,相机C1观测到路标点P1,P2,P3,P4,相机C2观测到路标点P3,P4,P5,P6;
408B)场景下的目标函数为:
408C)e11描述了在C1观测到P1,与其他的相机位姿和路标无关;令J11为e11所对应的雅可比矩阵,这里把所有变量以(T1,T2,x1,...,x6)T的顺序摆放,有式:
408D)将这些Jij按照顺序列为向量,得到整体雅可比矩阵及相应的H矩阵的稀疏;
408E)在H矩阵中,将左上角维度与相机维度相等的对角矩块记为B,右下角与路标点维度相等的对角矩阵块记为C,其余的非对角矩阵块,左下角的非对角矩阵块记为ET,右上角的非对角矩阵块记为E,得到与对应的线性方程组,将其变为公式:
其中Δxc指代相机位姿的最优变化量,Δxp指代特征点三维位置的最优变化量;U,W代表整个代价函数在当前状态下对相机位姿的偏导数和对路标点的偏导数,也就是雅可比矩阵分块之后左右两个区域;
408F)对线形方程组进行高斯消元,消去右上角的非对角部分E,得公式:
把左上角维度与相机维度相等的对角矩块记为B,右下角与路标点维度相等的对角矩阵块记为C,其余的非对角矩阵块,左下角的非对角矩阵块记为ET,右上角的非对角矩阵块记为E;
经过消元之后,方程组第一行变成和Δxp无关的项,单独取出,得到关于位姿部分的增量方程为式:
[B-EC-1ET]Δxc=v-EC-1w
408G)通过上述方程,快速计算出Δxc的值,然后把解得的Δxc带入到原方程中,求解Δxp。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810532344.5A CN108717712B (zh) | 2018-05-29 | 2018-05-29 | 一种基于地平面假设的视觉惯导slam方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810532344.5A CN108717712B (zh) | 2018-05-29 | 2018-05-29 | 一种基于地平面假设的视觉惯导slam方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108717712A true CN108717712A (zh) | 2018-10-30 |
CN108717712B CN108717712B (zh) | 2021-09-03 |
Family
ID=63912529
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810532344.5A Active CN108717712B (zh) | 2018-05-29 | 2018-05-29 | 一种基于地平面假设的视觉惯导slam方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108717712B (zh) |
Cited By (45)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109523595A (zh) * | 2018-11-21 | 2019-03-26 | 南京链和科技有限公司 | 一种建筑工程直线棱角间距视觉测量方法 |
CN109764880A (zh) * | 2019-02-19 | 2019-05-17 | 中国科学院自动化研究所 | 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统 |
CN109801379A (zh) * | 2019-01-21 | 2019-05-24 | 视辰信息科技(上海)有限公司 | 通用的增强现实眼镜及其标定方法 |
CN109900704A (zh) * | 2019-03-12 | 2019-06-18 | 西安交通大学 | 在用齿轮磨损表面微观形貌原位三维重构方法 |
CN110135396A (zh) * | 2019-05-27 | 2019-08-16 | 百度在线网络技术(北京)有限公司 | 地面标志的识别方法、装置、设备及介质 |
CN110135387A (zh) * | 2019-05-24 | 2019-08-16 | 李子月 | 一种基于传感器融合的图像快速识别方法 |
CN110375739A (zh) * | 2019-06-26 | 2019-10-25 | 中国科学院深圳先进技术研究院 | 一种移动端视觉融合定位方法、系统及电子设备 |
CN110458892A (zh) * | 2019-07-17 | 2019-11-15 | 北京自动化控制设备研究所 | 基于惯性导航的相机标定方法 |
CN110473258A (zh) * | 2019-07-24 | 2019-11-19 | 西北工业大学 | 基于点线统一框架的单目slam系统初始化算法 |
CN110517324A (zh) * | 2019-08-26 | 2019-11-29 | 上海交通大学 | 基于变分贝叶斯自适应算法的双目vio实现方法 |
CN110647609A (zh) * | 2019-09-17 | 2020-01-03 | 上海图趣信息科技有限公司 | 视觉地图定位方法及系统 |
CN110942483A (zh) * | 2019-11-21 | 2020-03-31 | 三一重工股份有限公司 | 函数快速收敛模型构建方法、装置和终端 |
CN110992487A (zh) * | 2019-12-10 | 2020-04-10 | 南京航空航天大学 | 手持式飞机油箱快速三维地图重建装置及重建方法 |
CN111028125A (zh) * | 2019-11-14 | 2020-04-17 | 天津大学 | 一种用于slam的已知自身位姿的光束平差法fpga加速器 |
CN111145267A (zh) * | 2019-12-25 | 2020-05-12 | 重庆自行者科技有限公司 | 基于imu辅助的360度全景视图多相机标定方法 |
CN111156984A (zh) * | 2019-12-18 | 2020-05-15 | 东南大学 | 一种面向动态场景的单目视觉惯性slam方法 |
CN111213364A (zh) * | 2018-12-21 | 2020-05-29 | 深圳市大疆创新科技有限公司 | 拍摄设备的控制方法、拍摄设备的控制装置及拍摄设备 |
WO2020135183A1 (zh) * | 2018-12-29 | 2020-07-02 | 广州文远知行科技有限公司 | 点云地图的构建方法、装置、计算机设备和存储介质 |
CN111588315A (zh) * | 2020-06-19 | 2020-08-28 | 苏州亿倍智能清洁股份有限公司 | 一种扫地机陀螺仪角度修正方法及扫地机 |
CN111739359A (zh) * | 2020-06-30 | 2020-10-02 | 上海乂学教育科技有限公司 | 增强现实的课件生成系统 |
CN111913499A (zh) * | 2020-07-17 | 2020-11-10 | 天津大学 | 基于单目视觉slam及深度不确定性分析的云台控制方法 |
CN111932637A (zh) * | 2020-08-19 | 2020-11-13 | 武汉中海庭数据技术有限公司 | 一种车身相机外参自适应标定方法和装置 |
CN112132888A (zh) * | 2019-06-25 | 2020-12-25 | 黑芝麻智能科技(重庆)有限公司 | 大规模室内稀疏激光雷达点云内的单目相机定位 |
CN112215884A (zh) * | 2019-07-09 | 2021-01-12 | 北京地平线机器人技术研发有限公司 | 一种平面标志物的位姿确定方法和装置 |
CN112288813A (zh) * | 2020-11-03 | 2021-01-29 | 浙江大学 | 基于多目视觉测量与激光点云地图匹配的位姿估计方法 |
CN112304321A (zh) * | 2019-07-26 | 2021-02-02 | 北京初速度科技有限公司 | 一种基于视觉和imu的车辆融合定位方法及车载终端 |
CN112489091A (zh) * | 2020-12-18 | 2021-03-12 | 湖南华南光电(集团)有限责任公司 | 一种基于直瞄模板的全捷联图像导引头目标跟踪方法 |
CN112669196A (zh) * | 2021-03-16 | 2021-04-16 | 浙江欣奕华智能科技有限公司 | 一种硬件加速引擎中因子图优化数据的方法和设备 |
CN112785655A (zh) * | 2021-01-28 | 2021-05-11 | 中汽创智科技有限公司 | 一种基于车道线检测的环视相机外参自动标定方法、装置、设备及计算机存储介质 |
CN113096185A (zh) * | 2021-03-29 | 2021-07-09 | Oppo广东移动通信有限公司 | 视觉定位方法、视觉定位装置、存储介质与电子设备 |
CN113311412A (zh) * | 2021-05-13 | 2021-08-27 | 哈尔滨工业大学 | 用于移动机器人的激光雷达传感器六维外参标定方法 |
CN113340293A (zh) * | 2021-05-28 | 2021-09-03 | 上海高仙自动化科技发展有限公司 | 一种定位方法、装置、电子设备及存储介质 |
CN113570708A (zh) * | 2021-07-30 | 2021-10-29 | 重庆市特种设备检测研究院 | 缺陷立体建模方法、装置及计算机可读存储介质 |
CN113580134A (zh) * | 2021-08-03 | 2021-11-02 | 湖北亿咖通科技有限公司 | 视觉定位方法、设备、机器人、存储介质及程序产品 |
CN114216482A (zh) * | 2021-12-14 | 2022-03-22 | Oppo广东移动通信有限公司 | 轨迹外参真值确定方法及装置、存储介质、电子设备 |
CN114485620A (zh) * | 2022-01-29 | 2022-05-13 | 中国科学院国家空间科学中心 | 融合轨道动力学的小行星探测器自主视觉定位系统及方法 |
CN114546125A (zh) * | 2022-04-27 | 2022-05-27 | 北京影创信息科技有限公司 | 键盘跟踪方法及跟踪系统 |
CN114935316A (zh) * | 2022-05-20 | 2022-08-23 | 长春理工大学 | 基于光学跟踪与单目视觉的标准深度图像生成方法 |
CN115345943A (zh) * | 2022-08-08 | 2022-11-15 | 恩纳基智能科技无锡有限公司 | 一种基于差模概念的标定方法 |
WO2023060964A1 (zh) * | 2021-10-14 | 2023-04-20 | 上海商汤智能科技有限公司 | 标定方法及相关装置、设备、存储介质和计算机程序产品 |
CN116758237A (zh) * | 2023-08-17 | 2023-09-15 | 山东萌山钢构工程有限公司 | 一种基于实时建模的装配建筑监测系统 |
CN116883502A (zh) * | 2023-09-05 | 2023-10-13 | 深圳市智绘科技有限公司 | 相机位姿和路标点位置的确定方法、装置、介质及设备 |
CN117998032A (zh) * | 2024-01-04 | 2024-05-07 | 北京图知科技有限公司武汉分公司 | 基于实时slam技术的设计图纸和航拍视频叠加显示方法和系统 |
CN118096742A (zh) * | 2024-04-23 | 2024-05-28 | 鑫创鑫自动化设备科技(漳州)有限公司 | 一种基于角点检测和三维建模技术的贴片机吸嘴检测方法 |
CN118262336A (zh) * | 2024-05-30 | 2024-06-28 | 南昌智能新能源汽车研究院 | 一种基于视觉slam的室内停车场定位方法及系统 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20150212521A1 (en) * | 2013-05-23 | 2015-07-30 | Irobot Corporation | Simultaneous Localization And Mapping For A Mobile Robot |
CN106767785A (zh) * | 2016-12-23 | 2017-05-31 | 成都通甲优博科技有限责任公司 | 一种双回路无人机的导航方法及装置 |
CN107085422A (zh) * | 2017-01-04 | 2017-08-22 | 北京航空航天大学 | 一种基于Xtion设备的多功能六足机器人的远程控制系统 |
CN107167140A (zh) * | 2017-05-26 | 2017-09-15 | 江苏大学 | 一种无人机视觉定位累积误差抑制方法 |
CN107180215A (zh) * | 2017-05-31 | 2017-09-19 | 同济大学 | 基于库位和二维码的停车场自动建图与高精度定位方法 |
CN107193279A (zh) * | 2017-05-09 | 2017-09-22 | 复旦大学 | 基于单目视觉和imu信息的机器人定位与地图构建系统 |
US20180112985A1 (en) * | 2016-10-26 | 2018-04-26 | The Charles Stark Draper Laboratory, Inc. | Vision-Inertial Navigation with Variable Contrast Tracking Residual |
-
2018
- 2018-05-29 CN CN201810532344.5A patent/CN108717712B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20150212521A1 (en) * | 2013-05-23 | 2015-07-30 | Irobot Corporation | Simultaneous Localization And Mapping For A Mobile Robot |
US20180112985A1 (en) * | 2016-10-26 | 2018-04-26 | The Charles Stark Draper Laboratory, Inc. | Vision-Inertial Navigation with Variable Contrast Tracking Residual |
CN106767785A (zh) * | 2016-12-23 | 2017-05-31 | 成都通甲优博科技有限责任公司 | 一种双回路无人机的导航方法及装置 |
CN107085422A (zh) * | 2017-01-04 | 2017-08-22 | 北京航空航天大学 | 一种基于Xtion设备的多功能六足机器人的远程控制系统 |
CN107193279A (zh) * | 2017-05-09 | 2017-09-22 | 复旦大学 | 基于单目视觉和imu信息的机器人定位与地图构建系统 |
CN107167140A (zh) * | 2017-05-26 | 2017-09-15 | 江苏大学 | 一种无人机视觉定位累积误差抑制方法 |
CN107180215A (zh) * | 2017-05-31 | 2017-09-19 | 同济大学 | 基于库位和二维码的停车场自动建图与高精度定位方法 |
Non-Patent Citations (2)
Title |
---|
M. RAMEZANI 等: "INDOOR POSITIONING BY VISUAL-INERTIAL ODOMETRY", 《ISPRS ANNALS OF THE PHOTOGRAMMETRY, REMOTE SENSING AND SPATIAL INFORMATION SCIENCES》 * |
陈常 等: "基于视觉的同时定位与地图构建的研究进展", 《计算机应用研究》 * |
Cited By (65)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109523595B (zh) * | 2018-11-21 | 2023-07-18 | 南京链和科技有限公司 | 一种建筑工程直线棱角间距视觉测量方法 |
CN109523595A (zh) * | 2018-11-21 | 2019-03-26 | 南京链和科技有限公司 | 一种建筑工程直线棱角间距视觉测量方法 |
CN111213364A (zh) * | 2018-12-21 | 2020-05-29 | 深圳市大疆创新科技有限公司 | 拍摄设备的控制方法、拍摄设备的控制装置及拍摄设备 |
WO2020124517A1 (zh) * | 2018-12-21 | 2020-06-25 | 深圳市大疆创新科技有限公司 | 拍摄设备的控制方法、拍摄设备的控制装置及拍摄设备 |
WO2020135183A1 (zh) * | 2018-12-29 | 2020-07-02 | 广州文远知行科技有限公司 | 点云地图的构建方法、装置、计算机设备和存储介质 |
CN109801379A (zh) * | 2019-01-21 | 2019-05-24 | 视辰信息科技(上海)有限公司 | 通用的增强现实眼镜及其标定方法 |
CN109801379B (zh) * | 2019-01-21 | 2023-02-17 | 视辰信息科技(上海)有限公司 | 通用的增强现实眼镜及其标定方法 |
CN109764880A (zh) * | 2019-02-19 | 2019-05-17 | 中国科学院自动化研究所 | 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统 |
CN109900704A (zh) * | 2019-03-12 | 2019-06-18 | 西安交通大学 | 在用齿轮磨损表面微观形貌原位三维重构方法 |
CN110135387A (zh) * | 2019-05-24 | 2019-08-16 | 李子月 | 一种基于传感器融合的图像快速识别方法 |
CN110135396A (zh) * | 2019-05-27 | 2019-08-16 | 百度在线网络技术(北京)有限公司 | 地面标志的识别方法、装置、设备及介质 |
CN112132888A (zh) * | 2019-06-25 | 2020-12-25 | 黑芝麻智能科技(重庆)有限公司 | 大规模室内稀疏激光雷达点云内的单目相机定位 |
CN112132888B (zh) * | 2019-06-25 | 2024-04-26 | 黑芝麻智能科技(重庆)有限公司 | 大规模室内稀疏激光雷达点云内的单目相机定位 |
CN110375739A (zh) * | 2019-06-26 | 2019-10-25 | 中国科学院深圳先进技术研究院 | 一种移动端视觉融合定位方法、系统及电子设备 |
CN112215884A (zh) * | 2019-07-09 | 2021-01-12 | 北京地平线机器人技术研发有限公司 | 一种平面标志物的位姿确定方法和装置 |
CN110458892A (zh) * | 2019-07-17 | 2019-11-15 | 北京自动化控制设备研究所 | 基于惯性导航的相机标定方法 |
CN110473258A (zh) * | 2019-07-24 | 2019-11-19 | 西北工业大学 | 基于点线统一框架的单目slam系统初始化算法 |
CN110473258B (zh) * | 2019-07-24 | 2022-05-13 | 西北工业大学 | 基于点线统一框架的单目slam系统初始化算法 |
CN112304321A (zh) * | 2019-07-26 | 2021-02-02 | 北京初速度科技有限公司 | 一种基于视觉和imu的车辆融合定位方法及车载终端 |
CN110517324A (zh) * | 2019-08-26 | 2019-11-29 | 上海交通大学 | 基于变分贝叶斯自适应算法的双目vio实现方法 |
CN110517324B (zh) * | 2019-08-26 | 2023-02-17 | 上海交通大学 | 基于变分贝叶斯自适应算法的双目vio实现方法 |
CN110647609B (zh) * | 2019-09-17 | 2023-07-18 | 上海图趣信息科技有限公司 | 视觉地图定位方法及系统 |
CN110647609A (zh) * | 2019-09-17 | 2020-01-03 | 上海图趣信息科技有限公司 | 视觉地图定位方法及系统 |
CN111028125A (zh) * | 2019-11-14 | 2020-04-17 | 天津大学 | 一种用于slam的已知自身位姿的光束平差法fpga加速器 |
CN111028125B (zh) * | 2019-11-14 | 2023-04-28 | 天津大学 | 一种用于slam的已知自身位姿的光束平差法fpga加速器 |
CN110942483B (zh) * | 2019-11-21 | 2023-05-23 | 三一重工股份有限公司 | 函数快速收敛模型构建方法、装置和终端 |
CN110942483A (zh) * | 2019-11-21 | 2020-03-31 | 三一重工股份有限公司 | 函数快速收敛模型构建方法、装置和终端 |
CN110992487B (zh) * | 2019-12-10 | 2020-09-29 | 南京航空航天大学 | 手持式飞机油箱快速三维地图重建装置及重建方法 |
CN110992487A (zh) * | 2019-12-10 | 2020-04-10 | 南京航空航天大学 | 手持式飞机油箱快速三维地图重建装置及重建方法 |
CN111156984A (zh) * | 2019-12-18 | 2020-05-15 | 东南大学 | 一种面向动态场景的单目视觉惯性slam方法 |
CN111145267A (zh) * | 2019-12-25 | 2020-05-12 | 重庆自行者科技有限公司 | 基于imu辅助的360度全景视图多相机标定方法 |
CN111145267B (zh) * | 2019-12-25 | 2023-09-05 | 重庆自行者科技有限公司 | 基于imu辅助的360度全景视图多相机标定方法 |
CN111588315B (zh) * | 2020-06-19 | 2021-05-28 | 苏州亿倍智能清洁股份有限公司 | 一种扫地机陀螺仪角度修正方法及扫地机 |
CN111588315A (zh) * | 2020-06-19 | 2020-08-28 | 苏州亿倍智能清洁股份有限公司 | 一种扫地机陀螺仪角度修正方法及扫地机 |
CN111739359A (zh) * | 2020-06-30 | 2020-10-02 | 上海乂学教育科技有限公司 | 增强现实的课件生成系统 |
CN111913499A (zh) * | 2020-07-17 | 2020-11-10 | 天津大学 | 基于单目视觉slam及深度不确定性分析的云台控制方法 |
CN111913499B (zh) * | 2020-07-17 | 2023-11-14 | 天津大学 | 基于单目视觉slam及深度不确定性分析的云台控制方法 |
CN111932637A (zh) * | 2020-08-19 | 2020-11-13 | 武汉中海庭数据技术有限公司 | 一种车身相机外参自适应标定方法和装置 |
CN112288813A (zh) * | 2020-11-03 | 2021-01-29 | 浙江大学 | 基于多目视觉测量与激光点云地图匹配的位姿估计方法 |
CN112489091B (zh) * | 2020-12-18 | 2022-08-12 | 湖南华南光电(集团)有限责任公司 | 一种基于直瞄模板的全捷联图像导引头目标跟踪方法 |
CN112489091A (zh) * | 2020-12-18 | 2021-03-12 | 湖南华南光电(集团)有限责任公司 | 一种基于直瞄模板的全捷联图像导引头目标跟踪方法 |
CN112785655A (zh) * | 2021-01-28 | 2021-05-11 | 中汽创智科技有限公司 | 一种基于车道线检测的环视相机外参自动标定方法、装置、设备及计算机存储介质 |
CN112669196A (zh) * | 2021-03-16 | 2021-04-16 | 浙江欣奕华智能科技有限公司 | 一种硬件加速引擎中因子图优化数据的方法和设备 |
CN113096185A (zh) * | 2021-03-29 | 2021-07-09 | Oppo广东移动通信有限公司 | 视觉定位方法、视觉定位装置、存储介质与电子设备 |
CN113311412A (zh) * | 2021-05-13 | 2021-08-27 | 哈尔滨工业大学 | 用于移动机器人的激光雷达传感器六维外参标定方法 |
CN113311412B (zh) * | 2021-05-13 | 2024-03-01 | 哈尔滨工业大学 | 用于移动机器人的激光雷达传感器六维外参标定方法 |
CN113340293A (zh) * | 2021-05-28 | 2021-09-03 | 上海高仙自动化科技发展有限公司 | 一种定位方法、装置、电子设备及存储介质 |
CN113570708A (zh) * | 2021-07-30 | 2021-10-29 | 重庆市特种设备检测研究院 | 缺陷立体建模方法、装置及计算机可读存储介质 |
CN113580134A (zh) * | 2021-08-03 | 2021-11-02 | 湖北亿咖通科技有限公司 | 视觉定位方法、设备、机器人、存储介质及程序产品 |
CN113580134B (zh) * | 2021-08-03 | 2022-11-04 | 亿咖通(湖北)技术有限公司 | 视觉定位方法、设备、机器人、存储介质及程序产品 |
WO2023060964A1 (zh) * | 2021-10-14 | 2023-04-20 | 上海商汤智能科技有限公司 | 标定方法及相关装置、设备、存储介质和计算机程序产品 |
CN114216482A (zh) * | 2021-12-14 | 2022-03-22 | Oppo广东移动通信有限公司 | 轨迹外参真值确定方法及装置、存储介质、电子设备 |
CN114485620A (zh) * | 2022-01-29 | 2022-05-13 | 中国科学院国家空间科学中心 | 融合轨道动力学的小行星探测器自主视觉定位系统及方法 |
CN114485620B (zh) * | 2022-01-29 | 2023-07-28 | 中国科学院国家空间科学中心 | 融合轨道动力学的小行星探测器自主视觉定位系统及方法 |
CN114546125A (zh) * | 2022-04-27 | 2022-05-27 | 北京影创信息科技有限公司 | 键盘跟踪方法及跟踪系统 |
CN114935316A (zh) * | 2022-05-20 | 2022-08-23 | 长春理工大学 | 基于光学跟踪与单目视觉的标准深度图像生成方法 |
CN115345943B (zh) * | 2022-08-08 | 2024-04-16 | 恩纳基智能装备(无锡)股份有限公司 | 一种基于差模概念的标定方法 |
CN115345943A (zh) * | 2022-08-08 | 2022-11-15 | 恩纳基智能科技无锡有限公司 | 一种基于差模概念的标定方法 |
CN116758237B (zh) * | 2023-08-17 | 2023-10-20 | 山东萌山钢构工程有限公司 | 一种基于实时建模的装配建筑监测系统 |
CN116758237A (zh) * | 2023-08-17 | 2023-09-15 | 山东萌山钢构工程有限公司 | 一种基于实时建模的装配建筑监测系统 |
CN116883502B (zh) * | 2023-09-05 | 2024-01-09 | 深圳市智绘科技有限公司 | 相机位姿和路标点位置的确定方法、装置、介质及设备 |
CN116883502A (zh) * | 2023-09-05 | 2023-10-13 | 深圳市智绘科技有限公司 | 相机位姿和路标点位置的确定方法、装置、介质及设备 |
CN117998032A (zh) * | 2024-01-04 | 2024-05-07 | 北京图知科技有限公司武汉分公司 | 基于实时slam技术的设计图纸和航拍视频叠加显示方法和系统 |
CN118096742A (zh) * | 2024-04-23 | 2024-05-28 | 鑫创鑫自动化设备科技(漳州)有限公司 | 一种基于角点检测和三维建模技术的贴片机吸嘴检测方法 |
CN118262336A (zh) * | 2024-05-30 | 2024-06-28 | 南昌智能新能源汽车研究院 | 一种基于视觉slam的室内停车场定位方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
CN108717712B (zh) | 2021-09-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108717712A (zh) | 一种基于地平面假设的视觉惯导slam方法 | |
CN110296691B (zh) | 融合imu标定的双目立体视觉测量方法与系统 | |
CN111156998B (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
CN110070615B (zh) | 一种基于多相机协同的全景视觉slam方法 | |
CN105716542B (zh) | 一种基于柔性特征点的三维数据拼接方法 | |
CN100417231C (zh) | 立体视觉半实物仿真系统及方法 | |
CN109238235B (zh) | 单目序列图像实现刚体位姿参数连续性测量方法 | |
CN104268935A (zh) | 一种基于特征的机载激光点云与影像数据融合系统及方法 | |
CN105469389B (zh) | 一种用于视觉传感器标定的网格球靶标及相应标定方法 | |
CN108846857A (zh) | 视觉里程计的测量方法及视觉里程计 | |
CN108170297B (zh) | 实时六自由度vr/ar/mr设备定位方法 | |
CN108615244A (zh) | 一种基于cnn和深度滤波器的图像深度估计方法及系统 | |
CN108917753A (zh) | 基于从运动恢复结构的飞行器位置确定方法 | |
CN111462241B (zh) | 一种基于单目视觉的目标定位方法 | |
CN108362205A (zh) | 基于条纹投影的空间测距方法 | |
CN106296718A (zh) | 基于立体视觉导航系统的相机阵列快速标定方法 | |
Basu | Active calibration of cameras: theory and implementation | |
CN113034571A (zh) | 一种基于视觉—惯性的物体三维尺寸测量方法 | |
CN114529585A (zh) | 基于深度视觉和惯性测量的移动设备自主定位方法 | |
CN112712566B (zh) | 基于结构参数在线校正的双目立体视觉传感器测量方法 | |
Junejo et al. | Autoconfiguration of a dynamic nonoverlapping camera network | |
CN112907633A (zh) | 动态特征点识别方法及其应用 | |
Chen et al. | Low cost and efficient 3D indoor mapping using multiple consumer RGB-D cameras | |
CN115830116A (zh) | 一种鲁棒视觉里程计方法 | |
Ross et al. | Vision-based target geolocation and optimal surveillance on an unmanned aerial vehicle |
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 |