折腾笔记[14]-使用rust进行深度估计

摘要

使用rust实现基于REMODE数据集的单目相机在已知轨迹下的稠密深度估计.
Implement dense depth estimation for a monocular camera with a known trajectory based on the REMODE dataset using Rust.

关键词

rust;slam;REMODE;epipolar;depth evaludate;

关键信息

项目地址:[https://github.com/ByeIO/slambook2.rs]

[package]
name = "exp65-rust-ziglang-slambook2"
version = "0.1.0"
edition = "2021"

[dependencies]
env_logger = { version = "0.11.6", default-features = false, features = [
    "auto-color",
    "humantime",
] }

# 随机数
rand = "0.8.5"
rand_distr = "0.4.3"
fastrand = "2.3.0"

# 线性代数
nalgebra = { version = "0.33.2",features = ["rand"]}
ndarray = "0.16.1"

# winit
wgpu = "23.0.1"
winit = "0.30.8"

# egui
eframe = "0.30.0"
egui = { version = "0.30.0", features = [
    "default"
]}
egui_extras = {version = "0.30.0",features = ["default", "image"]}

# three_d
three-d = {path = "./static/three-d" , features=["egui-gui"] }
three-d-asset = {version = "0.9",features = ["hdr", "http"] }

# sophus
sophus = { version = "0.11.0" }
sophus_autodiff = { version = "0.11.0" }
sophus_geo = "0.11.0"
sophus_image = "0.11.0"
sophus_lie = "0.11.0"
sophus_opt = "0.11.0"
sophus_renderer = "0.11.0"
sophus_sensor = "0.11.0"
sophus_sim = "0.11.0"
sophus_spline = "0.11.0"
sophus_tensor = "0.11.0"
sophus_timeseries = "0.11.0"
sophus_viewer = "0.11.0"
tokio = "1.43.0"
approx = "0.5.1"
bytemuck = "1.21.0"
thingbuf = "0.1.6"

# rust-cv计算机视觉
cv = { version = "0.6.0" , features = ["default"] }
cv-core = "0.15.0"
cv-geom = "0.7.0"
cv-pinhole = "0.6.0"
akaze = "0.7.0"
eight-point = "0.8.0"
lambda-twist = "0.7.0"
image = "0.25.5"
imageproc = "0.25.0"

# 最小二乘优化
gomez = "0.5.0"

# 图优化
factrs = { version = "0.2.0", features = [] }
gs-rs = { version = "0.1.0", path = "./static/gs-rs" }

# ORB角点检测
bye_orb_rs = { version = "0.1.1", path = "./static/bye_orb_rs" }

# 词袋
bye_abow_rs = { version = "0.1.0", path = "./static/bye_abow_rs" }

# 依赖覆盖
[patch.crates-io]
pulp = { path = "./static/pulp" }
mio = { path = "./static/mio", version = "1.0.3" }

原理简介

REMODE数据集简介

[https://blog.csdn.net/qq_36122936/article/details/89002724]
[https://www.cnblogs.com/zonghaochen/p/8858585.html]
[http://rpg.ifi.uzh.ch/datasets/remode_test_data.zip]
REMODE数据集是一个用于单目相机实时三维重建研究的数据集，主要包含单目RGB图像及其对应的位姿信息。
REMODE数据集由苏黎世大学的研究人员在进行单目RGB相机实时三维重建研究时采集。数据集中的图像通常由无人机采集的单目俯视图像组成。
数据集包含一系列单目RGB图像，例如在REMODE测试数据集中，共有约200张图像。
每张图像都配有真实位姿信息，这些位姿信息通常以文本文件的形式记录。

稠密深度估计简介

稠密深度估计是计算机视觉领域中的一项重要技术，旨在为图像中的每个像素都估计出一个深度值，从而生成一幅完整的深度图，以获取场景的三维信息。以下是其简介：

定义与目标

传统的图像是二维的，仅包含颜色和纹理等信息，而稠密深度估计的目标就是在二维图像的基础上，为每个像素赋予一个深度值，使得计算机能够理解场景中物体的远近关系，恢复出场景的三维结构。深度图中的每个像素值代表了该像素所对应的场景点与相机之间的距离，距离值越大，表示该点离相机越远，反之则越近。

方法分类

基于立体视觉的方法：模拟人类双眼视觉原理，使用两个或多个相机从不同视角拍摄同一场景，通过计算不同图像之间的视差来估计深度。例如，通过对左右两幅图像中的对应点进行匹配，根据三角测量原理，利用相机的相对位置和姿态以及匹配点的视差来计算场景点的深度。
基于单目视觉的方法：仅使用单目相机拍摄的图像来估计深度。这种方法通常需要借助一些先验知识或假设，例如场景的几何结构、物体的运动信息、图像的纹理和明暗等线索。如利用图像序列中物体的运动信息，结合相机的运动模型和几何约束，通过对多帧图像的分析来估计深度。
基于多模态的方法：融合多种传感器的数据来进行深度估计，如将激光雷达（LiDAR）获取的深度信息与相机图像数据相结合。激光雷达可以直接测量场景中物体的距离，但数据稀疏且成本较高；相机图像则具有丰富的纹理信息，但缺乏深度信息。通过将两者的数据进行融合，可以充分利用各自的优势，提高深度估计的精度和可靠性。

挑战与难点

纹理缺失区域：在一些纹理较少或无纹理的区域，如平滑的墙面、大面积的天空等，缺乏足够的特征信息来进行准确的深度估计，容易导致深度值不准确或模糊。
遮挡问题：当场景中存在物体相互遮挡时，被遮挡部分的深度信息难以直接获取，需要通过一些额外的假设或推理来估计，这增加了深度估计的难度和不确定性。
光照变化：光照条件的变化会影响图像的灰度和颜色信息，从而影响基于图像特征的深度估计算法的准确性。例如，在强光或阴影下，图像的对比度和纹理特征可能会发生较大变化，导致匹配错误或深度估计偏差。
实时性要求：在一些实时应用场景中，如自动驾驶和机器人导航，需要在短时间内完成大量图像的深度估计，对算法的实时性和计算效率提出了很高的要求。

代码中涉及的数学原理:

1. 极线搜索与三角化

极线搜索：在参考图像和当前图像之间，通过极线约束寻找匹配点。极线是参考图像中的点在当前图像中的投影线，搜索范围由深度均值和方差决定。
三角化：通过两帧图像中的匹配点，利用相机的位姿变换，计算三维点的深度。公式为：

d_{ref} \cdot f_{ref} = d_{cur} \cdot (R_{RC} \cdot f_{cur}) + t_{RC}

其中， $f_{ref}$ 和 $f_{cur}$ 是归一化的像素方向向量， $R_{RC}$ 和 $t_{RC}$ 是相对位姿的旋转和平移。

2. NCC（归一化互相关）

NCC 用于衡量两个图像块的相似度，公式为：

NCC = \frac{\sum (I_{ref} - μ_{ref}) (I_{curr} - μ_{curr})}{\sqrt{\sum (I_{ref} - μ_{ref})^{2} \sum (I_{curr} - μ_{curr})^{2}}}

其中， $I_{ref}$ 和 $I_{curr}$ 是参考图像和当前图像的像素值， $μ_{ref}$ 和 $μ_{curr}$ 是图像块的均值。

3. 深度滤波器

深度更新：通过高斯融合更新深度估计，公式为：

μ_{fuse} = \frac{σ_{curr}^{2} \cdot μ_{prev} + σ_{prev}^{2} \cdot μ_{curr}}{σ_{prev}^{2} + σ_{curr}^{2}}

σ_{fuse}^{2} = \frac{σ_{prev}^{2} \cdot σ_{curr}^{2}}{σ_{prev}^{2} + σ_{curr}^{2}}

其中， $μ_{prev}$ 和 $σ_{prev}^{2}$ 是之前的深度均值和方差， $μ_{curr}$ 和 $σ_{curr}^{2}$ 是当前估计的深度均值和方差。

4. 像素与相机坐标转换

像素到相机坐标：通过相机内参将像素坐标转换为相机坐标系下的方向向量，公式为：

p_{cam} = (\frac{u - c_{x}}{f_{x}}, \frac{v - c_{y}}{f_{y}}, 1)

相机坐标到像素：将相机坐标系下的点投影到像素平面，公式为：

p_{px} = (\frac{f_{x} \cdot x}{z} + c_{x}, \frac{f_{y} \cdot y}{z} + c_{y})

5. 深度估计的不确定性

不确定性计算：通过几何关系计算深度估计的不确定性，公式为：

d_{cov} = p^{'} - d_{est}

其中， $p^{'}$ 是通过极线方向调整后的深度估计值， $d_{est}$ 是当前的深度估计值。

6. 深度图评估

误差计算：通过比较真实深度图和估计深度图，计算平均误差和平方误差，公式为：

error = d_{truth} - d_{estimate}

ave\_error = \frac{\sum error}{N}

ave\_error\_sq = \frac{\sum {error}^{2}}{N}

这些数学原理构成了单目相机稠密深度估计的核心算法。

实现

原始cpp代码:[https://github.com/gaoxiang12/slambook2/blob/master/ch12/dense_mono/dense_mapping.cpp]

#include <iostream>
#include <vector>
#include <fstream>

using namespace std;

#include <boost/timer.hpp>

// for sophus
#include <sophus/se3.hpp>

using Sophus::SE3d;

// for eigen
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace cv;

/**********************************************
* 本程序演示了单目相机在已知轨迹下的稠密深度估计
* 使用极线搜索 + NCC 匹配的方式，与书本的 12.2 节对应
* 请注意本程序并不完美，你完全可以改进它——我其实在故意暴露一些问题(这是借口)。
***********************************************/

// ------------------------------------------------------------------
// parameters
const int boarder = 20;         // 边缘宽度
const int width = 640;          // 图像宽度
const int height = 480;         // 图像高度
const double fx = 481.2f;       // 相机内参
const double fy = -480.0f;
const double cx = 319.5f;
const double cy = 239.5f;
const int ncc_window_size = 3;    // NCC 取的窗口半宽度
const int ncc_area = (2 * ncc_window_size + 1) * (2 * ncc_window_size + 1); // NCC窗口面积
const double min_cov = 0.1;     // 收敛判定：最小方差
const double max_cov = 10;      // 发散判定：最大方差

// ------------------------------------------------------------------
// 重要的函数
/// 从 REMODE 数据集读取数据
bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    vector<SE3d> &poses,
    cv::Mat &ref_depth
);

/**
 * 根据新的图像更新深度估计
 * @param ref           参考图像
 * @param curr          当前图像
 * @param T_C_R         参考图像到当前图像的位姿
 * @param depth         深度
 * @param depth_cov     深度方差
 * @return              是否成功
 */
bool update(
    const Mat &ref,
    const Mat &curr,
    const SE3d &T_C_R,
    Mat &depth,
    Mat &depth_cov2
);

/**
 * 极线搜索
 * @param ref           参考图像
 * @param curr          当前图像
 * @param T_C_R         位姿
 * @param pt_ref        参考图像中点的位置
 * @param depth_mu      深度均值
 * @param depth_cov     深度方差
 * @param pt_curr       当前点
 * @param epipolar_direction  极线方向
 * @return              是否成功
 */
bool epipolarSearch(
    const Mat &ref,
    const Mat &curr,
    const SE3d &T_C_R,
    const Vector2d &pt_ref,
    const double &depth_mu,
    const double &depth_cov,
    Vector2d &pt_curr,
    Vector2d &epipolar_direction
);

/**
 * 更新深度滤波器
 * @param pt_ref    参考图像点
 * @param pt_curr   当前图像点
 * @param T_C_R     位姿
 * @param epipolar_direction 极线方向
 * @param depth     深度均值
 * @param depth_cov2    深度方向
 * @return          是否成功
 */
bool updateDepthFilter(
    const Vector2d &pt_ref,
    const Vector2d &pt_curr,
    const SE3d &T_C_R,
    const Vector2d &epipolar_direction,
    Mat &depth,
    Mat &depth_cov2
);

/**
 * 计算 NCC 评分
 * @param ref       参考图像
 * @param curr      当前图像
 * @param pt_ref    参考点
 * @param pt_curr   当前点
 * @return          NCC评分
 */
double NCC(const Mat &ref, const Mat &curr, const Vector2d &pt_ref, const Vector2d &pt_curr);

// 双线性灰度插值
inline double getBilinearInterpolatedValue(const Mat &img, const Vector2d &pt) {
    uchar *d = &img.data[int(pt(1, 0)) * img.step + int(pt(0, 0))];
    double xx = pt(0, 0) - floor(pt(0, 0));
    double yy = pt(1, 0) - floor(pt(1, 0));
    return ((1 - xx) * (1 - yy) * double(d[0]) +
            xx * (1 - yy) * double(d[1]) +
            (1 - xx) * yy * double(d[img.step]) +
            xx * yy * double(d[img.step + 1])) / 255.0;
}

// ------------------------------------------------------------------
// 一些小工具
// 显示估计的深度图
void plotDepth(const Mat &depth_truth, const Mat &depth_estimate);

// 像素到相机坐标系
inline Vector3d px2cam(const Vector2d px) {
    return Vector3d(
        (px(0, 0) - cx) / fx,
        (px(1, 0) - cy) / fy,
        1
    );
}

// 相机坐标系到像素
inline Vector2d cam2px(const Vector3d p_cam) {
    return Vector2d(
        p_cam(0, 0) * fx / p_cam(2, 0) + cx,
        p_cam(1, 0) * fy / p_cam(2, 0) + cy
    );
}

// 检测一个点是否在图像边框内
inline bool inside(const Vector2d &pt) {
    return pt(0, 0) >= boarder && pt(1, 0) >= boarder
           && pt(0, 0) + boarder < width && pt(1, 0) + boarder <= height;
}

// 显示极线匹配
void showEpipolarMatch(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_curr);

// 显示极线
void showEpipolarLine(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_min_curr,
                      const Vector2d &px_max_curr);

/// 评测深度估计
void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate);
// ------------------------------------------------------------------


int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: dense_mapping path_to_test_dataset" << endl;
        return -1;
    }

    // 从数据集读取数据
    vector<string> color_image_files;
    vector<SE3d> poses_TWC;
    Mat ref_depth;
    bool ret = readDatasetFiles(argv[1], color_image_files, poses_TWC, ref_depth);
    if (ret == false) {
        cout << "Reading image files failed!" << endl;
        return -1;
    }
    cout << "read total " << color_image_files.size() << " files." << endl;

    // 第一张图
    Mat ref = imread(color_image_files[0], 0);                // gray-scale image
    SE3d pose_ref_TWC = poses_TWC[0];
    double init_depth = 3.0;    // 深度初始值
    double init_cov2 = 3.0;     // 方差初始值
    Mat depth(height, width, CV_64F, init_depth);             // 深度图
    Mat depth_cov2(height, width, CV_64F, init_cov2);         // 深度图方差

    for (int index = 1; index < color_image_files.size(); index++) {
        cout << "*** loop " << index << " ***" << endl;
        Mat curr = imread(color_image_files[index], 0);
        if (curr.data == nullptr) continue;
        SE3d pose_curr_TWC = poses_TWC[index];
        SE3d pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC;   // 坐标转换关系： T_C_W * T_W_R = T_C_R
        update(ref, curr, pose_T_C_R, depth, depth_cov2);
        evaludateDepth(ref_depth, depth);
        plotDepth(ref_depth, depth);
        imshow("image", curr);
        waitKey(1);
    }

    cout << "estimation returns, saving depth map ..." << endl;
    imwrite("depth.png", depth);
    cout << "done." << endl;

    return 0;
}

bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    std::vector<SE3d> &poses,
    cv::Mat &ref_depth) {
    ifstream fin(path + "/first_200_frames_traj_over_table_input_sequence.txt");
    if (!fin) return false;

    while (!fin.eof()) {
        // 数据格式：图像文件名 tx, ty, tz, qx, qy, qz, qw ，注意是 TWC 而非 TCW
        string image;
        fin >> image;
        double data[7];
        for (double &d:data) fin >> d;

        color_image_files.push_back(path + string("/images/") + image);
        poses.push_back(
            SE3d(Quaterniond(data[6], data[3], data[4], data[5]),
                 Vector3d(data[0], data[1], data[2]))
        );
        if (!fin.good()) break;
    }
    fin.close();

    // load reference depth
    fin.open(path + "/depthmaps/scene_000.depth");
    ref_depth = cv::Mat(height, width, CV_64F);
    if (!fin) return false;
    for (int y = 0; y < height; y++)
        for (int x = 0; x < width; x++) {
            double depth = 0;
            fin >> depth;
            ref_depth.ptr<double>(y)[x] = depth / 100.0;
        }

    return true;
}

// 对整个深度图进行更新
bool update(const Mat &ref, const Mat &curr, const SE3d &T_C_R, Mat &depth, Mat &depth_cov2) {
    for (int x = boarder; x < width - boarder; x++)
        for (int y = boarder; y < height - boarder; y++) {
            // 遍历每个像素
            if (depth_cov2.ptr<double>(y)[x] < min_cov || depth_cov2.ptr<double>(y)[x] > max_cov) // 深度已收敛或发散
                continue;
            // 在极线上搜索 (x,y) 的匹配
            Vector2d pt_curr;
            Vector2d epipolar_direction;
            bool ret = epipolarSearch(
                ref,
                curr,
                T_C_R,
                Vector2d(x, y),
                depth.ptr<double>(y)[x],
                sqrt(depth_cov2.ptr<double>(y)[x]),
                pt_curr,
                epipolar_direction
            );

            if (ret == false) // 匹配失败
                continue;

            // 取消该注释以显示匹配
            // showEpipolarMatch(ref, curr, Vector2d(x, y), pt_curr);

            // 匹配成功，更新深度图
            updateDepthFilter(Vector2d(x, y), pt_curr, T_C_R, epipolar_direction, depth, depth_cov2);
        }
}

// 极线搜索
// 方法见书 12.2 12.3 两节
bool epipolarSearch(
    const Mat &ref, const Mat &curr,
    const SE3d &T_C_R, const Vector2d &pt_ref,
    const double &depth_mu, const double &depth_cov,
    Vector2d &pt_curr, Vector2d &epipolar_direction) {
    Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Vector3d P_ref = f_ref * depth_mu;    // 参考帧的 P 向量

    Vector2d px_mean_curr = cam2px(T_C_R * P_ref); // 按深度均值投影的像素
    double d_min = depth_mu - 3 * depth_cov, d_max = depth_mu + 3 * depth_cov;
    if (d_min < 0.1) d_min = 0.1;
    Vector2d px_min_curr = cam2px(T_C_R * (f_ref * d_min));    // 按最小深度投影的像素
    Vector2d px_max_curr = cam2px(T_C_R * (f_ref * d_max));    // 按最大深度投影的像素

    Vector2d epipolar_line = px_max_curr - px_min_curr;    // 极线（线段形式）
    epipolar_direction = epipolar_line;        // 极线方向
    epipolar_direction.normalize();
    double half_length = 0.5 * epipolar_line.norm();    // 极线线段的半长度
    if (half_length > 100) half_length = 100;   // 我们不希望搜索太多东西

    // 取消此句注释以显示极线（线段）
    // showEpipolarLine( ref, curr, pt_ref, px_min_curr, px_max_curr );

    // 在极线上搜索，以深度均值点为中心，左右各取半长度
    double best_ncc = -1.0;
    Vector2d best_px_curr;
    for (double l = -half_length; l <= half_length; l += 0.7) { // l+=sqrt(2)
        Vector2d px_curr = px_mean_curr + l * epipolar_direction;  // 待匹配点
        if (!inside(px_curr))
            continue;
        // 计算待匹配点与参考帧的 NCC
        double ncc = NCC(ref, curr, pt_ref, px_curr);
        if (ncc > best_ncc) {
            best_ncc = ncc;
            best_px_curr = px_curr;
        }
    }
    if (best_ncc < 0.85f)      // 只相信 NCC 很高的匹配
        return false;
    pt_curr = best_px_curr;
    return true;
}

double NCC(
    const Mat &ref, const Mat &curr,
    const Vector2d &pt_ref, const Vector2d &pt_curr) {
    // 零均值-归一化互相关
    // 先算均值
    double mean_ref = 0, mean_curr = 0;
    vector<double> values_ref, values_curr; // 参考帧和当前帧的均值
    for (int x = -ncc_window_size; x <= ncc_window_size; x++)
        for (int y = -ncc_window_size; y <= ncc_window_size; y++) {
            double value_ref = double(ref.ptr<uchar>(int(y + pt_ref(1, 0)))[int(x + pt_ref(0, 0))]) / 255.0;
            mean_ref += value_ref;

            double value_curr = getBilinearInterpolatedValue(curr, pt_curr + Vector2d(x, y));
            mean_curr += value_curr;

            values_ref.push_back(value_ref);
            values_curr.push_back(value_curr);
        }

    mean_ref /= ncc_area;
    mean_curr /= ncc_area;

    // 计算 Zero mean NCC
    double numerator = 0, demoniator1 = 0, demoniator2 = 0;
    for (int i = 0; i < values_ref.size(); i++) {
        double n = (values_ref[i] - mean_ref) * (values_curr[i] - mean_curr);
        numerator += n;
        demoniator1 += (values_ref[i] - mean_ref) * (values_ref[i] - mean_ref);
        demoniator2 += (values_curr[i] - mean_curr) * (values_curr[i] - mean_curr);
    }
    return numerator / sqrt(demoniator1 * demoniator2 + 1e-10);   // 防止分母出现零
}

bool updateDepthFilter(
    const Vector2d &pt_ref,
    const Vector2d &pt_curr,
    const SE3d &T_C_R,
    const Vector2d &epipolar_direction,
    Mat &depth,
    Mat &depth_cov2) {
    // 不知道这段还有没有人看
    // 用三角化计算深度
    SE3d T_R_C = T_C_R.inverse();
    Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Vector3d f_curr = px2cam(pt_curr);
    f_curr.normalize();

    // 方程
    // d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC
    // f2 = R_RC * f_cur
    // 转化成下面这个矩阵方程组
    // => [ f_ref^T f_ref, -f_ref^T f2 ] [d_ref]   [f_ref^T t]
    //    [ f_2^T f_ref, -f2^T f2      ] [d_cur] = [f2^T t   ]
    Vector3d t = T_R_C.translation();
    Vector3d f2 = T_R_C.so3() * f_curr;
    Vector2d b = Vector2d(t.dot(f_ref), t.dot(f2));
    Matrix2d A;
    A(0, 0) = f_ref.dot(f_ref);
    A(0, 1) = -f_ref.dot(f2);
    A(1, 0) = -A(0, 1);
    A(1, 1) = -f2.dot(f2);
    Vector2d ans = A.inverse() * b;
    Vector3d xm = ans[0] * f_ref;           // ref 侧的结果
    Vector3d xn = t + ans[1] * f2;          // cur 结果
    Vector3d p_esti = (xm + xn) / 2.0;      // P的位置，取两者的平均
    double depth_estimation = p_esti.norm();   // 深度值

    // 计算不确定性（以一个像素为误差）
    Vector3d p = f_ref * depth_estimation;
    Vector3d a = p - t;
    double t_norm = t.norm();
    double a_norm = a.norm();
    double alpha = acos(f_ref.dot(t) / t_norm);
    double beta = acos(-a.dot(t) / (a_norm * t_norm));
    Vector3d f_curr_prime = px2cam(pt_curr + epipolar_direction);
    f_curr_prime.normalize();
    double beta_prime = acos(f_curr_prime.dot(-t) / t_norm);
    double gamma = M_PI - alpha - beta_prime;
    double p_prime = t_norm * sin(beta_prime) / sin(gamma);
    double d_cov = p_prime - depth_estimation;
    double d_cov2 = d_cov * d_cov;

    // 高斯融合
    double mu = depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];
    double sigma2 = depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];

    double mu_fuse = (d_cov2 * mu + sigma2 * depth_estimation) / (sigma2 + d_cov2);
    double sigma_fuse2 = (sigma2 * d_cov2) / (sigma2 + d_cov2);

    depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = mu_fuse;
    depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = sigma_fuse2;

    return true;
}

// 后面这些太简单我就不注释了（其实是因为懒）
void plotDepth(const Mat &depth_truth, const Mat &depth_estimate) {
    imshow("depth_truth", depth_truth * 0.4);
    imshow("depth_estimate", depth_estimate * 0.4);
    imshow("depth_error", depth_truth - depth_estimate);
    waitKey(1);
}

void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate) {
    double ave_depth_error = 0;     // 平均误差
    double ave_depth_error_sq = 0;      // 平方误差
    int cnt_depth_data = 0;
    for (int y = boarder; y < depth_truth.rows - boarder; y++)
        for (int x = boarder; x < depth_truth.cols - boarder; x++) {
            double error = depth_truth.ptr<double>(y)[x] - depth_estimate.ptr<double>(y)[x];
            ave_depth_error += error;
            ave_depth_error_sq += error * error;
            cnt_depth_data++;
        }
    ave_depth_error /= cnt_depth_data;
    ave_depth_error_sq /= cnt_depth_data;

    cout << "Average squared error = " << ave_depth_error_sq << ", average error: " << ave_depth_error << endl;
}

void showEpipolarMatch(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_curr) {
    Mat ref_show, curr_show;
    cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
    cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

    cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5, cv::Scalar(0, 0, 250), 2);
    cv::circle(curr_show, cv::Point2f(px_curr(0, 0), px_curr(1, 0)), 5, cv::Scalar(0, 0, 250), 2);

    imshow("ref", ref_show);
    imshow("curr", curr_show);
    waitKey(1);
}

void showEpipolarLine(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_min_curr,
                      const Vector2d &px_max_curr) {

    Mat ref_show, curr_show;
    cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
    cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

    cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5, cv::Scalar(0, 255, 0), 2);
    cv::circle(curr_show, cv::Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), 5, cv::Scalar(0, 255, 0), 2);
    cv::circle(curr_show, cv::Point2f(px_max_curr(0, 0), px_max_curr(1, 0)), 5, cv::Scalar(0, 255, 0), 2);
    cv::line(curr_show, Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), Point2f(px_max_curr(0, 0), px_max_curr(1, 0)),
             Scalar(0, 255, 0), 1);

    imshow("ref", ref_show);
    imshow("curr", curr_show);
    waitKey(1);
}

使用rust重构:

#![allow(dead_code)]
#![allow(unused_variables)]
#![allow(unused_imports)]
#![allow(non_fmt_panics)]
#![allow(unused_mut)]
#![allow(unused_assignments)]
#![cfg_attr(not(debug_assertions), windows_subsystem = "windows")]
#![allow(rustdoc::missing_crate_level_docs)]
#![allow(unsafe_code)]
#![allow(clippy::undocumented_unsafe_blocks)]
#![allow(unused_must_use)]
#![allow(non_snake_case)]
#![allow(unused_doc_comments)]

//! 本程序演示了单目相机在已知轨迹下的稠密深度估计，
//! 使用极线搜索 + NCC 匹配的方式，与书本的 12.2 节对应。
//! 添加了图像保存到文件的功能。

use std::fs::File;
use std::io::{self, BufRead, Write, BufReader, Cursor};
use std::path::Path;
use std::fs::create_dir_all;

use std::cell::{
    RefMut, Ref, RefCell
};

// 线性代数
use nalgebra::{
    Quaternion, Vector3, Matrix3, UnitQuaternion, 
    Isometry3, Translation3, Const,
    Matrix, Point3, ViewStorage, Rotation3,
    Matrix3x1, VectorView3, DMatrix, DVector, SVector,
    Vector6, Matrix6, Vector2, Matrix2, 
};

// 李代数
use factrs::{
    assign_symbols,
    core::{BetweenResidual, GaussNewton, Graph, Values},
    dtype, fac,
    linalg::{ ForwardProp, Numeric, NumericalDiff, VectorX, DiffResult, MatrixX },
    residuals::{Residual1, Residual2},
    traits::*,
    variables::{VectorVar2, SE2, VectorVar3, SE3, SO3, SO2, MatrixLieGroup},
    containers::Key,
    noise::{GaussianNoise},
    optimizers::{LevenMarquardt}
};

// 图像处理
use image::{
    DynamicImage, GrayImage, ImageBuffer, Luma, 
    GenericImageView, Rgb, RgbImage, 
    buffer::ConvertBuffer,
};
use imageproc::drawing::{
    draw_cross_mut, draw_filled_circle_mut, draw_line_segment_mut,
};

// 随机数
use rand::Rng;
use rand_distr::{
    Distribution, Normal
};

// 边缘宽度
const BOARDER: i32 = 20;     
// 图像宽度    
const WIDTH: i32 = 640;          
// 图像高度
const HEIGHT: i32 = 480;         
// 相机内参
const FX: f64 = 481.2;           
const FY: f64 = -480.0;
const CX: f64 = 319.5;
const CY: f64 = 239.5;
// NCC 取的窗口半宽度
const NCC_WINDOW_SIZE: i32 = 3;  
// NCC窗口面积
const NCC_AREA: i32 = (2 * NCC_WINDOW_SIZE + 1) * (2 * NCC_WINDOW_SIZE + 1); 
// 收敛判定：最小方差
const MIN_COV: f64 = 0.1;        
// 发散判定：最大方差
const MAX_COV: f64 = 10.0;    
// 图像保存目录
const OUTPUT_DIR: &str = "./result/ch12-dense_mono_dense_mapping";

// 从 REMODE 数据集读取数据
fn read_dataset_files(
    path: &str,
    color_image_files: &mut Vec<String>,
    poses: &mut Vec<SE3>,
    ref_depth: &mut ImageBuffer<Luma<f64>, Vec<f64>>,
) -> io::Result<()> {
    println!("开始读取数据集文件...");
    let file = File::open(format!("{}/first_200_frames_traj_over_table_input_sequence.txt", path))?;
    let reader = io::BufReader::new(file);

    for line in reader.lines() {
        let line = line?;
        let parts: Vec<&str> = line.split_whitespace().collect();
        if parts.len() < 8 {
            continue;
        }

        let image = parts[0];
        let data: Vec<f64> = parts[1..8].iter().map(|s| s.parse().unwrap()).collect();

        color_image_files.push(format!("{}/images/{}", path, image));
        poses.push(SE3::from_rot_trans(
            SO3::from_xyzw(data[3], data[4], data[5],data[6]),
            Vector3::new(data[0], data[1], data[2]),
        ));
    }

    println!("读取轨迹和图像文件完成，共读取 {} 张图像", color_image_files.len());

    // 加载参考深度图
    let depth_file = File::open(format!("{}/depthmaps/scene_000.depth", path))?;
    let depth_reader = io::BufReader::new(depth_file);

    for (y, line) in depth_reader.lines().enumerate() {
        let line = line?;
        let depths: Vec<f64> = line.split_whitespace().map(|s| s.parse().unwrap()).collect();
        for (x, depth) in depths.iter().enumerate() {
            let x_index = std::cmp::min(ref_depth.width() as usize - 1, x);
            let y_index = std::cmp::min(ref_depth.height() as usize - 1, y);
            ref_depth.put_pixel(x_index as u32, y_index as u32, Luma([*depth / 100.0]));
        }
    }

    println!("参考深度图加载完成");
    Ok(())
}

// 极线搜索
fn epipolar_search(
    ref_img: &GrayImage,
    curr_img: &GrayImage,
    t_c_r: &SE3,
    pt_ref: &Vector2<f64>,
    depth_mu: f64,
    depth_cov: f64,
    pt_curr: &mut Vector2<f64>,
    epipolar_direction: &mut Vector2<f64>,
) -> bool {
    println!("开始极线搜索，参考点：{:?}", pt_ref);
    let f_ref = px2cam(pt_ref);
    let p_ref = f_ref * depth_mu;

    let px_mean_curr = cam2px(&(t_c_r.apply((&p_ref).into())));
    let d_min = depth_mu - 3.0 * depth_cov;
    let d_max = depth_mu + 3.0 * depth_cov;
    let px_min_curr = cam2px(&(t_c_r.apply((&(f_ref * d_min)).into())));
    let px_max_curr = cam2px(&(t_c_r.apply((&(f_ref * d_max)).into())));

    let epipolar_line = px_max_curr - px_min_curr;
    *epipolar_direction = epipolar_line;
    epipolar_direction.normalize();
    let half_length = 0.5 * epipolar_line.norm();

    // 在极线上搜索，以深度均值点为中心，左右各取半长度
    let mut best_ncc = -1.0;
    let mut best_px_curr = Vector2::zeros();

    for l in (-half_length as i32..=half_length as i32).step_by(1) {
        let px_curr = px_mean_curr + (l as f64) * *epipolar_direction;
        if !inside(&px_curr) {
            continue;
        }

        let ncc = ncc(ref_img, curr_img, pt_ref, &px_curr);
        if ncc > best_ncc {
            best_ncc = ncc;
            best_px_curr = px_curr;
        }
    }

    if best_ncc < 0.85 {
        println!("极线搜索失败，最佳 NCC 为 {}", best_ncc);
        return false;
    }

    *pt_curr = best_px_curr;
    println!("极线搜索成功，最佳匹配点：{:?}", pt_curr);
    true
}

// 计算 NCC 评分
fn ncc(ref_img: &GrayImage, curr_img: &GrayImage, pt_ref: &Vector2<f64>, pt_curr: &Vector2<f64>) -> f64 {
    println!("计算 NCC 评分，参考点：{:?}，当前点：{:?}", pt_ref, pt_curr);
    let mut mean_ref = 0.0;
    let mut mean_curr = 0.0;
    let mut values_ref = Vec::new();
    let mut values_curr = Vec::new();

    for x in -NCC_WINDOW_SIZE..=NCC_WINDOW_SIZE {
        for y in -NCC_WINDOW_SIZE..=NCC_WINDOW_SIZE {
            let value_ref = ref_img.get_pixel((pt_ref.x + x as f64) as u32, (pt_ref.y + y as f64) as u32)[0] as f64 / 255.0;
            mean_ref += value_ref;

            let value_curr = get_bilinear_interpolated_value(curr_img, &(pt_curr + Vector2::new(x as f64, y as f64)));
            mean_curr += value_curr;

            values_ref.push(value_ref);
            values_curr.push(value_curr);
        }
    }

    mean_ref /= NCC_AREA as f64;
    mean_curr /= NCC_AREA as f64;

    let mut numerator = 0.0;
    let mut demoniator1 = 0.0;
    let mut demoniator2 = 0.0;

    for i in 0..values_ref.len() {
        let n = (values_ref[i] - mean_ref) * (values_curr[i] - mean_curr);
        numerator += n;
        demoniator1 += (values_ref[i] - mean_ref) * (values_ref[i] - mean_ref);
        demoniator2 += (values_curr[i] - mean_curr) * (values_curr[i] - mean_curr);
    }

    let ncc_score = numerator / (demoniator1 * demoniator2 + 1e-10).sqrt();
    println!("NCC 评分：{}", ncc_score);
    ncc_score
}

// 双线性灰度插值
fn get_bilinear_interpolated_value(img: &GrayImage, pt: &Vector2<f64>) -> f64 {
    println!("双线性插值，点：{:?}", pt);
    let x = pt.x as u32;
    let y = pt.y as u32;
    let xx = pt.x - x as f64;
    let yy = pt.y - y as f64;

    let d00 = img.get_pixel(x, y)[0] as f64;
    let d01 = img.get_pixel(x, y + 1)[0] as f64;
    let d10 = img.get_pixel(x + 1, y)[0] as f64;
    let d11 = img.get_pixel(x + 1, y + 1)[0] as f64;

    let interpolated_value = ((1.0 - xx) * (1.0 - yy) * d00 + xx * (1.0 - yy) * d10 + (1.0 - xx) * yy * d01 + xx * yy * d11) / 255.0;
    println!("插值结果：{}", interpolated_value);
    interpolated_value
}

// 像素到相机坐标系
fn px2cam(px: &Vector2<f64>) -> Vector3<f64> {
    println!("像素到相机坐标系，像素点：{:?}", px);
    Vector3::new((px.x - CX) / FX, (px.y - CY) / FY, 1.0)
}

// 相机坐标系到像素
fn cam2px(p_cam: &Vector3<f64>) -> Vector2<f64> {
    println!("相机坐标系到像素，相机点：{:?}", p_cam);
    Vector2::new(p_cam.x * FX / p_cam.z + CX, p_cam.y * FY / p_cam.z + CY)
}

// 检测一个点是否在图像边框内
fn inside(pt: &Vector2<f64>) -> bool {
    println!("检查点是否在图像边框内，点：{:?}", pt);
    pt.x >= (BOARDER as f64) && pt.y >= (BOARDER as f64) && pt.x + (BOARDER as f64) < (WIDTH as f64) && pt.y + (BOARDER as f64) < (HEIGHT as f64)
}

// 更新深度滤波器
fn update_depth_filter(
    pt_ref: &Vector2<f64>,
    pt_curr: &Vector2<f64>,
    t_c_r: &SE3,
    epipolar_direction: &Vector2<f64>,
    depth: &mut ImageBuffer<Luma<f64>, Vec<f64>>,
    depth_cov2: &mut ImageBuffer<Luma<f64>, Vec<f64>>,
) -> bool {
    println!("更新深度滤波器，参考点：{:?}，当前点：{:?}", pt_ref, pt_curr);
    let t_r_c = t_c_r.inverse();
    let f_ref = px2cam(pt_ref).normalize();
    let f_curr = px2cam(pt_curr).normalize();

    let t = t_r_c.xyz().clone();
    let f2 = t_r_c.rot().apply((&f_curr).into());
    let b = Vector2::new(t.dot(&f_ref), t.dot(&f2));
    let a = Matrix2::new(
        f_ref.dot(&f_ref),
        -f_ref.dot(&f2),
        -f_ref.dot(&f2),
        -f2.dot(&f2),
    );
    let ans = a.try_inverse().unwrap() * b;
    let xm = ans[0] * f_ref;
    let xn = t + ans[1] * f2;
    let p_esti = (xm + xn) / 2.0;
    let depth_estimation = p_esti.norm();

    // 计算不确定性
    let p = f_ref * depth_estimation;
    let a = p - t;
    let t_norm = t.norm();
    let a_norm = a.norm();
    let alpha = (f_ref.dot(&t) / t_norm).acos();
    let beta = (-a.dot(&t) / (a_norm * t_norm)).acos();
    let f_curr_prime = px2cam(&(pt_curr + epipolar_direction)).normalize();
    let beta_prime = (f_curr_prime.dot(&-t) / t_norm).acos();
    let gamma = std::f64::consts::PI - alpha - beta_prime;
    let p_prime = t_norm * beta_prime.sin() / gamma.sin();
    let d_cov = p_prime - depth_estimation;
    let d_cov2 = d_cov * d_cov;

    // 高斯融合
    let mu = depth.get_pixel(pt_ref.x as u32, pt_ref.y as u32)[0];
    let sigma2 = depth_cov2.get_pixel(pt_ref.x as u32, pt_ref.y as u32)[0];

    let mu_fuse = (d_cov2 * mu + sigma2 * depth_estimation) / (sigma2 + d_cov2);
    let sigma_fuse2 = (sigma2 * d_cov2) / (sigma2 + d_cov2);

    depth.put_pixel(pt_ref.x as u32, pt_ref.y as u32, Luma([mu_fuse]));
    depth_cov2.put_pixel(pt_ref.x as u32, pt_ref.y as u32, Luma([sigma_fuse2]));

    println!("深度更新完成，新深度：{}", mu_fuse);
    true
}

// 显示并保存极线匹配
fn show_epipolar_match(ref_img: &GrayImage, curr_img: &GrayImage, px_ref: (i32, i32), px_curr: (i32, i32), index: usize, output_dir: &Path) {
    let mut ref_show : RgbImage = ref_img.convert();
    let mut curr_show : RgbImage = curr_img.convert();

    draw_filled_circle_mut(&mut ref_show, px_ref, 5, Rgb([0, 255, 0]));
    draw_filled_circle_mut(&mut curr_show, px_curr, 5, Rgb([255, 0, 0]));

    let rand_number = rand::random::<u32>() % 1000;
    ref_show.save(output_dir.join(format!("ref_match_{index}_{rand_number}.png"))).unwrap();
    curr_show.save(output_dir.join(format!("curr_match_{index}_{rand_number}.png"))).unwrap();
}

// 显示并保存极线
fn show_epipolar_line(ref_img: &GrayImage, curr_img: &GrayImage, px_ref: (f32, f32), px_min_curr: (f32, f32), px_max_curr: (f32, f32), index: usize, output_dir: &Path) {
    let mut curr_show : RgbImage = curr_img.convert();

    draw_line_segment_mut(&mut curr_show, px_min_curr, px_max_curr, Rgb([0, 255, 0])); //极线
    draw_filled_circle_mut(&mut curr_show, (px_ref.0 as i32, px_ref.1 as i32), 5, Rgb([255, 0, 0])); // 参考点
    let rand_number = rand::random::<u32>() % 1000;
    curr_show.save(output_dir.join(format!("epipolar_line_{index}_{rand_number}.png"))).unwrap();
}

// 评估深度估计
fn evaluate_depth(depth_truth: &ImageBuffer<Luma<f64>, Vec<f64>>, depth_estimate: &ImageBuffer<Luma<f64>, Vec<f64>>) {
    let mut error_sum = 0.0;
    let mut error_sq_sum = 0.0;
    let mut count = 0;

    for (x, y, pixel) in depth_truth.enumerate_pixels() {
        let est_pixel = depth_estimate.get_pixel(x, y)[0];
        let error = pixel[0] - est_pixel;
        error_sum += error;
        error_sq_sum += error * error;
        count += 1;
    }

    let avg_error = error_sum / count as f64;
    let avg_sq_error = error_sq_sum / count as f64;

    println!("Average Error: {avg_error}, Average Squared Error: {avg_sq_error}");
}

// 保存深度图可视化
fn save_depth_image(depth_image: &ImageBuffer<Luma<f64>, Vec<f64>>, file_path: &Path) {
    let max_depth = depth_image.pixels().map(|p| p[0]).fold(f64::MIN, f64::max);
    let scale_factor = 255.0 / max_depth;
    let buffer: Vec<u8> = depth_image
        .pixels()
        .map(|p| (p[0] * scale_factor) as u8)
        .collect();
    let new_img = GrayImage::from_raw(depth_image.width(), depth_image.height(), buffer).unwrap();
    new_img.save(file_path).unwrap();
}

// 主函数
fn main() -> Result<(), Box<dyn std::error::Error>> {
    println!("程序开始执行...");
    // 判断图像保存目录是否存在
    let output_dir = Path::new(OUTPUT_DIR);
    if !output_dir.exists() {
        create_dir_all(output_dir)?;
    }

    // 加载数据集
    let path = "./assets/ch12-REMODE";
    let mut color_image_files = Vec::new();
    let mut poses = Vec::new();
    let mut ref_depth = ImageBuffer::new(WIDTH as u32, HEIGHT as u32);

    read_dataset_files(path, &mut color_image_files, &mut poses, &mut ref_depth)?;

    let ref_img = image::open(&color_image_files[0])?.to_luma8();
    let pose_ref_twc = &poses[0];
    let init_depth = 3.0;
    let init_cov2 = 3.0;
    let mut depth = ImageBuffer::from_pixel(WIDTH as u32, HEIGHT as u32, Luma([init_depth]));
    let mut depth_cov2 = ImageBuffer::from_pixel(WIDTH as u32, HEIGHT as u32, Luma([init_cov2]));

    for index in 1..color_image_files.len() {
        println!("处理第 {} 张图像", index);
        let curr_img = image::open(&color_image_files[index])?.to_luma8();
        let pose_curr_twc = &poses[index];
        let pose_t_c_r = pose_curr_twc.inverse() * pose_ref_twc.clone();

        // 更新深度图
        for x in BOARDER..WIDTH - BOARDER {
            for y in BOARDER..HEIGHT - BOARDER {
                let depth_cov = (depth_cov2.get_pixel(x as u32, y as u32)[0] as f64).sqrt();
                if depth_cov < MIN_COV || depth_cov > MAX_COV {
                    continue;
                }

                let mut pt_curr = Vector2::zeros();
                let mut epipolar_direction = Vector2::zeros();
                let pt_ref = Vector2::new(x as f64, y as f64);

                if epipolar_search(&ref_img, &curr_img, &pose_t_c_r, &pt_ref, depth.get_pixel(x as u32, y as u32)[0], depth_cov, &mut pt_curr, &mut epipolar_direction) {
                    update_depth_filter(&pt_ref, &pt_curr, &pose_t_c_r, &epipolar_direction, &mut depth, &mut depth_cov2);
                    // 显示极线(线段)
                    show_epipolar_line(&ref_img, &curr_img, (x as f32, y as f32), (pt_curr.x as f32, pt_curr.y as f32), (pt_curr.x as f32 + epipolar_direction.x as f32, pt_curr.y as f32 + epipolar_direction.y as f32), index, output_dir);
                    // 显示极线匹配
                    show_epipolar_match(&ref_img, &curr_img, (x, y), (pt_curr.x as i32, pt_curr.y as i32), index + 1, output_dir);
                }
            }
        }
        evaluate_depth(&ref_depth, &depth);
        let rand_number = rand::random::<u32>() % 1000;
        save_depth_image(&depth, &output_dir.join(format!("depth_{index}_{rand_number}.png")));
    }

    println!("程序执行完成");
    Ok(())
}

效果

程序开始执行...
开始读取数据集文件...
读取轨迹和图像文件完成，共读取 201 张图像
参考深度图加载完成
处理第 1 张图像
开始极线搜索，参考点：[[20.0, 20.0]]
像素到相机坐标系，像素点：[[20.0, 20.0]]
相机坐标系到像素，相机点：[[-1.8693913433010692, 1.3664523892875033, 3.0006036072515196]]
相机坐标系到像素，相机点：[[1.3686227157496347, -1.0011821385364805, -2.197007571459172]]
相机坐标系到像素，相机点：[[-5.107405402351772, 3.734086917111487, 8.198214785962211]]
检查点是否在图像边框内，点：[[19.709947017813022, 20.911598228801864]]
极线搜索失败，最佳 NCC 为 -1
开始极线搜索，参考点：[[20.0, 21.0]]
像素到相机坐标系，像素点：[[20.0, 21.0]]
相机坐标系到像素，相机点：[[-1.8693827350313288, 1.3602023912233974, 3.000598715452996]]
相机坐标系到像素，相机点：[[1.3686164140588197, -0.9966068224063478, -2.197003990414112]]
相机坐标系到像素，相机点：[[-5.107381884121477, 3.717011604853142, 8.198201421320103]]
检查点是否在图像边框内，点：[[19.71083876879817, 21.91104202810277]]
极线搜索失败，最佳 NCC 为 -1
开始极线搜索，参考点：[[20.0, 22.0]]
像素到相机坐标系，像素点：[[20.0, 22.0]]
相机坐标系到像素，相机点：[[-1.8693741267615887, 1.3539523931592907, 3.0005938236544716]]
相机坐标系到像素，相机点：[[1.3686101123680046, -0.992031506276215, -2.197000409369052]]
相机坐标系到像素，相机点：[[-5.107358365891182, 3.699936292594796, 8.198188056677997]]
检查点是否在图像边框内，点：[[19.7117305226908, 22.910489086143826]]
极线搜索失败，最佳 NCC 为 -1
开始极线搜索，参考点：[[20.0, 23.0]]
像素到相机坐标系，像素点：[[20.0, 23.0]]
相机坐标系到像素，相机点：[[-1.8693655184918485, 1.3477023950951843, 3.0005889318559475]]
相机坐标系到像素，相机点：[[1.3686038106771896, -0.987456190146082, -2.196996828323992]]
相机坐标系到像素，相机点：[[-5.107334847660885, 3.6828609803364505, 8.198174692035886]]
检查点是否在图像边框内，点：[[19.71262227949103, 23.90993940294092]]
极线搜索失败，最佳 NCC 为 -1
开始极线搜索，参考点：[[20.0, 24.0]]
插值结果：0.45378002051700056
双线性插值，点：[[34.941196008644, 273.8573589563265]]
插值结果：0.44161817260912173
双线性插值，点：[[34.941196008644, 274.8573589563265]]
插值结果：0.4616305554813449
双线性插值，点：[[34.941196008644, 275.8573589563265]]
插值结果：0.48169858265169746
双线性插值，点：[[34.941196008644, 276.8573589563265]]
插值结果：0.4624760172710629
双线性插值，点：[[34.941196008644, 277.8573589563265]]
插值结果：0.4833474248532138
双线性插值，点：[[34.941196008644, 278.8573589563265]]
插值结果：0.5117548311170721
双线性插值，点：[[35.941196008644, 272.8573589563265]]
插值结果：0.4478432207576696
双线性插值，点：[[35.941196008644, 273.8573589563265]]
插值结果：0.42718292336624775
双线性插值，点：[[35.941196008644, 274.8573589563265]]
插值结果：0.45748061624595493
双线性插值，点：[[35.941196008644, 275.8573589563265]]
插值结果：0.4734576985005847
双线性插值，点：[[35.941196008644, 276.8573589563265]]
插值结果：0.4516662716854218
双线性插值，点：[[35.941196008644, 277.8573589563265]]
插值结果：0.42698323561167817
双线性插值，点：[[35.941196008644, 278.8573589563265]]
插值结果：0.4694125351752237
NCC 评分：0.924036091750607
极线搜索成功，最佳匹配点：[[32.941196008644, 275.8573589563265]]
更新深度滤波器，参考点：[[33.0, 275.0]]，当前点：[[32.941196008644, 275.8573589563265]]
像素到相机坐标系，像素点：[[33.0, 275.0]]
像素到相机坐标系，像素点：[[32.941196008644, 275.8573589563265]]
像素到相机坐标系，像素点：[[32.92309742015061, 276.001493721201]]
深度更新完成，新深度：0.08288561637779884
开始极线搜索，参考点：[[33.0, 276.0]]
像素到相机坐标系，像素点：[[33.0, 276.0]]
相机坐标系到像素，相机点：[[-1.7861402456029445, -0.23343549458515642, 2.9993614389929117]]
相机坐标系到像素，相机点：[[1.3076786824487272, 0.17001707887216214, -2.196098241182322]]
相机坐标系到像素，相机点：[[-4.879959173654615, -0.6368880680424751, 8.194821119168147]]
检查点是否在图像边框内，点：[[32.94210970694951, 276.8576308424161]]
计算 NCC 评分，参考点：[[33.0, 276.0]]，当前点：[[32.94210970694951, 276.8576308424161]]

分析

这份日志记录了稠密深度估计过程中的关键步骤和数据，包括图像读取、极线搜索、坐标转换、插值计算、NCC评分以及深度滤波器更新等。以下是对各部分的详细分析：

数据读取
- “开始读取数据集文件...读取轨迹和图像文件完成，共读取201张图像，参考深度图加载完成”：程序成功读取了包含201张图像的数据集以及参考深度图，为后续的深度估计提供数据基础。
极线搜索
- 日志中多次出现“开始极线搜索，参考点：[[x, y]]”，表明在不同的像素位置上进行极线搜索，尝试在当前图像中找到与参考图像中特定点匹配的点。极线搜索的范围可能由深度均值和方差等因素决定（根据前面提到的极线搜索原理）。
- “极线搜索失败，最佳NCC为 -1”：多次出现此提示，说明在某些参考点的极线搜索过程中，未能找到合适的匹配点。NCC（归一化互相关）值为 -1 表明参考图像块与当前图像块之间的相似度极低，可能由于场景的复杂性、图像噪声或纹理缺失等原因导致匹配失败。
- “极线搜索成功，最佳匹配点：[[x, y]]”：当极线搜索成功时，记录了找到的最佳匹配点的坐标。例如，在参考点为 [[20.0, 24.0]] 附近的搜索过程中，最终找到了最佳匹配点 [[32.941196008644, 275.8573589563265]]，并且给出了该匹配的NCC评分：0.924036091750607，表明这两个图像块之间具有较高的相似度。
坐标转换
- “像素到相机坐标系，像素点：[[x, y]]”：记录了将像素坐标转换为相机坐标系下的方向向量的操作，通过相机内参实现这种转换，为后续三角化计算深度提供基础。
- “相机坐标系到像素，相机点：[[x, y, z]]”：展示了将相机坐标系下的点投影回像素平面的过程，这一操作有助于验证坐标转换的正确性以及检查点是否在图像边框内。
双线性插值与NCC评分
- “插值结果：x”和“双线性插值，点：[[x, y]]”：在极线搜索过程中，可能对图像进行了双线性插值操作以获取更精确的像素值，用于计算NCC评分。多个插值结果表明在不同位置进行了插值计算，以评估不同位置的相似度。
- “NCC评分：x”：通过计算参考图像块与当前图像块之间的NCC值来衡量它们的相似度，当NCC评分足够高时，认为找到了较好的匹配点。如前面提到的极线搜索成功案例中，NCC评分为0.924036091750607，说明匹配效果较好。
深度滤波器更新
- “更新深度滤波器，参考点：[[x, y]]，当前点：[[x, y]]”：在找到极线搜索的最佳匹配点后，使用参考点和当前点的信息来更新深度滤波器。
- “深度更新完成，新深度：x”：根据深度更新公式（如前面提到的深度滤波器部分的公式），通过高斯融合更新深度估计，得到新的深度值。例如，这里得到的新深度为0.08288561637779884 。