ceres slover ceres slover实现ba优化

学习高博的书已有很长一段时间了，一直看理论，看代码，而没有自己亲自上手，最近在做BA优化，大部分SLAM是用g2o进行的，而对于ceres用的很少，由于博主根本看不懂g2o的代码风格，个人觉得很无语（其实是博主zz），那咋办？，于是就想干脆用ceres实现BA优化吧。

而关于ceres，其实主要还是残差的定义了，然后求解过程都是大同小异，直接上我写的代码

struct cost_function_define
{
  cost_function_define(Point3d p1,Point3d p2):_p1(p1),_p2(p2){}
  template<typename T>
  bool operator()(const T* const cere_r,const T* const cere_t,T* residual)const
  {
    T p_1[3];
    T p_2[3];
    p_1[0]=T(_p1.x);
    p_1[1]=T(_p1.y);
    p_1[2]=T(_p1.z);
    ceres::AngleAxisRotatePoint(cere_r,p_1,p_2);
    p_2[0]=p_2[0]+cere_t[0];
    p_2[1]=p_2[1]+cere_t[1];
    p_2[2]=p_2[2]+cere_t[2];
    const T x=p_2[0]/p_2[2];
    const T y=p_2[1]/p_2[2];
    const T u=x*520.9+325.1;
    const T v=y*521.0+249.7;
    T p_3[3];
    p_3[0]=T(_p2.x);
    p_3[1]=T(_p2.y);
    p_3[2]=T(_p2.z);
    const T x1=p_3[0]/p_3[2];
    const T y1=p_3[1]/p_3[2];
    const T u1=x1*520.9+325.1;
    const T v1=y1*521.0+249.7;
    residual[0]=u-u1;
    residual[1]=v-v1;
    return true;
  }
   Point3d _p1,_p2;
};

主要是用模板类编写，博主一开始也很不习惯，但是没办法，用多了也就这样了，注意的是需要重载操作符（），然后就可以定义残差了，相信学过BA优化的同学应该对此不陌生，就是简单的重投影误差。需要说明的是，这里面用到了ceres提供的空间点的旋转向量，这就很愉快了，因为旋转向量和旋转矩阵其实就是我们所说的李代数和李群的关系，这就在迭代优化时，使得旋转向量在+是封闭的，可以进行求导。

然后就是配置求解器

ceres::Problem problem;
  for(int i=0;i<image1_3d.size();i++)
  {
    ceres::CostFunction* costfunction=new ceres::AutoDiffCostFunction<cost_function_define,2,3,3>(new cost_function_define(image2_3d[i],image1_3d[i]));
    problem.AddResidualBlock(costfunction,NULL,cere_r,cere_t);
  }
  ceres::Solver::Options option;
  option.linear_solver_type=ceres::DENSE_SCHUR;
  //输出迭代信息到屏幕
  option.minimizer_progress_to_stdout=true;
  //显示优化信息
  ceres::Solver::Summary summary;
  //开始求解
  ceres::Solve(option,&problem,&summary);
  //显示优化信息
  cout<<summary.BriefReport()<<endl;

中间关于空间点的求解博主就省略了，在这里，我们可以看到其实ceres给人的感觉还是很清晰的，至少不像g2o一样，而对于优化结果，可是说效果是比较好的，对应3D-3D的ICP问题，直接的一个代数解和用g2o做的优化它们实际上差别不是很大，而ceres的结果就给人感觉转角变了一些，位移也变了一些，一开始博主以为自己做错了，但是用反投影发现ceres优化的结果确实要比ICP要好很多，对应一个空间三维点，ceres似乎能更好的接近，博主其实也很奇怪为什么至少在这里g2o的效果会没有ceres好，毕竟一个是自动求导，一个是要自己写雅克比矩阵的。。。

想不出那就算了，博主继续用ceres做BA优化了。。。