使用单目相机进行地图构建 你学会了吗
1.前言
知识干货免费分享
http://www.makeru.com.cn/video/2099_11457.html?s=144282
经过我们连续超长时间的学习，我们终于可以到介绍建图问题了！
之前我们一致在介绍SLAM系统的前三大组成部分，前端估计、后端估计、回环检测，那么，第四大部分，建图问题，就从本文开始介绍，这也是SLAM系统中基础环节的最后一个问题。
试想一下，对于基于人工设计的特征点，它构建的地图是怎么样的呢？
由于特征点的稀疏性，这会导致SLAM地图构建的稀疏性，对于稀疏的地图，在一些场景下是可以使用的，比如大致的路线规划，但是，在大多数场景下，我们还是希望构建一个不是那么稀疏的地图，即稠密地图或者半稠密地图。
2.单目相机建图
对于建图过程，首先需要特征匹配，就是可以使用之前讲解的BA优化求解问题，对于特征匹配的算法，通常可以使用如下2种方法：
极线搜索；
块匹配。
对于不同的相机，要获得像素点的深度信息，要怎么办呢？
通常情况下，有如下几种方案：
双目相机提供立体匹配得出深度图；
单目相机通过三角化提供深度图；
深度相机直接提供深度信息。
这里，我们使用单目相机构建地图为例进行讲解，当然，单目相机构建深度地图有着很多缺点，得出的深度图还不一定可靠，但是，通过这个例子，大家可以很容易的实现并且理解整个系统和方法。
三角化代码：
// 方程
2// d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC
3// => [ f_ref^T f_ref, -f_ref^T f_cur ] [d_ref] = [f_ref^T t]
4// [ f_cur^T f_ref, -f_cur^T f_cur ] [d_cur] = [f_cur^T t]
5// 二阶方程用克莱默法则求解并解之
6Vector3d t = T_R_C.translation();
7Vector3d f2 = T_R_C.rotation_matrix()* f_curr;
8Vector2d b =Vector2d( t.dot ( f_ref ), t.dot ( f2 ));
9double A[4];
10 A[0]= f_ref.dot ( f_ref );
11 A[2]= f_ref.dot ( f2 );
12 A[1]=-A[2];
13 A[3]=- f2.dot ( f2 );
14double d = A[0]A[3]-A[1]A[2];
15Vector2d lambdavec =
16Vector2d( A[3] b (0,0)- A[1] b (1,0),
17-A[2]* b (0,0)+ A[0]* b (1,0))/d;
18Vector3d xm = lambdavec (0,0)* f_ref;
19Vector3d xn = t + lambdavec (1,0)* f2;
20Vector3d d_esti =( xm+xn )/2.0;// 三角化算得的深度向量
21double depth_estimation = d_esti.norm();// 深度值
3.极线搜索与块匹配
如上图所示，我们之前讲解过这方面的概念，相信大家也很熟悉了，通过极限约束我们可以将一个搜索问题从二维变为一维，这会大大减少计算量。
简单的来讲，一个图像的特征点我们可以通过比较搜索另一个图像的相似特征点而不用特征匹配，就可以简单的比较光度误差就可以了，比如直接法就是直接比较光度误差，这样的话就比较快，但是，光度误差只有灰度值，单一的比较大小会存在很多误差，比如光照变化等等，如何让搜索的误差减少呢？
这就需要块匹配的方法。
何为块匹配呢，就是在极线搜索的时候不在使用一个像素点对比，而是使用一块像素块来对比。这避免了单一像素对比时出现的问题。
有如下几种计算差异的办法：
SAD
SSD
NCC
但是，在搜索的时候可能会出现非凸函数，这就需要一个滤波器或者非线性优化来求解。
4.滤波器
在衡量深度是否估计正确，需要一个不确定度的概率模型来衡量，不断优化整个不确定度，就得到了最优的深度值。
假设深度值服从高斯分布。
a = p - t
α = arccos
β = arccos
δβ=arctan1/f
β’= β-δβ
γ=π-α -β
σobs=||p||-||p’||
有δβ的像素误差，就会造成p和p‘之间的差异。两个深度向量的1范数之差定义为σobs，作为一个像素误差带来的深度不确定度。
5.实践
主函数：
int main(int argc,charargv){
if(argc !=2){
cout <<“Usage: dense_mapping path_to_test_dataset”<< endl;
return-1;
}
// 从数据集读取数据
vector color_image_files;
vector poses_TWC;
Mat ref_depth;
bool ret = readDatasetFiles(argv[1], 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_cov2 =3.0;// 方差初始值
Mat depth(height, width, CV_64F, init_depth);// 深度图
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;
return0;
}
一些参数：
// parameters
constint boarder =20;// 边缘宽度
constint width =640;// 图像宽度
constint height =480;// 图像高度
constdouble fx =481.2f;// 相机内参
constdouble fy =-480.0f;
constdouble cx =319.5f;
constdouble cy =239.5f;
constint ncc_window_size =3;// NCC 取的窗口半宽度
constint ncc_area =(2* ncc_window_size +1)(2 ncc_window_size +1);// NCC窗口面积
constdouble min_cov =0.1;// 收敛判定：最小方差
constdouble max_cov =10;// 发散判定：最大方差
/@@**
*根据新的图像更新深度估计
*@param ref 参考图像
*@param curr 当前图像
*@param T_C_R 参考图像到当前图像的位姿
*@param depth 深度
*@param depth_cov 深度方差
@return是否成功
/
bool update(
constMat&ref,
constMat&curr,
const SE3d &T_C_R,
Mat&depth,
Mat&depth_cov2
);
/@@
*极线搜索
*@param ref 参考图像
*@param curr 当前图像
*@param T_C_R 位姿
*@param pt_ref 参考图像中点的位置
*@param depth_mu 深度均值
*@param depth_cov 深度方差
*@param pt_curr 当前点
*@param epipolar_direction 极线方向
@return是否成功
/
bool epipolarSearch(
constMat&ref,
constMat&curr,
const SE3d &T_C_R,
constVector2d&pt_ref,
constdouble&depth_mu,
constdouble&depth_cov,
Vector2d&pt_curr,
Vector2d&epipolar_direction
);
/@@
*更新深度滤波器
*@param pt_ref 参考图像点
*@param pt_curr 当前图像点
*@param T_C_R 位姿
*@param epipolar_direction 极线方向
*@param depth 深度均值
*@param depth_cov2 深度方向
*@return是否成功
*/
bool updateDepthFilter(
constVector2d&pt_ref,
constVector2d&pt_curr,
const SE3d &T_C_R,
constVector2d&epipolar_direction,
Mat&depth,
Mat&depth_cov2
);
6.小结
这是一个简单的例子，在实际场景中，还有很多的复杂性，这也是如何减少这方面误差称为一个研究热点，也有很多方法的而提出，比如在滤波器优化时，有均值-高斯分布的假设，而不简简单单时高斯分布的假设。
免费课程提供给大家学习哈，想看的小伙伴们赶紧行动起来了
http://www.makeru.com.cn/video/2100_11463.html?s=144282
