基于aloam研究ceres的构建方式

aloam相比于loam增加了ceres库自动微分，而不需要手推高斯牛顿，这是非常方便的。

对于aloam源码，在laserOdometry中，有关ceres的部分：

   ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);    // 鲁棒核
   ceres::LocalParameterization *q_parameterization =
       new ceres::EigenQuaternionParameterization();
   ceres::Problem::Options problem_options;

   ceres::Problem problem(problem_options);
   problem.AddParameterBlock(para_q, 4, q_parameterization);   // 类似于AddResidual函数的输入维度+待优化变量
   problem.AddParameterBlock(para_t, 3);

上述代码建立问题，并设置了参数块。值得注意的是，普通的ceres使用并不用这么设置，直接最终AddResidual就可以了，为什么在这里，先使用了ceres::LocalParameterization，再传入参数块（待优化变量）呢？

参考https://blog.csdn.net/weixin_43991178/article/details/100532618#ProblemAddParameterBlock__26

LocalParameterization类的作用是解决非线性优化中的过参数化问题。所谓过参数化，即待优化参数的实际自由度小于参数本身的自由度。例如SLAM中，当采用四元数表示位姿时，由于四元数本身的约束（模长为1），实际的自由度为3而非4。此时，若直接传递四元数进行优化，冗余的维数会带来计算资源的浪费，需要使用Ceres预先定义的QuaternionParameterization对优化参数进行重构：

problem.AddParameterBlock(quaternion, 4);// 直接传递4维参数
ceres::LocalParameterization* local_param = new ceres::QuaternionParameterization();
problem.AddParameterBlock(quaternion, 4, local_param)//重构参数，优化时实际使用的是3维的等效旋转矢量，避免计算资源的浪费

	//	loam寻找当前帧角点对应的上一帧的最近点，需要提前将当前帧角点用上一次的q_last_curr位姿进行变换，这是不精确的，但是也没有其他办法......
   ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
   problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);   // loss_function是鲁棒核

最后进行求解：

     ceres::Solver::Options options;
     options.linear_solver_type = ceres::DENSE_QR;
     options.max_num_iterations = 4;     // 迭代数量较小，保证运算速度
     options.minimizer_progress_to_stdout = false;
     ceres::Solver::Summary summary;
     ceres::Solve(options, &problem, &summary);  

如果之前加入参数块的时候直接加入，而不进行重构，就会报错：problem.AddParameterBlock(quaternion, 4);

报错内容为：
[ERROR] [1606368318.343523718]: Ignoring transform for child_frame_id “aft_mapped” from authority “unknown_publisher” because of a nan value in the transform (-0.011258 0.005297 -0.000222) (-nan -nan -nan -nan)
[ERROR] [1606368318.343559777]: Ignoring transform for child_frame_id “aft_mapped” from authority “unknown_publisher” because of an invalid quaternion in the transform (-nan -nan -nan -nan)
可以看见四元数直接发散掉了。故在遇到位姿优化问题，使用四元数的时候一定要使用如下部分加入参数块：

            ceres::LocalParameterization *q_parameterization =
                new ceres::EigenQuaternionParameterization();
                
            problem.AddParameterBlock(para_q, 4, q_parameterization);   // 类似于AddResidual函数的输入维度+待优化变量
            // problem.AddParameterBlock(para_q, 4);    // 不进行重构，会报错
            problem.AddParameterBlock(para_t, 3);