视觉SLAM十四讲笔记 第五讲视觉里程计 特征点法
·
前言：
视觉里程计是SLAM中的一个重要模块。视觉里程计通过相邻的图像，进而估计相邻图像之间是如何运动的。这个估计会有累计误差，通过后端优化和回环检测来消除这个误差。
一、特征点的提取与匹配
特征点法的思路是：选取图像中的特征点，这些点能在帧间保持不变，从而可以在连续的两帧图像中观察到这些特征点，进而根据这些点的像素变化，根据几何学来估计相机运动。这些特征点被称为SLAM中的路标。
特征点是图像中具有代表性的点，要求具有可重复性（在不同的图像中看到这些点的时候都可以提取出这些点）、可区别性（在不同图像中看到的特征点是有差别的）、高效（提取特征点不能花费过多时间）等特点。特征点的位置、大小、方向、评分等信息被称为关键点；特征点周围的图像信息被称为描述子(Descriptor)。
关键问题：如何提取特征点即快速又具有良好的可重复性和可区别性。在SLAM这种对实时性要求很高的场合中，偏向使用一些实时性比较好的特征，比如ORB、Brife这样的一些特征。特征是有很多种的，我们倾向于提取角点，但是角点和角点之间有所不同。有的角点是通过计算奇异值来提取角点，有的角点提取方式比较简单，比如FAST，只是简单比较一下这个点和附近点的大小关系。
特征点提取出来后，获得的特征点位置、大小、方向、评分等信息被称为关键点；特征点周围的图像信息称为描述子（Descriptor）；描述子会对特征点周围一块的图像信息进行特定的编码。描述子有很多种，如ORB有ORB的描述子，SIFT有SIFT的描述子等等。本讲以ORB为例。
ORB特征
ORB特征由FAST关键点和改进后的Brief描述子组成。
FAST关键点：FAST关键点是一种计算非常快速的关键点。其核心思想是比较一个像素点和其周围像素点的灰度值大小关系，如果一个像素点周围连续N个点的灰度值的大小关系满足所定义的阈值，则可作为关键点。
除此之外，还有一些其他必要的操作。比如说N取12时可以先检测第1,5,9,13个点是否满足特征条件，以快速筛选。获取特征点后会出现特征点扎堆的情况，需要对各区域进行非极大值抑制来减少相邻特征点的数目 （作者：MizarLyc https://www.bilibili.com/read/cv13728217/ 出处：bilibili）；
Oriented FAST:
ORB特征在FAST关键点的基础上进行了改进，增加了尺度和旋转的特征，保证了特征点在缩放和旋转后仍能被检测。增加的尺度特征（旋转不变性）由图像金字塔来实现，对于不同尺度下的特征点，我们都可以在图像金字塔中寻找对应的匹配，从而实现尺度不变性。旋转不变性由灰度质心法来实现，计算步骤如下：
BRIEF描述子：
Brief描述子是一个二进制描述子，其描述向量由0和1编码组成。其编码方式是按照一定的pattern（如下图）或者随机选择周围两个像素p，q；如果像素点p的亮度高于q，则编码为1，反之编码为0。重复N次编码的过程，编码出一个N维描述向量。原始的brief描述子没有旋转不变性这个性质，ORB特征将brief改良为steer brief，使其具备旋转不变性，使得ORB对旋转具有一定的鲁棒性。
特征匹配：
提取出特征点后，接下来要做的事情就是对两帧图像之间的特征点进行匹配，根据匹配关系，利用几何学知识求解位姿变化。特征点匹配是根据描述子的差异来判断的，最简单最暴力的匹配方式被称为暴力匹配，即图A中的特征点与图B中的特征点逐一进行比较。通过计算描述子找到对应关系。
实践部分1：使用OpenCV进行特征匹配
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <chrono>
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);
assert(img_1.data != nullptr && img_2.data != nullptr);
//-- 初始化
std::vector<KeyPoint> keypoints_1, keypoints_2;
Mat descriptors_1, descriptors_2;
Ptr<FeatureDetector> detector = ORB::create(); //声明OpenCV中的ORB是它的一个类
Ptr<DescriptorExtractor> descriptor = ORB::create();
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming"); //使用DescriptorMatcher类对两个图像进行匹配，这个类在创建的时候可以指定使用的方法（在头文件中），这里用的是暴力匹配hamming；
//-- 第一步:检测 Oriented FAST 角点位置
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
detector->detect(img_1, keypoints_1);
detector->detect(img_2, keypoints_2); //调用detect函数，从img中提取特征
//-- 第二步:根据角点位置计算 BRIEF 描述子
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;
Mat outimg1;
drawKeypoints(img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
imshow("ORB features", outimg1);
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
vector<DMatch> matches;
t1 = chrono::steady_clock::now();
matcher->match(descriptors_1, descriptors_2, matches);
t2 = chrono::steady_clock::now();
time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;
//-- 第四步:匹配点对筛选
// 计算最小距离和最大距离
auto min_max = minmax_element(matches.begin(), matches.end(),
[](const DMatch &m1, const DMatch &m2) { return m1.distance < m2.distance; });
double min_dist = min_max.first->distance;
double max_dist = min_max.second->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);
imshow("all matches", img_match);
imshow("good matches", img_goodmatch);
waitKey(0);
return 0;
}
实验结果如图所示：
1、特征点提取
2、暴力匹配
3、过滤点后
实验部分2：手写ORB特征
typedef vector<uint32_t> DescType; // 描述子类型
// 读取图片
cv::Mat first_image = cv::imread(first_file, 0);
cv::Mat second_image = cv::imread(second_file, 0); //使用OpenCV中的cv::imread函数读取两个灰度图像first_image和second_image
assert(first_image.data != nullptr && second_image.data != nullptr);
// 提取FAST关键点，其中阈值T为40,将提取的关键点存储在Keypoint1和Keypoint2中
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
vector<cv::KeyPoint> keypoints1;
cv::FAST(first_image, keypoints1, 40);
vector<DescType> descriptor1;
ComputeORB(first_image, keypoints1, descriptor1);//使用自己写的ComputeORB方法，计算ORB描述子，并将结果存储在descriptor1和descriptor2中
vector<cv::KeyPoint> keypoints2;
vector<DescType> descriptor2;
cv::FAST(second_image, keypoints2, 40);
ComputeORB(second_image, keypoints2, descriptor2);
//获取提取用时
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;
// 计算匹配
vector<cv::DMatch> matches;
t1 = chrono::steady_clock::now();
BfMatch(descriptor1, descriptor2, matches);//使用自定义的Bfmatch函数，将两个图像的描述子进行匹配，并将结果存储在matches中
t2 = chrono::steady_clock::now();
time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;
cout << "matches: " << matches.size() << endl;
// 显示匹配结果并保存
cv::Mat image_show;
cv::drawMatches(first_image, keypoints1, second_image, keypoints2, matches, image_show);
cv::imshow("matches", image_show);
cv::imwrite("matches.png", image_show);
cv::waitKey(0);
// ORB特征计算
void ComputeORB(const cv::Mat &img, vector<cv::KeyPoint> &keypoints, vector<DescType> &descriptors) { //ComputerORB函数计算ORB描述子，根据关键点和描述子之间的Hamming距离决定匹配结果
const int half_patch_size = 8;
const int half_boundary = 16;
int bad_points = 0;
//对每个FAST关键点进行操作
for (auto &kp: keypoints) {
//去除坏点
if (kp.pt.x < half_boundary || kp.pt.y < half_boundary ||
kp.pt.x >= img.cols - half_boundary || kp.pt.y >= img.rows - half_boundary) {
// outside
bad_points++;
descriptors.push_back({});
continue;
} //迭代处理关键点，如果关键点位于图像边界附近，则将其视为badpoint，跳过处理，防止越界访问
//灰度质心法，计算方向，确保描述子具有旋转不变性
//计算质心
float m01 = 0, m10 = 0; //m10和mo1分别表示水平方向和垂直方向上的质心，质心用于计算描述子的旋转角度；
for (int dx = -half_patch_size; dx < half_patch_size; ++dx) {
for (int dy = -half_patch_size; dy < half_patch_size; ++dy) {
uchar pixel = img.at<uchar>(kp.pt.y + dy, kp.pt.x + dx);
m10 += dx * pixel;
m01 += dy * pixel;
}
}
// 计算方向向量
float m_sqrt = sqrt(m01 * m01 + m10 * m10) + 1e-18; // avoid divide by zero
float sin_theta = m01 / m_sqrt;
float cos_theta = m10 / m_sqrt;
// 描述子计算
//生成256位的二进制描述向量；每个关键点计算256位描述子，分成8个32位整数
DescType desc(8, 0);
for (int i = 0; i < 8; i++) {
uint32_t d = 0;
for (int k = 0; k < 32; k++) {
int idx_pq = i * 32 + k;
//ORB_pattern是一个256*4的数组，猜测与描述子生成规则有关
//随机选取两个点p,q
cv::Point2f p(ORB_pattern[idx_pq * 4], ORB_pattern[idx_pq * 4 + 1]);
cv::Point2f q(ORB_pattern[idx_pq * 4 + 2], ORB_pattern[idx_pq * 4 + 3]);
// 添加旋转，使描述子具有旋转不变性
cv::Point2f pp = cv::Point2f(cos_theta * p.x - sin_theta * p.y, sin_theta * p.x + cos_theta * p.y)
+ kp.pt;
cv::Point2f qq = cv::Point2f(cos_theta * q.x - sin_theta * q.y, sin_theta * q.x + cos_theta * q.y)
+ kp.pt;
if (img.at<uchar>(pp.y, pp.x) < img.at<uchar>(qq.y, qq.x)) {
d |= 1 << k;
}
}
desc[i] = d;
}
descriptors.push_back(desc);
}
cout << "bad/total: " << bad_points << "/" << keypoints.size() << endl;
}
// 暴力匹配算法，两重遍历筛选出最佳匹配
void BfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches) {
const int d_max = 40; //规定匹配描述子之间最大的Hamming距离阈值为40，如果超过这个阈值，则不认为是有效匹配
for (size_t i1 = 0; i1 < desc1.size(); ++i1) { //遍历desc1和desc2中的描述子，跳过空描述子
if (desc1[i1].empty()) continue;
cv::DMatch m{i1, 0, 256};
for (size_t i2 = 0; i2 < desc2.size(); ++i2) {
if (desc2[i2].empty()) continue;
int distance = 0;
for (int k = 0; k < 8; k++) {
distance += _mm_popcnt_u32(desc1[i1][k] ^ desc2[i2][k]);
}
if (distance < d_max && distance < m.distance) {
m.distance = distance;
m.trainIdx = i2;
}
}
if (m.distance < d_max) {
matches.push_back(m);
}
}
} 二、相机运动估计
上述特征匹配完后，根据特征点之间的对应关系，去进行相机运动估计；根据不同相机类型，可将问题分成如下三类：
1、如果只有两个单目图像，我们需要根据两组2D点估计运动----对极几何
2、如果匹配的是双目或者RGB-D相机，我们需要根据两组3D点去估计运动----ICP
3、为了节省计算资源，我们可以使用一组2D点和一组3D点估计运动----PNP
对极几何
根据若干对匹配点，恢复两帧之间的相机运动。
如图所示，两帧图像I1和I2，相机光心为O1和O2，从第一帧到第二帧的旋转矩阵和平移向量为R和t；考虑第一帧图像中有一个特征点P1，在第二帧图像中对应位P2，它们是同一空间点P在两个平面上的投影，连线O1P1和O2P2交于点P，O1O2P三点构成的平面称为极平面；O1O2连线交极平面与e1e2两点，称为极点；极平面和象平面的交线l1和l2称为极线，O1、O2称为基线；从第一帧的角度上看，射线O1P上的所有点都有可能是P点存在的位置，若不知道P点真实位置，则I2平面上的极线l2都是P2可能出现的投影位置。不过现在我们通过特征匹配确定了P2的位置，则可通过几何关系来推断P点位置；现在我们对这个几何关系进行建模：
对极约束
设空间点P在第一帧坐标系的坐标为：
根据针孔模型
有如下关系
对于用齐次坐标表示像素点坐标，其乘上任一一个向量表示投影关系，我们称之为尺度意义下相等，记作
那么，上述式子进行如下推导：
我们将上述式子重新带回到像素平面：
这个式子就称为对极约束，这个约束中包含了旋转和平移，我们将中间部分分别记作基础矩阵F和本质矩阵E，写作如下：
现在要根据匹配点信息求解本质矩阵E和基础矩阵F，再根据本质矩阵E和基础矩阵F求解旋转矩阵R和平移向量t。
求解本质矩阵E
首先讨论下本质矩阵的性质：
1、对极约束是等式为0的约束，所以本质矩阵E乘以任一非零常数仍满足对极约束，这个性质称为本质矩阵E在不同尺度下等价。
2、
3、由于平移和旋转各具有三个自由度，而不同尺度下等价丢失一个自由度，从而本质矩阵E实际上有五个自由度。
因为E有五个自由度，所以我们至少可以用五组点来求解E，不过本质矩阵内在的非线性性质会使只用五个点求解的时候非常麻烦。因为本质矩阵本质还是一个3x3矩阵，如果仅仅丢失不同尺度下等价这个自由度，而不考虑其表达平移和旋转的冗余自由度，那我们可以用八个点来求解E，称为八点法。
这八个方程构成线性方程组，如果系数矩阵满秩，那么本质矩阵E中的各个元素都可以解出来；最后由求解出的E来恢复出相机运动的R和t。这个过程由奇异值分解（SVD）来解决。（SVD求解过程跳过）；
求解单应矩阵
除了本质矩阵和基本矩阵，二试图几何还有一种常见的矩阵，称为单应矩阵H；它描述的是两个平面之间的映射关系。若场景中的特征点都落在同一平面上，则可以通过单应性来进行运动估计。接下来进行单应矩阵的数学建模
和对极约束中尺度意义下相等原则一样
这里记H为单应矩阵。首先考虑一对匹配点的情况
和求解E差不多，先计算H，再用H恢复R,t,n,d,K。
在SLAM中，单应性具有很重要的意义，因为当出现特征点共面的情况或者相机发生纯旋转而无平移时，本质矩阵的自由度下降，出现退化的情况。此时退化的自由度反而由噪声主导，对实验结果造成负面影响。为了SLAM过程中的鲁棒性，我们选择同时计算本质矩阵E和单应矩阵H，选择重投影误差（所谓的重投影误差就是将最终估计空间点P重新通过针孔模型投影到二视图中，两个投影点与真实点像素误差的范数的平方和称为重投影误差）较小的一个作为最终的运动估计矩阵。
实验部分3：2D-2D对极几何
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#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);
assert(img_1.data && img_2.data && "Can not load images!"); //验证图像是否能成功加载
vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches); //调用find_feature_matches函数来查找特征点和匹配，输出找到的匹配数量
cout << "一共找到了" << matches.size() << "组匹配点" << endl;
//-- 估计两张图像间运动
Mat R, t;
pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t); //调用pose_estimation_2d2d函数，通过特征匹配估计旋转矩阵R和平移t
//-- 验证E=t^R*scale，矩阵t_x表示平移向量的反对称矩阵，t_x*R应当是零矩阵，满足对极约束，反之，则存在误差
Mat 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) { //DMatch m表示一个特征匹配
Point2d pt1 = pixel2cam(keypoints_1[m.queryIdx].pt, K); //queryIdx是第一个图像中的特征点索引，这里pixe12cam..表示将第一个图像中的特征点从像素坐标转换位相机坐标。
Mat y1 = (Mat_<double>(3, 1) << pt1.x, pt1.y, 1); //将特征点坐标转换为3x1矩阵，用于进行矩阵运算
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;
}
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)
);
}
void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
std::vector<KeyPoint> keypoints_2,
std::vector<DMatch> matches,
Mat &R, Mat &t) {
// 相机内参,TUM Freiburg2
Mat 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);
points2.push_back(keypoints_2[matches[i].trainIdx].pt);
}
//-- 计算基础矩阵
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT); //8点法计算基本矩阵
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;
//-- 从本质矩阵中恢复旋转和平移信息.
// 此函数仅在Opencv3中提供
recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
cout << "R is " << endl << R << endl;
cout << "t is " << endl << t << endl;
}
实验结果：
由结果我们可以看到，各组匹配点的对极约束都十分接近于0，由于噪声存在导致不为0；说明算法是没有问题的。
说明问题： 由于本质矩阵E具有尺度等价性，其分解得到的R,t也具有尺度等价性。在分解的过程中，t乘以任意一个非零常数等式都是成立的。因此，我们通常归一化t，令其为1；我们称这种现象为单目视觉尺度具有不确定性。而我们对两张图像进行t归一化处理，相当于固定了一个尺度，尽管我们不知道这个尺度代表的物理值实际上是多少，但我们以此为一个单位长度，完成了单目SLAM不可避免的初始化过程。
除此之外，单目SLAM还有一个初始化纯旋转问题，如果初始化相机运动是纯旋转运动，那么t=0则本质矩阵E也为0，此时无法通过求解本质矩阵来求R。所以单目初始化必须有一定的平移。 
三、PnP
PnP是求解3D到2D点对的方法，它描述了当知道n个3D空间点的真实位置及其投影位置时，如何去估计相机的位姿。可以通过代数的解法（DLT、P3P、EPnP..）或者优化的解法（BA）。
DLT (Direct Linear Transformation)：
矩阵t包含了旋转和平移的信息。通过像素齐次坐标的最后一行约去s，可以得到两个有关系数矩阵t的约束，t一共有12个自变量，最少需要六对没有退化情况的点就可以求解出矩阵t。在根据t对旋转矩阵和平移向量构造即可；
BA (Bundle Adjustment):
采用非线性优化的方式，构建最小二乘问题求解。在这个模型下构建的最小二乘问题称为光束法平差（Bundle Adjustment,BA）.BA将相机位姿和空间点同时作为优化变量进行优化，是一种非常通用的求解方式。BA求解方法如下：
该最小二乘问题的本质实际上是找到一个位姿，使得空间点集中通过针孔模型投影到像素平面上的点集与实际在这一帧观测到的点集之间误差和最小。（这个误差就是投影位置和观测位置做差，称为重投影误差，如图所示）
优化算法中，我们使用李代数来进行优化，因为李代数没有李群的各种约束条件。根据非线性优化的内容，我们需要求出代价函数的一阶导数，即雅可比矩阵，才可以顺利的使用编程工具完成优化过程。首先：
当误差维数为像素坐标误差（2维），x为se(3)李代数表示的位姿（6维）时，雅可比矩阵的转置是一个2x6的矩阵。
求投影误差时，可以用这里的u,v值与观测值进行做差。现在我们 对T进行左乘扰动，对e求扰动的量导数，根据链式法则：
根据上面u,v的表达式和误差项的表达式，我们可以得到：
第二项时变换后的点关于李代数的导数，根据左乘扰动模型的结果可以得到
因为我们只取了前三维的非齐次坐标，所以有
将所得到的这两部分相乘，可以得到：
如果我们构建优化空间点位置的最小二乘问题，即
则需要雅可比矩阵根据链式法则：
第一项以及求解了，现在只需求解式子中的第二项：
从而可以得到：
四、ICP
给定匹配好的两组3D点，求其旋转和平移。可用ICP的方法求解。
运动关系是，和PnP用同样的方法定义误差项：
。同样的目标是找到一个欧式变换R,t，使得误差项范数平方和最小：
可以通过SVD方法和非线性优化这两种方法来求解，这里暂时不展开，后续补充。
五、三角化与深度估计
当我们已知两帧图像之间相机是如何运动的之后，想要得到特征点的真是空间位置，由于单目相机无法直接获取深度信息，这个问题我们通过三角测量来完成。
几何关系：s1x1=s2Rx2+t；求解很简单：
六、实践部分
3D-2D DLT:
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <chrono>
using namespace std;
using namespace cv;
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);
// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K);
// BA by g2o
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;
void bundleAdjustmentG2O(
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3d &pose
);
// BA by gauss-newton
void bundleAdjustmentGaussNewton(
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3d &pose
);
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);
assert(img_1.data && img_2.data && "Can not load images!");
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;
// 通过RGB-D的深度信息，建立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 depth
continue;
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;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
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公式转换为矩阵
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;
cout << "R=" << endl << R << endl;
cout << "t=" << endl << t << endl;
VecVector3d pts_3d_eigen;
VecVector2d pts_2d_eigen;
for (size_t i = 0; i < pts_3d.size(); ++i) {
pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
}
cout << "calling bundle adjustment by gauss newton" << endl;
Sophus::SE3d pose_gn;
t1 = chrono::steady_clock::now();
bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn);
t2 = chrono::steady_clock::now();
time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << endl;
cout << "calling bundle adjustment by g2o" << endl;
Sophus::SE3d pose_g2o;
t1 = chrono::steady_clock::now();
bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o);
t2 = chrono::steady_clock::now();
time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve pnp by g2o cost time: " << time_used.count() << " seconds." << endl;
return 0;
}
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)
);
}//将像素坐标转换为相机归一化坐标
void bundleAdjustmentGaussNewton(//实现了高斯牛顿法进行图优化，用于优化相机位姿。它通过计算重投影误差来定义优化问题，然后使用高斯牛顿法来迭代优化相机位姿
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3d &pose) {
typedef Eigen::Matrix<double, 6, 1> Vector6d;
const int iterations = 10;
double cost = 0, lastCost = 0;
double fx = K.at<double>(0, 0);
double fy = K.at<double>(1, 1);
double cx = K.at<double>(0, 2);
double cy = K.at<double>(1, 2);
for (int iter = 0; iter < iterations; iter++) {
Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
Vector6d b = Vector6d::Zero();
cost = 0;
// compute cost
for (int i = 0; i < points_3d.size(); i++) {
Eigen::Vector3d pc = pose * points_3d[i];
double inv_z = 1.0 / pc[2];
double inv_z2 = inv_z * inv_z;
Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);
Eigen::Vector2d e = points_2d[i] - proj;
cost += e.squaredNorm();
Eigen::Matrix<double, 2, 6> J;
J << -fx * inv_z,
0,
fx * pc[0] * inv_z2,
fx * pc[0] * pc[1] * inv_z2,
-fx - fx * pc[0] * pc[0] * inv_z2,
fx * pc[1] * inv_z,
0,
-fy * inv_z,
fy * pc[1] * inv_z2,
fy + fy * pc[1] * pc[1] * inv_z2,
-fy * pc[0] * pc[1] * inv_z2,
-fy * pc[0] * inv_z;
H += J.transpose() * J;
b += -J.transpose() * e;
}
Vector6d dx;
dx = H.ldlt().solve(b);
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost >= lastCost) {
// cost increase, update is not good
cout << "cost: " << cost << ", last cost: " << lastCost << endl;
break;
}
// update your estimation
pose = Sophus::SE3d::exp(dx) * pose;
lastCost = cost;
cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
if (dx.norm() < 1e-6) {
// converge
break;
}
}
cout << "pose by g-n: \n" << pose.matrix() << endl;
}
/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
virtual void setToOriginImpl() override {
_estimate = Sophus::SE3d();
}
/// left multiplication on SE3
virtual void oplusImpl(const double *update) override {
Eigen::Matrix<double, 6, 1> update_eigen;
update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
}
virtual bool read(istream &in) override {}
virtual bool write(ostream &out) const override {}
};
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}
virtual void computeError() override {
const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
Sophus::SE3d T = v->estimate();
Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
pos_pixel /= pos_pixel[2];
_error = _measurement - pos_pixel.head<2>();
}
virtual void linearizeOplus() override {
const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
Sophus::SE3d T = v->estimate();
Eigen::Vector3d pos_cam = T * _pos3d;
double fx = _K(0, 0);
double fy = _K(1, 1);
double cx = _K(0, 2);
double cy = _K(1, 2);
double X = pos_cam[0];
double Y = pos_cam[1];
double Z = pos_cam[2];
double Z2 = Z * Z;
_jacobianOplusXi
<< -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
}
virtual bool read(istream &in) override {}
virtual bool write(ostream &out) const override {}
private:
Eigen::Vector3d _pos3d;
Eigen::Matrix3d _K;
};
void bundleAdjustmentG2O(
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3d &pose) {
// 构建图优化，先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType; // pose is 6, landmark is 3
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// vertex
VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
vertex_pose->setId(0);
vertex_pose->setEstimate(Sophus::SE3d());
optimizer.addVertex(vertex_pose);
// K
Eigen::Matrix3d K_eigen;
K_eigen <<
K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);
// edges
int index = 1;
for (size_t i = 0; i < points_2d.size(); ++i) {
auto p2d = points_2d[i];
auto p3d = points_3d[i];
EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
edge->setId(index);
edge->setVertex(0, vertex_pose);
edge->setMeasurement(p2d);
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(10);
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 << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
pose = vertex_pose->estimate();
因暂时没有对g2o库学习的需求，后续补充，未完待续哈哈
魔乐社区（Modelers.cn) 是一个中立、公益的人工智能社区，提供人工智能工具、模型、数据的托管、展示与应用协同服务，为人工智能开发及爱好者搭建开放的学习交流平台。社区通过理事会方式运作，由全产业链共同建设、共同运营、共同享有，推动国产AI生态繁荣发展。
更多推荐
43
0- 0
已为社区贡献2条内容
所有评论(0)