视觉slam十四讲 pdf_视觉slam第七讲学习笔记
七八讲的主要内容是视觉里程计（VO），属于前端技术领域。开始学习时会感到不太适应。由于这部分描述得不够深入，在讲解算法的基本原理时仅做简要介绍。其核心特性并未展开讲解。如需深入理解其中的原理，则需自行查阅相关资料。目前尚未着手进行相关研究工作，请耐心等待后续内容更新。
代码本质上也是一大堆格式化的指令；类似于学习神经网络的基础知识；了解函数的基本使用方法；而编写这个框架的基本结构则不需要过多复杂的解释；特别需要注意的地方比较少。
1.feature_extraction.cpp中的一些内容
1.1.main函数的参数
从argc里可以判断参数个数，argv里可以找到对应的字符串。
if ( argc != 3 ) // argc=3表示除了程序名外还有2个参数。argv[0][1][2]指向输入的程序路径及名称,参数para_1字符串,参数para_2字符串.
{ // 程序名加两张图像一共三个,argv[1]是"1.png"字符串
cout<<"usage: feature_extraction img1 img2"<<endl;
return 1;
}1.2.图像读取函数的参数
Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );第一个argv[1]是图像的名称，因为同目录，所以直接名称就行。
第二个参数是读取的内容，一共有五种：
- CV_LOAD_IMAGE_UNCHANGED – 每个像素的深度为8 bits，并且各色通道的数量保持不变。
- CV_LOAD_IMAGE_GRAYSCALE – 深度设置为8 bits且仅有一个颜色组件。
- CV_LOAD_IMAGE_COLOR - 在未指定期素深度的情况下包含三个颜色组件。
- CV_LOAD_IMAGE_ANYDEPTH – 指定了像素深度但未指定期素的颜色数目。
- CV_LOAD_IMAGE_ANYCOLOR – 在未指定期素深度的情况下确定了颜色数量。
1.3.特征点绘制的函数drawKeypoints
drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT );我对之前使用的JS编写的小接口记忆犹新。这个函数非常实用，并且我对它的前三个参数理解得非常清晰：第一个参数对应的是图像路径名；第二个参数对应的是缩放比例；第三个参数对应的是裁剪区域坐标值。第四个参数是color参数：在这里使用-1即代表调用默认颜色；第五个标志稍微复杂一些：具体共有四种不同的选择：分别是cv2和cv3含义相同但实现方式有所区别
- cv2.DRAW_MATCHES_FLAGS_DEFAULT：生成一个目标图，并基于该图显示匹配结果与关键点多边形。该过程仅在关键点处显示中间标记
- cv2.DRAW_MATCHES_FLAGS_DRAW_OVER_OUTIMG：无需生成输出图像矩阵即可实现，在目标图上展示匹配结果
- cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS：生成包含大小与方向信息的详细关键点标记
- cv2.DRAW_MATCHES_FLAGS_NOT_DRAW_SINGLE_POINTS：不在结果中显示孤立特征点
1.4.一点小问题
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
vector<DMatch> matches;
//BFMatcher matcher ( NORM_HAMMING );
matcher->match ( descriptors_1, descriptors_2, matches );在使用matcher->match时产生了红色下划线提示，在调用'set_match'函数时存在歧义性问题。经查阅发现问题源于函数重载导致与原有功能的冲突但未提供明确的解决方案尽管运行时未出现错误提示信息（如有），但操作界面仍需进一步优化以提升用户体验。
1.5.仅供娱乐的写法
// 仅供娱乐的写法
min_dist = min_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance;
max_dist = max_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distanc仅仅是为了休闲活动。。。。等效于前面那种遍历方式表述得相当清晰。
1.6.drawMatches
代码中使用的drawMatches参数数量有限，并且其含义非常清楚。其余参数则采用了默认设置，在实际操作中相对麻烦。
// Draws matches of keypints from two images on output image.
void drawMatches( const Mat& img1, const vector<KeyPoint>& keypoints1,
const Mat& img2, const vector<KeyPoint>& keypoints2,
const vector<vector<DMatch> >& matches1to2, Mat& outImg,
const Scalar& matchColor=Scalar::all(-1), const Scalar& singlePointColor=Scalar::all(-1),
const vector<vector<char> >& matchesMask=vector<vector<char> >(), int flags=DrawMatchesFlags::DEFAULT );具体含义如下：
- image1为原始图像
- keypoints1代表原始图像中的关键点位置
- image2为另一幅待处理图像
- keypoints2表示待处理图像中的关键点坐标
- matches1to2记录了两个图像之间的关键点对应关系
- output image generation relies on flags设置
- matchColor定义了特征点配对的颜色逻辑
当matchColor被设置为-1时，默认随机分配颜色 - singlePointColor用于指定未成功配对的关键点显示颜色
当matchColor被设置为-1时，默认随机分配颜色 - matchesMask是一个掩模矩阵，在实际应用中会根据需要选择是否显示特定匹配结果
- flags参数决定了最终输出图片的具体生成方式
原文链接：
OpenCV学习笔记：drawMatches函数的功能解析
2.对极几何
2.1.理论部分总结
第7章最初讲述的是特征点提取与匹配的过程，在这一部分仅作快速掠过处理；然而，在极几何方面则进行了深入阐述，并未展开具体推导过程；最后只是简单梳理了一下整个流程。
2D-2D：对极几何旨在利用两张图片（准确地提取并配对正确的特征点）来恢复相机在两个连续帧之间的运动参数。
首先我们需要回顾一下相机模型的内容。
其中P是目标点在相机参考系下的坐标，而
是像素平面下的坐标。
经过推导，我们得到两帧图像
中一对正确匹配的特征点
(指的是像素平面下的坐标)满足
对极约束 如下：（
是归一化平面上的坐标）
其中
称
本质矩阵 ，
称为
基础矩阵 ，实践中往往采用形式更简单的本质矩阵。
由于对极约束右端项为零时, 换取E的常数倍值不会改变结果（并称此情况下,E在不同尺度下具有等价性）。因此, 我们只需选取八个对应点即可唯一确定基本矩阵——这即所谓的八点法。若对应点数量超过八个, 则构成超定系统方程组, 解决方法通常采用最小二乘法或随机采样一致性(RANSAC)算法。
而求解得到E后，在基于本质矩阵的定义下进行奇异值分解操作，则能够恢复出相机的姿态和平移参数t与R；从而达到了我们的目标。
另：单应矩阵H
如果场景中的特征点都落在平面
上，与上述对极约束相类似，我们有如下式子成立：
,其中
。通过八点法同样可以计算H，然后分解计算出平移运动t和旋转运动R。
单应矩阵的作用体现在：当特征点位于同一平面或相机仅进行纯旋转时，在这种情况下基础矩阵的有效自由度会降低（即出现降维现象）。使用八点算法可能导致模型对噪声数据过度拟合。因此为了得到更可靠的解决方案我们不仅估计F矩阵还估计H矩阵并选择重投影误差最小的那个作为最终结果。
2.2.代码部分
直接运行pose_estimate _2d2d.cpp报了错
terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(3.4.3) /home/yangyi/下载/opencv-3.4.3/modules/calib3d/src/fundam.cpp:782: error: (-5:Bad argument)
The input arrays should be 2D or 3D point sets in function 'findFundamentalMat'The issue lies in the data types of the input arrays within the function 'findFundamentalMat', which requires them to be either 2D or 3D point sets. Attempts to search online for a corresponding bug yielded no results. Upon examining the code, compiler errors were flagged at two locations, with one of them being
调用'match'存在歧义。上一次在特征匹配时也标记出来,然而似乎并未影响运行状态。这次对函数进行了封装处理后未发现变化,因此问题可能并非如此。
matcher->match ( descriptors_1, descriptors_2, match );数据类型'Mat'与'int'之间存在不兼容性。该问题经常在程序中出现，并且通常是由它引发的。
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );网上有一个帖子提到了这个问题
ubuntu Clion报莫名其妙的错误，红线了依然能够编译tieba.baidu.com
Mat K ;
K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );按照这样的方法，确实不标红线了，但是实际运行起来依然报错没有改变。
。。。。。。
这里需要一个大大的表情包，我是个憨批。。。。。。
我在某处的图片名称输入有误，造成了之前遇到的问题。将1.png误写为img1.png后引发了这一问题。误以为是Run的结果导致了这一情况。修正后问题得以解决。之前采用的消除红色线条的方法仍然有效。随后更换为第二版电子书。
代码整体的结构是主函数通过调用find_feature_matches（与原有功能相同，并进行了封装处理）识别并配准特征点后进行计算运动。
除此之外，在研究过程中作者进行了两次验证工作。第一项是检验所估计出的旋转矩阵R和平移向量t是否满足本质矩阵的定义要求；第二项则是对极约束的验证工作。在这一过程中作者采用了pixel2cam工具将图像空间中的像素坐标转换为相机归一化坐标进行处理
转归一化坐标的依据就是
，故归一化坐标
.
Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
return Point2d
(
( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
);
}在pose_estimation_2d2d函数内部，其中涉及的计算均为标准流程，无需过多记忆。而前几个步骤则需要重点说明。
for ( int i = 0; i < ( int ) matches.size(); i++ )
{
points1.push_back ( keypoints_1[matches[i].queryIdx].pt );
points2.push_back ( keypoints_2[matches[i].trainIdx].pt );
}matches[i].queryIdx记录了第一条图片上的匹配点索引(matches[i].queryIdx记录了第一条图片上的匹配点索引)，而matches[i].trainIdx对应的是第二幅图片(matches[i].trainIdx对应的是第二幅图片)。在验证这对极约束的有效性过程中也采用了类似的处理方式(the process of validating the epipolar constraint also adopted a similar approach)。具体而言(具体而言)，验证这对极约束就是将提取到的关键点代入该式的左边进行计算(for the purpose of validating this epipolar constraint, the extracted corresponding points were substituted into the equation's left side)。实验结果表明(in the experimental results)，在大多数情况下这些关键点满足等式成立的条件(即值接近于零)，但在少数几个案例中出现了约1至10像素的偏差(the majority of cases showed good agreement with the theoretical predictions))。总体来看(总体来看)，这种方法的效果令人满意(the overall performance of this method is quite satisfactory)
使用OpenCV提取关键点坐标的方法是什么？
3.三角测量
SLAM的主要目标之一是精确估计相机的姿态和位置，并且构建全局地图。在二维到二维的立体几何模型中，在连续帧之间成功地建立了相对运动模型。随后需要利用这些运动信息来计算特征点在三维空间中的具体坐标。这可以通过三角测量方法实现。
由前述可知
，由于
，故有下面式子成立：
,已知R和t求
。
最基础的是左乘
得：
解
，然后解
。更常见的是求
的最小二乘解。
这一部分主要学习三角测量的函数即可
Mat pts_4d;
cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );cv::triangulatePoints共有五个参数：
前两个变量代表两个相机的姿态（均为3×4矩阵）。基于此，在我们当前的问题中，默认第一个相机被视为基准坐标系，则其姿态即为。
,第二个为
,这样的位姿为紧凑格式，被称为
扩展矩阵；而不是说使用4×4的转换矩阵来表示。其中中间两个参数用于归一化坐标；而第五个位置则是输出的位置。
核心在于需要明确该函数的结果到底是什么。直接查阅是否存在相关资料后, 从而可以通过后续操作来进一步了解其功能机制：
// 转换成非齐次坐标
for ( int i=0; i<pts_4d.cols; i++ )
{
Mat x = pts_4d.col(i);
x /= x.at<float>(3,0); // 归一化
Point3d p (
x.at<float>(0,0),
x.at<float>(1,0),
x.at<float>(2,0)
);
points.push_back( p );
}在主函数的调用中，我们知道p[i]表示第i个点在标准坐标系下的坐标信息。具体来说，我们可以通过p[i].x、p[i].y、p[i].z三个分量来获取该点的空间位置数据。当采用第一个相机作为基准时，对应的即为第一个相机坐标系下的空间位置信息
。
所谓的是通过比较三角化所得坐标与原始归一化点坐标间的偏差来确认重投影关系的存在。这种基于第一个相机作为基准系统的方法下得出的Z值实际上代表了空间点至该相机平面的距离。特别提醒：若未将第一个摄像头选定为主机，则需依照第二个摄像头所采用的形式进行配准，并对深度信息进行额外校正。
//-- 验证三角化点与特征点的重投影关系
Mat K ;
K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
for ( int i=0; i<matches.size(); i++ )
{
Point2d pt1_cam = pixel2cam( keypoints_1[ matches[i].queryIdx ].pt, K );
Point2d pt1_cam_3d(
points[i].x/points[i].z,
points[i].y/points[i].z
);
cout<<"point in the first camera frame: "<<pt1_cam<<endl;
cout<<"point projected from 3D "<<pt1_cam_3d<<", d="<<points[i].z<<endl;
// 第二个图
Point2f pt2_cam = pixel2cam( keypoints_2[ matches[i].trainIdx ].pt, K );
Mat pt2_trans = R*( Mat_<double>(3,1) << points[i].x, points[i].y, points[i].z ) + t;
pt2_trans /= pt2_trans.at<double>(2,0);
cout<<"point in the second camera frame: "<<pt2_cam<<endl;
cout<<"point reprojected from second frame: "<<pt2_trans.t()<<endl;
cout<<endl;
}基于平移原理进行的几何定位技术被称为三角测量。如果仅进行纯旋转操作，则无法被称为典型的三角测量其局限性之一即为无法实现精确的距离测定。为了提高该技术的精度可采取两种不同的策略：第一种策略是优化图像分辨率设置这将导致整体计算开销明显提升；第二种策略则是适当扩大平移幅度但这种做法可能会使整体图像呈现出现较大变化从而影响特征识别和匹配过程因此在这一领域中存在着显著的技术挑战
4.3D-2D:PnP(Perspective-n-point)
PnP问题涉及已知n个三维空间点及其投影位置时求解相机姿态的问题。与经典的双视图（2D-2D）定位几何相比，PnP方法仅需三对对应点即可实现，并且不依赖于双视图约束条件，在姿态估计领域具有重要地位。三维坐标可以通过三角测量法或基于深度信息的方法得以确定。双目摄像头和基于深度信息的相机可以直接应用PnP算法进行姿态估计，而单目摄像头则需要先进行初始化步骤以获取必要的参考信息。
补充：单目相机初始化：单目视觉系统在尺度上存在不确定性，在进行相机初始化时，默认将两张原始图像在水平方向上的移动量视为基准单位（即1），通过此基准计算后续各位置坐标的过程即为初始化操作。通常采用左右平移的方式完成这一初始化过程。
4.1.直接线性变换(DLT)
考虑某个空间点P，写成齐次坐标形式：
，在
的归一化平面上其对应的特征点坐标为
,而我们前面提到的增广矩阵
恰好能实现这样的映射 ，即
。
为了简化表示，将T的行向量定义为
,用最后一行消去s可得两个约束。
每一个点贡献两个方程，则至少需要六个点来解T的所有分量。需要注意的是，在未考虑到旋转矩阵R的特殊性质时，所得结果只是一个普通矩阵；必须找到最合适的旋转矩阵来逼近左边那个3×3矩阵，并可采用QR分解方法
4.2.P3P
已知三个三维空间中的点A、B、C（这些点位于世界坐标系中而非相机坐标系）及其对应的二维图像位置a、b、c（其中c为第三个对应点），并包含一个用于验证的点对d
设
借助余弦定理可获得涉及x和y的两组二元二次方程，在这些方程中最多有四组可能的解；随后通过验证点对筛选出最优的一组解以确定A、B、C在相机坐标系中的具体位置；从而推导出物体的姿态变化。
在应用中面临的一个主要问题是，在这种情况下难以充分利用更多的配准点；当观测的点受到噪声污染或发生错误配准时，则会导致算法失效。针对这一缺陷提出了多种优化方案如EPnP与UPnP等；在实际应用中通常按照以下流程执行：首先通过P3P/EPnP方法估算出物体的姿态；随后运用最小二乘法对估计结果进行精确校正（Bundle Adjustment光束平差法BA）。
4.3.光束平差法Bundle Adjustment——大名鼎鼎的BA
将相机位姿与空间点位置均视为优化变量，并综合考虑其最优解。其中目标函数即为重投影误差（Reprojection error）。基于前面的知识可知，在某个观察点Pi处，则存在以下等式成立：
（左右两边必须进行齐次到非齐次形式的转换，并对右侧取前三个维度），由于存在误差关系的存在性导致等式通常无法严格成立；因此我们建立误差函数
,由于第三位恒为1，所以实际上误差只有两维。
为了更好地完成定位任务并实现最小化误差的目标，在优化过程中必须计算误差相对于优化变量的变化率。尽管如此，在实际应用中我们可以采用数值方法来计算这一变化率；然而，在能够推导出解析表达式的情况下，则更倾向于采用解析形式以提高计算效率与准确性。
在这里, 我们不再详细推导, 直接提供了导数解析形式. 由于误差维度为2, 位姿李代数维度为6, 因此对于位姿的导数是一个2 \times 6矩阵（特别地, 这里是对左扰动计算得到的），而点坐标的维度是3, 所以对于位置坐标的导数是一个2 \times 3矩阵.
4.4.求解PnP代码
主函数中的一段，让我蒙了很长一段时间，特别加上注释放出来。
for ( DMatch m:matches )
{
ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
// 两个小括号里面的是第m对匹配点中第一个图的点的y和x坐标,前面解释过,然后强制转成int型
// Mat.ptr<type>(row)[col]得到的是指向Mat第row+1行第col+1列元素而非指针,虽然是指针类
// ushort是因为一个通道的深度只有8bit,ushort已经被宏定义成unsigned short
if ( d == 0 ) // bad depth
continue;
float dd = d/5000.0; // 这里提示double和float可能有问题,但实际相当于隐式类型转换,应该问题不大
// 这个5000应该是一个自带的常数,就是深度图深度和实际的Z之间的一个比例吧
Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) ); // dd相当于相机坐标系下的Z,这个point3f是相机一坐标系下XYZ
pts_2d.push_back ( keypoints_2[m.trainIdx].pt ); // 这是图二归一化平面的坐标
}