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

基本思想（update)

通過Dlib獲得當前人臉的特征點，然后通過(1)修改模型的幾何形狀參數和(2)旋轉平移模型，進行擬合，計算標准模型求得的特征點與Dlib獲得的特征點之間的差，使用Ceres不斷迭代優化，最終得到最佳的(1)模型幾何形狀參數和(2)旋轉和平移參數。

使用環境

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

第三方工具

Dlib：用於獲得人臉特征點
Ceres：用於進行非線性優化
CMinpack：用於進行非線性優化 （OPTIONAL）
HDF5：用於讀取.h5格式數據（new）

源代碼

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

基礎概念

旋轉矩陣(update)

頭部的任意姿態可以轉化為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;
typedef dlib::point point2d;

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

為了嘗試使用分析式的優化求解，我們可以選擇不采用dlib的API進行旋轉，而是通過直接將其數學式子列出，如下：

/* We use Z1Y2X3 format of Tait–Bryan angles */
double c1 = cos(roll  * pi / 180.0), s1 = sin(roll  * pi / 180.0);
double c2 = cos(yaw   * pi / 180.0), s2 = sin(yaw   * pi / 180.0);
double c3 = cos(pitch * pi / 180.0), s3 = sin(pitch * pi / 180.0);

for(std::vector<point3f>::iterator iter=landmarks3d.begin(); iter!=landmarks3d.end(); ++iter) {
	double X = iter->x(), Y = iter->y(), Z = iter->z(); 
	iter->x() = ( c1 * c2) * X + (c1 * s2 * s3 - c3 * s1) * Y + ( s1 * s3 + c1 * c3 * s2) * Z + tx;
	iter->y() = ( c2 * s1) * X + (c1 * c3 + s1 * s2 * s3) * Y + (-c3 * s1 * s2 - c1 * s3) * Z + ty;
	iter->z() = (-s2     ) * X + (c2 * s3               ) * Y + ( c2 * c3               ) * Z + tz; 
}

這樣同樣也完成了旋轉過程，並且方便進行求導。
[注] 但目前還是選擇使用數學求解的方法，因為分析式求解經常會出現不收斂的情況，原因未知。

LM算法

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

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

針孔模型

根據針孔相機模型我們可以輕松的得到三維坐標到二維坐標的映射：
\(x=f_x\frac{X}{Z}+c_x\)
\(y=f_y\frac{Y}{Z}+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

去除z軸的直接映射（new）

在程序中使用直接去除z軸的直接映射先進行一次迭代求解，用該解來作為真正求解過程的初始值，詳細內容見下方初始值的選定。
\(x=X\)
\(y=Y\)

BFM模型的表示(new)

BFM模型是通過PCA的方法得到的，可參見之前博文：BFM模型介紹及可視化實現（C++）
需要了解的是，雖然人臉幾何有上萬個頂點，但我們可以通過199個PC系數來進行表示，這也就是我們要求得的最終參數之一。

具體步驟

獲得模型的特征點(new)

根據BFM模型介紹及可視化實現（C++）中的介紹，我將其中對bfm模型的操作的代碼整合到了該項目當中，並且將其中的數據類型根據實際使用dlib進行的改進：
*　使用point3f（dlib::vector<double, 3>）/ point2d（dlib::vector<int, 2>）替換自己寫的vec類：使得整體操作更為流暢和統一；

• 使用dlib::matrix替換std::vector<...>/ std::vector<std::vector<...>>：統一向量和矩陣，方便運算；讀入數據也更加簡單；在矩陣乘法等計算量比較大的運算當中，dlib/opencv有對此進行優化，速度會大大提升。
  [注] 盡管如此，在模型幾何形狀的迭代上速度依舊很慢，在我的電腦上完成一次收斂的交替迭代需要40min左右。也是因為這個時間關系，如果要進行real-time的頭部姿態捕捉的話，可以考慮先拍攝一張用戶照片作為輸入，后續的迭代中不對幾何形狀進行優化，在DVP其視頻上的處理就是這樣的。

模型獲得之后，我們根據GitHub上的這個朋友github.com/anilbas/BFMLandmarks給出的68個特征點下標獲得對應的特征點。

獲得標准模型的特征點(deprecated)

該部分可見前一篇文章：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進行非線性優化(update)

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;

[注] 具體的方法使用了Ridders（ceres::RIDDERS），而不是向前差分（ceres::FORWARD）或者中分（ceres::CENTRAL）,因為用后兩者進行處理的時候，LM算法\(\beta_{k+1}=\beta_k-(J^TJ+\lambda I)^{-1}J^Tr)\)的更新項為0，無法進行迭代，暫時沒有想到原因，之前這里也被卡了很久。
[注] 源代碼中還有使用了CMinpack的版本，該版本不可用的原因也是使用了封裝最淺的lmdif1_調用（返回結果INFO=4），該版本下使用的向前差分，如果改為使用lmdif_對其中的一些參數進行調整應該是可以實現的。

HeadPoseCostFunctor的構建 - 頭部姿態的6個參數(update)

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

• LANDMARK_NUM：該程序中為68，因為Dlib算法獲得的特征點數為68；；
• \(q_i\)：通過Dlib獲得的2維特征點坐標，大小為68的vector<dlib::point>
• \(p_i\)：經過一系列變換得到的標准模型的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\)：標准模型的三維3維特征點坐標，大小為68的vector<point3f>；
• \(R(yaw, roll, pitch)\)：旋轉矩陣；
• \(T(t_x, t_y, t_z)\)：平移矩陣；
• \(\prod()\)：3維點轉2維點的映射，如上所描述通過相機內參獲得。
• \(\|·\|\)：因為是兩個2維點的差，我們使用歐幾里得距離(的平方)來作為2點的差。
  Ceres當中的CostFunctor只需要寫入平方以內的內容，因此我們如下構建：

struct HeadPoseCostFunctor {
public:
	HeadPoseCostFunctor(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個參數。

ShapeCostFunctor的構建 - 模型幾何的199個參數(new)

同HeadPoseCostFunctor的構建，不同的是求解的參數改為幾何形狀的PC個數．

double head_pose[6] = {0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
double shape_pc_basis[N_PC] = {0.f};

Problem head_pose_problem, shape_pc_problem;
CostFunction* head_pose_cost_function = new NumericDiffCostFunction<HeadPoseNumericCostFunctor, ceres::RIDDERS, LANDMARK_NUM, 6>(new HeadPoseNumericCostFunctor(obj_detection));
head_pose_problem.AddResidualBlock(head_pose_cost_function, NULL, head_pose);
CostFunction* shape_pc_cost_function = new NumericDiffCostFunction<ShapeNumericCostFunctor, ceres::RIDDERS, LANDMARK_NUM, N_PC>(new ShapeNumericCostFunctor(obj_detection));
shape_pc_problem.AddResidualBlock(shape_pc_cost_function, NULL, shape_pc_basis);

通過交替求解，即先求得頭部姿態參數的最優解，以此為基礎去求幾何形狀參數的最優解，以此往復，直到二者都達到收斂。偽代碼如下：

while(true) {
    Solve(head_pose);
    Solve(shape_pc_basis);
    if(two problems are both convergence)
        break;
}

通過該方法對同一張圖片進行測試，記錄最終cost，如下：

	初始值	僅頭部姿態參數	交替迭代
cost(數量級)	1e9	1e5	4e3

初始值的選定(update)

之前並沒有多考慮這個因素，在程序中除了第一幀的初始值是提前設置好的以外，后續的初始值都是前一幀的最優值。
后面的表現都很好，但這第一幀確實會存在紊亂的情況（后來發現是超出最大迭代次數50后依舊NO_CONVERGENCE）。
因為對於這些迭代優化方法，初始值的選擇決定了會不會陷入局部最優的情況，因此考慮使用一個粗估計的初始值。

粗暴的估計

通過例如鼻子的傾斜角度、嘴巴的傾斜角度、頭部的寬度等等直觀上的數據，進行一個粗步估計。但影響效果一般。

使用直接映射進行估計

不使用針孔相機模型，而是使用直接去掉z軸來進行三維到二維的映射，在此基礎上進行迭代，將迭代結果作為真正迭代的初始值。
影響效果一般，往往在這個迭代模型上已經下降了3個數量級，但換作真正迭代的時候計算初始值的cost又會恢復到1e9級別。

后續考慮

了解到了DVP中初值使用POSIT算法（改進版SoftPOSIT算法），后續考慮嘗試這種方法進行估計。

測試結果(deprecated)

臉部效果：

輸出工作環境：