頭部姿態估計 - OpenCV/Dlib/Ceres

基本思想

經過Dlib得到當前人臉的特徵點，而後經過旋轉平移標準模型的特徵點進行擬合，計算標準模型求得的特徵點與Dlib得到的特徵點之間的差，使用Ceres不斷迭代優化，最終獲得最佳的旋轉和平移參數。

使用環境

系統環境：Ubuntu 18.04
使用語言：C++
編譯工具：CMake

第三方工具

Dlib：用於得到人臉特徵點
Ceres：用於進行非線性優化
CMinpack：用於進行非線性優化 （OPTIONAL）

源代碼

https://github.com/Great-Keith/head-pose-estimation

基礎概念

旋轉矩陣

頭部的任意姿態能夠轉化爲6個參數（yaw, roll, pitch, tx, ty, tz），前三個爲旋轉參數，後三個爲平移參數。
平移參數好理解，原座標加上對應的變化值便可；旋轉參數須要構成旋轉矩陣，三個參數分別對應了繞y軸旋轉的角度、繞z軸旋轉的角度和繞x軸旋轉的角度。

具體代碼實現咱們能夠經過Dlib已經封裝好的API，rotate_around_x/y/z(angle)。該函數返回的類型是dlib::point_transform_affine3d，能夠經過括號進行三維的變形，咱們將其封裝成一個rotate函數使用以下：

void rotate(std::vector<point3f>& points, const double yaw, const double pitch, const double roll) 
{
    dlib::point_transform_affine3d around_z = rotate_around_z(roll * pi / 180);
    dlib::point_transform_affine3d around_y = rotate_around_y(yaw * pi / 180);
    dlib::point_transform_affine3d around_x = rotate_around_x(pitch * pi / 180);
    for(std::vector<point3f>::iterator iter=points.begin(); iter!=points.end(); ++iter)
        *iter = around_z(around_y(around_x(*iter)));
}

[NOTE] 其中point3f是我本身定義的一個三維點座標類型，由於Dlib中並無提供，而使用OpenCV中的cv::Point3f會與dlib::point定義起衝突。定義以下：

typedef dlib::vector<double, 3> point3f;

[NOTE] Dlib中的dlib::vector不是std::vector，注意兩者區分。

LM算法

這邊不進行贅訴，建議跟着推導一遍高斯牛頓法，LM算法相似於高斯牛頓法的進階，用於迭代優化求解非線性最小二乘問題。在該程序中使用Ceres/CMinpack封裝好的API（具體使用見後文）。

三維空間到二維平面的映射

根據針孔相機模型咱們能夠輕鬆的獲得三維座標到二維座標的映射：
\(X^{2d}=f_x(\frac{X^{3d}}{Z^{3d}})+c_x\)
\(Y^{2d}=f_y(\frac{Y^{3d}}{Z^{3d}})+c_y\)
[NOTE] 使用上角標來表示3維座標仍是2維座標，下同。
其中\(f_x, f_y, c_x, c_y\)爲相機的內參，咱們經過OpenCV官方提供的Calibration樣例進行獲取：
例如個人電腦所得到的結果以下：

從圖中矩陣對應關係能夠得到對應的參數值。

#define FX 1744.327628674942
#define FY 1747.838275588676
#define CX 800
#define CY 600

[NOTE] 本程序不考慮外參。

具體步驟

得到標準模型的特徵點

該部分可見前一篇文章：BFM使用 - 獲取平均臉模型的68個特徵點座標
咱們將得到的特徵點保存在文件 landmarks.txt 當中。

使用Dlib得到人臉特徵點

該部分不進行贅訴，官方有給出了詳細的樣例。
具體能夠參考以下樣例：

• https://github.com/davisking/dlib/blob/master/examples/face_landmark_detection_ex.cpp
• https://github.com/davisking/dlib/blob/master/examples/webcam_face_pose_ex.cpp（經過這個樣例能夠學習OpenCV如何調用攝像頭）

其中使用官方提供的預先訓練好的模型，下載地址：http://dlib.net/files/shape_predictor_68_face_landmarks.dat.bz2

具體在代碼中使用以下：

cv::Mat temp;
    if(!cap.read(temp))
        break;
    dlib::cv_image<bgr_pixel> img(temp);
    std::vector<rectangle> dets = detector(img);
    cout << "Number of faces detected: " << dets.size() << endl;
    std::vector<full_object_detection> shapes;
    for (unsigned long j = 0; j < dets.size(); ++j) {
        /* Use dlib to get landmarks */
        full_object_detection shape = sp(img, dets[j]);
        /* ... */
    }

其中shape.part就存放着咱們經過Dlib得到的當前人臉的特徵點二維點序列。

[NOTE] 在最後CMake配置的時候，須要使用Release版本（最重要），以及增長選項USE_AVX_INSTRUCTIONS和USE_SSE2_INSTRUCTIONS/USE_SSE4_INSTRUCTIONS，不然由於Dlib的檢測耗時較長，使用攝像頭即時擬合會有嚴重的卡頓。

使用Ceres進行非線性優化

Ceres的使用官方也提供了詳細的樣例，在此咱們使用的是數值差分的方法，可參考：https://github.com/ceres-solver/ceres-solver/blob/master/examples/helloworld_numeric_diff.cc

Problem problem;
    CostFunction* cost_function = new NumericDiffCostFunction<CostFunctor, ceres::RIDDERS, LANDMARK_NUM, 6>(new CostFunctor(shape));
    problem.AddResidualBlock(cost_function, NULL, x);
    Solver::Options options;
    options.minimizer_progress_to_stdout = true;
    Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << endl;

這裏我直接使用了數值差分的方法（NumericDiffCostFunction），而不是使用自動差分（AutoDiffCostFunction），是由於自動差分的CostFunctor是經過Template實現的，利用Template來實現Jacobian矩陣的計算使用的同一個結構，這樣的話下方旋轉矩陣就不能直接經過調用Dlib提供的三維座標旋轉接口，而是要將整個矩陣拆解開來實現（這邊暫時沒有細想到底能不能實現），所以出於簡便，使用數值差分，在準確性上是會受到影響的。
而且注意到，具體的方法使用了Ridders（ceres::RIDDERS），而不是向前差分（ceres::FORWARD）或者中分（ceres::CENTRAL）,由於用後二者進行處理的時候，LM算法\(\beta_{k+1}=\beta_k-(J^TJ+\lambda I)^{-1}J^Tr)\)的更新項爲0，沒法進行迭代，暫時沒有想到緣由，以前這裏也被卡了好久。
[NOTE] 源代碼中還有使用了CMinpack的版本，該版本不可用的緣由也是使用了封裝最淺的lmdif1_調用（返回結果INFO=4），該版本下使用的向前差分，若是改成使用lmdif_對其中的一些參數進行調整應該是能夠實現的。

CostFunctor的構建

CostFunctor的構建是Ceres，也是這個程序，最重要的部分。首先咱們須要先把想要計算的式子寫出來：
\(Q=\sum_i^{LANDMARK\_NUM} \|q_i^{2d}-p_i^{2d}\|^2\)
\(Q=\sum_i^{LANDMARK\_NUM} \|q_i^{2d}-Map(R(yaw, roll, pitch)p_i^{3d}+T(t_x, t_y, t_z))\|^2\)。
其中：

• LANDMARK_NUM：該程序中爲68，由於Dlib算法得到的特徵點數爲68；；
• \(q_i^{2d}\)：經過Dlib得到的2維特徵點座標，大小爲68的vector<dlib::point>
• \(p_i^{2d}\)：通過一系列變換獲得的標準模型的2維特徵點座標，大小爲68的vector<dlib::point>，具體計算方法是經過\(p_i^{2d}=Map(R(yaw, roll, pitch)(p_i^{3d})+T(t_x, t_y, t_z))\)；
• \(p_i^{3d}\)：標準模型的三維3維特徵點座標，大小爲68的vector<point3f>；
• \(R(yaw, roll, pitch)\)：旋轉矩陣；
• \(T(t_x, t_y, t_z)\)：平移矩陣；
• \(Map()\)：3維點轉2維點的映射，如上所描述經過相機內參得到。
• \(\|·\|\)：由於是兩個2維點的差，咱們使用歐幾里得距離來做爲2點的差。
  Ceres當中的CostFunctor只須要寫入平方之內的內容，所以咱們以下構建：

struct CostFunctor {
public:
    CostFunctor(full_object_detection _shape){ shape = _shape; }
    bool operator()(const double* const x, double* residual) const {
        /* Init landmarks to be transformed */
        fitting_landmarks.clear();
        for(std::vector<point3f>::iterator iter=model_landmarks.begin(); iter!=model_landmarks.end(); ++iter)
            fitting_landmarks.push_back(*iter);
        transform(fitting_landmarks, x);
        std::vector<point> model_landmarks_2d;
        landmarks_3d_to_2d(fitting_landmarks, model_landmarks_2d);

        /* Calculate the energe (Euclid distance from two points) */
        for(int i=0; i<LANDMARK_NUM; i++) {
            long tmp1 = shape.part(i).x() - model_landmarks_2d.at(i).x();
            long tmp2 = shape.part(i).y() - model_landmarks_2d.at(i).y();
            residual[i] = sqrt(tmp1 * tmp1 + tmp2 * tmp2);
        }
        return true;
    }
private:
    full_object_detection shape;    /* 3d landmarks coordinates got from dlib */
};

其中的參數x是一個長度爲6的數組，對應了咱們要得到的6個參數。

初始值的選定

當前並無多考慮這個因素，在landmark-fitting-cam程序中除了第一幀的初始值是提早設置好的之外，後續的初始值都是前一幀的最優值。
後面的表現都很好，但這第一幀確實會存在紊亂的狀況。
所以後續優化能夠考慮使用一個粗估計的初始值，由於對於這些迭代優化方法，初始值的選擇決定了會不會陷入局部最優的狀況。

測試結果

臉部效果：

輸出工做環境：