BA模型 第10章

1.BA模型
BA模型就是世界坐标到像素坐标的转换过程。这里多了一个去畸变。因为归一化平面坐标在转成像素坐标的过程中会出现畸变。这里只处理了径向畸变，径向畸变包括桶形失真和枕形失真，都是由于图像放大率随着与光轴之间的距离改变而增大或减小造成的。切向畸变是由于透镜与成像平面没有严格平行造成的。处理径向畸变就是拿归一化平面坐标比如uc,vc乘以（1+k1*rc*rc+k2*rc*rc*rc*rc)得到的，这里的k1,k2是外参，rc=sqrt(uc*uc+vc*vc).而切向畸变就是用到uc*vc了。
世界坐标P到相机坐标P'
P'=Rp+t=[X',Y',Z'].t
相机坐标到归一化坐标
[uc,vc,1].t=[X'/Z',Y'/Z',1].t
归一化坐标去畸变
uc'=uc(1+k1*rc*rc+k2*rc*rc*rc*rc)
vc'=vc(1+k1*rc*rc+k2*rc*rc*rc*rc)
归一化坐标uc',vc'到像素坐标us,vs
us=fx*uc'+cx
vs=fy*vc'+cy
观测方程中，z=h(x,y),x就是相机位姿，y就是路标，即这里的三维点p.求得的z就是观测数据z,也就是像素坐标[us,vs].t.所以h(x,y)也可以写成h(ep,p)
而误差就为e=z-h(x,y)=z-h(ep,p)
那BA模型就是在位姿epi处观测到了路标pj，产生观测数据zi,j.
就是1/2||zi,j-h(epi,pj)||的平方。
2.求解BA
BA的求解简单。用非线性优化方向的话，就是要求一个deltax了。
也就是Hdeltax=g
如果是高斯牛顿法的话，就是
J(x).t*J(x)=-J(x).t*f(x)
这里的J(x)是f(x)对x的导数
令x={ep1,ep2,...,epm,p1,p2,...,pn}.t
那么原来的BA模型就可以定义为f(x)
令xc=[ep1,ep2,..,epm].t
xp=[p1,p2,..,pm].t
F为误差函数对位姿的导数，E为误差函数对路标点的导数，一个为2*6矩阵，du/dp*[I,-P的反对称矩阵】一个是2*3矩阵，du/dp*R。
那么
1/2||f(x+delatx||的平方=1/2||eij+F*deltaxc+E*deltaxp||的平方
那么J=[F,E]
如果高斯牛顿，H=J(x).t*J(x)
如果列文伯格-马夸而特的话H=J(x).t*J(x)+lamda*I
以高斯牛顿为例
H=[H1,H2].t
H1=[F.t*F,F.t*E]
H2=[E.t*F,E.t*E]
其实是要解方程，H*delatx=g.如果直接求Ｈ矩阵的逆，非常麻烦，但H是有特殊结构的。也就是它的稀疏性。
3.稀疏性和边缘化
因为eij只和epi,pj有关，跟其他的位姿和路标点无关。所以
Jij=[0,0,..,epi,0,..,0,pj,0,...,0】
那么Jij只在i,i+j处有非零块，而Jij.t*Jij只是(i,i)处，(i,i+j),(i+j,i),(i+j,i+j)处有非零块。
而Ｈ就为所有的Jij.t*Jij的和。
那么如果把Ｈ按照F.t*F,F.t*E,E.t*F,E.t*E来分，H11=F.t*F，H22=E.t*E。那么H11,H22就是对角阵，只在对角线处有非零块来着。而且H11比较小，H22则比较大。
而H12=F.t*E,H21=E.t*F，可能为稀疏阵也可能为稠密阵，视具体的观测矩阵来定。
邻接矩阵：比如说W.Wij,如果节点i和节点j存在一条边，那么Wij=1,否则为0.
这是图论中的内容，而H当中的H12,H21确实是这样的。如果它们当中存在非零块，说明这个非零块对应的两个变量在图中存在着一条边，也就是位姿epi和路标pj存在着一条边，这可以称之为约束。
H矩阵又称为镐头矩阵和箭头形矩阵，因为它的形状很像箭头和一把镐子。
3.1Schur消元
在slam称之为边缘化(Marginalization)
把H看成是B,E,E.t,C组成的矩阵。其中B其实就是F.t*F,C其实就是E.t*E.此E非彼E.
Hdeltax=g可以分解为
B*deltaxc+E*deltaxp=v
E.t*deltaxc+C*deltaxp=w
其实就是对H矩阵求逆比较难，但是对它里面的对角线矩阵B,C求逆简单啊。这里消的是deltaxp.
把第二个等式两边同乘E*C.n
则
E*C.n*E.t*deltaxc+E*deltaxp=E*C.n*w
两个相减得
（B-E*C.n*E.t)*deltaxc=v-E*C.n*w
因为C.n比较容易求，而B比较小，B-E*C.n*E.t的逆也比较好求，比较困惑的是B-E*C.n*E.t也不是对角线矩阵啊。同样是要对矩阵求逆啊。
deltaxp也可以很容易得出。
记S=B-E*C.n*E.t,则S*deltaxc=v-E*Cn*w
而S矩阵的稀疏性是这么决定的，S矩阵的非对角线的上的非零对角块，表明两个相机之间有共同观测的部分，也称共视。
比如S14,S24不为0，表明相机1,2和相机4有共同观测部分。
S矩阵取决于实际观测的结构，是无法提取预知的。
ORB_SLAM在local_mapping环节，刻意选择具有共同观测的帧作为关键帧，这样S矩阵是稠密矩阵。DSO，OKVIS等用了滑动窗口法，对每一帧都要做一次BA，所以要用一些技巧保持S矩阵的稀疏性。
之所以称为边缘化是从概率的角度讲，求deltaxc,deltaxp变成了
P(xc,xp)=P(xp|xc)*P(xc)
也可以用cholesky分解来进行消元。也可以选择一部分进行消元。
4.鲁棒核函数
问题：如果出现误匹配，会出现一条误差很大的边，算法会专注于调节一个错误的值。
解决：鲁棒核函数。给二范数度量加一个限制 sigma,保证每条边的误差不会太大而掩盖其他的边。
常见的Huber核
H(e)，变量是e.
当|e|<=sigma的时候，H(e)=1/2*e*e
其他，H(e)=sigma(|e|-1/2sigma)
ta是光滑连续的。当|e|=sigma的时候，H(e)=1/2*sigma*sigma,另外一个分段也趋向于它。
除了Huber核之外，还有Cauchy核，Tukey核。
5.实践
我们只需要设置BA问题，然后设置Schur消元，然后调用稠密或稀疏矩阵求解器对变量进行优化就可以了。
g2o/ceres都可以用来做BA.

6.BA实际求解 ，g2o
6.1数据信息
data/problem-16-22106-pre.txt
第一行：相机数量，路标数量，观测数据数量
这里是 16 22016 83718
从第二行开始，第i个相机观测j路标所得到的像素坐标
6.2求预测值得几个h文件
6.2.1 rotation.h
这里面定义了轴角和四元数，空间点经轴角旋转之后得到的坐标函数。具体实现看轴角和四元数和空间点.note
这里的轴角变成了angle_axis=[nx,ny,nz].t*theta
（1）轴角变成四元数
那么[q1,q2,q3]=[a0,a1,a2]*sin(theta/2)/theta
这里theta=sqrt(a0*a0+a1*a1+a2*a2)
定义sin(theta/2)/theta为k
当角不为0的时候，如常计算k.
当角趋近于0的时候,k趋近于0.5.
（2）四元数变轴角
[a0,a1,a2]=[q1,q2,q3]*theta/sin(theta/2)
定义theta/sin(theta/2)为k
这里sin(theta/2)为sqrt(q1*q1+q2*q2+q3*q3)
阈值用sin(theta/2)的平方。
如果不为0，theta为2*arctan(-sin(theta/2),-q0)或者2*arctan(sin(theta/2),q0).
如果为0，k直接趋近于2.
（3）求旋转后的点，用轴角算的。

这里theta=sqrt(a0*a0+a1*a1+a2*a2),用theta的平方做了一个阈值判断。
如果不为0.计算w=[a0,a1,a2]/theta,也就是n.
计算w cross pt结果为w_cross_pt.(1-cos(theta))*w*pt为tmp.tmp是一个数值来着。
result[0]=cos(theta)*pt[0]+sin(theta)*w_cross_pt[0]+w[0]*tmp
如果为0.
cos(theta)=1,sin(theta)~theta
所以R=I+hat(w)*theta=I+hat(angle_axis)
令w_cross_pt为angle_axis和pt的叉乘结果
所以result[0]=pt[0]+w_cross_pt[0]
6.2.2 projection.h
整个主题就是函数CamProjectionWithDistortion函数，变量camera,point,predictions,都是指针。
这里的camera代表9个未知量，依次是轴角，位移，焦距，外参l1,l2.
计算跟公式相同，不同的是
归一化平面坐标xp=-p[0]/p[2],yp=-p[1]/p[2]
最后预测值为focal*distortion*xp,focal*distortion*yp,后面并没有加上cx,cy.

camera应该是一个单独的类来着。point为什么也用指针形式，还有预测值，这个还只是归一化平面上的值吧。
cmera,point是指针形式，指针形式[0],[1],[2]也可以这样吗？
还有经过平移的
还有一个问题是为什么xp取负值。
projection.h
就是定义了一个CamProjectionWithDistortion,三个变量，camera,point,predictions,三个都是指针。把camera当成了9个未知量。前3个是轴角，然后平移，然后focal,l1,l2就是去畸变的时候用到的。
但是归一化平面坐标xp=-p[0]/p[2],yp=-p[1]/p[2].
预测结果是fx*xp*distortion,没有加cx,cy.
6.3g2o求解用到的h文件
6.3.1 g2o_bal_class.h
顶点是两个，一个是相机位姿camera,这里变成了9个量，和之前没什么不同，但是重写了oplusImpl函数，原来是_estimate+=Eigen::Vector3d(update);这里变成了+=v,v是
Eigen::VectorXd::ConstMapType v(update,VertexCameraBAL::Dimension);
typedef const Eigen::Map<const Derived, Unaligned> ConstMapType;
一个就是路标点，就是3*1的，也重写了oplusImpl函数，和camera不同，它的v是
Eigen::Vector3d::ConstMapType v(update),这里没有加路标点的维度。

边的计算误差函数computeError被重写了，这里它定义
cam=static_cast<const VertexCameraBAL*>(vertex(0));
里面的变量变成了vertex(0).point对应的是vertex(1).
由cam的estimate().data(),point的estimate().data(),_error.data()，这3个数据是变量，然后用括号运算符来计算了误差。
下面定义了operator()的具体形式。
cam的估计值和point的估计值代入到CamProjectionWithDistortion函数，得到预测值。
误差也可以说是残差residual[0]=predictions[0]-T(measurement()(0))
就是预测值减去测量值。

接下来看边的定义导数的函数linearizeOplus,它也被重写了。
同样先定义cam,point.
定义bal的自动求导类型 BALAutoDiff
typedef ceres::internal::AutoDiff<EdgeObervationBAL,double,VertexCameraBAL::Dimension,VertexPointBAL::Dimension> BALAutoDiff;
其实就是边对两个顶点进行求导。边的数据可以当成是误差数据对位姿和路标的求导，也可以看成是预测值对位姿和路标的求导。
定义矩阵dError_Camera,dError_dPoint,维度分别为它们自己本身的维度*Eigen::RowMajor
参数由camera和point的估计值组成，
雅克比矩阵由dError_Camera,dError_dPoint组成。
value为Dimension维的向量。
BALAutoDiff有一个函数Differentiate,放入变量*this,parameters,Dimension,value,jacobians，返回bool值diffState.
如果返回值为true,_jacobianOplusXi就为dError_Camera,Xj就为dError_Point.
否则，导数设为0.