如何用ceres进行两帧之间的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
  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(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<

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

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

你可能感兴趣的:(SLAM)