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