基于单目相机的标靶三维定位——编程实现

        上一章内容中我们描述了基于单目相机实现标靶三维定位的原理，关键步骤为1）计算得到相机的内参和畸变系数；2）计算得到标靶角点的世界坐标和像素坐标；3）计算标靶坐标系到相机坐标系的变换矩阵。

        第一点我们通过相机标定得到；第二点的核心功能我们可以借助cv::findChessboardCorners和cv::cornerSubPix实现；第三点的核心功能则通过调用cv::solvePnP来实现。上述函数的功能和接口介绍在前面内容中已经包含，这里不再赘述。

        首先我们实现一个函数，输入为包含棋盘格的图像，相机内参，输出为标靶坐标系相对于相机坐标系的变换信息，具体实现如下。

/** @brief 计算包含了棋盘格的图像相对于相机坐标系的位姿信息，其中棋盘格所在的世界坐标系原点为左上角角点，X向右，Y向下
@param img: 包含棋盘格的图像
@param board_size: 棋盘格中角点分布行列数
@param board_length: 棋盘格中方块的边长，单位为mm
@param cameraMatrix: 相机内参矩阵
@param distCoeffs: 相机畸变系数矩阵
@param rvec: 计算得到的世界坐标系变换到相机坐标系的旋转向量
@param tvec: 计算得到的世界坐标系变换到相机坐标系的平移向量
@return 若计算成功返回0，否则返回非0值
*/
int CalculateExtrinsics(const cv::Mat& img, cv::Size board_size, float board_length, cv::Mat& cameraMatrix,
	const cv