本文详细介绍了单目稠密重建的过程，包括地图分类的应用、单目重建的方法、深度滤波器的使用以及代码实现。通过极线搜索和块匹配确定像素深度，利用深度滤波器处理不确定性，最终实现稠密地图的构建。同时，文章讨论了立体视觉的局限性和可能的改进策略。
目录
一、地图分类：
1.定位：指机器人在地图中确定自己的位置。需要稀疏地图即可
2.导航：机器人在地图中进行路径规划，在任意两个点寻找路径，需要稠密地图。
3.避障：更关注局部地图，仅有特征点无法判断是否为障碍物，需要稠密地图。
4.重建：主要用于向人类展示，需要看上去更舒服、更美观。也可以用于通信，可以远程观看重建的三维地图，如网上购物或者三维视频通话。这种地图对外观有要求，更希望构建带纹理的平面。需要稠密地图。
5.交互： 主要是指人与地图之间的互动。如AR设备中放置虚拟物体与人交互或 语义地图中机器人识别地图中的物体。
二、单目稠密重建
单目通过三角化得到像素的距离，双目通过左右目的视差计算像素的距离，这两种方式称为立体视觉。
关于极线约束参考【1】
单目稠密重建中，通过极线搜索和块匹配技术确定某帧图像中的像素点在其他帧的位置，然后使用深度滤波确定深度值。
视觉里程计中使用三角测量计算深度的前提是特征点已经匹配好，而且位姿已知。稠密重建需要知道参考帧所有像素点的深度，由于大部分像素点都不是特征点，对于这些点通过极线搜索和快匹配方法确定在其他帧图像位置。
上图表示第一帧的某个像素点，在第二帧图像沿着极线计算得分的分布。选择最高点当匹配点计算深度值，但是某个像素点不可能只被观测两次，导致每次计算的深度值不一样。因此把深度估计建模为一个状态估计问题，使用深度滤波器方法求解。
step1：首先假设像素的深度满足高斯分布：
double init_depth = 3.0; // 深度初始值
double init_cov2 = 3.0; // 方差初始值step2：新观测到的深度满足高斯分布：, 其中
是每次计算的新深度值，
表示不确定度，书中给出了具体的计算方法，深度滤波器就是不断缩小这个不确定度
step3:使用观测的信息更新原先 的分布，融合后的均值和方差为：
但是实际的深度值不一定符合高斯分布，因此有了均匀-高斯混合分布。（SVO实现了均匀-高斯分布）
三角化误差来源：
①图像分辨率越高，误差越小，尽可能选择高分辨率相机。
②像素点求取精度时选择亚像素，提高分辨率（亚像素是将像素这个基本单位再进行细分，是比像素还小的单位），具体做法是极线搜索时对像素点进行双线性插值，进行亚像素的优化。
③根据书中公式12.9和12.10可知，平移越大，误差越小，尽可能增加平移。
三、单目稠密代码
深度和距离的区别（参考【2】）：
距离是像素的空间位置到图像平面的距离，也就是相机坐标系下的Z坐标；
深度是指像素的空间位置到相机光心的距离，也就是相机坐标系下坐标的模值(sqrt(x*x+y*y+z*z))
//观测到的深度值u_obs, // Vector3d p_esti
double depth_estimation = p_esti.norm(); // 深度值用200张单目图像估计第一张图像（参考帧）每个像素的深度，已知的信息有201张图像及变换矩阵。
主程序：读取所需数据，然后初始化参考帧的深度图和深度图方差，初始化时认为每个像素值相同。然后循环1-200号图像，不断更新第0幅图像的像素深度分布信息。
1.readDatasetFiles() 读取图片color_image_files、位姿Twc poses 和第一张图像(参考帧)像素点的深度值ref_depth
bool readDatasetFiles(
const string &path,
vector<string> &color_image_files,
std::vector<SE3d> &poses,
cv::Mat &ref_depth) {
ifstream fin(path + "/first_200_frames_traj_over_table_input_sequence.txt");
if (!fin) return false;
while (!fin.eof()) { ///eof()为真表示文件为空 !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(
SE3d(Quaterniond(data[6], data[3], data[4], data[5]),
Vector3d(data[0], data[1], data[2]))
);
if (!fin.good()) break; //good()该方法在没有发生任何错误的时候返回true。该方法也指出的最后一次读取输入的操作是否成功。
}
fin.close();
//cout<<"变换矩阵:"<<poses[0].matrix()<<endl;
// load reference depth 获取第一幅图像的所有像素点的深度信息
fin.open(path + "/depthmaps/scene_000.depth");
ref_depth = cv::Mat(height, width, CV_64F);
if (!fin) return false;
for (int y = 0; y < height; y++)
for (int x = 0; x < width; x++) {
double depth = 0;
fin >> depth;
ref_depth.ptr<double>(y)[x] = depth / 100.0;
}
return true;
}2. 对第一张图像（参考帧）初始化深度图和深度图方差
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); // 深度图方差3.计算其余图像与参考帧的位姿变换关系：
SE3d pose_ref_TWC = poses_TWC[0];
SE3d pose_curr_TWC = poses_TWC[index];
//参考帧到当前帧的位姿变换Tcr 坐标转换关系：Tcr =Tcw * Twr
SE3d pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC; 4.update() 根据参考帧到当前帧的位姿Tcr 更新深度图depth和深度图方差depth_cov2
bool update(const Mat &ref, const Mat &curr, const SE3d &T_C_R, Mat &depth, Mat &depth_cov2) {
for (int x = boarder; x < width - boarder; x++) //按列遍历像素么？why???
for (int y = boarder; y < height - boarder; y++) {
// 遍历每个像素
if (depth_cov2.ptr<double>(y)[x] < min_cov || depth_cov2.ptr<double>(y)[x] > max_cov) // 深度已收敛或发散
continue;
// 在极线上搜索 (x,y) 的匹配
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;
// 取消该注释以显示匹配
// showEpipolarMatch(ref, curr, Vector2d(x, y), pt_curr);
// 匹配成功，更新深度图
updateDepthFilter(Vector2d(x, y), pt_curr, T_C_R, epipolar_direction, depth, depth_cov2);
}
}遍历深度图方差的像素，小于min_cov 认为收敛，大于max_cov认为发散；然后在极线上匹配当前图像与参考帧图像匹配的像素点（epipolarSearch函数），匹配成功后使用三角化确定深度并进行优化（updateDepthFilter函数）
epipolarSearch() 参数：参考帧像素点 pt_ref, 深度图像素值depth_mu,深度图方差像素值depth_cov, 与当前帧匹配的像素点pt_curr，极线方向epipolar_direction
//像素坐标变为归一化坐标
Vector3d f_ref = px2cam(pt_ref);
//归一化：将归一化平面的路标点进行深度值为1的操作,因为实际的深度不是z值，而是三维点到相机光心的距离
f_ref.normalize();
//cout<< f_ref<<endl;
Vector3d P_ref = f_ref * depth_mu; // 参考帧的 P 向量
//将当前帧，也就是相机坐标系坐标投影到像素坐标系中
Vector2d px_mean_curr = cam2px(T_C_R * P_ref); // 按深度均值投影的像素空间点P到相机光心穿过平面上的点组成的直线叫极线，图来源参考【1】，代码中取不同深度值进行重投影，得到两个点以此确定极线
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; // 极线（线段形式）
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 );确认极线后，在极线上以深度均值点px_mean_curr为中心，左右各取半长度寻找匹配点。得到NCC分布后，选择合适的作为匹配点pt_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;计算参考点pt_ref与待匹配点pt_curr的NCC值，公式参考322页12.11
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;
// 计算 Zero mean 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); // 防止分母出现零
}到此极线搜索和块匹配结束，找到了当前帧与参考帧对应的像素点，然后进行三角化更新深度。
updateDepthFilter函数参数：参考帧像素点 Vector2d(x, y)，当前帧匹配的像素点pt_curr, 位姿矩阵 T_C_R, 极线方向epipolar_direction, 更新的深度图depth, 深度图方差depth_cov2
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);
f_ref.normalize();
Vector3d f_curr = px2cam(pt_curr);
f_curr.normalize();
// 公式推导参考下图
// 方程
// 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 ]
Vector3d t = T_R_C.translation(); //平移向量
Vector3d f2 = T_R_C.so3() * f_curr; //旋转向量*归一化坐标
Vector2d b = Vector2d(t.dot(f_ref), t.dot(f2)); //
Matrix2d A;
A(0, 0) = f_ref.dot(f_ref);
A(0, 1) = -f_ref.dot(f2);
A(1, 0) = -A(0, 1);
A(1, 1) = -f2.dot(f2);
Vector2d ans = A.inverse() * b; // A * x = b => x = A^-1 * b
Vector3d xm = ans[0] * f_ref; // d_ref * f_ref
Vector3d xn = t + ans[1] * f2; // d_cur *f2 + t
Vector3d p_esti = (xm + xn) / 2.0; // P的位置，取两者的平均
//观测到的深度值u_obs
double depth_estimation = p_esti.norm(); // 深度值
// 计算不确定性（以一个像素为误差）
Vector3d p = f_ref * depth_estimation; //参考帧某点在相机坐标系坐标
Vector3d a = p - t; //式12.7
double t_norm = t.norm();
double a_norm = a.norm();
//根据点乘方法计算夹角： a点乘b = |a|*|b|*cos角度
double alpha = acos(f_ref.dot(t) / t_norm); //f_ref已经归一化了，模长是1
double beta = acos(-a.dot(t) / (a_norm * t_norm));
Vector3d f_curr_prime = px2cam(pt_curr + epipolar_direction);
f_curr_prime.normalize();
//式12.8
double beta_prime = acos(f_curr_prime.dot(-t) / t_norm);
double gamma = M_PI - alpha - beta_prime;
double p_prime = t_norm * sin(beta_prime) / sin(gamma);
double d_cov = p_prime - depth_estimation;
//观测的方差sigma_obs
double d_cov2 = d_cov * d_cov;
// 高斯融合
//式12.4
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))];
//式12.6
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_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = sigma_fuse2;
return true;
}关于三角化公式参考书177页7.24可知：
此时update函数结束，得到了估计的深度图和深度图方差，然后通过evaludateDepth函数计算均方误差和平均误差，plotDepth函数显示真实的深度图、估计的深度图和带误差的深度图。
void plotDepth(const Mat &depth_truth, const Mat &depth_estimate) {
imshow("depth_truth", depth_truth * 0.4); //322页
imshow("depth_estimate", depth_estimate * 0.4);
imshow("depth_error", depth_truth - depth_estimate); //图片直接相减
waitKey(1);
}深度图显示过程将深度值乘0.4，对于纯白点（数值为1）的深度为2.5m。颜色越深表示深度值越小，物体离我们越近
代码结束。
四、分析与讨论
立体视觉对物理纹理的依赖性：梯度明显的像素，深度信息相对准确；梯度不明显的像素，块匹配没有区分性，很难估计深度。
像素梯度和极线匹配的不确定性是存在联系的，具体表现在：梯度与极线夹角较大，极线匹配不确定性大；夹角较小，极线匹配的不确定性小。
深度的分布可能不是高斯分布，但是深度的倒数(逆深度)比较接近高斯分布。
五、课后题
1.均匀-高斯混合滤波器的原理
SVO单目稠密重建使用了该滤波器，参考高博对SVO的理解【3】 【4】
2.把稠密估计变为半稠密估计
在update函数中修改：对原图进行处理，通过相邻两个像素之间的差分来近似当前像素的梯度值，这里需要注意像素值的数据类型是uchar，不是double。
通过norm()函数求得当前像素梯度的二范数，阈值设为25
Vector2d gradient(ref.ptr<uchar>(y)[x + 1] - ref.ptr<uchar>(y)[x - 1],
ref.ptr<uchar>(y + 1)[x] - ref.ptr<uchar>(y - 1)[x]);
if (gradient.norm() < 25)
continue;
对比试验结果：半稠密计算速度很快，但是均方差变大
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
