视觉SLAM实践入门——（20）视觉里程计之直接法

最新推荐文章于 2025-09-07 15:53:20 发布

原创

最新推荐文章于 2025-09-07 15:53:20 发布 · 1.1k 阅读

3 ·

CC 4.0 BY-SA版权

文章标签：

#slam

本文介绍了一种多层直接法来估计相机的位姿，该方法通过图像金字塔进行处理，避免了特征点提取，随机选取像素点并计算深度。利用G-N、L-M等优化方法或g2o、ceres库进行优化，同时修复了可能导致段错误的代码。通过双线性插值计算像素值，并计算误差、雅可比矩阵和Hessian矩阵，最终实现相机位姿的估计。

多层直接法的过程：

1、读图，随机取点并计算深度

2、创建图像金字塔（相机内参也需要缩放），并计算对应点的像素坐标

3、应用单层直接法（使用G-N、L-M等方法，或者使用g2o、ceres库）进行优化

源码中有一些地方会引起段错误，修改方法见下面代码中注释

总的来说，直接法与特征点法的实现过程大同小异。不同的地方在于：特征点法中需要提取角点，直接法则可以随机取点（也可以取角点）并且取点数不能太少；直接法与特征点法计算误差不同（因此雅可比矩阵也不同）

#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <pangolin/pangolin.h>

using namespace std;
using namespace cv;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

//相机内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
//基线
double baseline = 0.573;
//图片路径
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png");    

//用于计算增量方程各参数的类
class JacobianAccumulator {
public:
    JacobianAccumulator(
        const cv::Mat &img1_,
        const cv::Mat &img2_,
        const VecVector2d &px_ref_,
        const vector<double> depth_ref_,
        Sophus::SE3d &T21_) :
        img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_) {
        projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));
    }

    //计算各参数
    void accumulate_jacobian(const cv::Range &range);

    Matrix6d hessian() const { return H; }

    Vector6d bias() const { return b; }

    double cost_func() const { return cost; }

    VecVector2d projected_points() const { return projection; }

    //初始化各参数
    void reset() 
    {
        H = Matrix6d::Zero();
        b = Vector6d::Zero();
        cost = 0;
    }

private:
    const cv::Mat &img1;
    const cv::Mat &img2;
    const VecVector2d &px_ref;
    const vector<double> depth_ref;
    Sophus::SE3d &T21;
    VecVector2d projection;

    std::mutex hessian_mutex;
    Matrix6d H = Matrix6d::Zero();
    Vector6d b = Vector6d::Zero();
    double cost