slam 单目稠密深度详解

单目计算深度比较复杂，一般可以用RGB-D相机直接得到深度，但还是练习一下。

因为是稠密重建，对每个像素都算深度，所以就不是提取特征了。

仅凭一幅图像无法估计出深度，要用不同视角下的图像来估计。
特征点匹配法中，是根据不同视角中同一特征点的不同位置来进行三角测量估计深度，
但是稠密重建是不用特征点的，要对每个像素都进行匹配，然后进行三角测量。怎么匹配呢，用极线搜索和块匹配。

什么是极线搜索，如下（贴一张别人的图）：
首先说神马是极线，相机1中的某像素p1, 它可能的深度是，比如[0.1, 5]（范围是图上的d), 然后p1的像素坐标结合这个最小和最大深度（d的两头），形成了俩位置，这俩位置映射到相机2的两个点，这两个点形成的线段就是极线。

那搜索呢，就是在极线这条线段上，搜索和p1一样的点，怎么判断一样，就是用各种相似度函数，这里用了NCC（后面说），只搜索一个点鲁棒性不行，就搜索一个块，块的大小自己定义。

搜索到p2点和p1点匹配度高之后，用三角测量估计p的深度，
现把图片1当作p1所在的图片，要估计图片1中所有像素的深度，目前有不同视角下拍的图片2来匹配p2，估计深度。
仅一张图片2，深度肯定是不够稳定的，
那么就来多张不同视角下的图片3,4,5…,
每张都和p1匹配，再估计图片1中所有像素的深度，于是得到了很多张不同的深度图。

那这么多张估计出来的深度图，总要融合成一个深度图吧，毕竟只要估计一张图片1的深度。
下面就是高斯融合了。

高斯融合就是假设深度d是服从高斯分布的，初始均值是 μ \mu μ, 方差是 σ \sigma σ,
然后根据图片2，3，4…得到了新的深度均值方差： μ o b s \mu_{obs} μobs, σ o b s \sigma_{obs} σobs,

根据高斯分布的乘积，得到融合公式
σ o b s 2 μ + σ 2 μ o b s σ 2 + σ o b s 2 \frac{ \sigma ^{2}_{obs}\mu + \sigma ^{2}\mu_{obs} }{\sigma ^{2} + \sigma ^{2}_{obs}} σ2+σobs2σobs2μ+σ2μobs
注意，因为每个像素都要算深度，所以这个均值，方差都是图像大小的矩阵。

最后得到的 μ \mu μ就是要得到的深度图。

接下来问题来了， μ o b s \mu_{obs} μobs和 σ o b s \sigma_{obs} σobs怎么算。
μ o b s \mu_{obs} μobs就是本次估计的深度，因为估计的时候就是求的平均值（后面代码会说）
σ o b s \sigma_{obs} σobs要根据不确定性公式算，
先上结论： σ o b s = ∥ p ∥ − ∥ p ′ ∥ \sigma_{obs} = \left \| p \right \| - \left \| p' \right \| σobs=∥p∥−∥p′∥,
其中p指的是图中的 O 1 P O_{1}P O1P向量，p’指的是 O 1 P ′ O_{1}P' O1P′向量
那么问题回归到P和P’两个点怎么算出来。

首先p1对应的p2点是通过极线搜索出来的，通过p1, p2两点三角测量得到p1的估计位置P，
然后把p2沿极线移动一个像素，得到p2’,
再根据p1, p2’这两个点再估计一个位置P’。

而实际上不需要估计两次，在移动p2得到p2’后，
根据图上标的一些边和角，得到如下关系
a = p − t a = p - t a=p−t (向量减）
α = a r c c o s < p , t > \alpha = arccos<p, t> α=arccos<p,t>
β = a r c c o s < p , − t > \beta = arccos<p, -t> β=arccos<p,−t>
β ′ = a r c c o s < O 2 p 2 ′ , − t > \beta' = arccos<O_{2}p_{2}', -t> β′=arccos<O2p2′,−t>
γ = π − α − β \gamma = \pi - \alpha - \beta γ=π−α−β (三角形内角和=180度）

然后根据正弦定理：
∥ p ′ ∥ s i n β ′ = ∥ t ∥ s i n γ \frac{\left \| p' \right \| }{sin\beta'} = \frac{\left \| t \right \| }{sin\gamma} sinβ′∥p′∥=sinγ∥t∥

就得到了
∥ p ′ ∥ = ∥ t ∥ s i n β ′ s i n γ \left \| p' \right \| = \left \| t \right \|\frac{sin\beta' }{sin\gamma} ∥p′∥=∥t∥sinγsinβ′

注意这个t是相机位姿变换T的平移部分。

至于三角测量具体怎么算，这个在代码中注释。

下面一步一步完成上面的过程。
首先初始化上面的 μ \mu μ和 σ \sigma σ矩阵

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);  //深度图方差

由p1估计p2是需要相机的位姿的，这个位姿从data中读取
这是第一幅图的相机位姿。

SE3d pose_ref_TWC = poses_TWC[0];

第一幅图像

Mat ref = imread(color_image_files[0], 0);  //灰度图

接下来是遍历所有的图像（不同位姿下拍的图片）
我们称第一幅图像为ref, TWC指从current相机坐标系到world坐标系的变换矩阵T，
那么p1到world坐标系，再到p2, 就需要一个把ref坐标系变换到current相机坐标系的变换T_C_R。
它是由ref的T_W_C(current to world)左乘相机2的T_C_W(world to current)。

for(int index = 1; index < color_image_files.size(); index ++) {cout << "*** loop " << index << " ***" << endl;Mat curr = imread(color_image_files[index], 0);if(curr.data == nullptr) continue;SE3d pose_curr_TWC = poses_TWC[index];   //当前坐标系到世界坐标系的变换矩阵TSE3d pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC;  //左乘，ref->world->cur: T_C_W * T_W_R = T_C_Rupdate(ref, curr, pose_T_C_R, depth, depth_cov2);
}

好了位姿解决了，下面就是update中需要做的极线搜索和高斯融合了。

Vector2d pt_curr;  //当前位置
Vector2d epipolar_direction;  //极线方向
bool ret = epipolarSearch(ref,curr,T_C_R,Vector2d(x, y),depth.ptr<double>(y)[x],sqrt(depth_cov2.ptr<double>(y)[x]),pt_curr,epipolar_direction);
if(ret == false)  continue;  //匹配失败//匹配成功，更新深度滤波器
updateDepthFilter(Vector2d(x, y), pt_curr, T_C_R, epipolar_direction, depth, 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, Vector2d &epipolar_direction) {Vector3d f_ref = px2cam(pt_ref);   //转归一化相机坐标（X/Z, Y/Z, 1)f_ref.normalize();  //单位向量Vector3d P_ref = f_ref * depth_mu;  //(X/Z, Y/Z, 1) * Z = (X, Y, Z), 参考帧的P向量Vector2d px_mean_curr = cam2px(T_C_R * P_ref); //投到第2个相机的像素坐标，书上的p2点double d_min = depth_mu - 3 * depth_cov;  //小概率事件左边mu - 3*sigmadouble d_max = depth_mu + 3 * depth_cov;  //小概率事件右边mu + 3*sigmaif(d_min < 0.1) d_min = 0.1;  //实际情况考虑，不会再近了//因为已知位姿，把P点按深度范围投影到p2，可得到极线Vector2d px_min_curr = cam2px(T_C_R * (f_ref * d_min));  //按深度d_min投影的p2点Vector2d px_max_curr = cam2px(T_C_R * (f_ref * d_max));  //按深度d_max投影的p2点Vector2d epipolar_line = px_max_curr - px_min_curr;  //极线epipolar_direction = epipolar_line;   //向量形式epipolar_direction.normalize();   //按长度归一化,得到单位向量double half_length = 0.5 * epipolar_line.norm();    //极线线段的半长度if(half_length > 100) half_length = 100;    //不需要搜索太多// 在极线上搜索，以深度均值点(p2)为中心，左右各取半长度double best_ncc = -1.0;Vector2d best_px_curr;for(double l = -half_length; l <= half_length; l += 0.7) { // l+=sqrt(2)/2Vector2d px_curr = px_mean_curr + l * epipolar_direction;  //px_mean_curr:按深度均值计算出的p1点直接投影到p2if(!inside(px_curr)) continue;//计算NCCdouble 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) return false;   //大于阈值才认为是匹配pt_curr = best_px_curr;  //通过引用形参传递匹配点return true;
}

块匹配在NCC函数里

double NCC(const Mat &ref, const Mat &curr,const Vector2d &pt_ref, const Vector2d &pt_curr) {double mean_ref = 0;double mean_curr = 0;vector<double> values_ref, values_curr;  //参考帧和当前帧的均值//以参考点p1, p2为中心，块扫描，像素值归一化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(pt_ref(1, 0) + y))[int(pt_ref(0, 0) + x)]) / 255.0;mean_ref += value_ref;//pt_curr是极线搜索的坐标，不是整数，用到双线性插值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;//计算去均值的NCCdouble numerator = 0, demoniator1 = 0, demoniator2 = 0;for(int i = 0; i < values_ref.size(); i++) {//套NCC公式，用的是去均值的NCCdouble 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);   //防止分母为0
}

高斯融合，里面包含了三角测量，三角测量公式的推导写在注释里。

bool updateDepthFilter(const Vector2d &pt_ref,const Vector2d &pt_curr,const SE3d &T_C_R,const Vector2d &epipolar_direction,Mat &depth,Mat &depth_cov2) {//三角化计算深度SE3d T_R_C = T_C_R.inverse();Vector3d f_ref = px2cam(pt_ref);   //p1的归一化相机坐标f_ref.normalize();  //O1P的单位方向向量Vector3d f_curr = px2cam(pt_curr);  //p2的归一化相机坐标f_curr.normalize();  //O2P的单位方向向量//三角测量法计算深度// 方程// d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC// f2 = R_RC * f_cur// 转化成下面这个矩阵方程组// => [ f_ref^T f_ref, -f_ref^T f2 ] [d_ref]   [f_ref^T t]//    [ f_2^T f_ref, -f2^T f2      ] [d_cur] = [f2^T t   ]//这里我要注释一下是怎么转化的，依据是什么//三角测量公式为s2x2 = s1Rx1 + t, 也就是两个相机估的p位置一样，s2为p2深度，s1为p1深度，x1,x2为特征点p1,p2的归一化坐标(书上177页)//现在把相机2当成ref,倒推相机1下p的深度，//所以 d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC//等式两边左乘f_ref^T, 得到d_ref * f_ref^T * f_ref = d_cur * f_ref^T * f2 + f_ref^T * t_RC, 其中f2 = R_RC * f_cur//等式两边左乘f2^T, 得到d_ref * f2^T * f_ref = d_cur * f2^T * f2 + f2^T * t_RC, 其中f2 = R_RC * f_cur//整理一下就得到上面的矩阵方程组Vector3d t = T_R_C.translation();   //第2个相机到第1个相机的平移,三角测量公式里的t，同时还是两个相机的平移Vector3d f2 = T_R_C.so3() * f_curr;  //O2P的方向旋转到第1个相机，代表三角测量公式里的Rx1Vector2d b = Vector2d(t.dot(f_ref), t.dot(f2));Matrix2d A;A(0, 0) = f_ref.dot(f_ref); //点乘，相当于f_ref^T * f_refA(0, 1) = -f_ref.dot(f2);A(1, 0) = -A(0, 1);A(1, 1) = -f2.dot(f2);Vector2d ans = A.inverse() * b;  //矩阵规模小，直接求逆Vector3d xm = ans[0] * f_ref;  //相机1估计的p位置Vector3d xn = ans[1] * f2 + t;  //T_R_C.so3() * (ans[1] * f_curr) + t, 相机2估计的p位置变换回相机1的坐标系下Vector3d p_esti = (xm + xn) / 2.0;   //p的位置取两者的平均double depth_estimation = p_esti.norm();  //深度值,2范数，平方和开方，即mu_obs//计算不确定性（以一个像素为误差）Vector3d p = f_ref * depth_estimation;  //p的位置,O1P向量Vector3d a = p - t;    //O2P向量double t_norm = t.norm();double a_norm = a.norm();double alpha = acos(f_ref.dot(t)/t_norm); //acos要求取值在[-1,1],所以要用单位方向向量来算acos<p,t>double beta = acos(-a.dot(t)/(a_norm * t_norm)); //acos<a, -t>Vector3d f_curr_prime = px2cam(pt_curr + epipolar_direction);  //p2沿极线方向移动一个像素得到p2'f_curr_prime.normalize();  //得到单位方向向量double beta_prime = acos(f_curr_prime.dot(-t) / t_norm);   //acos<O2p2', -t>double gamma = M_PI - alpha - beta_prime;double p_prime =  t_norm * sin(beta_prime) / sin(gamma);   //移动一个像素以后新的p的深度估计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_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];double mu_fuse = (d_cov2 * mu + sigma2 * depth_estimation) / (sigma2 + d_cov2); //融合后的mudouble sigma_fuse2 = (sigma2 * d_cov2) / (sigma2 + d_cov2);  //融合后的sigmadepth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = mu_fuse;depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = sigma_fuse2;return true;
}

完整版代码参考链接

slam 单目稠密深度详解相关推荐

视觉SLAM笔记（61）单目稠密建图
视觉SLAM笔记(61) 单目稠密建图 1. 立体视觉 2. 极线搜索与块匹配 3. 高斯分布的深度滤波器 1. 立体视觉相机,很久以来被认为是只有角度的传感器(Bearing only) 单个图像 ...
视觉slam中的一种单目稠密建图方法
之所以只讲单目的稠密建图,这是因为,首先,对于RGBD图,我们已知所有点的深度,可以直接生成稠密的点云.其次,对于稀疏地图,其实很多VO中已经完成了稀疏地图的构建,但是稀疏地图只能用于定位,不能用于导 ...
SLAM第十二讲实践：【建图】单目稠密图重建实践、RGB_D稠密建图、从RGB_D稠密建图点云重建网格图、八叉树地图实践
2022.10.20 bionic 目录 1 单目稠密图重建实践 1.1 修改CMakeLists.txt 1.2 实现 2 RGB_D稠密建图 2.1 c_cpp_properties.json 2 ...
单目图像深度估计 - SLAM辅助篇：MegaDepth
目录入门篇:图像深度估计相关总结应用篇:Learning to be a Depth Camera 尺度篇:Make3D 迁移篇:Depth Extraction from Video Using ...
单目图像深度估计 - 泛化篇：S2R-DepthNet
单目图像深度估计 - 泛化篇:S2R-DepthNet 偶然看到微软亚研的单目图像深度估计发表在了CVPR2021上,决定更新一下这个系列. 官方已经有了十分详细的论文解读,我认为这篇文章比较有意思的 ...
华为诺亚方舟加拿大实验室提出BANet，双向视觉注意力机制用于单目相机深度估计...
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达这篇文章的作者是华为加拿大诺亚方舟实验室,是一篇将双向注意力机制用于弹幕深度估计的工作.主要的创新点在 ...
单目图像深度估计 - 迁移篇：Depth Extraction from Video Using Non-parametric Sampling
目录入门篇:图像深度估计相关总结应用篇:Learning to be a Depth Camera 尺度篇:Make3D 迁移篇:Depth Extraction from Video Using ...
【论文精读】基于网络立体数据监督的单目相对深度感知
基于网络立体数据监督的单目相对深度感知 Paper Information Abstract 1 Introduction 2 Related Work 3 Proposed method 3.1 O ...
深度详解 Android 之 Context
文章目录一. 简介 1.1 Context 概述 1.2 Context 体系结构 1.3 Context 作用域 1.4 总结二. Context 详解 2.1 Application Cont ...

slam 单目稠密深度详解

slam 单目稠密深度详解相关推荐

最新文章

热门文章