标签:匹配 特征 机器人 像素 相机 智能 实验 图像 灰度
图像特征匹配、跟踪与相机运动估计实验
一、实验目的
在熟悉相机模型和点云图的基础上,深入理解图像特征点及其描述子概念,掌握常见特征点原理,编程实现双目图像中 ORB 特征点的提取和匹配方法。
运用 ICP 方法对匹配的 3D 特征点进行 SVD 分解和非线性优化,求解相机姿态运动。为实现连续的相机姿态解算,需要跟踪特征点在后一帧画面的位置,需掌握相较特征点法更为快速的光流特征跟踪原理,数学描述及其编程实现。
二、实验内容
1. g2o 库的安装与使用
2. ORB 特征点提取
3. 双目图像的位姿变换体验
4. ICP 法相机位姿估计
5. 光流法特征跟踪
三、实验设备
自带笔记本 PC 机摄像头/USB 摄像头,Ubuntu系统,OpenCV 开发库
四、实验原理
1. 图像特征点的性质已经几种常见特征点的检测原理
特征点是图像中具有代表性的部分,具有以下性质:
- 可重复性:相同的特征可以在不同的图像中找到。
- 可区别性:不同的特征有不同的表达。
- 高效率:同一图像中,特征点的数量应远小于像素的数量。
- 本地性:特征仅与一小片图像区域有关。
首先我们来介绍几种常见的特征点的原理。
FAST特征点:
FAST是一种角点,主要检测局部像素灰度变化明显的地方,以速度快著称。它的思想是:如果一个像素与邻域像素差别较大(过亮或过暗),那么它更可能是角点。
检测过程:
1. 在图像中选取一点像素p,假设它的亮度为Ip。
2. 设置一个亮度阈值T。
3. 以像素p为中心,选取半径为3的圆上的16个像素点。
4. 加入选取的圆上有连续N个点的额亮度大于(Ip+T)或(Ip-T),那么像素P就被认为是特征点(N通常取12,即FAST12。其他的常用的N的取值为9和11,分别被称为是FAST9和FAST11)。
5. 循环以上四步,对每一个像素执行相同的操作。
在FAST-12 算法中,为了更高效,可以添加一项预测试操作,以快速地排除绝大多数不是角点的像素。具体操作为,对于每个像素,直接检测邻域圆上的第1,5,9,13个像素的亮度。只有当这四个像素中有三个同时大于Ip + T 或小于Ip - T 时,当前像素才有可能是一个角点,否则应该直接排除。这样的预测试操作大大加速了角点检测。此外,原始的FAST 角点经常出现“扎堆”的现象。
其次,FAST角点不具有方向信息。而且,由于它固定取半径为3 的圆,存在尺度问题:远处看着像是角点的地方,接近后看可能就不是角点了。
ORB特征点:
ORB特征实质是一种改进的Fast角点,改进的地方主要有以下两点,分别对关键点和描述子进行了改进。特征描述子应该具有特性:对光照不敏感(亮度),具有尺度一致性(大小),旋转一致性(角度)等。在ORB中的尺度不变性可由构建图像金字塔解决,旋转不变性可由灰度质心法来实现。
关键点: Fast角点改进
ORB在原来的Fast角点的基础上改进,第一指定提取一定数量的角点,第二分别对Fast角点计算Harris响应值,然后选取前N个具有最大响应值的角点作为最终的角点集合。
BRIEF 描述子
BRIEF 是一种二进制描述子,它的描述向量由许多个 0 和 1 组成,这里的 0 和 1 编码了关键点附近两个像素(比如说 p 和 q)的大小关系:如果 p 比 q 大,则取 1,反之就取 0。
采用灰度质心法
质心是指以图像块灰度值作为权重的中心。其具体操作步骤如下:
1. 在一个小的图像块 B 中,定义图像块的矩为:
2. 通过矩可以找到图像块的质心:
3. 连接图像块的几何中心 O 与质心 C,得到一个方向向量 OC,于是特征点的方向可以定义为:
通过以上方法,FAST角点便具有了尺度与旋转的描述大大提升了它们在不同图像之间表述的鲁棒性。所以在ORB中,把这种改进后的FAST称为 Oriented FAST。
SIFT特征点(尺度不变特征变换):
SIFT描述子是关键点邻域高斯图像梯度统计结果的一种表示。每一个关键点拥有三个信息:位置、尺度以及方向。通过对关键点周围图像区域分块,计算块内梯度直方图,生成具有独特性的向量,这个向量是该区域图像信息的一种抽象,具有唯一性。
SIFT算法主要有以下几个步骤:
1. 高斯差分金字塔的构建
使用组和层的结构构建了一个具有线性关系的金字塔(尺度空间),这样可以在连续的高斯核尺度上查找图像的特征点;另外,它使用一阶的高斯差分来近似高斯拉普拉斯核,大大的减少了运算量。
2. 尺度空间的极值检测及特征点的定位
搜索上一步建立的高斯尺度空间,通过高斯差分来识别潜在的对尺度和旋转不变的特征点。但是在离散空间中,局部极值点可能并不是真正意义上的极值点,真正的极值点有可能落在离散点的间隙中,SIFT通过尺度空间DoG函数进行曲线拟合寻找极值点。
3. 特征方向赋值
基于图像局部梯度方向,分配给每个关键点位置一个或多个方向,后续的所有操作都是对于关键点的方向、尺度和位置进行变换,从而提供这些特征的不变性。
4. 特征描述子的生成
通过上面的步骤已经找到SIFT特征点的位置、方向和尺度信息,最后使用一组向量来描述特征点及其周围邻域像素的信息。
2. 求解相机运动的法DLT与ICP法
DLT方法(直接线性变换):
考虑某个空间点 P,它的齐次坐标为 P = (X; Y; Z; 1)T。在图像I1中,投影到特征点x1 = (u1; v1; 1)T(以归一化平面齐次坐标表示)。此时相机的位姿 R; t是未知的。与单应矩阵的求解类似,我们定义增广矩阵 [R|t] 为一个3×4的矩阵,包含了旋转与平移信息。我们把它的展开形式列写如下:
用最后一行把s 消去,得到两个约束:
为了简化表示,定义 T 的行向量:
于是有:
t 是待求的变量,可以看到每个特征点提供了两个关于t 的线性约束。假设一共有N 个特征点,可以列出线性方程组:
由于t 一共有12 维,因此最少通过六对匹配点,即可实现矩阵T 的线性求解,这种方法(也)称为直接线性变换(Direct Linear Transform,DLT)。当匹配点大于六对时,(又)可以使用SVD 等方法对超定方程求最小二乘解。
在DLT 求解中,我们直接将T 矩阵看成了12个未知数,忽略了它们之间的联系。因为旋转矩阵R ∈ SO(3),用DLT 求出的解不一定满足该约束,它是一个一般矩阵。平移向量比较好办,它属于向量空间。对于旋转矩阵R,我们必须针对DLT 估计的T 的左边3 × 3 的矩阵块,寻找一个最好的旋转矩阵对它进行近似。这可以由QR 分解完成[3, 48],相当于把结果从矩阵空间重新投影到SE(3) 流形上,转换成旋转和平移两部分。
这里的x1 使用了归一化平面坐标,去掉了内参矩阵K 的影响——这是因为内参K 在SLAM 中通常假设为已知。如果内参未知,那么我们也能用PnP去估计K; R; t 三个量。然而由于未知量的增多,效果会差一些。
ICP法(迭代最近点):
思想:
ICP算法能够使不同的坐标下的点云数据合并到同一个坐标系统中,首先是找到一个可用的变换,配准操作实际是要找到从坐标系1到坐标系2的一个刚性变换。
ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对,计算最优刚体变换,直到满足正确配准的收敛精度要求。
按照一定的约束条件,找到最邻近点(pi,qi),然后计算出最优匹配参数R和t,使得误差函数最小(无相机模型)。
计算流程:
假设给两个三维点集 X1 和 X2,ICP方法的配准步骤如下:
第一步,计算X2中的每一个点在X1 点集中的对应近点;
第二步,求得使上述对应点对平均距离最小的刚体变换,求得平移参数和旋转参数;
第三步,对X2使用上一步求得的平移和旋转参数,得到新的变换点集;
第四步,如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值,则停止迭代计算,否则新的变换点集作为新的X2继续迭代,直到达到目标函数的要求。
给定配对好的两组3D点,求其旋转和平移,可用迭代最近点(Iterative Closest Point,ICP求解)。
现在,想要找一个欧氏变换 R;t,使得:
这个问题可以用迭代最近点(Iterative Closest Point, ICP)求解。
和PnP 类似,ICP 的求解也分为两种方式:利用线性代数的求解(主要是SVD),以及利用非线性优化方式的求解(类似于Bundle Adjustment)。
SVD方法:
首先我们看以SVD 为代表的代数方法。根据前面描述的ICP 问题,我们先定义第i对点的误差项:
然后,构建最小二乘问题,求使误差平方和达到极小的R; t:
下面我们来推导它的求解方法。首先,定义两组点的质心:
随后,在误差函数中,我们作如下的处理:
注意到交叉项部分中,(pi - p - R (pi′ - p′)) 在求和之后是为零的,因此优化目标函数可以简化为:
仔细观察左右两项,发现左边只和旋转矩阵 R 相关,而右边既有 R 也有 t,但只和质心相关。只要我们获得了 R,令第二项为零就能得到 t。于是ICP 可以分为以下三个步骤求解:
1.计算两组点的质心位置 p; p′,然后计算每个点的去质心坐标:
2. 根据以下优化问题计算旋转矩阵:
3. 根据第二步的R,计算t:
可以看到,只要求出了两组点之间的旋转,平移量是非常容易得到的。所以我们重点关注R 的计算。展开关于R 的误差项,得:
注意到第一项和R 无关,第二项由于RT R = I,亦与R 无关。因此,实际上优化目标函数变为:
接下来,我们介绍怎样通过SVD 解出上述问题中最优的R,但是关于最优性的证明较为复杂。为了解R,先定义矩阵:
W 是一个3 × 3 的矩阵,对W 进行SVD 分解,得:
其中,Σ 为奇异值组成的对角矩阵,对角线元素从大到小排列,而U 和V 为正交矩阵。当W 满秩时,R 为:
解得 R 后,求解 t 即可。
3. 光流法基本原理
光流的概念是Gibson在1950年首先提出来的。它是空间运动物体在观察成像平面上的像素运动的瞬时速度,是利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来找到上一帧跟当前帧之间存在的对应关系,从而计算出相邻帧之间物体的运动信息的一种方法。一般而言,光流是由于场景中前景目标本身的移动、相机的运动,或者两者的共同运动所产生的。
简单来说,光流是空间运动物体在观测成像平面上的像素运动的“瞬时速度”。光流的研究是利用图像序列中的像素强度数据的时域变化和相关性来确定各自像素位置的“运动”。研究光流场的目的就是为了从图片序列中近似得到不能直接得到的运动场。
光流场是运动场在二维图像上的投影,而光流就是在图像灰度模式下,像素点的运动矢量。光流法技术的核心就是求解出运动目标的光流,即速度。根据视觉感知原理,客观物体在空间上一般是相对连续运动的,在运动过程中,投射到传感器平面上的图像实际上也是连续变化的。为此可以假设:瞬时灰度值不变,即灰度不变性原理。由此可以得到光流基本方程,灰度对时间的变化率等于灰度的空间梯度与光流速度的点积。此时需要引入另外的约束条件,从不同的角度引入约束条件,导致了不同的光流分析方法。光流的计算方法大致分为五类:基于匹配的方法、基于梯度的方法、基于频域的方法、基于相位的方法和神经动力学方法。
下面详细介绍一下在实际中经常使用的Lucas-Kanada算法。Lucas-Kanada即L-K光流法最初于1981年提出,该算法假设在一个小的空间邻域内运动矢量保持恒定,使用加权最小二乘法估计光流。由于该算法应用于输入图像的一组点上时比较方便,因此被广泛应用于稀疏光流场,L-K算法的提出是基于以下三个假设: 1. 亮度恒定不变。目标像素在不同帧间运动时外观上是保持不变的,对于灰度图像,假设在整个被跟踪期间,像素亮度不变。 2. 时间连续或者运动是“小运动”。图像运动相对于时间来说比较缓慢,实际应用中指时间变化相对图像中运动比例要足够小,这样目标在相邻帧间的运动就比较小。3. 空间一致。同一场景中同一表面上的邻近点运动情况相似,且这些点在图像上的投影也在邻近区域。
对于假设条件:
1. 亮度恒定,就是同一点随着时间的变化,其亮度不会发生改变。这是基本光流法的假定(所有光流法变种都必须满足),用于得到光流法基本方程;
2. 小运动,这个也必须满足,就是时间的变化不会引起位置的剧烈变化,这样灰度才能对位置求偏导(换句话说,小运动情况下我们才能用前后帧之间单位位置变化引起的灰度变化去近似灰度对位置的偏导数),这也是光流法不可或缺的假定;
3. 空间一致,一个场景上邻近的点投影到图像上也是邻近点,且邻近点速度一致。这是Lucas-Kanade光流法特有的假定,因为光流法基本方程约束只有一个,而要求x,y方向的速度,有两个未知变量。我们假定特征点邻域内做相似运动,就可以联立n多个方程求取x,y方向的速度(n为特征点邻域总点数,包括该特征点)。
在 LK 光流中,我们认为来自相机的图像是随时间变化的。图像可以看作时间的函数:I(t)。那么,一个在 t 时刻,位于 (x; y) 处的像素,它的灰度可以写成:
灰度不变假设:同一个空间点的像素灰度值,在各个图像中是固定不变的。
对于t 时刻位于(x; y) 处的像素,我们设t + dt 时刻,它运动到(x + dx; y + dy) 处。由于灰度不变,我们有:
对左边进行泰勒展开,保留一阶项,得:
因为我们假设了灰度不变,于是下一个时刻的灰度等于之前的灰度,从而
两边除以dt,得:
其中dx/dt 为像素在x 轴上运动速度,而dy/dt 为y 轴速度,把它们记为u; v。同时αI/αx为图像在该点处x 方向的梯度,另一项则是在y 方向的梯度,记为Ix; Iy。把图像灰度对时间的变化量记为It,写成矩阵形式,有:
我们想计算的是像素的运动 u; v,但是该式是带有两个变量的一次方程仅凭它无法计算出 u; v。因此,必须引入额外的约束来计算 u; v。在 LK 光流中,我们假设某一个窗口内的像素具有相同的运动。考虑一个大小为 w × w 大小的窗口,它含有 w² 数量的像素。由于该窗口内像素具有同样的运动,因此我们共有 w² 个方程:
记:
于是整个方程为:
这是一个关于 u; v 的超定线性方程,传统解法是求最小二乘解。最小二乘在很多时候都用到过:
这样就得到了像素在图像间的运动速度 u; v。当 t 取离散的时刻而不是连续时间时,我们可以估计某块像素在若干个图像中出现的位置。由于像素梯度仅在局部有效,所以如果一次迭代不够好的话,我们会多迭代几次这个方程。
五、实验代码分析
1. ORB特征点的提取和匹配代码分析
携带摄像头的机器人在运动过程中,会连续性地获取多帧图像,辅助其感知周围环境和自身运动。时间序列上相连的两幅或多幅图像,通常存在相同的景物,只是它们在图像中的位置不同。而位置的变换恰恰暗含了相机的运动,这时就需要相邻图像间的相似性匹配。
选取一大块图像区域进行运动估计是不可取的。已知图像在计算机内部是以数字矩阵的形式存储的,[如灰度图的每个元素代表了单个像素的灰度值]。而对于点的提取和匹配较为方便,且和数字值对应,所以引入图像特征点辅助估计相机的运动。
特征点组成:特征点包括关键点(Keypoint)和描述子(Descriptor)两部分。对特征点作处理时,涉及关键点的提取和描述子的计算。其中,关键点表示该特征点在图像中的位置、朝向、大小等信息;描述子通常为一个向量,描述该关键点周围像素的信息,是加以对比的信息。如果两个特征点的描述子向量之间的距离较小,则可以认为这两幅图像中的特征点是同一个。
特征匹配实际上描述的是连续的两张图像中景物的对应关系,以此来估计相机的运动。匹配方法有:
暴力匹配:即图像1中取特征点集合A,图像2中取特征点集合B,对A中的每一个特征点,依次与B中所有的特征点测量两者描述子的距离,然后排序,取距离最近的作为匹配的两个特征点。
这里,描述子距离表示了两张图片两个特征点之间的相似程度,在实际应用中可以取不同的距离度量范数。如:对于浮点类型,使用欧氏距离;对于二进制类型,使用汉明距离(即两个二进制串之间不同位数的个数)。
缺点:计算量随特征点数量增大呈递增趋势。
快速近似最近邻(FLANN):集成于OpenCV,适合于匹配点数量极多的情况。
双目图像中ORB特征点的提取和匹配代码分析(包含注释):
1 #include <iostream> 2 #include <opencv2/core/core.hpp> 3 #include <opencv2/features2d/features2d.hpp> 4 #include <opencv2/highgui/highgui.hpp> 5 6 using namespace std; 7 using namespace cv; 8 9 int main ( int argc, char** argv ) 10 { 11 if ( argc != 3 ) 12 { 13 cout<<"usage: feature_extraction img1 img2"<<endl; 14 return 1; 15 } 16 //-- 读取图像 17 Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ); 18 Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR ); 19 20 //-- 初始化 21 std::vector<KeyPoint> keypoints_1, keypoints_2; //声明存放两张图片各自关键点的向量 22 Mat descriptors_1, descriptors_2;//两张图片特征点的描述子,形式为向量 23 Ptr<FeatureDetector> detector = ORB::create(); 24 Ptr<DescriptorExtractor> descriptor = ORB::create(); 25 // Ptr<FeatureDetector> detector = FeatureDetector::create(detector_name); 26 // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create(descriptor_name); 27 Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" ); 28 //DescriptorMatcher特征匹配类,提供了一些特征点匹配的方法,该类主要包含图像对之间以及图像集之间的匹配 29 //匹配时计算描述子之间的距离使用汉明距离 30 31 // 第一步:检测 Oriented FAST 角点位置,使用detect方法,将其存入Keypoints变量中 32 detector->detect ( img_1,keypoints_1 ); 33 detector->detect ( img_2,keypoints_2 ); 34 35 // 第二步:根据角点位置计算 BRIEF 描述子,使用compute方法,将其存入descriptors变量中 36 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); 37 descriptor->compute ( img_2, keypoints_2, descriptors_2 ); 38 39 Mat outimg1; 40 drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); 41 imshow("ORB特征点",outimg1); 42 43 // 第三步:对两幅图像中的BRIEF描述子进行匹配,使用 Hamming 距离 44 vector<DMatch> matches;//DMatch类型的向量 45 //BFMatcher matcher ( NORM_HAMMING ); 46 matcher->match ( descriptors_1, descriptors_2, matches ); 47 //使用match方法,获取最匹配的两个特征点(匹配的特征点可以有多对,都存放在matche中) 48 49 // 第四步:匹配点对筛选 50 double min_dist=10000, max_dist=0; 51 52 //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离 53 /*descriptors的每行是一个特征点的描述子向量,图像1中有多少个特征点,matches中存储多少个匹配的图像2的特征点相关类型*/ 54 for ( int i = 0; i < descriptors_1.rows; i++ ) 55 { 56 double dist = matches[i].distance;//得出每对匹配的描述子之间的距离 57 if ( dist < min_dist ) min_dist = dist; 58 if ( dist > max_dist ) max_dist = dist; 59 } 60 61 // 仅供娱乐的写法 62 min_dist = min_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance; 63 max_dist = max_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance; 64 65 printf ( "-- Max dist : %f \n", max_dist ); 66 printf ( "-- Min dist : %f \n", min_dist ); 67 68 //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限. 69 std::vector< DMatch > good_matches; 70 for ( int i = 0; i < descriptors_1.rows; i++ ) 71 { 72 if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) ) 73 { 74 good_matches.push_back ( matches[i] ); 75 } 76 } 77 78 //-- 第五步:绘制匹配结果,数量分别为matches和good_matches 79 Mat img_match; 80 Mat img_goodmatch; 81 drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_match ); 82 drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch ); 83 imshow ( "所有匹配点对", img_match ); 84 imshow ( "优化后匹配点对", img_goodmatch ); 85 waitKey(0); 86 87 return 0; 88 }
2. ICP求解算法分析
SVD方法:
1 void pose_estimation_3d3d ( 2 const vector<Point3f>& pts1, 3 const vector<Point3f>& pts2, 4 Mat& R, Mat& t 5 ) 6 { 7 Point3f p1, p2; // center of mass 8 int N = pts1.size(); 9 for ( int i=0; i<N; i++ ) 10 { 11 p1 += pts1[i]; 12 p2 += pts2[i]; 13 } 14 p1 = Point3f( Vec3f(p1) / N); 15 p2 = Point3f( Vec3f(p2) / N); 16 vector<Point3f> q1 ( N ), q2 ( N ); // remove the center 17 for ( int i=0; i<N; i++ ) 18 { 19 q1[i] = pts1[i] - p1; 20 q2[i] = pts2[i] - p2; 21 } 22 23 // compute q1*q2^T 24 Eigen::Matrix3d W = Eigen::Matrix3d::Zero(); 25 for ( int i=0; i<N; i++ ) 26 { 27 W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose(); 28 } 29 cout<<"W="<<W<<endl; 30 31 // SVD on W 32 Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV ); 33 Eigen::Matrix3d U = svd.matrixU(); 34 Eigen::Matrix3d V = svd.matrixV(); 35 cout<<"U="<<U<<endl; 36 cout<<"V="<<V<<endl; 37 38 Eigen::Matrix3d R_ = U* ( V.transpose() ); 39 Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z ); 40 41 // convert to cv::Mat 42 R = ( Mat_<double> ( 3,3 ) << 43 R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ), 44 R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ), 45 R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 ) 46 ); 47 t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) ); 48 }
我们调用 Eigen 进行 SVD,然后计算 R; t矩阵。我们输出了匹配后的结果,不过请注意,由于前面的推导是按照 pi = Rp′ i + t 进行的,这里的 R; t 是第二帧到第一帧的变换,与前面 PnP 部分是相反的。所以在输出结果中,我们同时打印了逆变换。
非线性方法:
下面我们考虑用非线性优化来计算 ICP,使用李代数来表达相机位姿。与SVD 思路不同的地方在于,在优化中我们不仅考虑相机的位姿,同时会优化 3D 点的空间位置。对我们来说, RGB-D 相机每次可以观测到路标点的三维位置,从而产生一个3D 观测数据。不过,由于 g2o/sba 中没有提供 3D 到 3D 的边,而我们又想使用 g2o/sba 中李代数实现的位姿节点,所以最好的方式是自定义一种这样的边,并向 g2o 提供解析求导方式。这部分代码如下:
1 class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap> 2 { 3 public: 4 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 5 EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {} 6 7 virtual void computeError() 8 { 9 const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] ); 10 // measurement is p, point is p' 11 _error = _measurement - pose->estimate().map( _point ); 12 } 13 14 virtual void linearizeOplus() 15 { 16 g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]); 17 g2o::SE3Quat T(pose->estimate()); 18 Eigen::Vector3d xyz_trans = T.map(_point); 19 double x = xyz_trans[0]; 20 double y = xyz_trans[1]; 21 double z = xyz_trans[2]; 22 23 _jacobianOplusXi(0,0) = 0; 24 _jacobianOplusXi(0,1) = -z; 25 _jacobianOplusXi(0,2) = y; 26 _jacobianOplusXi(0,3) = -1; 27 _jacobianOplusXi(0,4) = 0; 28 _jacobianOplusXi(0,5) = 0; 29 30 _jacobianOplusXi(1,0) = z; 31 _jacobianOplusXi(1,1) = 0; 32 _jacobianOplusXi(1,2) = -x; 33 _jacobianOplusXi(1,3) = 0; 34 _jacobianOplusXi(1,4) = -1; 35 _jacobianOplusXi(1,5) = 0; 36 37 _jacobianOplusXi(2,0) = -y; 38 _jacobianOplusXi(2,1) = x; 39 _jacobianOplusXi(2,2) = 0; 40 _jacobianOplusXi(2,3) = 0; 41 _jacobianOplusXi(2,4) = 0; 42 _jacobianOplusXi(2,5) = -1; 43 } 44 45 bool read ( istream& in ) {} 46 bool write ( ostream& out ) const {} 47 protected: 48 Eigen::Vector3d _point; 49 };
这是一个一元边,观测量从 2维变成了 3 维,内部没有相机模型,并且只关联到一个节点。要注意这里雅可比矩阵的书写,它必须与我们前面的推导一致。雅可比矩阵给出了关于相机位姿的导数,是一个3 × 6 的矩阵。
3. 光流法求解运动代码分析
1 #include <iostream> 2 #include <fstream> 3 #include <list> 4 #include <vector> 5 #include <chrono> 6 using namespace std; 7 8 #include <opencv2/core/core.hpp> 9 #include <opencv2/highgui/highgui.hpp> 10 #include <opencv2/features2d/features2d.hpp> 11 #include <opencv2/video/tracking.hpp> 12 13 int main( int argc, char** argv ) 14 { 15 if ( argc != 2 ) 16 { 17 cout<<"usage: useLK path_to_dataset"<<endl; 18 return 1; 19 } 20 string path_to_dataset = argv[1]; 21 string associate_file = path_to_dataset + "/associate.txt"; 22 23 ifstream fin( associate_file ); 24 if ( !fin ) 25 { 26 cerr<<"I cann't find associate.txt!"<<endl; 27 return 1; 28 } 29 30 string rgb_file, depth_file, time_rgb, time_depth; 31 list< cv::Point2f > keypoints; // 因为要删除跟踪失败的点,使用list 32 cv::Mat color, depth, last_color; 33 34 for ( int index=0; index<100; index++ ) 35 { 36 fin>>time_rgb>>rgb_file>>time_depth>>depth_file; 37 color = cv::imread( path_to_dataset+"/"+rgb_file ); 38 depth = cv::imread( path_to_dataset+"/"+depth_file, -1 ); 39 if (index ==0 ) 40 { 41 // 对第一帧提取FAST特征点 42 vector<cv::KeyPoint> kps; 43 cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create(); 44 detector->detect( color, kps ); 45 for ( auto kp:kps ) 46 keypoints.push_back( kp.pt ); 47 last_color = color; 48 continue; 49 } 50 if ( color.data==nullptr || depth.data==nullptr ) 51 continue; 52 // 对其他帧用LK跟踪特征点 53 vector<cv::Point2f> next_keypoints; 54 vector<cv::Point2f> prev_keypoints; 55 56 for ( auto kp:keypoints ) 57 prev_keypoints.push_back(kp); 58 vector<unsigned char> status; 59 vector<float> error; 60 chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); 61 cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints, status, error ); 62 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 63 chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 ); 64 cout<<"LK Flow use time:"<<time_used.count()<<" seconds."<<endl; 65 // 把跟丢的点删掉 66 int i=0; 67 for ( auto iter=keypoints.begin(); iter!=keypoints.end(); i++) 68 { 69 if ( status[i] == 0 ) 70 { 71 iter = keypoints.erase(iter); 72 continue; 73 } 74 *iter = next_keypoints[i]; 75 iter++; 76 } 77 78 cout<<"tracked keypoints: "<<keypoints.size()<<endl; 79 if (keypoints.size() == 0) 80 { 81 cout<<"all keypoints are lost."<<endl; 82 break; 83 } 84 // 画出 keypoints 85 cv::Mat img_show = color.clone(); 86 for ( auto kp:keypoints ) 87 cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1); 88 cv::imshow("corners", img_show); 89 cv::waitKey(0); 90 last_color = color; 91 } 92 return 0; 93 }
六、实验运行结果
1.双目图像中ORB特征点的提取和匹配运行结果
我们看到未筛选的匹配中带有大量的误匹配。经过一次筛选之后,匹配数量减少了许多,但大多数匹配都是正确的。这里,我们筛选的依据是汉明距离小于最小距离的两倍,这是一种工程上的经验方法。不过,尽管在示例图像中能够筛选出正确的匹配,但我们仍然不能保证在所有其他图像中得到的匹配全是正确的。因此,在后面的运动估计中,还需要使用去除误匹配的算法。
接下来,我们可以根据匹配的点对,来估计相机的运动。这里由于相机的原理不同,情况也有一些不同,分为以下几种:
1. 当相机为单目时,我们只知道 2D 的像素坐标,因而问题是根据两组 2D 点估计运动。该问题用对极几何来解决。
2. 当相机为双目、 RGB-D 时,或者我们通过某种方法得到了距离信息,那问题就是根据两组 3D 点估计运动。该问题通常用 ICP 来解决。
3. 如果我们有 3D 点和它们在相机的投影位置,也能估计相机的运动。该问题通过 PnP求解。
2. ICP 法相机姿态估计运行结果
我们调用 Eigen 进行 SVD,然后计算 R; t矩阵。我们输出了匹配后的结果,不过请注意,由于前面的推导是按照 pi = Rp′ i + t 进行的,这里的 R; t 是第二帧到第一帧的变换,与前面 PnP 部分是相反的。所以在输出结果中,我们同时打印了逆变换。
在非线性优化中,我们发现只迭代一次后,总体误差就已经稳定不变,说明仅在一次迭代之后算法即已收敛。从位姿求解的结果可以看出,它和 SVD 给出的位姿结果几乎一模一样,这说明 SVD 已经给出了优化问题的解析解。所以,本实验中可以认为 SVD 给出的结果是相机位姿的最优值。
3. 光流特征跟踪运行结果
我们会在每次循环后暂停程序,按任意键可以继续运行。可以看到图像中大部分特征点能够顺利跟踪到,但也有特征点会丢失。丢失的特征点或是被移出了视野外,或是被其他物体挡住了。如果不提取新的特征点,那么光流的跟踪会越来越少。最初我们大约有 1700 个特征点。跟踪过程中一部分特征点会丢失,相机视角相对于最初的图像也发生了较大改变。仔细观察特征点的跟踪过程,我们会发现位于物体角点处的特征更加稳定。边缘处的特征会沿着边缘“滑动”,这主要是因为沿着边缘移动时特征块的内容基本不变,因此程序容易认为是同一个地方。
标签:匹配,特征,机器人,像素,相机,智能,实验,图像,灰度 来源: https://www.cnblogs.com/xm20200429/p/12817588.html
本站声明: 1. iCode9 技术分享网(下文简称本站)提供的所有内容,仅供技术学习、探讨和分享; 2. 关于本站的所有留言、评论、转载及引用,纯属内容发起人的个人观点,与本站观点和立场无关; 3. 关于本站的所有言论和文字,纯属内容发起人的个人观点,与本站观点和立场无关; 4. 本站文章均是网友提供,不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属;如您发现该文章侵犯了您的权益,可联系我们第一时间进行删除; 5. 本站为非盈利性的个人网站,所有内容不会用来进行牟利,也不会利用任何形式的广告来间接获益,纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。