头部姿态估计 - OpenCV/Dlib/Ceres (2)

这里我直接使用了数值差分的方法（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程序中除了第一帧的初始值是提前设置好的以外，后续的初始值都是前一帧的最优值。
后面的表现都很好，但这第一帧确实会存在紊乱的情况。
因此后续优化可以考虑使用一个粗估计的初始值，因为对于这些迭代优化方法，初始值的选择决定了会不会陷入局部最优的情况。

测试结果

脸部效果：

输出工作环境：