高博SLAM基础课第五讲——几何基础

首先，经过思考上一讲中关于非线性优化的思考：

　　为何其函数形式为每一个待加项内部求出的H和b相加，而不是把其平方当做fx相加再算总函数做优化？

　　因为非线性优化的几类方法比如高斯牛顿法Hx=b的形式本就是从 ||f(x+δx)||² 这个平方形式的式子推出来的，求出的δx就是这一步迭代使得平方值下降最快的解。

　　所以直接加H和b才是正确解。

　　所以非线性优化的方法和线性最小二乘的形式是密切相关的。

回到这一讲，这一讲内容包括了ORB特征点的提取匹配原理、双目视觉尤其是2d-2d对极几何和2d-3d PnP求解的方法，

以及用非线性优化实现的位姿求解，用到了第三讲中的李代数求导公式和更新策略。

依然以习题为重点

Let's go

此大题代码集中在一个整体文件computeORB.cpp中，下述小题依次实现各函数功能。

　　1. 代码首先使用自带的cv::FAST(image,keypoints,threshold)获取了图像中的角点

　　2. 随后调用computeAngle(image,keypoints)函数计算了每个角点的点心至重心的连线角度

　　3. 再调用computeORBDesc(image,keypoints,descriptors) 计算了每个符合条件特征点的描述子，不符合的特征点描述子为空 （keypoint数据类型自带特征点angle值，所以输入项中只需要特征点向量）

　　4. 最后调用bfMatch(descriptor1,discriptors2,matches) 用暴力匹配方式求得特征点与特征点之间的匹配，绘图查看结果

注意熟悉几种基本opencv数据结构的表示方法和用法　　

另外ORB特征点匹配有opencv自带函数，一般情况调库就可以实现，但是ORB-SLAM2的作者是自己手写的特征点匹配模块，这部分原理了解一下也有帮助

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)
        kp.angle = 0;// compute kp.angle
        cv::Point2f p=kp.pt; 
        if(p.x<8||p.x>image.cols-8||p.y<8||p.y>image.rows-8){continue;} // 把越界点去除不考虑
        double m10=0,m01=0,m00=0;
        for(int i=-8;i<8;i++){
            const uchar* col = image.ptr<uchar>(p.y+i);
            for(int j=-8;j<8;j++){
                m00+=col[(int)p.x+j];
                m10+=j*col[(int)p.x+j];
                m01+=i*col[(int)p.x+j];
            }
        }
        double cx=0,cy=0;
        cx=m10/m00;
        cy=m01/m00;
        kp.angle=atan2(cy,cx)*180/pi;

        // END YOUR CODE HERE
    }

    return;
}

注意点:

　　1. 关于KeyPoint，其内含一个Point2f对象存储xy值，用.pt获取，含有一个angle值也可直接获取。

　　　　另外还有size，response，octave，class_id等其他特征点或者分类任务会用到的域

　　2. math.h库中：

　　　　atan 给斜率求正切，仅对-90°-90°有效
　　　　atan2给两点值，值域为整个圆 -180°-180°，输入两个值第一个是y，第二个是x（注意！）

　　　　另外注意角度弧度转换，angle存储度数，atan使用弧度

　　3. cv::mat存放图像时，读取方法：

　　　　Mat存储的图像，每一行都是连续的，可以取得每一行开头指针来访问图像像素。例如提取一副图像中R通道的图像,G、B通道像素全部置零，可以获取每一行开头的指针，使用指针遍历每一行的所有像素。如果图像在内存中的存储是连续的，还可以一次遍历所有像素。

　　　　所以const uchar* col = image.ptr<uchar>(p.y+i);这一行，读取p.y+i行的数组指针，再使用横坐标就能访问

　　　　另外再次强化记忆：点的x是横坐标和col对应，y是纵坐标和row对应，

　　4. 质心m10和m01:

　　　　累加时乘在前面的值可以是绝对坐标p.x+j或p.y+i，也可以用j和i的相对坐标。

　　　　使用相对坐标的质心有优势：在求cx和cy时，如果是绝对坐标，那么m00每一步也必须乘上p.x和p.y，并且算出来是绝对质心计算角度还需要做一次差，相对坐标可以避免多步乘法和减法运算。

把每个特征点的角度绘制出来如下图：

void computeORBDesc(const cv::Mat &image, vector<cv::KeyPoint> &keypoints, vector<DescType> &desc) {
    for (auto &kp: keypoints) {
        DescType d(256, false);
        for (int i = 0; i < 256; i++) {
            // START YOUR CODE HERE (~7 lines)
            float cos_ = cos(kp.angle * pi / 180);
            float sin_ = sin(kp.angle * pi / 180);
            //旋转两个pattrn点到特征点角度旋转后的位置上
            cv::Point2f up_t(cos_ * ORB_pattern[4 * i] - sin_ * ORB_pattern[4 * i + 1],
                             sin_ * ORB_pattern[4 * i] + cos_ * ORB_pattern[4 * i + 1]);
            cv::Point2f uq_t(cos_ * ORB_pattern[4 * i + 2] - sin_ * ORB_pattern[4 * i + 3],
                             sin_ * ORB_pattern[4 * i + 2] + cos_* ORB_pattern[4 * i + 3]);
            //求作比较两点的坐标
            cv::Point2f up = up_t + kp.pt;
            cv::Point2f uq = uq_t + kp.pt;
            //出界点把特征向量清空,并且不计入总数
            if (up.x < 0 || up.y < 0 || up.x > image.cols || up.y > image.rows ||
                uq.x < 0 || uq.y < 0 || uq.x > image.cols || uq.y > image.rows) {
                d.clear();
                break;// if kp goes outside, set d.clear()
            }
            d[i] = image.at<uchar>(up) > image.at<uchar>(uq) ? 0 : 1;//又是一种读取像素的方式
            // 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;
}

注意点：

　　1. 首先，①式是对一个点进行旋转，包括u,v坐标的计算，注意算式代值。

　　2. 另外根据代码的原理推知，ORB pattern 的每行四个值依次是p1u,p1v,p2u,p2v 

　　3. 初始化d为256维向量，界外点clear使得其能够使用empty进行出界判断

　　4. ORB特征点的原理就是对特定方位上像素对的灰度值关系进行比较构建出特征向量，此处是256对

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.
    for (size_t i = 0; i < desc1.size(); ++i){
        if(desc1[i].empty())
            continue;

        int d_min=256;
        int index2=-1;
        for(size_t j=0;j < desc2.size();j++){
            if(desc2[j].empty())continue;
            int dist=0;
            for(size_t k=0;k<256;k++){
                dist+=desc1[i][k]^desc2[j][k];
                if(dist>d_max)break;
            }
            if(dist<d_max&&dist<d_min){
                d_min=dist;
                index2=j;
            }
        }
        if(d_min<d_max){
            matches.push_back(cv::DMatch(i,index2,d_min));
        }
    }
}

注意点：

　　1. cv::DMach数据结构内只含有4个值，分别是int _queryIdx, int _trainIdx, int _imgIdx, float _distance

　　234卡掉了，懒得写了。。。

最终效果如下

这部分参考博客https://blog.csdn.net/weixin_41074793/article/details/85133424

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sophus/so3.h>
using namespace Eigen;
using namespace std;
int main(int argc, char **argv) { // 给定Essential矩阵
    Matrix3d E;
    E <<     -0.0203618550523477, -0.4007110038118445, -0.03324074249824097,
             0.3939270778216369, -0.03506401846698079, 0.5857110303721015,
             -0.006788487241438284, -0.5815434272915686, -0.01438258684486258;

    // 待计算的R,t
    Matrix3d R;
    Vector3d t;

    // SVD and fix singular values
    Eigen::JacobiSVD<Matrix3d> svd(E, ComputeFullU | ComputeFullV); //svd分解
    Vector3d sigma1 = svd.singularValues(); //svd分解出来的sigma是3×1的向量
    Matrix3d SIGMA; //将向量sigma调整成矩阵SIGMA
    cout << "sigma = 
" << sigma1 << endl;
    SIGMA << (sigma1(0, 0) + sigma1(1, 0)) / 2, 0, 0,
             0, (sigma1(0, 0) + sigma1(1, 0)) / 2, 0,
             0, 0, 0;
//    SIGMA<<1,0,0,
//            0,1,0,
//            0,0,0;
    cout << "SIGMA = 
" << SIGMA << endl;

    // set t1, t2, R1, R2
    Matrix3d t_wedge1;
    Matrix3d t_wedge2;
    Matrix3d R1;
    Matrix3d R2;

    Matrix3d R_z1 = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix(); //定义旋转矩阵,沿 Z 轴旋转 90 度
    Matrix3d R_z2 = AngleAxisd(-M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix(); //定义旋转矩阵沿 Z 轴旋转 -90 度

    Matrix3d U = svd.matrixU(); //u的值
    Matrix3d V = svd.matrixV(); //v的值

    t_wedge1 = U * R_z1 * SIGMA * U.transpose(); //t1的值
    t_wedge2 = U * R_z2 * SIGMA * U.transpose(); //t2的值
    R1 = U * R_z1.transpose() * V.transpose();
    R2 = U * R_z2.transpose() * V.transpose();
    cout << "R1 =
" << R1 << endl;
    cout << "R2 =
" << R2 << endl;
    cout << "t1 =
" << Sophus::SO3::vee(t_wedge1) << endl;
    cout << "t2 =
" << Sophus::SO3::vee(t_wedge2) << endl; // check t^R=E up to scale
    Matrix3d tR = t_wedge1 * R1;
    cout << "E = t^R =
" << tR << endl;
    return 0;
}

暂不深究了，一般此处调用的是opencv的findEssentialMat和recoverpose函数

 结果如下