工业相机测距开发（2）：实战篇

前言

本文将不再涉及原理部分，想要了解基础知识的话，请看上一篇的文章，我们使用的是opencv的里面的函数，这里面也是重点看这个函数们，我们通过这个函数来得到外参，在通过外参来得到我们最后的结果！

Opencv:SolvePNP

参考：https://www.jianshu.com/p/b97406d8833c

简介：如果场景的三维结构已知，利用多个控制点在三维场景中的坐标及其在图像中的透视投影坐标即可求解出摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R，该类求解方法统称为N点透视位姿求解（Perspective-N-Point，PNP问题）

void solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags = CV_ITERATIVE)

接下来我们来具体看下这几个参数：

• objectPoints - 世界坐标系下的控制点的坐标，vector的数据类型在这里可以使用
• imagePoints - 在图像坐标系下对应的控制点的坐标。vector在这里可以使用
• cameraMatrix - 相机的内参矩阵
• distCoeffs - 相机的畸变系数
• rvec - 输出的旋转向量。使坐标点从世界坐标系旋转到相机坐标系
• tvec - 输出的平移向量。使坐标点从世界坐标系平移到相机坐标系
• flags - 默认使用CV_ITERATIV迭代法
  这里面参数第二个不是像素坐标系，不要弄混了，然后前两个可能得需要像素坐标一定的变化得到，具体过程见上一篇文章，还有看上个文章的时候，一定要把相机的内参矩阵得到。如下：

代码书写

不管理论多么明白，最后都要通过代码实现出来

加载相机参数

我们这里是先新建一个xml文件，然后我们可以用opencv的库去读取它：

<opencv_storage>
<Y_DISTANCE_BETWEEN_GUN_AND_CAM>0.Y_DISTANCE_BETWEEN_GUN_AND_CAM>
<CAMERA_MATRIX_1 type_id="opencv-matrix">
  <rows>3rows>
  <cols>3cols>
  <dt>ddt>
  <data>
    1096.8 0. 551.3 0.
    1201.0 544.3 0. 0. 1.data>CAMERA_MATRIX_1>
<DISTORTION_COEFF_1 type_id="opencv-matrix">
  <rows>4rows>
  <cols>1cols>
  <dt>ddt>
  <data>
    0.0160 -3113
    0. 0.data>DISTORTION_COEFF_1>
opencv_storage>

然后就是记得更改参数，对照我们上面得到的参数

定义物体的真实尺寸

其实这步非常关键，因为这样的话可以通过假设物体的中心在关心上，然后写出在世界坐标系的坐标，反求外参，从而求出距离和位姿。（具体的看点理论就懂了）

这里我是用的装甲板，但是装甲板没有到，所以这里面我把大小先设置为0

//距离
double my_distance=0;
//定义大小装甲板的大小
double small_armor_height = 0; 
double small_armor_width = 0;
double big_armor_height = 0; 
double big_armor_width = 0;

在我们得到真实物体的大小后，我们来书写在世界坐标系的坐标，这里用到了齐次坐标。

/*******************************************************************
函数名：  void AngleSolver::set_object_point(double height,double width)
作用：    给外部的接口去接收参数
细节：    设置大小在set_size中
输入：    物体的长与宽
返回：    世界坐标系的点
********************************************************************/
void AngleSolver::set_object_point(double height,double width)
{
    double half_x = width / 2.0;
	double half_y = height / 2.0;

    object_point.push_back(Point3f(-half_x, half_y, 0));   //左上
    object_point.push_back(Point3f(half_x, half_y, 0));	    //右上
    object_point.push_back(Point3f(half_x, -half_y, 0));   //右下
    object_point.push_back(Point3f(-half_x, -half_y, 0));  //左下
}

放置图像坐标

这个首先要对特征点进行识别，然后将这些点放到一个容器里面去，这里的话我是一装甲板为例：

LeftLight_rect = left_light.boundingRect();
RightLight_rect = right_light.boundingRect();  

//求解4个顶点并且放到容器里面
double p1_x = (LeftLight_rect.tl().x + LeftLight_rect.width/2);
double p1_y = LeftLight_rect.tl().y; 
armor_point.push_back(Point2f(p1_x,p1_y));
double p2_x = (RightLight_rect.tl().x + RightLight_rect.width/2);
double p2_y = RightLight_rect.tl().y;
armor_point.push_back(Point2f(p2_x,p2_y));
double p3_x = (RightLight_rect.br().x - RightLight_rect.width/2);
double p3_y = RightLight_rect.br().y;
armor_point.push_back(Point2f(p3_x,p3_y));
double p4_x = (LeftLight_rect.br().x - LeftLight_rect.width/2);
double p4_y = LeftLight_rect.br().y;
armor_point.push_back(Point2f(p4_x,p4_y));

测距加角度解算

通过pnp算法我们得到平移向量，同时自然会得到3个轴的坐标，我们利用这些坐标来求解：

/*******************************************************************
函数名：  void AngleSolver::solve_angle()
作用：    根据类中的参数和世界坐标进行角度解算，先对距离求解，在求出偏航角和俯仰角
细节：    注意后期要加入抢口到相机的距离和，这个得根据具体情况（+/-），还有后期要考虑一个重力的影响
输入：    无
返回：    无
********************************************************************/
void AngleSolver::solve_angle()
{
	//定义旋转矩阵
	Mat _rvec;
	//定义平移向量
	Mat tVec;
	//得到上述外参
    solvePnP(object_point, targetContour, CAMERA_MATRIX, DISTORTION_COEFF, _rvec, tVec, false, SOLVEPNP_ITERATIVE);
	
	//P4P_solver：利用外参求取我们想要的东西
	//根据平移向量的xyz轴投影求取距离
	tVec.at<double>(1, 0) -= GUN_CAM_DISTANCE_Y;
	double x_pos = tVec.at<double>(0, 0);
	double y_pos = tVec.at<double>(1, 0);
	double z_pos = tVec.at<double>(2, 0);
	//求取距离
	distance = sqrt(x_pos * x_pos + y_pos * y_pos + z_pos * z_pos);

	//利用投影来求解角度的正切值
	double tan_pitch = y_pos / z_pos;//sqrt(x_pos*x_pos + z_pos * z_pos);
	double tan_yaw = x_pos / z_pos;
	//角度转弧度
	x_pitch = -atan(tan_pitch) * 180 / CV_PI;
    y_yaw = atan(tan_yaw) * 180 / CV_PI;
}