头部姿态估计 - 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)
脸部效果：
输出工作环境：
