视觉SLAM十四讲——ch7

视觉SLAM十四讲——ch7

ch7视觉里程计

本章目标：

1.理解图像特征点的意义，并掌握在单副图像中提取出特征点及多副图像中匹配特征点的方法
2.理解对极几何的原理，利用对极几何的约束，恢复出图像之间的摄像机的三维运动
3.理解PNP问题，以及利用已知三维结构与图像的对应关系求解摄像机的三维运动
4.理解ICP问题，以及利用点云的匹配关系求解摄像机的三维运动
5.理解如何通过三角化获得二维图像上对应点的三维结构

本章目的：基于特征点法的vo，将介绍什么是特征点，如何提取和匹配特征点，以及如何根据配对的特征点估计运动和场景结构，从而实现两帧间视觉里程计。

1 特征点

角点、SIFT（尺度不变特征变换）、SURF、ORB（后三者为人为设计，具有更多优点）。

1.1 特征点的组成

1 关键点：指特征点在图像里的位置

2 描述子：通常是一个向量，按照某种人为设计的方式，描述了该关键点周围像素的信息。相似的特征应该有相似的描述子（即当两个特征点的描述子在向量空间上的距离相近，认为这两个特征点是一样的）。

1.2 ORB特征

ORB特征：OrientedFAST关键点+BRIEF描述子。提取ORB特征的步骤：

1 .2.1提取FAST角点：

找出图中的角点，计算特征点的主方向，为后续BRIEF描述子增加了旋转不变特性。（FAST角点主要检测局部像素灰度变化明显的地方）。它的检测过程如下：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-C6zkqAV4-1647311685760)(视觉SLAM十四讲——ch7.assets/14d5abad89ab841faeebd7534c42e42-16462760426531.png)]

特点：速度快
缺点： 1).FAST特征点数量很大且不确定，但是我们希望对图像提取固定数量的特征
2).FAST角点不具有方向信息，并且存在尺度问题

解决方式：1).指定要提取的角点数量N，对原始FAST角点分别计算Harris响应值，然后选取前N个具有最大响应值的角点作为最终的角点集合
2).添加尺度和旋转的描述

​ 尺度不变性的实现：构建图像金字塔，并在金字塔的每一层上检测角点（金字塔：指对图像进行不同层次的降采样，以获得不同分辨率的图像）
​ 特征旋转的实现：灰度质心法（质心：指以图像块灰度值作为权重的中心）

1.2.2.计算BRIEF描述子

对前一步提取出的特征点周围图像区域进行扫描
特点：使用随机选点的比较，速度非常快，由于使用了二进制表达，存储起来也十分方便，适用于实时的图像匹配。

在不同图像之间进行特征匹配的方法：
1.暴力匹配：浮点类型的描述子，使用欧式距离度量
二进制类型的描述子（比如本例中的BRIEF描述子），使用汉明距离度量
缺点：当特征点数量很大时，暴力匹配法的运算量会变得很大

2.快速近似最近邻（FLANN）：适合匹配特征点数量极多的情况。

1.2.3 图像特征提取代码详解

#include
#include
#include
#include
#include

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("/home/tgc/slambook2/CH7/1.png",1);
    Mat img_2 = imread("/home/tgc/slambook2/CH7/2.png",1);

    //断言，用于代码的检查
    assert(img_1.data != nullptr && img_2.data != nullptr); 
    
    //初始化
    std::vector<KeyPoint> keypoint_1, keypoint_2; //声明关键点名字和类型
    Mat description_1, description_2;//声明描述子的名字和类型
    Ptr<FeatureDetector> detector = ORB::create();  //ORB创建关键点类
    Ptr<DescriptorExtractor> descriptor = ORB::create();//ORB创建描述子类
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//用于匹配关键点描述符的抽象基类

    //第一步，检测oriented fast角点位置
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    detector->detect(img_1, keypoint_1);
    detector->detect(img_2, keypoint_2);

    //第二步，根据角点位置计算brief描述子
    descriptor->compute(img_1, keypoint_1, description_1);
    descriptor->compute(img_2, keypoint_2, description_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<<"time cost:"<< time_used.count()<<endl;

    Mat outimg1;
    drawKeypoints(img_1,keypoint_1,outimg1,Scalar::all(-1),DrawMatchesFlags::DEFAULT);
    imshow("orb_features",outimg1);

    //第三步。对两幅图像中的brief描述子进行匹配，使用hamming距离
    vector<DMatch> matches;
    chrono::steady_clock::time_point t3 = chrono::steady_clock::now();
    matcher->match(description_1,description_2,matches);//描述子匹配，
    chrono::steady_clock::time_point t4 = chrono::steady_clock::now();
    chrono::duration<double> time_used_match = chrono::duration_cast<chrono::duration<double>>(t4-t3);
    cout<<"match time cost :"<< time_used_match.count()<<endl;

    //第四步，匹配点对筛选
    //计算最小和最大距离
    auto min_max = minmax_element(matches.begin(),matches.end());//指向最小和最大的迭代器，begin返回指向的第一个读/写迭代器
    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作为下限
    vector<DMatch> good_match;
    for (int i = 0; i < description_1.rows; i++)
    {
        if (matches[i].distance <= max(2* min_dist,30.0))
        {
            good_match.push_back(matches[i]);
        }
    }

    //第五步。绘制匹配结果
    Mat img_match;
    Mat img_goodmatch;
    drawMatches(img_1,keypoint_1,img_2,keypoint_2,matches,img_match);
    drawMatches(img_1,keypoint_1,img_2,keypoint_2,good_match,img_goodmatch);
    imshow("match",img_match);
    imshow("good_match",img_goodmatch);
    waitKey(0);
    
    return 0;
    
}

1.2.4 结果分析

图片1：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-UecP7jPi-1647311685761)(视觉SLAM十四讲——ch7.assets/1372381-20180727153341182-1525732735-16462766616633.png)]

图片2

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-gR0Rrr8H-1647311685762)(视觉SLAM十四讲——ch7.assets/1372381-20180727153414203-226154175-16462766676635.png)]

暴力匹配：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-V4DLqjl6-1647311685762)(视觉SLAM十四讲——ch7.assets/1372381-20180727153506919-387915385-16462767832818.png)]

设置30为下限的经验值匹配：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-370Tx8F1-1647311685762)(视觉SLAM十四讲——ch7.assets/1372381-20180727153635299-359755985-16462767875569.png)]

结果分析：筛选的依据是汉明距离小于最小距离的两倍，这是工程经验方法，但还是有误匹配，所以后面在估计运动时，需要去除误匹配的算法。

2 2D-2D ：对极几何约束

从两张图片中得到一对匹配好的特征点，如果有若干对这样的匹配点，就可以通过二维图像对应的点，计算出两帧之间相机的运动（R、t）。

2.1单目相机——对极几何求解

如果是单目相机：只能得到2D的像素坐标->根据两组2D点估计运动，方法：对极几何

步骤如下： 1).根据配对点的像素位置求出本质矩阵E或基础矩阵F

2).根据E或F求出R，t。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-y4DJKzn0-1647311685763)(视觉SLAM十四讲——ch7.assets/c1407a05f0d0e6a9ed05d717a2ab762-164627824431811-164627824612812.png)]

2.1.1本质矩阵E：

1).E在不同尺度下是等价的

2).本质矩阵的内在性质：E的奇异值一定是[σ,σ,0]T的形式

3).由于尺度等价性，E实际上有5个自由度。

​ 求解方法：八点法(Eight-point -algorithm)。求出E后，对其分解->R,t

2.1.2 单应矩阵（Homograohy）H：

描述了两个平面之间的映射关系。如果场景中的特征点都落在同一个平面上（墙、地面等），可通过单应性来估计运动

求解方法：直接线性变换法（Direct Linear Transform）。求出H后，对其分解（分解方法：数值法、解析法）->R,t

2.2 对极约束求解相机运动代码详解

#include
#include
#include
#include
#include

using namespace std;
using namespace cv;
//声明特征匹配函数
void find_feature_matches(
    const Mat &img_1, const Mat &img_2,
    std::vector<KeyPoint> &keyponts_1,
    std::vector<KeyPoint> &keyponts_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
);
//像素坐标公式，公式5.5变换一下就ok
// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K); //p是像素坐标，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!");
//定义要用到的变量 ，如keypoints_1 matches
  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);//计算旋转和平移
  
  //当然还需要验证以下R和t是否满足对极约束
  //-- 验证E=t^R*scale
  Mat t_x =//t_x是反对称矩阵，公式3.3
    (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;//E=t^R,公式7.10

  //-- 验证对极约束
  //queryIdx是查询图像的描述子和特征点的下标，
  //trainIdx是训练图像的描述子和特征点的下标。
  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;//y2.t()是转置矩阵，公式7.8，若d很趋近于零，则说明没啥问题
    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) {
  //先初始化，创建咱们要用到的对象
    //定义两个关键点对应的描述子，同时创建检测keypoints的检测器
  Mat descriptors_1, descriptors_2;
  Ptr<FeatureDetector> detector = ORB::create();//keypoints检测器
  Ptr<DescriptorExtractor> descriptor = ORB::create();//描述子提取器
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//描述子匹配器（方法：暴力匹配）
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);//得到图1的关键点（keypoints_1）
  detector->detect(img_2, keypoints_2);//得到图2的关键点（keypoints_2）

  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);//得到descriptors_1
  descriptor->compute(img_2, keypoints_2, descriptors_2);//得到descriptors_2

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector<DMatch> match;//暂时存放匹配点,因为后面还要进行筛选
  matcher->match(descriptors_1, descriptors_2, match);

  //-- 第四步:对match进行筛选，得到好的匹配点，把好的匹配点放在matches中
  //先定义两个变量，一个是最大距离，一个是最小距离
  double min_dist = 10000, max_dist = 0;

  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {//描述子本质是由 0,1 组成的向量
    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)//p是像素坐标，k是相机内参
{
    return Point2d(
        (p.x - k.at<double>(0,2))/ k.at<double>(0,0),//世界坐标系的X
        (p.y - k.at<double>(1,2) )/ k.at<double>(1,1)//世界坐标系的Y，其中Z被归一化
    );
}
//实现位姿估计函数
void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1, 
                                                            std::vector<KeyPoint> keypoints_2,
                                                            std::vector<DMatch> matches,
                                                            Mat &R, Mat &t)
{
    //相机内参
    Mat K =(Mat_<double> (3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);

    //把匹配点转换为float点形式，咱们要把关键点的像素点拿出来 ,定义两个容器接受两个图关键点的像素位置
    vector<Point2f> points1;  // 计算本质矩阵和基础矩阵需要浮点类型的数据
    vector<Point2f> points2; 

    for (int i = 0; i < (int)matches.size(); i++)
    {
        //queryIdx是图1中匹配的关键点的对应编号
        //trainIdx是图2中匹配的关键点的对应编号
        //pt可以把关键点的像素位置取出来
        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<<"fundenmatal matrix" <<fundamental_matrix<<endl;

    //计算本质矩阵
    Point2d principal_point(325.1, 249.7); //相机光心，//把内参K中的cx ,cy放进一个向量里面 =相机的光心
    double focal_length = 521; // 相机焦距
    //之所以取上面的principal_point、focal_length是因为计算本质矩阵的函数要用
    //得到本质矩阵essential_matrix
    Mat essential_matrix;
    essential_matrix = findEssentialMat(points1,points2,focal_length,principal_point);
    cout<<"essential matrix" <<essential_matrix<<endl;
   
   //计算单应矩阵
   Mat homography_matrix;
   homography_matrix = findHomography(points1,points2,RANSAC,3);
    cout<<"homography matrix" <<homography_matrix<<endl;
    
    //从本质矩阵中恢复旋转和平移信息
    //此函数仅在opencv3中提供
    recoverPose(essential_matrix,points1,points2,R,t,focal_length,principal_point);
    cout<< "R is"<< R << endl;
    cout<< "t is"<< t << endl;
}

2.3 结果分析

-- Max dist : 95.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
fundamental_matrix is 
[4.844484382466111e-06, 0.0001222601840188731, -0.01786737827487386;
 -0.0001174326832719333, 2.122888800459598e-05, -0.01775877156212593;
 0.01799658210895528, 0.008143605989020664, 1]
essential_matrix is 
[-0.02036185505234771, -0.4007110038118444, -0.033240742498241;
 0.3939270778216368, -0.03506401846698084, 0.5857110303721015;
 -0.006788487241438231, -0.5815434272915687, -0.01438258684486259]
homography_matrix is 
[0.9497129583105288, -0.143556453147626, 31.20121878625771;
 0.04154536627445031, 0.9715568969832015, 5.306887618807696;
 -2.81813676978796e-05, 4.353702039810921e-05, 1]
R is 
[0.9985961798781877, -0.05169917220143662, 0.01152671359827862;
 0.05139607508976053, 0.9983603445075083, 0.02520051547522452;
 -0.01281065954813537, -0.02457271064688494, 0.9996159607036126]
t is 
[-0.8220841067933339;
 -0.0326974270640541;
 0.5684264241053518]
t^R=
[-0.02879601157010514, -0.5666909361828475, -0.0470095088643642;
 0.5570970160413605, -0.04958801046730488, 0.8283204827837457;
 -0.009600370724838811, -0.8224266019846685, -0.02034004937801358]
epipolar constraint = [0.002528128704106514]
epipolar constraint = [-0.001663727901710814]
epipolar constraint = [-0.0008009088410885212]
epipolar constraint = [0.0001705869410470254]
epipolar constraint = [-0.0003338015008984979]
epipolar constraint = [0.0003385525272308065]
epipolar constraint = [0.0001729349818584552]
epipolar constraint = [-9.552408320477601e-06]
epipolar constraint = [-0.0008834408754688165]
epipolar constraint = [-0.000444586092781381]
epipolar constraint = [-0.000156067923593961]
epipolar constraint = [0.001512967129777068]
epipolar constraint = [-0.0002644334828964742]
epipolar constraint = [-3.514414351252562e-06]
epipolar constraint = [-0.0004170632811044614]
epipolar constraint = [-0.0007749589896892117]
epipolar constraint = [0.002091463454860276]
epipolar constraint = [-0.001016195254389909]
epipolar constraint = [0.0005870797511206075]
epipolar constraint = [0.0002701337927295891]
epipolar constraint = [-0.0008153290073634545]
epipolar constraint = [0.005595329855570208]
epipolar constraint = [0.004456949384590653]
epipolar constraint = [-0.00109225844630996]
epipolar constraint = [-0.00122027527435923]
epipolar constraint = [0.0001206258081121597]
epipolar constraint = [-0.0007167735646266809]
epipolar constraint = [0.002034045481378033]
epipolar constraint = [-0.001256283161205782]
epipolar constraint = [-2.841028832301085e-06]
epipolar constraint = [-0.0009452266349239957]
epipolar constraint = [-0.0003143393086872948]
epipolar constraint = [-0.003421410506807192]
epipolar constraint = [0.0006511227496003788]
epipolar constraint = [-0.001310996225762057]
epipolar constraint = [-0.0001590978941137114]
epipolar constraint = [-0.002209031974010213]
epipolar constraint = [-0.0007840664639768846]
epipolar constraint = [-0.00219092181574767]
epipolar constraint = [0.002922765516590181]
epipolar constraint = [-0.0002852488690396338]
epipolar constraint = [0.001288650990044271]
epipolar constraint = [0.002981122529430141]
epipolar constraint = [0.001104024767925333]
epipolar constraint = [0.0005839797440089639]
epipolar constraint = [-0.002811645087152688]
epipolar constraint = [-0.001723388366795087]
epipolar constraint = [0.0001541481260837613]
epipolar constraint = [0.0006004071491191379]
epipolar constraint = [-0.001728591166312573]
epipolar constraint = [-0.0007782250239872224]
epipolar constraint = [-0.001075058873840032]
epipolar constraint = [0.004654782908027483]
epipolar constraint = [-0.00145774661621554]
epipolar constraint = [0.0003259585394422768]
epipolar constraint = [-9.139514634399354e-06]
epipolar constraint = [2.094089762112034e-06]
epipolar constraint = [-0.00122725042555329]
epipolar constraint = [-0.0008551935807612487]
epipolar constraint = [0.001650773210968584]
epipolar constraint = [0.00116044245491314]
epipolar constraint = [0.001879717958470126]
epipolar constraint = [-5.97742462742773e-06]
epipolar constraint = [-0.0003369336336238871]
epipolar constraint = [0.004360922097753794]
epipolar constraint = [-0.005310637569393865]
epipolar constraint = [-0.0006060103678098214]
epipolar constraint = [1.216121374464363e-06]
epipolar constraint = [-0.003401336870289186]
epipolar constraint = [0.002238878760289525]
epipolar constraint = [0.001475291883444502]
epipolar constraint = [-0.003206338609952966]
epipolar constraint = [-0.001462525388296471]
epipolar constraint = [-0.0007503932332671159]
epipolar constraint = [0.00384724837206624]
epipolar constraint = [6.646617176919722e-07]
epipolar constraint = [0.0007123827789497824]
epipolar constraint = [-0.0005911111586385312]
epipolar constraint = [-0.004921591124588801]

结果分析：其中求出来的本质矩阵和通过E=t^R的出来的结果有差异，对极约束中精度在0.001,

2.4 单目相机的不足

2.4.1 尺度不确定性

单目视觉slam：尺度不确定性。即对轨迹和地图缩放任意倍数，得到的图像依然是一样的

固定尺度的做法：1.对两张图像的t归一化

2.令所有的特征点平均深度为1（相比较而言，特征点深度归一化可控制场景的规模大小，使得计算在数值上更稳定）

2.4.2 初始化的旋转问题

单目初始化不能只有纯旋转，必须要有一定程度的平移，才可进行单目的初始化.

2.4.3 对于多8点的情况

对于本质矩阵E中的元素构建最小二乘法，通过优化方法来求解R和t，但当存在误匹配的情况下，不适用。更加倾向于随机采样一致性（RANSAC）

3 三角测量

通过ORB特征匹配，进行图片之间的关联，得到特征点之间的关系，在通过对极约束
$$x_2^{T}E{x}_1=0$$
计算本质矩阵E，在将E分解为R和t，即可得到相机的运动，下一步需要使用相机的运动来估计特征点的空间位置。即估计地图点的深度。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-cMaD82e8-1647311685763)(视觉SLAM十四讲——ch7.assets/838f7c5485b6efb0b861be20951997a-16463096450793-16463096465574.png)]

三角测量：通过对不同位置对同一个路标点进行观测，从观测位置推断路标点的距离，按照对极几何定义，设$$x_1,x_2$$为两个特征点的的归一化坐标，满足
$$s_2{x}_2=s_{1}R{x}_1+t$$
对上式两侧左乘$$x_2$$的反对称矩阵，就可以求得$$s_1$$。

3.1代码详解

#include
#include

using namespace std;
using namespace cv;

/*
**********************************
该程序是在2d_2d的基础上加上三角化，
以求得匹配的特征点在世界坐标系下的三维点
************************************
*/

//声明特征匹配函数
void find_feature_matches(
    const Mat &img_1, const Mat &img_2,
    std::vector<KeyPoint> &keyponts_1,
    std::vector<KeyPoint> &keyponts_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
);
//像素坐标公式，公式5.5变换一下就ok
// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K); //p是像素坐标，k是相机内参

void triangulation(
  const vector<KeyPoint> &keypoint_1,
  const vector<KeyPoint> &keypoint_2,
  const std::vector<DMatch> &matches,
  const Mat &R, const Mat &t,
  vector<Point3d> &points
);

/// 作图用
inline cv::Scalar get_color(float depth) {
  float up_th = 50, low_th = 10, th_range = up_th - low_th;
  if (depth > up_th) depth = up_th;
  if (depth < low_th) depth = low_th;
  return cv::Scalar(255 * depth / th_range, 0, 255 * (1 - depth / th_range));
}

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!");
//定义要用到的变量 ，如keypoints_1 matches
  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);//计算旋转和平移
  
  //三角化
   //定义一个容器 points 用来存放特征匹配点在世界坐标系下的3d点
  vector<Point3d> points;
  triangulation(keypoints_1, keypoints_2,matches, R, t, points);  //得到三维点

  //验证三角化点与特征点的重投影关系
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  Mat img1_plot = img_1.clone(); //数组副本
  Mat img2_plot = img_2.clone();
  //利用循环找到图1和图2特征点在图上的位置，并圈出来
  for (int i = 0; i < matches.size(); i++)
  {    //第一个图
        //先画图1中特征点
        //在这里，为什么从一个世界坐标系下的3d点，就可以得到，图1相机坐标下的深度点呢？
        //我觉得是因为 图1的位姿: R是单位矩阵，t为0（在三角化函数中有写到） 所以可以把图1的相机坐标看成是世界坐标
      float depth1 = points[i].z;//取出图1各个特征点的深度信息
      cout<<"depth:  " <<depth1<<endl;
      Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt, K);
      cv::circle(img1_plot,keypoints_1[matches[i].queryIdx].pt,2,get_color(depth1),2);

      //第二个图
      //得到图2坐标系下的3d点，得到图2的深度信息
      Mat pt2_trans = R * (Mat_<double>(3,1)<<points[i].x, points[i].y, points[i].z) + t ;
      float depth2 = pt2_trans.at<double>(2,0);
      cv::circle(img2_plot, keypoints_2[matches[i].trainIdx].pt, 2, get_color(depth2), 2);   
  }
  cv::imshow("img_1",img1_plot);
  cv::imshow("img_2",img2_plot);
  cv::waitKey(0);
  
  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) {
  //先初始化，创建咱们要用到的对象
    //定义两个关键点对应的描述子，同时创建检测keypoints的检测器
  Mat descriptors_1, descriptors_2;
  Ptr<FeatureDetector> detector = ORB::create();//keypoints检测器
  Ptr<DescriptorExtractor> descriptor = ORB::create();//描述子提取器
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//描述子匹配器（方法：暴力匹配）
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);//得到图1的关键点（keypoints_1）
  detector->detect(img_2, keypoints_2);//得到图2的关键点（keypoints_2）

  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);//得到descriptors_1
  descriptor->compute(img_2, keypoints_2, descriptors_2);//得到descriptors_2

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector<DMatch> match;//暂时存放匹配点,因为后面还要进行筛选
  matcher->match(descriptors_1, descriptors_2, match);

  //-- 第四步:对match进行筛选，得到好的匹配点，把好的匹配点放在matches中
  //先定义两个变量，一个是最大距离，一个是最小距离
  double min_dist = 10000, max_dist = 0;

  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {//描述子本质是由 0,1 组成的向量
    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)//p是像素坐标，k是相机内参
{
    return Point2d(
        (p.x - k.at<double>(0,2))/ k.at<double>(0,0),//世界坐标系的X
        (p.y - k.at<double>(1,2) )/ k.at<double>(1,1)//世界坐标系的Y，其中Z被归一化
    );
}
//实现位姿估计函数
void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1, 
                                                            std::vector<KeyPoint> keypoints_2,
                                                            std::vector<DMatch> matches,
                                                            Mat &R, Mat &t)
{
    //相机内参
    Mat K =(Mat_<double> (3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);

    //把匹配点转换为float点形式，咱们要把关键点的像素点拿出来 ,定义两个容器接受两个图关键点的像素位置
    vector<Point2f> points1;  // 计算本质矩阵和基础矩阵需要浮点类型的数据
    vector<Point2f> points2; 

    for (int i = 0; i < (int)matches.size(); i++)
    {
        //queryIdx是图1中匹配的关键点的对应编号
        //trainIdx是图2中匹配的关键点的对应编号
        //pt可以把关键点的像素位置取出来
        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<<"fundenmatal matrix" <

    //计算本质矩阵
    Point2d principal_point(325.1, 249.7); //相机光心，//把内参K中的cx ,cy放进一个向量里面 =相机的光心
    double focal_length = 521; // 相机焦距
    //之所以取上面的principal_point、focal_length是因为计算本质矩阵的函数要用
    //得到本质矩阵essential_matrix
    Mat essential_matrix;
    essential_matrix = findEssentialMat(points1,points2,focal_length,principal_point,RANSAC);
    cout<<"essential matrix" <<essential_matrix<<endl;
   
   //计算单应矩阵
   //Mat homography_matrix;
   //homography_matrix = findHomography(points1,points2,RANSAC,3);
    //cout<<"homography matrix" <
    
    //从本质矩阵中恢复旋转和平移信息
    //此函数仅在opencv3中提供
    recoverPose(essential_matrix,points1,points2,R,t,focal_length,principal_point);
    cout<< "R is"<< R << endl;
    cout<< "t is"<< t << endl;
}

void triangulation(
    const vector<KeyPoint> &keypoint_1,
    const vector<KeyPoint> &keypoint_2,
    const std::vector<DMatch> &matches,
    const Mat &R, const Mat &t,
    vector<Point3d> &points){
        Mat T1 = (Mat_<float>(3,4)<<//定义图1在世界坐标系下的位姿
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0);
    Mat T2 = (Mat_<float>(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)
    );
    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    //容器 pts_1、pts_2分别存放图1和图2中特征点对应的自己相机归一化坐标中的 x与 y
    vector<Point2f> 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++)
    {
        //定义x来接收每一个三维点
        Mat x = pts_4d.col(i);
        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);

    }
    
}

3.2 结果分析

-- Max dist : 95.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
essential matrix[-0.02036185505234771, -0.4007110038118444, -0.033240742498241;
 0.3939270778216368, -0.03506401846698084, 0.5857110303721015;
 -0.006788487241438231, -0.5815434272915687, -0.01438258684486259]
R is[0.9985961798781877, -0.05169917220143662, 0.01152671359827862;
 0.05139607508976053, 0.9983603445075083, 0.02520051547522452;
 -0.01281065954813537, -0.02457271064688494, 0.9996159607036126]
t is[-0.8220841067933339;
 -0.0326974270640541;
 0.5684264241053518]
depth:  45.3724
depth:  16.6993
depth:  14.046
depth:  13.267
depth:  13.6788
depth:  30.0102
depth:  14.6864
depth:  13.8394
depth:  14.408
depth:  15.118
depth:  17.1833
depth:  16.2955
depth:  14.4425
depth:  15.5032
depth:  16.6199
depth:  29.6163
depth:  29.8479
depth:  14.2034
depth:  17.2911
depth:  13.952
depth:  14.7138
depth:  51.8393
depth:  50.3779
depth:  15.2634
depth:  15.1423
depth:  13.5731
depth:  15.2204
depth:  17.2002
depth:  15.1018
depth:  13.1856
depth:  28.1319
depth:  27.8687
depth:  14.8212
depth:  16.2624
depth:  13.3843
depth:  14.8062
depth:  14.6493
depth:  16.8406
depth:  14.6166
depth:  44.9113
depth:  28.6374
depth:  28.7027
depth:  46.6309
depth:  15.6839
depth:  14.9472
depth:  14.5156
depth:  14.0974
depth:  13.5684
depth:  19.3896
depth:  17.501
depth:  29.7291
depth:  15.5625
depth:  53.3157
depth:  16.3022
depth:  12.9899
depth:  17.7342
depth:  18.2738
depth:  17.8923
depth:  16.5829
depth:  15.7578
depth:  17.9717
depth:  18.049
depth:  14.3796
depth:  10.4467
depth:  11.2243
depth:  13.9109
depth:  16.0181
depth:  22.9477
depth:  15.9597
depth:  10.2454
depth:  9.95815
depth:  16.3941
depth:  14.9895
depth:  18.6442
depth:  18.0098
depth:  11.627
depth:  16.2583
depth:  13.4797
depth:  17.2989

结果分析：

纯旋转无法使用三角测量，因为对极约束会永远满足。提高三角化精度的方法：

​ 1.提高特征点的提取精度，即提高图像分辨率。缺点：会导致图像变大，增加计算成本。

2.使平移量增大（平移较大时，在同样的相机分辨率下，三角化测量会更精确）。

​ 缺点：导致图像的外观会发生明显变化，使得特征提取和匹配变得困难。

三角测量的矛盾：增大平移量->匹配失效；平移太小->三角化精度不够。

4 3D-2D: PnP

PnP(Perspective-n-Point)是求解 3D 到 2D 点对运动的方法。它描述了当我们知道n个 3D 空间点以及它们的投影位置时,如何估计相机所在的位姿。

在双目或 RGB-D 的视觉里程计中,我们可以直接使用 PnP 估计相机运动。而在单目视觉里程计中,必须先进行初始化,然后才能使用 PnP。

​ 优点：3D-2D 方法不需要使用对极约束,又可以在很少的匹配点中获得较好的运动估计,是最重要的一种姿态估计方法。

PnP 问题有很多种求解方法,直接线性变换(DLT)、EPnP(Efficient PnP),UPnP[47] 。此外,还能用非线性优化的方式,构建最小二乘问题并迭代求解,即Bundle Adjustment。

1).DLT（Direct Linear Transform）

2).P3P

​ 输入数据：3对3D-2D匹配点。记3D点为A，B，C。2D点为a,b,c。小写字母对应的点为对应大写字母代表的点在相应的成像平面上的 　　　投影

​ 注意：A，B，C表示的是在世界坐标系中的坐标，而不是在相机坐标系中的坐标。如果一旦能算出3D点在相机坐标系下的坐标，就能得到3D-3D的对应点，从而把PnP问题转化为ICP问题。

缺点：1.P3P只利用3个点的信息，当给定的配对点多于3组时，难以利用更多的信息。

​ 2.如果2D点或3D点受噪声的影响，或存在误匹配，则算法失效

3).EPnP、UPnP等

相较于P3P来说，优点：1.利用更过的信息

2.用迭代的方式对相机的位姿进行优化，尽可能消除噪声的影响

缺点：原理更复杂

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

线性方法：先求相机位姿，再求空间位置

非线性优化：把相机位姿和空间位置均看作优化变量，放在一起优化

4).Bundle Adjustment

两个重要的公式见P164（公式7.45）和P165（公式7.47）-分别描述了观测相机方程关于相机位姿与特征点的两个导数矩阵。能够在优化过程中（优化位姿和优化特征点的空间位置）提供重要的梯度方向，指导优化的迭代。

4.1 实验代码如下：

首先用OpenCV提供的EPnP求解PnP问题，然后通过g2o对结果进行优化

第一步：使用EPnP求解位姿

//created by 2022/3/4
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
//该程序调用三种方法实现位姿估计
//第一种，调用cv的函数pnp求解 R ,t
//第二种，手写高斯牛顿进行位姿优化
//第三种，利用g2o进行位姿优化
using namespace std;
using namespace cv;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;//VecVector2d可以定义存放二维向量的容器
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector3d;//VecVector3d可以定义存放三维向量的容器
//特征匹配函数
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优化 高斯牛顿优化
void bundleAdjustmentGaussNewton(
        const VecVector3d &points_3d,
        const VecVector2d &points_2d,
        const Mat &K,
        Sophus::SE3d &pose
);

//利用g2o优化pose
void bundleAdjustmentG2O(
        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:3d_2d img1 imh2 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;
    //建立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<Point3d> pts_3d;//创建容器pts_3d存放3d点（图1对应的特征点的相机坐标下的3d点）
    vector<Point2d> pts_2d;//创建容器pts_2d存放图2的特征点
    for ( DMatch m:matches)
    {  //把对应的图1的特征点的深度信息拿出来
        ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
        if(d == 0)//深度有问题
            continue;
        float dd = d/5000.0;//用dd存放换算过尺度的深度信息
        Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);//p1里面放的是图1特征点在相机坐标下的归一化坐标（只包含 x,y）
        pts_3d.push_back(Point3d(p1.x * dd, p1.y * dd, dd));//得到图1特征点在相机坐标下的3d坐标
        pts_2d.push_back(keypoints_2[m.trainIdx].pt);//得到图2特张点的像素坐标
    }
    cout<< "3d_2d pairs: "<<pts_3d.size() << endl;//3d-2d配对个数得用pts_3d的size
    cout<<"使用cv求解 位姿"<<endl;
    cout<<"***********************************opencv***********************************"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    Mat r, t;//Mat()这个参数指的是畸变系数向量？
    solvePnP(pts_3d, pts_2d, K, Mat(),r, t, false);//调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;
    cv::Rodrigues(r, R);//r是旋转向量，利用cv的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 is: " << R << endl;
    cout <<"t is: " << t << endl;
    cout<<"***********************************opencv***********************************"<<endl;
    cout<<"手写高斯牛顿优化位姿"<<endl;
    cout<<"***********************************手写高斯牛顿***********************************"<<endl;  
    VecVector3d pts_3d_eigen;//存放3d点（图1对应的特征点的相机坐标下的3d点）
    VecVector2d pts_2d_eigen;//存放图2的特征点
    for (size_t i = 0; i < pts_3d.size(); i++)//size_t
    {
        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));
    }
    Sophus::SE3d pose_gn;//位姿（李群）
    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<<"R = \n"<<pose_gn.rotationMatrix()<<endl;
    cout<<"t = "<<pose_gn.translation().transpose()<<endl;
    cout<<"***********************************手写高斯牛顿***********************************"<<endl;

    cout<<"g2o优化位姿"<<endl;
    cout << "***********************************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;
    cout << "***********************************g2o***********************************" << 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)
{
    //先初始化，创建要用到的对象
    //定义两个关键点对应的描述子，同时创建检测keypoints的检测器
    Mat descriptors_1, descriptors_2;
    Ptr<FeatureDetector> detector = ORB::create();//keypoints检测器
    Ptr<DescriptorExtractor> descriptor = ORB::create();//描述子提取器
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//描述子匹配器（方法：暴力匹配）
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect(img_1, keypoints_1);//得到图1的关键点（keypoints_1）
    detector->detect(img_2, keypoints_2);//得到图2的关键点（keypoints_2）

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);//得到descriptors_1
    descriptor->compute(img_2, keypoints_2, descriptors_2);//得到descriptors_2

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;//暂时存放匹配点,因为后面还要进行筛选
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 第四步:对match进行筛选，得到好的匹配点，把好的匹配点放在matches中
    //先定义两个变量，一个是最大距离，一个是最小距离
    double min_dist = 10000, max_dist = 0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for (int i = 0; i < descriptors_1.rows; i++) {//描述子本质是由 0,1 组成的向量
        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)//p是像素坐标，k是相机内参
{
    return Point2d(
        (p.x - k.at<double>(0,2))/ k.at<double>(0,0),//世界坐标系的X
        (p.y - k.at<double>(1,2) )/ k.at<double>(1,1)//世界坐标系的Y，其中Z被归一化
    );
}
//手写高斯牛顿
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 iters = 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 < iters; iter++)
    {
        Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();//初始化H矩阵
        Vector6d b = Vector6d::Zero();//对b矩阵初始化

        cost = 0;
        for (int i = 0; i < points_3d.size(); i++)// 遍历所有的特征点  计算cost
        {
            Eigen::Vector3d pc = pose * points_3d[i];//利用待优化的pose得到图2的相机坐标下的3d点
            double inv_z = 1.0 / pc[2];//得到图2的相机坐标下的3d点的z的倒数，也就是1/z
            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();//cost=e*e 向量的平方范数，等价于向量做点积
            //定义雅克比矩阵J 公式7.46
            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;//高斯牛顿法的增量方程 公式6.32
            b += -J.transpose() * e;
        }
        //出了这个内循环，表述结束一次迭代的计算，接下来，要求pose了
        Vector6d dx;
        dx = H.ldlt().solve(b);//公式6.33 计算增量方程 Hdx=b
        if(isnan(dx[0])) //判断dx这个数是否有效
        {
            cout << "result is nan" <<endl;
            break;
        }
        //如果我们进行了迭代，且最后的cost>=lastcost的话，那就表明满足要求了，可以停止迭代了
        if (iter >0 && cost >= lastcost)
        {
            cout <<"cost" << cost <<",last cost :" <<lastcost << endl;
            break;
        }
        //优化pose 也就是用dx更新pose
        pose = Sophus::SE3d::exp(dx) * pose;//dx是李代数，要转换为李群
        lastcost = cost;
        cout << "iteration" <<iter << "cost" << std::setprecision(12) << cost << endl;
        //std::setprecision(12)浮点数控制位数为12位
        //如果误差特别小了，也结束迭代
        if (dx.norm() <1e-6)
        {
            break;
        }
    }
    cout<<"pose by g-n \n"<<pose.matrix()<<endl;
}

class Vertexpose: public g2o::BaseVertex<6,Sophus::SE3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//必须写，我也不知道为什么
    //重载setToOriginImpl函数 这个应该就是把刚开的待优化的pose放进去
    virtual void setToOriginImpl() override
    {
        _estimate = Sophus::SE3d();
    }
    //重载oplusImpl函数，用来更新pose（待优化的系数）
    virtual void oplusImpl(const double *update) override
    {
        Eigen::Matrix<double,6,1> update_eigen;//更新的量，就是增量呗，dx
        update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate=Sophus::SE3d::exp(update_eigen)* _estimate;//更新pose 李代数要转换为李群，这样才可以左乘
    }
    //存盘 读盘 ：留空
    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;//必须写，我也不知道为什么
    //有参构造，初始化 图1中的3d点 以及相机内参K
    EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos),_K(K) {}
    //计算误差
    virtual void computeError() override
    {
        const Vertexpose *v=static_cast<const Vertexpose *>(_vertices[0]);
        Sophus::SE3d T=v->estimate();
        Eigen::Vector3d pos_pixel = _K * (T * _pos3d);//T * _pos3d是图2的相机坐标下的3d点,pos_pixel 是公式7.35中的su
        pos_pixel /= pos_pixel[2];//得到了像素坐标的齐次形式，公式7.35中的u
        _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;//图2的相机坐标下的3d点
        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;
        //雅克比矩阵见 书 p187 公式7.46
        _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;
};

//利用g2o优化pose
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);   // 设置求解器 算法g-n
    optimizer.setVerbose(true);       // 打开调试输出
    //加入顶点
    Vertexpose *v=new Vertexpose();
    v->setEstimate(Sophus::SE3d());
    v->setId(0);
    optimizer.addVertex(v);
    //K
    // 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);

    //加入边
    int index=1;
    for(size_t i=0;i<points_2d.size();++i)
    {
        //遍历 把3d点和像素点拿出来
        auto p2d = points_2d[i];
        auto p3d = points_3d[i];
        EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);//有参构造
        edge->setId(index);
        edge->setVertex(0,v);
        edge->setMeasurement(p2d);//设置观测值，其实就是图2 里的匹配特征点的像素位置
        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" << v->estimate().matrix() << endl;
    pose = v->estimate();
}

4.2 结果分析

一共用了三种求解方式。第一种，调用cv的函数pnp求解 R ,t；第二种，手写高斯牛顿进行位姿优化；第三种，利用g2o进行位姿优化。

三种方式中的R与前面对极几何求出来的R都相差不大，但t相差许多，通过添加深度信息提高了准确性。代码方面的用时都是小于1ms，但其中通过g2o可以添加多张图片进去。（具体结果自己运行代码查看）

其中关于g2o的知识可以查看文档《g2o的原理》在这个里面进行了讲解。

5 3D-3D:ICP

对于一组匹配好的3D点可以用迭代最近点（ICP）进行求解R，t。主要用两种解法，线性代数求解（SVD）、非线性优化。

线性代数求解（SVD）：通过构建图片之间的误差项，构建最小二乘法，求使误差平方和达到极小的R和t。

非线性优化：构建误差的最小二乘法，对误差项求一次导数得到雅可比矩阵，通过高斯牛顿法优化法进行求解。

5.1 代码详解

//
// Created by nnz on 2020/11/5.
//
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
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);

void pose_estimation_3d3d(
        const vector<Point3f> &pts1,
        const vector<Point3f> &pts2,
        Mat &R, Mat &t
);

//
void bundleAdjustment(
        const vector<Point3f> &pts1,
        const vector<Point3f> &pts2,
        Mat &R, Mat &t);
int main(int argc,char** argv)
{
    if(argc!=5)
    {
        cout<<" usage: pose_estimation_3d3d 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);
    vector<KeyPoint> keypoints_1, keypoints_2;//容器keypoints_1, keypoints_2分别存放图1和图2的特征点
    vector<DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);//得到图1与图2的特征匹配点
    cout << "一共找到了" << matches.size() << "组匹配点" << endl;
    //接下来的是建立3d点 利用深度图可以获取深度信息
    //depth1是图1对应的深度图 depth2是图2对应的深度图
    Mat depth1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);       // 深度图为16位无符号数，单通道图像
    Mat depth2 = imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);

    //内参矩阵
    Mat K =(Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    vector<Point3f> pts1, pts2;

    for(DMatch m:matches)
    {
        //先把两图特征匹配点对应的深度拿出来
        ushort d1=depth1.ptr<unsigned short >(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
        ushort d2=depth2.ptr<unsigned short >(int(keypoints_2[m.trainIdx].pt.y))[int(keypoints_2[m.trainIdx].pt.x)];
        if(d1==0 || d2==0)//深度无效
            continue;
        Point2d p1=pixel2cam(keypoints_1[m.queryIdx].pt,K);//得到图1的特征匹配点在其相机坐标下的x ,y
        Point2d p2=pixel2cam(keypoints_2[m.trainIdx].pt,K);//得到图2的特征匹配点在其相机坐标下的x ,y
        //对深度进行尺度变化得到真正的深度
        float dd1 = float(d1) / 5000.0;
        float dd2 = float(d2) / 5000.0;
        //容器 pts_1与pts_2分别存放 图1中的特征匹配点其相机坐标下的3d点 和 图2中的特征匹配点其相机坐标下的3d点
        pts1.push_back(Point3f(p1.x * dd1, p1.y * dd1, dd1));
        pts2.push_back(Point3f(p2.x * dd2, p2.y * dd2, dd2));
    }
    //这样就可以得到 3d-3d的匹配点
    cout << "3d-3d pairs: " << pts1.size() << endl;
    Mat R, t;
    cout<<" ************************************ICP 3d-3d by SVD**************************************** "<<endl;
    pose_estimation_3d3d(pts1, pts2, R, t);
    cout << "ICP via SVD results: " << endl;
    cout << "R = " << R << endl;
    cout << "t = " << t << endl;
    cout << "R^T = " << R.t() << endl;
    cout << "t^T = " << -R.t() * t << endl;
    cout<<" ************************************ICP 3d-3d by SVD**************************************** "<<endl;
    cout<<endl;
    cout<<" ************************************ICP 3d-3d by g2o**************************************** "<<endl;
    bundleAdjustment(pts1, pts2, R, t);
    cout<<"R= \n"<<R<<endl;
    cout<<"t = "<< t.t() <<endl;
    cout<<"验证 p2 = R*P1 +t "<<endl;
    for (int i = 0; i < 5; i++) {
        cout << "p1 = " << pts1[i] << endl;
        cout << "p2 = " << pts2[i] << endl;
        cout << "(R*p1+t) = " <<
             R * (Mat_<double>(3, 1) << pts1[i].x, pts1[i].y, pts1[i].z) + t
             << endl;
        cout << endl;
    }
    cout<<" ************************************ICP 3d-3d by g2o**************************************** "<<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 detector = FeatureDetector::create ( "ORB" );
    // Ptr 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]);
        }
    }
}

//实现像素坐标到相机坐标的转换（求出来的只是包含相机坐标下的x,y的二维点）
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)
            );
}

//参考书上的p197页
void pose_estimation_3d3d(
        const vector<Point3f> &pts1,
        const vector<Point3f> &pts2,
        Mat &R, Mat &t
)
{
    int N=pts1.size();//匹配的3d点个数
    Point3f p1,p2;//质心
    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);
    for(int i=0;i<N;i++)
    {
        //去质心
        q1[i]=pts1[i]-p1;
        q2[i]=pts2[i]-p2;
    }
    //计算 W+=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 = "<<endl<<W<<endl;
    //利用svd分解 W=U*sigema*V
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W,Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U=svd.matrixU();//得到U矩阵
    Eigen::Matrix3d V=svd.matrixV();//得到V矩阵
    cout << "U=" << U << endl;
    cout << "V=" << V << endl;
    Eigen::Matrix3d R_=U*(V.transpose());
    if (R_.determinant() < 0)//若旋转矩阵R_的行列式<0 则取负号
    {
        R_ = -R_;
    }
    Eigen::Vector3d t_=Eigen::Vector3d (p1.x,p1.y,p1.z)-R_*Eigen::Vector3d (p2.x,p2.y,p2.z);//得到平移向量
    //把 Eigen形式的 r 和 t_ 转换为CV 中的Mat格式
    R = (Mat_<double>(3, 3) <<
                            R_(0, 0), R_(0, 1), R_(0, 2),
            R_(1, 0), R_(1, 1), R_(1, 2),
            R_(2, 0), R_(2, 1), R_(2, 2)
    );
    t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

//对于用g2o来进行优化的话，首先要定义顶点和边的模板
//顶点，也就是咱们要优化的pose 用李代数表示它 6维
class Vertexpose: public g2o::BaseVertex<6,Sophus::SE3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//必须写，我也不知道为什么
    //重载setToOriginImpl函数 这个应该就是把刚开的待优化的pose放进去
    virtual void setToOriginImpl() override
    {
        _estimate = Sophus::SE3d();
    }
    //重载oplusImpl函数，用来更新pose（待优化的系数）
    virtual void oplusImpl(const double *update) override
    {
        Eigen::Matrix<double,6,1> update_eigen;//更新的量，就是增量呗，dx
        update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate=Sophus::SE3d::exp(update_eigen)* _estimate;//更新pose 李代数要转换为李群，这样才可以左乘
    }
    //存盘 读盘 ：留空
    virtual bool read(istream &in) override {}

    virtual bool write(ostream &out) const override {}
};
//定义边
class EdgeProjectXYZRGBD: public g2o::BaseUnaryEdge<3,Eigen::Vector3d,Vertexpose>
{
public:
    EdgeProjectXYZRGBD(const Eigen::Vector3d &point) : _point(point) {}//赋值这个是图1坐标下的3d点
    //计算误差
    virtual void computeError() override
    {
        const Vertexpose *v=static_cast<const Vertexpose *>(_vertices[0]);//顶点v
        _error = _measurement - v->estimate() * _point;


    }
    //计算雅克比
    virtual void linearizeOplus() override
    {
        const Vertexpose *v=static_cast<const Vertexpose *>(_vertices[0]);//顶点v
        Sophus::SE3d T=v->estimate();//把顶点的待优化系数拿出来
        Eigen::Vector3d xyz_trans=T*_point;//变换到图2下的坐标点
        //下面的雅克比没看懂
        _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
        _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);

    }

    bool read(istream &in) {}

    bool write(ostream &out) const {}
protected:
    Eigen::Vector3d _point;
};
//利用g2o
void bundleAdjustment(const vector<Point3f> &pts1,
                      const vector<Point3f> &pts2,
                      Mat &R, Mat &t)
{
// 构建图优化，先设定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);   // 设置求解器 算法g-n
    optimizer.setVerbose(true);       // 打开调试输出
    //加入顶点
    Vertexpose *v=new Vertexpose();
    v->setEstimate(Sophus::SE3d());
    v->setId(0);
    optimizer.addVertex(v);

    //加入边
    int index=1;
    for(size_t i=0;i<pts1.size();i++)
    {
        EdgeProjectXYZRGBD *edge = new EdgeProjectXYZRGBD(Eigen::Vector3d(pts1[i].x,pts1[i].y,pts1[i].z));
        edge->setId(index);//边的编号
        edge->setVertex(0,v);//设置顶点  顶点编号
        edge->setMeasurement(Eigen::Vector3d(pts2[i].x,pts2[i].y,pts2[i].z));
        edge->setInformation(Eigen::Matrix3d::Identity());//set信息矩阵为单位矩阵
        optimizer.addEdge(edge);//加入边
        index++;
    }
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    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 << endl << "after optimization:" << endl;
    cout << "T=\n" << v->estimate().matrix() << endl;

    // 把位姿转换为Mat类型
    Eigen::Matrix3d R_ = v->estimate().rotationMatrix();
    Eigen::Vector3d t_ = v->estimate().translation();
    R = (Mat_<double>(3, 3) <<
                            R_(0, 0), R_(0, 1), R_(0, 2),
            R_(1, 0), R_(1, 1), R_(1, 2),
            R_(2, 0), R_(2, 1), R_(2, 2)
    );
    t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

6 总结

第七章看书理解加手敲代码花费了8天左右，其中手敲代码是最花费时间但又必不可少的一部分，在一些函数，参数不懂时，可以直接查看该参数的定义。第二部分就是安装软件加cmake，由于使用的十四讲第二版的代码其中的g2o（最新版）就只能运行第二版代码，第一版无法运行，还有一些cmake相关的问题，查阅资料花费许多时间。第三部分就是对于这部分公式的推导和代码的理解，第七章中用到了第三章中的旋转向量转旋转矩阵，第四章的李群与李代数，重点是左扰动更新模型，第五章中的相机模型，归一化坐标大量用到，需要弄懂空间坐标系到相机坐标系，空间坐标系到像素坐标系及相机坐标系到像素坐标系的关系（这部分很重要）。而第六部分就比较晦涩难懂，要着重理解最小二乘法在优化方面的含义，对于优化方面，需要看懂高斯牛顿法和列文伯格法，还有g2o原理不复杂，只是在运用方面有许多要求。

结束于2022/3/5 20:30

bug

1.#include

./usr/local/include/sophus/common.hpp:36:10: fatal error: fmt/core.h: 没有那个文件或目录**
#include

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0U6MdjE6-1647311685764)(视觉SLAM十四讲——ch7.assets/482a27b3355eba79de8a9c1616235d5-16464838018401.png)]

解决方案：需要在cmakelists中添加类似于第五行和第12行的代码，将其中的3d_2d改为自己的可执行文件名。

add_executable(3d_2d 3d_2d.cpp)
target_link_libraries(3d_2d
        g2o_core g2o_stuff
        ${OpenCV_LIBS})
target_link_libraries(3d_2d Sophus::Sophus)


add_executable(3d_3d 3d_3d.cpp)
target_link_libraries(3d_3d
        g2o_core g2o_stuff
      ${OpenCV_LIBS})
      target_link_libraries(3d_3d Sophus::Sophus)