激光三角测量法中像素坐标与相机坐标转换

对激光三角测量原理进行数学建模。以相机光心Oc为原点建立相机坐标系Oc-XcYcZc。沿OcZc轴在距离光心Oc焦距f处建立等效像素坐标系Opix-uv。设轴OcZc交Opix-uv平面于Op，建立图像坐标系Op-xpyp。
设点P在像素坐标系中的对应点为(u,v)，在图像坐标系中的对应点为 (xp,yp)。设x和y方向的像元尺寸分别为dx和dy，根据像素坐标系到图像坐标系的变换，有
(1)
当f=1mm时，上式可写成
(2)
设点在相机坐标系下对应点坐标为 ，则有
(3)

设结构光平面方程为
(4)
由于点又在结构光平面的方程上，则满足结构光平面方程，有
(5)
联立（2），（3）和（5），可以求出点P在相机坐标系下的坐标值。

/******************************************************************************** 
** @ Copyright(c) $year$ $registered organization$ All Rights Reserved. 
** @auth： taify
** @date： 2021/01/31
** @desc： 尚未编写描述 
** @Ver : V1.0.0 
*********************************************************************************/

#include <iostream>
#include <fstream>
#include <vector>
#include <iomanip>

using namespace std;

struct point3d
{
	double x;
	double y;
	double z;
};

int main(int argc, char** argv)
{
	ifstream infile;
	infile.open("2_2d.txt"); //输入存放u,v像素坐标的txt
	ofstream outfile;
	outfile.open("2_3d.txt"); //输出存放xc,yc,zc相机坐标的txt
	double u, v;
	point3d pt;
	vector<point3d> pts;
	double a = -7.498590993921792e-05, b = -0.004991561928462, c = 0.001383665140253; //激光平面方程系数，方程形式为：ax+by+cz=1

	while (infile >> u >> v)
	{
		//相机内参
		double u0 = 1.284384534032673e+03, v0 = 9.451060871459099e+02;
		double fx = 2.311149383289115e+03, fy = 2.310190182917388e+03;

		//归一化图像坐标
		double x1 = (double)(u - u0) / fx;
		double y1 = (double)(v - v0) / fy;

		//相机坐标
		pt.x = (double)x1 / (a*x1 + b*y1 + c);
		pt.y = (double)y1 / (a*x1 + b*y1 + c);
		pt.z = (double)1 / (a*x1 + b*y1 + c);
		pts.push_back(pt);

		outfile << setprecision(16) << pt.x << " " << pt.y << " " << pt.z << endl;
	}

	infile.close();
	outfile.close();

	//误差评价
	vector<double> distance;
	double distance_mean = 0;
	for (int i = 0; i < pts.size(); i++)
	{
		distance.push_back(abs(a*pts[i].x + b*pts[i].y + c*pts[i].z - 1) / sqrt(a*a + b*b + c*c));
		distance_mean += abs(a*pts[i].x + b*pts[i].y + c*pts[i].z - 1) / sqrt(a*a + b*b + c*c);
	}
	distance_mean /= pts.size();
	cout << "平均偏差 " << distance_mean << endl;
	double sigma = 0;
	for (int i = 0; i < pts.size(); i++)
	{
		sigma += (distance[i] - distance_mean)*(distance[i] - distance_mean);
	}
	cout << "平均标准差 " << sqrt(sigma) / pts.size() << endl;

	return 0;
}

参考文章：
1.基于激光结构光视觉引导的焊缝跟踪技术研究添加链接描述
2.一种新的线结构光标定方法添加链接描述
3.焊接机器人视觉焊缝跟踪系统研究添加链接描述