机器视觉在工业领域应用主要分为四大类：识别、检测、测量和定位。据机器视觉产业联盟最新调查结果显示，工业应用场景中，测量和定位应用占比约为26%，广泛应用于3C电子、半导体、汽车制造、新能源等支柱行业。根据笔者多年从业经验，定位类应用是入门门槛最高的一类机器视觉应用，涉及到大量空间几何等数理基础。

本期公开课的主题是N点标定。

N点标定概述

在介绍N点标定算法之前，我们首先要搞清楚什么是N点标定？为什么要做N点标定？
N点标定是根据输入或配置的N组坐标点对（图像——世界），计算图像坐标映射到世界坐标的射影变换关系。
上面这段话翻译过来就是：通过N组对应的图像坐标与世界坐标，求解图像坐标到世界坐标的变换矩阵。
求解这个变换矩阵做什么用？以工业领域某定位应用场景为例进行介绍，场景示意如图1所示。

如上图所示，相机拍摄料台上的工件，控制机械臂准确拾取工件。在这个系统中，相机作为视觉传感器，输入是采集得到的工件图像，输出是机械臂要到达的拾取位姿。也即是说，要根据工件图像中的特征信息，即特征点像素坐标，通过某种运算，得到工件在机械臂末端工具坐标系下的坐标，进而控制引导机械臂到达指定位置拾取工件。在这个过程中，我们需要搭一座桥梁，实现特征点像素坐标到机械臂末端工具坐标（后面统称为世界坐标）的转换。这座桥梁，业内通常称之为标定。
由于两个平面坐标系之间的射影变换关系为透视变换（射影变换和透视变换两个概念暂时不用理会，后面会介绍），透视变换为8自由度，即需要至少8组方程才能求唯一解。每一组对应点$\left( x,y \right)$提供两组方程，因此至少需要不共线的四组对应点才能求唯一解。通常为保证求解结果的精度和鲁棒性，会采集大于四组对应点实现透视矩阵求解。据笔者猜测，N点标定术语大概率来自美国康耐视公司的CogCalibNPointToNPointTool算子，OpenCV中对应算子为findHomography、Halcon中对应算子为vector_to_hom_mat2d，后两者命名偏学术化，业内广泛采用N点标定术语。

机器视觉领域商业闭源代表：美国康耐视（Cognex）的VisionPro、德国MVTec的HALCON、加拿大Teledyne DALSA的Sherlock、中国海康机器人（HIKROBOT）的VisionMaster，开源代表：OpenCV。

射影变换

射影变换概念

众所周知，平面上的一点可以用$IR^{2}$中的一对坐标$\left( x,y \right)$来表示。因此，通常$IR^{2}$等同于一张平面。把$IR^{2}$看作一个向量空间时，坐标对$\left( x,y \right)$是向量，也就是说点等同于向量。研究$IR^{2}$的几何称为射影几何。

上面这段定义摘自3D立体视觉“圣经”级别的教科书《Multiple View Geometry in Computer Vision》，强烈建议读者朋友对照原书仔细揣摩体悟。如果仍未揣摩明白，只需记住$IR^{2}$等同于一张平面，研究$IR^{2}$的几何称为射影几何。
那么，两个平面（或两个平面坐标系）之间存在哪些几何关系呢？这里，笔者先给出结论：两个平面之间的线性映射关系均满足射影变换或单应（homography）。也即是说，当不考虑两个平面之间发生非线性变换（如畸变）时，两平面之间的变换关系均为射影变换。
两个平面间的射影变换存在以下层次：

1. 欧氏变换，即平移+旋转
2. 相似变换，即平移+旋转+缩放（不做特殊说明时，均指同步缩放）
3. 仿射变换，即平移+旋转+缩放+纵横比（X轴和Y轴缩放比率不同）+斜切（X轴和Y轴不垂直）
4. 透视变换，即平移+旋转+缩放+纵横比+斜切+透视（物理世界中两条平行的直线在图像中发生相交）

不同变换层次的矩阵形式、失真示意和不变性质如图2所示。。

由上图可知，透视变换（射影变换）为8自由度，即需要至少四组对应点才能求唯一解；仿射变换为6自由度，即需要至少三组对应点才能求唯一解；相似变换为4自由度，即需要至少两组对应点才能求唯一解。

射影变换形式

欧式变换（平移和旋转的复合）可表示为：

$\left[ \begin{array}{} x^{'} \\ y^{'} \\ 1 \\ \end{array} \right]\left[ \begin{array}{} cos\theta & -sin\theta & t_{x} \\ sin\theta & cos\theta & t_{y} \\ 0 & 0 & 1 \\ \end{array} \right]=\left[ \begin{array}{} x \\ y \\ 1 \\ \end{array} \right]$

欧式变换是刚体的运动模型。
相似变换（欧式变换与均匀缩放的复合）可表示为：

$\left[ \begin{array}{} x^{'} \\ y^{'} \\ 1 \\ \end{array} \right]\left[ \begin{array}{} scos\theta & -ssin\theta & t_{x} \\ ssin\theta & scos\theta & t_{y} \\ 0 & 0 & 1 \\ \end{array} \right]=\left[ \begin{array}{} x \\ y \\ 1 \\ \end{array} \right]$

相似变换也称为等形变换，因为它保持“形状”。
仿射变换（非奇异线性变换与平移变换的复合）可表示为：

$\left[ \begin{array}{} x^{'} \\ y^{'} \\ 1 \\ \end{array} \right]\left[ \begin{array}{} a_{11} & a_{12} & t_{x} \\ a_{21} & a_{22} & t_{y} \\ 0 & 0 & 1 \\ \end{array} \right]=\left[ \begin{array}{} x \\ y \\ 1 \\ \end{array} \right]$

仿射变换也可以看做旋转、非均匀缩放与平移变换的复合。
射影变换（透视变换）可表示为：

$\left[ \begin{array}{} x^{'} \\ y^{'} \\ 1 \\ \end{array} \right]\left[ \begin{array}{} a_{11} & a_{12} & t_{x} \\ a_{21} & a_{22} & t_{y} \\ a_{31} & a_{32} & 1 \\ \end{array} \right]=\left[ \begin{array}{} x \\ y \\ 1 \\ \end{array} \right]$

射影变换是齐次坐标的一般非奇异线性变换，是非齐次坐标的一般非奇异线性变换与平移变换的复合，是两个平面间最一般的变换关系。

射影变换本质

N点标定概述章节，我们提到需要搭一座桥梁，实现特征点像素坐标到世界坐标的转换，这和前面提到的射影变换有什么联系呢？
接下来，让我们从摄像机投影模型出发，厘清二者之间千丝万缕的联系。
如图3所示，空间中某点P，在世界坐标系$\left( X_{W},Y_{W},Z_{W} \right)$、相机坐标系$\left( X_{C},Y_{C},Z_{C} \right)$、图像物理坐标系$\left( x,y \right)$和像素坐标系$\left( u,v \right)$中均有对应的坐标或投影坐标。

根据摄像机投影模型，3D世界点到像素点的投影关系可以用齐次坐标的线性映射表示为：

$\left[ \begin{array}{} u \\ v \\ 1 \\ \end{array} \right] =P_{3\times4}\left[ \begin{array}{} X_{W} \\ Y_{W} \\ Z_{W} \\ 1 \\ \end{array} \right]$

如果所有的点都在一个平面上（可以选择此平面为Z=0 ），那么上述线性映射可简化为：

$\left[ \begin{array}{} u \\ v \\ 1 \\ \end{array} \right] =H_{3\times3}^{-1} \left[ \begin{array}{} X_{W} \\ Y_{W} \\ 1 \\ \end{array} \right]$

上式中，$H_{3\times3}^{-1}$就是世界坐标系到像素坐标系的射影变换（投影变换）矩阵。
N点标定，就是求解像素坐标系到世界坐标系的射影变换矩阵（单应矩阵），即$H_{3\times3}$（后面统称为单应矩阵）。

单应矩阵估计

N点标定阶段，采集得到N组对应像素坐标与世界坐标点集，根据摄像机投影模型有：

$\left[ \begin{array}{} X_{W0} & X_{W1} & X_{W2} & ... & X_{Wn} \\ Y_{W0} & Y_{W1} & Y_{W2} & ... & Y_{Wn} \\ 1 & 1 & 1 & ... & 1\\ \end{array} \right] =H_{3\times3}\left[ \begin{array}{} u_{0} & u_{1} & u_{2} & ... & u_{n} \\ v_{0} & v_{1} & v_{2} & ... & v_{n} \\ 1 & 1 & 1 & ... & 1\\ \end{array} \right]$

记$\left[ \begin{array}{} u_{0} & u_{1} & u_{2} & ... & u_{n} \\ v_{0} & v_{1} & v_{2} & ... & v_{n} \\ 1 & 1 & 1 & ... & 1\\ \end{array} \right]$为矩阵A，记$\left[ \begin{array}{} X_{W0} & X_{W1} & X_{W2} & ... & X_{Wn} \\ Y_{W0} & Y_{W1} & Y_{W2} & ... & Y_{Wn} \\ 1 & 1 & 1 & ... & 1\\ \end{array} \right]$为矩阵B，记$H_{3\times3}$为矩阵X，则上式可表示为：

$XA=B$

则矩阵X的最小二乘解为：

$X=BA^{T}\left( AA^{T} \right)^{-1}$

然而，由于对应点集通常受噪声干扰，导致求解得到的单应矩阵不够准确，因此可采用迭代重加权最小二乘法或RANSAC算法对单应矩阵优化求解，获取准确的单应矩阵，提高N点标定的精度。

注意：由于系数矩阵A通常不是可逆矩阵，因此不能直接求逆，$X=BA^{-1}$是不正确的。
另外，还可以采用SVD分解来估计单应，鲁棒性更强。本文提供的算法代码亦采用此方法。

算法总体结构

单应矩阵求解算法总体结构如图4所示。

如上图所示，初始化模块完成模块输入数据的初始化，包括输入点集的归一化，并根据不同变换模型计算线性超定方程组的系数矩阵；优化求解模块采用最小二乘法计算三种变换模型对应的单应矩阵的估计值。

算法处理流程

单应矩阵求解算法流程如图5所示。

如上图所示，首先通过初始化模块获取归一化点集和线性超定方程组的系数矩阵，然后通过优化求解模块经最小二乘估计或加权最小二乘估计，获得单应矩阵以及估计误差。单应矩阵求解算法对不同变换模型的单应矩阵分别进行优化求解，包括：透视变换模型、仿射变换模型以及相似变换模型。

初始化模块

初始化模块对输入数据进行有效性检查，对输入点集数据进行归一化并根据不同变换模型，求解其对应的线性超定方程组的系数矩阵。

优化求解模块

优化求解模块流程图如图6所示。

如上图所示，优化求解模块采用迭代重加权最小二乘法分别求解透视变换、仿射变换以及相似变换模型的单应矩阵估计值，加权最小二乘法采用huber、tukey权重函数分别对三种变换模型的单应矩阵估计值进行优化求解。
至此，本文实现了根据输入或配置的N组坐标点对（图像——世界），计算图像坐标到世界坐标的射影变换关系的N点标定。

完整算法代码（透视投影变换模型）

/// <summary>
/// 单应矩阵透视投影模型求解函数（点集归一化处理）
/// </summary>
/// <param name="srcPoints"></param>
/// <param name="dstPoints"></param>
/// <param name="HomoMat"></param>
/// <returns></returns>
int computePerspectiveHomographyMatrix(const std::vector<Point>& srcPoints, const std::vector<Point>& dstPoints, float HomoMat[9])
{
	int nRet = MVD_OK;
	try
	{
		int numPoints = srcPoints.size();

		// 输入点集零均值化（Z-score）
		double x_average_imagePoints = 0, y_average_imagePoints = 0, x_average_worldPoints = 0, y_average_worldPoints = 0, sigma_imagePoints = 0, sigma_worldPoints = 0;
		for (int i = 0; i < numPoints; i++)
		{
			x_average_imagePoints += srcPoints[i].x;
			y_average_imagePoints += srcPoints[i].y;
			x_average_worldPoints += dstPoints[i].x;
			y_average_worldPoints += dstPoints[i].y;
		}
		x_average_imagePoints /= numPoints;
		y_average_imagePoints /= numPoints;
		x_average_worldPoints /= numPoints;
		y_average_worldPoints /= numPoints;

		double sigma_imagePoints_sum = 0, sigma_worldPoints_sum = 0;
		for (int i = 0; i < numPoints; i++)
		{
			sigma_imagePoints_sum += sqrt(pow((srcPoints[i].x - x_average_imagePoints), 2) + pow((srcPoints[i].y - y_average_imagePoints), 2));
			sigma_worldPoints_sum += sqrt(pow((dstPoints[i].x - x_average_worldPoints), 2) + pow((dstPoints[i].y - y_average_worldPoints), 2));
		}
		sigma_imagePoints = sigma_imagePoints_sum / numPoints;
		sigma_worldPoints = sigma_worldPoints_sum / numPoints;

		Eigen::MatrixXd T_imagePoints(3, 3);
		Eigen::MatrixXd T_worldPoints(3, 3);
		GetNormalizationMatrix(T_imagePoints, x_average_imagePoints, y_average_imagePoints, sigma_imagePoints);
		GetNormalizationMatrix(T_worldPoints, x_average_worldPoints, y_average_worldPoints, sigma_worldPoints);

		Eigen::MatrixXd imagePoints(3, numPoints);
		Eigen::MatrixXd worldPoints(3, numPoints);
		for (int i = 0; i < numPoints; i++)
		{
			imagePoints(0, i) = srcPoints[i].x;
			imagePoints(1, i) = srcPoints[i].y;
			imagePoints(2, i) = 1;
			worldPoints(0, i) = dstPoints[i].x;
			worldPoints(1, i) = dstPoints[i].y;
			worldPoints(2, i) = 1;
		}

		Eigen::MatrixXd imagePoints_Norm(3, numPoints);
		Eigen::MatrixXd worldPoints_Norm(3, numPoints);
		imagePoints_Norm = T_imagePoints * imagePoints;
		worldPoints_Norm = T_worldPoints * worldPoints;

		// 构建系数矩阵A
		Eigen::MatrixXd A(2 * numPoints, 8);
		Eigen::MatrixXd B(2 * numPoints, 1);
		for (int i = 0; i < numPoints; ++i) {
			const double& x = imagePoints_Norm(0, i);
			const double& y = imagePoints_Norm(1, i);
			const double& u = worldPoints_Norm(0, i);
			const double& v = worldPoints_Norm(1, i);

                        // TODO CODE 系数矩阵A的赋值，留给读者自行补充实现

			B(2 * i, 0) = u;
			B(2 * i + 1, 0) = v;
		}

		// 使用奇异值分解求解最小二乘问题
		Eigen::MatrixXd H = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);

		// 将9维向量转换为3x3的单应矩阵
		Eigen::MatrixXd H_Mat(3, 3);
		H_Mat << H(0), H(1), H(2),
			H(3), H(4), H(5),
			H(6), H(7), 1;
		Eigen::MatrixXd h = (T_worldPoints.inverse() * H_Mat * T_imagePoints).transpose();
		h /= h(8);

		HomoMat[0] = h(0);
		HomoMat[1] = h(1);
		HomoMat[2] = h(2);
		HomoMat[3] = h(3);
		HomoMat[4] = h(4);
		HomoMat[5] = h(5);
		HomoMat[6] = h(6);
		HomoMat[7] = h(7);
		HomoMat[8] = 1;
	}
	catch (...)
	{
		return MVD_ERROR;
	}
	return nRet;
}

/// <summary>  
/// 获取归一化矩阵  
/// </summary>  
/// <param name="mat"></param>  
/// <param name="x"></param>  
/// <param name="y"></param>  
/// <param name="sigma"></param>  
void GetNormalizationMatrix(Eigen::MatrixXd& mat, double x, double y, double sigma)
{
	mat(0, 0) = 1.0 / sigma;
	mat(0, 1) = 0;
	mat(0, 2) = -(1.0 / sigma) * x;
	mat(1, 0) = 0;
	mat(1, 1) = 1.0 / sigma;
	mat(1, 2) = -(1.0 / sigma) * y;
	mat(2, 0) = 0;
	mat(2, 1) = 0;
	mat(2, 2) = 1;
}

编译上述代码需要配置Eigen库（Eigen库是一个开源的C++库，专注于高效的线性代数运算、矩阵运算、数值解法等领域），Eigen库配置步骤相对简单，本文不再赘述，感兴趣的读者朋友可以从互联网上查阅相关资料，并亲自动手实践。
通过对本课程的学习，希望每位读者都能掌握机器视觉核心算法之N点标定，轻松驾驭测量类和定位类应用。

本期课程就到此结束了，下期课程我们不见不散。

如觉得本文内容不错，请记得给小编点赞、收藏、分享一键三连，您的支持是笔者更新的不懈动力。