视觉SLAM的基础知识

TheBasicsaboutVisualSLAM视觉SLAM的基础知识高翔（半闲居士）清华大学自动化系2016.7ROS暑期学校WhatpeoplethinkWhatIamactuallydoingRobotics提要1.视觉SLAM概述2.视觉里程计3.后端优化4.回环检测5.地图构建6.小结1.视觉SLAM概述1.视觉SLAM概述•SLAM:SimultaneousLocalizationandMapping•应用：机器人、无人机、AR/VR、智能硬件……1.视觉SLAM概述机器人的“内外兼修”内在了解自身位置WhereamI?外在了解周围环境WhereamI?精确的地图精确的位姿解决方式同时建模二者误差并优化1.视觉SLAM概述•如何确定位置与地图？——传感器•内部传感器（Interoceptive）•激光、视觉、IMU、轮子等•间接、受噪声影响大•外部传感器（Exteroceptive）•GPS、导轨、标志物等•直接、受噪声影响小•为何强调内部传感器？•对环境不存在假设——适用于任何环境1.视觉SLAM概述•激光传感器•精确•快速•计算量小•研究充分•体积大、功耗大•价格昂贵•视觉传感器•便宜•轻量•信息丰富•计算量大•对环境假设强•易受干扰•细分：单目、双目、RGB-D传感器SLAM的实现方式与难度，与传感器形式和安装方式有很大关联。1.视觉SLAM概述•视觉SLAM细分：单目双目RGB-D其他1.视觉SLAM概述单目成本低距离不受限尺度不确定性初始化问题双目计算深度距离不受限配置复杂计算量大RGB-D主动测深度重建效果好测量范围小受日光干扰受材质干扰1.视觉SLAM概述视觉SLAM过程1.视觉SLAM概述•经典SLAM数学表述•机器人在环境中运动，产生一条轨迹：•环境中存在许多路标点：•机器人从时刻运动到时刻：•在时刻观察到了路标，产生观测𝑘−1𝑘运动模型输入噪声𝑘𝑦𝑗𝑧𝑘𝑗观测模型观测点噪声1.视觉SLAM概述•SLAM数学模型：•视觉SLAM有何不同？•相机位姿是三维空间旋转和平移——李群与李代数•观测模型是空间点到像素的投影——相机模型•如何确定路标点和数据关联？——视觉里程计（前端）状态估计问题滤波器或非线性优化1.视觉SLAM概述•视觉SLAM框架1.视觉SLAM概述SLAMismorelikeaconceptthanasinglealgorithm.*•视觉里程计估算相机的帧间运动以及路标的位置•后端优化接受不同时刻视觉里程计测量的相机位姿，计算最大后验概率估计•回环检测判断机器人是否曾经到达过先前的位置•建图根据相机轨迹和图像，建立与任务要求对应的地图1.视觉SLAM概述•3D刚体运动•旋转矩阵/变换矩阵•3*3和4*4矩阵•角轴（旋转向量）•3*1向量•欧拉角•3*1向量，r-p-y角•直观•有奇异性•四元数•4*1向量，扩展的复数•完备，但不直观𝑝𝑤=𝑇𝑤𝑐𝑝𝑐=𝑅𝑤𝑐𝑡𝑤𝑐0𝑇1𝑝𝑐=𝑅𝑤𝑐𝑝𝑐+𝑡𝑤𝑐1.视觉SLAM概述•旋转/变换矩阵最为常用•但只有乘法，没有加法，代数结构上不够好，只成群•旋转/变换矩阵没有加法，无法计算微分•SLAM中需要微调相机位姿——李代数1.视觉SLAM概述相关资料[1]StateEstimationforRobotics:AMatrix-Lie-GroupApproach[2]Aninvitationto3Dcomputervision程序库：Eigen,Sophus,tf2.视觉里程计2.视觉里程计•视觉里程计的目的•根据图像，计算相机的帧间运动（ego-motion）•*估计特征点（路标点）大致空间位置•Motion&Strcutre•问题1.图像是怎么来的？2.特征点是怎么来的？3.如何根据特征点计算相机运动？4.如何估计特征点位置？2.视觉里程计•图像是怎么来的？——相机模型相似关系：2.视觉里程计•空间点到成像平面：•从成像平面到像素坐标•齐次坐标形式：2.视觉里程计•图像是怎么来的？——相机模型2.视觉里程计•图像是怎么来的？——相机模型•视觉观测模型空间点相机坐标系下的点成像平面下的点像素坐标𝑝𝑤𝑝𝑐=𝑅𝑝𝑤+𝑡𝑝=1𝑍𝑝𝑐𝑢𝑣1=𝐾𝑝𝑧=𝑢𝑣=1𝑍𝐷𝐾𝑅𝑝𝑤+𝑡𝐷=100010𝑧=𝐷𝐾𝑅𝑝+𝑡齐次坐标下：2.视觉里程计•视觉里程计基本问题•给定一组空间点在两个相机上的投影•计算两个相机之间的运动单目：只知道像素位置，没有深度RGB-D、双目：深度已知2D-2D：只根据两组像素位置，估计运动——对极几何3D-2D：已知空间位置和投影位置，估计运动——PnP3D-3D：已知两组空间位置，估计运动——ICP2.视觉里程计•2D-2D对极几何空间点P在两个相机中成像：去掉标定矩阵：整理：两边叉乘t：两边左乘𝑥1=𝐾𝑃𝑥2=𝐾𝑅𝑃+𝑡𝑦1=𝐾−1𝑥1,𝑦2=𝐾−1𝑥2𝑦2=𝑅𝑦1+𝑡𝑡×𝑦2=𝑡×𝑅𝑦1+𝑡×𝑡=𝑡×𝑅𝑦1𝑦2𝑇𝑦2𝑇(𝑡×𝑦2)=𝑦2𝑇𝑡×𝑅𝑦1=0对极约束：𝑦2𝑇𝑡×𝑅𝑦1=0几何意义：𝑂1𝑃,𝑂2𝑃,𝑡三个向量共面2.视觉里程计•对极约束：•一对点提供了一个对极约束•E有八个自由度（9个数，但乘任意常数等价）•单目相机存在尺度问题•最少提供八对点即可计算E——八点法•多于八对点时：最小二乘估计•获得E矩阵后，如何计算R,t?•SVD分解•由于，当t=0时，E为零，无法解出R•单目相机在纯旋转时无法估计运动𝑦2𝑇𝑡×𝑅𝑦1=0𝑡×𝑅≜𝐸称为Essential矩阵𝐸=𝑡×𝑅2.视觉里程计•单目相机的尺度不确定性•对极约束常用于单目相机初始化，设定t=1以固定尺度𝑦2𝑇𝐸𝑦1=0⇒𝑦2𝑇(𝑘𝐸)𝑦1=04m?40m?1.8m?2.视觉里程计•通过优化估计相机运动：BundleAdjustment•相机模型：•由于R,t不准确，上式不会精确成立，于是有误差：•求解R,t，使得误差最小化：𝑧=𝑢𝑣=1𝑍𝐷𝐾𝑅𝑝𝑤+𝑡𝑒=𝑧−1𝑍𝐷𝐾𝑅𝑝𝑤+𝑡𝑅,𝑡∗=argmin𝑖=1𝑁𝑒𝑖𝑇𝑒𝑖迭代策略•梯度下降•Newton法•Gauss-Newton法•Levenburg-Marquadt2.视觉里程计•BundleAdjustment•问题：梯度如何计算？•需要用李代数se(3)表达R,t，使用扰动模型•优点•通用性强，对于3D-2D,3D-3D问题都可用BA优化；（只要有观测模型即可建立误差项）•可以同时优化许多变量，包括相机位姿和特征点位置；•缺点•对初值敏感•对非凸问题，易落入局部极小𝑅,𝑡∗=argmin𝑖=1𝑁𝑒𝑖𝑇𝑒𝑖𝑒=𝑧−1𝑍𝐷𝐾𝑅𝑝𝑤+𝑡𝑒=𝑞−𝑅𝑝+𝑡PnP:ICP:2.视觉里程计•剩下的问题•特征点怎么来？——特征点提取算法（SIFT、ORB等）•特征提取算法会计算图像中的关键点（Key-Point），以及每个关键点对应的描述子（Descriptor）•关键点代表图像信息集中的部分•描述子能够区别每个关键点——可以用于匹配2.视觉里程计•基于特征点的视觉里程计•步骤:1.对图像提取特征点和描述子2.匹配当前图像与先前图像3.通过最小化重投影误差，计算相机运动：PnP,ICP•但是:1.特征提取很耗时2.特征提取并不一定成功，匹配也并不一定找的到3.可能存在误匹配视觉里程计主流算法2.视觉里程计•直接法•灰度不变假设•最小化灰度差（PhotometricError）•可以考虑所有的像素（但只有梯度明显的像素对优化有贡献）•可以计算稀疏、半稠密乃至稠密的结构•省去特征提取的时间𝑒=𝐼1𝑝1−𝐼2𝑝2=𝐼1𝑝1−𝐼21𝑍𝐾𝑅𝑃+𝑡例子：稀疏直接——SVO半稠密直接——LSD稠密直接——DVO2.视觉里程计Feature-basedMethods•Select100-1000representativepoints(orlines,planes)anddiscardtheothers.•Estimationthemotionfromthekey-points.•Trackthekey-pointsusingdescriptors.•Sparse•RobusttooutliersDirectmethods•Estimatethemotiondirectlyfrompixels.•Useallinformationfromimages.•Sparseisfast,butdenseisslow•Difficulttoremovetheoutliers•Needsgoodinitialization2.视觉里程计•计算特征点位置（Motion&Structure）三角化理论上，P应该在延长线交点处但受噪声影响，这两条线一般不相交•根据左图：•两边分别乘和：•可解出𝑂1𝑥1,𝑂2𝑥2𝑥1𝑇𝑥2𝑇𝑘1,𝑘2𝑃=𝑘1𝑥1=𝑘2𝑅𝑥2+𝑡𝑥1𝑇𝑥1𝑘1−(𝑥1𝑇𝑅𝑥2)𝑘2=𝑥1𝑇𝑡𝑥2𝑇𝑥1𝑘1−𝑥2𝑇𝑅𝑥2𝑘2=𝑥2𝑇𝑡𝑃∗=12𝑘1𝑥1+𝑘2𝑅𝑥2+𝑡但是，三角化之后的特征点深度是带有误差的。我们可以进一步使用后续图像来估计像素深度，引出DepthFilter。2.视觉里程计•视觉里程计小结•在视觉里程计中，我们：•提取、匹配了特征点•根据配对的特征点计算相机运动•估计特征点的空间位置•但是它们会：•带有噪声•随着误差累计，发生飘移•丢失（相机被遮挡或运动过快）Solutions•Globaloptimization•Loopclosure•Re-localization3.后端优化3.后端优化•回顾SLAM数学模型•视觉里程计：提取了关键帧、特征点，并提供了初值•后端：处理观测和运动模型之间的误差，计算最大后验概率估计Observation:Motion:Observation:𝑧=𝑢𝑣=1𝑍𝐷𝐾𝑅𝑝𝑤+𝑡𝑥∗=argmax𝑥𝑝𝑥|𝑧=argmax𝑝𝑧|𝑥𝑝𝑥3.后端优化•后端存在两种主流方法•基于滤波器：KalmanFilter,EKF,PF,RBPF,UKF•基于非线性优化：图优化、因子图•历史上，EKF曾占据很长一段时间的主流•EKF思路•在工作点线性化，将系统线性化，并用高斯分布近似其噪声•按照卡尔曼滤波进行预测——更新步骤•EKF存在的问题•即使是高斯分布，经过非线性变换后也不是高斯分布，近似有时是没道理的。•线性化过程中丢弃了高阶项，且工作点不是输入状态真实的均值，而是一个估计的均值。•需要更新状态的协方差矩阵，复杂度为从状态估计角度来说，SLAM是一个非线性非高斯系统𝑂𝑛23.后端优化•全局优化•转化成优化问题•与滤波器的差异•考虑所有时间的Pose和Point•局部线性化并迭代求解𝑒𝑘,𝑣=𝑥𝑘−𝑓𝑥𝑘,𝑢𝑘∼𝑁0,𝛴𝑘,𝑣𝑒𝑘𝑗,𝑜=𝑧𝑘,𝑗−ℎ𝑦𝑗,𝑥𝑘∼𝑁0,𝛴𝑘𝑗,𝑜3.后端优化•图优化•优化变量——节点•误差项——边•问题可以用一个图来描述•给定初值后，迭代更新•稀疏性•认识到SLAM中图优化的稀疏性是21世纪SLAM研究的重要进展•图优化中的位姿节点和路标节点可以先后优化，大幅提高优化效率•详细地稀疏性需要介绍稀疏Schur和Cholesky分解•工具：g2o,gtsam等G={V,E}3.后端优化Theloopconstraintscanhelptocorrectthedrift.3.后端优化•图优化中的问题•加入了错误的边怎么办？•使用鲁棒核函数•图的规模随着SLAM过程增长•剪枝、合并•大规模图优化很耗时•控制优