1 特征点法
- 1.1 特征点
- 1.2 ORB特征
- 1.3 特征匹配
2 特征提取与匹配
3 2D-2D：对极几何
- 3.1 对极约束
- 3.2 本质矩阵
- 3.3 单应矩阵
- 讨论
4 实践：对极约束求相机运动
5 三角测量
6 实践：三角测量
7 3D-2D:PnP
- 7.1 直接线性变换
- 7.2 P3P
- 7.3 Bundle Adjustment
8 实践：求解PnP
- 8.1 使用EPnP求解位姿
- 8.2 使用BA优化
9 3D-3D:ICP
- 9.1 SVD方法
- 9.2 非线性优化方法
10 实践：求解ICP
- 10.1 SVD方法
- 10.2 非线性优化方法

1 特征点法

视觉slam主要分为视觉前段和优化后端。前段称为视觉里程计（VO），它根据相邻图象的信息估计出粗略的相机运动，给后端提供较好的初始值。
VO的实现方法可以根据是否需要提取特征分为两类：基于特征点的方法，不使用特征点的直接方法。基于特征点的VO运行稳定，对光照、动态物体不敏感。

1.1 特征点

为了能够更好的进行图像匹配，需要在图像中选择具有代表性的区域，例如：图像中的角点、边缘和一些区块。
更为稳定的的局部图像特征特征点，例如：SIFT,SURF,ORB等这些特征点不会随着相机的移动，旋转或者光照的变化而变化，其四个特征:
- 可重复性Repeatability
- 可区别性Distinctiveness
- 高效率Efficiency
- 本地性Locality
特征点由(关键点Key-Point)和描述子（Descriptor）。关键点指的是该特征点在图像中的位置，有些还具有方向、尺度信息；描述子通常是一个向量，按照人为的设计的方式，描述关键点周围像素的信息。通常描述子是按照外观相似的特征应该有相似的描述子设计的。
SIFT（Scale Invariant Feature Transform）尺度不变特征变换，SIFT特征对旋转、尺度缩放、亮度变化等保持不变性，是一种非常稳定的局部特征。
SURF（ Speeded Up Robust Features），主要针对SIFT算法运算速度慢，计算量大的缺点进行了改进。
ORB（Oriented FAST and Rotated BRIEF）算法，改进了FAST检测子不具有方向性的问题，并使用速度极快的二进制描述子BRIEF。

1.2 ORB特征

提取ORB（Oriented FAST and Rotated BRIEF)特征分为两个步骤：
1. FAST 角点提取：找出图像中的” 角点”。相较于原版的 FAST, ORB 中计算了特征点的主方向，为后续的 BRIEF 描述子增加了旋转不变特性。
2. BRIEF 描述子：对前一步提取出特征点的周围图像区域进行描述
Fast 角点检测，FAST 是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。其思想是：如果一个像素与它邻域的像素差别较大（过亮或过暗）, 那它更可能是角点。检测过程如下：
1. 在图像中选取像素 ppp，假设它的亮度为 IpI_{p}Ip。
2. 设置一个阈值 TTT(比如IpI_{p}Ip的 20%)。
3. 以像素 ppp为中心, 选取半径为 3 的圆上的 16 个像素点。
4. 假如选取的圆上，有连续的 NNN 个点的亮度大于 Ip+TI_{p}+TIp+T或小于Ip+TI_{p}+TIp+T，那么像素 ppp可以被认为是特征点 (N通常取 12，即为 FAST-12。其它常用的 NNN 取值为 9 和 11，他们分别被称为 FAST-9，FAST-11)。
5. 循环以上四步，对每一个像素执行相同的操作。
在 ORB 中，对原始的 FAST 算法进行了改进。可以指定最终要提取的角点数量N，对原始 FAST 角点分别计算 Harris 响应值，然后选取前N个具有最大响应值的角点，作为最终的角点集合。
FAST 角点不具有方向信息.针对 FAST 角点不具有方向性和尺度的弱点，ORB 添加了尺度和旋转的描述。
- 尺度不变性由构建图像金字塔（金字塔是指对图像进行不同层次的降采样，以获得不同分辨率的图像），并在金字塔的每一层上检测角点来实现。
- 特征的旋转是由灰度质心法（Intensity Centroid）实现的。
  1. 质心是指以图像块灰度值作为权重的中心。其具体操作步骤如下：
    
    （1）在一个小的图像块 BBB 中，定义图像块的矩为:
    mpq=∑xpyqI(x,y),p,q={0,1}m _ { p q } = \sum x ^ { p } y ^ { q } I ( x , y ) , \quad p , q = \{ 0,1 \}mpq=∑xpyqI(x,y),p,q={0,1}
    （2）通过矩可以找到图像块的质心：
    CC=(m10m00,m01m00)CC = \left( \frac { m _ { 10 } } { m _ { 00 } } , \frac { m _ { 01 } } { m _ { 00 } } \right)CC=(m00m10,m00m01)
    （3）连接图像块的几何中心 O 与质心 C，得到一个方向向量 OC⃗\vec { O C }OC，于是特征点的方向可以定义为：
    θ=arctan⁡(m01/m10)\theta = \arctan \left( m _ { 01 } / m _ { 10 } \right)θ=arctan(m01/m10)
BRIEF 描述子
1. BRIEF 是一种二进制描述子，它的描述向量由许多个 0 和 1 组成，这里的 0 和 1 编码了关键点附近两个像素（比如说 p 和q）的大小关系：如果 p 比 q 大，则取 1，反之就取 0。如果我们取了 128 个这样的 p, q，最后就得到 128 维由 0，1 组成的向量。
2. BRIEF 使用了随机选点的比较，速度非常快，使用了二进制表达，存储起来也十分方便，适用于实时的图像匹配。
3. ORB 在 FAST 特征点提取阶段计算了关键点的方向，可以利用方向信息，计算了旋转之后的“Steer BRIEF”特征，使 ORB 的描述子具有较好的旋转不变性。

1.3 特征匹配

暴力匹配：即对每一个特征点 xtmx _ { t } ^ { m }xtm，与所有的xt+1mx _ { t +1} ^ { m }xt+1m测量描述子的距离，然后排序，取最近的一个作为匹配点。描述子距离表示两个特征之间的相似程度。
浮点类型的描述子，使用欧氏距离进行度量即可。
二进制的描述子（比如 BRIEF 这样的），往往使用**汉明距离（**Hamming distance）做为度量——两个二进制串之间的汉明距离，指的是它们不同位数的个数。
快速近似最近邻（FLANN）：更加适合匹配点数量极多的情况。

2 特征提取与匹配

代码及注释：

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>using namespace std;
using namespace cv;int main ( int argc, char** argv )
{if ( argc != 3 ){cout<<"usage: feature_extraction img1 img2"<<endl;return 1;}//-- 读取图像Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );//-- 初始化//keypoint是opencv里的数据类型std::vector<KeyPoint> keypoints_1, keypoints_2;Mat descriptors_1, descriptors_2;//描述子//opencv3Ptr<FeatureDetector> detector = ORB::create();//特征检测器Ptr<DescriptorExtractor> descriptor = ORB::create();//描述子提取器//opencv2// Ptr<FeatureDetector> detector = FeatureDetector::create(detector_name);// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create(descriptor_name);//-- 第一步:检测 Oriented FAST 角点位置detector->detect ( img_1,keypoints_1 );//固定用法，检测图一中的角点位置detector->detect ( img_2,keypoints_2 );//-- 第二步:根据角点位置计算 BRIEF 描述子//描述子通常是一个向量，保存在Mat中descriptor->compute ( img_1, keypoints_1, descriptors_1 );//计算图一特征点的描述子descriptor->compute ( img_2, keypoints_2, descriptors_2 );//定义输出检测特征点的图片Mat outimg1;//随机像素值drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT );namedWindow("ORB特征点", WINDOW_AUTOSIZE );imshow("ORB特征点",outimg1);imwrite("ORB特征点.jpg", outimg1);

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配//使用了opencv中封装后的暴力匹配算法BFMatcher,使用 Hamming 距离vector<DMatch> matches;//特征匹配算法：汉明距离Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );//描述子匹配器//BFMatcher matcher ( NORM_HAMMING );matcher->match ( descriptors_1, descriptors_2, matches );//-- 第四步:匹配点对筛选//上面的匹配结果有大量的错误匹配，需要使用一些机制来过滤掉错误的匹配//汉明距离小于最小距离的两倍double min_dist=10000, max_dist=0;//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离for ( int i = 0; i < descriptors_1.rows; i++ ){double dist = matches[i].distance;if ( dist < min_dist ) min_dist = dist;if ( dist > max_dist ) max_dist = dist;}// 仅供娱乐的写法//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.distance<m2.distance;} )->distance;printf ( "-- Max dist : %f \n", max_dist );printf ( "-- Min dist : %f \n", min_dist );

//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.std::vector< DMatch > good_matches;for ( int i = 0; i < descriptors_1.rows; i++ ){if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) ){good_matches.push_back ( matches[i] );}}//-- 第五步:绘制匹配结果Mat img_match;Mat img_goodmatch;drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_match );drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch );namedWindow("所有匹配点对", WINDOW_AUTOSIZE );imshow ( "所有匹配点对", img_match );imwrite("所有匹配点对.jpg", img_match);namedWindow("优化后匹配点对", WINDOW_AUTOSIZE );imshow ( "优化后匹配点对", img_goodmatch );imwrite ( "优化后匹配点对.jpg", img_goodmatch );waitKey(0);

所有匹配点对：优化后匹配点对：
根据匹配的点对,估计相机的运动。

当相机为单目时,我们只知道 2D 的像素坐标,因而问题是根据两组 2D 点估计运
动。该问题用对极几何来解决。
当相机为双目、RGB-D 时,或者我们通过某种方法得到了距离信息,那问题就是根
据两组 3D 点估计运动。该问题通常用 ICP 来解决。
如果我们有 3D 点和它们在相机的投影位置,也能估计相机的运动。该问题通过 PnP
求解。

3 2D-2D：对极几何

3.1 对极约束

设第一帧到第二帧的运动为R,tR,tR,t,O1,O2O_{1},O_{2}O1,O2三个点平面为极平面，e1,e2e_{1},e_{2}e1,e2为极点，O1O2O_{1}O_{2}O1O2称为基线，l1l2l_{1}l_{2}l1l2为极线。

已知：匹配点对p1p1，p2p2的像素坐标。
给定：两张二维图像，二维图像上特征点的匹配关系。
未知：P的三维空间坐标，I1I1I1到I2I2I2的变换矩阵(T1,2T_{1,2}T1,2，即R,t）。

对极约束

设第一帧的坐标系下P的空间位置为：
P=[X,Y,Z]TP=[X,Y,Z]^TP=[X,Y,Z]T
两个像素点p1，p2p1，p2p1，p2的像素坐标（单位像素）：
s1p1=KP,s2p2=K(RP+t)s_{1}p_{1}=KP, \quad s_{2}p_{2}=K(RP+t)s1p1=KP,s2p2=K(RP+t)
使用齐次坐标：
p1=KP,p2=K(RP+t)p_{1}=KP, \quad p_{2}=K(RP+t)p1=KP,p2=K(RP+t)
取：
x1=K−1p1,x2=K−1p2x_{1}=K^{−1}p_{1},\quad x_{2}=K^{−1}p_{2}x1=K−1p1,x2=K−1p2
x1,x2x_{1},x_{2}x1,x2是两个像素点的归一化的坐标，代入上式得：
x2=Rx1+tx_{2}=Rx_{1}+tx2=Rx1+t
两边同时左乘ttt^:
t∧x2=t∧Rx1t^{\wedge }x_{2}=t^{\wedge }Rx_{1}t∧x2=t∧Rx1
再左乘一个x2Tx^{T}_{2}x2T,有:
x2Tt∧x2=x2Tt∧Rx1x^{T}_{2}t^{\wedge }x_{2}=x^{T}_{2}t^{\wedge }Rx_{1}x2Tt∧x2=x2Tt∧Rx1
由上式可得：
x2Tt∧Rx1=0x^{T}_{2}t^{\wedge }Rx_{1}=0x2Tt∧Rx1=0
带入p1，p2p_{1}，p_{2}p1，p2：
p2TK−Tt∧Rk−1p1p^{T}_{2}K^{-T}t^{\wedge }Rk^{-1}p_{1}p2TK−Tt∧Rk−1p1
本质矩阵E,基础矩阵F,简化对极约束：
E=t∧R,F=K−TEK−1,x2Tt∧Ex1=p2TFp1=0E=t^{\wedge }R,F=K^{-T}EK^{-1},x^{T}_{2}t^{\wedge }Ex_{1}=p^{T} _{2}Fp_{1}=0E=t∧R,F=K−TEK−1,x2Tt∧Ex1=p2TFp1=0
对极约束简化给出两个点的空间位置关系，相机位姿估计问题变成一下两部：

根据匹配点的位置，求出E或者F。
根据E或者F，求出R,t。

3.2 本质矩阵

本质矩阵E=t∧RE=t^{\wedge }RE=t∧R,3x3矩阵

E在不同尺度下等价的
本质矩阵的内在性质：E的奇异值必定为[σ,σ,0]\left [ \sigma ,\sigma ,0 \right ][σ,σ,0]的形式，
由平移和旋转定义，共有3+3=6个自由度，因为对极约束，E具有尺度等价性，自由度-1，共五个。

八点法
根据对极约束，最少五对点就能求出来E，为了方便，一般使用八对点。

3.3 单应矩阵

它描述的是两个平面之间的映射关系，如果场景中的特征点都落在同一平面上(比如情面、地面)，则可以通过单应性进行运动估计。
假设特征点落在平面P上
nTP+d=0n^{T}P+d=0nTP+d=0
−nTPd=1-\frac{n^{T}P}{d} =1−dnTP=1
p2=K(RP+t)p_{2}=K(RP+t)p2=K(RP+t)
得到一个从图像坐标p1p1到p2p2之间的变换:
p2=K(R−tnTd)K−1p1p_{2}=K(R−\frac{tn^T}{d})K^−1p_{1}p2=K(R−dtnT)K−1p1
把中间部分记为H,于是:
p2=Hp1p_{2}=Hp_{1}p2=Hp1
它的定义与平移、旋转和平面的参数相关。
把H矩阵看成向量，通过求解该向量的线性方程来恢复H,又称直线线性变换法(Direct Linear Transform)
与求解本质矩阵类似，求出来后需要对其进行分解才能得到R,t,分解方法有数值法和解析法。分解同样得到四组解，最后根据先验信息进行排除得到唯一的一组R,t解。
为了避免退化现象造成的影响，通常会同时估计基础矩阵F和单应矩阵H，选择重投影误差比较小的那个最终的运动估计矩阵。

讨论

由于 E 本身具有尺度等价性,它分解得到的 t, R 也有一个尺度等价性。通常把 t 进行归一化,让它的
长度等于 1。

尺度不确定性
对t长度的归一化，直接导致单目视觉的尺度不确定性(Scale Ambiguity),在单目视觉中对两张图像的t归一化相当于固定了尺度，我们不知道长度是多少，以这时t的单位是1，计算相机运动和特征点的3D的位置，称为单目视觉SLAM的初始化。
初始化的纯旋转问题
单目视觉初始化不能只有纯旋转，必须要有一定程度的平移。如果没有平移，从E分解到R,t的过程中，导致t为零，那么得到的E也为零，这将导致无法求解R。不过此时可以依靠H求取旋转，但是仅有旋转时，无法使用三角测量估计特征点的空间位置。
单目初始化不能只有纯旋转，必须有一定程度的平移
对于8对点的情况
当给定匹配点多余8个的时候，比如算出79对匹配点，使用一个最小二乘计算：
min⁡e∥Ae∥22=min⁡eTATAe\min_{e}\left \| Ae \right \|_{2}^{2}=\min e^{T}A^TAeemin∥Ae∥22=mineTATAe

不过在存在错误匹配的情况下使用随机抽样一致算法(Random Sample Concensus,RANSAC)，可以处理带有错误匹配的数据。

4 实践：对极约束求相机运动

代码及注释：

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>//Features2D， 2D功能框架
//高层GUI图形用户界面，包含媒体的I / O输入输出，视频捕捉、图像和视频的编码解码、图形交互界面的接口等内容
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>//主要是相机校准和三维重建相关的内容
// #include "extra.h" // use this if in OpenCV2
using namespace std;
using namespace cv;/***************************************************** 本程序演示了如何使用2D-2D的特征匹配估计相机运动* **************************************************/void find_feature_matches (const Mat& img_1, const Mat& img_2,std::vector<KeyPoint>& keypoints_1,std::vector<KeyPoint>& keypoints_2,std::vector< DMatch >& matches );void pose_estimation_2d2d (std::vector<KeyPoint> keypoints_1,std::vector<KeyPoint> keypoints_2,std::vector< DMatch > matches,Mat& R, Mat& t );// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );int main ( int argc, char** argv )
{if ( argc != 3 ){cout<<"usage: pose_estimation_2d2d img1 img2"<<endl;return 1;}//-- 读取图像Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );vector<KeyPoint> keypoints_1, keypoints_2;vector<DMatch> matches;find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;//-- 估计两张图像间运动Mat R,t;pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );//-- 验证E=t^R*scaleMat t_x = ( Mat_<double> ( 3,3 ) <<0,                      -t.at<double> ( 2,0 ),     t.at<double> ( 1,0 ),t.at<double> ( 2,0 ),      0,                      -t.at<double> ( 0,0 ),-t.at<double> ( 1,0 ),     t.at<double> ( 0,0 ),      0 );cout<<"t^R="<<endl<<t_x*R<<endl;//-- 验证对极约束Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );for ( DMatch m: matches ){Point2d pt1 = pixel2cam ( keypoints_1[ m.queryIdx ].pt, K );Mat y1 = ( Mat_<double> ( 3,1 ) << pt1.x, pt1.y, 1 );Point2d pt2 = pixel2cam ( keypoints_2[ m.trainIdx ].pt, K );Mat y2 = ( Mat_<double> ( 3,1 ) << pt2.x, pt2.y, 1 );Mat d = y2.t() * t_x * R * y1;cout << "epipolar constraint = " << d << endl;}return 0;
}

find_feature_matches 定义

void find_feature_matches ( const Mat& img_1, const Mat& img_2,std::vector<KeyPoint>& keypoints_1,std::vector<KeyPoint>& keypoints_2,std::vector< DMatch >& matches )
{//-- 初始化Mat descriptors_1, descriptors_2;// used in OpenCV3 Ptr<FeatureDetector> detector = ORB::create();Ptr<DescriptorExtractor> descriptor = ORB::create();// use this if you are in OpenCV2 // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );//-- 第一步:检测 Oriented FAST 角点位置detector->detect ( img_1,keypoints_1 );detector->detect ( img_2,keypoints_2 );//-- 第二步:根据角点位置计算 BRIEF 描述子descriptor->compute ( img_1, keypoints_1, descriptors_1 );descriptor->compute ( img_2, keypoints_2, descriptors_2 );//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离vector<DMatch> match;//BFMatcher matcher ( NORM_HAMMING );matcher->match ( descriptors_1, descriptors_2, match );//-- 第四步:匹配点对筛选double min_dist=10000, max_dist=0;//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离for ( int i = 0; i < descriptors_1.rows; i++ ){double dist = match[i].distance;if ( dist < min_dist ) min_dist = dist;if ( dist > max_dist ) max_dist = dist;}printf ( "-- Max dist : %f \n", max_dist );printf ( "-- Min dist : %f \n", min_dist );//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.for ( int i = 0; i < descriptors_1.rows; i++ ){if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ){matches.push_back ( match[i] );}}
}// 像素坐标转相机归一化坐标
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 定义
从特征点(keypoints)和匹配关系（matches）求解位姿(R,t)

void pose_estimation_2d2d ( std::vector<KeyPoint> keypoints_1,std::vector<KeyPoint> keypoints_2,std::vector< DMatch > matches,Mat& R, Mat& t )
{// 相机内参,TUM Freiburg2Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );//-- 把匹配点转换为vector<Point2f>的形式vector<Point2f> points1;vector<Point2f> points2;for ( int i = 0; i < ( int ) matches.size(); i++ ){points1.push_back ( keypoints_1[matches[i].queryIdx].pt );//queryIdx为query描述子的索引,match函数中前面的那个描述子 points2.push_back ( keypoints_2[matches[i].trainIdx].pt );//trainIdx为train描述子的索引,match函数中后面的那个描述子 }//-- 计算基础矩阵Mat fundamental_matrix;fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl;//-- 计算本质矩阵Point2d principal_point ( 325.1, 249.7 ); //相机光心, TUM dataset标定值double focal_length = 521;           //相机焦距, TUM dataset标定值Mat essential_matrix;essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point );cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;//-- 计算单应矩阵Mat homography_matrix;homography_matrix = findHomography ( points1, points2, RANSAC, 3 );cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;//-- 从本质矩阵中恢复旋转和平移信息.recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );cout<<"R is "<<endl<<R<<endl;cout<<"t is "<<endl<<t<<endl;}

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 ));
}

运行结果：

[ INFO:0] Initialize OpenCL runtime...
-- Max dist : 95.000000
-- Min dist : 7.000000
一共找到了81组匹配点
fundamental_matrix is
[5.435453065936354e-06, 0.0001366043242989653, -0.02140890086948144;-0.0001321142229824715, 2.339475702778067e-05, -0.006332906454396007;0.02107630352202796, -0.003666833952953114, 0.9999999999999999]
essential_matrix is
[0.01724015832721706, 0.328054335794133, 0.0473747783144249;-0.3243229585962962, 0.03292958445202408, -0.6262554366073018;-0.005885857752320116, 0.6253830041920333, 0.0153167864909267]
homography_matrix is
[0.9131751791807657, -0.1092435315823255, 29.95860009984688;0.02223560352312278, 0.9826008005062553, 6.508910839573075;-0.0001001560381022834, 0.000103777943639824, 0.9999999999999999]
R is
[0.9985534106102478, -0.05339308467584758, 0.006345444621108698;0.05321959721496264, 0.9982715997131746, 0.02492965459802003;-0.007665548311697523, -0.02455588961730239, 0.9996690690694516]
t is
[-0.8829934995085544;-0.05539655431450562;0.4661048182498402]
t^R=
[-0.02438126572381045, -0.4639388908753606, -0.06699805400667856;0.4586619266358499, -0.04656946493536188, 0.8856589319599302;0.008323859859529846, -0.8844251262060034, -0.0216612071874423]
epipolar constraint = [0.004334754136721797]
epipolar constraint = [-0.0002809243685121809]
......
epipolar constraint = [-0.004259380631293275]
epipolar constraint = [-0.0007738612238716719]

5 三角测量

6 实践：三角测量

单目SLAM(2d-2d)中，使用对极约束估计R,tR,t，使用三角测量估计深度

void triangulation ( const vector< KeyPoint >& keypoint_1, const vector< KeyPoint >& keypoint_2, const std::vector< DMatch >& matches,const Mat& R, const Mat& t, vector< Point3d >& points )
{Mat T1 = (Mat_<float> (3,4) <<1,0,0,0,0,1,0,0,0,0,1,0);Mat T2 = (Mat_<float> (3,4) <<R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0),R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0),R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0));Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );vector<Point2f> pts_1, pts_2;for ( DMatch m:matches ){// 将像素坐标转换至相机坐标pts_1.push_back ( pixel2cam( keypoint_1[m.queryIdx].pt, K) );pts_2.push_back ( pixel2cam( keypoint_2[m.trainIdx].pt, K) );}Mat pts_4d;cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );// 转换成非齐次坐标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 );}
}

int main ( int argc, char** argv )
{if ( argc != 3 ){cout<<"usage: triangulation img1 img2"<<endl;return 1;}//-- 读取图像Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );vector<KeyPoint> keypoints_1, keypoints_2;vector<DMatch> matches;find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;//-- 估计两张图像间运动Mat R,t;pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );//-- 三角化vector<Point3d> points;triangulation( keypoints_1, keypoints_2, matches, R, t, points );//-- 验证三角化点与特征点的重投影关系Mat 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;}return 0;
}

7 3D-2D:PnP

PnP(Perspective-n-Point)是求解 3D 到 2D 点对运动的方法。最少只需三个点对(需要至少一个额外点验证结果)就可以估计相机运动。
PnP 问题有很多种求解方法,例如用三对点估计位姿的 P3P,直接线性变换(DLT),EPnP(Efficient PnP),UPnP等等)。此外,还能用非线性优化的方式,构建最小二乘问题并迭代求解,也就是万金油式的 Bundle Adjustment。

7.1 直接线性变换

7.2 P3P

7.3 Bundle Adjustment

8 实践：求解PnP

8.1 使用EPnP求解位姿

int main ( int argc, char** argv )
{if ( argc != 5 ){cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;return 1;}//-- 读取图像Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );vector<KeyPoint> keypoints_1, keypoints_2;vector<DMatch> matches;find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;// 建立3D点Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );vector<Point3f> pts_3d;vector<Point2f> pts_2d;for ( DMatch m:matches ){ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];if ( d == 0 )   // bad depthcontinue;float dd = d/5000.0;Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );pts_2d.push_back ( keypoints_2[m.trainIdx].pt );}cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;Mat r, t;solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法Mat R;cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵cout<<"R="<<endl<<R<<endl;cout<<"t="<<endl<<t<<endl;cout<<"calling bundle adjustment"<<endl;bundleAdjustment ( pts_3d, pts_2d, K, R, t );
}

结果：

-- Max dist : 95.000000
-- Min dist : 7.000000
一共找到了81组匹配点
3d-2d pairs: 77
R=
[0.9979193252225089, -0.05138618904650331, 0.03894200717386666;0.05033852907733834, 0.9983556574295412, 0.02742286944793203;-0.04028712992734059, -0.02540552801469367, 0.9988651091656532]
t=
[-0.1255867099750398;-0.007363525258777434;0.06099926588678889]

8.2 使用BA优化

void bundleAdjustment (const vector< Point3f > points_3d,const vector< Point2f > points_2d,const Mat& K,Mat& R, Mat& t )
{// 初始化g2otypedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );g2o::SparseOptimizer optimizer;optimizer.setAlgorithm ( solver );// vertexg2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera poseEigen::Matrix3d R_mat;R_mat <<R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );pose->setId ( 0 );pose->setEstimate ( g2o::SE3Quat (R_mat,Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )) );optimizer.addVertex ( pose );int index = 1;for ( const Point3f p:points_3d )   // landmarks{g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();point->setId ( index++ );point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容optimizer.addVertex ( point );}// parameter: camera intrinsicsg2o::CameraParameters* camera = new g2o::CameraParameters (K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0);camera->setId ( 0 );optimizer.addParameter ( camera );// edgesindex = 1;for ( const Point2f p:points_2d ){g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();edge->setId ( index );edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );edge->setVertex ( 1, pose );edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );edge->setParameterId ( 0,0 );edge->setInformation ( Eigen::Matrix2d::Identity() );optimizer.addEdge ( edge );index++;}chrono::steady_clock::time_point t1 = chrono::steady_clock::now();optimizer.setVerbose ( true );optimizer.initializeOptimization();optimizer.optimize ( 100 );chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;cout<<endl<<"after optimization:"<<endl;cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}

calling bundle adjustment
iteration= 0    chi2= 0.001292     time= 0.000468135  cumTime= 0.000468135   edges= 77  schur= 1   lambda= 80.003723  levenbergIter= 1
iteration= 1    chi2= 0.000000     time= 2.6183e-05   cumTime= 0.000494318   edges= 77  schur= 1   lambda= 53.335815  levenbergIter= 1
iteration= 2    chi2= 0.000000     time= 2.541e-05    cumTime= 0.000519728   edges= 77  schur= 1   lambda= 35.557210  levenbergIter= 1
iteration= 3    chi2= 0.000000     time= 2.3129e-05   cumTime= 0.000542857   edges= 77  schur= 1   lambda= 23.704807  levenbergIter= 1
iteration= 4    chi2= 0.000000     time= 2.3182e-05   cumTime= 0.000566039   edges= 77  schur= 1   lambda= 15.803205  levenbergIter= 1
iteration= 5    chi2= 0.000000     time= 2.2782e-05   cumTime= 0.000588821   edges= 77  schur= 1   lambda= 10.535470  levenbergIter= 1
iteration= 6    chi2= 0.000000     time= 7.2227e-05   cumTime= 0.000661048   edges= 77  schur= 1   lambda= 230150.846755  levenbergIter= 6
iteration= 7    chi2= 0.000000     time= 5.2096e-05   cumTime= 0.000713144   edges= 77  schur= 1   lambda= 235674467.076671   levenbergIter= 4
optimization costs time: 0.000928678 seconds.after optimization:
T=0.997841  -0.0518393   0.0403291   -0.1275160.0507013      0.9983    0.028745 -0.00947167-0.0417507  -0.0266382    0.998773   0.05950370           0           0           1

9 3D-3D:ICP

迭代最近点ICP（ Iterative Closest Point），指代匹配好的两组点云间运动估计问题。求解 3D坐标到 3D坐标的转换矩阵(不用求解距离激光SLAM 以及 RGB-D SLAM)

9.1 SVD方法

先定义第i对点的误差项：
ei=pi−(Rpi′+t)e_{i}=p_{i}-(Rp^{'}_{i}+t)ei=pi−(Rpi′+t)
然后，构建最小二乘问题，求使误差平方和达到极小的R,t:
min⁡R,t=12∑1n∥(pi−(Rpi′+t))∥22\min_{R,t}=\frac{1}{2}\sum_{1}^{n}\left \| \left ( p_{i} -\left ( Rp^{'}_{i}+t \right )\right ) \right \|\tfrac{2}{2}R,tmin=211∑n∥∥∥(pi−(Rpi′+t))∥∥∥22

9.2 非线性优化方法

min⁡ξ=12∑i=1n∥(pi−exp(ξ∧))pi′∥22\min_{\xi}=\frac{1}{2}\sum_{i=1}^{n}\left \| (p_{i}-exp\left ( \xi ^{\wedge } \right )) p_{i}^{'}\right \|\tfrac{2}{2}ξmin=21i=1∑n∥∥∥(pi−exp(ξ∧))pi′∥∥∥22代数扰动模型：
∂e∂δξ=−(exp(ξ∧)pi′)⨀\frac{\partial e}{\partial \delta \xi }=-\left ( exp\left ( \xi ^{\wedge } \right )p^{'}_{i} \right )^{\bigodot }∂δξ∂e=−(exp(ξ∧)pi′)⨀
在非线性优化中，只需要不断的迭代，就能找到极小值
这里ICP指已由图像特征给定了匹配的情况下进行位姿估计的问题。在匹配已知情况下，最小二乘解具有解析解，没必要进行迭代优化
在RGB-D SLAM中，由于一个像素的深度数据可能测不到，可以混合使用PnP和ICP优化：

对深度已知的特征点，建模3D-3D误差。
对于深度未知的特征点，建模3D-2D的重投影误差

10 实践：求解ICP

10.1 SVD方法

void pose_estimation_3d3d (const vector<Point3f>& pts1,const vector<Point3f>& pts2,Mat& R, Mat& t
)
{Point3f p1, p2;     // center of massint N = pts1.size();for ( int i=0; i<N; i++ ){p1 += pts1[i];p2 += pts2[i];}p1 = Point3f( Vec3f(p1) /  N);p2 = Point3f( Vec3f(p2) / N);vector<Point3f>     q1 ( N ), q2 ( N ); // remove the centerfor ( int i=0; i<N; i++ ){q1[i] = pts1[i] - p1;q2[i] = pts2[i] - p2;}// compute q1*q2^TEigen::Matrix3d W = Eigen::Matrix3d::Zero();for ( int i=0; i<N; i++ ){W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();}cout<<"W="<<W<<endl;// SVD on WEigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );Eigen::Matrix3d U = svd.matrixU();Eigen::Matrix3d V = svd.matrixV();if (U.determinant() * V.determinant() < 0){for (int x = 0; x < 3; ++x){U(x, 2) *= -1;}}cout<<"U="<<U<<endl;cout<<"V="<<V<<endl;Eigen::Matrix3d R_ = U* ( V.transpose() );Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );// convert to cv::MatR = ( Mat_<double> ( 3,3 ) <<R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 ));t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}

结果：

-- Max dist : 95.000000
-- Min dist : 7.000000
一共找到了81组匹配点
3d-3d pairs: 75
W=  11.8688 -0.717698   1.89486-1.88065   3.83391  -5.782193.25846  -5.82734   9.65267
U=  0.592295  -0.805658 -0.0101195-0.418171  -0.318113   0.8508450.688709   0.499719   0.525319
V=   0.64736  -0.761401 -0.0345329-0.388765  -0.368829   0.8442910.655581   0.533135   0.534772
ICP via SVD results:
R = [0.9972065647956201, 0.05834273442898391, -0.04663895869192625;-0.05787745545449197, 0.998260122172866, 0.01126626067193237;0.04721511705620757, -0.008535444848613793, 0.9988482762174666]
t = [0.1379879629890433;-0.06551699470729988;-0.02981649388290575]
R_inv = [0.9972065647956201, -0.05787745545449197, 0.04721511705620757;0.05834273442898391, 0.998260122172866, -0.008535444848613793;-0.04663895869192625, 0.01126626067193237, 0.9988482762174666]
t_inv = [-0.1399866702492459;0.05709791102272541;0.03695589996443214]

10.2 非线性优化方法

// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {}virtual void computeError(){const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );// measurement is p, point is p'_error = _measurement - pose->estimate().map( _point );}virtual void linearizeOplus(){g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);g2o::SE3Quat T(pose->estimate());Eigen::Vector3d xyz_trans = T.map(_point);double x = xyz_trans[0];double y = xyz_trans[1];double z = xyz_trans[2];_jacobianOplusXi(0,0) = 0;_jacobianOplusXi(0,1) = -z;_jacobianOplusXi(0,2) = y;_jacobianOplusXi(0,3) = -1;_jacobianOplusXi(0,4) = 0;_jacobianOplusXi(0,5) = 0;_jacobianOplusXi(1,0) = z;_jacobianOplusXi(1,1) = 0;_jacobianOplusXi(1,2) = -x;_jacobianOplusXi(1,3) = 0;_jacobianOplusXi(1,4) = -1;_jacobianOplusXi(1,5) = 0;_jacobianOplusXi(2,0) = -y;_jacobianOplusXi(2,1) = x;_jacobianOplusXi(2,2) = 0;_jacobianOplusXi(2,3) = 0;_jacobianOplusXi(2,4) = 0;_jacobianOplusXi(2,5) = -1;}bool read ( istream& in ) {}bool write ( ostream& out ) const {}
protected:Eigen::Vector3d _point;
};

calling bundle adjustment
iteration= 0    chi2= 18157.747747     time= 3.7213e-05   cumTime= 3.7213e-05    edges= 75  schur= 0
iteration= 1    chi2= 18151.933202     time= 1.1232e-05   cumTime= 4.8445e-05    edges= 75  schur= 0
iteration= 2    chi2= 18151.932131     time= 1.0144e-05   cumTime= 5.8589e-05    edges= 75  schur= 0
iteration= 3    chi2= 18151.932130     time= 1.097e-05    cumTime= 6.9559e-05    edges= 75  schur= 0
iteration= 4    chi2= 18151.932130     time= 9.71e-06     cumTime= 7.9269e-05    edges= 75  schur= 0
iteration= 5    chi2= 18151.932130     time= 9.615e-06    cumTime= 8.8884e-05    edges= 75  schur= 0
iteration= 6    chi2= 18151.932130     time= 9.521e-06    cumTime= 9.8405e-05    edges= 75  schur= 0
iteration= 7    chi2= 18151.932130     time= 9.583e-06    cumTime= 0.000107988   edges= 75  schur= 0
iteration= 8    chi2= 18151.932130     time= 9.505e-06    cumTime= 0.000117493   edges= 75  schur= 0
iteration= 9    chi2= 18151.932130     time= 9.484e-06    cumTime= 0.000126977   edges= 75  schur= 0
optimization costs time: 0.000380717 seconds.after optimization:
T=0.997207   0.0583427   -0.046639    0.137988-0.0578775     0.99826   0.0112663   -0.0655170.0472151 -0.00853546    0.998848  -0.02981690           0           0           1
p1 = [-0.0374123, -0.830816, 2.7448]
p2 = [-0.0111479, -0.746763, 2.7652]
(R*p2+t) = [-0.04566300488482969;-0.7791822151698653;2.738046267661636]p1 = [-0.243698, -0.117719, 1.5848]
p2 = [-0.299118, -0.0975683, 1.6558]
(R*p2+t) = [-0.243212054430598;-0.1269486029625337;1.610786345002579]p1 = [-0.627753, 0.160186, 1.3396]
p2 = [-0.709645, 0.159033, 1.4212]
(R*p2+t) = [-0.6266796777024644;0.1503229238263245;1.354883323538178]p1 = [-0.323443, 0.104873, 1.4266]
p2 = [-0.399079, 0.12047, 1.4838]
(R*p2+t) = [-0.3221508525590339;0.09455772952719482;1.432403794814274]p1 = [-0.627221, 0.101454, 1.3116]
p2 = [-0.709709, 0.100216, 1.3998]
(R*p2+t) = [-0.6291763602679332;0.09137127679150184;1.334006907588644]

SLAM系列——视觉里程计（特征法）相关推荐

SLAM 06.视觉里程计-3-直接法
1.特征法和直接法比较 1.1.特征法的缺点特征点法有如下几个缺点: 特征点法需要提取多个特征点以及描述子,并且要进行多个特征点之间的匹配,运算量很大,难于满足实时要求.SIFT目前在CPU上是无法 ...
SLAM 05.视觉里程计-2-特征法
相机模型是理解视觉里程计之前的基础.本文主要是对高翔博士的<SLAM十四讲>的总结. 我们希望测量一个运动物体的轨迹,这可以通过许多不同的手段来实现.例如,我们在汽车轮胎上安装轮式计数码盘 ...
SLAM之视觉里程计和回环检测
1. 通常的惯例是把 VSLAM 分为前端和后端.前端为视觉里程计和回环检测,相当于是对图像数据进行关联:后端是对前端输出的结果进行优化,利用滤波或非线性优化理论,得到最优的位姿估计和全局一致性地图. ...
计算机视觉与深度学习 | 开源SLAM、视觉里程计综述（SLAM、Visual Odometry）
视觉里程计综述引言 Visual Odometry or VSLAM OF-VO:Robust and Efficient Stereo Visual Odometry Using Points a ...
计算机视觉与深度学习 | SLAM、视觉里程计、VIO、Net-SLAM、激光SLAM、语义SLAM、数据集（ state-of-the-art）
============================================== 博主github:https://github.com/MichaelBeechan 博主CSDN:htt ...
VSLAM视觉里程计总结
相机模型是理解视觉里程计之前的基础.视觉里程计(VIO)主要分为特征法和直接法.如果说特征点法关注的是像素的位置差,那么,直接法关注的则是像素的颜色差.特征点法通常会把图像抽象成特征点的集合,然后去缩 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-对极几何和对极约束、本质矩阵、基础矩阵
专栏系列文章如下: 专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLA ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点法和特征提取和匹配实践
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
【视觉SLAM十四讲】视觉里程计—特征点法
本文为视觉 SLAM 学习笔记,讲解视觉里程计中的特征点法. 本讲内容概要图像特征的意义,在单幅及多幅图像中提取特征点. 对极几何的原理,利用对极几何的约束恢复图像间相机的三维运动 PnP 问题,利 ...

SLAM系列——视觉里程计（特征法）

目录