高翔博士SLAM十四讲（第二篇）

ch7 视觉里程计1

库的基本使用见上一篇，本篇侧重于：1.利用库实现算法；2.或库自带的算法的使用
高博写的简介明了（建议读），这里仅做一下总结。
【这部分内容的本质就是去求解 位姿 R, t （各种情况下的）】

第一部分 对极几何，2D-2D图像之间的摄像机三维运动

步骤	（特征匹配的）实现
读取图像	cv::Mat img_1 = cv:: imread ( argv[1], CV_LOAD_IMAGE_COLOR );
	初始化
存放关键点的变量	std::vector<cv::KeyPoint> keypoints_1, keypoints_2; 类型 KeyPoint
存放描述子的变量	cv::Mat descriptors_1, descriptors_2; 类型 Mat
存放匹配关系的变量	std::vector<cv::DMatch> matches; 类型 DMatch
关键点检测器	cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create(); ORB
描述子计算器	cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create(); ORB
匹配器	cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create ( "BruteForce-Hamming" ); 暴力匹配+汉明距离度量
	步骤
1.检测给定图片关键点	detector->detect ( img_1,keypoints_1 );
2.计算关键点的描述子	descriptor->compute ( img_1, keypoints_1, descriptors_1 );
3.匹配	matcher->match ( descriptors_1, descriptors_2, matches );
4.筛选	if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) ) good_matches.push_back ( matches[i] );
	绘图+imshow+waitKey(0)
	cv::drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch );

步骤	（利用前面的特征匹配，求得位姿的）实现
1.前面的内容，特征提取与匹配	find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches ); 高博将前面的内容封装成了函数
2.根据特征匹配，求出位姿，即R和t	pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t ); 此亦为高博封装的函数，是本节的总结的重点
	pose_estimation_2d2d的流程
2.1计算本质矩阵	essential_matrix = cv::findEssentialMat ( points1, points2, focal_length, principal_point );
2.2从本质矩阵解出R，t	cv::recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
参数解释	focal_length和principal_point是相机的内参，points1和points2是两幅图的匹配的特征点（注意元素顺序并放在vector里），这两个函数均为opencv自带（本质上只需为它提供所需参数）

*步骤	（根据前面特征点对，两幅图各自的位姿，求解特征点对应的世界坐标的）实现
*3.即，三角测量	cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );
	高博将其封装为triangulation()，核心即上面一句，其余部分为参数准备及结果处理

第二部分 PNP 已知世界坐标(3D)和像素坐标(2D)求解相机运动

步骤	实现
核心一句	cv::solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false,cv::SOLVEPNP_EPNP );
输入参数解释	3D-2D对：pts_3d及pts_2d；相机内参及畸变矩阵：K及Mat()
输出参数解释	旋转矢量：r；位移矢量：t；是否优化：false；PnP的解法：SOLVEPNP_EPNP

g2o优化部分先略过

第三部分 ICP 已知3D-3D信息

如，两幅RGB-D图像，求解位姿

步骤	实现（高博根据SVD法自己编写的函数）
1求质心	两幅图质心分别为：$p,p^{\prime }$
2.各点去质心	$q_i=p_i-p$
3.求W矩阵	$W={\Sigma }_{i=1}^n{q}_i{q}_i^T$
4.SVD分解，求R	$W=U\Sigma V^T,R=U{V}^T$
5.求t	$t=p-R{p}^{\prime }$

g2o优化部分下一步继续