第7讲 视觉里程计1 下篇

5 求解PnP

5.1 使用EPnP求解位姿

  已知图像1、图像2和图像1的深度图，那么就可以得到图像1中的路标点和图像2中相对应的特征点，利用OpenCV中的solvePnP()函数求解两帧之间的旋转运动和平移运动。
  cpp文件内容如下，

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

void find_feature_matches(Mat img1, Mat img2, vector<KeyPoint>& keypoints1, vector<KeyPoint>& keypoints2, vector<DMatch>& matches);
void compute_landmarks(Mat depth, vector<KeyPoint> keypoints1, vector<KeyPoint> keypoints2, vector<DMatch> matches, vector<Point3f>& landmarks1, vector<Point2f>& pixels2);

Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

//图片路径
string path_img1 = "../1.png";
string path_img2 = "../2.png";
string path_depth1 = "../1_depth.png";


int main()
{
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    Mat img1 = imread(path_img1, IMREAD_COLOR);  //IMREAD_COLOR表示返回彩色图
    Mat img2 = imread(path_img2, IMREAD_COLOR);
    vector<KeyPoint> keypoints1, keypoints2;
    vector<DMatch> matches;
    find_feature_matches(img1, img2, keypoints1, keypoints2, matches);
    cout << "匹配点对的数目为" << matches.size() << "！" << endl;

    Mat depth1_img = imread(path_depth1, IMREAD_ANYDEPTH);  //IMREAD_ANYDEPTH表示返回图片实际深度
    vector<Point3f> landmarks1;
    vector<Point2f> pixels2;
    compute_landmarks(depth1_img, keypoints1, keypoints2, matches, landmarks1, pixels2);
    //landmarks1表示第1帧图像上的路标点，pixels2表示相对应的第2帧图像上的像素点


    Mat r, t;  //r表示旋转向量
    solvePnP(landmarks1, pixels2, K, Mat(), r, t, false);  //调用OpenCV中的solvePnP()函数求解PnP问题
    Mat R;
    Rodrigues(r, R);  //将旋转向量r转换成选旋转矩阵R
    cout << "旋转矩阵R为：\n" << R << endl;
    cout << "平移向量t为：\n" << t << endl;

    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_used.count() << "秒！" << endl;
    return 0;
}


void find_feature_matches(Mat img1, Mat img2, vector<KeyPoint>& keypoints1, vector<KeyPoint>& keypoints2, vector<DMatch>& matches)
{
    Ptr<ORB> ptrORB = ORB::create(1000, 1.2, 8);

    //提取特征点
    ptrORB->detect(img1, keypoints1);
    ptrORB->detect(img2, keypoints2);

    //计算描述子
    Mat desc1, desc2;
    ptrORB->compute(img1, keypoints1, desc1);
    ptrORB->compute(img2, keypoints2, desc2);

    //特征匹配
    vector<DMatch> raw_matches;
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    matcher->match(desc1, desc2, raw_matches);

    //排除误匹配
    float mindist = 196.0;
    for(auto m : raw_matches) mindist = min(mindist, m.distance);
    float threshold = max(30.0f, 2 * mindist);
    for(auto m : raw_matches)
        if(m.distance <= threshold)
            matches.push_back(m);

}


void compute_landmarks(Mat depth, vector<KeyPoint> keypoints1, vector<KeyPoint> keypoints2, vector<DMatch> matches, vector<Point3f>& landmarks1, vector<Point2f>& pixels2)
{
    for(auto m : matches)
    {
        Point2f pixel1 = keypoints1[m.queryIdx].pt;
        Point3f landmark1;
        double X, Y, Z;  //unsigned short表示无符号短整型，占2个字节
        unsigned int val = depth.at<unsigned short>(pixel1.y, pixel1.x);
        if(val == 0) continue;  //val为0表示无测量
        Z = val / 5000.0;
        X = (pixel1.x - K.at<double>(0, 2)) / K.at<double>(0, 0) * Z;
        Y = (pixel1.y - K.at<double>(1, 2)) / K.at<double>(1, 1) * Z;
        landmark1.x = X; landmark1.y = Y; landmark1.z = Z;
        landmarks1.push_back(landmark1);

        Point2f pixel2 = keypoints2[m.trainIdx].pt;
        pixels2.push_back(pixel2);

    }
}

  CMakeLists.txt文件内容如下，

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES})

  结果运行如下，

匹配点对的数目为180！
旋转矩阵R为：
[0.9980519308491493, -0.05152539254935481, 0.03517779485583998;
 0.0506452041014253, 0.998391870298167, 0.02547030867604423;
 -0.03643359205197375, -0.02363910415313768, 0.9990564479172472]
平移向量t为：
[-0.1201992647802895;
 -0.00436009046505001;
 0.06336692066008472]
执行程序花费的时间为：0.0562408秒！

5.2 手写位姿估计

5.2.1 增量方程推导

  记图像1中的路标点集合为 P 1 = { P 1 1 , P 1 2 , . . . , P 1 n } P_1=\{P_1^1, P_1^2,...,P_1^n\} P1​={P11​,P12​,...,P1n​}，图像2中的相对应的特征点集合为 q 2 = { q 2 1 , q 2 2 , . . . , q 2 n } q_2=\{q_2^1,q_2^2,...,q_2^n\} q2​={q21​,q22​,...,q2n​}。其中$P_1^i$的形式为$(X_1^i,Y_1^i,Z_1^i)$，$q_2^i$的形式为$(u_2^i,v_2^i)$。那么，优化问题可表述如下，
$$T^{\ast }=\underset{T}{\mathrm{argmin}}\sum _{i=1}^n\Vert q_2^i-\frac{1}{Z_2^i}KT{P}_1^i{\Vert }_2^2\text{\hspace{1em}(1)}$$
其中$Z_2^i$表示图像2的路标点的$z$分量；$K$是内参矩阵；$T$为图像1到图像2的变换矩阵，它是待求量。那么，记误差项$e_i$为，
$$e_i=q_2^i-\frac{1}{Z_2^i}KT{P}_1^i\text{\hspace{1em}(2)}$$
故代价函数Cost Function记为，
$$F=\sum _{i=1}^n\Vert e_i{\Vert }_2^2\text{\hspace{1em}(3)}$$
其中$\Vert \cdot {\Vert }_2$在此处表示2 * 1维向量的模。那么雅克比矩阵$J$可计算如下，
$$J_i=\frac{\partial e_i}{\partial T}=\frac{\partial e_i}{\partial (T{P}_1^i)}\cdot \frac{\partial (T{P}_1^i)}{\partial T}\text{\hspace{1em}(4)}$$
注意此处中$\partial T$的写法是错误的，因为欧式群对加法不封闭，所以无法进行求导运算，在此处这样写是为了方便理解，正确的写法是这样$\partial \delta \xi$李代数增量的形式。依次求解上式各分量为，首先将$\partial (T{P}_1^i)$记作$P_2^i$，其形式为$(X_2^i,Y_2^i,Z_2^i{)}^T$；而$e_i$的展开项为$(\Delta u_2^i,\Delta v_2^i{)}^T$。因此，公式(2)展开写成如下形式，
$$\left[\begin{array}{c}\Delta u_2^i\\ \Delta v_2^i\\ 1\end{array}\right]=\left[\begin{array}{c}{u}_2^i\\ v_2^i\\ 1\end{array}\right]-\frac{1}{Z_2^i}\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_2^i\\ Y_2^i\\ Z_2^i\end{array}\right]\text{\hspace{1em}(5)}$$
把各行乘出来，得，
$$\Delta u_2^i=u_2^i-(f_x\frac{X_2^i}{Z_2^i}+c_x)\text{\hspace{1em}(6)}$$

$$\Delta v_2^i=v_2^i-(f_y\frac{Y_2^i}{Z_2^i}+c_y)\text{\hspace{1em}(7)}$$
那么，$\frac{\partial e_i}{\partial (T{P}_1^i)}$可计算如下，
$$\frac{\partial e_i}{\partial (T{P}_1^i)}=-\left[\begin{array}{ccc}\frac{f_x}{Z_2^i}& 0& -\frac{f_x{X}_2^i}{{Z_2^i}^2}\\ 0& \frac{f_y}{Z_2^i}& -\frac{f_y{Y}_2^i}{{Z_2^i}^2}\end{array}\right]\text{\hspace{1em}(8)}$$
而对于$\frac{\partial (T{P}_1^i)}{\partial T}$直接套用公式有，
$$\frac{\partial (T{P}_1^i)}{\partial T}=\left[\begin{array}{cc}{I}_{3\times 3}& -P_2^i\times \end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& Z_2^i& -Y_2^i\\ 0& 1& 0& -Z_2^i& 0& X_2^i\\ 0& 0& 1& Y_2^i& -X_2^i& 0\end{array}\right]\text{\hspace{1em}(9)}$$
故公式(4)可写成如下，
$$J_i=\frac{\partial e_i}{\partial T}=-\left[\begin{array}{cccccc}\frac{f_x}{Z_2^i}& 0& -\frac{f_x{X}_2^i}{{Z_2^i}^2}& -\frac{f_x{X}_2^i{Y}_2^i}{{Z_2^i}^2}& f_x+\frac{f_x{X_2^i}^2}{{Z_2^i}^2}& -\frac{f_x{Y}_2^i}{Z_2^i}\\ 0& \frac{f_y}{Z_2^i}& -\frac{f_y{Y}_2^i}{{Z_2^i}^2}& -f_y-\frac{f_y{Y_2^i}^2}{{Z_2^i}^2}& \frac{f_y{X}_2^i{Y}_2^i}{{Z_2^i}^2}& \frac{f_y{X}_2^i}{Z_2^i}\end{array}\right]\text{\hspace{1em}(10)}$$
那么，该优化问题的增量方程为，
$$(\sum _{i=1}^n{J}_i^T{J}_i)\Delta x=\sum _{i=1}^n-J_i^T{e}_i\text{\hspace{1em}(11)}$$
注意，此处中的$e_i$为其前2行，而$\Delta x$表示李代数增量。

5.2.2 代码部分

  cpp文件内容为，

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <Eigen/Core>  //Eigen核心模块

using namespace std;
using namespace cv;
using namespace Sophus;
using namespace Eigen;

typedef vector<Vector2d, aligned_allocator<Vector2d>> VecVector2d;
typedef vector<Vector3d, aligned_allocator<Vector3d>> VecVector3d;

void computeLandmarksAndPixels(Mat img1, Mat img2, Mat depthImg1, VecVector3d& landmarks1, VecVector2d& pixels2);
void bundleAdjustmentGaussNewton(VecVector3d landmarks1, VecVector2d pixels2, SE3d& pose);  //pose为优化变量

//相机内参
double fx = 520.9, cx = 325.1, fy = 521.0, cy = 249.7;

string img1_path = "../1.png";  //图像1路径
string img2_path = "../2.png";  //图像2路径
string depth1_path = "../1_depth.png";  //深度图1路径


int main()
{
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    Mat img1 = imread(img1_path, IMREAD_COLOR);  //IMREAD_COLOR返回彩色图
    Mat img2 = imread(img2_path, IMREAD_COLOR);  //IMREAD_COLOR返回彩色图
    Mat depthImg1 = imread(depth1_path, IMREAD_ANYDEPTH);  //IMREAD_ANYDEPTH返回原图

    VecVector3d landmarks1;
    VecVector2d pixels2;
    computeLandmarksAndPixels(img1, img2, depthImg1, landmarks1, pixels2);
    cout << "3D-2D匹配点对的数目为：" << landmarks1.size() << endl;

    SE3d pose;  //优化变量，图像1到图像2的变换矩阵
    cout << "pose = \n" << pose.matrix() << endl;  //默认初始化为单位阵，零向量
    bundleAdjustmentGaussNewton(landmarks1, pixels2, pose);

    cout << "利用高斯牛顿法优化的求解出来的从图像1到图像2的变换矩阵为：\n" << pose.matrix() << endl;

    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_used.count() << "秒！" << endl;

    return 0;
}


void computeLandmarksAndPixels(Mat img1, Mat img2, Mat depthImg1, VecVector3d& landmarks1, VecVector2d& pixels2)
{
    Ptr<ORB> ptrORB = ORB::create(1000, 1.2, 8);
    vector<KeyPoint> keypoints1, keypoints2;
    ptrORB->detect(img1, keypoints1);
    ptrORB->detect(img2, keypoints2);
    Mat desc1, desc2;
    ptrORB->compute(img1, keypoints1, desc1);
    ptrORB->compute(img2, keypoints2, desc2);

    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    vector<DMatch> rawMatches, matches;
    matcher->match(desc1, desc2, rawMatches);
    float minDist = 196;
    for(auto m : rawMatches) minDist = min(minDist, m.distance);
    float threshold = max(30.0f, 2 * minDist);
    for(auto m : rawMatches)
        if(m.distance <= threshold)
            matches.push_back(m);

    cout << "特征点对数目为：" << matches.size() << endl;

    for(auto m : matches)
    {
        Point2f pixel1, pixel2;
        pixel1 = keypoints1[m.queryIdx].pt;
        pixel2 = keypoints2[m.trainIdx].pt;

        ushort val = depthImg1.at<ushort>(pixel1.y, pixel1.x);
        if(val == 0) continue;
        float Z1 = val / 5000.0;
        float X1 = (pixel1.x - cx) / fx * Z1;
        float Y1 = (pixel1.y - cy) / fy * Z1;

        landmarks1.push_back(Vector3d(X1, Y1, Z1));
        pixels2.push_back(Vector2d(pixel2.x, pixel2.y));

    }
}


void bundleAdjustmentGaussNewton(VecVector3d landmarks1, VecVector2d pixels2, SE3d& pose)
{
    int maxIterations = 10;
    double lastCost = 0, currentCost = 0;

    for(int iter = 0; iter < maxIterations; iter++)
    {
        Matrix6d H = Matrix6d::Zero();
        Vector6d b = Eigen::Matrix<double, 6, 1>::Zero();

        assert(landmarks1.size() == pixels2.size());  //断言函数，其值为假程序终止执行！

        currentCost = 0;  //代价函数值置为0
        //计算增量方程中的H和b以及代价函数值
        for(int i = 0; i < landmarks1.size(); i++)
        {
            Vector3d landmark2 = pose * landmarks1[i];
            Vector2d pixel2 = pixels2[i];

            Eigen::Matrix<double, 2, 6> Ji;
            Ji << fx / landmark2[2], 0, -fx * landmark2[0] / (landmark2[2] * landmark2[2]),
                  -fx * landmark2[0] * landmark2[1] / (landmark2[2] * landmark2[2]),
                  fx + fx * landmark2[0] * landmark2[0] / (landmark2[2] * landmark2[2]),
                  - fx * landmark2[1] / landmark2[2],
                  0, fy / landmark2[2], - fy * landmark2[1] / (landmark2[2] * landmark2[2]),
                  -fy - fy * landmark2[1] * landmark2[1] / (landmark2[2] * landmark2[2]),
                  fy * landmark2[0] * landmark2[1] / (landmark2[2] * landmark2[2]),
                  fy * landmark2[0] / landmark2[2];
            Ji = -1 * Ji;
            Eigen::Matrix<double, 2, 1> ei;
            ei[0] = pixel2[0] - (fx * landmark2[0] / landmark2[2] + cx);
            ei[1] = pixel2[1] - (fy * landmark2[1] / landmark2[2] + cy);

            H += Ji.transpose() * Ji;
            b += (-Ji.transpose() * ei);

            currentCost += ei.transpose() * ei;

        }

        Vector6d dx;
        dx = H.ldlt().solve(b);

        cout << "当i等于" << iter << "时，代价函数值为" << currentCost << "，李代数增量dx为：" << dx.transpose() << "! " << endl;

        if(isnan(dx[0]))
        {
            cout << "结果不是一个数，退出迭代！" << endl;
            break;
        }

        if(iter > 0 && currentCost >= lastCost)
        {
            cout << "代价不再减小，退出迭代！" << endl;
            break;
        }

        pose = SE3d::exp(dx) * pose;
        lastCost = currentCost;

        //更新完pose之后再检验dx的大小
        if(dx.norm() < 1e-6)
        {
            cout << "李代数增量dx的模小于1e-6，退出迭代！" << endl;
            break;
        }


    }

}

  CMakeLists.txt文件内容为，

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

include_directories("/usr/include/eigen3")

find_package(Sophus REQUIRED)
include_directories(${Sophus_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES})

  程序运行结果为，

特征点对数目为：180
3D-2D匹配点对的数目为：171
pose = 
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
当i等于0时，代价函数值为116276，李代数增量dx为：  -0.114901 -0.00358983   0.0642109  -0.0242627   0.0335207   0.0491333! 
当i等于1时，代价函数值为1376.56，李代数增量dx为： -0.00669116   0.00147267  -0.00307227 -0.000319212   0.00232894   0.00197689! 
当i等于2时，代价函数值为1067.18，李代数增量dx为： 1.33342e-05 -2.64446e-05 -1.78147e-05 -1.52531e-05 -1.17136e-06 -9.18748e-06! 
当i等于3时，代价函数值为1067.18，李代数增量dx为：-1.17326e-07  2.30428e-08   2.2405e-08  1.58587e-08  6.57601e-08 -2.95587e-08! 
李代数增量dx的模小于1e-6，退出迭代！
利用高斯牛顿法优化的求解出来的从图像1到图像2的变换矩阵为：
  0.998052 -0.0515254  0.0351778  -0.120199
 0.0506452   0.998392  0.0254703 -0.0043601
-0.0364336 -0.0236391   0.999056  0.0633669
         0          0          0          1
执行整个程序花费的时间为：0.068727秒！

5.3 使用g2o进行BA优化

  cpp文件内容为，

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>  //Eigen核心模块
#include <sophus/se3.hpp>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>


using namespace std;
using namespace cv;
using namespace Eigen;
using namespace Sophus;

typedef vector<Vector2d, aligned_allocator<Vector2d>> VecVector2d;
typedef vector<Vector3d, aligned_allocator<Vector3d>> VecVector3d;

void computeLandmarksAndPixels(Mat img1, Mat img2, Mat depthImg1, VecVector3d& landmarks1, VecVector2d& pixels2);
void bundleAdjustmentG2O(VecVector3d landmarks1, VecVector2d pixels2, SE3d& pose);


//相机内参
double fx = 520.9, cx = 325.1, fy = 521.0, cy = 249.7;

string img1_path = "../1.png";  //图像1路径
string img2_path = "../2.png";  //图像2路径
string depth1_path = "../1_depth.png";  //深度图1路径


//写顶点的类
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>  //告诉g2o顶点的变量维数和变量类型
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;  //解决Eigen库数据结构中存储对齐问题

    //g2o中的_estimate初始化
    virtual void setToOriginImpl() override  //virtual表示虚函数，override表示重写了基类的虚函数setToOriginImpl()
    {
        _estimate = SE3d();
    }

    //g2o中的_estimate的更新
    virtual void oplusImpl(const double* update) override  //virtual表示虚函数，override表示重写了基类的虚函数oplusImpl()
    {
        Eigen::Matrix<double, 6, 1> update_eigen;
        update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate = SE3d::exp(update_eigen) * _estimate;
    }

    //存盘和读盘：留空
    virtual bool read(istream& in) override {}  //istream是C++标准输入流的一个基类
    virtual bool write(ostream& out) const override {}  //ostream是C++标准输出流的一个基类
};


//写边的类
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose>  //告诉g2o边的变量维数、变量类型及其连接的顶点的类
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;  //解决Eigen类中数据结构的存储对齐问题

    EdgeProjection(const Vector3d& pos, const Matrix3d& K) : _pos3d(pos), _K(K) {}  //类EdgeProjection的构造函数，使用列表赋初值

    virtual void computeError() override  //重写基类中计算误差的虚函数computeError()
    {
        const VertexPose* v = static_cast<VertexPose*> (_vertices[0]);  //_vertices[0]哪里出来的呢？
        //v是VertexPose类型指针
        Sophus::SE3d T = v->estimate();  //将_estimate的值赋给变量T
        Vector3d pos_pixel = _K * (T * _pos3d);
        pos_pixel /= pos_pixel[2];
        _error = _measurement - pos_pixel.head<2>();  //.head<2>()表示取前两维
    }

    virtual void linearizeOplus() override  //重写基类中计算雅可比矩阵的虚函数linearizeOplus()
    {
        const VertexPose* v = static_cast<VertexPose*> (_vertices[0]);
        SE3d T = v->estimate();
        Vector3d pos_cam = T * _pos3d;
        double fx = _K(0, 0), fy = _K(1, 1), cx = _K(0, 2), cy = _K(1, 2);
        double X = pos_cam[0], Y = pos_cam[1], Z = pos_cam[2];
        double Z2 = Z * Z;
        _jacobianOplusXi
        << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
           0, -fy / Z, fy * Y / Z2, 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;  //定义私有成员变量_pos3d
    Eigen::Matrix3d _K;  //定义私有成员变量_K

};


int main()
{
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    Mat img1 = imread(img1_path, IMREAD_COLOR);  //IMREAD_COLOR返回彩色图
    Mat img2 = imread(img2_path, IMREAD_COLOR);  //IMREAD_COLOR返回彩色图
    Mat depthImg1 = imread(depth1_path, IMREAD_ANYDEPTH);  //IMREAD_ANYDEPTH返回原图

    VecVector3d landmarks1;
    VecVector2d pixels2;
    computeLandmarksAndPixels(img1, img2, depthImg1, landmarks1, pixels2);

    SE3d pose;
    bundleAdjustmentG2O(landmarks1, pixels2, pose);
    cout << "g2o优化的图像1到图像2的变换矩阵pose为：\n" << pose.matrix() << endl;

    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_used.count() << "秒！" << endl;
    return 0;
}


void computeLandmarksAndPixels(Mat img1, Mat img2, Mat depthImg1, VecVector3d& landmarks1, VecVector2d& pixels2)
{
    Ptr<ORB> ptrORB = ORB::create(1000, 1.2, 8);
    vector<KeyPoint> keypoints1, keypoints2;
    ptrORB->detect(img1, keypoints1);
    ptrORB->detect(img2, keypoints2);
    Mat desc1, desc2;
    ptrORB->compute(img1, keypoints1, desc1);
    ptrORB->compute(img2, keypoints2, desc2);

    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    vector<DMatch> rawMatches, matches;
    matcher->match(desc1, desc2, rawMatches);
    float minDist = 196;
    for(auto m : rawMatches) minDist = min(minDist, m.distance);
    float threshold = max(30.0f, 2 * minDist);
    for(auto m : rawMatches)
        if(m.distance <= threshold)
            matches.push_back(m);

    cout << "特征点对数目为：" << matches.size() << endl;

    for(auto m : matches)
    {
        Point2f pixel1, pixel2;
        pixel1 = keypoints1[m.queryIdx].pt;
        pixel2 = keypoints2[m.trainIdx].pt;

        ushort val = depthImg1.at<ushort>(pixel1.y, pixel1.x);
        if(val == 0) continue;
        float Z1 = val / 5000.0;
        float X1 = (pixel1.x - cx) / fx * Z1;
        float Y1 = (pixel1.y - cy) / fy * Z1;

        landmarks1.push_back(Vector3d(X1, Y1, Z1));
        pixels2.push_back(Vector2d(pixel2.x, pixel2.y));

    }

    cout << "3D-2D的匹配点对数目为：" << landmarks1.size() << endl;
}


void bundleAdjustmentG2O(VecVector3d landmarks1, VecVector2d pixels2, SE3d& pose)
{
    //构建图优化，先设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  //顶点是6维的，边是2维的，为什么写成3呢？
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;  //线性求解器类型
    //梯度下降方法，可以从GN，LM和DogLeg中选。此处选择的是GN。
    auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;  //图模型
    optimizer.setAlgorithm(solver);  //设置求解器
    optimizer.setVerbose(true);  //向屏幕中输出优化过程信息

    //顶点
    VertexPose* vertex_pose = new VertexPose();
    vertex_pose->setId(0);  //设置顶点编号为0
    vertex_pose->setEstimate(SE3d());  //设置_estimate的初始值
    optimizer.addVertex(vertex_pose);  //将顶点加入到优化器中

    //构建内参矩阵K
    Matrix3d K_eigen;
    K_eigen << fx, 0, cx, 0, fy, cy, 0, 0, 1;

    //边
    int index = 1;
    for(size_t i = 0; i < pixels2.size(); i++)
    {
        auto landmark1 = landmarks1[i];
        auto pixel2 = pixels2[i];
        EdgeProjection* edge = new EdgeProjection(landmark1, K_eigen);
        edge->setId(index);
        edge->setVertex(0, vertex_pose);  //设置连接的顶点，那0表示啥呢？
        edge->setMeasurement(pixel2);
        edge->setInformation(Eigen::Matrix2d::Identity());  //设置信息矩阵（测量噪声协方差之逆）为2*2单位阵
        optimizer.addEdge(edge);  //往优化器中添加边
        index++;
    }

    optimizer.initializeOptimization();  //初始化优化器
    optimizer.optimize(10);  //设置优化器中的迭代次数

    pose = vertex_pose->estimate();  //保存结果
}

  CMakeLists.txt文件内容为，

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

find_package(Sophus REQUIRED)
include_directories(${Sophus_DIRECTORIES})

include_directories("/usr/include/eigen3")

find_package(g2o REQUIRED)  #只能写g2o，不能写G2O
include_directories(${G2O_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES} g2o_core g2o_stuff)

  程序执行结果为，

特征点对数目为：180
3D-2D的匹配点对数目为：171
iteration= 0	 chi2= 1376.558049	 time= 0.00408863	 cumTime= 0.00408863	 edges= 171	 schur= 0
iteration= 1	 chi2= 1067.182435	 time= 0.00405133	 cumTime= 0.00813996	 edges= 171	 schur= 0
iteration= 2	 chi2= 1067.179614	 time= 0.0041188	 cumTime= 0.0122588	 edges= 171	 schur= 0
iteration= 3	 chi2= 1067.179614	 time= 0.00407222	 cumTime= 0.016331	 edges= 171	 schur= 0
iteration= 4	 chi2= 1067.179614	 time= 0.00405259	 cumTime= 0.0203836	 edges= 171	 schur= 0
iteration= 5	 chi2= 1067.179614	 time= 0.00415358	 cumTime= 0.0245372	 edges= 171	 schur= 0
iteration= 6	 chi2= 1067.179614	 time= 0.00403974	 cumTime= 0.0285769	 edges= 171	 schur= 0
iteration= 7	 chi2= 1067.179614	 time= 0.00414052	 cumTime= 0.0327174	 edges= 171	 schur= 0
iteration= 8	 chi2= 1067.179614	 time= 0.00411838	 cumTime= 0.0368358	 edges= 171	 schur= 0
iteration= 9	 chi2= 1067.179614	 time= 0.00410085	 cumTime= 0.0409367	 edges= 171	 schur= 0
g2o优化的图像1到图像2的变换矩阵pose为：
  0.998052 -0.0515254  0.0351778  -0.120199
 0.0506452   0.998392  0.0254703 -0.0043601
-0.0364336 -0.0236391   0.999056  0.0633669
         0          0          0          1
执行程序花费的时间为：0.0968127秒！

注chi2那列表示代价函数值！

6 求解ICP

  已知图像1和图像2，两张彩色图相应的深度图1和深度图2，求图像1到图像2的变换矩阵$T$。

6.1 SVD方法

6.1.1 理论推导

  知道彩色图和深度图，我们是可以计算出路标点$(X,Y,Z)$的。将图像1的路标点集记作 P 1 = { p 1 1 , p 1 2 , . . . , p 1 n } P_1=\{p_1^1,p_1^2,...,p_1^n\} P1​={p11​,p12​,...,p1n​}，将图像2相应的路标点集记作 P 2 = { p 2 1 , p 2 2 , . . . , p 2 n } P_2=\{p_2^1,p_2^2,...,p_2^n\} P2​={p21​,p22​,...,p2n​}，那么，计算两个点集的质心坐标如下，
$$p_1=\frac{1}{n}\sum _{i=1}^n{p}_1^i\text{\hspace{1em}(1)}$$

$$p_2=\frac{1}{n}\sum _{i=1}^n{p}_2^i\text{\hspace{1em}(2)}$$
计算两个点集的去质心坐标如下，
$$q_1^i=p_1^i-p_1\text{\hspace{1em}(3)}$$

$$q_2^i=p_2^i-p_2\text{\hspace{1em}(4)}$$
计算3 * 3维$W$矩阵如下，
$$W=\sum _{i=1}^n{q}_2^i(q_1^i{)}^T\text{\hspace{1em}(5)}$$
对矩阵$W$进行奇异值分解，
$$W=U\Sigma V^T\text{\hspace{1em}(6)}$$
故最优的旋转矩阵$R^{\ast }$为,
$$R^{\ast }=\{\begin{array}{ll}U{V}^T& det(U{V}^T)\ge 0\\ -U{V}^T& det(U{V}^T)<0\end{array}\text{\hspace{1em}(7)}$$
最优的平移向量为$t^{\ast }$为，
$$t^{\ast }=p_2-R^{\ast }{p}_1\text{\hspace{1em}(8)}$$

6.1.2 代码部分

  cpp文件内容为，

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>  //Eigen核心模块
#include <Eigen/Dense>  //Eigen稠密矩阵模块
#include <sophus/se3.hpp>

using namespace std;
using namespace cv;
using namespace Eigen;
using namespace Sophus;

//图片路径
string img1_path = "../1.png";
string img2_path = "../2.png";
string depth1_path = "../1_depth.png";
string depth2_path = "../2_depth.png";

//相机内参
double fx = 520.9, cx = 325.1, fy = 521.0, cy = 249.7;

typedef vector<Vector3d, aligned_allocator<Vector3d>> VecVector3d;

void computeLandmarks(Mat img1, Mat img2, Mat depthImg1, Mat depthImg2, VecVector3d& landmarks1, VecVector3d& landmarks2);
void computePose(VecVector3d landmarks1, VecVector3d landmarks2, SE3d& pose);

int main()
{
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    Mat img1 = imread(img1_path, IMREAD_COLOR);  //IMREAD_COLOR返回彩色图
    Mat img2 = imread(img2_path, IMREAD_COLOR);  //IMREAD_COLOR返回彩色图
    Mat depthImg1 = imread(depth1_path, IMREAD_ANYDEPTH);  //IMREAD_ANYDEPTH返回原图
    Mat depthImg2 = imread(depth2_path, IMREAD_ANYDEPTH);  //IMREAD_ANYDEPTH返回原图

    VecVector3d landmarks1, landmarks2;
    computeLandmarks(img1, img2, depthImg1, depthImg2, landmarks1, landmarks2);

    SE3d pose;
    computePose(landmarks1, landmarks2, pose);
    cout << "图像1到图像2的变换矩阵pose为：\n" << pose.matrix() << endl;

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> timeUsed = chrono::duration_cast<chrono::duration<double>> (t2 - t1);
    cout << "执行整个程序花费的时间为：" << timeUsed.count() << "秒！" << endl;
    return 0;
}


void computeLandmarks(Mat img1, Mat img2, Mat depthImg1, Mat depthImg2, VecVector3d& landmarks1, VecVector3d& landmarks2)
{
    Ptr<ORB> ptrORB = ORB::create(1000, 1.2, 8);
    vector<KeyPoint> keypoints1, keypoints2;
    ptrORB->detect(img1, keypoints1);
    ptrORB->detect(img2, keypoints2);

    Mat desc1, desc2;
    ptrORB->compute(img1, keypoints1, desc1);
    ptrORB->compute(img2, keypoints2, desc2);

    vector<DMatch> rawMatches, matches;
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    matcher->match(desc1, desc2, rawMatches);
    //剔除误匹配点对
    float minDist = 196.0;
    for(auto m : rawMatches) minDist = min(minDist, m.distance);
    float threshold = max(30.0f, 2 * minDist);
    for(auto m : rawMatches)
        if(m.distance <= threshold)
            matches.push_back(m);
    cout << "特征匹配点对数目为：" << matches.size() << endl;

    for(auto m : matches)
    {
        Vector2d pixel1(keypoints1[m.queryIdx].pt.x, keypoints1[m.queryIdx].pt.y);
        Vector2d pixel2(keypoints2[m.trainIdx].pt.x, keypoints2[m.trainIdx].pt.y);
        ushort val1, val2;
        val1 = depthImg1.at<ushort>(pixel1[1], pixel1[0]);
        val2 = depthImg2.at<ushort>(pixel2[1], pixel2[0]);

        if(val1 == 0 || val2 == 0) continue;

        double Z1, Z2;
        Z1 = val1 / 5000.0; Z2 = val2 / 5000.0;
        double X1, Y1, X2, Y2;
        X1 = (pixel1[0] - cx) / fx * Z1; Y1 = (pixel1[1] - cy) / fy * Z1;
        X2 = (pixel2[0] - cx) / fx * Z2; Y2 = (pixel2[1] - cy) / fy * Z2;

        landmarks1.push_back(Vector3d(X1, Y1, Z1));
        landmarks2.push_back(Vector3d(X2, Y2, Z2));

    }
    cout << "3D-3D匹配点对数目为：" << landmarks1.size() << endl;


}


void computePose(VecVector3d landmarks1, VecVector3d landmarks2, SE3d& pose)
{
    Vector3d avge1 = Vector3d::Zero();
    Vector3d avge2 = Vector3d::Zero();
    for(int i = 0; i < landmarks1.size(); i++)
    {
        avge1 += landmarks1[i];
        avge2 += landmarks2[i];
    }
    avge1 /= landmarks1.size();
    avge2 /= landmarks2.size();

    assert(landmarks1.size() == landmarks2.size());  //断言函数，条件为假，程序终止执行！

    //计算去质心坐标
    VecVector3d points1, points2;
    for(int i = 0; i < landmarks1.size(); i++)
    {
        Vector3d tmp;
        tmp = landmarks1[i] - avge1;
        points1.push_back(tmp);
        tmp = landmarks2[i] - avge2;
        points2.push_back(tmp);
    }

    //计算3*3维的W矩阵
    Matrix3d W = Matrix3d::Zero();
    for(int i = 0; i < points1.size(); i++)
        W += points2[i] * points1[i].transpose();

    //对W矩阵进行奇异值分解
    JacobiSVD<Matrix3d> svd(W, ComputeFullU | ComputeFullV);
    Matrix3d U = svd.matrixU();
    Matrix3d V = svd.matrixV();

    //计算最优的旋转矩阵和平移向量
    Matrix3d R = U * V.transpose();
    if(R.determinant() < 0) R = -R;
    Vector3d t = avge2 - R * avge1;

    pose = SE3d(R, t);

}

  CMakeLists.txt文件内容为，

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

include_directories("/usr/include/eigen3")

find_package(Sophus REQUIRED)
include_directories(${Sophus_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES})

  程序运行结果为，

特征匹配点对数目为：180
3D-3D匹配点对数目为：167
图像1到图像2的变换矩阵pose为：
   0.996022  -0.0492558   0.0742626   -0.189286
  0.0501182    0.998695 -0.00979202   0.0581007
 -0.0736834    0.013475    0.997191   0.0272408
          0           0           0           1
执行整个程序花费的时间为：0.0391535秒！

6.2 手写高斯牛顿优化

6.2.1 理论推导

  将图像1中的路标点集记作 P 1 = { p 1 1 , p 1 2 , . . . , p 1 n } P_1=\{p_1^1,p_1^2,...,p_1^n\} P1​={p11​,p12​,...,p1n​}，将图像2中的路标点集记作 P 2 = { p 2 1 , p 2 2 , . . . , p 2 n } P_2=\{p_2^1,p_2^2,...,p_2^n\} P2​={p21​,p22​,...,p2n​}。那么该优化问题可表述为，
$$T^{\ast }=\underset{T}{\mathrm{argmin}}\sum _{i=1}^n\Vert p_2^i-T{p}_1^i{\Vert }_2^2\text{\hspace{1em}(1)}$$
  增量方程记为，
$$H\ast dx=b\text{\hspace{1em}(2)}$$
现求解海塞矩阵$H$和矩阵$b$及代价函数$f$如下，
  已知$\frac{\partial (T{P}_1)}{\partial T}$可计算如下，
$$\frac{\partial (T{P}_1)}{\partial T}=\left[\begin{array}{cc}{I}_{3\times 3}& -P_2\times \end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& Z_2& -Y_2\\ 0& 1& 0& -Z_2& 0& X_2\\ 0& 0& 1& Y_2& -X_2& 0\end{array}\right]\text{\hspace{1em}(3)}$$
其中$P_2=T{P}_1=(X_2,Y_2,Z_2{)}^T$。
  将$T{p}_1^i$记作$q_1^i=(a_i,b_i,c_i{)}^T$，那么雅可比矩阵$J_i$可计算如下，
$$J_i=-\frac{\partial (T{p}_1^i)}{\partial T}=-\left[\begin{array}{cccccc}1& 0& 0& 0& c_i& -b_i\\ 0& 1& 0& -c_i& 0& a_i\\ 0& 0& 1& b_i& -a_i& 0\end{array}\right]\text{\hspace{1em}(4)}$$
故海塞矩阵$H$为，
$$H=\sum _{i=1}^n{J}_i^T{J}_i\text{\hspace{1em}(5)}$$
  将$p_2^i-T{p}_1^i$记作$e_i$，那么代价函数$f$可计算为，
$$f=\sum _{i=1}^n{e}_i^T{e}_i\text{\hspace{1em}(6)}$$
同时，矩阵$b$可计算为，
$$b=\sum _{i=1}^n-J_i^T{e}_i\text{\hspace{1em}(7)}$$
故增量方程得李代数增量$dx$，更新优化变量，完成一次迭代。

6.2.2 代码实践

  cpp文件内容为，

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>  //Eigen核心模块
#include <Eigen/Dense>  //Eigen稠密矩阵模块
#include <sophus/se3.hpp>

using namespace std;
using namespace cv;
using namespace Eigen;
using namespace Sophus;

//图片路径
string img1_path = "../1.png";
string img2_path = "../2.png";
string depth1_path = "../1_depth.png";
string depth2_path = "../2_depth.png";

//相机内参
double fx = 520.9, cx = 325.1, fy = 521.0, cy = 249.7;

typedef vector<Vector3d, aligned_allocator<Vector3d>> VecVector3d;

void computeLandmarks(Mat img1, Mat img2, Mat depthImg1, Mat depthImg2, VecVector3d& landmarks1, VecVector3d& landmarks2);
void GaussNewton(VecVector3d landmarks1, VecVector3d landmarks2, SE3d& pose);

int main()
{
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    Mat img1 = imread(img1_path, IMREAD_COLOR);  //IMREAD_COLOR返回彩色图
    Mat img2 = imread(img2_path, IMREAD_COLOR);  //IMREAD_COLOR返回彩色图
    Mat depthImg1 = imread(depth1_path, IMREAD_ANYDEPTH);  //IMREAD_ANYDEPTH返回原图
    Mat depthImg2 = imread(depth2_path, IMREAD_ANYDEPTH);  //IMREAD_ANYDEPTH返回原图

    VecVector3d landmarks1, landmarks2;
    computeLandmarks(img1, img2, depthImg1, depthImg2, landmarks1, landmarks2);

    SE3d pose;
    GaussNewton(landmarks1, landmarks2, pose);
    cout << "图像1到图像2的变换矩阵pose为：\n" << pose.matrix() << endl;

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> timeUsed = chrono::duration_cast<chrono::duration<double>> (t2 - t1);
    cout << "执行整个程序花费的时间为：" << timeUsed.count() << "秒！" << endl;
    return 0;
}


void computeLandmarks(Mat img1, Mat img2, Mat depthImg1, Mat depthImg2, VecVector3d& landmarks1, VecVector3d& landmarks2)
{
    Ptr<ORB> ptrORB = ORB::create(1000, 1.2, 8);
    vector<KeyPoint> keypoints1, keypoints2;
    ptrORB->detect(img1, keypoints1);
    ptrORB->detect(img2, keypoints2);

    Mat desc1, desc2;
    ptrORB->compute(img1, keypoints1, desc1);
    ptrORB->compute(img2, keypoints2, desc2);

    vector<DMatch> rawMatches, matches;
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    matcher->match(desc1, desc2, rawMatches);
    //剔除误匹配点对
    float minDist = 196.0;
    for(auto m : rawMatches) minDist = min(minDist, m.distance);
    float threshold = max(30.0f, 2 * minDist);
    for(auto m : rawMatches)
        if(m.distance <= threshold)
            matches.push_back(m);
    cout << "特征匹配点对数目为：" << matches.size() << endl;

    for(auto m : matches)
    {
        Vector2d pixel1(keypoints1[m.queryIdx].pt.x, keypoints1[m.queryIdx].pt.y);
        Vector2d pixel2(keypoints2[m.trainIdx].pt.x, keypoints2[m.trainIdx].pt.y);
        ushort val1, val2;
        val1 = depthImg1.at<ushort>(pixel1[1], pixel1[0]);
        val2 = depthImg2.at<ushort>(pixel2[1], pixel2[0]);

        if(val1 == 0 || val2 == 0) continue;

        double Z1, Z2;
        Z1 = val1 / 5000.0; Z2 = val2 / 5000.0;
        double X1, Y1, X2, Y2;
        X1 = (pixel1[0] - cx) / fx * Z1; Y1 = (pixel1[1] - cy) / fy * Z1;
        X2 = (pixel2[0] - cx) / fx * Z2; Y2 = (pixel2[1] - cy) / fy * Z2;

        landmarks1.push_back(Vector3d(X1, Y1, Z1));
        landmarks2.push_back(Vector3d(X2, Y2, Z2));

    }
    cout << "3D-3D匹配点对数目为：" << landmarks1.size() << endl;


}


void GaussNewton(VecVector3d landmarks1, VecVector3d landmarks2, SE3d& pose)
{
    int maxIteration = 10;
    double currentCost = 0, lastCost = 0;

    for(int iter = 0; iter < maxIteration; iter++)
    {
        Matrix6d H = Matrix6d::Zero();
        Vector6d b = Vector6d::Zero();

        currentCost = 0;
        for(int i = 0; i < landmarks1.size(); i++)
        {
            Vector3d landmark1 = landmarks1[i];
            Vector3d landmark2 = landmarks2[i];
            Vector3d point2 = pose * landmark1;

            //计算误差ei
            Vector3d ei = landmark2 - point2;

            //计算雅可比矩阵Ji
            Eigen::Matrix<double, 3, 6> Ji;
            Ji << 1, 0, 0, 0, point2[2], -point2[1],
                  0, 1, 0, -point2[2], 0, point2[0],
                  0, 0, 1, point2[1], -point2[0], 0;
            Ji = -1 * Ji;

            //计算H和b
            H += Ji.transpose() * Ji;
            b += -Ji.transpose() * ei;

            //计算代价
            currentCost += ei.transpose() * ei;

        }

        //解增量方程H*dx=b
        Vector6d dx = H.ldlt().solve(b);
        //判断是否可解
        if(isnan(dx[0]))
        {
            cout << "增量方程不可解，退出迭代！" << endl;
            break;
        }


        //输出迭代信息
        cout << "当前iter等于" << iter << ",此时的代价函数currentCost为" << currentCost << "，解出的增量dx为" << dx.transpose() << "！" << endl;

        //判断代价是否不再减小
        if(iter > 0 && currentCost >= lastCost)
        {
            cout << "代价不再减小，退出迭代！" << endl;
            break;
        }

        //更新lastCost
        lastCost = currentCost;

        //更新优化变量pose
        pose = SE3d::exp(dx) * pose;

        if(dx.norm() < 1e-6)
        {
            cout << "增量的模小于1e-6，退出迭代！" << endl;
            break;
        }


    }

}

  CMakeLists.txt文件内容为，

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

include_directories("/usr/include/eigen3")

find_package(Sophus REQUIRED)
include_directories(${Sophus_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES})

  程序运行结果为，

特征匹配点对数目为：180
3D-3D匹配点对数目为：167
当前iter等于0,此时的代价函数currentCost为13.1084，解出的增量dx为-0.159106 0.0613907   0.02309 0.0118023  0.055623 0.0492358！
当前iter等于1,此时的代价函数currentCost为11.507，解出的增量dx为  -0.0235478    0.0013222  -0.00345371 -0.000329825    0.0145258  0.000707452！
当前iter等于2,此时的代价函数currentCost为11.4877，解出的增量dx为  -0.0050446 -0.000321008 -0.000287074 -0.000193625   0.00309084 -4.53903e-05！
当前iter等于3,此时的代价函数currentCost为11.487，解出的增量dx为 -0.00106001  -8.5328e-05 -6.07003e-05 -5.12311e-05  0.000647479 -1.55232e-05！
当前iter等于4,此时的代价函数currentCost为11.4869，解出的增量dx为-0.000221601 -2.13569e-05  -1.2721e-05 -1.28481e-05   0.00013536 -3.43765e-06！
当前iter等于5,此时的代价函数currentCost为11.4869，解出的增量dx为-4.63247e-05 -5.40239e-06 -2.66626e-06 -3.25876e-06  2.82964e-05 -7.15454e-07！
当前iter等于6,此时的代价函数currentCost为11.4869，解出的增量dx为-9.68791e-06 -1.39449e-06 -5.59566e-07  -8.4334e-07  5.91763e-06 -1.46306e-07！
当前iter等于7,此时的代价函数currentCost为11.4869，解出的增量dx为-2.02734e-06 -3.67151e-07 -1.17658e-07 -2.22549e-07  1.23835e-06 -2.95648e-08！
当前iter等于8,此时的代价函数currentCost为11.4869，解出的增量dx为-4.24633e-07 -9.83574e-08 -2.48035e-08 -5.97344e-08  2.59373e-07 -5.88783e-09！
增量的模小于1e-6，退出迭代！
图像1到图像2的变换矩阵pose为：
   0.996022  -0.0492559   0.0742625   -0.189286
  0.0501182    0.998695 -0.00979204   0.0581007
 -0.0736833    0.013475    0.997191   0.0272408
          0           0           0           1
执行整个程序花费的时间为：0.108949秒！

6.3 使用g2o进行优化

  cpp文件内容为，

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>  //Eigen核心模块
#include <Eigen/Dense>  //Eigen稠密矩阵模块
#include <sophus/se3.hpp>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>

using namespace std;
using namespace cv;
using namespace Eigen;
using namespace Sophus;
using namespace g2o;

//图片路径
string img1_path = "../1.png";
string img2_path = "../2.png";
string depth1_path = "../1_depth.png";
string depth2_path = "../2_depth.png";

//相机内参
double fx = 520.9, cx = 325.1, fy = 521.0, cy = 249.7;

typedef vector<Vector3d, aligned_allocator<Vector3d>> VecVector3d;

void computeLandmarks(Mat img1, Mat img2, Mat depthImg1, Mat depthImg2, VecVector3d& landmarks1, VecVector3d& landmarks2);
void bundleAdjustmentG2O(VecVector3d landmarks1, VecVector3d landmarks2, SE3d& pose);


//写顶点，顶点就是优化变量
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>  //6和Sophus::SE3d分别表示顶点中变量维数和变量类型
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;  //解决Eigen库数据结构的存储对齐问题

    virtual void setToOriginImpl() override  //重写基类中将估计置零的虚函数setToOriginImpl()
    {
        _estimate = SE3d();
    }

    virtual void oplusImpl(const double* update) override  //重写基类中更新估计的虚函数oplusImpl()，update是一个double类型数组
    {
        Eigen::Matrix<double, 6, 1> update_eigen;
        update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate = SE3d::exp(update_eigen) * _estimate;
    }

    virtual bool read(istream& in) override {}  //istream表示C++中的一个标准输入流的基类
    virtual bool write(ostream& out) const override {}  //ostream表示C++中的一个标准输出流的基类
};


//写边，边就是误差项
class EdgeProjection : public g2o::BaseUnaryEdge<3, Vector3d, VertexPose>  //3、Vector3d和VertexPose分别表示边中变量维数、变量类型和所连接顶点的类型
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;  //解决Eigen库数据结构的存储对齐问题

    EdgeProjection(const Vector3d& point) : _point(point) {}  //使用列表给成员变量初始化的构造函数

    virtual void computeError() override  //重写基类中计算误差的虚函数computeError()
    {
        const VertexPose* pose = static_cast<const VertexPose*> (_vertices[0]);
        _error = _measurement - pose->estimate() * _point;  //_point就是landmark1，而_measurement就是landmark2
    }

    virtual void linearizeOplus() override  //重写基类中计算雅可比矩阵的虚函数linearizeOplus()
    {
        VertexPose* pose = static_cast<VertexPose*> (_vertices[0]);
        SE3d T = pose->estimate();
        Vector3d xyz_trans = T * _point;
        _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
        _jacobianOplusXi.block<3, 3>(0, 3) = SO3d::hat(xyz_trans);
    }

    bool read(istream& in) {}  //istream表示C++中的一个标准输入流的基类
    bool write(ostream& out) const {}  //ostream表示C++中的一个标准输出流的基类

protected:
    Vector3d _point;  //_point就是landmark1
};



int main()
{
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    Mat img1 = imread(img1_path, IMREAD_COLOR);  //IMREAD_COLOR返回彩色图
    Mat img2 = imread(img2_path, IMREAD_COLOR);  //IMREAD_COLOR返回彩色图
    Mat depthImg1 = imread(depth1_path, IMREAD_ANYDEPTH);  //IMREAD_ANYDEPTH返回原图
    Mat depthImg2 = imread(depth2_path, IMREAD_ANYDEPTH);  //IMREAD_ANYDEPTH返回原图

    VecVector3d landmarks1, landmarks2;
    computeLandmarks(img1, img2, depthImg1, depthImg2, landmarks1, landmarks2);

    SE3d pose;
    bundleAdjustmentG2O(landmarks1, landmarks2, pose);
    cout << "图像1到图像2的变换矩阵pose为：\n" << pose.matrix() << endl;

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> timeUsed = chrono::duration_cast<chrono::duration<double>> (t2 - t1);
    cout << "执行整个程序花费的时间为：" << timeUsed.count() << "秒！" << endl;
    return 0;
}


void computeLandmarks(Mat img1, Mat img2, Mat depthImg1, Mat depthImg2, VecVector3d& landmarks1, VecVector3d& landmarks2)
{
    Ptr<ORB> ptrORB = ORB::create(1000, 1.2, 8);
    vector<KeyPoint> keypoints1, keypoints2;
    ptrORB->detect(img1, keypoints1);
    ptrORB->detect(img2, keypoints2);

    Mat desc1, desc2;
    ptrORB->compute(img1, keypoints1, desc1);
    ptrORB->compute(img2, keypoints2, desc2);

    vector<DMatch> rawMatches, matches;
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    matcher->match(desc1, desc2, rawMatches);
    //剔除误匹配点对
    float minDist = 196.0;
    for(auto m : rawMatches) minDist = min(minDist, m.distance);
    float threshold = max(30.0f, 2 * minDist);
    for(auto m : rawMatches)
        if(m.distance <= threshold)
            matches.push_back(m);
    cout << "特征匹配点对数目为：" << matches.size() << endl;

    for(auto m : matches)
    {
        Vector2d pixel1(keypoints1[m.queryIdx].pt.x, keypoints1[m.queryIdx].pt.y);
        Vector2d pixel2(keypoints2[m.trainIdx].pt.x, keypoints2[m.trainIdx].pt.y);
        ushort val1, val2;
        val1 = depthImg1.at<ushort>(pixel1[1], pixel1[0]);
        val2 = depthImg2.at<ushort>(pixel2[1], pixel2[0]);

        if(val1 == 0 || val2 == 0) continue;

        double Z1, Z2;
        Z1 = val1 / 5000.0; Z2 = val2 / 5000.0;
        double X1, Y1, X2, Y2;
        X1 = (pixel1[0] - cx) / fx * Z1; Y1 = (pixel1[1] - cy) / fy * Z1;
        X2 = (pixel2[0] - cx) / fx * Z2; Y2 = (pixel2[1] - cy) / fy * Z2;

        landmarks1.push_back(Vector3d(X1, Y1, Z1));
        landmarks2.push_back(Vector3d(X2, Y2, Z2));

    }
    cout << "3D-3D匹配点对数目为：" << landmarks1.size() << endl;


}


void bundleAdjustmentG2O(VecVector3d landmarks1, VecVector3d landmarks2, SE3d& pose)
{
    //构建图优化，先设定g2o
    typedef g2o::BlockSolverX BlockSolverType;  //在这里本来要说明顶点的维数和边的维数
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;  //线性求解器类型

    //梯度下降方法，可以从GN，LM和DogLeg中选
    auto solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;  //定义稀疏图模型变量optimizer
    optimizer.setAlgorithm(solver);  //为optimizer设置求解器
    optimizer.setVerbose(true);  //向屏幕中输出优化过程信息

    //添加顶点
    VertexPose* pose1 = new VertexPose();  //定义顶点变量pose1
    pose1->setId(0);
    pose1->setEstimate(SE3d());
    optimizer.addVertex(pose1);  //往优化器中添加顶点

    //添加边
    for(size_t i = 0; i < landmarks1.size(); i++)
    {
        EdgeProjection* edge = new EdgeProjection(landmarks1[i]);  //输入的是landmarks1[i]
        edge->setVertex(0, pose1);  //设置边连接到的顶点
        edge->setMeasurement(landmarks2[i]);  //_measurement就是landmark2
        edge->setInformation(Eigen::Matrix3d::Identity());  //设置测量噪声的信息矩阵，即协方差矩阵的逆
        optimizer.addEdge(edge);  //往优化器中添加边
    }

    optimizer.initializeOptimization();  //优化器初始化
    optimizer.optimize(10);  //设置优化器迭代次数，并执行优化

    pose = SE3d(pose1->estimate().rotationMatrix(),pose1->estimate().translation());  //保存优化结果
}

  CMakeLists.txt文件内容为，

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

include_directories("/usr/include/eigen3")

find_package(g2o REQUIRED)
include_directories(${G2O_DIRECTORIES})

find_package(Sophus REQUIRED)
include_directories(${Sophus_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES} g2o_core g2o_stuff)

  程序运行结果为，

特征匹配点对数目为：180
3D-3D匹配点对数目为：167
iteration= 0	 chi2= 11.507010	 time= 0.0064815	 cumTime= 0.0064815	 edges= 167	 schur= 0	 lambda= 0.001678	 levenbergIter= 1
iteration= 1	 chi2= 11.487739	 time= 0.00647884	 cumTime= 0.0129603	 edges= 167	 schur= 0	 lambda= 0.000559	 levenbergIter= 1
iteration= 2	 chi2= 11.486956	 time= 0.00650553	 cumTime= 0.0194659	 edges= 167	 schur= 0	 lambda= 0.000373	 levenbergIter= 1
iteration= 3	 chi2= 11.486922	 time= 0.0063916	 cumTime= 0.0258575	 edges= 167	 schur= 0	 lambda= 0.000249	 levenbergIter= 1
iteration= 4	 chi2= 11.486920	 time= 0.00647745	 cumTime= 0.0323349	 edges= 167	 schur= 0	 lambda= 0.000166	 levenbergIter= 1
iteration= 5	 chi2= 11.486920	 time= 0.00635465	 cumTime= 0.0386896	 edges= 167	 schur= 0	 lambda= 0.000110	 levenbergIter= 1
iteration= 6	 chi2= 11.486920	 time= 0.0063258	 cumTime= 0.0450154	 edges= 167	 schur= 0	 lambda= 0.000074	 levenbergIter= 1
iteration= 7	 chi2= 11.486920	 time= 0.00638853	 cumTime= 0.0514039	 edges= 167	 schur= 0	 lambda= 0.000049	 levenbergIter= 1
iteration= 8	 chi2= 11.486920	 time= 0.00638399	 cumTime= 0.0577879	 edges= 167	 schur= 0	 lambda= 0.000033	 levenbergIter= 1
iteration= 9	 chi2= 11.486920	 time= 0.00640026	 cumTime= 0.0641881	 edges= 167	 schur= 0	 lambda= 0.000022	 levenbergIter= 1
图像1到图像2的变换矩阵pose为：
   0.996022  -0.0492558   0.0742626   -0.189286
  0.0501182    0.998695 -0.00979202   0.0581007
 -0.0736834    0.013475    0.997191   0.0272408
          0           0           0           1
执行整个程序花费的时间为：0.118518秒！