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