Slam14讲学习笔记

(七) 视觉里程计

本讲主要使用了特征提取关键点和描述子,对极约束估计位姿(RT),三角计算估计深度等

  • 头文件及函数声明
#include
#include
#include
#include
#include

using namespace std;
using namespace cv;

//本程序演示了如何使用2D-2D的特征匹配估计相机运动

void find_feature_matches(
const Mat& img_1, const Mat& img_2,
std::vector& keypoints_1,
std::vector& keypoints_2,
std::vector& matches);

void pose_estimation_2d2d(
std::vector keypoints_1,
std::vector keypoints_2,
std::vector matches,
Mat& R, Mat& t);
//三角计算
void triangulation(
const vector& keypoint_1,
const vector& keypoint_2,
const std::vector& matches,
const Mat& R, const Mat& t,
vector& points
);
//像素坐标转相机归一化坐标 返回值是Point2f类型
Point2f pixel2cam (const Point2d& p, const Mat& K);

接下来先对这四个函数进行分析

  • find_feature_matches 特征匹配
//该函数的功能是输入img_1,img_2,计算得到两组描述子和一个匹配结果
void find_feature_matches(
const Mat& img_1, const Mat& img_2,
std::vector& keypoints_1,
std::vector& keypoints_2,
std::vector& matches)
{
//初始化
Mat descriptors_1, descriptors_2;
//开始使用opencv里面的匹配算法
Ptr detector = ORB::create();//特征提取器
Ptr descriptor = ORB::create(); //描述子
Ptr matcher = DescriptorMatcher::create("BruteForce-Hamming");//匹配结果

//1 检测Oriented Fast 角点位置
detector->detect(img_1, keypoints_1);
detector->detect(img_2, keypoints_2);

//2 根据角点计算BRIEF 描述子
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);

//3 对两幅图像中的BRIEF描述子进行匹配,使用Hamming 距离进行度量
vector match;//用来存放匹配结果 同1 不同0
matcher->match(descriptors_1, descriptors_2, match);

//4 将得到的汉明距离进行筛选 匹配点筛选
double min_dist = 1000, 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]);//将筛选后的描述子放入最终匹配结果
}
}
}
  • 完整代码
#include
#include
#include
#include
#include

using namespace std;
using namespace cv;

//本程序演示了如何使用2D-2D的特征匹配估计相机运动

//函数声明

//特征提取器
void find_feature_matches(
const Mat& img_1, const Mat& img_2,
std::vector& keypoints_1,
std::vector& keypoints_2,
std::vector& matches);

void pose_estimation_2d2d(
std::vector keypoints_1,
std::vector keypoints_2,
std::vector matches,
Mat& R, Mat& t);
//三角计算
void triangulation(
const vector& keypoint_1,
const vector& keypoint_2,
const std::vector& matches,
const Mat& R, const Mat& t,
vector& points
);
//像素坐标转相机归一化坐标 返回值是Point2d类型
Point2f pixel2cam (const Point2d& p, const Mat& K);


int main(int argc, const char** argv) {
if (argc!=3)
{
cout<<"usage:pose_estimation_2d2d img1 img2"< return 1;
}

//读取图像
Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

vector keypoints_1, keypoints_2;
vector matches;

find_feature_matches(img_1, img_2, keypoints_1,keypoints_2 ,matches);
cout<<"一共找到了"<
//估计两张图像间运动
Mat R, t;
pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);

// 三角化
vector points;
triangulation(keypoints_1, keypoints_2, matches, R, t, points);//points 里面是世界坐标系下的点 x y z

//验证三角化点与特征点的重投影关系
Mat K = (Mat_ (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:/n"< cout<<"point projected from 3D/n"<
// 第二个图
Point2f pt2_cam = pixel2cam(keypoints_2[matches[i].trainIdx].pt, K);//取到第二个图上的点
Mat pt2_trans = R*( Mat_(3, 1)< pt2_trans/=pt2_trans.at(2, 0);
cout<<"point in the second camera fram:"< cout<<"point reprojected from second frame:"<
}

return 0;
}

//该函数的功能是输入img_1,img_2,计算得到两组描述子和一个匹配结果
void find_feature_matches(
const Mat& img_1, const Mat& img_2,
std::vector& keypoints_1,
std::vector& keypoints_2,
std::vector& matches)
{
//初始化
Mat descriptors_1, descriptors_2;
//开始使用opencv里面的匹配算法
Ptr detector = ORB::create();//特征提取器
Ptr descriptor = ORB::create(); //描述子
Ptr matcher = DescriptorMatcher::create("BruteForce-Hamming");//匹配结果

//1 检测Oriented Fast 角点位置
detector->detect(img_1, keypoints_1);
detector->detect(img_2, keypoints_2);

//2 根据角点计算BRIEF 描述子
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);

//3 对两幅图像中的BRIEF描述子进行匹配,使用Hamming 距离进行度量
vector match;//用来存放匹配结果 同1 不同0
matcher->match(descriptors_1, descriptors_2, match);

//4 将得到的汉明距离进行筛选 匹配点筛选
double min_dist = 1000, 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]);//将筛选后的描述子放入最终匹配结果
}
}
}

void pose_estimation_2d2d(
std::vector keypoints_1,
std::vector keypoints_2,
std::vector matches,
Mat& R, Mat& t)
{
//相机内参 TUM Freiburg2
Mat K = (Mat_(3,3)<<
520.9, 0, 325.1,
0, 521.0, 249.7,
0, 0, 1);

//把匹配点转换为vector的形式
vector points1;
vector 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);
cout<<"fundamental_matrix is:/n"<
//计算本质矩阵
Point2d principal_point (325.1, 249.7); // 相机光心 TUM dataset 标定值
double focal_length = 521; // 相机焦距
Mat essential_matrix;
essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
cout<<"essential_matrix is/n"<
//计算单应矩阵
Mat homography_matrix;
homography_matrix = findHomography(points1, points2, RANSAC, 3);
cout<<"homography_matrix = /n"<
//从本质矩阵中恢复旋转和平移
recoverPose (essential_matrix, points1, points2, R, t, focal_length, principal_point);
cout<<"R is /n"< cout<<"t is /n"<
}

void triangulation(
const vector& keypoint_1,
const vector& keypoint_2,
const std::vector& matches,
const Mat& R, const Mat& t,
vector& points)
{
Mat T1 = (Mat_(3,4)<<
1,0,0,0,
0,1,0,0,
0,0,1,0);
Mat T2 = (Mat_(3,4)<<
R.at(0, 0), R.at(0, 1), R.at(0, 2), t.at(0, 0),
R.at(1,0), R.at(1,1), R.at(1, 2), t.at(1, 0),
R.at(2,0), R.at(2,1), R.at(2,2), t.at(2,0));// T2=[R t]
// 内参
Mat K = (Mat_(3,3)<< 520.9, 0 ,325.1, 0, 521.0, 249.7, 0, 0, 1);
vector 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(3, 0);//归一化 (x, y, z)/d
Point3d p(
x.at(0,0),
x.at(1,0),
x.at(2,0)
);
points.push_back(p);
}
}

//像素坐标转相机归一化坐标 返回值为Point2d类型
Point2f pixel2cam (const Point2d& p, const Mat& K)
{
return Point2f
(
(p.x - K.at(0,2 ))/K.at(0, 0),
(p.y - K.at(1, 2))/K.at(1, 1)
);
}