视觉SLAM ch7代码总结（二）

目录

一、对极约束求解相机运动(2D-2D)

二、三角测量

三、求解PnP(3D-2D)

1.使用EPnP求解位姿

2.使用高斯牛顿法计算相机位姿

3.使用g2o进行BA优化 

四、求解ICP (3D-3D)

 1.SVD 方法

 2、非线性优化方法

---

一、对极约束求解相机运动(2D-2D)

1.特征提取与匹配函数

//特征提取和匹配
void find_feature_matches(
            const Mat &img_1,const Mat &img_2,
            vector &keypoints1, vector &keypoints2,
            vector &matches_good)

这里的代码和orb_cv中的一样

2.位姿估计函数

//位姿估计函数
void pose_estimation_2d2d(
            vector keypoints1,vector keypoints2,
            vector matches_good,Mat &R,Mat &t);

①Point2f

进行对极几何计算使用特征点的2D坐标，而此时特征点保存在两个KeyPoint对象的容器中，因此需要把坐标信息提取出来。 

keypoints1的数组大小 = descriptors1.rows = matches的数组大小 = 500； 

matches_good 数组大小是 81

这里是根据 matches_good 里queryIdx 和 trainIdx 的索引值找到筛选后的正确关键点位置，再把它的x、y坐标存储到Point2f中。取到的坐标是像素坐标。

queryIdx是图1中匹配的关键点的对应编号

trainIdx是图2中匹配的关键点的对应编号

pt可以把关键点的像素位置(x,y)取出来

②计算基础矩阵函数findFundamentalMat()

//寻找基础矩阵算法
enum
{
    FM_7POINT = CV_FM_7POINT, //!< 7-point algorithm
    FM_8POINT = CV_FM_8POINT, //!< 8-point algorithm
    FM_LMEDS = CV_FM_LMEDS,  //!< least-median algorithm
    FM_RANSAC = CV_FM_RANSAC  //!< RANSAC algorithm


CV_EXPORTS_W Mat findFundamentalMat( InputArray points1, InputArray points2,
                           int method = FM_RANSAC,
                           double ransacReprojThreshold = 3., double confidence = 0.99,
                           OutputArray mask = noArray() );

points1: 来自第一幅图像的N个2D点的数组。点坐标应为浮点型。

points2: 第二副图像的点的数组，格式、大小与第一幅图像相同。

method:计算基础矩阵的方法：七点法、八点法、LMedS算法、RANSAC算法

ransacReprojThreshold :该参数用于RANSAC算法（随机采样过程一致性），它是从点到对极线的最大距离（以像素为单位），超过该距离，该点被视为异常值，并且不用于计算最终的基础矩阵。 可以将其设置为1-3，具体取决于点定位的精度，图像分辨率和图像噪声。

confidence :该参数仅仅在RANSAC算法以及LMedS算法中， 它指定了估计矩阵正确的期望置信度（概率）

③计算本质矩阵函数findEssentialMat()

Mat cv::findEssentialMat 	( 	InputArray  	points1,
		InputArray  	points2,
		double  	focal = 1.0,
		Point2d  	pp = Point2d(0, 0),
		int  	method = RANSAC,
		double  	prob = 0.999,
		double  	threshold = 1.0,
		OutputArray  	mask = noArray() 
	) 	

points1: 来自第一幅图像的N个2D点的数组。点坐标应为浮点型。

points2: 第二副图像的点的数组，格式、大小与第一幅图像相同。

focal ：相机焦距

pp ：相机光心

method ：计算本征矩阵的方法:  RANSAC、LMedS算法

prob:参数仅用于RANSAC或LMedS方法。它规定了估计矩阵正确的理想置信水平（概率）。

threshold：用于RANSAC的参数。它是从一个点到一条外极线的最大距离（以像素为单位），超过该距离的点被视为异常值，不用于计算最终的基本矩阵。它可以设置为1-3，具体取决于点定位的精度、图像分辨率和图像噪声。

④计算单应性矩阵findHomography() 

Mat cv::findHomography 	( 	InputArray  	srcPoints,
		InputArray  	dstPoints,
		int  	method = 0,
		double  	ransacReprojThreshold = 3,
		OutputArray  	mask = noArray(),
		const int  	maxIters = 2000,
		const double  	confidence = 0.995 
	) 	

srcPoints：源平面中点的坐标矩阵

dstPoints：目标平面中点的坐标矩阵

method：计算单应矩阵所使用的方法。方法如下：

        0 - 利用所有点的常规方法
        RANSAC - RANSAC-基于RANSAC的鲁棒算法
        LMEDS - 最小中值鲁棒算法

ransacReprojThreshold：将点对视为内点的最大允许重投影错误阈值（仅用于RANSAC和RHO方法

mask：可选输出掩码矩阵，通常由鲁棒算法（RANSAC或LMEDS）设置.

maxIters：RANSAC算法的最大迭代次数，默认值为2000。

confidence：可信度值，取值范围为0到1.

 基础矩阵F、本质矩阵E、单应矩阵H 函数解析

 ④从本质矩阵恢复R和t使用recoverPose()函数

int recoverPose( InputArray E, InputArray points1, InputArray points2,
                 OutputArray R, OutputArray t, double focal = 1.0,
                 Point2d pp = Point2d(0, 0), InputOutputArray mask = noArray() );

E：已经求解出来的本质矩阵，它是3x3的矩阵；
points1：第一张图片中关键点的坐标；
points2：第二张图片中的关键点的坐标；

R：求解出来的两帧图片之间的旋转矩阵；
t：求解出来的两帧图片之间的平移向量；

focal：相机焦距；
pp：像素坐标的原点；

 R和t就是所求的最合适的结果，函数内部已经去掉了其他三种结果。

recoverPose()函数详细介绍

3.内参矩阵K

这里定义内参矩阵的时候用到了Mat_，它和Mat是有区别的。

Mat_ 也是一个模板类，注意它有一个下划线，以与Mat作为区别。在实际使用中，Mat_ 与 Mat 的操作函数没有多大区别，只不过Mat_需要在创建时定义元素类型，以后再调用它的方法是就不需要再传入数据的类型，而且还定义了一个操作符()来获取元素的位置。

这两个类之间的区别就是一个是定义时指定类型（Mat_），一个是使用时指定类型，可以按照不同的情况来使用。

Mat_ M(20, 20);
    for (int i = 0; i < M.rows; i++)
        for (int j = 0; j < M.cols; j++)
            M(i, j) = i + j + 1;//不使用at，直接用()索引，更方便

如果是使用Mat,  最后一行应该是：

M.at(i,j) = ********

4.像素坐标转换为归一化坐标

书99页公式5.5
 

二、三角测量

从2D图像进行ORB特征提取–>特征匹配–>极线约束八点法求E–>根据E求R,t–>三角测量求深度

1.triangulatePoints()函数

void triangulatePoints(InputArray projMatr1, 
                       InputArray projMatr2, 
                       InputArray projPoints1, 
                       InputArray projPoints2, 
                       OutputArray points4D)

projMatr1：第一个相机位姿（4x3的矩阵）
projMatr2：第二个相机位姿（4x3的矩阵）
projPoints1：第一个相机坐标系下的特征点坐标（需要转化为归一化坐标）
projPoints2：第二个相机坐标系下的特征点坐标

points4D：输出三角化后的特征点的3D坐标。但需要注意的是，输出的3D坐标是齐次坐标，共四个维度，因此需要将前三个维度除以第四个维度以得到非齐次坐标xyz

2.OpenCV circle()函数

circle(CvArr* img, CvPoint center, int radius, CvScalar color, int thickness=1, int lineType=8, int shift=0)

这个函数其实就是画圆

img为源图像

center为画圆的圆心坐标

radius为圆的半径

color为设定圆的颜色，CV_RGB(255, 0,0)设置为红色，CV_RGB(255, 255,255)设置为白色，CV_RGB(0, 0,0)设置为黑色 

thickness 如果是正数，表示组成圆的线条的粗细程度。否则，-1表示圆是否被填充

line_type 线条的类型。默认是8

shift 圆心坐标点和半径值的小数点位数

我使用circle()函数的时候报错，缺少一个问文件：

#include "opencv2/imgproc/imgproc.hpp"

总结：

关于关键点的归一化坐标计算方法有两种：

一是将关键点的像素坐标转化为归一化坐标

Point2d pt1_cam = pixel2cam(keypoints1[matches_good[i].queryIdx] .pt,K); 

二是通过三角函数计算得到的pts_4d转化为非齐次坐标 （x,y,z）

z是深度，它的归一化坐标是（x/z,y/z,1）

Point2d pt1_cam_3d (points[i].x /points[i].z, points[i].y /points[i].z);

三、求解PnP(3D-2D)

1.使用EPnP求解位姿

默认不考虑第一个相机的观测，因此把第一个相机的坐标系设定为世界坐标系

使用OpenCV的EPnP求解PnP问题

solvePnP()函数：

void solvePnP(InputArray objectPoints, 
              InputArray imagePoints, 
              InputArray cameraMatrix, 
              InputArray distCoeffs, 
              OutputArray rvec, 
              OutputArray tvec, 
              bool useExtrinsicGuess=false, 
              int flags = SOLVEPNP_ITERATIVE)

1. objectPoints -使用vector的数据类型
2. imagePoints - 使用vector的数据类型
3. cameraMatrix - 相机的内参矩阵
4. distCoeffs - 相机的畸变系数
5. rvec - 输出的旋转向量，使坐标点从世界坐标系旋转到相机坐标系
6. tvec - 输出的平移向量，使坐标点从世界坐标系平移到相机坐标系
7. flags - 默认使用SOLVEPNP_ITERATIVE迭代法

关于SOLVEPNP_ITERATIVE的取值对应不同的方法

enum {

       SOLVEPNP_ITERATIVE = 0,
       SOLVEPNP_EPNP      = 1,  //EPnP
       SOLVEPNP_P3P       = 2, //P3P
       SOLVEPNP_DLS       = 3, //DLS
       SOLVEPNP_UPNP      = 4, //UPnP
       SOLVEPNP_AP3P      = 5, //AP3P
       SOLVEPNP_MAX_COUNT      //!< Used for count
};

 第一个参数是图像一中特征点的空间位置为3D点；第二个参数是图像二的像素坐标为2D点

solvePnP函数存在误匹配的情况，所以需要使用solvePnPRansac()函数，该函数的参数具体看博客

 Rodrigues()    罗德里格斯公式

上一步算出的rvec是旋转向量，使用罗德里格斯公式将它变为矩阵的形式

void Rodrigues( const CvMat* src，CvMat* dst，CvMat* jacobian=0 );

src:为输入的旋转向量(3x1或1x3) 或者旋转矩阵                                                                    dst:为输出的旋转矩阵或者旋转向量                                                                                           jacobian:为可选的输出雅可比矩阵（3x9或者9x3），是输入与输出数组的偏导数

注意：rvec和罗德里格斯求出的R都是Mat类型的变量，需要使用cv::cv2eigen将其变为Matrix类型

(1条消息) 【opencv】Mat与Eigen_windistance的博客-CSDN博客

2.使用高斯牛顿法计算相机位姿

总结：高斯牛顿法思路：

1.代价函数是重投影误差 

2.对相机位姿 T 进行优化( T是一个带约束的矩阵，对它求导会特别复杂，所以用李代数表示)

3.计算雅可比矩阵 J   （187页公式7.46）

4. 根据J 算出 H  求解线性方程 H x = b

5.本次迭代误差和上次迭代误差进行比较，若大于则迭代结束。

自定义容器 pts_3d_eigen

将pts_3d中的坐标以三维向量的形式存储到容器pts_3d_eigen中 

vector pts_3d;
typedef vector>  VecVector3d; //存放三维向量的容器
VecVector3d pts_3d_eigen;  //存放图1的特征点的三维空间坐标
//遍历容器pts_3d中的坐标
for(int i = 0; i

增量方程：  H x = g

待优化的变量是相机位姿T，计算雅可比矩阵 J(2x6) 的时候将它用李代数形式表示，因此待优化的变量变为了 （6x1的列向量）。

这里的 H =     g =     (是 2x1 的列向量​) 

norm()是求模长，squaredNorm() 模长的平方。

3.使用g2o进行BA优化 

可以先去看看g2o类图，熟悉g2o优化过程。

总结一下做图优化的流程。

1.选择你想要的图里的节点与边的类型，确定它们的参数化形式；
2.往图里加入实际的节点和边；
3.选择初值，开始迭代；
4.每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；
5.求解稀疏线性方程HkΔx=−bk，得到梯度方向；
6.继续用GN或LM进行迭代。如果迭代结束，返回优化值。

实际上，g2o能做好第3-6步，我们要做的只是前两步。

顶点：相机的位姿 T （李群Sophus::SE3d）

边：空间点到像素坐标的投影关系，也就是误差项 

 关于g2o优化我在ch6中总结过。

第一步：定义顶点的类型

第二步：定义边的类型

第三步：构建图优化

         ① 配置块求解器  BlockSolver 

        ②配置线性方程求解器 LinearSolver

        ③配置总求解器solver，并从GN,LM,Dogleg优化算法中选一个，再用上述块求解器BlockSolver初始化

        ④配置稀疏优化器SparseOptimizer

        ⑤往图中增加顶点和边，并添加到SparseOptimizer 

第四步：启动图优化

 estimate()函数

// 返回优化之后顶点的值.
const EstimateType& estimate() const { return _estimate;}

 最终调试的时候报错：

/usr/local/include/glog/logging.h:802：对‘google::base::CheckOpMessageBuilder::CheckOpMessageBuilder(char const*)’未定义的引用

参考这篇文章下载两个库就行

vslam14讲中编译G2O报错： 对‘google::LogMessageFatal::LogMessageFatal（xxxxx）’未定义的引用； GFLAGS报错_又决定放弃的博客-CSDN博客

写完这部分看完后可以看看课后习题6，如果把第一帧相机的观测加进来会变成什么样？

参考：g2o图优化中的要点与难点

           代码

四、求解ICP (3D-3D)

 1.SVD 方法

①分别求出质心

,

②求区质心

 , 

③定义W 矩阵  

 

④对W 进行分解求出 U 和 V(使用JacobiSVD()函数)

JacobiSVD(const MatrixType& matrix, unsigned int computationOptions = 0)

第一个参数是进行计算的矩阵

第二参数有四个取值：

ComputeFullU:在Jacobisvd中用于表示要计算方阵U

ComputeThinU:在Jacobisvd中用于表示要计算薄矩阵U

ComputeFullV:在Jacobisvd中用于表示要计算方阵V

ComputeThinV:在Jacobisvd中用于表示要计算薄矩阵V

⑤ 求 R

当W满秩时，   , 求出后对R的行列式进行判断，如果小于零 ， R = -R

⑥求 t

 2、非线性优化方法(g2o)

PnP 3D-2D重投影误差求导的雅各比矩阵 J 是2*6维的 

ICP 3D-3D误差求导的雅各比矩阵 J 是3*6维的

Eigen子矩阵操作(block)

MatrixXf m(4,4);
    m<< 1,2,3,4,
        5,6,7,8,
        9,10,11,12,
        13,14,15,16;
cout<<"Block in the middle"<(1,1)<