SLAM十四讲-ch12-建图（包含单目和RGBD的代码详细注释）

1、单目稠密建图

单目相机因为无法获得深度信息，所以要计算深度
步骤：
1、利用极线搜索、块匹配得到参考帧和当前帧像素的匹配（快匹配利用ncc评分进行匹配）。
2、利用三角化计算深度估计值
3、利用深度滤波器对深度估计值进行滤波 【滤波就是算出估计的深度（这时候估计的深度加上了一个像素的误差了）的不确定性，然后估计的深度与之前的原深度进行高斯融合】
4、更新深度
补：三角化的公式如下图

源码如下：

//
// Created by nnz on 2020/11/16.
//
/*************
 *  1、读取图片
 *  2、极限搜索 利用NCC评分进行块匹配
 *  3、利用三角化算出估计的深度
 *  3、深度滤波(算出加上一个像素误差的估计深度的不确定性)
 *  4、估计的深度与之前的深度进行高斯融合
 *  5、深度更新
 *
 ************/

#include <iostream>
#include <vector>
#include <fstream>
#include <boost/timer.hpp>
#include <sophus/se3.hpp> //位姿
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace std;
using Sophus::SE3d;
using namespace Eigen;
using namespace cv;

// 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;      // 发散判定：最大方差
//读取数据集函数
bool readDatasetFiles(
        const string &path,//数据集路经
        vector<string> &color_image_files,//图片路径
        vector<SE3d> &poses,//容器poses存放各图片的位姿 注：位姿是Twc 相机坐标到世界坐标
        cv::Mat &ref_depth//ref_depth存放第一政图片的真实深度图，以便和我们自己计算出来的深度进行比较
        );
//对深度图进行更新
bool update(
        const Mat &ref,//参考帧
        const Mat &curr,//当前帧
        const SE3d &T_C_R,//位姿变换 （参考帧到当前帧）
        Mat &depth,//深度图
        Mat &depth_cov2//深度方差
        );

//极线搜索
//利用极线搜索进行参考帧和当前帧像点的匹配
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,//与参考帧像素点匹配的当前帧像素点 这是output
        Vector2d &epipolar_direction//极线 向量 （单位向量，描述极线的方向）
        );

// 计算 NCC 评分 就是算块与块之间的相似性 从而看这两个点是否匹配
double NCC(
        const Mat &ref, //参考图像
        const Mat &curr,//当前图像
        const Vector2d &pt_ref,//  参考点
        const Vector2d &pt_curr//  当前点
        );

//深度滤波器，更新深度和深度方差
bool updateDepthFilter(
        const Vector2d &pt_ref,//参考图像点
        const Vector2d &pt_curr,//当前图像点
        const SE3d &T_C_R,//变换矩阵（参考帧到当前帧）
        const Vector2d &epipolar_direction,//极线方向
        Mat &depth,//深度均值
        Mat &depth_cov2//深度方差
);

// 双线性灰度插值
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;
}


// 像素到相机坐标系(相机的归一化)
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(0,0)+ boarder <width &&
           pt(1,0)>=boarder && pt(1,0)+boarder<=height;
}

//深度误差计算
void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate);

//画深度图
void plotDepth(const Mat &depth_truth, const Mat &depth_estimate);

//显示极线搜索匹配
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
        );

int main(int argc,char **argv)
{
   
    //判断使用规不规范
    if(argc!=2)
    {
   
        cout<<" usge: ./dense_mapping path to dataset "<<endl;
        return 1;
    }
    //读取数据（数据集）
    //容器color_image_files里存放的是图片的路径  容器poses_TWC 存放图片的位姿
    //ref_depth 第一张图的真实深度图
    vector<string> color__images_files;
    vector<SE3d> poses_TWC;
    Mat ref_depth;
    bool ret=readDatasetFiles(argv[1],color__images_files,poses_TWC,ref_depth);
    //一个问题 这里读取文件的函数 深度图只读取了scene_000的深度 不过拿一个图的真实深度应该也够用了
    if(ret==false) //判断读取有没有失败
    {
   
        cout<<" Reading files failed! "<<endl;
        return 1;
    }
    cout<<"read total "<<color__images_files.size()<< "files. "<<endl;
    Mat ref =imread(color__images_files[0],0);//ref现在是第一张图
    SE3d pose_ref_TWC=poses_TWC[0];//pose_ref_TWC现在是第一张图片的位姿
    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);//初始化深度方差
    //参考帧不变 一直是第一张图片
    //当前帧从第二张到第200张
    //利用循环遍历第二张图片到第200张图片 利用这些当前帧与固定参考帧进行深度更新，计算出第一张图的深度图
    for (int index = 1; index <color__images_files.size(); index++)
    {
   
        cout<<"*** loop "<<index<<"***"<<endl;
        Mat curr =imread(color__images_files[index],0);//curr当前帧
        if(curr.data== nullptr) continue;
        SE3d pose_curr_TWC=poses_TWC[index];
        SE3d pose_T_C_R =pose_curr_TWC.inverse()*pose_ref_TWC;//位姿变换 参考帧到当前帧
        update(ref,curr,pose_T_C_R,depth,depth_cov2);//更新深度图和深度方差图
        evaludateDepth(ref_depth,depth);//这里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