基于双目立体视觉的人脸特征点三维坐标测量

使用双目相机拍摄人脸图像，然后用OpenCV自带的人脸识别工具识别出人脸的特征点，利用双目视觉成像原理确定特征点在物理空间的三维坐标，精度能达到亚毫米级。

双目视觉成像原理

单个相机成像

这个部分网上资料很多，在此仅作简单介绍。

为了描述物理世界的一个点到相机图像中的转换关系，需要建立下图所示的四个坐标系：世界坐标系(Ow-XwYwZw)、相机坐标系(Oc-XcYcZc)、图像坐标系(o-xy)、像素坐标系(uv)。

图中物理世界的点P最终要显示在相机图片中，用像素坐标系表示，因此需要建立世界坐标系到像素坐标系的转换关系。如下图公式所示。我们的目的是输入u、v，输出X_w、Y_w、Z_w ，因此解决其他未知参数即可解方程。公式中包含相机的内参和外参，其中内参有焦距（fx、fy），主点坐标（x0、y0），坐标轴倾斜参数s（理想情况下为0），外参即旋转矩阵R_3x3、t_3x1。

相机的内参和外参可以通过相机标定来解决。常见的标定方法有OpenCV自带的标定函数和Matlab标定工具两种，根据个人经验，Matlab标定方便卫生起效快，很容易获得误差极小的标定结果，具体操作网上也很容易搜到，不再赘述。

双目相机坐标计算

通过双目视觉计算三维坐标主要有两种方法，一种是光轴会聚模型（又叫三角测量，《Multiple View Geometry in Computer Vision》中有详细介绍），依赖于上文提到的转换关系，另一种是光轴平行模型。根据个人的应用场景，光轴平行模型精度略差，再加上这种方法大部分应用于立体匹配，因此本文不多叙述。

在光轴会聚模型中，物理世界的点P到两个相机分别都具有上述转换关系，每个相机对应一个方程，联立两个方程，用最小二乘法即可求得三维坐标。为什么要用最小二乘法，而不是求一个精确的值？那是因为像下图这样，由于一些不可消除的误差存在，例如相机畸变，图像噪声等，使得映射的两条射线不相交，也就没有交点P存在。因此只能使用最小二乘法计算出一个误差最小的点P坐标。

OpenCV中集成了这个方法：

cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

• T1，T2是两个相机标定后的外参（平移矩阵R和旋转矩阵T）拼接而成的两个3x4矩阵，在上面的公式中也有体现，长下面这样：

注意：

1.T1应该是个3x4的零矩阵，因为相机1的外参代表从相机1转换到相机1自身，不需要平移和旋转。

2. 输入的参数pts_1和pts_2应该是相机坐标系下的点，如果刚开始获得的点是像素坐标系下的话（u,v）需要转换到相机坐标系。

cv::Mat T1 = (cv::Mat_<double>(3, 4) <<   //这是我项目里的实际参数
	1.0, 0.0, 0.0, 0.0,
	0.0, 1.0, 0.0, 0.0,
	0.0, 0.0, 1.0, 0.0);
cv::Mat T2 = (cv::Mat_<double>(3, 4) <<
	0.999977229479619, -0.00532193046070156, 0.00414940699831593, -119.261032810442,
	0.00532910465608532, 0.999984321284919, -0.00171983368511187, -0.197778050925297,
	-0.00414018910566970, 0.00174190714775862, 0.999989912245948, -2.50548372131854
	);

• pts_1，pts_2是输入的两幅图像中点P的像素坐标，pts_4d是输出的齐次坐标，简单处理一下就能变成3维坐标（前三项分别除以第四项，变成Xw，Yw，Zw）。把输入和输出的点扩展成多个点也能照常使用。

人脸特征点检测

人脸特征点（facial landmarks），也可以叫人脸关键点，是人脸面部比较显著的一系列位置。目前常见的人脸特征点检测方法，会选择面部最为显著的68个特征点作为检测对象。在本文中，使用双目相机拍摄人脸图像，并做人脸特征点检测，左右两幅图像中均检测到对应的特征点之后，就能利用双目视觉计算人脸特征点的三维坐标，进而确定人脸的位置，应用到头动跟踪和点云配准里面。

OpenCV里面集成的人脸特征点检测方法是出自《Face Alignment at 3000 FPS via Regressing Local Binary Features》这篇论文，顾名思义速度非常快，也能同时检测出一幅图中多个人脸，缺点是错误率比较高，会把一些不是人脸的地方也检测为人脸，但也可以经过简单的修改提高表现。

首先需要导入OpenCV的一些头文件：

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/face/facemark.hpp>
#include <opencv2/objdetect.hpp>
#include <opencv2/face/facemarkLBF.hpp>

然后加载训练好的模型和人脸特征分类器：

cv::CascadeClassifier faceDetector;
cv::Ptr<cv::face::Facemark> facemark;
this->facemark->loadModel("lbfmodel.yaml");
this->faceDetector = cv::CascadeClassifier("haarcascade_frontalface_alt2.xml");

接下来就需要读入图像并识别图像中的人脸了，注意拍摄到的图像是有畸变的，需要利用相机的标定结果（内参矩阵和畸变参数）来消去畸变：

//相机内参矩阵
cv::Mat Mml1 = (cv::Mat_<double>(3, 3) <<
	814.331542566370, 2.54302537477624, 320.166811462154,
	0, 816.348689641441, 239.130200710676,
	0, 0, 1);
cv::Mat Mml2 = (cv::Mat_<double>(3, 3) <<
	813.005491652729, 2.01737529125153, 322.141637483107,
	0, 815.153838450258, 232.732313416557,
	0, 0, 1);

//相机畸变参数，1-5行分别为K1，K2，P1，P2，K3。K是径向畸变，P是切向畸变，注意别搞错了排放顺序
cv::Mat Dml1 = (cv::Mat_<double>(5, 1) <<
	-0.598617485967720,
	0.473453551957653,
	0.00169547674384290,
	0.000208409890958463,
	-0.933829145147369);
cv::Mat Dml2 = (cv::Mat_<double>(5, 1) <<
	-0.596710310550388,
	0.110101991305559,
	0.00194901593490481,
	0.000724389850583840,
	2.92541612872564);

cv::Mat image1,image2;
cv::undistort(this->left_image, image1, Mml1, Dml1);//undistort是OpenCV提供的去畸变函数
cv::undistort(this->right_image, image2, Mml2, Dml2);

声明存放人脸的变量，注意这里用了vector是因为OpenCV的人脸识别方法会检测出图像中所有的人脸，不一定只有一个，然后将检测到的人脸存到faces1和faces2中

std::vector<cv::Rect> faces1;//存放人脸
std::vector<cv::Rect> faces2;
std::vector< vector<Point2f> > landmarks1;//存放特征点
std::vector< vector<Point2f> > landmarks2;

this->faceDetector.detectMultiScale(image1, faces1);//识别人脸
this->faceDetector.detectMultiScale(image2, faces2);

bool success1 = this->facemark->fit(img1, faces1, landmarks1);//在识别到人脸的基础上检测人脸特征点
bool success2 = this->facemark->fit(img2, faces2, landmarks2);

检测出特征点后就可以显示在图像上，并且计算关键点的三维坐标了（使用第一部分提到的cv::triangulatePoints函数）：

if (success1 && success2)
{
    //画出特征点
    for (int idx = 0; idx < landmarks1[0].size(); idx++)
    {
        circle(image1, landmarks1[0][idx], 2, cv::Scalar(0, 0, 255),-1);
        circle(image2, landmarks2[0][idx], 2, cv::Scalar(0, 0, 255),-1);
    }
    //显示图像
    cv::Mat result(image1.size().height, image1.size().width * 2, image1.type());
	cv::Mat part1 = result(cv::Rect(0, 0, image1.size().width, image1.size().height));
	cv::Mat part3 = result(cv::Rect(image2.size().width, 0, image1.size().width, image1.size().height));

	cv::resize(img1, part1, part1.size(), 0, 0, cv::INTER_AREA);
	cv::resize(img2, part2, part2.size(), 0, 0, cv::INTER_AREA);
 
	cv::imshow("result", result);
    
    //计算三维坐标
    vector<cv::Point2f> pts_1, pts_2;//临时保存像素坐标系点变换到相机坐标系点的变量
	std::vector<std::vector<float>> xyz(landmarkNum, std::vector<float>(3, 0.0));//xyz即输出的三维坐标，landmarkNum即为68
    for (int count = 0; count < landmarkNum; count++)
	{
		pts_1.push_back(pixel2cam(landmarks1[0][count], Mml1));//注意这里的pixel2cam是把点从像素坐标系转换到相机坐标系的函数
		pts_2.push_back(pixel2cam(landmarks2[0][count], Mml2));
	}
	cv::Mat pts_4d = cv::Mat(4, landmarkNum, CV_32F);//输出变量，函数输出的是齐次坐标，所以是4x68
    
    cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);//用最小二乘法求三维坐标
    for (int i = 0; i < pts_4d.cols; i++)//归一化，即将齐次坐标转换为非齐次坐标(x,y,z)
	{
		cv::Mat x = pts_4d.col(i);
		x /= x.at<float>(3, 0); 
		cv::Point3d p(
			x.at<float>(0, 0),
			x.at<float>(1, 0),
			x.at<float>(2, 0)

		);
		xyz[i][2] = p.z;
		xyz[i][0] = p.x;
		xyz[i][1] = p.y;
		std::cout << p.x << ", " << p.y << ", " << p.z << std::endl;
	}
}

//把点从像素坐标系转换到相机坐标系的函数
cv::Point2f pixel2cam(const cv::Point2d& p, const cv::Mat& K)
{
	return cv::Point2f
	(
		(p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
		(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
	);
}

结果如下：

如果想求某些特定的点的话，只需按照上面图片中给出的特征点序号，选择landmarks1和landmarks2里面特定的点即可。

到此，基于双目立体视觉的人脸特征点三维坐标测量也就完成了。如果你也照着大概实现了一遍，你会发现存在一些问题：

1. OpenCV的人脸识别会经常把图片中不是人脸的地方当做人脸= =
2. 识别出来的特征点会明显抖动，不稳定

针对这些问题，当然存在着解决方案，敬请期待后续博文更新。