Ceres求解直接法BA实现自动求导

作者:郭田峰

来源:公众号@3D视觉工坊

链接:Ceres求解直接法BA实现自动求导

 

BA,即Bundle Adjustment,通常译为光束法平差,束调整,捆绑调整等。但高翔博士觉得这些译名不如英文名称来得直观,所以保留英文名,简称BA。

所谓BA,是指从视觉图像中提炼出最优的3D模型和相机参数。在视觉SLAM里,BA特征点法和直接法两种。前者是最小化重投影误差作为优化目标,后者是以最小化光度误差为目标。

对于特征点法BA,高翔博士所著的《视觉SLAM十四讲》第二版第九章作了非常详细的说明。对于直接法BA,在深蓝学院的课程《视觉SLAM理论与实践》中有用g2o求解的习题,但没有提到Ceres求解。而且,习题中给出的是双线性插值来得到图像点的灰度值。我们知道,直接法BA需要判断图像边界,而且Ceres对双线性插值是不能自动求导的。这都会增加代码实现的难度。

在课后作业里有一题要求用g2o实现直接法BA。相对来说g2o来说,我个人更喜欢用Ceres,毕竟Ceres是谷歌出品,而且,谷歌的非线性优化大多是用Ceres来解决的,功能和效率应该是值得我们信任的。

我们知道,Ceres是推荐我们尽可能使用自动求导的,一是准确性更有保障;二是求解更快速。所以,我们要寻找能实现自动求导的实现方法。

Ceres提供的Ceres的Grid2D和BiCubicInterpolator联合使用可以解决上述两个问题。Grid2D和BiCubicInterpolator的使用需要包含头文件cubic_interpolation.h。

Grid2D是无限二维网格的对象,可以用来载入图像数据,如果是灰度图,其值用标量,如果是彩色图像,其值用3维向量。由于网格的输入数据总是有限的,而网格本身是无限的,因为需要通过使用双三次插值BiCubicInterpolator来计算网格之间的值。而超出网格范围,则将返回最近边缘的值。

将灰度图像数据加载到Grid2D对象,可以避免我们在代码中判断图像边界的问题。而且,Grid2D还可以利用BiCubicInterpolator实现双三次插值,它相对于双线性插值的优点是能实现自动求导。利用它们写出的代码更简洁,执行效率也更高。

Grid2D对象和BiCubicInterpolator对象的定义:

std::unique_ptr<ceres::Grid2D<数据类型[,数据维数]> > 变量名;std::unique_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<数据类型[,数据维数]> > > 变量

数据类型一般是简单类型,如int,float,double等,上面两个定义的数据类型和数据维数必须相同。数据维数是指值是几维数据,默认值为1,即函数值为标量时可以不指定该参数。定义好了对象变量,就可以初始化了,格式如下:

Grid2D对象.reset(new ceres::Grid2D<数据类型[,数据维数]>(数据首地址, 首行号, 总行数, 首列号, 总列数));BiCubicInterpolator对象.reset(new ceres::BiCubicInterpolator<ceres::Grid2D<数据类型[,数据维数]> >(* Grid2D对象))

数据一般使用vector类型以及嵌套,对于灰度图,我使用的是vector<vector<float>>。

Grid2D还有两个参数,分别是表示数据的储存顺序为行还是列,以及值为向量时向量的每一维值的存储顺序是行还是列,由于在本文中并不重要,所以这里不作介绍。代码中直接采用了默认值。

计算残差代码如下:

struct GetPixelGrayValue {    GetPixelGrayValue(const float pixel_gray_val_in[16],                      const int rows,                      const int cols,                      const std::vector<float>& vec_pixel_gray_values)    {        for (int i = 0; i < 16; i++)            pixel_gray_val_in_[i] = pixel_gray_val_in[i];        rows_ = rows;        cols_ = cols;                // 初始化grid2d        grid2d.reset(new ceres::Grid2D<float>(            &vec_pixel_gray_values[0], 0, rows_, 0, cols_));                //双三次插值        get_pixel_gray_val.reset(            new ceres::BiCubicInterpolator<ceres::Grid2D<float> >(*grid2d));    }        template <typename T>bool operator()(const T* const so3t,              //模型参数,位姿,6维const T* const xyz,             //模型参数,3D点,3维T* residual ) const              //残差,4*4图像块,16维{        // 计算变换后的u和v        T u_out, v_out, pt[3], r[3];        r[0] = so3t[0];        r[1] = so3t[1];        r[2] = so3t[2];        ceres::AngleAxisRotatePoint(r, xyz, pt);        pt[0] += so3t[3];        pt[1] += so3t[4];        pt[2] += so3t[5];        u_out = pt[0] * T(fx) / pt[2] + T(cx);        v_out = pt[1] * T(fy) / pt[2] + T(cy);                        for (int i = 0; i < 16; i++)        {            int m = i / 4;            int n = i % 4;            T u, v, pixel_gray_val_out;            u = u_out + T(m - 2);            v = v_out + T(n - 2);            get_pixel_gray_val->Evaluate(u, v, &pixel_gray_val_out);            residual[i] = T(pixel_gray_val_in_[i]) - pixel_gray_val_out;        }                return true;    }
    float pixel_gray_val_in_[16];    int rows_,cols_;    std::unique_ptr<ceres::Grid2D<float> > grid2d;     std::unique_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<float> > > get_pixel_gray_val;};添加残差块的代码:
for (int ip = 0; ip < poses_num; ip++){    for (int jp = 0; jp < points_num; jp++)    {        double *pose_position = first_poses_pos + ip * 6;        double *point_position = first_point_pos + jp * 3;        float gray_values[16]{};
        for ( int i = 0; i < 16; i++ )            gray_values[i] = patch_gray_values[jp][i];
        problem.AddResidualBlock(            new ceres::AutoDiffCostFunction<GetPixelGrayValue, 16, 6, 3> (                new GetPixelGrayValue ( gray_values,                                        multi_img_rows_cols[ip * 2],                                        multi_img_rows_cols[ip * 2 + 1],                                        multi_img_gray_values[ip]                )            ),            new ceres::HuberLoss(1.0),            pose_position,            point_position       );    }}

添加残差块的代码:

for (int ip = 0; ip < poses_num; ip++){    for (int jp = 0; jp < points_num; jp++)    {        double *pose_position = first_poses_pos + ip * 6;        double *point_position = first_point_pos + jp * 3;        float gray_values[16]{};
        for ( int i = 0; i < 16; i++ )            gray_values[i] = patch_gray_values[jp][i];
        problem.AddResidualBlock(            new ceres::AutoDiffCostFunction<GetPixelGrayValue, 16, 6, 3> (                new GetPixelGrayValue ( gray_values,                                        multi_img_rows_cols[ip * 2],                                        multi_img_rows_cols[ip * 2 + 1],                                        multi_img_gray_values[ip]                )            ),            new ceres::HuberLoss(1.0),            pose_position,            point_position       );    }}

这里有必要就说明的是,要判定变换后的u和v是否在图像内,如果超界了,则该组数据弃之不用。在使用Grid2D和BiCubicInterpolator后,超界后使用的值是最接近的边缘的值。这两者处理结果看似差别很大,但对结果影响很小的,几乎可以忽略不计。

下面的原来的题目:

对于相同的数据,g2o和Ceres求解执行结果如下。从截图数据显示,Ceres优化一共进行了33次迭代,用时7秒多;g2o优化一共进行了199次迭代,用时64秒左右。在这个优化题目里,两者差异非常明显。

g2o求解直接法BA执行结果截图

Ceres求解直接法BA执行结果截图

在公众号后台回复「DirectBA」,获取g2o和Ceres的求解代码。

本文仅做学术分享,如有侵权,请联系删文。

posted @ 2020-08-01 10:56  3D视觉工坊  阅读(1229)  评论(0编辑  收藏  举报