作者：郭田峰

來源：公眾號@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(newceres::Grid2D<資料型別[,資料維數]>(資料首地址,首行號,總行數,首列號,總列數));BiCubicInterpolator物件.reset(newceres::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的求解程式碼。

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