视觉理论与实践7-后端

一、Bundle Adjustment

1.1 文献阅读

我们在第五讲中已经介绍了Bundle Adjustment，指明它可以⽤于解PnP 问题。现在，我们又在后端中说明了它可以⽤于解⼤规模的三维重构问题，但在实时SLAM 场合往往需要控制规模。事实上，Bundle Adjustment 的历史远⽐我们想象的要长。请阅读Bill Triggs 的经典论⽂Bundle Adjustment: A Modern Synthesis（见paper/⽬录）1，了解BA 的发展历史，然后回答下列问题：
1. 为何说Bundle Adjustment is slow 是不对的？
BA在slam后端常用的图优化框架里起到了核心的作用。人们认为BA处理起来很缓慢，因为人们在使用一些BA通用例程时往往忽略了它的稀疏性，但它本身的模型具有稀疏性，基于这一点特征，可对后端进行加速，如采用舒尔补方法将原本一个大的稀疏的矩阵变成几个小的对角矩阵的运算。这也是为什么BA经历了40年还一直被人们使用，所以说Bundle Adjustment is slow是不对的。

2. BA 中有哪些需要注意参数化的地⽅？Pose 和Point 各有哪些参数化⽅式？有何优缺点。
总结一下BA过程首先选择你想要的图里的节点与边的类型，确定它们的参数化形式；
1)往图里加入实际的节点和边；
2) 选择初值，开始迭代；
3) 每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；
4) 求解稀疏线性方程HkΔx=−bk，得到梯度方向；
5) 继续用GN或LM进行迭代。如果迭代结束，返回优化值。

Pose和point参数化方式有四元素，欧拉角，变换矩阵。
1）变换矩阵： 优点：旋转轴可以是任意向量 缺点：旋转其实只需要知道一个向量+一个角度(共4个信息值)，但矩阵却用了16个元素(消耗时间和内存)
2）欧拉角 优点：容易理解，形象直观；表示更方便，只需要三个值(分别对应x、y、z轴的旋转角度) 缺点：欧拉角这种方法是要按照一个固定的坐标轴的顺序旋转的，因此不同的顺序会造成不同结果；欧拉角旋转会造成万向锁现象，这种现象的发生就是由于上述固定的坐标轴旋转顺序造成的。由于万向锁的存在，欧拉旋转无法实现球面平滑插值。
3）四元数 优点：可以避免万向锁；只需要一个4维的四元数就可以执行绕任意过原点的向量的旋转，方便快捷，在某些实现下比旋转矩阵效率更高；而且四元数旋转可以提供平滑插值。 缺点：比欧拉旋转稍微复杂了一点，因为多了一个维度，理解更困难，不直观。

3. 本⽂写于2000 年，但是⽂中提到的很多内容在后⾯⼗⼏年的研究中得到了印证。你能看到哪些⽅向在后续⼯作中有所体现？请举例说明。
本书提到的第五小节提到了“Network Structure”，如下图所示：
“图优化”也就是基于本文提到的“网络图”，在现在的SLAM研究中，图优化几乎代替了以往的滤波算法。

1.2 BAL-dataset
BAL（Bundle Adjustment in large）数据集是⼀个⼤型BA 数据集，它提供了相机与点初始值与观测，你可以⽤它们进⾏Bundle Adjustment。现在，请你使⽤g2o，⾃⼰定义Vertex 和Edge（不要使⽤⾃带的顶点类型，也不要像本书例程那边调⽤Ceres来求导），书写BAL 上的BA 程序。你可以挑选其中⼀个数据，运⾏你的BA，并给出优化后的点云图。
本题不提供代码框架，请独⽴完成。提⽰：

1. 注意BAL 的投影模型⽐教材中介绍的多了个负号；

本题下载了多个数据集，选用problem-16-22106-pre.txt.bz2,解压之后得到problem-16-22106-pre.txt文件，如下图所示：

代码框架参考高博视觉SLAM十四讲第10讲g2o的代码。
Problem-16-22106-pre.txt数据集实现结果如下图所示：

1. 终端截图如下：
   
   2)meshlab打开initial.ply如下：
   
2. meshlab打开final.ply如下图所示：
   
   经过了多次优化迭代，从初始和最终的点云地图（ply文件）可以看出，BA起到了一定的优化效果

二、直接法的Bundle Adjustment

2.1 数学模型

1. 如何描述任意一点投影在任意一图像中形成的 error?
   代码描述如下：

_error[num]=origColor[num]-GetPixelValue(targetImg,h+i,v+j);

2)每个 error 关联几个优化变量?

3）error 关于各变量的雅可比是什么?

代码片段如下：
第一项：

Matrix<double,1,2> J_I_Puv; for(int i = -2; i<2; i++) for(int j = -2; 
j<2; j++) { int num = 4 * i + j + 10;//0-15 //像素梯度，即前后做差除以2 
J_I_Puv(0,0) = (GetPixelValue(targetImg,u+i+1,v+j) - GetPixelValue(targetImg,u+i-1,v+j))/2; 
J_I_Puv(0,1) = (GetPixelValue(targetImg,u+i,v+j+1) - GetPixelValue(targetImg,u+i,v+j-1))/2;// 
_jacobianOplusXi.block<1,3>(num,0) = -J_I_Puv * J_Puv_Pc * vertexTcw->estimate().rotation_matrix();
 //地图点J 
 _jacobianOplusXj.block<1,6>(num,0) = -J_I_Puv * J_Puv_Pc * J_Pc_kesi;//位姿J 
 }

第二项：

//u，v分别对x,y,z求导 
Matrix<double,2,3> J_Puv_Pc; 
J_Puv_Pc(0,0) = fx * inv_z; 
J_Puv_Pc(0,1) = 0; 
J_Puv_Pc(0,2) = -fx * x * inv_z2; 
J_Puv_Pc(1,0) = 0; J_Puv_Pc(1,1) = fy * inv_z; 
J_Puv_Pc(1,2) = -fy * y * inv_z2;

第三项：

Matrix<double,3,6> J_Pc_kesi = Matrix<double,3,6>::Zero(); J_Pc_kesi(0,0) = 1; 
J_Pc_kesi(0,4) = z; 
J_Pc_kesi(0,5) = -y; 
J_Pc_kesi(1,1) = 1; 
J_Pc_kesi(1,3) = -z; 
J_Pc_kesi(1,5) = x; 
J_Pc_kesi(2,2) = 1;
J_Pc_kesi(2,3) = y; 
J_Pc_kesi(2,4) = -x;

将三个雅克比连乘，最后得到一个1x6的雅克比矩阵，即最终的雅克比矩阵。

2.2 数学模型
1)计算误差代码片段:

const VertexSBAPointXYZ* vertexPw=static_cast<const VertexSBAPointXYZ*> (vertex(0)); 
const VertexSophus* vertexTcw=static_cast<const VertexSophus*>(vertex(1)); 
Eigen::Vector3d Pc=vertexTcw->estimate()*vertexPw->estimate(); 
float u=Pc[0]/Pc[2]*fx+cx; 
float v=Pc[1]/Pc[2]*fy+cy; 
if(u-3<0||(u+2)>w||(v-3)<0||(v+2)>h) 
{ 
_error(0,0)=0.0;
this->setLevel(1); 
} 
else
{ 
	for(int i=-2;i<2;i++) 
	{ 
		for(int j=-2;j<2;j++) 
		{ 
		int num=4*i+j+10;//0-15 _error[num]=origColor[num]-GetPixelValue(targetImg,h+i,v+j); 
		}
	} 
}

2. 在图优化中计算顶点和边代码片段：

typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;
 // pose 维度为 6, landmark 维度为 3 
 //第1步：创建一个线性求解器LinearSolver
Block::LinearSolverType*linearSolver=new g2o::LinearSolverDense<Block::PoseMatrixType>(); 
//第2步：创建BlockSolver。并用上面定义的线性求解器初始化 Block* solver_ptr = new Block ( linearSolver ); 
//第3步：创建总求解器solver 
g2o::OptimizationAlgorithmLevenberg*solver=new g2o::OptimizationAlgorithmLevenberg (solver_ptr ); 
// 第4步：创建稀疏优化器 g2o::SparseOptimizer optimizer; 
optimizer.setAlgorithm ( solver ); //添加点 for(int i=0;
i<points.size();i++) 
{
VertexSBAPointXYZ* vertexPw=new VertexSBAPointXYZ();
vertexPw->setEstimate(points[i]); 
vertexPw->setId(i);
vertexPw->setMarginalized(true); 
optimizer.addVertex(vertexPw); 
} //添加边 
for(int j=0;j<poses.size();j++) 
{ 
VertexSophus* vertexTcw=new VertexSophus(); vertexTcw->setEstimate(poses[j]); 
vertexTcw->setId(j+points.size()); 
optimizer.addVertex(vertexTcw); 
} 
for(int c=0;c<poses.size();c++) 
for(int p=0;p<points.size();p++) 
{ 
EdgeDirectProjection* edge=new EdgeDirectProjection(color[p],images[c]); 
edge->setVertex(0,dynamic_cast<VertexSBAPointXYZ*>(optimizer.vertex(p))); 
edge→setVertex(1,dynamic_cast<VertexSophus*>(optimizer.vertex(c+points.size()))); 
edge->setInformation(Eigen::Matrix<double,16,16>::Identity());
RobustKernelHuber* rk = new RobustKernelHuber(); 
rk->setDelta(1.0); 
edge->setRobustKernel(rk); 
optimizer.addEdge(edge); 
}

运行截图如下：

友情提示：代码下载需要C币，请事先判断是否对您有帮助，谨慎下载哦！！！