SLAM学习——建图问题（一）

1.单目稠密地图的构建

在上述中，我们讨论的是稀疏地图的构建，但是在实际的定位、导航和壁障过程中，我们需要有稠密地图。常见的单目稠密地图的构建思路有：
1.单目：通过运动，得出运动轨迹，计算出运动的关系，通过三角测量计算出像素深度。（同下）
2.双目：利用俩个相机的视差计算出像素的深度。（吃力不讨好，但是大型场合可能有比较好的效果。）
3.RGBD：自带深度图，可直接得到像素的深度。（比较好，但是不适合大型场合）

对于单目而言，即我们提到的第一种方法，通过单目建立稠密地图：
1.提取特征点，然后在很多图像中，进行特征点的匹配，跟踪特征点在各张图像的轨迹（经常用ORB特征提取）
2.然后通过三角测量估计每一个像素的深度。在这里，我们需要利用很多的三角测量使得某点的深度进行收敛，得到确切的深度信息。

在稠密深度地图中，无法对每个像素计算其描述子，所以在稠密估计问题中，匹配成为很重要的一环，我们用到了极线搜索和块匹配技术，极线搜索的原理如下图所示：

而块匹配技术就相当于把像素的比较换成了块的比较，我们直接上代码可得：

#include <iostream>
#include <vector>
#include <fstream>
using namespace std;
#include <boost/timer.hpp>

// for sophus
#include <sophus/se3.h> //这里使用了sophus这个工具，使用SE3
using Sophus::SE3;

// 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;
// ------------------------------------------------------------------
// 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 = 2;  // 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<SE3>& poses
)；
// 根据新的图像更新深度估计
bool update(
    const Mat& ref,
    const Mat& curr,
    const SE3& T_C_R,
    Mat& depth,
    Mat& depth_cov
);
// 极线搜索
bool epipolarSearch(
    const Mat& ref,
    const Mat& curr,
    const SE3& T_C_R,
    const Vector2d& pt_ref,
    const double& depth_mu,
    const double& depth_cov,
    Vector2d& pt_curr
);
// 更新深度滤波器
bool updateDepthFilter(
    const Vector2d& pt_ref,
    const Vector2d& pt_curr,
    const SE3& T_C_R,
    Mat& depth,
    Mat& depth_cov
);
// 计算 NCC 评分
double NCC( const Mat& ref, const Mat& curr, const Vector2d& pt_ref, const Vector2d& pt_curr );
// 双线性灰度插值
inline double getBilinearInterpolatedValue( const Mat