g2o求解BA 第10章

Posted 2020-10-03 踏路girl

1、g2o_bal_class.h
1.1 projection.h
g2o还是用图模型和边，顶点就是相机和路标，边就是观测，就是像素坐标。只不过这里的相机是由
旋转（3个参数，轴角形式，就是theta*nx,theta*ny,theta*ny),位移(3个参数），f,k1,k2.
就是之前BA模型的实现。
但是这里归一化平面坐标取得是负值，而且最后没有加cx,cy.
具体实现在projection.h中的CamProjectionWithDistortion函数实现的，而其中世界坐标转成相机坐标是由rotation.h中的函数AngleAxisRotatePoint实现的。具体参见轴角和四元数和空间点.note
1.2顶点
自定义了相机顶点和路标顶点（名字自己定），继承自g2o::BaseVertex.一个9维，Eigen::VectorXd，一个3维，Eigen::Vector3d.
跟之前一样，不一样的是有了覆盖。就是对oplusImpl函数用override进行覆盖。覆盖形式为
virtual void oplusImpl(const double* update) override{}
里面会对update进行操作，变成v，之前都是 _estimate += Eigen::Vector3d(update);
而这里update不再是Eigen::Vector3d的形式，而是变成了地图类型。
一个是Eigen::VectorXd::ConstMapType v(update,VertexCameraBAL::Dimension);
一个是Eigen::Vector3d::ConstMapType v(update);
然后_estimate+=v就可以了。
1.3边
观测值在当前图片中可以看到这个路标，通过计算描述子可以在当前图片中找到和路标匹配的特征点，这个匹配的特征点的像素坐标就是观测值。
同样还是把vertex(0)赋值给顶点camera,vertex(1)赋值给顶点point.
这里的计算误差的函数被重写了。
同样virtual void computeError() override{ }
而计算误差用了括号运算符()，在template<typename T>下。
过程是由CamProjectionWithDistortion(camera,point,predictions)得到预测值predictions,然后预测值与测量值measurement()的对应项相减得到residual[0],residual[2].
virtual void linearizeOplus() override{}，这里linearizeOplus也被重写了。
同样还是先赋值顶点。然后自定义BalAutoDiff自动求导函数。
typedef ceres::internal::AutoDiff<EdgeObservationBAL,double,VertexCameraBAL::Dimension,VertexPointBAL::Dimension> BalDutoDiff,
用了ceres的autodiff,里面放的四个参数分别是边类，数据类型，两个顶点类。
定义对Camera顶点求导后的矩阵，dError_dCamera,维度Dimension*顶点的维度，应该是9.
因为过往的雅克比矩阵是2*6形式。每个误差都有一个雅可比矩阵，这里应该是每个雅可比矩阵的形式。
和对Point顶点求导后的矩阵dError_dPoint.
Eigen::Matrix<double,Dimension,VertexCameraBAL::Dimension,Eigen::RowMajor> dError_dCamera;
Eigen::Matrix<double,Dimension,VertexPointBAL::Dimension,Eigen::RowMajor> dError_dPoint.
parameters是由cam的估计值和point的估计值组成，指针形式。
jacobian是由camera的导数和point的导数组成。
使用BALAutoDiff中的Differentiate函数。放入边，参数，维度Dimension,value(它的值就是Dimension),还有要得到雅可比矩阵jacobian.
返回值是一个bool值，表示的是差分状态，也就是求导状态。
如果它为true的话，_jacobianXi就为dError_dCamera,_jacobianXj就为dError_dPoint,如果返回false,那么，_jacobianXi,_jacobianXj就设为0.
问题就是Dimension这个值具体是什么，不知道。
2.g2o_bundle.cpp
2.1BuildProblem函数
作用：给图模型添加顶点，添加边
变量：bal_problem类，指针形式，图模型optimizer,优化参数params
2.1.1添加顶点
从bal_problem中读取路标数量，相机位姿数量，相机块大小9或10（取决于旋转用四元数还是用轴角表示），路标块大小3。
造一个数raw_cameras,它是一个指针，由balProblem的cameras函数得出，这个函数的返回值是parameters,parameters为9*相机位姿的数量再加上3*路标点的数量。

做一个循环，阈值是相机位姿数量，接下来是要添加相机顶点。
类型为typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
值为(raw_cameras+9*i,9)
相机的估计值就是temVecCamera;估计值应该是跟相机顶点类型一样的啊。应该也是Eigen::VectorXd的。
然后顶点的估计值就是temVecCamera,id就是i(一般在for循环里，顶点id都设成i.)图模型添加相机顶点。顶点就是之前定义的类。一般指针形式，后面加new,是为了分配一个对象给它。
VertexCameraBAL* pCamera = new VertexCameraBAL();
添加路标顶点
也是造一个数指针raw_points,它的返回值是parameters+9*相机位姿数量，对每个路标点来说，
constVectorRef temVecPoint(raw_points+3*j,3)
同样设置路标顶点为pPoint,pPoint的估计值就为temVecPoint.
这里要设置路标点可以进行边缘化，也就是可以进行消元。但为什么相机没有设这个值。
图模型添加这个顶点pPoint.
这里设置路标点id为j+num_cameras,因为为了保证每个顶点都有一个对应的id.
2.1.2添加边
定义常量观测值的数量为num_observations。
定义指针observations对应的值为2*观测值的数量。
利用观测值造一个循环

设置鲁棒核函数，之所以要设置鲁棒核函数是为了平衡误差，不让二范数的误差增加的过快。鲁棒核函数里要自己设置delta值，这个delta值是，当误差的绝对值小于等于它的时候，误差函数不变。否则误函数变为delta(|e|-1/2delta).
如果输入的参数params的鲁棒性检验通过if(params.robustify)
那么设置鲁棒核函数为g2o里的huber loss函数。
g2o::RobustKernelHuber* rk=new g2o::RobustKernelHuber;
设置delta值为1.0.设置边的鲁棒核函数为rk..setDelta,.setRobustKernel().
设置边的顶点0对应的相机顶点为optimizer->vertex(camera_id),指针camera_id为观测值的数量[i]
设置边的顶点1对应的路标顶点为optimizer->vertex(point_id),指针point_id为观测值的数量[i]+相机数量.
设置其协方差矩阵也就是.setInformation，为2*2的单位矩阵。
设置其测量值为observations[2*i+0],observations[2*i+1].
图模型添加边。
设顶点，设估计值，设id，图模型添加顶点。
设边，设顶点，设协方差矩阵，设测量值，图模型添加边。

3.图模型求解
还是之前一样的过程，这里自定义类型名为BalBlockSolver,这里是造了一个函数SetSolverOptionsFromFlags,变量是bal_problem类，params参数，图模型optimizer.
先把矩阵块求解器solver_ptr,线性方程求解器linearSolver初值设为0
如果params.linear_solver的值为dense_schur,那么线性方程求解器用稠密的，也就是Dense,
如果params.linear_solver的值为sparse_schur，那么linearSolver用稀疏的，也就是Cholmod,而且设置linearSolver对矩阵排序保持稀疏性
dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>* >(linearSolver)->setBlockOrdering(true); 这个只在稀疏的情况下使用。
由linearSolver得到矩阵块求解器solver_ptr.
如果params.trust_region_strategy为列文伯格-马夸而特，那么梯度下降方法solver就选为列文伯格-马夸而特
如果为dogleg,那就用dogleg来做梯度下降方法solver.

g2o在做BA的优化时必须将其所有点云全部schur掉，否则会出错。其原因在于我们使用了g2o::LinearSolver<BalBlockSolver::PoseMatrixType>这个类型来指定linearsolver,其中模板参数当中的位姿矩阵类型在程序中为相机姿态参数的维度，于是BA当中schur消元后解得线性方程组必须是只含有相机姿态变量。
Ceres库则没有g20这样的限制，Ceres给了开发者很大的空间去操作自己的优化策略，它在Schur消元操作时完全不需要把所有点云都消元掉，用户可以自己编写函数选择消元哪些点云。接下来我们也会给出Ceres的BA。