稀疏直接法

获取图像中指定位置像素值的函数，通过插值的方法获取（x，y）处的像素：

inline float GetPixelValue(const cv::Mat &img, float x, float y) {}
   

计算雅可比矩阵：

几个重要的参数：

const VecVector2d &px_ref_；//像素坐标 xy
const vector<double> depth_ref_；//深度信息
T21;//相机位姿
Eigen::Vector3d point_ref；//相机坐标系下坐标
Eigen::Vector3d point_cur；//世界坐标系下坐标
u，v;//经变换矩阵求得的第二张图片对应的像素坐标

有了这些我们就可以计算出雅可比矩阵了

for (int x = -half_patch_size; x <= half_patch_size; x++)
            for (int y = -half_patch_size; y <= half_patch_size; y++) {
                double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) -
                               GetPixelValue(img2, u + x, v + y);
                Matrix26d J_pixel_xi;
                Eigen::Vector2d J_img_pixel;

                J_pixel_xi(0, 0) = fx * Z_inv;
                J_pixel_xi(0, 1) = 0;
                //.........
                J_img_pixel = Eigen::Vector2d(
                    0.5 * (GetPixelValue(img2, u + 1 + x, v + y) - GetPixelValue(img2, u - 1 + x, v + y)),
                    0.5 * (GetPixelValue(img2, u + x, v + 1 + y) - GetPixelValue(img2, u + x, v - 1 + y))
                );//获得像素梯度

                // total jacobian
                Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();
                hessian += J * J.transpose();
                bias += -error * J;
                cost_tmp += error * error;
            }

这边是遍历了以（x，y）为中心的像素块，获得了误差，梯度方向，H矩阵，这边求像素梯度，只是简单的求了u+1和u-1的灰度之差除以2，作为其方向的梯度。

估计两个图像之间的位姿

使用高斯牛顿法来获得相机位姿，并将估计的像素位置绘画到第二幅图像上。

void DirectPoseEstimationSingleLayer(){}

多层直接法

把单层直接法拓展到金字塔式的多层直接法上

几个参数

int pyramids = 4;//金字塔层数
double pyramid_scale = 0.5;//金字塔缩放因子
double scales[] = {1.0, 0.5, 0.25, 0.125};//金字塔尺度

多层金字塔

通过循环迭代，逐层构建金字塔，每一层都是上一层图像的一半。

 vector<cv::Mat> pyr1, pyr2; // image pyramids
    for (int i = 0; i < pyramids; i++) {
        if (i == 0) {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        } else {
            cv::Mat img1_pyr, img2_pyr;
            cv::resize(pyr1[i - 1], img1_pyr,
                       cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
            cv::resize(pyr2[i - 1], img2_pyr,
                       cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
            pyr1.push_back(img1_pyr);
            pyr2.push_back(img2_pyr);
        }
    }

  

还要记得根据金字塔尺度调整相机内参

double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // backup the old values
    for (int level = pyramids - 1; level >= 0; level--) {
        VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
        for (auto &px: px_ref) {
            px_ref_pyr.push_back(scales[level] * px);
        }

        // scale fx, fy, cx, cy in different pyramid levels
        fx = fxG * scales[level];
        fy = fyG * scales[level];
        cx = cxG * scales[level];
        cy = cyG * scales[level];
        DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);

main

这边是稀疏直接法，我选取2000个像素点，分别把他们添加进去容器里面。调用多层直接法函数。