视觉里程计1 高翔

最新推荐文章于 2023-05-16 19:17:54 发布

荆黎明

最新推荐文章于 2023-05-16 19:17:54 发布

阅读量335

点赞数

分类专栏： slam 文章标签： slam

本文链接：https://blog.csdn.net/jlm7689235/article/details/107246360

版权

slam 专栏收录该内容

18 篇文章 0 订阅

订阅专栏

小白（我）本着学习，提高自我，增加知识的想法，决定认真分析高翔博士slam。在此立下一个flag：

每周进行知识总结（CSDN）；
每周要有一个计划
决定一年时间入门vslam。
我也不知道要说什么了，总之就是这个意思吧，“不荒废自己，撸起袖子加油干，做出成绩来”。废话不说了。

视觉里程计1

特征点法

特征点是由关键点和描述子两部分组成，关键点是指该特征点在图像里面的位置，描述子通常是一个向量，描述了该关键点周围像素的信息。到目前为止ORB特征则是目前非常具有代表性的实时图像特征。

ORB特征由关键点和描述子两部分组成，因此提取ORB特征分为两个步骤：
1、FAST角点提取；2、BRIEF描述子。
FAST角点不具有方向性和尺度的弱点，ORB添加了尺度和旋转的描述。尺度不变性由构建图像金字塔，并在金字塔的每一层上检测角点来实现。而特征的旋转是由灰度质心法实现的。

特征匹配解决了SLAM中的数据关联问题，即确定当前看到的路标与之前看到的路标之间的对应关系。

汉明距离：表示了两个特征之间的相似程度。

实践：特征提取和匹配（代码详解）

#include <iostream>
//core模块包含了OpenCV库的基础结构以及基本操作
#include <opencv2/core/core.hpp>
//features2d包含了用于检测，描述以及匹配特征点的算法
#include <opencv2/features2d/features2d.hpp>
//highgui模块包含可以用来显示图像或者简单的输入的用户交互函数
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;

int main(int argc, char ** argv) {
//argc :表示可执行程序输入参数的个数，argv：表示输入参数的字符串名称
//argc为1时，argv[0]是程序本身（默认的）
    if(argc != 3)
    {
        cout << "usage: feture_ectraction 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);

    //--初始化(为什么要定义以下变量，请查看detect()函数和compute()函数）
    std::vector<KeyPoint> keypoints_1, keypoints_2;
    Mat descriptors_1,descriptors_2;
    Ptr<ORB> orb = ORB::create();//请查看ORB的构造函数
    //Ptr<ORB> orb = ORB::create(500, 1.2f, 8, 31, 0, 2, ORB::HARRIS_SCORE, 31,20);
    //ORB::create都是默认值

    //--第1步：检测Oriented FAST角点位置（并存储到vector中）
    orb -> detect (img_1, keypoints_1);
    orb -> detect (img_2, keypoints_2);

    //--第2步：根据角点位置计算BRIEF描述子
    orb -> compute(img_1, keypoints_1, descriptors_1);
    orb -> compute(img_2, keypoints_2, descriptors_2);

    Mat outimg1;
    drawKeypoints(img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
    imshow("ORB特征点", outimg1);

    //-- 第3步：对两幅图像中的BRIEF描述子进行匹配，使用汉明距离
    vector<DMatch> matches;
    BFMatcher matcher (NORM_HAMMING);
    matcher.match(descriptors_1, descriptors_2, matches);

    //--第4步：匹配点对筛选
    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;
*/
    cout << "-- Max dist : " << max_dist << endl;
    cout << "-- Min dist : " << min_dist << endl;

    //当描述子之间的距离大于两倍的最小距离时，即认为匹配有误。
    //但有时候最小距离会非常小，设置一个经验值30作为下限。
    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]);
        }
    }
//注意注意注意：筛选的依据是汉明距离小于最小距离的两倍，这是一种工程上的经验的方法，不一定有理论依据
//尽管在示例图像中能够筛选出正确的匹配，但我们仍然不嫩高保真挂在所有其他图像中得到的匹配都是正确的。
//因此，在后面的运动估计中，还需要使用去除误匹配的算法。
    //--第5步：绘制匹配结果
    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("所有匹配点对", img_match);
    imshow("优化后匹配点对", img_goodmatch);
    waitKey(0);
    return 0;
}

注意：储存特征描述子的矩阵（descriptors_1,descriptors_2）应该是：行数表示特征分量，列数表示第几个特征点的特征向量.

cmake_minimum_required(VERSION 3.16)
project(feature2d)

set(CMAKE_CXX_STANDARD 11)

find_package(OpenCV REQUIRED)

include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(feature2d main.cpp)

target_link_libraries(feature2d ${OpenCV_LIBS})

对极几何

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d_c.h>
#include <cv.hpp>

using namespace std;
using namespace cv;

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

//两张图像得到关键点（keypoints）和描述子之间匹配关系（matches）
void find_feature_matches(
        const Mat& img_1, const Mat& img_2,
        vector<KeyPoint>& keypoints_1,
        vector<KeyPoint>& keypoints_2,
        vector<DMatch>& matches
        );
//从特征点(keypoints)和匹配关系（matches）求解位姿(R,t)
void pose_estimation_2d2d(
        const vector<KeyPoint> keypoints_1,
        const vector<KeyPoint> keypoints_2,
        const vector<DMatch> matches,
        Mat& R, Mat& t
        );

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

    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*scale
    //--计算反对称矩阵
    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){
        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；
}

void find_feature_matches(
        const Mat& img_1, const Mat& img_2,
        vector<KeyPoint>& keypoints_1,
        vector<KeyPoint>& keypoints_2,
        vector<DMatch>& matches
        ){
    //--初始化
    Mat descriptors_1, descriptors_2;
    Ptr<ORB> orb = ORB::create();

    //-- 第1步：检测Oriented FAST角点位置
    orb -> detect( img_1, keypoints_1);
    orb -> detect( img_2, keypoints_2);

    //-- 第2步：根据角点位置计算BRIEF描述子
    orb -> compute(img_1, keypoints_1, descriptors_1);
    orb -> compute(img_2, keypoints_2, descriptors_2);

    //--第3步：对两幅图像中的BRIEF描述子进行匹配，使用汉明距离
    vector<DMatch> ngmatches;
    BFMatcher matcher(NORM_HAMMING);
    matcher.match(descriptors_1, descriptors_2, ngmatches);

    //--第4步：匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    //找出所有匹配之间的最小距离和最大距离，即最相似的和最不相似的两组点之间的距离
    for (int i = 0; i < descriptors_1.rows; i++){
        double dist = ngmatches[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);

    for (int i = 0; i < descriptors_1.rows; i++)
    {
        if (ngmatches[i].distance <= max( 2 * min_dist, 30.0))
        {
            matches.push_back(ngmatches[i]);
        }
    }
}

void pose_estimation_2d2d(
        const vector<KeyPoint> keypoints_1,
        const vector<KeyPoint> keypoints_2,
        const vector<DMatch> matches,
        Mat& R, Mat& t
        ){
    // 相机内参， TUM Freiburg2
    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 249.7, 0, 0, 1);

    //-- 对齐匹配的点对，并用.pt转化为像素坐标。把匹配点转换为简单的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第一个图像索引
        points2.push_back( keypoints_2[matches[i].trainIdx].pt);//trainIdx第二个图像索引
    }

    //-- 计算基础矩阵
    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);  // 光心，ＴＵＭ　ｄａｔａｓｅｔ标定值
    int focal_length = 521;                         // 焦距，ＴＵＭ　ｄａｔａｓｅｔ标定值
    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);
    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;
}
//像素坐标系p到相机坐标系x
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 )
            );
}

像素坐标系p到相机坐标系的转换函数
注意：最后得到的坐标是归一化平面上的坐标（位于相机前方z=1出的平面）

在这里插入图片描述

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

findFundamentalMat函数（计算基础矩阵F）

//函数原型

 Mat findFundamentalMat(InputArray points1, InputArray points2, int method = FM_RANSAC, double ransacReprojThreshold = 3., 
 
double confidence = 0.99, OutputArray mask = noArray())

points1,points2 是匹配点对的像素坐标，并且能够一一对应
method 使用那种方法，默认的是FM_RANSAC也就是RANSAC的方法估算基础矩阵。CV_FM_8POINT是8点法。
ransacReprojThreshold 表示RANSAC迭代过程中，判断点是内点还是外点的阈值（到对极线的像素距离）；
confidence 表示内点占的比例，以此来判断估计出的基础矩阵是否正确。

findEssentialMat函数（计算本质矩阵E）

//函数原型

Mat findEssentialMat(InputArray points1, InputArray points2, double focal = 1.0, Point_<double> pp = Point2d(0, 0), 

int method = RANSAC, double prob = 0.999, double threshold = 1.0, OutputArray mask = noArray())

focal: 焦距
pp：光心坐标

findHomography函数（计算单应矩阵H）

//函数原型

Mat findHomography(InputArray srcPoints, InputArray dstPoints, int method = 0, double ransacReprojThreshold = 3,

 OutputArray mask = noArray(), const int maxIters = 2000, const double confidence = 0.995)

maxIter 最大迭代次数

recoverPose 函数（从本质矩阵中恢复旋转矩阵和平移信息）

int recoverPose(InputArray E, InputArray points1, InputArray points2, OutputArray R, OutputArray t, double focal = 1.0, Point_<double> pp = Point2d(0, 0), InputOutputArray mask = noArray())

E: 本质矩阵E

对极几何的实践部分程序比较简单，中规中矩，没有什么特别复杂的地方。

初始换的纯旋转问题

从E分解到R,t的过程，如果相机发生的是纯旋转，导致t为零，那么，得到的E也将为零，这将导致我们无法求解R。不过，此时我们可以依靠H求取像旋转，但仅有旋转时，我们无法用三角测量估计特征点的空间位置。
结论：单目初始化不能只有纯旋转，必须要有一定程度的平移。
在实践当中，如果初始换时平移太小，会使得位姿求解与三角化结果不稳定，从而导致失败。相对地，如果把相机左右移动而不是原地旋转，就容易让单目SLAM初始化。

多于8对点的情况

当可能存在误匹配的情况时，更倾向于使用随机采样一致性（Random Sample Concensus, RANSAC）来求，这个方法是一种通用的方法，适用于很多带错误数据的情况，可以处理带有错误匹配的数据。

三角测量

三角测量（高斯提出）是指，通过在两处观察同一个点的夹角，从而确定该点的距离。
三角化的矛盾：增大平移，会导致匹配失效，平移太小，则三角化精度不够

三角测量代码实践部分<triangulation()函数>

void triangulation(
        const vector< KeyPoint > & keypoints_1,
        const vector< KeyPoint > & keypoints_2,
        const vector< DMatch > & matches,
        const Mat & R, const Mat & t,
        vector<Point3d> & points
    ){
    Mat T1 = (Mat_<double> (3,4) <<
            1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0);//这块表示图1的相机作为参考，变换矩阵中没有相机旋转和平移
    Mat T2 = (Mat_<double> (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)
                    );  //T2中传入R,t，即图2相对图1这个参考位置的变换矩阵  
    Mat K = (Mat_<double> (3, 3) <<
            520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    vector<Point2d> pts_1, pts_2;
    //图1和图2上的2D图像像素点转换后的归一化相机坐标点，对应的Zc=1,所以只要计算Xc和Yc
    for (DMatch m: matches)
    {
        //--将像素坐标转换至相机坐标
        pts_1.push_back( pixel2cam( keypoints_1[m.queryIdx].pt, K));
        pts_2.push_back( pixel2cam( keypoints_2[m.trainIdx].pt, K));
    }

    Mat pts_4d;
    cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );
    //--传入两个图像对应相机的变化矩阵，各自相机坐标系下归一化相机坐标，输出的3D坐标是齐次坐标，
    //--共四个维度，因此需要将前三个维度除以第四个维度以得到非齐次坐标xyz
    //　转换成非齐次坐标
    for (int i = 0; i < pts_4d.cols; i++) //遍历所有的点，列数表述点的数量
    {
        Mat x = pts_4d.col(i);  //x为4x1维度
        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 );//将图1测得的目标相对相机实际位置（Xc,Yc,Zc）存入points
    }
}

3D-2D: PnP

PnP(Perspective-n-Point) 是求解3D到2D点对运动的方法。它描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。
如果两张图像中其中一张特征点的3D位置已知，那么最少只需3个点对（需要至少一个额外点验证结果）就可以估计相机运动。特征点的3D位置可以由三角化或者RGB-D相机的深度图确定。因此，在双目或RGB-D的视觉里程计中，我们可以直接使用PnP估计相机运动。而在单目视觉里程计中，必须先进行初始化，然后才能使用PnP。3D-2D方法不要使用对极约束，又可以在很少的匹配点中获得较好的运动估计，是最重要的一种姿态估计方法。
为了求解PnP，我们利用了三角形相似性质，P3P存在的问题：

P3P只利用了3个点的信息。当给定的配对点对于3组时，难以利用更多的信息。
如果3D点后2D点受噪声影响，或者存在误匹配，则算法失效

在SLAM当中，通常的做法是先使用P3P/EPnP等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）。

荆黎明

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
视觉里程计1 高翔

小白（我）本着学习，提高自我，增加知识的想法，决定认真分析高翔博士slam。在此立下一个flag：每周进行知识总结（CSDN）；每周要有一个计划决定一年时间学好slam。我也不知道要说什么了，总之就是这个意思吧，“不荒废自己，撸起袖子加油干，做出成绩来”。废话不说了。视觉里程计1特征点法特征点是由关键点和描述子两部分组成，关键点是指该特征点在图像里面的位置，描述子通常是一个向量，描述了该关键点周围像素的信息。到目前为止ORB特征则是目前非常具有代表性的实时图像特征。ORB特征由关键点和描
复制链接

扫一扫