像素坐标转化为世界坐标python代码

一、 原版

该程序实现了输入一个像素坐标点，然后计算出该像素点实际位置距离摄像头水平距离和垂直距离，即实现了单目摄像头测距。

适用范围：

摄像头固定角度，倾斜照射到地面，目前摄像头为无畸变摄像头。

/*****************************************************************************************************
2018.6.25:视场已经测好，该进行计算了

希腊字母读法
Αα：阿尔法 Alpha
Ββ：贝塔 Beta
Γγ：伽玛 Gamma
Δδ：德尔塔 Delte
Εε：艾普西龙 Epsilon
ζ ：捷塔 Zeta
Ζη：依塔 Eta
Θθ：西塔 Theta
Ιι：艾欧塔 Iota
Κκ：喀帕 Kappa
∧λ：拉姆达 Lambda
Μμ：缪 Mu
Νν：拗 Nu
Ξξ：克西 Xi
Οο：欧麦克轮 Omicron
∏π：派 Pi
Ρρ：柔 Rho

*****************************************************************************************************/

#pragma warning(disable:4996)
#include <stdio.h>
 #include <time.h>
 #include <math.h>
 #include <iostream>
 #include <io.h>
 #include “windows.h”
 #include “fstream”//opencv相关
 #include <opencv2/opencv.hpp>
 #include <opencv2/video.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv/cv.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/ml/ml.hpp>
 #include <opencv2/objdetect/objdetect.hpp> //hog特征的c文件//自己编写的文件
 #include “UART.h”
 #include “findline.h”
 #include “DrawImage.h”//绘制图形using namespace std;
 using namespace cv;
 using namespace cv::ml;RNG rng(12345);
int main()
 {

float Alpha;//俯仰角Αα：阿尔法 Alpha
float Theta;//垂直视场角	西塔 Theta
float Beta;//水平视场角Ββ：贝塔 Beta

//测量出的参数，摄像头俯仰角一变，就会变化
float H1=44.0;//摄像头高度
float Dmin = 19.2;//最短距离
float Dmax = 187.1;//最长距离
float D0 = 29.8;//摄像头坐标与水平面线交点差值


float X0 = 0;//像素坐标值
float Y0 = 0;//像素坐标值

float X1 = 0;//实际距离摄像头水平距离，左负右正
float Y1 = 0;//实际距离摄像头垂直距离
float width = 0;
float height =0;
//中间变量
float d0 = 0;//步进视场角，用来计算离摄像头垂直距离
float B1 = 0;//每个Y1实际水平距离

//画圈半径，便于显示
int radius = 40;
//参数计算
Beta = atan(40.5 / 22.5);//水平视场角β
Alpha = atan(Dmin / H1);//俯仰角
//H2 = Dmin*H1 / Dmin1; //摄像头理论高度
Theta = atan(Dmax / H1) - Alpha;//垂直视场角

cout << "水平视场角β为：      " << Beta*180.0 / 3.1415926 << " 度" << endl;
cout << "垂直视场角θ为：      " << Theta*180.0 / 3.1415926 << " 度" << endl;
cout << "俯仰角α为：          " << Alpha*180.0 / 3.1415926 << " 度" << endl;
cout << "摄像头实际高度H1为:   " << H1 << " 厘米" << endl;
cout << "最短距离Dmin为:   " << Dmin << " 厘米" << endl;
cout << "最长距离Dmax为:   " << Dmax << " 厘米" << endl;



/*Mat srcImage = imread("1.jpg");*/



long int iiii = 1000;
	Mat frame;

	//【1】从摄像头读入视频
VideoCapture capture(1);  //该参数为0，则打开计算机自带摄像头，如果为1则打开外置USB摄像头

while (1)
{  
	iiii++;
	//隔一段时间保存一张图片
	if (iiii >= 100)//通过改变i后面的值来刷新图片界面
	{
		iiii = 0;
		capture >> frame;// //读取当前帧，capture >> frame与capture.read(frame)功能一样，
		if (frame.empty())
		{
			return 0;
		}
		 width = frame.cols;
		 height = frame.rows;

			cout << "图像宽度" << width << endl;
			cout << "图像高度" << height << endl;

		//视场中央线
		line(frame, Point(width / 2, 0), Point(width / 2, height), Scalar(6, 88, 255), 3, LINE_AA);

		line(frame, Point(0, height / 2), Point(width, height / 2), Scalar(6, 88, 255), 3, LINE_AA);

		cout << "请输入像素坐标值X0（以回车结束）:" << endl;
		cin >> X0;
		cout << "请输入像素坐标值Y0（以回车结束）:" << endl;
		cin >> Y0;

		cout << "输入的结果为" << endl;
		cout << "当前像素坐标值X0为： " << X0 << endl;
		cout << "当前像素坐标值Y0为   " << Y0 << endl;

		Point center(X0, Y0);
		//绘制圆心  
		circle(frame, center, 3, Scalar(0, 255, 0), -1, 8, 0);
		//绘制圆轮廓  
		circle(frame, center, radius, Scalar(155, 50, 255), 3, 8, 0);

		namedWindow("原图", 0);
		imshow("原图", frame);
		waitKey(2);
		waitKey(200);
		waitKey(200);
		waitKey(200);

		d0 = (height - Y0)*Theta / height; //步进小角度
		Y1 = H1*tan(Alpha + d0); //垂直距离

		B1 = (Y1 + D0)*tan(Beta / 2.0);
		X1 = 2.0*B1*(X0 - width / 2.0) / width;
		cout << "距离摄像头水平距离X1为：    " << X1 << " 厘米" << endl;
		cout << "距离摄像头垂直距离Y1为：    " << Y1 << " 厘米" << endl;

	}
}

waitKey(0);

二、 更新版本（更详细一些的）

在计算机视觉中,如何由单幅二维投影图像确定目标方位及距离是很重要的研究内容,在相机标定、三维重建等很多方面有广泛的应用价值。根据摄像机光学镜头的成像原理，推导出了图像坐标系与物理坐标系对应关系。即只要知道目标点在图像中的像素坐标，就可以推导出目标点与摄像头底部的实际水平和垂直距离。小车通过一个朝向行进路线正前方并向下倾斜的摄像头来获取当前路面上的图像，并根据图像上数字与摄像头相对位置及偏转角度来实现小车的精确定位。

由于摄像头朝向位置是倾斜向下，但又并非完全朝向正下方，获得的图像存在梯形畸变，即出现在图像下方(近端)的物体宽大；出现在图像上方(远端)的物体窄小。此梯形畸变会造成后续的导航线识别及计算出的偏转角度存在较大误差，从而无法达到准确定位和沿线行进的目的。因此直接根据摄像机光学镜头的成像原理以及其他几何关系，（如图5.14、图5.15）可以推导出图像坐标系与实际世界坐标系的映射关系，只需测量出摄像机俯仰角、水平视场角、垂直视场角、摄像头高度、图像底边距摄像头实际垂直距离、图像顶边距摄像头实际垂直距离。

通过下面的公式即可计算出图像中目标点在实际空间中距离摄像头的水平距离X1和垂直距离Y1。

实际测试效果图如图5.16。

后面为了方便使用，把程序封装了一下：

//测量出的参数，摄像头俯仰角一变，就会变化
float H1 = 50.0;//摄像头高度
float Dmin = 25;//最短距离
float Dmax = 250;//最长距离
float D1 = 29.8;//摄像头坐标与水平面线交点差值
float Beta = atan(40.5 / 22.5);//水平视场角β;//水平视场角Ββ：贝塔 Beta

/**********************************************************
*功能：输入图片及像素坐标值，返回摄像头到实际位置的距离(水平，垂直)
*参数：image， 图片

Point_in   像素坐标点

返回值：point_out 实际距离，以摄像头中轴线为届，左负右正
************************************************************/
Point2f coordinate_transformation(Mat image, Point2f Point_in)
{

Point2f point_out;
float Alpha;//俯仰角Αα：阿尔法 Alpha
float Theta;//垂直视场角	西塔 Theta

float X0 = Point_in.x;//像素坐标值
float Y0 = Point_in.y;//像素坐标值

float X1 = 0;//实际距离摄像头水平距离，左负右正
float Y1 = 0;//实际距离摄像头垂直距离

float width = image.cols;
float height = image.rows;

//中间变量
float d0 = 0;//步进视场角，用来计算离摄像头垂直距离
float B1 = 0;//每个Y1实际水平距离

//参数计算
Alpha = atan(Dmin / H1);//俯仰角
Theta = atan(Dmax / H1) - Alpha;//垂直视场角

d0 = (height - Y0)*Theta / height; //步进小角度
Y1 = H1*tan(Alpha + d0); //Y1为距离摄像头垂直距离

B1 = (Y1 + D1)*tan(Beta / 2.0);
X1 = 2.0*B1*(X0 - width / 2.0) / width;//X1为距离摄像头水平距离水平距离

point_out.x = X1;
point_out.y = Y1;

return point_out;

</div>