视觉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<<angle<<endl;vector<string> color_image_files;vector<SE3d> 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 imageSE3d 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<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);}
}

要完成上述过程，首先是要进行图像匹配，我们使用极限约束及块匹配。这里的计算过程中有一些地方要注意：

（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;// 计算待匹配点与参考帧的 NCCdouble 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<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 NCCdouble 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<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);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;
}

这段代码的其他部分都比较简单了，基本就是按照公式进行计算，找好对应的公式就很简单了。最后更新先验信息：

//融合先验信息
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<double>(y)[x] - depth_estimate.ptr<double>(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;
}

其余的绘图等较为简单不说了，针对单目稠密重建的结果书中也有相关讨论，包括“像素梯度问题”、“逆深度”以及效率问题，书中讲得很清晰，这里不再赘述。

视觉SLAM十四讲学习笔记——第十二讲建图相关推荐

python学习笔记分享(二十四)python学习笔记分期补充(二)复数，randint与sample，进制转换表，转义字符，二维数组，键，end，pass，迭代器和生成器
一:复数 Python支持复数,复数由实数部分和虚数部分构成,可以用a + bj,或者complex(a,b)表示, 复数的实部a和虚部b都是浮点型. complex(x) 将x转换到一个复数,实数部 ...
SLAM14讲学习笔记（十五）卡尔曼滤波器的直观理解
之前在SLAM14讲学习笔记(六)后端(最难一章:卡尔曼滤波器推导.理解以及扩展)中,介绍了卡尔曼滤波器的推导. 但是感觉不太直观,因此这次用了几个简单的图,希望能一目了然卡尔曼滤波器是在干什么. 先 ...
视觉SLAM十四讲学习笔记——第十三讲实践：设计SLAM系统
1.如何运行示例代码首先是如何运行示例代码,这里遇到了很多问题: (1)首先要下载Kitti数据集,并在config/default.yaml文件内修改路径. (2)安装Glog.GTest.GFl ...
MySQL实战45讲学习笔记：第七讲
一.两阶段锁 1.持有哪些锁,以及在什么时候释放我先给你举个例子.在下面的操作序列中,事务 B 的 update 语句执行时会是什么现象呢? 假设字段 id 是表 t 的主键. 这个问题的结论取决于 ...
FPGA学习笔记（十二）IP核之FIFO的学习总结
系列文章目录一.FPGA学习笔记(一)入门背景.软件及时钟约束二.FPGA学习笔记(二)Verilog语法初步学习(语法篇1) 三.FPGA学习笔记(三) 流水灯入门FPGA设计流程四.FPGA ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-三角测量和实践
专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-对极几何和对极约束、本质矩阵、基础矩阵
专栏系列文章如下: 专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLA ...
视觉SLAM十四讲学习笔记-第六讲学习笔记总结（1）---非线性优化原理
第六讲学习笔记如下: 视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_ ...
视觉SLAM十四讲学习笔记-第四讲---第五讲学习笔记总结---李群和李代数、相机
第四讲---第五讲学习笔记如下: 视觉SLAM十四讲学习笔记-第四讲-李群与李代数基础和定义.指数和对数映射_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模 ...

视觉SLAM十四讲学习笔记——第十二讲建图

视觉SLAM十四讲学习笔记——第十二讲建图相关推荐

最新文章

热门文章

视觉SLAM十四讲学习笔记——第十二讲 建图

视觉SLAM十四讲学习笔记——第十二讲 建图相关推荐

最新文章

热门文章

视觉SLAM十四讲学习笔记——第十二讲建图

视觉SLAM十四讲学习笔记——第十二讲建图相关推荐