2. 稠密深度估计

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

// for sophus
#include <sophus/se3.h>
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;

// ------------------------------------------------------------------
// 参数
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 数据集读取数据
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& 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;
}

// ------------------------------------------------------------------
// 一些小工具
// 显示估计的深度图
bool plotDepth(const Mat& depth);

// 像素到相机坐标系
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);
// ------------------------------------------------------------------

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<SE3> poses_TWC;
bool ret = readDatasetFiles(argv[1], color_image_files, poses_TWC);
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
SE3 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_cov(height, width, CV_64F, init_cov2);          // 深度图方差

for (int index = 1; index < color_image_files.size(); index++)
{
cout << "*** loop " << index << " ***" << endl;
if (curr.data == nullptr) continue;
SE3 pose_curr_TWC = poses_TWC[index];
SE3 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_cov);
plotDepth(depth);
imshow("image", curr);
waitKey(1);
}

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

return 0;
}

const string& path,
vector< string >& color_image_files,
std::vector<SE3>& poses
)
{
ifstream fin(path + "../test_data/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(
SE3(Quaterniond(data[6], data[3], data[4], data[5]),
Vector3d(data[0], data[1], data[2]))
);
if (!fin.good()) break;
}
return true;
}

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

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

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

// 匹配成功，更新深度图
updateDepthFilter(Vector2d(x, y), pt_curr, T_C_R, depth, 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)
{
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;    // 极线（线段形式）
Vector2d 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;

// 计算去均值化 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 SE3& T_C_R,
Mat& depth,
Mat& depth_cov
)
{
// 用三角化计算深度
SE3 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
// => [ f_ref^T f_ref, -f_ref^T f_cur ] [d_ref] = [f_ref^T t]
//    [ f_cur^T f_ref, -f_cur^T f_cur ] [d_cur] = [f_cur^T t]
// 二阶方程用克莱默法则求解并解之
Vector3d t = T_R_C.translation();
Vector3d f2 = T_R_C.rotation_matrix() * f_curr;
Vector2d b = Vector2d(t.dot(f_ref), t.dot(f2));
double A[4];
A[0] = f_ref.dot(f_ref);
A[2] = f_ref.dot(f2);
A[1] = -A[2];
A[3] = -f2.dot(f2);
double d = A[0] * A[3] - A[1] * A[2];
Vector2d lambdavec =
Vector2d(A[3] * b(0, 0) - A[1] * b(1, 0),
-A[2] * b(0, 0) + A[0] * b(1, 0)) / d;
Vector3d xm = lambdavec(0, 0) * f_ref;
Vector3d xn = t + lambdavec(1, 0) * f2;
Vector3d d_esti = (xm + xn) / 2.0;  // 三角化算得的深度向量
double depth_estimation = d_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));
double beta_prime = beta + atan(1 / fx);
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_cov.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_cov.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = sigma_fuse2;

return true;
}

1. main 函数非常简单
它只负责从数据集中读取图像，然后交给 update 函数，对深度图进行更新
2. update 函数中，遍历了参考帧的每个像素
先在当前帧中寻找极线匹配，若能匹配上，则利用极线匹配的结果更新深度图的估计
3. 极线搜索原理大致和 视觉SLAM笔记（61） 单目稠密建图 介绍的相同
但实现上添加了一些细节：因为假设深度值服从高斯分布，就以均值为中心，左右各取 ±3σ 作为半径，然后在当前帧中寻找极线的投影。然后，遍历此极线上的像素（步长取$\sqrt{2}/2$ 的近似值 0.7），寻找 NCC 最高的点，作为匹配点
如果最高的 NCC 也低于阈值（这里取 0.85），则认为匹配失败
4. NCC 的计算使用了去均值化后的做法，即对于图像块 A, B，取：

\$ ./dense_mapping ../test_data/


4. 逆深度

1. 实际想表达的是：这个场景深度大概是 5-10 米，可能有一些更远的点
但近处肯定不会小于相机焦距（或认为深度不会小于 0）
这个分布并不是像高斯分布那样，形成一个对称的形状
它的尾部可能稍长，而负数区域则为零
2. 在一些室外应用中，可能存在距离非常远，乃至无穷远处的点
初始值中难以涵盖这些点，并且用高斯分布描述它们会有一些数值计算上的困难

6. 并行化：效率的问题

GPU 的并行计算架构非常适合这样的问题

7. 其他的改进

1. 现在各像素完全是独立计算的，可能存在这个像素深度很小，边上一个又很大的情况
可以假设深度图中相邻的深度变化不会太大，从而给深度估计加上了空间正则项
这种做法会使得到的深度图更加平滑
2. 没有显式地处理外点（Outlier）的情况
事实上，由于遮挡、光照、运动模糊等各种因素的影响，不可能对每个像素都能保持成功的匹配
而演示程序的做法中，只要 NCC 大于一定值，就认为出现了成功的匹配，没有考虑到错误匹配的情况
处理错误匹配亦有若干种方式，例如均匀——高斯混合分布下的深度滤波器，显式地将内点与外点进行区别并进行概率建模，能够较好的处理外点数据，然而这种类型的滤波器理论较为复杂

《视觉SLAM十四讲》

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie