1.前言
g2o是根据边来保存每一个代价函数,它是在边类中构造误差函数,构造边的时候,会设置顶点、测量值、协方差矩阵等。而在ceres中,用problem类型来构造最终的目标函数。先是使用AddResidualBlock来添加代价函数,代价函数和核函数等构造成目标函数。
在ceres中,代价函数就是误差项,目标函数是由很多误差项的范数的平方再各自乘以核函数除以2得到的。
2.定义代价函数类型,定义其create成员来使用ceres当中的AutoDiff特性
所谓的定义代价函数类型是定义了一个SnavelyReprojectionError.h文件,它的构造函数中用的成员变量是observed_x,observed_y,会把传入的值赋值给observed_x,observed_y,这两个值是测量值,在重载运算符()中,只有输入camera,point,residuals,那么camera,point就会计算出预测值,和测量值相减,就可以得到误差项。
而ceres::AutoDiffCostFunction的参数就是这个SnavelyProjectionError类和2,9,3,输入值就是SnavelyProjectionError类。
3.ceresBundle.cpp 添加代价函数
定义point_block_size和camera_block_size,这里是3和9
定义points和cameras指针,返回值分别是parameters_+cmear_block_size()*num_cameras_;和parameters_
定义观测值常量指针observations,返回值是2*num_observations_的列。observations数据不能更改。没有给出观测值的具体值啊,这个是要读取的。
根据观测值数量做一个for循环,定义代价函数指针cost_function.它的值形式就是SnavelyProjectionError.h的create函数,输入变量是observations[2*i+0],observations[2*i+1].
定义损失函数指针为loss_function,这里的损失函数类似于鲁棒核函数,也是用来怕不好的数据点影响迭代效果的。它的值为如果params.robustify是健壮的,就用HuberLoss(1.0)函数,这里的delta定位1.0.如果不健壮就为NULL.
定义数据指针camera,point为cameras+9*camera_index()[i]和points+3*point_index()[i];
point_index为num_observations_长度的列,camera_index同样。
然后往问题里添加代价函数就可以了。是这么添加的
problem->AddResidualBlock(cost_function,loss_function,camera,point)
只有放入代价函数,损失函数,放入camera,point这个SnavelyProjectionError.h就可以用到这两个值得到代价函数的形式。看problem.h可以看到,在problem类中,添加目标函数的变量都是指针形式,分别是代价函数,损失函数,x0和x1.
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1);
4.ceresBundle.cpp 用ParameterBlockOrdering来管理Schur的消元顺序
定义SetOrdering函数来做。在函数里分别定义
num_points,points_block_size,指针points,points值为parameters_ + camera_block_size() * num_cameras_; 跟之前一样。
还有num_cameras,camera_block_size,指针cameras.
如果参数的ordering值为"automatic",那么跳出函数,否则
定义ParameterBlockOrdering指针 ordering.
用ordering的AddElementToGroup对变量进行编号从而定义消元顺序。这里设置点云变量为0,相机变量为1.
先做points的循环
ordering->AddElementToGroup(points+point_block_size*i,0);
然后把ordering放到ceres的求解器options中
options->linear_solver_ordering.reset(ordering)
5.ceresBundle.cpp 设置优化策略
ceres设置优化策略等直接对Solver::Options的类型成员进行赋值就可以了,很方便。
这里设了一个setSolverOptionsFromFlags函数,变量有bal_problem用来提供各种参数值的,params用来判断的,options选择优化策略的。
来看options的几个参数值。
max_num_iterations:最大迭代次数
minimizer_progress_to_stdout:要不要输出到cout,如果输出,填true;
num_threads:用于计算的线程数目,可以加速雅克比矩阵的计算。
trust_region_strategy_type:下降策略的选取
linear_solver_type:增量方程如何求解,例如ceres::DENSE_QR.
设置好options之后,把它放入之前的SetOrdering函数,对schur消元进行排序。
6求解的设置
也还是options的参数
gradient_tolerance=1e-16;梯度
function_tolerance=1e-16:两次迭代之间目标函数之差的阈值。
定义统计信息summary,然后一次把options,&problem,&summary放入求解 函数solve就可以了。其中options是设置求解策略和阈值等的,problem是用来设置目标函数是怎么求得,summary是求解之后的统计信息。