SLAM十四讲ch8 direct_method.cpp 代码详解

#include 
#include 
#include 

using namespace std;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// 相机内参
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");    // 连续图

// typedef
typedef Eigen::Matrix<double, 6, 6> Matrix6d; // 6*6的矩阵
typedef Eigen::Matrix<double, 2, 6> Matrix26d; // 2*6的矩阵
typedef Eigen::Matrix<double, 6, 1> Vector6d; // 6*1的矩阵

// 计算雅可比
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; } // 获取海塞矩阵H的函数

    Vector6d bias() const { return b; } // 获取偏执b的函数

    double cost_func() const { return cost; } // 获取代价cost的函数

    VecVector2d projected_points() const { return projection; } // 获取像素在第二个图像中的坐标的函数

    // 重置 h, b, cost 为0
    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; // 变换矩阵T
    VecVector2d projection; // 第一个图像的像素在第二个图像中的坐标

    std::mutex hessian_mutex;
    Matrix6d H = Matrix6d::Zero(); // 海塞矩阵H
    Vector6d b = Vector6d::Zero(); // 偏置b
    double cost = 0; // 代价
};

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

// 计算雅可比的函数，传入的参数是Range(0,px_ref.size())，px_ref.size()就是像素点的个数
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++) 
    {
        // 计算像素在第二个图象中的坐标
        // point_ref是第一个图像中的像素（假设坐标为[u1，v1]）在第一个相机坐标系下的空间坐标，假设为[X1,Y1,Z1]，
        // 计算公式为[X1,Y1,Z1]=Z1*[(u1-cx)/fx, (v1-cy)/fy, 1]
        Eigen::Vector3d point_ref = depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
        // point_cur是该像素在第二个相机坐标系下的空间坐标，假设为[X2,Y2,Z2]，
        // 等于第一个相机位姿到第二个相机位姿的变换矩阵乘以第一个空间坐标系下的坐标。即[X2,Y2,Z2] = T21 * [X1,Y1,Z1]
        Eigen::Vector3d point_cur = T21 * point_ref;
        
        if (point_cur[2] < 0)   
        // point_cur[2]是像素在第二个相机坐标系下的位深，如果小于0说明计算出来的坐标应该是错误的，认为该坐标无效
            continue;
            
		// [u,v]是计算出来的由第一个图像中的像素[u1,v1]在第二个图像中的像素坐标，
		// 计算公式为[u,v] = [fx*X/Z + cx, fy*Y/Z + cy]
        float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;
        // 如果计算出来的u,v在图像的边界或边界以外，认为该像素坐标无效，继续计算下一个像素
        if (u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size || v > img2.rows - half_patch_size)
            continue;
		// 如果u,v符合条件，就将该坐标存入projection中
        projection[i] = Eigen::Vector2d(u, v);
		
        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;
        cnt_good++; // 有效角点数+1

        // 计算雅可比J和误差函数
        for (int x = -half_patch_size; x <= half_patch_size; x++)
            for (int y = -half_patch_size; y <= half_patch_size; y++) 
            {
				// (x,y)是像素周围的9个点，
				// 误差函数为 e =  GetPixelValue(img1, u1 + x, v1 + y) - GetPixelValue(img2, u + x, v + y);
                double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) - GetPixelValue(img2, u + x, v + y);
                // 先计算J_pix_xi，是像素坐标对变换的导数，是一个(2*6)的矩阵
				// J_pix_xi = [fx/Z， 0 ， -fx*X/Z^2, -fx*X*Y/Z^2     , fx + fx*X^2/Z^2, -fx*Y/Z]
				//            [0  , fy/Z, -fy*Y/Z^2, -fy - fy*Y^2/Z^2, fy*X*Y/Z^2    ,   fy*X/Z ]
                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是像素在第二个图像中的梯度（水平梯度，纵向梯度），
				// 是一个2*1的矩阵，计算时转换成1*2的矩阵
                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))
                );

                // 总的雅可比J，是一个(1,2)*(2,6) = (1,6)的矩阵，转置成(6,1)的矩阵
                Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();

                hessian += J * J.transpose(); // 计算当前像素点的海塞Hessian
                bias += -error * J; // 计算当前像素的偏置bias
                cost_tmp += error * error; // 计算当前像素的代价cost_tmp
            }
    }

    if (cnt_good)  // 如果存在有效的像素点，就将其Hessian, bias, cost_tmp累加到总的H, b, cost
    {
        unique_lock<mutex> lck(hessian_mutex);
        H += hessian;
        b += bias;
        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); //创建一个计算雅可比的对象jaco_accu

    for (int iter = 0; iter < iterations; iter++) 
    {
        jaco_accu.reset(); // 每次迭代开始前，都将H, b, cost 置 0
        cv::parallel_for_(cv::Range(0, px_ref.size()),std::bind(&JacobianAccumulator::accumulate_jacobian,
        				   &jaco_accu,std::placeholders::_1));
 		// px_ref.size()是像素点的数量。bind是一个绑定函数，它相当于调用jaco_accu.accumulate_jacobian()。
 		// placeholders::_1是占位符，表示传入第一个参数，此处cv::Range(0, px_ref.size()为传入的参数。
        Matrix6d H = jaco_accu.hessian();
        Vector6d b = jaco_accu.bias();

        // 求解更新量，求解出的update是一个6维向量，相当于变换矩阵的se3，
        // 通过指数映射将其转换为变换矩阵，并将其添加到之前的估计，直接相乘即可
        Vector6d update = H.ldlt().solve(b);
        T21 = Sophus::SE3d::exp(update) * T21;
        cost = jaco_accu.cost_func();
		
		// 迭代终止条件
        if (std::isnan(update[0])) 
        {	// 当出现黑色或白色像素并且H不可逆时，可能会出现nan值
            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, cv::COLOR_GRAY2BGR);
    VecVector2d projection = jaco_accu.projected_points(); // projection存储的是追踪到的像素点的坐标
    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--)
    {
        cout  << "pyr level = " << level << "-------------------------------------------" << endl;
        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); 
    }

}

// 同我的另一边博客，光流法，这里只是坐标确定的方法不同，这里用的是指针
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) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
    // data属性指向该图像的首地址，step属性等于该图像每一行的步长。
    // 所以这里的data指向浮点(x,y)的左上角的整数坐标(int(x),int(y))的地址。
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    // 同理，data[0],data[1],data[img.step],data[img.step + 1]分别指向该像素的左上，右上，左下，右下，
    // 也是应用双线性插值法。
    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]
    );
}

int main(int argc, char **argv) {

    cv::Mat left_img = cv::imread(left_file, 0); // 初始时刻的图
    cv::Mat disparity_img = cv::imread(disparity_file, 0); // 视差图

    // 随机选择第一张图像中的2000个像素并在第一张图像的帧中生成一些3d点
    cv::RNG rng;
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // 生成先前像素点的坐标信息，并且导入其位深
    for (int i = 0; i < nPoints; i++)
    {
    	// rng,uniform(a, b)表示在[a, b)之间均匀取值
        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);
        // 通过视差来求位深Z，公式为Z = fx*b/d，b是基线长度。d是视差
        double depth = fx * baseline / disparity;
        depth_ref.push_back(depth); // 将位深信息添加到depth_ref
        pixels_ref.push_back(Eigen::Vector2d(x, y)); //将像素坐标(x,y)添加到pixels_ref
    }

    // 用这些信息来估计位姿
    Sophus::SE3d T_cur_ref;

    for (int i = 1; i < 6; i++)
    {  // 1~10
        cout << i << "--------------------------------------------------------------------------------" << endl;
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref); // 单层直接法
        DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref); // 多层直接法
    }
    return 0;
}