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的求解代碼。

本文僅做學術分享，如有侵權，請聯系刪文。