机器人视觉 课件 06第六章 机器人位姿估计

机器人视觉第六章机器人位姿估计本章内容安排1机器人位姿估计的概念与数学表示基于特征点法的视觉里程计基于直接法的视觉里程计关键帧的概念及其选取策略机器人位姿估计的概念与数学表示2位姿=位置+姿态相机与环境之间的关系机器人位姿估计的概念与数学表示3相机与环境之间的关系（按照运动与静止与否）划分依据相机静止相机运动环境目标静止环境目标运动目标识别：分类、检测、分割视频监控、行为分析视觉里程计、视觉SLAM目标追踪、视觉伺服机器人位姿估计的概念与数学表示4以下场景中的位姿估计划分依据相机静止相机运动环境目标静止目标在像素平面上的位姿估计相机在三维空间中的位姿估计环境目标运动目标在三维空间中的位姿估计目标与相机的相对位姿估计机器人位姿估计的概念与数学表示5本教材涉及的位姿估计运动相机的自运动估计（Ego-MotionEstimation）位姿估计的本质：运动的相对性利用相对于惯性系不变的视觉信息估计相机的姿态变化数学基础：三维空间中的刚体运动线性代数李群与李代数优化理论机器人位姿估计的概念与数学表示6位姿估计的数学表示

7视觉里程计分类像素的变化（光流）光强的变化（梯度）纹理特征语义特征可以在图像上直接计算，无需特征提取或描述子计算需要提取特征，并且区分不同的特征，需要描述子直接法特征点法特征点法VO视觉里程计的主流方法稳定，对光照、动态物体不敏感特征提取耗时、只利用了稀疏特征、对纹理信息依赖性强直接法VO通过最小化光度误差来建立图像帧之间的数据关联无需复杂的特征提取具有稠密、半稠密、稀疏需要光度一致性假设（假设偏强）有利于跨模态匹配与数据融合基于特征点的VO基于光度误差的VO本章内容安排8机器人位姿估计的概念与数学表示基于特征点法的视觉里程计基于对极几何的2D-2D位姿求解基于PnP的2D-3D位姿求解基于ICP的3D-3D位姿求解基于直接法的视觉里程计关键帧的概念及其选取策略位姿估计的主要方法9基于不同的数据关联方式估计相机的位姿：图像帧与帧之间的匹配关联：2D-2D的数据关联—对极几何地图和图像帧之间的匹配关联：3D-2D的数据关系—PnP（PerspectivenPoint）双目或RGB-D三维数据的匹配关联：3D-3D的数据关系—ICP（IterativeClosestPoint）对极几何ICPPnP2D-2D：对极几何10几何关系：P在两个图像的投影为两个相机之间的变换为

在第二个图像上投影为记，称为极线，反之亦然

称为极点问题描述：

通过特征匹配得到，P未知，未知

待求

2D-2D：对极几何11

世界坐标：以左相机坐标系为参考系，投影方程：使用归一化坐标：齐次关系：两侧左乘：再一步左乘：对极约束：带内参的形式：注：左右视图为同一相机拍摄，所以K相同2D-2D：对极几何12

对极约束刻画了共面的关系定义：Essential矩阵（本质矩阵）Fundamental矩阵（基础矩阵）在内参已知的情况下，可以使用E两步计算位姿：由匹配点计算E由E恢复R,t对极约束的性质：乘任意非零常数依然满足E当成普通矩阵的话，有八个自由度可用八点法求解

2D-2D：对极几何13八点法求E将E看成3x3的矩阵，去掉因子后剩八个自由度一对匹配点带来的约束：向量形式：八对点构成方程组根据以上线性方程组，可求解得到E2D-2D：对极几何14从E计算R,t：奇异值(SingularValueDecomposition，SVD）分解我们可以构造出四个可能的解其中U，V为正交阵，为奇异值矩阵将任意一点代入4种解，检测该点在两个相机下的深度2D-2D：对极几何15SVD过程中：一般取

因为E的内在性质要求它的奇异值为八点法的讨论用于单目视觉里程计的初始化尺度不确定性：归一化

t或特征点的平均深度纯旋转问题：t=0时无法求解多于八对点时：最小二乘有外点时：可使用RANSAC方法最少可使用五个点计算R和t，称为五点法为什么？需要利用E

的非线性性质（自学）（尺度等价性）RANSAC的基本思想和算法流程如下：随机采样K个点，K是求解模型参数的最少点个数；使用K个点估计模型参数；计算剩余点到估计模型的距离，距离小于阈值则为内点，统计内点的数目；重复步骤1~3，重复次数M且保留数目最多的内点；使用所有的内点重新估计模型。2D-2D：对极几何-单应矩阵16如果场景中的所有特征点共面摄像头朝向墙面无人机下视视觉观测到的地面扫地机器顶视摄像头观测天花板八点法在特征点共面时会退化设特征点位于某平面上：两个图像特征点的坐标关系：H矩阵称为单应矩阵2D-2D：对极几何-单应矩阵17实际处理中可以令这一项等于1去掉第三行得：H矩阵中有8个未知数，构造8个方程求解H通过4对点，可以构造8个线性方程一对点可以提供两个约束与本质矩阵分解类似，求解R和t通过单应矩阵H分解同样可以得到四组解通过地图点深度和场景平面法向量方向排除3组解2D-2D：对极几何18小结2D-2D情况下，只知道图像坐标之间的对应关系当特征点共面时（例如俯视或仰视），可求解单应矩阵H，并恢复R,t当特征点不共面时，可使用8点法或5点法求解E或F，并恢复R,t求得R,t后，即已知相机的运动，可以得到一个额外的信息：利用三角化计算特征点的3D位置（即深度）192D-2D：对极几何-三角化已知相机运动时，如何求解特征点的3D位置几何关系：，其中

和

为两个特征点的归一化坐标求时，两侧同时乘以

，可以得到：求时，同理。也可同时求解和可利用最小二乘来求解以下线性方程202D-2D：对极几何-三角化三角化中的问题：深度值求解精度与平移相关平移大时特征匹配可能不成功避免纯旋转（为什么？）三角测量的矛盾增大平移，导致匹配失效平移太小，测量精度不够特例：相机前进时，虽然有位移，但位于图像中心的点无法三角化（没有视差）213D-2D：PnP已知3D点的空间位置和相机上的投影点，求相机的旋转和平移（外参）代数的解法/优化的解法代数的DLTP3PEPnP/UPnP/…优化的：Bundle

Adjustment223D-2D：PnP-直接线性变换DLTDLT设空间点投影点为：投影关系：展开：

归一化坐标注意最下一行为用它消掉前两行中的s，则一个特征点可以提供两个约束方程：其中233D-2D：PnP-直接线性变换DLT将它看成一个关于

t的线性方程，求解t

为求解12个未知数，需要12/2=6对点当点超过6对时，出现超定情形，则需求最小二乘解DLT将R,t看成独立的未知量在求出结果后，需要将t组成的矩阵投影回SO(3)（通常用QR分解实现）243D-2D：PnP-P3PP3P问题举例：利用三对点求相机外参3D-2D对应关系如下根据余弦定理：已知条件：A、B、C的世界坐标，a，b，c的像素坐标求相机相对于世界坐标系的位姿253D-2D：PnP-P3P对上式两边同除以，并记可得：记，则：消去v，得到关于x,y的二次方程，可用吴氏消元法解此方程263D-2D：PnP-P3P得到x,y后，利用下式可计算出v解得v之后，从而可确定OC的长度OA，OB的长度也同理可得P3P缺点对三个点以上的情况难以处理误匹配时算法失效同时知道ABC在世界坐标系和相机坐标系中的坐标，可通过3D-3D求解R和t273D-2D：PnP-最小化投影误差PnP的优化解法：BundleAdjustment（光束平差或集束调整）最小化重投影误差（Minimizingareprojectionerror）投影关系：定义重投影误差并取最小化：因涉及到李群李代数，只介绍思想方法283D-3D：ICP给定配对好的两组3D点，求其旋转和平移，可用ICP求解假设有一组配对好的3D点：待求解问题：需要找一个旋转平移变换R，t，使得：我们可以定义在给定某个R，t下的匹配误差项：最小二乘问题构建：该公式中并没有相机模型我们如何来计算R和t呢？SVD方法非线性优化方法293D-3D：ICP-SVD方法SVD方法定义质心：改写目标函数：交叉项部分求和为零目标函数简化为：只和R有关和R,t都有关最小化左侧项，使右侧项为零可得t。303D-3D：ICP-SVD方法旋转R的求取定义去质心坐标后，最优旋转矩阵R*为：将R*展开：优化目标函数变为：SVD解法：与R无关如果此时R的行列式为负，则取-R为最优值[1]K.S.Arun,T.S.Huang,andS.D.Blostein,“Least-squaresfittingoftwo3-dpointsets,”PatternAnalysisandMachineIntelligence,IEEETransactionson,no.5,pp.698–700,1987.[2]F.Pomerleau,F.Colas,andR.Siegwart,“Areviewofpointcloudregistrationalgorithmsformobilerobotics,”FoundationsandTrendsinRobotics(FnTROB),vol.4,no.1,pp.1–104,2015.313D-3D：ICPICP也可以从非线性优化角度求解：已知一一匹配时，ICP问题存在唯一解或无穷多解的情况。在唯一解的情况下，只要能找到极小值解，那么这个极小值就是全局最优值。正常情况下，SVD结果和优化一样，且优化很快收敛。注：在激光点云情况下，匹配点未知，匹配数量不一致，将指定最近点为匹配点。只能通过优化求解，此时问题非凸，极小值不一定为最小值。3D点采样位置的变化，不存在R，t能实现两组3D点之间的完美匹配。利用非线性优化可以将ICP与PnP结合在一起求解。（即构建联合优化函数）本章内容安排32机器人位姿估计的概念与数学表示基于特征点法的视觉里程计基于直接法的视觉里程计光流估计直接法求解机器人位姿关键帧的概念及其选取策略33光流一般分为稀疏光流和稠密光流稀疏以Lucas-Kanade（LK）光流为代表稠密以Horn–Schunck（HS）光流为代表本质上是估计像素在不同时刻图像中的运动34光流设t时刻位于x,y处像素点的灰度值为在t+dt

时刻，该像素运动到了希望计算运动dx,

dy灰度不变假设：注意：灰度不变是一种理想的假设，实际当中由于高光/阴影/材质/曝光等不同，很可能不成立。提出假设是一种有用的研究方法论35光流对t+dt时刻的灰度进行Taylor展开并保留一阶项：由于灰度不变，所以希望求解dx/dt,

dy/dtx方向梯度y方向梯度像素灰度随时间变化量像素x方向速度像素x方向速度36光流但本式是一个二元一次线性方程，欠定需要引入额外的约束假定一个图像块（）内光度不变：含有数量的像素点满足上述方程通过超定最小二乘解求得运动u,v当t取离散的时刻，图像块可以作为一个整体来估计它在不同帧中的位置由于像素梯度仅在局部有效，如果一次迭代结果不够好，可以多迭代几次37光流注解：可以看成最小化像素误差的非线性优化每次使用了Taylor一阶近似，在离优化点较远时效果不佳，往往需要迭代多次运动较大时要使用金字塔可以用于跟踪图像中的稀疏关键点的运动轨迹光流仅估计了像素间的平移，但没有用到相机本身的几何结构没有考虑到相机的旋转和图像的缩放直接法则考虑了这些信息接下来我们将介绍直接法视觉里程计38直接法直接法的推导假设有两个帧，运动未知，但有初始估计R,t

第1帧上看到了点P，投影为p1按照初始估计，P在第2帧上投影为p2投影关系：39直接法为了估计相机的运动，建立最小化问题待估计的量为相机运动。是与相机变换T对应的李代数。最小化光度误差：关键在于推导误差相对于相机运动的导数40直接法推导过程41直接法第一部分：图像梯度第二部分：像素对相机坐标系下三维点的导数第三部分：投影点对位姿导数：综上：42直接法可以看到，直接法的雅可比项有一个图像梯度因子因此，在图像梯度不明显的地方，对相机运动估计的贡献就小根据使用的图像信息不同，可分为：稀疏直接法：只处理稀疏角点或关键点稠密直接法：使用所有像素半稠密直接法：使用部分梯度明显的像素43直接法直接法的直观解释像素灰度引导着优化的方向要使优化成立，必须保证从初始估计到最优估计中间的梯度一直下降这很容易受到图像非凸性的影响（可部分地由金字塔减轻）44直接法优缺点小结优势省略特征提取的时间只需有像素梯度而不必是角点（对白墙等地方有较好效果）可稠密或半稠密劣势灰度不变难以满足（易受曝光和模糊影响）单像素区分性差图像非凸性本章内容安排45机器人位姿估计的概念与数学表示基于特征点法的视觉里程计基于直接法的视觉里程计关键帧的概念及其选取策略46关键帧的概念关键帧关键帧指的是那些对于位姿估计和SLAM系统来说至关重要的图像帧。通常包含丰富且