深蓝学院-视觉SLAM十四讲-第五章作业

最新推荐文章于 2024-04-23 18:09:03 发布

hello689

最新推荐文章于 2024-04-23 18:09:03 发布

阅读量3.7k

点赞数 2

分类专栏：视觉SLAM十四讲文章标签：自动驾驶计算机视觉人工智能 slam

本文链接：https://blog.csdn.net/jiachang98/article/details/121269542

版权

视觉SLAM十四讲专栏收录该内容

8 篇文章 68 订阅

订阅专栏

第五节课作业

2. ORB特征点

ORB(Oriented FAST and BRIEF) 特征是SLAM 中一种很常用的特征，由于其二进制特性，使得它可以非常快速地提取与计算。下面，你将按照本题的指导，自行书写ORB 的提取、描述子的计算以及匹配的代码。代码框架参照computeORB.cpp 文件，图像见1.png 文件和2.png。

2.1 ORB提取

本题Fast特征点提取部分使用的OpenCV自带的FAST提取算法，只用按照公式进行角度计算就可以了。根据课本上或者视频给的PPT上的计算公式进行计算就能得到角度。通过代码实践，发现这一计算过程有几个需要注意的点：边界的判断，角度值弧度值的转换。还有image的取值方式是**(y,x)**而不是(x，y)。亲身经历了这个坑，找了好久的错误，如果写反，编译时不会出错，但是运行时直接出现下边的错误。

在这里插入图片描述

计算角度部分代码：

void computeAngle(const cv::Mat &image, vector<cv::KeyPoint> &keypoints) {
    int half_patch_size = 8;
    for (auto &kp : keypoints) {
        // START YOUR CODE HERE (~7 lines)
        int u = kp.pt.x, v = kp.pt.y;
        double m01=0, m10=0;
        //判断是否出界，并选择是否跳出
        if(u>=half_patch_size&&v>=half_patch_size&&u<=image.cols-half_patch_size &&v <=image.rows-half_patch_size)
        {
            for (int i = u-half_patch_size+1; i < u+half_patch_size-1; ++i) {
                for (int j = v-half_patch_size+1; j < v+half_patch_size-1; ++j) {
                    m10 += i * image.at<uchar>(j,i);
                    m01 += j * image.at<uchar>(j,i);
                }
            }
        }
        //atan会返回弧度制的旋转角,但 OpenCV 中使用角度制,需要进行弧度转换
        kp.angle = (float)atan(m01/m10)*180/pi;
        // END YOUR CODE HERE
    }
    return;
}

实现效果：

在这里插入图片描述

2.2 ORB描述

描述子的计算过程中，最关键的是边界判断和计算旋转后的p、q点。这一计算过程中一定要分清 $u_p , v_p , u_q , v_q$ 的顺序

边界点的判断也同样需要细心再细心。因为自己的粗心，在计算旋转后的点p,q坐标时，曾经出现过下边的错误现象。

错误写法：

int u_p_ = (int)(ORB_pattern[i*4] * cos(theta) - ORB_pattern[i*4+1]* sin(theta)) + u;
int v_p_ = (int)(ORB_pattern[i*4+2] * sin(theta) + ORB_pattern[i*4+3]* cos(theta)) + v;
int u_q_ = (int)(ORB_pattern[i*4] * cos(theta) - ORB_pattern[i*4+1]* sin(theta)) + u;
int v_q_ = (int)(ORB_pattern[i*4+2] * sin(theta) + ORB_pattern[i*4+3]* cos(theta)) + v;

出现的怪异现象

在这里插入图片描述

计算描述子部分代码：

void computeORBDesc(const cv::Mat &image, vector<cv::KeyPoint> &keypoints,
                    vector<DescType> &desc) {
    for (auto &kp : keypoints) {
        DescType d(256, false); //256个0,用于存放描述子
        for (int i = 0; i < 256; i++) {
            int u = kp.pt.x, v = kp.pt.y;
            // START YOUR CODE HERE (~7 lines)
            //判断旋转前是否出界，并计算旋转后的p、q坐标
            if(u-8>=0 && v-8>=0 && u+7 <= image.cols && v+7 <= image.rows)
            {
                //角度转弧度
                double theta = kp.angle*pi/180;
                int u_p_ = (int)(ORB_pattern[i*4] * cos(theta) - ORB_pattern[i*4+1]* sin(theta)) + u;
                int v_p_ = (int)(ORB_pattern[i*4] * sin(theta) + ORB_pattern[i*4+1]* cos(theta)) + v;
                int u_q_ = (int)(ORB_pattern[i*4+2] * cos(theta) - ORB_pattern[i*4+3]* sin(theta)) + u;
                int v_q_ = (int)(ORB_pattern[i*4+2] * sin(theta) + ORB_pattern[i*4+3]* cos(theta)) + v;
                //判断根据关键点得到的经过旋转的p、q是否出界
                if(u_p_<0 || v_p_<0 || u_p_ >image.cols || v_p_ > image.rows || u_q_<0 || v_q_<0 || u_q_ >image.cols || v_q_ > image.rows){
                    d.clear();
                    break;
                }
                d[i] = image.at<uchar>(v_p_,u_p_) > image.at<uchar>(v_q_, u_q_) ? false : true;

            }else{
                d.clear();
            }
            // END YOUR CODE HERE
        }
        desc.push_back(d);
    }

    int bad = 0;
    for (auto &d : desc) {
        if (d.empty())
            bad++;
    }
    //cout << "bad/total: " << bad << "/" << desc.size() << endl;
    return;
}

2.3 暴力匹配

核心部分代码

void bfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2,
             vector<cv::DMatch> &matches) {
    int d_max = 50;
    // START YOUR CODE HERE (~12 lines)
    // find matches between desc1 and desc2.
    //遍历描述子p
    for (int i=0; i<(int)desc1.size(); i++)
    {
        int min_distance = 256, index = 0; //设置一个最小的距离
        if(desc1[i].empty()) continue;
        for (int j = 0; j < (int)desc2.size(); ++j) {
            if(desc2[j].empty()) continue;
            int hanming_distance = 0;
            //计算两个描述子的汉明距离
            for(int k=0; k<256; k++){
                hanming_distance += desc1[i][k]^desc2[j][k];
                //cout<<hanming_distance<<endl;
            }
            if (hanming_distance < min_distance) {
                min_distance = hanming_distance;
                index = j;
            }
        }
        //阈值筛选，最大距离为50
        if(min_distance<d_max)
        {
            cv::DMatch match;
            match.distance = min_distance;
            match.queryIdx = i;
            match.trainIdx = index;
            matches.push_back(match);
        }
    }
    // END YOUR CODE HERE
//    for (auto &m : matches) {
//        cout << m.queryIdx << ", " << m.trainIdx << ", " << m.distance << endl;
//    }
    return;
}

匹配结果图：

在这里插入图片描述

2.4 多线程ORB

多线程这道题，tbb库的配置太难受了，各种问题，各种重装、重新编译，然后还是找不到库。推荐下边两个链接来配置环境，gcc的版本安装9.1，我之前安装9.3爆出各种花样的错误。tbb就按推荐的2019来安装下载地址。

gcc的安装编译：https://blog.csdn.net/davidhopper/article/details/79681695
tbb的安装编译：https://blog.csdn.net/davidhopper/article/details/98309966

注：其中gcc安装编译这一篇文章中，可以在清华源中下载gmp、mpfr、mpc这三个包，地址：https://mirrors.tuna.tsinghua.edu.cn/gnu/ ；gmp-6.1.0.tar.bz2这个压缩包需要使用解压命令 tar -jxvf gmp-6.1.0.tar.bz2，否则会报错。
在这里插入图片描述

计算角度并行代码

void computeAngleMT(const cv::Mat &image, vector<cv::KeyPoint> &keypoints) {
    int half_patch_size = 8;
  std::for_each(std::execution::par_unseq, keypoints.begin(), keypoints.end(),
                [&half_patch_size, &image](auto &kp) {
                    // START YOUR CODE HERE (~7 lines)
                    int u = kp.pt.x, v = kp.pt.y;

                    //判断是否出界，并选择是否跳出
                    if(u>=half_patch_size&&v>=half_patch_size&&u<=image.cols-half_patch_size &&v <=image.rows-half_patch_size)
                    {
                        int m01=0, m10=0;
                        for (int i = u-half_patch_size+1; i < u+half_patch_size-1; ++i) {
                            for (int j = v-half_patch_size+1; j < v+half_patch_size-1; ++j) {
                                m10 += i * image.at<uchar>(j,i);
                                m01 += j * image.at<uchar>(j,i);
                            }
                        }
                        //atan会返回弧度制的旋转角,但 OpenCV 中使用角度制,需要进行弧度转换
                        kp.angle = (float)atan(m01/m10)*180/pi;
                    }
                    // END YOUR CODE HERE
                });
    return;
}

其中关于描述子的并行代码：

void computeORBDescMT(const cv::Mat &image, vector<cv::KeyPoint> &keypoints,
                      vector<DescType> &desc) {
    // START YOUR CODE HERE (~20 lines)
    std::for_each(std::execution::par_unseq, keypoints.begin(), keypoints.end(),
                  [&desc, &image](auto &kp) {
                      DescType d(256, false); //256个0,用于存放描述子
                      for (int i = 0; i < 256; i++) {
                          int u = kp.pt.x, v = kp.pt.y;
                          // START YOUR CODE HERE (~7 lines)
                          //判断旋转前是否出界，并计算旋转后的p、q坐标
                          if(u-8>=0 && v-8>=0 && u+7 <= image.cols && v+7 <= image.rows)
                          {
                              //角度转弧度
                              double theta = kp.angle*pi/180;
                              int u_p_ = (int)(ORB_pattern[i*4] * cos(theta) - ORB_pattern[i*4+1]* sin(theta)) + u;
                              int v_p_ = (int)(ORB_pattern[i*4] * sin(theta) + ORB_pattern[i*4+1]* cos(theta)) + v;
                              int u_q_ = (int)(ORB_pattern[i*4+2] * cos(theta) - ORB_pattern[i*4+3]* sin(theta)) + u;
                              int v_q_ = (int)(ORB_pattern[i*4+2] * sin(theta) + ORB_pattern[i*4+3]* cos(theta)) + v;
                              //判断根据关键点得到的经过旋转的p、q是否出界
                              if(u_p_<0 || v_p_<0 || u_p_ >image.cols || v_p_ > image.rows || u_q_<0 || v_q_<0 || u_q_ >image.cols || v_q_ > image.rows){
                                  d.clear();
                                  break;
                              }
                              d[i] = image.at<uchar>(v_p_,u_p_) > image.at<uchar>(v_q_, u_q_) ? false : true;

                          }else{
                              d.clear();
                          }
                          // END YOUR CODE HERE
                      }
                      std::lock_guard<std::mutex> guard(m);//代替m.lock; m.unlock();
                      desc.push_back(d);
    });
    // END YOUR CODE HERE
    int bad = 0;
    for (auto &d : desc) {
        if (d.empty())
            bad++;
    }
    //cout << "bad/total: " << bad << "/" << desc.size() << endl;
    return;
}

CMakeLists.txt代码：

cmake_minimum_required(VERSION 3.0)
project(ComputeORB)

#IF(NOT DEFINED ${CMAKE_BUILD_TYPE})
    SET(CMAKE_BUILD_TYPE Release)
#ENDIF()

MESSAGE(STATUS "CMAKE_BUILD_TYPE IS ${CMAKE_BUILD_TYPE}")

SET(CMAKE_CXX_STANDARD 17)

#寻找opencv库
find_package(OpenCV REQUIRED)

#添加头文件
include_directories( ${OpenCV_INCLUDE_DIRS} "/usr/local/include/eigen3" "/usr/local/include/sophus")

SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/OUTPUT)

add_executable(computeORB computeORB.cpp)

#链接OpenCV库 和tbb库
target_link_libraries(computeORB ${OpenCV_LIBS} tbb)

运行结果：
在这里插入图片描述

最后问题

为什么说 ORB 是一种二进制特征?

因为ORB描述子是256位的二进制码进行描述的。
为什么在匹配时使用 50 作为阈值,取更大或更小值会怎么样?

50相当于一个超参数，是通过多次实验得到的。阈值设置为50效果会好一些。阈值变大匹配到的点数增加，匹配错误会增加，阈值变小匹配点数减少。
暴力匹配在你的机器上表现如何?你能想到什么减少计算量的匹配方法吗?

使用暴力匹配方法在我电脑上大概需要2秒左右出结果。课堂上高博讲了快速近似最近邻的方法（FLANN）。
多线程版本相比单线程版本是否有提升?在你的机器上大约能提升多少性能?

在计算角度的多线程代码计算时间比单线程慢，可能我代码写错了，或者线程调用也需要花费很多时间，导致多线程较慢。而关于计算描述子的并行实现，在我删除一些判断条件后，速度明显比单线程快很多。但由于未能成功实现这部分代码，只是猜测这部分代码能实现性能上的提升。

3. 从E恢复R,t

按照对应的计算流程进行计算，运行结果如下所示。

具体的代码和CmakeLists.txt的编写可查看代码文件。

运行结果：

在这里插入图片描述

4. 用G-N 实现Bundle Adjustment 中的位姿估计

请你根据上述条件，用G-N 法求出最优位姿，初始估计为 $T_0 = I$ 。

具体代码在code文件夹中可见。

运行结果如下图所示：

在这里插入图片描述

注：本题需要修改提供代码的头文件，因为高博的代码中引用的是非模板方式#include “sophus/se3.h”，而现在大多数人用的是模板类。相应的修改代码中Sophus::SE3为Sophus::SE3d。

问题回答

如何定义重投影误差?

$u_i - \frac{1}{s_i}K\exp(\xi^{\wedge})P_i$

该误差关于自变量的雅可比矩阵是什么？

$\frac{\partial e}{\partial \delta \xi} = - \begin{bmatrix} \frac{f_x}{Z^{'}}& 0 & -\frac{f_xX^{'}}{Z^{'2}} & -\frac{f_xX^{'}Y^{'}}{Z^{'2}} & f_x + \frac{f_xX^{'}}{Z^{'2}} & -\frac{f_xY^{'}}{Z^{'}} \\ 0 & \frac{f_y}{Z^{'}}& -\frac{f_yY^{'}}{Z^{'2}} & -f_y + \frac{f_yY^{'2}}{Z^{'2}} & \frac{f_yX^{'}Y^{'}}{Z^{'2}}& \frac{f_yX^{'}}{Z^{'}} \end{bmatrix}$

解出更新量之后,如何更新至之前的估计上?

可以左乘或者右乘微小扰动。

5. 用ICP实现轨迹对齐

参照之前的第三次课计算轨迹误差的代码和slam课本P199页的代码进行代码编写，

主要实现了轨迹的读取函数、画出轨迹、ICP的实现三部分。代码分别如下所示：

轨迹读取：

TrajectoryType ReadTrajectory(const string &path, int flag, vector<Point3f> &pts) {
    ifstream fin(path);
    TrajectoryType trajectory;
    if (!fin) {
        cerr << "trajectory " << path << " not found." << endl;
        return trajectory;
    }

    while (!fin.eof()) {
        double t1,tx1,ty1,tz1,qx1,qy1,qz1,qw1;
        double t2,tx2,ty2,tz2,qx2,qy2,qz2,qw2;

        fin>>t1>>tx1>>ty1>>tz1>>qx1>>qy1>>qz1>>qw1>>t2>>tx2>>ty2>>tz2>>qx2>>qy2>>qz2>>qw2;

        if(flag == 0){
            //读取误差轨迹
            Sophus::SE3d p(Eigen::Quaterniond(qw1, qx1, qy1, qz1), Eigen::Vector3d(tx1, ty1, tz1));
            pts.push_back(Point3f(tx1,ty1,tz1));
            trajectory.push_back(p);
        }else{
             //读取真是轨迹
            Sophus::SE3d p(Eigen::Quaterniond(qw2, qx2, qy2, qz2), Eigen::Vector3d(tx2, ty2, tz2));
            pts.push_back(Point3f(tx2,ty2,tz2));
            trajectory.push_back(p);
        }
    }
    return trajectory;
}

ICP实现部分：

void pose_estimation_3d3d(const vector<Point3f> &pts1,
                          const vector<Point3f> &pts2,
                          Eigen::Matrix3d &R_, Eigen::Vector3d &t_) {
    Point3f p1, p2;     // center of mass
    int 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 center
    for (int i = 0; i < N; i++) {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::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 W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    cout << "U=" << U << endl;
    cout << "V=" << V << endl;

    R_ = U * (V.transpose());
    if (R_.determinant() < 0) {
        R_ = -R_;
    }
    t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);
}

详细代码见代码文件。

变换之前的效果

在这里插入图片描述

变换之后的效果：

在这里插入图片描述

github地址: https://github.com/ximing1998/slam-learning.git
国内gitee地址：https://gitee.com/ximing689/slam-learning.git

hello689

关注

2
点赞
踩
14

收藏

觉得还不错? 一键收藏
打赏
2
评论
深蓝学院-视觉SLAM十四讲-第五章作业

第五节课作业2. ORB特征点ORB(Oriented FAST and BRIEF) 特征是SLAM 中一种很常用的特征，由于其二进制特性，使得它可以非常快速地提取与计算。下面，你将按照本题的指导，自行书写ORB 的提取、描述子的计算以及匹配的代码。代码框架参照computeORB.cpp 文件，图像见1.png 文件和2.png。2.1 ORB提取本题Fast特征点提取部分使用的OpenCV自带的FAST提取算法，只用按照公式进行角度计算就可以了。根据课本上或者视频给的PPT上的计算公式进行计算
复制链接

扫一扫