opencv和C++使用Halcon标定的相机内参和外参,将图像坐标转为世界坐标

坐标变换详解

提示：这里可以添加技术概要

1.1 坐标关系

相机中有四个坐标系，分别为world，camera，image，pixel

• world为世界坐标系，可以任意指定$x_w$轴和$y_w$轴，为上图P点所在坐标系。
• camera为相机坐标系，原点位于小孔，z轴与光轴重合，轴和轴平行投影面，为上图坐标系$X_c{Y}_c{Z}_c$。
• image为图像坐标系，原点位于光轴和投影面的交点，轴和轴平行投影面，为上图坐标系xy。
• pixel为像素坐标系，从小孔向投影面方向看，投影面的左上角为原点，uv轴和投影面两边重合，该坐标系与图像坐标系处在同一平面，但原点不同。

1.2 坐标转换

下式为像素坐标pixel与世界坐标world的转换公式，右侧第一个矩阵为相机内参数矩阵，第二个矩阵为相机外参数矩阵。假设图像坐标已知，同时相机内参数矩阵通过标定已获取，还需计算比例系数s和外参数矩阵。

$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_1\\ r_{21}& r_{22}& r_{23}& t_2\\ r_{31}& r_{32}& r_{33}& t_3\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$
转换公式可简化为：

$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=M(R\left[\begin{array}{c}X\\ Y\\ Z_{const}\end{array}\right]+t)$

s为比例系数, M为相机内参数矩阵，R为旋转矩阵，t为平移矩阵，$Z_{const}$为世界坐标系高度，可设置为0。

通过矩阵变换可得下式：

$R^{-1}{M}^{-1}s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{c}X\\ Y\\ Z_{const}\end{array}\right]+R^{-1}t$

求解出旋转矩阵和平移矩阵即可算得s。

外参矩阵

提示：这里可以添加技术整体架构

外参数矩阵

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

• objectPoints，输入世界坐标系中点的坐标；
• imagePoints，输入对应图像坐标系中点的坐标；
• cameraMatrix, 相机内参数矩阵；
• distCoeffs, 畸变系数；
• rvec, 旋转向量，需输入一个非空Mat，需要通过cv::Rodrigues转换为旋转矩阵；
• tvec, 平移向量，需输入一个非空Mat；
• useExtrinsicGuess, 默认为false，如果设置为true则输出输入的旋转矩阵和平移矩阵；
• flags，选择采用的算法；
• solvePnP的参数rvec和tvec应该都是double类型的

程序实现

提示：这里可以添加技术名词解释
（1）计算参数s和旋转平移矩阵，需要输入一系列的世界坐标系的点及其对应的图像坐标系的点。

//输入参数
Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 摄像机内参数矩阵 */
Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0)); /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
double zConst = 0;//实际坐标系的距离，若工作平面与相机距离固定可设置为0
	
//计算参数
double s;
Mat rotationMatrix = Mat (3, 3, DataType<double>::type);
Mat tvec = Mat (3, 1, cv::DataType<double>::type);
void calcParameters(vector<cv::Point2f> imagePoints, vector<cv::Point3f> objectPoints)
{
	//计算旋转和平移
	Mat rvec(3, 1, cv::DataType<double>::type);
	cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
	cv::Rodrigues(rvec, rotationMatrix);
}

（2）根据输入的图像坐标计算世界坐标。

Point3f getWorldPoints(Point2f inPoints)
{
    //获取图像坐标
    cv::Mat imagePoint = cv::Mat::ones(3, 1, cv::DataType<double>::type); //u,v,1
	imagePoint.at<double>(0, 0) = inPoints.x;
	imagePoint.at<double>(1, 0) = inPoints.y;
 
	//计算比例参数S
	cv::Mat tempMat, tempMat2;
	tempMat = rotationMatrix.inv() * cameraMatrix.inv() * imagePoint;
	tempMat2 = rotationMatrix.inv() * tvec;
	s = zConst + tempMat2.at<double>(2, 0);
	s /= tempMat.at<double>(2, 0);
 
    //计算世界坐标
	Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * imagePoint - tvec);
	Point3f worldPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0));
	return worldPoint;
}

Opencv相机标定

提示：这里可以添加技术细节
内参矩阵：

或

内参矩阵参数含义：

f：焦距，单位毫米；dx：像素x方向宽度，单位毫米；1/dx:x方向1mm内有多少个像素；

f/dx：使用像素来描述x轴方向的焦距的长度

f/dy：使用像素来描述y轴方向的焦距的长度

u0,v0为主点的实际位置，单位也是毫米。

Halcon格式

内参参数转换：
fx = f/sx ; fy = f/sy;u0 = cx ; v0 = cy ;

参考

提示：这里可以添加总结

1. 图像坐标与世界坐标转换
2. http://answers.opencv.org/question/62779/image-coordinate-to-world-coordinate-opencv/
3. https://stackoverflow.com/questions/12299870/computing-x-y-coordinate-3d-from-image-point
4. https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html