立体防控

1. 测量WGS84坐标系下的经纬度 [lon, lat]
2. 测量中国大地原点坐标系下相机的旋转
3. 转换为中国大地原点坐标系下的[x,y,z]
4. 把相机的位置姿态设置到3D引擎里， 获得viewMatrix和projectionMatrix
5. 则任意一个世界坐标系下的坐标， 在相机二维图像坐标为
   [ x1, y1 ] = projectionMatrix * viewMatrix *[x, y, z]

如何将三维现实世界中的若干坐标点准确投影在相机的二维平面中

1.利用gps获取中国大地原点坐标系下的云台3D坐标和标机3D坐标

1.1利用gps获取云台和标机（B,L,H）坐标

1.2均转换为中国大地原点坐标系下的[x,y,z]

公式如下：
X = (N + H) * cos(B) * cos(L)
Y = (N + H) * cos(B) * sin(L)
Z = [N * (1 - e*e) + H] * sin(B)
其中：
B:大地纬度,rad
L:大地经度,rad
H:大地高,m
N:椭球面卯酉圈的曲率半径
e:椭球的第一偏心率
N和e的获取：
e = sqrt(a*a-b*b)/a;
N = a/sqrt(1-e*e * sin(B)*sin(B));
其中，
a = 6378137.0;（椭球长半径）
b = 6356752.3142;（椭球短半径）

2.构建以云台为原点的三维空间坐标系，将标机3D坐标转化为此三维空间的3D坐标

进行中国大地原点坐标系与观察坐标系之间的转换，矩阵变化如下
[https://www.cnblogs.com/creativityroom/p/7498945.html](https://www.cnblogs.com/creativityroom/p/7498945.html)
[https://www.xuebuyuan.com/3203428.html](https://www.xuebuyuan.com/3203428.html)
[https://blog.csdn.net/liubing8609/article/details/85339009](https://blog.csdn.net/liubing8609/article/details/85339009)

于是，从世界坐标系到相机坐标系，涉及到旋转和平移（其实所有的运动也可以用旋转矩阵和平移向量来描述）。绕着不同的坐标轴旋转不同的角度，得到相应的旋转矩阵，如下图所示：

那么从世界坐标系到相机坐标系的转换关系如下所示：

3.将转化后的3D坐标投影到云台相机上

[ x1, y1 ] = projectionMatrix *[x, y, z]

3.1相机坐标系与图像坐标系

从相机坐标系到图像坐标系，属于透视投影关系，从3D转换到2D。

此时投影点p的单位还是mm，并不是pixel，需要进一步转换到像素坐标系。

3.2图像坐标系与像素坐标系

像素坐标系和图像坐标系都在成像平面上，只是各自的原点和度量单位不一样。图像坐标系的原点为相机光轴与成像平面的交点，通常情况下是成像平面的中点或者叫principal point。图像坐标系的单位是mm，属于物理单位，而像素坐标系的单位是pixel，我们平常描述一个像素点都是几行几列。所以这二者之间的转换如下：其中dx和dy表示每一列和每一行分别代表多少mm，即1pixel=dx mm。

那么通过上面四个坐标系的转换就可以得到一个点从世界坐标系如何转换到像素坐标系的。

其中相机的内参和外参可以通过张正友标定获取（戳这里查看张正友标定的资料）。通过最终的转换关系来看，一个三维中的坐标点，的确可以在图像中找到一个对应的像素点，但是反过来，通过图像中的一个点找到它在三维中对应的点就很成了一个问题，因为我们并不知道等式左边的Zc的值。

若不用GPS仅有二维地图坐标

1.利用百度地图获取云台2D坐标和标机2D坐标

2.构建以云台为原点的三维空间坐标系，将标机2D坐标转化为此三维空间的3D坐标，Z轴为随机或设定值

3.将转化后的3D坐标投影到云台相机上