视觉SLAM十四讲学习笔记——第十二讲 建图
SLAM实际上是一种底层技术，往往用来给上层应用提供信息。在前文已实现的部分，我们最多得到的仅是一个稀疏的地图，在需要实现导航、避障、重建等应用时，稀疏地图很难提供足够的信息，需要考虑如何重建稠密地图，即建模所有看到过的部分。
根据使用相机的不同可以分为：“单目稠密重建”和“RGB-D稠密建图”（双目应该和RGBD类似，都可以主动测得或计算出图像深度）。其中RGB-D相机的稠密重建较为简单，最直观、简单的方法就是将RGB-D数据转换为点云地图，再结合相机位姿进行拼接，并对点云加一些滤波处理。这里使用了PCL点云库和两种滤波方式：
（1）外点去除滤波器：由于Kinect量程有限，去掉深度值无效的点。
（2）体素网格的降采样滤波器：多视角点云拼接时，由于视野重叠，重叠区域会有大量位置十分相近的点，体素滤波器保证一定大小的体素内仅有一个点，相当于对三维空间降采样。
点云地图属于比较初级的地图形式，我们可以在点云的基础上重建网格，同时还可以实现了常用于导航中的“八叉树地图”，这里使用octomap库可以简单地生成八叉树地图。最后介绍了一些常见的地图类型。
相比于RGB-D相机的稠密重建，单目相机的稠密重建较为复杂，因为不能主动测得图像深度，我们需要匹配相邻的两幅图像，并根据三角重建原理自行计算图像点的深度（视觉里程计讲到过）。因此给出了单目稠密重建的详细理论以及代码示例。
1.理论分析
单目相机估计稠密深度的完成过程如下：
（1）初始假设：所有像素深度都满足某一高斯分布（不是同一个）。
（2）通过极线搜索和块匹配确定重投影点的位置：极线搜索是根据已知的两次拍摄相机间的位置关系将一个点的匹配搜索范围从整幅图像缩小为一根直线；此时匹配仅能借助亮度不变性进行区分，但是由于单个点的亮度区分度不大，改为“图像块的灰度不变性”，设定评价标准（SAD、SSD、NCC等），提高匹配的准确度。
（3）根据几何关系计算三角化后的深度（均值）及不确定性（协方差矩阵）。
（4）融合上一次的估计值（高斯分布的乘积），若收敛则停止，认为估计到较为准确的深度分布信息，否则返回第二部。
了解了理论分析部分，再来看给出的非常复杂的示例代码。
2.代码分析
分析代码前要明确这段代码在做什么，示例程序使用开源数据集，共200张无人机采集的单目俯视图像，现在要利用这些图像估计第一帧图像（0号图像）每个像素的深度，即单目稠密重建。我们已知的信息有201张图像及其对应相机的外部参数（旋转和平移）。
先看主程序：首先是读取所有所需数据，然后初始化第0幅图像（0号）的深度图（高斯分布的矩阵）和不确定图像（高斯分布方差），初始化时认为每个像素的分布参数相同，注意不能为0，否则没办法更新了。初始化后循环1-200号图像，不断更新第0幅图像的像素深度分布信息。
int main(int argc, char **argv) {
// 从数据集读取数据
Vector3d test;
test<<2,2,2;
double angle=test.dot(test);
cout< color_image_files;
vector poses_TWC;
Mat ref_depth;
bool ret = readDatasetFiles("/home/svv12138/Downloads/slambook2-master/ch12/dense_mono/remode_test_data/test_data", color_image_files, poses_TWC, ref_depth);
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
SE3d pose_ref_TWC = poses_TWC[0];//初始图像位姿
double init_depth = 3.0; // 深度初始值
//double init_depth_inv = 0.3; // 深度逆初始值
double init_cov2 = 3.0; // 方差初始值
Mat depth(height, width, CV_64F, init_depth); // 深度图
//Mat depth(height, width, CV_64F, 1/init_depth_inv);
Mat depth_cov2(height, width, CV_64F, init_cov2); // 深度图方差
//从第二张开始循环每一张图像用以估计第一张图像的深度
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];
//参考图像到当前图像的位姿
SE3d 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_cov2);
//融合先验信息
evaludateDepth(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;
return 0;
} 接下来关注里面的几个重点函数，首先是 update函数：此函数的目的是利用已知相对位姿的当前图像计算参考图像的深度和深度不确定度信息。
/**
* 根据新的图像更新深度估计
* @param ref 参考图像
* @param curr 当前图像
* @param T_C_R 参考图像到当前图像的位姿
* @param depth 深度
* @param depth_cov 深度方差
* @return 是否成功
*/
bool update(
const Mat &ref,
const Mat &curr,
const SE3d &T_C_R,
Mat &depth,
Mat &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++)
for (int y = boarder; y < height - boarder; y++) {
// 深度已收敛或发散
if (depth_cov2.ptr(y)[x] < min_cov || depth_cov2.ptr(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(y)[x],
sqrt(depth_cov2.ptr(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);
}
} 要完成上述过程，首先是要进行图像匹配，我们使用极限约束及块匹配。这里的计算过程中有一些地方要注意：
（1）坐标归一化的目的：px2cam函数得到的是像素点在相机归一化平面上的坐标（Z坐标为1）因为我们不知道深度信息，这里要区分“深度”和“距离”是有区别的，我认为“距离”应该是像素的空间位置到图像平面的距离，也就是相机坐标系下的Z坐标；而“深度”是指像素的空间位置到摄像机光心的距离，也就是相机坐标系下坐标的模值。这里我们计算的depth，更应该是“深度”含义，也就理解了为什么要对坐标进行归一化normalize（对向量除以模值）。没必要纠结我对他们的命名，主要是理解物理含义。
// 像素到相机坐标系 归一化平面
inline Vector3d px2cam(const Vector2d px) {
return Vector3d(
(px(0, 0) - cx) / fx,
(px(1, 0) - cy) / fy,
1
);
}（2）极线方向的计算：极线方程本身是有计算公式的，如下图（出自我的本科毕业论文）
但是这里使用了另一种方法，我们知道“空间点到光心的直线上所有点都在同一极线上”，因此我们取两个不同深度值（以均值为中心左右各取3倍标准差为半径，取最大值和最小值）做重投影，两点确定一条直线，也就得到了极线段。
（3）块匹配：使用了去均值的NCC，公式在P322，代码实现比较简单。
// 极线搜索
// 方法见书 12.2 12.3 两节
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);
//归一化
f_ref.normalize();
//乘以深度，得到相机坐标系下的坐标
Vector3d P_ref = f_ref * depth_mu; // 参考帧的 P 向量
// 按深度均值投影的像素
Vector2d px_mean_curr = cam2px(T_C_R * P_ref);
//以均值为中心左右各取3倍标准差为半径
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 );
// 在极线上搜索，以深度均值点为中心，左右各取半长度
//归一化互相关结果
double best_ncc = -1.0;
Vector2d best_px_curr;
// 步长设置 l+=sqrt(2)
for (double l = -half_length; l <= half_length; l += 0.7) {
// 待匹配点坐标
Vector2d px_curr = px_mean_curr + l * epipolar_direction;
//检测是否在边界范围内
if (!inside(px_curr))
continue;
// 计算待匹配点与参考帧的 NCC
double ncc = NCC(ref, curr, pt_ref, px_curr);
//越接近1越相似
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 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(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); // 防止分母出现零
} 匹配过程中还有一些点没有完成匹配，不过会在后续的新图像中得到补充。匹配完成后，就要计算深度及深度不确定度了。原理分析在P313，主要看一下代码实现，整个过程基本就是三角重建原理，整个计算过程基本就是按照P313的公式计算，最让我不理解的反倒是三角化的问题。
在第七章的7.5讲三角测量给出了完成匹配后如何计算深度，公式在P177-178。这里的代码最难理解的反倒是作者的注释：
// 方程
// 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 ]
第一个方程很好解释，就是公式7.24：s2*x2=s1*R*x1+t。而第二步完全是定义了一个变量替代。而这个方程的求解就很值得思考了，以下是我的推导：
这部分内容在P178也有体现“由于噪声存在，很难使得等式精确为零，常使用最小二乘解”
说了半天其实就是想证明，他求解三角化问题的方式不是毫无根据的，而是一个求解最小二乘问题的方法。
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;
Vector3d xm = ans[0] * f_ref; // ref 侧的结果
Vector3d xn = t + ans[1] * f2; // cur 结果
Vector3d p_esti = (xm + xn) / 2.0; // P的位置，取两者的平均
double depth_estimation = p_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));
Vector3d f_curr_prime = px2cam(pt_curr + epipolar_direction);
f_curr_prime.normalize();
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;
double d_cov2 = d_cov * d_cov;
// 高斯融合
double mu = depth.ptr(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];
double sigma2 = depth_cov2.ptr(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(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = mu_fuse;
depth_cov2.ptr(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = sigma_fuse2;
return true;
} 这段代码的其他部分都比较简单了，基本就是按照公式进行计算，找好对应的公式就很简单了。最后更新先验信息：
//融合先验信息
void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate) {
double ave_depth_error = 0; // 平均误差
double ave_depth_error_sq = 0; // 平方误差
int cnt_depth_data = 0;
for (int y = boarder; y < depth_truth.rows - boarder; y++)
for (int x = boarder; x < depth_truth.cols - boarder; x++) {
double error = depth_truth.ptr(y)[x] - depth_estimate.ptr(y)[x];
ave_depth_error += error;
ave_depth_error_sq += error * error;
cnt_depth_data++;
}
ave_depth_error /= cnt_depth_data;
ave_depth_error_sq /= cnt_depth_data;
cout << "Average squared error = " << ave_depth_error_sq << ", average error: " << ave_depth_error << endl;
} 其余的绘图等较为简单不说了，针对单目稠密重建的结果书中也有相关讨论，包括“像素梯度问题”、“逆深度”以及效率问题，书中讲得很清晰，这里不再赘述。