slam位姿估计（自动驾驶SLAMSLAM初始化中如何找单应性矩阵）
slam位姿估计（自动驾驶SLAMSLAM初始化中如何找单应性矩阵）第40行是为了对A进行SVD分解，分解得到的vt中，有我们要的X。SVD的函数解释如下：故，代码里10--36行都是为了准备我们的方程组的A，那么这个vt是什么意思？vt是特征向量，v.row(8)是最小特征值对应的特征向量。为什么最小的特征值对应的特征向量就是单应性矩阵的值了？这里有个推导，从单应性矩阵把点p1变换为p2说起：以上推导中，我们利用N对点的坐标，建立方程组，但是这个方程组是个超定方程组，无解，所以我们采用最小二乘法去求近似解，所以我们就可以用SVD分解去解出X了。
单应性矩阵：点集P1经过单应性矩阵X变换为点集P2，一般单应性矩阵用H表示，因为单应性的英文是homography，但是我们这里要求取单应性矩阵，它是一个待求量，一个未知量，我们就用X来表示它。
先上代码，我尝试用数学原理去解释这段代码：
1 //homography
2 // 从特征点匹配求homography（normalized DLT）
3 // Algorithm 4.2 in Multiple View Geometry in Computer Vision.
4 // cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1 const vector<cv::Point2f> &vP2) {
5 cv::Mat ComputeH21(const vector<cv::Point2f> &vP1 const vector<cv::Point2f> &vP2) {
6 const int N = vP1.size();
7
8 cv::Mat A(2 * N 9 CV_32F);
9
10 for (int i = 0; i < N; i ) {
11 const float u1 = vP1[i].x;
12 const float v1 = vP1[i].y;
13 const float u2 = vP2[i].x;
14 const float v2 = vP2[i].y;
15
16 A.at<float>(2 * i 0) = 0.0;
17 A.at<float>(2 * i 1) = 0.0;
18 A.at<float>(2 * i 2) = 0.0;
19 A.at<float>(2 * i 3) = -u1;
20 A.at<float>(2 * i 4) = -v1;
21 A.at<float>(2 * i 5) = -1;
22 A.at<float>(2 * i 6) = v2 * u1;
23 A.at<float>(2 * i 7) = v2 * v1;
24 A.at<float>(2 * i 8) = v2;
25
26 A.at<float>(2 * i 1 0) = u1;
27 A.at<float>(2 * i 1 1) = v1;
28 A.at<float>(2 * i 1 2) = 1;
29 A.at<float>(2 * i 1 3) = 0.0;
30 A.at<float>(2 * i 1 4) = 0.0;
31 A.at<float>(2 * i 1 5) = 0.0;
32 A.at<float>(2 * i 1 6) = -u2 * u1;
33 A.at<float>(2 * i 1 7) = -u2 * v1;
34 A.at<float>(2 * i 1 8) = -u2;
35
36 }
37
38 cv::Mat u w vt;
39
40 cv::SVDecomp(A w u vt cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
41
42 return vt.row(8).reshape(0 3);
43 }
代码第42行，就是X的取值= vt.row(8).reshape(0 3)
我们这里得到一个3x3的矩阵，在opencv中，reshape(0 3) 是指不改变矩阵的通道，将矩阵的行数设置为3，列数= 总矩阵数目/通道数/3.
那么这个vt是什么意思？vt是特征向量，v.row(8)是最小特征值对应的特征向量。
为什么最小的特征值对应的特征向量就是单应性矩阵的值了？这里有个推导，从单应性矩阵把点p1变换为p2说起：
以上推导中，我们利用N对点的坐标，建立方程组，但是这个方程组是个超定方程组，无解，所以我们采用最小二乘法去求近似解，所以我们就可以用SVD分解去解出X了。
故，代码里10--36行都是为了准备我们的方程组的A，
第40行是为了对A进行SVD分解，分解得到的vt中，有我们要的X。SVD的函数解释如下：
这里为什么vt.row(8)就是最小奇异值对应的特征向量呢？后面我再接着研究一下。
基本上，SLAM代码中的这个求单应性矩阵的函数，算是基本搞清楚了。
对了，函数的输入点集P1 和点集P2来自于机器人拍摄的上一frame图片和当前frame图片。所以通过这个H，我们是可以近似求取机器人移动的位置和姿态的变化量的。
参考文献：
- 奇异值分解与最小二乘：https://www.cnblogs.com/houkai/p/6656894.html
- SLAM初始化流程：https://www.cnblogs.com/luyb/p/5260785.html，这个链接也是本文分析的代码的出处。这个文章里的代码细节挺多的，计算完H矩阵后，还要根据矩阵计算当前机器人的位置和姿态矩阵，这个在其ReconstructH函数里 这个函数ReconstructH就是用单应性矩阵H计算出一个三维世界坐标系中的变换矩阵[R|T} 表示机器人移动后的位置和姿态。
