贵州建设厅考试网站二建成绩查询,wordpress小程序调用数据库字段,免费网络空间,设计师 网站1、g2o_bal_class.h1.1 projection.hg2o还是用图模型和边#xff0c;顶点就是相机和路标#xff0c;边就是观测#xff0c;就是像素坐标。只不过这里的相机是由旋转#xff08;3个参数#xff0c;轴角形式#xff0c;就是theta*nx,theta*ny,theta*ny),位移(3个参数#…1、g2o_bal_class.h1.1 projection.hg2o还是用图模型和边顶点就是相机和路标边就是观测就是像素坐标。只不过这里的相机是由旋转3个参数轴角形式就是theta*nx,theta*ny,theta*ny),位移(3个参数f,k1,k2.就是之前BA模型的实现。但是这里归一化平面坐标取得是负值而且最后没有加cx,cy.具体实现在projection.h中的CamProjectionWithDistortion函数实现的而其中世界坐标转成相机坐标是由rotation.h中的函数AngleAxisRotatePoint实现的。具体参见轴角和四元数和空间点.note1.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);然后_estimatev就可以了。1.3边观测值在当前图片中可以看到这个路标通过计算描述子可以在当前图片中找到和路标匹配的特征点这个匹配的特征点的像素坐标就是观测值。同样还是把vertex(0)赋值给顶点camera,vertex(1)赋值给顶点point.这里的计算误差的函数被重写了。同样virtual void computeError() override{ }而计算误差用了括号运算符()在templatetypename T下。过程是由CamProjectionWithDistortion(camera,point,predictions)得到预测值predictions,然后预测值与测量值measurement()的对应项相减得到residual[0],residual[2].virtual void linearizeOplus() override{}这里linearizeOplus也被重写了。同样还是先赋值顶点。然后自定义BalAutoDiff自动求导函数。typedef ceres::internal::AutoDiffEdgeObservationBAL,double,VertexCameraBAL::Dimension,VertexPointBAL::Dimension BalDutoDiff,用了ceres的autodiff,里面放的四个参数分别是边类数据类型两个顶点类。定义对Camera顶点求导后的矩阵dError_dCamera,维度Dimension*顶点的维度应该是9.因为过往的雅克比矩阵是2*6形式。每个误差都有一个雅可比矩阵这里应该是每个雅可比矩阵的形式。和对Point顶点求导后的矩阵dError_dPoint.Eigen::Matrixdouble,Dimension,VertexCameraBAL::Dimension,Eigen::RowMajor dError_dCamera;Eigen::Matrixdouble,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.cpp2.1BuildProblem函数作用给图模型添加顶点添加边变量bal_problem类指针形式图模型optimizer,优化参数params2.1.1添加顶点从bal_problem中读取路标数量相机位姿数量相机块大小9或10取决于旋转用四元数还是用轴角表示路标块大小3。造一个数raw_cameras,它是一个指针由balProblem的cameras函数得出这个函数的返回值是parameters,parameters为9*相机位姿的数量再加上3*路标点的数量。 做一个循环阈值是相机位姿数量接下来是要添加相机顶点。类型为typedef Eigen::Mapconst Eigen::VectorXd ConstVectorRef;值为(raw_cameras9*i,9)相机的估计值就是temVecCamera;估计值应该是跟相机顶点类型一样的啊。应该也是Eigen::VectorXd的。然后顶点的估计值就是temVecCamera,id就是i(一般在for循环里顶点id都设成i.)图模型添加相机顶点。顶点就是之前定义的类。一般指针形式后面加new,是为了分配一个对象给它。 VertexCameraBAL* pCamera new VertexCameraBAL();添加路标顶点也是造一个数指针raw_points,它的返回值是parameters9*相机位姿数量对每个路标点来说constVectorRef temVecPoint(raw_points3*j,3)同样设置路标顶点为pPoint,pPoint的估计值就为temVecPoint.这里要设置路标点可以进行边缘化也就是可以进行消元。但为什么相机没有设这个值。图模型添加这个顶点pPoint.这里设置路标点id为jnum_cameras,因为为了保证每个顶点都有一个对应的id.2.1.2添加边定义常量观测值的数量为num_observations。定义指针observations对应的值为2*观测值的数量。利用观测值造一个循环 设置鲁棒核函数之所以要设置鲁棒核函数是为了平衡误差不让二范数的误差增加的过快。鲁棒核函数里要自己设置delta值这个delta值是当误差的绝对值小于等于它的时候误差函数不变。否则误函数变为delta(|e|-1/2delta).如果输入的参数params的鲁棒性检验通过if(params.robustify)那么设置鲁棒核函数为g2o里的huber loss函数。g2o::RobustKernelHuber* rknew 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*i0],observations[2*i1].图模型添加边。设顶点设估计值设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_castg2o::LinearSolverCholmodBalBlockSolver::PoseMatrixType* (linearSolver)-setBlockOrdering(true); 这个只在稀疏的情况下使用。由linearSolver得到矩阵块求解器solver_ptr.如果params.trust_region_strategy为列文伯格-马夸而特那么梯度下降方法solver就选为列文伯格-马夸而特如果为dogleg,那就用dogleg来做梯度下降方法solver. g2o在做BA的优化时必须将其所有点云全部schur掉否则会出错。其原因在于我们使用了g2o::LinearSolverBalBlockSolver::PoseMatrixType这个类型来指定linearsolver,其中模板参数当中的位姿矩阵类型在程序中为相机姿态参数的维度于是BA当中schur消元后解得线性方程组必须是只含有相机姿态变量。Ceres库则没有g20这样的限制Ceres给了开发者很大的空间去操作自己的优化策略它在Schur消元操作时完全不需要把所有点云都消元掉用户可以自己编写函数选择消元哪些点云。接下来我们也会给出Ceres的BA。 转载于:https://www.cnblogs.com/talugirl/p/7384115.html
