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

多层直接法的过程：

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 = 0;
};

//双线性插值
inline float GetPixelValue(const cv::Mat &img, float x, float y) 
{
    //边界检测，随机点不会取在边上，因此这里右边界和下边界是可以的
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols - 1) x = img.cols - 1;
    if (y >= img.rows - 1) y = img.rows - 1;
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1]
    );
}

//计算增量方程的参数
void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) 
{
    const int half_patch_size = 1;
    int cnt_good = 0;
    Matrix6d hessian = Matrix6d::Zero();
    Vector6d bias = Vector6d::Zero();
    double cost_tmp = 0;

    for (size_t i = range.start; i < range.end; i++) 
	{
		//估计运动后点的空间坐标
		//1、将像素坐标转换为归一化坐标
		//2、归一化坐标乘以深度，得到空间坐标
		//3、空间坐标左乘运动
        Eigen::Vector3d point_ref =
            depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
        Eigen::Vector3d point_cur = T21 * point_ref;
        if (point_cur[2] < 0)
            continue;

		//计算运动后点的像素坐标
		//根据下面for循环以及GetPixelValue，这里需要修改if的条件，否则会出现段错误
        float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;
        if (u < half_patch_size || 
			u >= img2.cols - 1 - half_patch_size - 1 || 
			v <= half_patch_size ||
            v >= img2.rows - 1 - half_patch_size - 1)
            continue;

        projection[i] = Eigen::Vector2d(u, v);
		
        cnt_good++;
		
        double X = point_cur[0], Y = point_cur[1], Z = point_cur[2],
            Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;
        //计算误差、目标函数、雅可比、H、b
		//3x3窗口
        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_pixel_xi(0, 2) = -fx * X * Z2_inv;
                J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
                J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
                J_pixel_xi(0, 5) = -fx * Y * Z_inv;

                J_pixel_xi(1, 0) = 0;
                J_pixel_xi(1, 1) = fy * Z_inv;
                J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
                J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
                J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
                J_pixel_xi(1, 5) = fy * X * Z_inv;
				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))
                );
                
                Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();
                hessian += J * J.transpose();
                bias += -error * J;
                cost_tmp += error * error;
            }
    }

	//更新H、b、cost
    if (cnt_good) 
	{
		//因为使用并行计算，为了防止抢占变量，需要先获得锁
        unique_lock<mutex> lck(hessian_mutex);	
        H += hessian;
        b += bias;
		//cost只是一个判断收敛的依据，可以进行缩放
        cost += cost_tmp / cnt_good;
    }
}

//单层直接法
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21) 
{

    const int iterations = 10;
    double cost = 0, lastCost = 0;
    auto t1 = chrono::steady_clock::now();
    JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);

    for (int iter = 0; iter < iterations; iter++) 
	{
		//初始化H、b、cost
        jaco_accu.reset();
		
		//并行计算H、b等参数
        cv::parallel_for_(cv::Range(0, px_ref.size()),
                          std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
//		jaco_accu.accumulate_jacobian(cv::Range(0, px_ref.size()));
        Matrix6d H = jaco_accu.hessian();
        Vector6d b = jaco_accu.bias();
        cost = jaco_accu.cost_func();

        //更新
        Vector6d update = H.ldlt().solve(b);;
        T21 = Sophus::SE3d::exp(update) * T21;

        if (std::isnan(update[0])) 
		{
            cout << "update is nan" << endl;
            break;
        }
        if (iter > 0 && cost > lastCost) 
		{
            cout << "cost increased: " << cost << ", " << lastCost << endl;
            break;
        }
        if (update.norm() < 1e-3) 
            break;

        lastCost = cost;
        cout << "iteration: " << iter << ", cost: " << cost << endl;
    }

    cout << "T21 = \n" << T21.matrix() << endl;
    auto t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "direct method for single layer: " << time_used.count() << endl;

	
    //画出跟踪结果
    cv::Mat img2_show;
    cv::cvtColor(img2, img2_show, COLOR_GRAY2BGR);
    VecVector2d projection = jaco_accu.projected_points();
    for (size_t i = 0; i < px_ref.size(); ++i) 
	{
        auto p_ref = px_ref[i];
        auto p_cur = projection[i];
        if (p_cur[0] > 0 && p_cur[1] > 0) 
		{
            cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                     cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("current", img2_show);
    cv::waitKey();
}

//多层直接法
void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21) 
{
	//金字塔参数
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    //建立金字塔
    vector<cv::Mat> pyr1, pyr2; 
    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; 
	
	//多层直接法
    for (int level = pyramids - 1; level >= 0; level--) 
	{
		//计算提取点在当前图层的位置
        VecVector2d px_ref_pyr; 
        for (auto &px: px_ref)
            px_ref_pyr.push_back(scales[level] * px);

        //缩放相机内参
        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);
    }

}

int main(int argc, char **argv) 
{
	//读取灰度图
    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    cv::RNG rng;
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    //随机取点（为了增加可用点，不取边界点）
    for (int i = 0; i < nPoints; i++) 
	{
        int x = rng.uniform(boarder, left_img.cols - boarder);  
        int y = rng.uniform(boarder, left_img.rows - boarder);  
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity;	//根据视差求深度
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    //待估计运动
    Sophus::SE3d T_cur_ref;

    for (int i = 1; i < 6; i++) 
	{  
		//以读取灰度图
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
    }
    return 0;
}